[智能车]平衡车直⽴车的⼊门经验（代码讲解）
做为第⼗六届智能车的FW，在半年的做车经历中把能踩的坑都踩了个遍。写这篇⽂章是为了留个纪念，也是为了帮新车友快速⼊门（可能完全0基础）。我⾃⼰的经验也不⾜，所以可能会存在⼀些漏洞，还请⼤佬指正。
车模的搭建
北京现代伊兰特悦动在调试的时候，我们多次换过车模的结构，最后发现车模的重⼼越低越好，质量分布要尽可能的集中。不到不得已的时候不要加配重，把车沿轴的转动惯量降到最低。车模结构还是挺重要的，重⼼低点能明显感觉车更稳。这是对电路设计也要提出⼀定的要求，省赛的时候见到有⼀辆车把电路集成到了⼀块板⼦上，⽽我们是三块板⼦（还⾮常⼤），所以更难搭出好的结构。新⼿设计的话没必要整那么难的，但要提前计划好每块板⼦的位置，尽可能的精简。我不是写程序的吗，怎么在考虑电路（bushi
（下图不是最终版，但⽐较接近）
车的整体⽅案
我⾃⼰是主要负责车的程序，所以就细说怎么让车动起来。我们的车是采⽤了最简单的并级PID，电磁引导的⽅案。（太菜了，摄像头不会，串级PID也不会QAQ）核⼼是⽤的灵动的MM32，Cortex-M0内核，后换到M3内核（笑死，3080都能买到，这个芯⽚买不到）。陀螺仪⽤了龙邱的MPU9250，后换到逐飞的ICM20602（问就是龙邱垃圾）。双车的通讯模块是Lc12s（问就是便宜）。
来简单说⼀下程序的整体框架。智能车程序和学弟们最开始学的51还是有些区别，底层的驱动函数已经是由商家帮忙写好了，不需要再从头开始⽤寄存器。下⾯的程序基本适⽤于任何芯⽚，只要理解每个函数的具体含义，以及程序整体的框架，套⽤不同芯⽚的具体例程，就能快速上⼿。我在给变量命名的时候基本没⽤缩写，所以还是⽐较容易看懂的。
//其他器件初始化⼀下就不⽤动了。最重要的是这三个定时器
tim_interrupt_init_ms(TIM_8, 1, 0);  //直⽴环中断
tim_interrupt_init_ms(TIM_6, 10, 1);  //速度环中断
tim_interrupt_init_ms(TIM_7, 5, 1);  //⽅向环中断二手法拉利f430
直⽴车的三个PID就都放在了这三个定时器中，固定时间判断直⽴，转向和⽅向的情况。⽽main函数⾥最后进到以下程序。不断循环这个就可以。（定时器是需要⽤到的最基本的中断，就是相当于定了3个闹钟，每个闹钟到点后放下⼿头活，去做指定的事）
while(1)
{
run();            //车的循迹程序
Branch();        //三岔路程序
fixedCircle();    //环岛程序
}
//车模运⾏控制算法
void run()
{
if (Vol_left1_actual + Vol_right1_actual > Threshold_circle && err_angle < 10 && err_angle > -10)
dirControlOut *= 0.05;                                  //屏蔽环岛
if (Flag_dirPID == open)
dirOut = dirControlOut;                                //⽅向环开关
duty_Left = angleControlOut - speedControlOut + dirOut;    //dirOut为正是左转
duty_Right = angleControlOut - speedControlOut - dirOut;
if (duty_Left > 0)            //加电机死区
duty_Left = duty_Left + duty_death_left_P;
if (duty_Left < 0)
duty_Left = duty_Left + duty_death_left_N;
if (duty_Right > 0)
duty_Right = duty_Right + duty_death_left_P;
if (duty_Right < 0)
duty_Right = duty_Right + duty_death_left_N;
if (duty_Left > duty_Max_P)    //输出限幅
duty_Left = duty_Max_P;
if (duty_Right > duty_Max_P)
duty_Right = duty_Max_P;
if (duty_Left < duty_Max_N)
duty_Left = duty_Max_N;
if (duty_Right < duty_Max_N)
duty_Right = duty_Max_N;
MotorCtrl(-duty_Left, -duty_Right);                        //输出到电机上
}
duty表⽰占空⽐，通过输出不同占空⽐的pwm波，到电机驱动上，就可以控制电机的转速。循迹函数中最主要的就是将三环的结果叠加到duty_Left和duty_Right，然后输出作⽤到电机上，其他都是辅助作⽤，先暂且不管。 接下来我就讲⼀下这三个环的具体内容。
直⽴环
直⽴环是最重要的⼀个部分，也是最先开始调的，第⼀次让车⽴起来，还是很有成就感的。我主要说⼀下程序的内容就好了，原理还是得看《第七届全国⼤学⽣“飞思卡尔”杯智能汽车竞赛 电磁组直⽴⾏车参考设计⽅案》。上古资料，但是特别详细，B站有配套的⾼糊视频。简单来说，就是通过采集到陀螺仪的原始数据，解算出车模的实际⾓度，得出⾓度误差。kp乘上这个误差（⽐例控制，就是差的越多，输出越多，很好理解）再叠加kd乘⾓速度（消除震荡，起到⼀个阻尼的作⽤，就是不让它转那么快）。
另⼀个⽐较重要的就是⾓度的解算，这个的响应速度影响了车控制的平稳。我采⽤的是⼀阶互补滤波算法，后换到⼆阶。下⾯附上完整代码。
/
*************************************** 互补滤波函数（包含直⽴环PD）*********************************************/
/*angle_Filtering = K1 * angle_m+ (1-K1) * (angle_Filtering + gyro_m * dt);
angle_Filtering 是融合后的⾓度值
angle_acc是加速度测量简单计算得到的⾓度
angle + angle_gyro * dt是陀螺仪积分得到的⾓度
dtt为采样周期，单位s，就是直⽴环中断的时间，这⾥是0.001
宁夏二手车K1是滤波器系数，取0.005。
⼀阶互补滤波也可以看做是加权平均。*/
void ComplementaryFiltering()
{
if (icm_acc_x >= 0)
icm_acc_x = -1;                                //tan90°
acc_x = - icm_acc_x * 9.8 / 4096.0;                //转化单位
acc_z = icm_acc_z * 9.8 / 4096.0;                  //转化单位
gyro_y = icm_gyro_y / 16.4;                        //转化单位
anglespeed = - (gyro_y - 0.25);                    //零点偏差
angle_acc = atan((acc_z) / acc_x) * 57.3;          //加速度得到的⾓度，可加零点偏差
angle_gyro = angle_Filtering + anglespeed * dtt;    //⾓速度得到的⾓度
angle_Filtering = K1 * angle_acc + (1 - K1) * angle_gyro;
err_angle = angle_Filtering - angle_balance;
angleControlOut = P_angle * err_angle + D_angle * anglespeed;
}
/
*⼆阶互补滤波(原理是啥?学会了教教我!)*/
void ComplementaryFiltering2()车船使用税标准
{
if (icm_acc_x >= 0)
icm_acc_x = -1;
acc_x = - icm_acc_x * 9.8 / 4096.0;            //转化单位
acc_z = icm_acc_z * 9.8 / 4096.0;              //转化单位
gyro_y = icm_gyro_y / 16.4;                    //转化单位
anglespeed = - (gyro_y - 0.25);                //零点偏差
angle_acc = atan((acc_z) / acc_x) * 57.3;      //加速度得到的⾓度，可加零点偏差
x1 = (angle_acc - angle_Filtering) * (1 - K2) * (1 - K2);
y1 = y1 + x1 * dtt;
x2 = y1 + 2 * (1 - K2) * (angle_acc - angle_Filtering) + anglespeed;
angle_Filtering = angle_Filtering + x2 * dtt;
err_angle = angle_Filtering - angle_balance;
angleControlOut = P_angle * err_angle + D_angle * anglespeed;
}
代码中的⼀些正负号和陀螺仪的安放位置有关，需要⾃⼰看。关于⼀阶的原理，注释⾥已经说的很详细了，⼆阶的我是没看懂QAQ。这个滤波算法就是⼀些固定的东西，icm_xxx为采集到的原始数据，把它们换⼀下就可以⽤了，甚⾄没必要搞懂原理（做⽐赛是为了学习，还是多看看好）。
注：⼀定要看“清华⽅案”把原理搞懂，再看代码会轻松⼀些。
中断⾥⾯长这个样⼦
get_icm20602_accdata_spi();    //读陀螺仪的原始数据
get_icm20602_gyro_spi();
//ComplementaryFiltering();
ComplementaryFiltering2();    //⽐赛时⽤了这个，原理可以不会，代进去数就算好了。
}
关于调参
emmm调参的话也应该看那个清华⽅案的配套视频，⾮常详细，⼤致流程就是先取kd为0，慢慢的加kp，差不多能⽴住在原地抖动时加kd。慢慢将参数调到车能⽴在原地且没有剧烈抖动。这时推⼀下车做测试，如果车要跑很长⼀段距离才能重新⽴住，说明参数⼩了，再慢慢加p和d。差不多稳⼀些就可以上另外两个环了，跑起来后参数还要略微修改。
速度环
关于这个，我⾃⼰也有些迷惑。我⽤的就是kp乘上速度误差，叠加ki乘误差的积分，这样简单的实现⼀个闭环。然鹅⼤佬们说直⽴车的速度环应该是开环控制，这个我是没搞明⽩。直接放上代码吧。
//速度闭环控制算法（PI）
void SpeedControl()
{
actualSpeed = (speed_Left + speed_Right) * 0.5;
err_speed = setSpeed - actualSpeed;                        //速度偏差
speedIntegral = speedIntegral + err_speed * I_speed;        //积分
if(speedIntegral>3000)  speedIntegral=3000;                //限幅
if(speedIntegral<-2000)  speedIntegral=-2000;              //限幅
speedControlOutOld = speedControlOutNew;
speedControlOutNew = err_speed * P_speed + speedIntegral;
bmw330}
void OutputSpeedControl()                          //速度环平滑输出
{
float fValue;
fValue = speedControlOutNew - speedControlOutOld;
speedControlOut = fValue * (speedControlPeriod + 1) / 10.0 + speedControlOutOld;
if (speedControlOut > 4500)
speedControlOut = 4500;                    //限幅
if (speedControlOut < -4500)
speedControlOut = -4500;                    //限幅
}
speedControlPeriod++;
OutputSpeedControl();                          //速度环平滑输出
speed_Left = tim_encoder_get_count(TIM_3);              //左编码器注意插槽位置！
tim_counter_rst (TIM_3);
speed_Right = -tim_encoder_get_count(TIM_4);            //右编码器
tim_counter_rst (TIM_4);
槽罐车Encoder_accumulate = Encoder_accumulate + speed_Left + speed_Right;
Encoder_total = Encoder_total + speed_Left + speed_Right;
if(speedControlPeriod > 10)                      //速度周期控制
{
SpeedControl();
speedControlPeriod = 0;
}
}
编码器（encoder）是⼀个在轮⼦旋转时能产⽣脉冲的传感器，脉冲频率和车速成正⽐，这个都有例程的，⾃⼰康康就好。这⾥车实际速度我直接采⽤了编码器读到的值，这⾥没必要换算成实际速度。将读到的速度带到这个简单的PI即可。这⾥⽤了⼀个速度的平滑输出，即每10个速度环周期更新⼀次速度输出，这10个周期内将作⽤在电机上的占空⽐逐步递增/减过去。
⽅向环
电磁引导是采⽤电感与赛道上20kHz的电磁线，产⽣互感，再将信号整流放⼤，得到能被单⽚机检测到的电压。⼀个车上安装3-5个电感，根据这⼏个电压强度来判断当前车的位置。电感与电磁线所成的⾓度，距离都会反映到这个电压值。所以合理安排电感就能判断赛道上不同的元素。
这个采⽤了PD。。。反正我是没理解这个D参数怎么影响运⾏效果。⽽且我们的⽅向偏差就是⽣硬的采⽤了最外边两个电感的差值，速度快的时候效果不好。（第⼗六届只要完赛就是成功，还轮不到⽐速度QAQ）有⼀些⽐较好的算法，⽐如差⽐和什么的，⾃⼰去研究吧，我太菜（懒）了。
//⽅向闭环控制算法(PD)
void directionControl()
{
dvar = BiasIndActual - BiasIndActual_last;
dirControlOut = P_direction * BiasIndActual + D_direction * dvar;
BiasIndActual_last = BiasIndActual;
}
最后
这次⽐赛我是学到了不少东西，但是有因为经验不⾜，耽误了太长时间，导致没有好的成绩。当初我让车动起来就花了2个⽉的时间，从⼀脸懵逼到上海之旅，感触还是挺深的，也希望这个⽂章能快速的帮⼩⽩⼊门（理解了之后就没那么难,M3的芯⽚从上⼿到去⽐赛只调了20个⼩时，相当于重新做了个直⽴车）。当然，我发现就算有学长帮助，也会把能踩的坑都踩⼀遍2333，这才能真正学习到吧。
完整代码我整理好之后放上来。
没想到有好多⼩伙伴想看看完整代码，但是我⼀直懒得上传hhhh。放到GitHub了,gitee太拉跨,⼀次只能传20个⽂件。