多房间、高精度的室内机器人SLMN导航算法

兰 勇, 马 力, 冯 晓, 夏国廷

(青岛大学 电气工程学院，山东 青岛 266071)

0 引 言

机器人在室内应达到在短时间内实现室内高效率打扫，减少重复清扫。传统的扫地机采用随机清扫模式，覆盖面积小、清扫效率低、易碰撞家具。采用z字型全覆盖算法可有效减少随机清扫带来的弊端，但前提是扫地机在室内能稳定走直线。mpu6050陀螺仪存在累计误差和漂移问题，利用PID控制的双轮也存在机械误差和打滑等问题，使得扫地机的位置和方向发生偏移，单独依靠陀螺仪无法实现正常全屋清扫。因此，研究了室内机器人SLMN导航算法。

文献[1]中提出了一种基于地图匹配的惯性导航轨迹推算算法(Improved Pedestrian Dead Reckoning, IPDR)。但是，由于传感器累计误差较大，导致其导航精确度不高。Dardari等[2-3]提出了Map Matching算法，与IPDR相比成本更低且算法也更加的高效。在此基础上，文献[4-5]中提出磁图匹配算法，但是该算法复杂度过高，精确度与之前相比并未得到明显改善且路径改变困难。文献[6-7]中采用基于滤波方案的贝叶斯算法(Bayesian Inference based Filtering Methods, BIFM)的WiFi和蓝牙方案，精确度得到改善，但WiFi路由器定位偏差依旧较大且成本较高。文献[8-9]中设备通过在一个预先确定的路径位置进行校准，但需要大量的数据集进行校准，很难进行管理，且校准结果依赖于定位精度，还需要精确的无线电地图。文献[10-11]中部分系统选择校准自由的方法(Calibration Free Approaches, CFA)，虽然成本得到了控制，但在递归式学习系统(Recursive Style Learning Systems, RSLS)中存在例如估计最大化(Estimation-maximization)或SLAM等不确定性和噪音。这些算法收敛速度比输入输出样式的学习算法慢，当初始误差或数据噪声太大时，可能收敛于一个错误的解决方案。文献[12-14]中提出基于WiFi的指纹定位系统，需要对信号强度进行采样，并提前构建无线电地图，可靠地使用WiFi覆盖信息和加速度计读数，但是WiFi的指纹识别需要大量的初始化工作，收益甚微。文献[15]中提出SLAM视觉导航，该方案灵活度高、可视化效果好，但是受环境光线的影响较大且位置的准确性也较为一般，由于引入了Kinect景深摄像机，成本也大大提高。

在保证高覆盖率的情况下充分考虑成本因素，本文选择了利用红外线和超声波辅助陀螺仪进行定位导航的方案：陀螺仪获取扫地机自身姿态信息执行z字清扫算法，再利用红外和超声波进行辅助定位和纠偏，效率提高近70.8%。与现有方案相比，该方案能够实现机器人直线运动、高精度且多房间导航。除此之外，该方案还具有成本低、不确定性和噪音低、易管理、灵活度高、可视化效果好等优点，并且在光线较暗的环境下依然可以高效工作。

1 机器人平台

为了验证导航算法，本文采用了智能扫地机和灯塔式导航仪的双设备导航平台。其中，智能扫地机选用了由意法半导体公司推出的基于Cotex-M3架构的32位微控制器STM32作为主控芯片；导航仪选用了美国德州仪器公司的msp430超低功耗单片机作为主控芯片。扫地机有两个由减速电动机控制的轮子，配备了测速片和光电码盘用来测定双轮速度，主板上配备了mpu6050六轴陀螺仪以解算自身姿态。扫地机和导航仪分别在顶部配置了红外收发模块进行通信，机身配备红外传感器进行直线校准、超声波距离感应器以获取两者之间的距离。如图1所示。

(a)(b)

图1 扫地机机器人(a)与导航仪(b)

2 室内机器人定位与导航

灯塔导航仪位于房间中间，定时发出基本不散射的红外信号，当扫地机检测到红外信号所在直线超声波时开始测得距离L1，同时开始记录轮子滚过的距离L0，一小段时间后再次用超声波测距，由于超声波测距范围为扇形，此时依然可以测得第2段距离L2，通过解算L0，L1，L2三边构成得三角形可以获得扫地机当前运行角度，与理论值进行比较后就可以纠正扫地机自身运行导致的偏差。如图2所示。

图2 机器人的定位与导航

2.1 保证走直线的方案设计

扫地机器人能够稳定执行算法的前提是扫地机器人的两个轮子本身能大致保持自身的直线运行。由于轮子的两个电动机电气特性不可能完全一致，同样的PWM占空比下，两个电动机的速度不会完全一致，长时间运行则误差不断累计，扫地机器人的运动会偏向一边。因此，需要对两边电动机的速度进行调整，使电动机转速一致，从而使机器人在轮子不打滑的情况下能够保持自身的直线运动。本文中在电动机后面安装两个26孔光电测速码盘，通过测量单位时间T内光电码盘产生的脉冲数N，结合轮子外圈的周长L，根据

v=(N/26)×L/T

(1)

就可以获得左右轮各自的前进速度v。

得到前进速度v后就可以利用经典自动控制领域中的PID控制算法对轮子的速度进行调节。考虑到公式转换可能增大计算量和产生误差，所以可以直接对光电码盘产生的脉冲数N进行PID调节。

位置型控制

(2)

增量型控制

Δu(n)=u(n)-u(n-1)=

(3)

式中：u(n)为PID输出值；e(n)=r(t)-c(t)，e(n)为偏差量，r(t)为设定目标值，c(t)=N为光电码盘当前测量值，即单位时间T内光电码盘产生脉冲数N,其与扫地机器人的两个轮子前进速度的关系为式(1)。

本文设计的机器人采用直流电动机，直接控制占空比以控制速度，采用增量型控制。递推公式为

u(n)=u(n-1)+Δu(n)=u(n-1)+

KPe(n-2)=u(n-1)+b0e(n)+

b1e(n-1)+b2e(n-2)

(4)

式中：u(n)是第n次给电动机PWM输出值；u(n-1)是第n-1次给电动机PWM输出值；e(n)，e(n-1)，e(n-2)分别是第n次、第n-1次、第n-2次目标速度与实际速度的偏差量；T为采样时间，一般为定值；KP，TI，TD为待整定的3个参数；

流程图如图3所示。图中：uL(n)，uR(n)，eL(n)，eR(n)分别是左右轮的PID输出量和偏差量。

图3 PID算法的实现流程图

通过PID算法，可以消除两轮的机械误差，尽可能保证机器人保持自身直线运行。

2.2 定位导航方案设计

机器人的两个后轮由独立的两个电动机驱动，虽然采用PID调节方式控制可保证两个轮子的速度相同，但由于打滑和机械误差，运行一定距离后机器人仍会发生偏移，无法保证直线运行。本文提出一种基于红外传感与定方向噪声波的角度和位置纠正的方法，如图4所示。

图4 基于红外传感与定方向噪声波的角度和位置纠正

导航仪放在房间一侧墙壁中部，垂直该侧墙壁发射红外线及两侧的扇形噪声波。机器人的运行角度总是与墙壁平行且从上到下、从右向左运行，机器人在房间的中间位置都可收到红外线和噪声波，结合机身装置判断自身的位置，若发现机身偏离直线，便可及时纠正。

基本工作原理：导航仪发射单束红外线做参考基准线，发射具有一定角度的噪声波起测距作用。根据参考基准线和距离，机器人可以测定位置。判定的原理如下：

步骤1机器人到达红外参考线后，测得与导航仪相距LOA，也即圆半径r。

步骤2机器人向前运行LAC后，再测出与导航仪之间的距离为LOC。

步骤3计算机器人水平姿态角。由几何关系，前行了LAC距离，机器人水平姿态角为

(5)

为提高测定精度，LAC一般较小，则LAB分别与LOA、LOB垂直，LBC=LOC-LOB=LOC-LOA。由几何关系得，β=arcsin(LBC/LAC)。则

(6)

相比上一算法降低了运算量，提高了运算速度，且在LAC较小的情况下满足实际运算精度。图5是偏移直线运行的角度为10°时的相对误差关系。

图5 偏移直线运行的角度为10°时的相对误差关系

由于式(5)和式(6)是反三角函数，可以直接考虑内部公式。将加法和减法的时间定义为t，乘法为10t，除法为20t。对于式(5)，整个等式的时间：

10t+10t+10t+2t+20t+10t+10t=72t

(7)

对于式(6)，整个等式的时间：

t+20t=21t

(8)

然后得到：

(9)

与式(5)、(6)相比，降低了计算复杂度，效率提高了近70.8%。

为了提高清洁工作的自动化程度，应使机器人可跨不同房间工作。其导航算法(见图6)步骤如下：

(1) 对多个房间进行编码，房间里的导航仪具有与房间编码一一对应的编码信号。

(2) 所在房间导航仪发出独特的编码信号。

(3) 机器人进入房间，机器人接收到信号后记录自身位置。

(4) 按照预定路线前进，到达两房间交界时可收到两个ID信号，则判断这两个房间相邻，并按序记录。

(5) 待完成本房间清扫后，调取按序记录的相邻房间编号，若无相邻房间则转到(6)；若有相邻房间则转到(3)。

(6) 若无上一级房间则转到(7)，若有则返回首次进入该房间时的上一级房间，并转到(3)。

(7) 机器人返回起始地点，此次清洁结束。

此处定义：机器人从该房间进入另一房间，则该房间为另一房间的上一级房间。

图6 多房间之间的导航

3 实验与仿真

在10.8 m×10 m室内环境中测试了机器人SLMN导航算法，机器人尺寸为0.8 m×0.4 m，如图7所示。

图7 机器人模型

在完成工作运行后，机器人局部轨迹如图8所示。红色和蓝色线分别表示机器人的真实轨迹和理想轨迹。因此可以发现当机器人由导航器通过一个红外线参考线(=5 m)，偏离轨迹将有效地纠正。

图8 机器人的局部轨迹

通过计算绝对值，可以创建此偏差的良好视图，出现的偏差可以得到有效的更正，如图9所示。

图9 偏差的变化趋势

成本和准确性是我们关注的主要问题，通过分析几个候选算法的导航问题，可以确定一个最有效的算法。本文将比较分析结果与其他算法进行了对比，如表1所示。可以看出，两个SLAM系统具有相似的精确度且是最高的。然而在相同的电池容量下它们的工作时间也是最短的，所以它需要更多的能量来保持工作。虽然惯性导航系统成本低，但其误差太大无法正常工作。因此与其他算法相比，本文提出的SLMN导航算法将机器人实现室内全覆盖导航成本降低14.3%，在满足实际应用中的能耗成本较低。

表1 测试结果

4 结 语

本文提出了一种高效的室内机器人导航方案，导航器和机器人之间的两个测量值和机器人运动距离可以形成一个三角形，然后通过解析几何原理求解正向角并校正前进速度方向。通过红外线校准以及超声波的两次测距构成三角形，利用余弦定理进行角度解算和纠偏，最后实现了室内机器人的高精度直线导航，将效率提高近70.8%，并在此基础上提出了多房间导航算法。经过仿真与实际测试，该算法可将机器人实现室内全覆盖导航成本降低14.3%，解决了算法过于复杂的问题且夜间也能良好工作，能够实现机器人直线运动、精度高且多房间导航。