张驰洲，熊根良，陈海初

(南昌大学 机电工程学院，江西 南昌330031)

0 引言

扫地机器人是一个涵盖机械、汽车、电子、自动控制及传感器技术等科目的综合系统[1]。其中，直线行走和路径规划是用来评价扫地机器人清扫任务的重要指标，这成为扫地机器人研究的重要部分[2]。自主智能的扫地机器人除拥有感知、规划等功能外，还应在清扫过程中拥有自主路径规划能力的同时完成清扫任务,而其中实现机器人系统的稳定性是偏航角数据的准确性与实时性。

本文采用卡尔曼滤波算法[3]对陀螺仪和编码器数据进行融合得到最优估计，利用最优偏航角和机器人速度组成串级比例、积分、微分(PID)控制算法[4-5]，实现机器人的直线行走和精准转弯，最后利用弓字形行走方式实现全遍历清扫，与随机清扫方式相比，其清扫效率明显提高。

1 系统的硬件设计

扫地机器人的硬件系统是保障其实现清扫任务的重要前提，硬件设计的优劣决定了扫地机的工作性能。因此，本文根据扫地机器人路径规划、避障等功能需求，自主搭建扫地机器人的硬件平台，其框图如图1所示。扫地机器人的硬件实物图如图2所示。

图1 扫地机器人的硬件电路结构框图

图2 扫地机器人硬件实物图

1.1 主控芯片

本系统采用ST公司的STM32F030R8T6作为主控制器，该芯片是基于Cortex-M0内核的32位微处理器，具有功耗低，性能高，集成度高和接口丰富等特点[6]。片上集成了IIC，USART，SPI等通信接口， 包含有5个通用16位定时器和1个高级脉冲宽度调制(PWM)定时器，可输出多路PWM信号从而控制电机转速。

1.2 传感器芯片

本系统采用的传感器芯片有单轴陀螺仪GGPM01、光电编码器。其中单轴陀螺仪[7]的角速度感测范围为±250 (°)/s、±500 (°)/s、±1 000 (°)/s、±2 000 (°)/s。该款陀螺仪的工作电压在3～3.6 V，本系统中为GGPM01陀螺仪提供了3.3 V直流电压，电路图如图3所示，其中数据线SDA和时钟线SCL与微控制器通过IIC进行通信，不断读取陀螺仪的数据。而光电编码器的线数为12，分辨率为1 536(机器人齿轮减速比为128)。

图3 GGPM01陀螺仪模块驱动电路

1.3 电源模块

本系统所选用的直流工作电压为14.4 V，由3节4.8 V可充电镍氢电池组成直流电源。而电机的工作电压为8 V，因此,板载非同步降压转换器TPS54335降压得到8 V电压(同时还可得到5 V电压)。陀螺仪需要的电压为3.3 V，所以板载正向可调集成稳压器LM117实现5 V到3.3 V的电压转变。

1.4 电机驱动模块

本系统中电机驱动电路核心采用电机驱动AM2849。AM2849芯片内有4个H桥电机驱动器，可以用来驱动有刷直流电机，其工作电压在6.5～45 V，具有PWM输出接口，通过改变高电平的占空比来实现电机调速。

2 系统的软件设计

本控制系统中软件控制算法可分为两部分：

1) 电机速度控制程序，它负责控制扫地机的速度。

2) 弓字形路径规划。

2.1 电机速度控制总体流程设计

本系统电机速度控制的主程序流程图如图4所示，其中系统初始化包括系统时钟初始化、中断初始化、陀螺仪GGPM01初始化、定时器外部中断初始化、IIC初始化、光电编码器初始化等。STM32的工作频率为48 MHz，IIC的串口波特率设置为115 200 bit/s。

图4 电机控制的主程序流程图

2.1.1 卡尔曼滤波数据融合算法

在对机器人偏航角的测量过程中，若出现地面不平整或从动轮打滑等现象,则会造成光电编码器脉冲计数不准确,产生随机误差，而陀螺仪受随机误差、温飘等影响也会产生数据漂移。同时码盘的静态稳定性好，而在机器人运动时其数据相对不可靠；陀螺仪的动态稳定性更好，但是静止时数据相对不可靠。所以，可通过对码盘和陀螺仪[8]的数据进行卡尔曼滤波融合来获得较准确的偏航角。

本系统机器人采用三轮驱动方式，左、右两侧两轮采用同轴相连，通过安装在左、右两侧驱动轮的光电编码器可测出左、右两轮的线速度和两轮连线中心的线速度。

通过下式且根据采样时间Δt可计算出机器人的偏航角。

(1)

θ(k+1)=θ(k)+dθ(k)=θ(k)+wdΔt

(2)

式中：vl，vr分别为人的左、右轮线速度；B为机器人左、右轮间距；ωd为光电编码器测得的机器人偏航变化的角速度；θ(k)为机器人的偏航角。

现取系统状态变量X=(vl,vr,ωg)T，其中ωg为陀螺仪读出的角速度。因对系统的控制量为0且当前机器人的状态与上一时刻相同，则系统的状态方程为

Xk+1=Ak+1|kXk+ΓkWk

(3)

式中:Xk+1和Xk分别为tk+1时刻、tk时刻的系统状态变量;Wk是协方差为Qk的系统过程激励噪声矩阵(是均值为0的高斯白噪声);Гk为系统噪声矩阵的驱动矩阵;Ak+1|k为tk～tk+1时刻的状态转移矩阵，即

(4)

式中α=1为根据实验确定的码盘修正系数。

系统的测量矩阵为

Zk+1=Hk+1Xk+1+Vk+1

(5)

式中：Hk+1为系统测量矩阵，本系统中为单位矩阵；Vk+1为Rk+1的系统测量噪声矩阵(是均值为0的高斯白噪声)，Rk+1为系统测量噪声矩阵协方差,本系统中陀螺仪测量的角速度作为预测值的控制量，光电编码器测量的角速度作为预测值，则融合卡尔曼滤波方程为

(6)

其中

(7)

式中:Qk为系统过程激励噪声矩阵协方差;Rk+1=0.1；P为误差协方差矩阵;K为卡尔曼增益矩阵;E为单位矩阵。

2.1.2 串级PID控制算法

扫地机器人的控制算法编写是实现其保持直线行走的核心问题，也是软件控制机器人的关键技术之一。本系统采用常用的离散型位置式PID算法和增量式PID[9]算法相结合，即用差分代替微分，用累加代替积分，实现串级PID控制与PWM信号的输出，其结构框图如图5所示，其中PID控制器的处理时间和卡尔曼滤波算法的步长为10 ms，光电编码器的采样时间为20 ms。

图5 系统串级PID控制算法框图

外环为机器人的姿态控制环，是基于位置式PID控制的比例、积分及微分控制，用来控制机器人的直线行走过程，利用卡尔曼滤波算法输出的机器人偏航角与期望偏航角进行PID运算后输出PWM信号的占空比，则有

PWM(k)=Kp·e(k)+Ki·∑e(k)+

Kd·[e(k)-e(k-1)]

(8)

式中：e(k)为当前偏航角偏差；e(k-1)为上一次偏航角偏差；PWM(k)为当前控制器输出的占空比。

内环为机器人的速度环，是基于离散式PID控制的比例、积分控制，对光电编码器输出的计数值经计算得到机器人的速度与期望机器人速度进行PI控制后得到的PWM占空比的增量值，则

ΔPWM(k)=K′p·[e′(k)-e′(k-1)]+

K′i·e′(k)

(9)

外环偏航角控制与内环速度控制相互耦合，将其融合后即可得到最终的当前PWM占空比为

PWMfinal(k)=PWM(k)+ΔPWM(k)

(10)

2.2 机器人路径规划算法

在扫地机器人开始移动前，以当前偏航角为参考方向，然后根据参考方向来调整扫地机的直线运动，当机器人左转时偏航角减小，右转时则增大。如果在直线行驶过程中偏航角发生了正向偏移，扫地机器人则应向左转，此时采用串级PID控制算法来调小左轮PWM占空比，同时增大右轮PWM占空比以实现方向的修正；如果偏航角发生了负向偏移，机器人则应向右转，此时PWM调整反向以确保扫地机保持当前路线。

机器人开机启动时即进入弓字形清扫方式[10]，由于机器人的两轮集成有光电编码器，所以可以通过编码器来获取里程信息，用于控制机器人的前进距离。扫地机的弓字形路径规划可分为4个部分：

1) 模式1。机器人向前直线至左右编码器计数值的和为60 000时(实际距离约为3 m)，扫地机转向标志位设为1，机器人左转90°。完成后，编码器计数值清0并重新计数，转向标志位被设置为0,同时转到模式2。

2) 模式2。机器人完成模式1后，将自动执行模式2。扫地机直线向前移动至左、右编码器计数值的和为1 800时(实际距离约为0.09 m)，扫地机器人转向标志位设为1，机器人左转90°。完成后，编码器计数值清0并重新计数，转向标志位被设置为0同时进入到模式3。

3) 模式3。模式3的实现过程与模式1基本相同。不同的是，模式3使机器人向右转，而不是向左转。当此过程完成时，机器人进入模式4。

4) 模式4。模式4的实现过程与模式2基本相同。不同的是，模式4使机器人向右转，而不是向左转。当此过程完成时，机器人重新启动循环。

3 实验

3.1 卡尔曼滤波融合算法仿真实验

为验证陀螺仪和编码器数据融合算法的有效性，利用Matlab中进行仿真实验。定义仿真初始变量X0|0=(0,0,0)T，误差协方差P0|0=zeros(3)，过程噪声协方差Q=diag(0.01，0.01，0.01)T，测量噪声协方差R=(0.01，0.01，0.01)。为了更好的模拟现实环境，编码器加入均值为0，标准差为理论值1%的高斯白噪声;陀螺仪加入均值为0，标准差为理论值1%的高斯白噪声。角速度仿真结果如图6所示。

图6 卡尔曼滤波误差补偿后角速度数据

由图6可知,未采用卡尔曼滤波前原始数据的均值为6.932×10-3(°)/s，方差为0.812 (°)2/s2；而经过卡尔曼滤波后数据的均值为5.627×10-4(°)/s，方差为5.132×10-2(°)2/s2。显然卡尔曼滤波算法对于陀螺仪与编码器的数据融合是有效的，能够补偿角度误差。

3.2 系统的串级PID参数整定

机器人系统采用串级PID控制算法，其PID参数需要多次实验采用试凑法进行参数整定，整定出的参数能够使机器人的速度稳定且直线行驶能力良好。先调节内环速度PID控制的参数，待速度稳定后再调节外环角度PID控制的参数，最终PID参数如表1所示。

表1 串级PID参数整定表

3.3 机器人路径规划测试实验

为了验证机器人的直线行驶能力，避障算法和路径保持的能力[11]，建立了扫地机器人测试平台，完成了相应的测试。

图7 机器人直行中偏航角和速度波形图(带干扰)

在直线行驶时间为900 s的过程中，通过蓝牙传输偏航角和速度数据至上位机(初始设定偏航角为0°，速度为0.2 m/s,采样时间为10 ms)，同时在4.60 s时加入瞬时干扰，使机器人的姿态角变为90°，且速度降为0(见图7)，经过实时对偏航角进行采样可知，在0～46.0 s时，机器人的当前偏航角波动在-0.4°～0.4°，而机器人的速度经过4 s后达到期望速度，且波动为-0.03～0.01 m/s，显然控制效果较好，满足直线行走要求；在46.0 s加入瞬时干扰后，经过串级PID算法调节后，机器人的偏航角经过2.01 s后稳定到初始角度，而机器人的速度经过1.35 s后进入稳态速度。

机器人的弓字形过程中偏航角的变化曲线如图8所示。实际路径轨迹用油墨印记法显示，如图9所示。

图8 弓字形清扫中偏航角变化曲线

图9 扫地机器人的弓字形路径轨迹

3.4 机器人系统性能指标

经过多次实验测试，本样机运行时各项性能指标如表2所示，与同类别的陀螺仪导航产品相比，本样机直线行走能力和对机器人速度的控制能力都优于同类产品。

表2 各类扫地机器人系统性能指标

4 结束语

本文设计了一个基于STM32F030R8T6微处理器和多种传感器的扫地机器人控制系统，采用卡尔曼滤波算法和串级PID算法对机器人的姿态和速度进行控制，利用弓字形算法实现了有规划式的清扫方式。仿真及实验证明，采用卡尔曼滤波算法能够有效地将陀螺仪和光电编码器的数据进行融合，消除传感器测量过程中的噪声影响和随机误差干扰，获得最优偏航角估计值。而采用串级PID控制算法能够将机器人的当前偏航角、当前速度值与期望值进行比较，得出误差信息后进行双闭环控制能有效地使机器人的速度稳定，同时维持相对平衡的姿态。最终机器人直线行驶的偏航角误差为[-0.4°，0.4°]，速度控制在10.2～12.6 m/min，通过弓字形路径规划完成了空间的全遍历。在以后的工作中将会进一步对扫地机器人的硬件和控制算法进行优化，如使用视觉SLAM技术，尽可能减少偏航误差。