全国大学生智能车竞赛经验分享—直立车

摘自：https://mp.weixin.qq.com/s/Jqy12s5BpsjBn1TocK260w

全国大学生智能车竞赛经验分享—直立车

原创 慕羽⭐ 古月居 1月26日

直立车涉及到的很多的东西，一篇文章很难说明白，本篇文章以话题的方式展开，大家可以挑自己感兴趣的部分阅读，当然本篇文章涉及到内容不限于直立车，因为很多东西都是通用的。

 

 

话题01

姿态传感器（陀螺仪和加速度计）的使用

 

 

谈到直立车，包括今年新增的单车，这两种涉及到平衡的车型，是肯定离不开姿态传感器的，我去年用的是MPU6050，本话题就以此为例介绍，如何使用姿态传感器获取我们想要的角度或者加速度。MPU6050的各轴正方向如下所示：

传感器安装的角度不同，编写程序时所用的轴也不一样，本文按照如下图所示的角度安装姿态传感器MPU6050。

接下来介绍一下如何使用MPU6050的加速度计获得直立车的倾角，在放程序前，我们先来看一下，利用加速度计和atan2函数求倾角的原理简介。

如上图所示，加速度传感器在XY平面内旋转，由于X与Y轴正交，X轴检测到重力加速度的正弦分量，Y蚰检测到重力加速度的余弦分量。旋转时随着一条娃的灵敏度下降，另一条轴的灵敏度将会上升。这时倾角可以通过计算X轴和Y轴比值的反正切来得到：

有了以上的基础，我们现在来看一下我写的由MPU6050的加速度计计算倾角的程序，我将其封装成了一个函数方便调用，其代码如下：

 

•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  

void ACC_Ang_Caculate()    //由加速度计计算得到角度值{
    float  ACC_Y , ACC_X;     ACC_Y=-1.0*mpu_acc_y;     ACC_X=mpu_acc_x;
    Ang_ACC=(180.0/3.1415926*(float)(atan2(ACC_Y,ACC_X)))+180 ;    //Ang_ACC是车与水平方向的夹角，实际值在0~90之间（车正常运行），其实没有限幅的必要   if(    Ang_ACC>180)   {Ang_ACC=0  ;}     if(Ang_ACC>90) Ang_ACC=90;    //限幅    if(Ang_ACC<0) Ang_ACC=0;
}

 

在该函数中我们先定义了两个变量 ACC_Y , ACC_X，用来存放读取的姿态传感器的数据mpu_acc_y和mpu_acc_x（至于怎么读取的一般商家给的库函数里都有），然后就是利用上面介绍的原理，用atan2函数来求其角度值，前面的180.0/3.1415926是将弧度制转化为角度制，后面+180（数值大家根据自己的实际情况而定）是为了使当车平放在地面上时计算出的角度值Ang_ACC为0，完全直立时角度值为90，这样角度值就在0到90度之间，便于后面的控制，后面的语句是为了限幅和错误纠正。

 

但是由于运动加速度的叠加，仅由加速度计计算出的角度，会有一些毛刺，我们无法分辨重力加速度和由运动带来的加速度，因此我们还需要另外用到MPU6050里的陀螺仪，我们接下来看一下，如何使用陀螺仪计算小车的倾角，同样我也将其写成了一个函数方便调用，其代码如下：

 

•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  

void Gyro_Ang_Caculate()   //由陀螺仪的角速度计算出角度{    float  Gyro_Z;  Gyro_Z=mpu_gyro_z;    Car_Gyro_Z=(float)(Gyro_Z*1.0-Gyro_Z_init0);    Gyro_Speed_Z=(float)((Gyro_Z*1.0-Gyro_Z_init0)*Gyro_speed_Z_Ratio);    Ang_Gyro=(float)(Ang_Gyro+(float)(Gyro_Speed_Z));
    if(Ang_Gyro>90) Ang_Gyro=90;    //限幅    if(Ang_Gyro<0) Ang_Gyro=0;
}

 

根据本部分一开始所展示的安装方向的照片，我们很容易知道，在小车的倾角上下变化时在陀螺仪的Z轴会产生角速度，因此在函数内部，我们先定义一个新的变量Gyro_Z，来存放读取的陀螺仪Z轴的值（角速度），我们知道对角速度进行积分就可以得到我们想要的角度值，其中Gyro_Z_init0是陀螺仪静止时测得Z轴角速度值，我的是-16.5，这个值也就是初始误差，Gyro_Z*1.0-Gyro_Z_init0才是真正的角速度值，将其乘以一个积分系数Gyro_speed_Z _Ratio，我将其取值为0.00029，大家根据自己的实际情况进行修改，对其进行累加就得到了我们想要的角度值Ang_Gyro，接下来的语句依然为限幅。

 

仅由陀螺仪计算出的角度，由于积分的作用，误差会随时间放大，所以我们需要同时参考加速度计和陀螺仪得出的角度来获取最终的角度值，常用的方法有互补滤波和简易卡尔曼滤波算法，由于去年直立组使用的是STC8H/G单片机，因此我采用了计算量小且快的互补滤波（其实两种滤波算法得到的角度近似）互补滤波算法如下：

 

•  
•  
•  
•  
•  
•  
•  

//互补滤波 参数Ang_Gyro是陀螺仪计算出的角度 ，Ang_ACC是加速度计计算出的角度float Complementary_filtering(float Ang_Gyro, float Ang_ACC ){float Ang_filtered;Ang_filtered=Ang_Gyro+(Ang_ACC-Ang_Gyro)*filter_ratio; //互补滤波 return Ang_filtered;}

 

为了方便调用我也将其封装成了一个函数，其输入参数为上面介绍的由加速度计和陀螺仪计算出的角度值，输出为经过互补滤波后的角度值，其中filter_ratio为互补滤波系数，我取值为0.0155，大家可根据自己的实际情况进行调整，至于卡尔曼滤波算法我之前已经做过简单的介绍，有兴趣的可以看一下，其链接如下：

 

卡尔曼滤波算法*

https://blog.csdn.net/qq_44339029/article/details/105597930

 

至此，我们已经获得了我们想要的角度值，然后就可以利用该角度值对直立车进行直立控制了。

 

 

话题2

关于编码器的数据处理问题

 

 

很多人将从编码器获取的数据直接进行滤波处理，然后使用了，其实如果你将编码器产生的数据进行记录的话，你会发现，过段时间，编码器就会产生一个异常的数据，如果不处理，直接拿来用的话，可能会造成车的不稳定或者抖动，先来看一下我记录的三组数据：

上面的三组数据，第一列为输出的PWM值，第二列和第三列分别为左右电机编码器采集到的数据，我们发现过段时间就会有一组异常数据，我们可以对异常数据的进行观察，并以此来确定消除异常数据的方法，比如可以设定最大值，最小值，单位时间内的最大变化率等，来检测异常数据，并对其进行处理，比如删除或者用上一组数据来代替等处理完异常数据后，再进行滤波处理，一般来说简单的一阶低通滤波就可以满足要求，所谓一阶低通滤波就是这一次的值乘以一个系数再加上上一次的值乘以一个系数来作为最终使用的值，系数根据需要进行调整，如：

 

•  

new_date=0.7*new_date+0.3*last_date;   //低通滤波

 

 

话题03

关于电感值的数据处理问题

 

 

一般来说采集的电感值需要先经过一个或者多个滤波函数的处理，下面放几个常见的滤波算法。

 

平均值滑动滤波算法，参数ValueInput[4]是输入数据，参数ValueOutput[4]是处理之后的数据，参数num是处理数据个数

 

•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  

void Normal_Average_Filter(uint16 ValueInput[],uint16 ValueOutput[],int num){  static uint16 ValueSum[4] = {0};  static uint16 count = 0;
  ValueSum[0] += ValueInput[0];  ValueSum[1] += ValueInput[1];  ValueSum[2] += ValueInput[2];  ValueSum[3] += ValueInput[3];
  count ++;
  if(count == num)  {    count = 0;    ValueOutput[0] = ValueSum[0] / num ;    ValueOutput[1] = ValueSum[1] / num ;    ValueOutput[2] = ValueSum[2] / num ;    ValueOutput[3] = ValueSum[3] / num ;       ValueSum[0] = 0;    ValueSum[1] = 0;    ValueSum[2] = 0;    ValueSum[3] = 0;  }}

 

防脉冲干扰平均滤波算法，参数ValueInput[4]是输入数据，参数ValueOutput[4]是处理之后的数据，参数num是处理数据个数

 

•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  

void Anti_Interference_Filter(uint16 ValueInput[],uint16 ValueOutput[],uint8 num){  static uint16 Value[4][10];//若num值大于10需要把10换成num对应的值  static uint16 count = 0;  int i,j = 0,k = 0;  uint16 Temp;
  static uint16 AD_Sum[4]={0};
  Value[0][count] = ValueInput[0];  Value[1][count] = ValueInput[1];  Value[2][count] = ValueInput[2];  Value[3][count] = ValueInput[3];
  count++;
  if(count == num)  {    count = 0;    for(i=0;i<4;i++)//冒泡排序，把每一路的AD值从小到大排列    {      for(j=0;j      {        for(k=0;k        {          if(Value[i][k] > Value[i][k+1])//前面的比后面的大则进行交换          {            Temp = Value[i][k+1];            Value[i][k+1] = Value[i][k];            Value[i][k] = Temp;          }        }      }    }     for(i=0;i<4;i++)//去掉最大最小值，求中间项的和，并进行平均运算    {      for(j=1;j<(num-1);j++)      {        AD_Sum[i] += Value[i][j];                 }      ValueOutput[i] = AD_Sum[i] / (num-2);          AD_Sum[i] = 0;    }    }  }

 

关于滤波后的电感值数据，一般要进行归一化处理，其算法如下：

 

•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  
•  

void ADC_Normalization(uint16 AD_Sensor[],float AD_Normalized[])//AD值归一化{  uint8 i;
  for(i=0;i<4;i++)  {    AD_Normalized[i]=(float)(AD_Sensor[i]-AD_MinValue[i])/(float)(AD_MaxValue[i]-AD_MinValue[i]);
    if(AD_Normalized[i]<=0.0)    {      AD_Normalized[i]=0.001;    }    if(AD_Normalized[i]>1.0)    {      AD_Normalized[i]=1.0;    }    AD_Normalized[i]=100 * AD_Normalized[i];  //AD[i]归一化后的值，0-100之间  }}

 

其实归一化算法非常好理解，简单点说就是将电感值按比例缩放到0到某个数之间，比如上面的AD_Normalized一开始缩放到0~1之间，然后乘以系数100，也就是缩放到0 ~ 100之间，中间的代码用于限幅和异常数据处理，归一化之后的数据就可以用来计算偏离赛道中心线的情况，来进行转向/差速控制了。

 

 

话题04

关于直立车的控制方案

 

 

一般来说，直立车的控制可分为直立环、转向环、和速度环，首先要写的肯定是直立环，直立环根据姿态传感器的数据，进行直立环PID控制，输出直立环PWM，使车保持直立状态，直立车直立是基础，当然直立环也不是越硬越好，这个看个人的需要吧，关于直立的角度，经过我的多次实验，一定不要太低，过低后会带来很多不必要的麻烦（比如上坡道时车不稳，贴地，电感值异常等），一般直立环调节到原地站立在机械零点，给予下压或者上挑干扰后可以在短距离的前后移动后恢复稳定状态，这时候就可以加转向环了，之前刚开始调完直立环后，我直接加的速度环，发现虽然给两个电机相同的PWM值，但是我的车就是不跑直线，后来经过数据采集后发现，两者的转速也没有什么规律可言，通过差值PWM来使其走直线也是不可行的，索性就直接先加转向环，通过转向环的作用来纠正其偏离中心赛道的偏差，发现效果很好，调完转向环后再调速度环（同时调也行），转向环和速度环都可以看做是对直立环的干扰，推荐使用串级PID的方法。

至于完整的控制代码就先不放了（给学弟学妹们留点小福利），简单说一下前期三个环的控制思路。

 

直立环：在周期性中断中采集姿态传感器的数据，比如10ms采集一次，在主函数中初始化相应的外设，比如姿态传感器MPU6050的初始化、IIC的初始化、电机PWM的初始化，然后在直立控制函数中，先调用写好的由陀螺仪和加速计计算角度的函数，分别计算出当前小车距离地面的夹角，然后采用滤波算法，获取其融合后真正使用的角度值，然后将该角度值与设定的机械零点值相减，获取偏差通过PID算法对其进行控制，调试参数使其保持直立。

 

转向环和速度环：在周期性中断中采集电感值ADC的数据，比如10ms采集一次，在主函数中初始化相应的外设，比如编码器所用的定时器的初始化、电感值ADC通道的初始化等，然后在控制函数里，读取编码器的值，并对其进行异常数据检查及处理，适当的滤波处理，然后由左右编码器的值得到车中心的速度值与设定值做差来获取速度偏差进行控制，在输出速度环和转向环PWM时建议采用平滑输出的方式，来减小短时间内对直立环的干扰，此外关于转向环，根据在不同的赛道元素采用不同的转向环系数的方式来增强控制效果，当然这种方法虽然使得控制效果更优化，但是需要检测和判断赛道元素，调试的参数也变多了，大家根据实际情况选择是否使用，还有非常重要的一点各环PWM的输出一定要限幅，限幅值的大小也是很重要的调节参数，尤其是直立环的。

 

以上内容均作为参考，每辆车的情况因机械结构，控制方式的不同都有所差别。

 

 

话题05

关于调试工具

 

 

在调试的时候，我们可采用的工具有很多，比如简单的串口打印、显示屏（OLED等），当然我还是推荐大家去选择或者自己写一个上位机，来辅助调试，上位机可以实时的返回我们需要的数据，以此来帮助我们寻找问题和纠正错误，还可以帮助我们优化程序等。

 

 

 

 

 

 

 

《ROS人机交互软件的界面开发 》课程会为大家介绍如何在Qt中订阅与发布ROS的话题，并将ROS的话题消息在Qt中进行可视化显示，实现：Qt中订阅与发布ROS话题，速度仪表盘显示与实现，键盘控制实现，电池电量显示，订阅图像话题与显示，快捷指令功能。

 

 

点击"阅读原文"查看课程详情

阅读原文