【NXP】直立车模实现原理（mpu6050控制）

在NXP恩智浦智能车比赛中，电磁直立组是近年来新增的一项有趣项目。比赛是要求仿照两轮自平衡电动车的行进模式，让车模以两个后轮驱动进行直立行走。 在电磁组比赛中，利用了原来C型车模双后轮驱动的特点，实现两轮自平衡行走。相对于传统的四轮行走的车模竞赛模式，车模直立行走在硬件设计、控制软件开发以及现场调试等方面提出了更高的要求。  接下来由19年的参赛者：一城烟雨一楼台，分享自己参赛时，从零基础，到理解平衡车，实战比赛的学习心得。 直立车模的调试分为三个部分：1.直立环 2.速度环3.转向环 有人说速度环可以省略，我认为在你的转向环调得好的情况下可以省略，因为失去了速度环的车模就会一直处于加速状态（在弯道时由于摩擦的作用会使速度降下来），这就会导致在直道入弯时的速度无法控制，在转向环不强的时候很容易飞出赛道。 ### 直立组 先上直立环的代码，当然这种代码网上很多，形式也都差不多。 ```javascript /@@************************************************************************** 函数功能：小车平衡电机占空比控制，直立PD控制 入口参数：倾角角度、角速度 返回 值：平衡控制用PWM **************************************************************************/ int Balance_Ctrl_Pwm(float Angfloat Gyro) { float Bias=Ang-QingJiaoZhongZhi; //中值，求出平衡的角度中值 和机械相关 int balance; float kp = 42 kd = - 26; balance= -(int)(kp*Bias+Gyro*kd); //计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数 return balance; } ``` **变量的含义** QingJiaoZhongZhi // 小车平衡时候的数值，关闭电机，用手使得小车正好平衡时候的 float Angfloat Gyro //这两个是mpu6050卡尔曼滤波后的倾角角度、角速度。 然后再来说一下PD两个参数的调试。 先调试kp，需要把kd置为0。首先需要确定正负，我们先随便给一个正值例如kp=10吧。然后我们相对于车模的平衡角度倾斜车模，如果车轮的转向和你倾斜车模的方向一致则可以确定kp应取正值，否则kp取负值。然后再来确定大小 逐渐增大Kp直至车身出现大幅低频摇摆的直立在地上，然后把这个数值乘以0.6基本就是一个比较稳定kp值。调试kd时同样需要把kp置为0。首先需要确定正负：例如先给他个kd=2；当你转动车模时车轮的转向与你车模转向一直则kd是正值，否则就是负值，注意pd与车模的平衡角度无关。大小的话同样也是逐渐增大直至出现低幅高频抖动时停止，也是把这个数值乘以0.6即可。 注意：这里涉及到一个负反馈调节因为直立环是要保证正常站立。因为当实际角度脱离平衡角度时，轮子只有给车身一个更大的同方向的力才会使车身保持平衡。 如果还是不理解可以拿支笔放在手指上试试，手指怎样移动才会使笔立在手指上？ ### 速度环 速度环也是一个标准的负反馈例子。例如：我们想让车模加速，速度环就会降低车轮的速度使车身前倾，就会导致直立环起作用让车轮加速保持直立。这就完成了一个加速过程。需要注意的是速度环会干扰直立环，如果速度环的强度过大，车身就会非常不稳定。我在调用该函数时每一百次直立环才会调用一次速度环，这样就降低了速度环对直立环的干扰。 ```javascript /@@************************************************************************** 函数功能：速度PI控制 修改前进后退速度，请修Target_Velocity，比如，改成60就比较慢了 入口参数：左轮编码器、右轮编码器 返回 值：速度控制PWM **************************************************************************/ float Target_Velocity = 0; extern float kp_steep; int Velocity_Ctrl_Pwm(int encoder_leftint encoder_right) { static float VelocityEncoder_LeastEncoder; static float Encoder_Integral; float sudumax = 2700; float kp=kp_steepki = kp/200; //速度PI控制 Encoder_Least =(encoder_left+encoder_right)-Target_Velocity; //获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零） Encoder *= 0.7; //一阶低通滤波器 Encoder += Encoder_Least*0.3; //一阶低通滤波器 Encoder_Integral +=Encoder; //积分出位移 积分时间：？ms if(Encoder_Integral>2000) Encoder_Integral=2000; //积分限幅 if(Encoder_Integral<-2000) Encoder_Integral=-2000; //积分限幅 Velocity=Encoder*kp+Encoder_Integral*ki; //速度控制 if(flag_zhongdian!=0) Encoder_Integral=0; //电机关闭后清除积分 //输出限幅 if(Velocity > sudumax) Velocity = sudumax; else if(Velocity < -sudumax) Velocity = -sudumax; return (int)Velocity; } ``` ### 转向环 转向环其实有很多方法。我是用的比较简单的一种办法：动态的pid算法，P的系数是电磁传过来的偏差，D的系数是MPU6050的Z轴加速度（因为车模静止时我所在的地区Z轴加速度为2，所以在代码最后gyro+ 2进行了一次加速度补偿）。速度越快P越大D越小。当电磁传过来的偏差小于5时，判为直道，采用很小的P。可有效地减少直道上的晃动。我一开始这样做发现过弯时会有特别严重的抖动，经常飞出赛道，因此在最后加了个判断 if((My_Abs(Turn_Target) - My_Abs(Turn_Target_last)) >= 37) //入弯防抖 else if((My_Abs(Turn_Target_last) - My_Abs(Turn_Target)) >= -40) //出弯防抖 入弯防抖同时增大3倍P2倍D是车模更快更准的转弯，避免速度快时过弯不及时（毕竟我这一届只有不到30cm的前瞻）。出弯防抖与前者配合减少转弯过度带来的晃动。 变量 RoadPianCha 是左右横电感和斜电感的差除以左右电感的和。也就是说这个变量的范围是 0-100。 ```javascript /@@************************************************************************** 函数功能：转向控制 修改转向速度，请修改Turn_MaxPwm即可 入口参数：左轮编码器、右轮编码器、Z轴陀螺仪 返回 值：转向控制PWM 作 者： **************************************************************************/ int Turn_Ctrl_Pwm2(int encoder_leftint encoder_rightfloat gyro)//转向控制 { static float Turn_TargetTurnTurn_Target_last; float Turn_MaxPwmKpKd; // if(My_Abs(RoadPianCha) <= 2) // RoadPianCha = 0; Turn_Target_last = Turn_Target; Turn_Target=RoadPianCha - (encoder_left-encoder_right)/5; if(My_Abs(RoadPianCha) >= 5) { Kp = 41.0 * (200+encoder_left + encoder_right)/200.0; Kd = -4.2/ ((200+encoder_left + encoder_right)/200.0); Turn_MaxPwm = 2000; } else { Kp =3; Kd = -5; Turn_MaxPwm =200; } if((My_Abs(Turn_Target) - My_Abs(Turn_Target_last)) >= 37) //入弯防抖 { // Target_Velocity = 100; Kp = Kp * 3.2; Kd = Kd * 2.3; } else if((My_Abs(Turn_Target_last) - My_Abs(Turn_Target)) >= -40) //出弯防抖 { // Target_Velocity = 1000; Kp = Kp * 1.7; Kd = Kd * 3.4; } if(Turn_Target>Turn_MaxPwm) Turn_Target=Turn_MaxPwm; //转向速度限幅 if(Turn_Target<-Turn_MaxPwm) Turn_Target=-Turn_MaxPwm; //=转向PD控制器==// Turn= -Turn_Target*Kp + (gyro+ 2)*Kd; //结合Z轴陀螺仪进行PD控制 return (int)Turn; } ``` ### 汇总 最后关于这几个环串起来： 在主函数中开一个定时器（时间大概为1-2毫米）把这几个函数轮番调用 下面的是我定时器触发函数 ```javascript ////====数据处理====平衡控制==速度环控制==转向环控制 void PIT1_isr (void) { flag1ms++; if(flag1ms%2 == 1) { Get_Dip_Angle(); //角度获取 Balance_Pwm = Balance_Ctrl_Pwm(Angle_BalanceGyro_Balance); //===平衡PID控制 if(flag1ms >= 100) { flag_100ms++; flag1ms = 0; Velocity_Pwm = Velocity_Ctrl_Pwm( Encoder_Left Encoder_Right); //===速度环PID控制 记住，速度反馈是正反馈，就是小车快的时候要慢下来就需要再跑快一点 } } else { Encoder_Right = FTM_AB_Get(FTM1); //脉冲为前进+，后退为- Encoder_Left = -FTM_AB_Get(FTM2); Get_AD_data(); //获取电感，测距的AD值 AD_chuli(); //处理AD值，得出最终偏差 Turn_Pwm = Turn_Ctrl_Pwm2(Encoder_Left Encoder_RightGyro_Turn); //===转向环PID控制 } //输出电机PWM————负脉冲向前 Moto_Left=Balance_Pwm+Velocity_Pwm+Turn_Pwm; //===计算左轮电机最终PWM Moto_Right=Balance_Pwm+Velocity_Pwm-Turn_Pwm; //===计算右轮电机最终PWM //end PIT_TFLG(1) |= PIT_TFLG_TIF_MASK; //清中断标志位 } ``` 版权声明：本文为CSDN博主「一城烟雨 一楼台」的原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接及本声明。 原文链接：