基于PID调节的两轮自平衡小车的循迹控制
基于PID调节的两轮自平衡小车的循迹控制
硬件电路篇
小车主控芯片使用飞思卡尔公司产的K60。
电源模块
车体电路使用两种电压，分别为3.3v和5v供电。车体使用7.2V的锂电池供电，为提供电路所需电压，设计了稳压电路模块。稳压电路采用常用的稳压芯片LM1117-3.3和7805。稳压电路如下
|
5V稳压电路
|
|
3.3V稳压电路 |
传感器电路模块
陀螺仪电路
陀螺仪采用ENC-03MB，传感器输出信号小，需要外接放大电路。
现有两种电路电路1和电路2。
|
电路1
|
|
电路2 |
经过实际试验，电路2输出的信号有明显过冲，电路1没有过冲。信号的过冲对车体直立控制影响很大。所以最终选用电路1作为陀螺仪的放大电路。
电机驱动电路
选用BTS7960作为驱动芯片。驱动电路如图：
驱动电路控制信号为PWM1L和PWM1R，通过控制控制信号的占空比控制驱动电路的输出电压，进而达到控制电机转速的目的。
控制电路与驱动电路之间要有隔离电路，隔离电路隔离驱动电路对主电路的影响。
计数电路
小车速度测量采用编码器检测，通过计数电路对编码器的输出脉冲计数。本电路采用CD4520芯片计数。串联两个16进制的电路达到256进制的计数器。
方向检测电路
在方向检测中使用电感感应当前位置的电磁场强度，输出与电磁场强度相关联的电压。电路采用LC振荡电路，信号放大电路和检波电路。
电路的输出信号为直流电压信号，但信号中含有锯齿波，如图。
通过加大R24的电阻值可以减少锯齿波的幅度，一般将电阻值设为100k ,锯齿波的幅度可以接受。
|
增加负载电阻后输出波形 |
软件篇
一、 信号处理
小车运行需要的信号包括陀螺仪输出信号，加速度传感器信号，编码器脉冲信号和电磁感应信号。
在小车运动过程中，外界的震动影响加速度传感器的输出，但在静止情况下加速度传感器的输出比较稳定。即加速度传感器的静态特性比较好，动态特性差。陀螺仪可以测量转速，陀螺仪的动态特性好，受外界震动影响比较小。
在测量车体倾斜角时，将陀螺仪输出与加速度传感器输出数据融合，这样可以达到所得到的结果稳定，受外界震动影响小。我们对陀螺仪输出与加速度输出进行卡尔曼滤波，得到车体倾斜角度与车体倾斜角速度。
卡尔曼滤波
建立系统的状态方程与测量方程。
其中， 为车体倾斜的真是角度， 为陀螺仪的常值温漂， 为包含固定偏差的陀螺仪输出角速度， 为陀螺仪测量噪声， 为加速度计经处理后得到的角度值， 为加速度计的测量噪声。
令系统的采样周期为 ,得到离散系统的状态方程：
在进行卡尔曼递归运算过程中，需要知道系统过程噪声协方差阵Q 以及测量误差的协方差矩阵R,对卡尔曼滤波器进行校正。
Q与R矩阵的形式如下：
式中,q_acce 和q_gyro 分别是加速度计和陀螺仪测量的协方差,其数值代表卡尔曼滤波器对其传感器数据的信任程度,值越小,表明信任程度越高.在该系统中陀螺仪的值更为接近准确值,因此取q_gyro 的值小于q_acce 的值。
卡尔曼递归运算的相关算式：
X（k|k）为k时刻的最优估计值，P（k|k）为对应X（k|k）的协方差。X（k|k-1）为根据k-1时刻的最优估计得到的k时刻的预测值。K(k)为卡尔曼增益。Z（k）为k时刻测量的角度值。这一次得到的最优估计值X（k|k），作为下一次预测值得参考X（k-1|k-1），这样周而复始反复循环计算这些算式，可以使P(k|k)收敛到最小，这样X(k|k)就是最优值。
在进行卡尔曼滤波过程中，需要根据实际情况调试Q与R，可以使滤波后的波形平滑，毛刺少，并且与实际波形的相位差小。在小车控制中Ts=0.001s， ；
卡尔曼滤波前后的波形如下：
速度信号
对于速度信号每5毫秒读取一次计数器的数，并将计数器清零。将得到的脉冲数乘上一个比例因子，得到小车的实际线速度。这个比例因子可以通过实际测量出来；也可以通过测量车轮的直径，从而得到车轮的周长，根据车轮转一圈所发出的脉冲数，计算得到这个比例因子。
方向信号
最初采用一般的IIR滤波器，但IIR滤波器有相位延迟，相位的延迟在实际中表现为时间的滞后。经过相位补偿，最终时间滞后为30毫秒左右。但在车速较快时，这种时间滞后就表现为小车转向反应滞后，不利于小车在较高的速度下运动。最后，取消IIR滤波器，直接根据AD的数据控制车体。
二、小车控制
在信号处理好后，小车的重点就在控制上面。小车的运动可以分为三个部分，即为站立、直线运动和转向。由于车体运动分为这三个部分，并且这三个部分必须几乎同时控制，所以采用分时控制每一部分的方法，类似CPU时间分片。
程序的控制和信号处理都是在中端里面执行，通过设置全局变量来确定执行什么操作。程序设置1毫秒中断，程序进入中断后，全局变量加1，判断全局变量的值，进入不同的执行部分，执行相应的操作。
中断服务程序的框架：
|
1毫秒中断进入 |
|
全局变量0~4循环计数 |
|
0：获取左右轮编码器信号 1：获取方向信号，进行方向控制计算 2：车体角度控制计算和电机输出控制 3：车体速度控制计算 4：获取车体运行实时数据，供调试使用 |
|
1毫秒中断退出 |
其中，除了上述基本框架外，在每次进入中断后，首先执行Kalman滤波程序，并且执行单片机AD采样。根据我们实际获得的经验，Kalman滤波是比较耗时间的，并且AD采样同样也是很消耗时间，如果在中断执行的总时间超过1毫秒，这将会影响Kalman滤波的效果，因此在这个方面，必须考虑AD所开的通道数目，当通道数目过多，将会导致一次中断服务程序执行的时间超过1毫秒。这时可以选择增加中断定时时间，进而也就需要改变Kalman的Ts。
在控制中，我们采用传统PID控制。我们对直立控制采用PD控制，这主要原因是在直立控制中的主要要求就是快速性，因而舍弃积分单元。对于速度控制采用PID控制，积分单元消除误差。对于方向控制采用PD控制。
在直立控制过程中，P越大，车体抗干扰能力越强，但过大会导致车体振荡，这时可以通过加大微分参数消除振荡。当P达到一定程度时，车体会抖动并不是振荡，这时调节D并不能消除抖动。车体抖动的主要原因来源于Kalman滤波后的信号精度，当精度比较低时，P过大，会直接放大信号的幅度，进而表现为车体的抖动。
在做车的速度控制过程中，速度控制的周期会影响车体的运行的稳定性。在我们实际测试中当速度控制周期定为100毫秒时，低速运行车体比较稳定。但当车速度给定为1.3m/s时，车体在转弯后，恢复平稳比较困难。设置速度控制周期为200毫秒后，效果改善明显。
在方向控制中，我们使用分段PID控制。这主要原因是车体在直道运行时，稍微偏离中心线对车体运行没有影响，也就是在知道上方向控制不需要太灵敏，太灵敏不利于车在直道的稳定和进入弯道的稳定。相反，在弯道就需要方向控制的灵敏，灵敏度越高越好，但前提是保证车体的直立基本稳定。
在实际中，车体陀螺仪传感器的安装水平程度对车体运行是否稳定影响很大，安装越水平越好，越利于车的提速。安装不水平会出现车载转弯的过程中加速和减速的现象。
参考文献：
[1] 第七届全国大学生“飞思卡尔”杯智能汽车竞赛电磁组直立行车参考方案(版本V1.0)
[2] 冯志勇，曾翰，张力，赵艺欣，黄伟.基于陀螺仪及加速度计信号融合的姿态角度测量.西南师范大学学报(自然科学版),2011(8):137-141.
[3] 何友，王国宏，彭应宁编著.多传感器信息融合及应用.2版.北京：电子工业出版社，2007.