一、引脚配置#define PWMA_PIN 29 #define PWMB_PIN 20 #define AIN1_PIN 35 #define AIN2_PIN 36 //#define STBY_PIN 20 #define BIN1_PIN 27 #define BIN2_PIN 28 #define ENC1A_PIN 25 #define ENC1B_PIN 24 #define ENC2A_PIN 32 #define ENC2B_PIN 18 - RX //#define STBY_PIN 20 - 固定3.3V 小智 sensor 传感器控制需要拉高PA30二、硬件选择参数主控黄山派 6轴陀螺仪 3轴地磁传感器 N20减速电机 3V-12V 100转/min 减速比150 7PPR 尺寸【图】 额定电压6V 电机驱动 TB6612模块 - IC -三、设计流程四、角度获取参考例程https://docs.sifli.com/projects/sdk/latest/sf32lb52x/example/rt_device/sensor/README.html#sensor lsm6d:6轴陀螺仪 mmc56x33轴地磁传感器0.0 坐标系选取●以地面为坐标物体的运动视为空间中的运动●物体坐标系○无论物体在哪都已物体本身为坐标系中心0.1 俯仰角、偏航角、翻滚角首先明确物体三轴对应的方向角度●物体三轴分为 x-y-z○绕z轴水平旋转为 - 偏航角yaw○绕x轴左右翻转为 - 翻滚角yoll○绕y轴前后翻转为 - 俯仰角pitch1.lsm6d加速度计加速度易受影响外部或、包括自身移动●测量 x-y-z 三轴的加速度○静止时 z轴加速度为重力加速度x - y 轴无加速度○当设备倾斜即可测得对应轴向上的重力加速度的分速度●通过方向加速度 反正切函数 计算角度○如上图静止倾斜时通过x轴测得的加速度与z轴测得的加速度值的反正切函数即可计算出对应的俯仰角度pitch翻滚角roll同理○需要注意水平的偏航角无法通过加速度计测量■无论如何水平旋转x-y轴加速度无变化●加速度计优缺点○水平 yaw 角度无法计算○动态漂移严重■静止或者匀速运动时物体加速度为0可准确计算出实际角度■物体运动时测量值带有物体自身移动加速度导致物体动态时漂移严重○长时间不漂移○以重力加速度为准测量绝对角度水平方向除外acce_angle.pitch atan2(lsm6d_acce.data.acce.x, lsm6d_acce.data.acce.z) / 3.1415927f * 180; //俯仰 acce_angle.roll atan2(lsm6d_acce.data.acce.y, lsm6d_acce.data.acce.z) / 3.1415927f * 180; //翻转 acce_angle.yaw 0;角速度计受设备零偏影响●测量 x-y-z 三轴的角速度○角速度积分得角度●角速度计优缺点○零偏误差静止不动时由于设备固有差异会出现不同的零偏值零偏的不断累积导致稳态误差无法完全消除○只能测量相对值需要初始明确值初始化○动态稳定性强不会受运动加速度影响/*其中0.005 为设定时间dt*/ gyro_offset.pitch (lsm6d_gyro.data.gyro.y*0.005*25/32768); gyro_offset.yaw (lsm6d_gyro.data.gyro.z*0.005*25/32758); gyro_offset.roll (lsm6d_gyro.data.gyro.x*0.005*25/32768);2、mmc56x3受磁场电磁、强磁影响磁感应器主要用于修正无法通过加速度计修正的水平yaw角度测量地磁地磁确定●测量 x-y-z 三轴的地磁强度○角度计算方式类似加速度计参考地磁北●主要校准水平 yaw 角度●归一化处理角度水平角度限制在-180 ~ 180°之间mmc56x3_angle.yaw -atan2f(mmc56x3.data.mag.y, mmc56x3.data.mag.x) * 180.0f / 3.1415927f; float diff mmc56x3_angle.yaw - gyro_angle.yaw; if (diff 180.0f) diff - 360.0f; if (diff -180.0f) diff 360.0f;3.普通加权计算角度●获取各个传感器的值并分别计算出角度数据然后根据稳定新计算加权计算出角度●只是单纯叠加无法解决零偏●Angle输出角度 Angle _g角速度角 a * (Angle_a加速度角 - Angle_g)4.互补滤波计算角度融合加速度计与角速度计优缺点计算角度 - pitch - roll 融合角速度计与磁感应计计算角度 - yaw●互补滤波融合两者间的有点消除两者的缺点●角速度计计算角度为主、加速度计计算角度为辅加速度计容易受自身运动影响●加速度计角度和陀螺仪角度取加权平均值 得到互补滤波后角度●Angle输出角度 Angle _g角速度角 a * (Angle_a加速度角 - Angle_g)●注意滤波后下一次Angle_g 的累加要在Angle基础上加【互补滤波公式关键】●互补滤波相当于使用加速度计角度/地磁感应器角度来修复加速度角度●互补滤波系数○互补滤波系数的大小决定了加速度计对角速度计的影响○系数过大会导致设备抖动体现设备开始小幅度摆动且摆动幅度会越来越大噪声振荡gyro_angle.pitch angle.pitch - ((lsm6d_gyro.data.gyro.y )* 0.005*25/32768 - gyro_offset.pitch) ;// gyro_offset.pitch 前倾为正 gyro_angle.roll angle.roll ((lsm6d_gyro.data.gyro.x )* 0.005*25/32768 - gyro_offset.roll) ;// gyro_offset.roll 右翻为负 gyro_angle.yaw ((lsm6d_gyro.data.gyro.z )* 0.005 * 25 / 32758 * 1.5 - gyro_offset.yaw);//- gyro_offset.yaw- gyro_offset.yaw 右转为负 acce_angle.pitch atan2(lsm6d_acce.data.acce.x, lsm6d_acce.data.acce.z) / 3.1415927f * 180; //俯仰 acce_angle.roll atan2(lsm6d_acce.data.acce.y, lsm6d_acce.data.acce.z) / 3.1415927f * 180; //翻转 acce_angle.yaw 0; mmc56x3_angle.yaw -atan2f(mmc56x3.data.mag.y, mmc56x3.data.mag.x) * 180.0f / 3.1415927f; float diff mmc56x3_angle.yaw - gyro_angle.yaw; if (diff 180.0f) diff - 360.0f; if (diff -180.0f) diff 360.0f; angle.pitch gyro_angle.pitch a *(acce_angle.pitch - gyro_angle.pitch); angle.roll gyro_angle.roll a * (acce_angle.roll - gyro_angle.roll); angle.yaw gyro_angle.yaw * 0.95 0.05 * diff;// 0.02 * diff5.消除零偏通过一定时间测量的平均初始值来得到传感器的初始偏移值●器件均有零偏无法完全消除只能尽可能减小其他为实现更加精确的角度计算可以再换更为复杂的算法滤波计算●四元数、卡尔曼滤波五、电机控制N20编码器电机控制 参考例程https://docs.sifli.com/projects/sdk/latest/sf32lb52x/example/rt_device/motor/README.html 编码器使用gptim1/2 pwm使用atim channl 1/20. PID控制PID负反馈调节 P比例 I积分 D微分P:比例P_term Kp * errorerror(目标值 - 测量值●根据当前误差大小纠正测量值逼近目标值●当误差较大时P_term值大更快接近目标值●kp越大 - 响应越快 - 振荡●kp越小 - 响应越慢 - 稳定对应直立无法快速直立I积分integral error;I_term Ki * integral;●消除稳态误差●消除长期积累的误差仅kp计算会有累计误差无法到达目标值●ki值过大引起超调抖动D:微分D_term Kd * (Error - LastError)LastError前一次误差值●预测误差变化抑制振荡●当误差值减小时D_term为负可抑制输出值输出P_out P_term I_term D_term;●可根据实际情况自由匹配PID算法●PID | PI | PD●算法优化不完全○根据需求添加适用○积分限幅防止积分深度饱和输出被限制在100%电机全速转动○微分先行对误差的积分转为对实际值的微分解决目标值跳变问题更加平滑○输出偏移输出非0时添加固定偏移值跳出输出死区○输入死区误差较小时不调控pid-LastError pid-NowError; pid-NowError pid-Target - pid-Measure; if (pid-Ki ! 0) { pid-Integral pid-NowError; } else { pid-Integral 0; } float max_integral pid-MaxOutput / 2.0 / pid-Ki; // 防止积分过大 if (pid-Integral max_integral) pid-Integral max_integral; if (pid-Integral -max_integral) pid-Integral -max_integral; pid-Result pid-Kp * pid-NowError pid-Ki * pid-Integral - pid-Kd * (pid-Measure - pid-LastMeasure); //weifenxiangxing // pid-Kd * (pid-NowError - pid-LastError)*2; if (pid-Result pid-MaxOutput) {pid-Result pid-MaxOutput;} if (pid-Result pid-MinOutput) {pid-Result pid-MinOutput;} if(pid-Result 0) { pid-Result pid-Resultoffset; } if(pid-Result 0) { pid-Result - pid-Resultoffset; } pid-LastMeasure pid-Measure; return pid-Result;1.角度环控制输入 目标 需要的目标 yaw 角度 测量 当前物体yaw角度 设定pid 输出 双轮差速 控制需求 调节PID值使得角度控制趋于稳定●通过输入目标角度和当前角度到PID计算器输出值为左右轮的差速●通过两轮的差速实现转动改变当前实际角度●反馈实际角度与目标角度误差实现单闭环控制if(cnt %10 0 cnt ! 0) { turn_pid.Measure measured_angle.yaw; //turn_pid.Target 0; dif_pulse pid_controller_cal_sat123(turn_pid); //转向环输出差分速度 }p值小p值增大2.直立环控制输入 目标 需要的目标 pitch 角度 单直立环 目标 速度环外环输出角度值 串级PID 测量 当前物体 pitch 角度 输出 双轮平均速度 设定pid 控制需求 单直立环控制PID参数调节PID值使得角度控制趋于稳定●单直立环控制可以实现小车直立○当小车pitch角度前倾时给一个向前的速度来保持小车的直立平衡●单直立环实现直立无法控制速度小车会前后摆动移动无法静止直立●加入速度环可解决angle_pid.Measure measured_angle.pitch;//0;// ave_pulse -pid_controller_cal_sat123(angle_pid); //直立角度环控制3.速度环控制输入 目标 需要的目标 速度 测量 轮子转速 输出 一个目标角度 设定pid 控制需求 调节PID值使得角度控制趋于稳定●速度环的控制为一个目标角度○该目标角度为直立环目标角度输入○静止时目标速度0 计算目标倾角 0保持直立○运动时目标速度x计算目标角度y目标实际偏差电机向前运动●需要平衡角度环与速度环的执行间隔○角度环作为内环需要快速响应○速度环作为外环需要更慢速的响应○当外环接近内环响应时出现内外环耦合现象内环还未完成偏移调整外环继续给出响应导致无法平衡实际现象可为设备剧烈抖动调节PID难解决if (cnt 100) //need to adjust the print frequency to avoid overwhelming the console { cnt 0; ave_speed (pulse_to_rpm(enc_l) pulse_to_rpm(enc_r)) / 2.0; //当前平均速度 dif_speed pulse_to_rpm(enc_l) - pulse_to_rpm(enc_r); //当前差速 speed_pid.Measure ave_speed; //speed_pid.Target tar_speed; //目标速度 angle_pid.Target -pid_controller_cal_sat123(speed_pid); //速度环输出,串级PID - 外环输出给内环当目标值 }前后摆动移动硬件问题 - 电机摩擦力 - 减速箱齿轮 - 可用无刷电机●当实现直立环 角度环小车仍旧一定幅度的前后摇摆○由于电机齿轮/减速比 造成的死区问题○在一个小角度的范围变化内电机无法实现执行PID输出的调节作用响应速度变慢了○解决 pid输出加上 输出偏移当输出不为0时 加或减一个固定值if(pid-Result 0) { pid-Result pid-Resultoffset; } if(pid-Result 0) { pid-Result - pid-Resultoffset; }●MCP服务语音控制void angle_turn(float angle) { angle_pid.Target angle; LOG_I(Target angle: %f, angle_pid.Target); } //MCP Code AddTool(self.turn.right, Set the left turn function., PropertyList( { Property(angle, kPropertyTypeInteger, 0, 180) } ), [](const PropertyList properties) - ReturnValue { int angle properties[angle].valueint(); angle_turn(-angle); return true; }); AddTool(self.turn.left, Set the left turn function., PropertyList( { Property(angle, kPropertyTypeInteger, 0, 180) } ), [](const PropertyList properties) - ReturnValue { int angle properties[angle].valueint(); angle_turn(angle); return true; });六、硬件结构设计硬件结构设计●当前缺陷○优化设计需更新改进IO线路○黄山派固定结构需要更新■孔位稍微偏移■孔大小匹配 / 排针插槽短于现有铜柱螺丝○需添加挖槽■电机编码器挖槽■显示屏排线挖槽