基于飞思卡尔单片机的两轮车控制系统设计

　　4.3 互补滤波算法

　　陀螺仪的动态响应较好，可以检测瞬态角度变化，但由于其本身存在累计漂移误差，不适合长时间单独工作；加速度计的静态性能较好，能够准确地检测静态角度，但受动态加速度影响较大，不适合测量动态变化角度。因此本文采用互补滤波算法将陀螺仪和加速度计测量到的角度信息进行融合，得到准确的车模倾角信息。

　　互补滤波算法公式为：

　　上式中θ_new是第n次滤波后的角度值，θ_old是第n次滤波前的角度值，α_gyro是陀螺仪滤波权重系数，在本系统中取值为0.97,β_acc是加速度计滤波权重系数，在本系统中取值为0.03,ω_gyro是第n次陀螺仪采样测量的角速度值，θ_acc时第n次加速度计采样测量的角度值。其中陀螺仪滤波权重系数和加速度计滤波权重系数纯在如下关系：

　　4.4 小车平衡控制算法

　　本系统在小车平衡控制上采用了PD算法，小车平衡控制算法公式为：

　　上式中ν_temp是控制小车平衡的电机控制变量，θ_new是当前小车倾斜角度，ν_gyro是当前小车的角速度。通过上位机观察在不同P、D参数情况下的响应曲线来确定具体的P、D参数。具体调节方法是首先改变P参数，得到一个超调量最小的响应曲线；然后改变D参数，使得响应曲线的反应速度快并且超调量小。

　　4.5 电机速度及转向控制算法

　　由于本系统采用了两个独立的直流电机，并且在每个电机的齿轮上组装了一只双相编码器作为速度反馈器件。因此两个电机在控制上均是独立的，这样便可实现小车的前进、后退及转向。在转向控制上，是通过控制左右电机差速的方法来实现。利用摄像头采集赛道边线位置信息，并通过赛道边线位置信息来拟合赛道中心线的位置信息。由于赛道宽度是已知并且固定的，所以当摄像头采集到左右两边赛道边线时，采用中值法获取赛道中心线；当摄像头只能采集到左右一边赛道边线时，给当前赛道信息补上赛道宽度一半的量来获取赛道中心线。最后通过计算，获得小车所需的转向比例。本系统在电机控制上采用了PI算法，左右电机控制算法公式为：

　　上式中Speed_New_Left是左电机速度，Speed_New_Right是右电机速度。K是小车转向比例，即当小车在直道上时K=0,当小车需要左转时K0,当小车需要右转时K>0,K的取值由小车偏离赛道中心线的程度决定。Speed_Dev是上一次期望速度与实际速度的偏差。Speed_Dev_Add是速度偏差的累积。其中P、I参数的确定方法是首先调节P参数，通过上位机观察响应曲线，直至得到反应快且超调量最小的响应曲线；然后调节I参数，以获得满意的响应曲线。

　　5.结语

　　本文介绍了一种自主循迹两轮智能车的设计与实现。在动态数据采集的基础上，主要研究了对由加速度传感器和陀螺仪采集的角度信号进行融合的互补滤波算法，在路径识别上对由摄像头采集路径信息的二值化处理方法。经过实验验证，该系统具有工作稳定、循迹准确和运行速度快的特点。本系统方案也能扩展到类似的机器人系统中，应用前景广阔。