stm32平衡小车的制作

制作平衡车之前需掌握的前置知识有：基本的控制语法，陀螺仪的姿态角计算，PID控制算法。其中我们着重去讲PID控制算法，因为它是平衡小车的关键。关于陀螺仪姿态角计算为了方便获取我们可以用jy61p陀螺仪去代替传统的mpu6050,因为jy61p陀螺仪自带卡尔曼滤波及姿态角计算，所以更加方便我们获取姿态角。

通过jy61p获取姿态角思路

    jy61p可以通过rx和tx向单片机接收和发送数据，它发送的数据有基本的加速度，角速度，也有经过计算得到的姿态角，所以我们可以通过接收jy61p的数据获得姿态角。

    如何接收到我们想要的姿态角而不接收其他的数据？这需要通过程序来判断接收到数据包的协议头和数据内容来挑选你想要的数据（具体协议可通过jy61p官网数据手册查看协议内容）其中我们关注这里

    大概意思为该陀螺仪发送的所有的数据包格式开头字节都为0x55，然后我们需要的姿态角度（即图中的角度）的第二个字节为0x53，随后的几个字节则分别代表着我们需要的姿态角的数据。所以我们只需要去判断前两个字节并接收后边的数据即可获得姿态角。

通过jy61p获得姿态角的具体代码及其注释

void jy61p_ReceiveData(uint8_t RxData){uint8_t i,sum=0;if (RxState == 0)//等待包头{if (RxData == 0x55)//收到包头{RxBuffer[RxIndex] = RxData;RxState = 1;RxIndex = 1;//进入下一状态}}else if (RxState == 1){if (RxData == 0x53)/*判断数据内容，修改这里可以改变要读的数据内容，0x53为角度输出*/{RxBuffer[RxIndex] = RxData;RxState = 2;RxIndex = 2;//进入下一状态}}else if (RxState == 2)//接收数据{RxBuffer[RxIndex++] = RxData;if(RxIndex == 11)//接收完成{for(i=0;i<10;i++){sum = sum + RxBuffer[i];//计算校验和}if(sum == RxBuffer[10])//校验成功{/*计算数据，根据数据内容选择对应的计算公式*/Roll = ((int16_t) ((int16_t) RxBuffer[3] << 8 | (int16_t) RxBuffer[2])) / 32768.0f * 180.0f;Pitch = ((int16_t) ((int16_t) RxBuffer[5] << 8 | (int16_t) RxBuffer[4])) / 32768.0f * 180.0f;Yaw = ((int16_t) ((int16_t) RxBuffer[7] << 8 | (int16_t) RxBuffer[6])) / 32768.0f * 180.0f;}RxState = 0;RxIndex = 0;//读取完成，回到最初状态，等待包头}}}

        具体相关计算方法和数据处理方法可见协议内容 ，大概意思就是接收到0x55然后又接收到0x53就可以判断这个数据包是姿态角的了，随后对数据处理就可得到姿态角，分别为Roll,Yaw,Pitch。

        于是我们得到了这个函数，大概作用是接收Roll,Yaw,Pitch。所以我们可以把他放在接收中断，那么只要陀螺仪发出数据我们就可以及时更新

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart){ if( huart == &huart4) { jy61p_ReceiveData(g_usart4_receivedata); HAL_UART_Receive_IT(&huart4,&g_usart4_receivedata,1); }}

别忘记在主函数的while前加HAL_UART_Receive_IT(&huart4,&g_usart4_receivedata,1);来开始中断。这样我们就接收到实时的姿态角了。

PID的撰写思路

  平衡车的PID为两个串级PID，分别为直立环与速度环，作用显而易见，为保持直立与控制速度。

直立环的思路为当小车向前倾时车轮应该产生一个向前的加速度来使小车保持平衡如图：

所以不难看出他的速度是和他的倾斜幅度相关的，即倾斜的幅度越大他应该产生的速度就越快，其中倾斜幅度与陀螺仪的摆放也有关，大部分是Roll或Pitch，这里我的倾斜角度是Roll。由此我们就可以写出一个闭环来了，即通过Roll来改变速度，而速度的变化也能改变Roll

此为直立环，也是平衡车能保持直立的关键。在此基础上我们可以进行升级来加固这个循环，也就是把他变成串级PID,串一个速度环，这样不仅能让直立更稳定，还能控制平衡车的前进速度。

其中的速度环需要编码器来测量真实速度。那么最后的串级PID大概如图。

于是我们就可以按照这个图来撰写PID了。

PID具体代码

首先我们应该先写直立环Vertical。直立环我们只需要PD项来控制就好。这里的Med为目标值，Angle为真实值，gyro_Y则为相应的角速度即角度的微分项。

int Vertical(float Med,float Angle,float gyro_Y){int temp;temp=Vertical_Kp*(Angle-Med)+Vertical_Kd*gyro_Y;return temp;}

然后撰写速度环Velocity。速度环只需要PI项。这里的Target为目标值，encoder_L为做编码器读数，encoder_R为右编码器读数，这两个的读数可以作为真实值。

int Velocity(int Target,int encoder_L,int encoder_R){static int Err_LowOut_last,Encoder_S;static float a=0.7;int Err,Err_LowOut,temp;Err=(encoder_L+encoder_R)-Target;//求误差Err_LowOut=(1-a)*Err+a*Err_LowOut_last;Err_LowOut_last=Err_LowOut;//简单的滤波Encoder_S+=Err_LowOut;Encoder_S=Encoder_S>2000?2000:(Encoder_S<-2000?-2000:Encoder_S);//给积分项限幅if(stop==1){Encoder_S=0;stop=0;}//刹车temp=Velocity_Kp*Err_LowOut+Velocity_Ki*Encoder_S;return temp;}

那么直立环和速度环已经准备好了，我们接下来要把他们串起来。按照我们串级PID的图片，应该先将速度环的输出值作为直立环的目标值，随后直立环的输出值作为PWM来改变速度。那么按照这样写的代码如下：

void Control(){int PWM_out;Encoder_L=Read_Speed(&htim1);//读左右编码器Encoder_R=Read_Speed(&htim8);Velocity_out=Velocity(Target_Speed,Encoder_L,Encoder_R);//得到速度环的输出值后付给直立环Vertical_out=Vertical(Velocity_out+Med_Angle,Roll,(Roll-Last_Vertical)*100);//直立环输出作为PWMLast_Vertical=Roll;PWM_out=Vertical_out;Motor1=PWM_out//以后可以在此加转向环Motor2=PWM_outLimit(&Motor1,&Motor2);//对PWM输出限幅Load(Motor1,Motor2);//PWM给到相应电机}

这里的有几处讲解一下

1.代码中的Med_Angle为机械中值，其作用是消除小车上的机械误差，即每个人的小车平衡角度不一样，要加上一些误差。

2.角速度可以通过陀螺仪直接获取，也可以去算，算法为这次的角度减去上次的角度除经过的时间。

综上我们的串级PID基本上就算完成了。

接下来我们只需要每隔一段时间去执行一下PID控制即可，所以这里用到定时器，每隔10ms去执行以下PID算法。代码如下：

uint8_t counttim=0;void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){if(htim==&htim5){counttim++;if(counttim>10){counttim=0;Control();}}}

这样我们的核心控制代码就完成了，接下来就到了最后的调参环节了

PID调参

PID调参其实也有固定的套路，我用的套路则是先调Kp,然后调Kd，最后调Ki，如果只是想粗略的调参的话Ki也可以直接付为Kp的1/200。然后调参顺序则是先调内环（直立环）再调外环。

1.在平衡车的调参中我们首先要确定符号位

把要调的参数先设为正数，如果极性不对（在平衡车中的现象是往前倾车反而往后走，这会造成正反馈，使误差越来越大）就修改为负数。

2.调直立环的Kp

我们先把其他参数置零，只去给直立环的Kp，从1开始不断增加十倍，当看到震荡幅度太大了就往回调。Kp不断增加的现象则是对误差翻译变得敏感，现象则是倾斜的角度即使很小速度也会很大。我们通过调直立环的Kp想达到的状态是让小车比较敏感但不要摇摆幅度太大。当达到这一状态后就可进行下一阶段

3.调直立环Kd

Kd的作用为阻尼，阻塞小车大幅度震荡，我们在调Kd时与Kp类似，一个数量级一个数量级地调。

那么Kd合适的现象则是小车小幅度高频振荡。达到这一现象后我们要做的就是把Kp和Kd成0.6，这是很多人得出的经验，毕竟调参靠得是经验。

4.调速度环

速度环只用调Kp,因为如果粗略调参的话Ki会是Kp的1/200。

Kp合适的现象就是平衡车大体平稳，这样就完成平衡车的制作了