# Waist **Repository Path**: mwh2035/waist ## Basic Information - **Project Name**: Waist - **Description**: 腰部康复机构下位机相关程序。 - **Primary Language**: Unknown - **License**: MulanPSL-2.0 - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 1 - **Forks**: 1 - **Created**: 2024-04-23 - **Last Updated**: 2025-11-29 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # Waist ## 软硬件信息汇总 ### 硬件 $NUCLEO-L4R5ZZI \rightarrow$[相关信息](https://www.st.com/zh/evaluation-tools/nucleo-l4r5zi.html)

$DRV8870电驱 \rightarrow$[相关信息](https://www.ti.com.cn/product/cn/DRV8870)

$BT24蓝牙(主从) \rightarrow$[相关信息](https://www.bilibili.com/video/BV1Ct4y1h7rd/?p=2)

$JY62陀螺仪 \rightarrow$[相关信息](https://wit-motion.cn/proztmz/53.html)

$AD620 \rightarrow$[相关信息](https://www.analog.com/cn/products/ad620.html)

**$TODO:$**时间精力足够前提下，会考虑陀螺仪、肌电信号采集硬件的加入以及所有硬件的集成。 ### 软件 $CUBEMX\rightarrow v6.3.0$ $HAL(L4)\rightarrow v1.17.2$ $\mu Vision\rightarrow v5.25.3$ $vofa+\rightarrow v1.3.10$ ## 文件结构 ```html . |-- APL | |-- include | `-- src |-- Core |-- MDK-ARM `-- Waist_Project.ioc ``` 该工程在原有$CUBEMX$工程基础上增加应用层$APL$，主要对接硬件层接口以及相关功能的实现。 **应用层** ```html . ├── include │ ├── APL_Control_Interface.h │ ├── APL_JY62.h │ ├── APL_MACRO.h │ ├── APL_control.h | ├── APL_LFT.h | ├── APL_impedance.h │ └── APL_frame.h └── src ├── APL_Control_Interface.c ├── APL_JY62.c ├── APL_control.c ├── APL_LFT.h ├── APL_impedance.h └── APL_frame.c ``` **APL_MACRO**: 应用层通用宏定义. **APL_frame**:解析上位机下发命令. **APL_Control_Interface**:应用层控制接口，用于封装信息采集过程、控制结果的硬件实现过程. **APL_control**:控制算法运算库. **APL_JY62**:六轴陀螺仪解析库 **APL_LFT**压力传感器采集处理 **APL_impedance**阻抗控制运算库 ## 应用层接口 ### 推杆控制接口 ```c //~/APL/APL_Control_Interface.h /* 电机控制相关宏 */ #define MOTO_LF_A F6 // 左上电机A相 #define MOTO_LF_B F7 // 左上电机B相 #define MOTO_RF_A F8 // 右上电机A相 #define MOTO_RF_B F9 // 右上电机B相 #define MOTO_LB_A D12 // 左下电机A相 #define MOTO_LB_B D13 // 左下电机B相 #define MOTO_RB_A D15 // 右下电机A相 #define MOTO_RB_B D14 // 右下电机B相 ``` ### 推杆反馈接口(ADC) ```c //~/APL/APL_Control_Interface.h /* 电机反馈相关宏 */ #define MOTO_LF_FB C0 // 左上电机 #define MOTO_RF_FB C1 // 右上电机 #define MOTO_LB_FB C2 // 左下电机 #define MOTO_RB_FB C3 // 右下电机 ``` 详细流程参考**$/APL/APL\_Control\_Interface.h$.** ### 压力传感器反馈接口(ADC) ```c /* 压力传感器反馈相关宏 */ #define LFT_LF_FB A4 #define LFT_RF_FB A7 #define LFT_LB_FB A5 #define LFT_RB_FB A6 ``` 详细流程参考**$/APL/APL\_Control\_Interface.h$.** ## 接收命令协议 | 帧头($8bits$) | 选择信息($8bits$) | LF目标值($32bits$) | RF目标值($32bits$) | LB目标值($32bits$) | RB目标值($32bits$) | 帧尾($8bits$) | | ------------- | ----------------- | ------------------ | ------------------ | ------------------ | ------------------ | ------------- | | $head$ | $sec$ | $LF\_Value$ | $RF\_Value$ | $LB\_Value$ | $RB\_Value$ | $end$ | 采用结构体加共用体形式便于接收，注意[字节对齐！！](https://zhuanlan.zhihu.com/p/210999004)，这里使用的[强制对齐](https://www.cnblogs.com/listxue/p/15515557.html)。 ```c //~/APL/APL_frame.h // 接收帧格式结构体定义 // 字节对齐 #pragma pack(1) typedef struct { APL_u8bit head; APL_u8bit sec; APL_float LF_Value; APL_float RF_Value; APL_float LB_Value; APL_float RB_Value; APL_u8bit end; } Con_Receive; #pragma pack() ``` ### 关键 $sec$作为一帧中第二顺位信息，决定接下来接收的四个目标值哪一个有效(仅仅接收有效的目标值)。 ```c //~/APL/APL_frame.h #define Frame_SEC1 0x11 //修改第一个 #define Frame_SEC2 0x22 //修改第二个 #define Frame_SEC3 0x33 //修改第三个 #define Frame_SEC4 0x44 //修改第四个 #define Frame_ALL 0xFF //修改全部 ``` 例如：当接收到$sec=Frame\_SEC2$时，该帧仅刷新$RF\_Value$值。 ## 控制算法 ### 增量式PID闭环控制 ```c // PID结构体 typedef struct { /* data */ APL_float setPoint; //目标值 APL_float actualPoint; //实际值 APL_float result; //输出调节结果 APL_float Error; //本次误差 APL_float preError; //上次误差 APL_float pre2Error; //上上次误差 APL_float integral; //误差积分 APL_float integral_max; //位置式积分限幅 APL_8bit index; //积分分离 APL_float integral_fenli; //积分分离阈值 APL_float KP,KI,KD; //比例，积分，微分系数 APL_float maximum; //输出值上限 APL_float minimum; //输出值下限 } Con_PID; ---\\--- /** * @brief 增量式PID计算 * @details 无 * @param[in] 无 * @param[out] 输出值 */ APL_float APL_PID_Realize(enum control_moto moto) { // 误差计算 APL_pid[moto].pre2Error = APL_pid[moto].preError; APL_pid[moto].preError = APL_pid[moto].Error; APL_pid[moto].Error = APL_pid[moto].actualPoint - APL_pid[moto].setPoint; APL_pid[moto].integral += APL_pid[moto].Error; // 积分限定幅度 if(APL_pid[moto].integral < 0) { if(uint_abs(APL_pid[moto].integral) > APL_pid[moto].integral_max) APL_pid[moto].integral = - APL_pid[moto].integral_max; }else{ if(APL_pid[moto].integral > APL_pid[moto].integral_max) APL_pid[moto].integral = APL_pid[moto].integral_max; } APL_pid[moto].result += APL_pid[moto].KP * (APL_pid[moto].Error - APL_pid[moto].preError) + APL_pid[moto].KI * APL_pid[moto].Error + APL_pid[moto].KD * (APL_pid[moto].Error - 2*APL_pid[moto].preError + APL_pid[moto].pre2Error); // 输出限幅 if(APL_pid[moto].result >= APL_pid[moto].maximum) APL_pid[moto].result = APL_pid[moto].maximum; if(APL_pid[moto].result <= APL_pid[moto].minimum) APL_pid[moto].result = APL_pid[moto].minimum; if(control_flag == 0) { // 精度判定 if(APL_pid[moto].setPoint*(1+PID_PRECISION) >= APL_pid[moto].actualPoint&&APL_pid[moto].setPoint*(1-PID_PRECISION) <= APL_pid[moto].actualPoint) { APL_pid[moto].result = 0; } } return APL_pid[moto].result; } ``` **由于推杆死区较大，实际运行过程中会产生啸叫，所以在推杆达到精度允许范围后不再进行控制，避免啸叫。** [参考](https://blog.csdn.net/qlexcel/article/details/103651072) ### 阻抗控制 ```c // 阻抗控制结构体 typedef struct { // 位置控制增益 APL_float K; // 质量控制增益 APL_float M; // 阻尼控制增益 APL_float B; // 期望位置 APL_float dis_p; // 根据末端力计算的期望位置 APL_float L; // PID期望偏置 APL_float L_pid; // 本次实际位置 APL_float real_p; // 上次实际位置 APL_float last_real_p; // 推杆速度 APL_float v; // 推杆加速度 APL_float a; }IM_data; ------ /** * @brief 计算PID目标值偏置 * @details 无 * @param[in] enum lft_channel枚举体,外置力,当前位置 * @param[out] 无 */ APL_float APL_IM_Caloffset(enum control_moto lf_ch,APL_float fe,APL_float pos) { // 设置阻抗控制目标值 APL_IM_SetDis_P(lf_ch,APL_Frame_Get(lf_ch,0)); // 记录上次位置 im[lf_ch].last_real_p = im[lf_ch].real_p; // 低通滤波 im[lf_ch].real_p = (4096 - pos)*SLIDER_RATE*0.2 + 0.8*im[lf_ch].last_real_p; // 计算推杆速度 im[lf_ch].v = (im[lf_ch].real_p - im[lf_ch].last_real_p)/SAMP_TIME; // 100*fe 速度为mm/s 统一尺度 im[lf_ch].a = ( im[lf_ch].K*(im[lf_ch].dis_p - im[lf_ch].real_p) - im[lf_ch].B*im[lf_ch].v + 100*fe )/im[lf_ch].M; // 加速度积分速度 im[lf_ch].v += im[lf_ch].a*SAMP_TIME; // 速度积分位置 im[lf_ch].L = im[lf_ch].real_p + im[lf_ch].v*SAMP_TIME; // 转换为PID期望偏置 im[lf_ch].L_pid = (100.0 - im[lf_ch].L)/100.f*4096.f; return im[lf_ch].L_pid; } ``` **/MATLAB 仿真/impedance_control.m**文件为阻抗控制仿真。 [参考](https://blog.csdn.net/xiaohejiaoyiya/article/details/105057619) ## 运动学逆解 ![image-20240815101615270](https://gitee.com/mwh2035/logimg/raw/master/img/202408151016199.png) 参考侯超论文运动学逆解部分， **/MATLAB 仿真/robot_main.m**文件为运动学逆解仿真，具体逆解代码在**/MATLAB 仿真/kinematics.m**。 ```matlab % 运动学逆解 % [旋转左右前后] function obj = reversed_solution(obj,alpha,beta,gamma) % 计算旋转矩阵 obj.R_ZYX = eul2rotm([alpha beta gamma],"ZYX"); % 计算P点在B坐标系下坐标作为平移矢量 obj.SP_coor = [obj.SP*sin(beta) -obj.SP*sin(gamma)*cos(beta) obj.SP*cos(gamma)*cos(beta)]; % 坐标变换 obj.B_M = obj.R_ZYX*obj.B + obj.SP_coor'; % 计算杆长 obj.L = [pdist([obj.B_M(:,1)';obj.A(:,1)'],'euclidean'); pdist([obj.B_M(:,2)';obj.A(:,2)'],'euclidean'); pdist([obj.B_M(:,3)';obj.A(:,3)'],'euclidean'); pdist([obj.B_M(:,4)';obj.A(:,4)'],'euclidean');]; end ``` ## 日志 ### 2024-4-22😑 完成$JY21$通讯，$DRV8870$测试 ### 2024-4-27😴 完成下位机通讯、应用层接口、控制算法应用；已实现四推杆位置闭环控制、主从蓝牙无线通信、简易上位机的命令发布(待优化)。

#### 待测试控制算法中间变量未观测，目前只对输入输出进行测试; $ADC$采集频率待测试; 定时器周期中断未测试，实际设置未$10ms$中断; 帧信息丢包率未测试(需添加校验位提升传输正确率); #### 待优化 $control，frame$文件注释未完善(今天[4-27]累了👴，注释抽空写😒) $TODO:$🥱👉🛏️😪 - 上位机开发 - 康复疗程相关资料 ### 2024-5-1😑 压力传感器，拟采用[AD620](https://item.szlcsc.com/606109.html)放大压力传感器惠斯通电桥差分压力信号; 电源拟采用[TPS5430](https://item.szlcsc.com/10396.html)搭配 [RT9013](https://item.szlcsc.com/48778.html)为主控供电。 ### 2024-5-3🥱 $control，frame$文件完善注释 ### 2024-5-5😑 $USART2$引脚重映射，测试可用；设置12通道$ADC$，初步测试可用，未接指定电压测试; ### 2024-5-14😋 ~~$A2,A3$引脚ADC采集可能出现混用情况，初步猜测为DMA指定内存大小设置错误。~~(**已解决**)😼 ### 2024-5-17🤦‍♂️ $ADC$实际测试正常输出，导致控制混乱情况原因为上位机传递单个目标值时会清空其他三个推杆目标值💀 ### 2024-6-12🥱👉🛏️💤 $JY62$陀螺仪通信成功。上位机界面搭建完成：

上位机实现功能： - 解析下位机发送数据包； - 发送位置命令控制下位机运动； - 蓝牙自动连接，支持断开重连；待增加功能：

- 康复流程实现，包含上位机可视化及连续的动作指令； - 游戏实现，可根据以安装的陀螺仪或推杆位置反馈实现简单的互动；TODO ### 2024-7-17🥲 实现上位机端运动学逆解、参数辨识，参数辨识条件较为苛刻。

### 2024-7-31😋 使用仪表放大器获取压力传感器信息成功，压力传感器原始信息与压力间转换关系不明确。 ### 2024-8-7☹️ 增加放大器调零功能，可同时检测拉力和压力。但是设备的四个压力传感器只有一个是可以正常使用的，修改程序后只能演示左右扭腰的阻抗控制。实际控制周期为$1ms$，但是算法中计算使用的控制周期为$10ms$。意外的是这种不匹配并没有产生异常，考虑到马上就要比赛，没有精力再去修改。后续可以修改控制周期为$10ms$，对控制参数进行修改即可，因为算法的可行性已经成功验证。 ### 2024-8-15😑🐒 完善文档，此项目结束🧑‍🔧