基于多传感器融合信息的移动机器人速度控制方法

王昕煜，平雪良

（1.江南大学机械工程学院，江苏无锡214122；2.江苏省食品先进制造装备技术重点实验室，江苏无锡214122）

目前，移动机器人在特种作业、物流仓储和安防服务等行业的需求非常大，但其在复杂环境下的控制性能与适应能力仍面临巨大的挑战［1-2］。为提高移动机器人在复杂环境下完成指定任务的能力，须对其运动性能进行改善，这对其速度控制的精度与稳健性提出了较高的要求。基于轮式里程计信息的速度控制方法因受滑移程度不确定的影响，存在速度控制精度低和环境适应能力差的问题，导致移动机器人的速度控制性能难以提升［3］。

随着视觉位姿估计技术［4-6］和惯性导航技术［7-8］的日益发展，基于视觉-惯性信息融合原理的位姿估计方法［9-13］逐渐应用于移动机器人导航领域，其可有效改善动态环境下视觉位姿的估计性能和惯性导航数据的稳定性。例如：Qin等人［12］提出了基于滑动窗口原理的视觉信息与惯性信息非线性优化的方法，实现了精准、实时的视觉-惯性信息融合；Mur-artal 等人［13-14］提出了基于ORB（oriented FAST and rotated BRⅠEF）视觉特征的视觉-惯性位姿非线性优化估计方法。虽然基于非线性优化理论的视觉-惯性位姿估计方法在精度上明显优于单一传感器位姿估计方法，但移动机器人的运动常呈匀速状态且局限于平面，这会导致惯性测量单元（inertial measurement unit，ⅠMU）因缺乏激励而出现测量失真的问题，从而限制了视觉-惯性位姿估计方法对移动机器人运动的估计效果［15］，使得相关的运动估计信息难以在移动机器人速度控制中应用。

为提高移动机器人速度控制的精度与稳健性，笔者提出了一种基于多传感器融合信息的移动机器人速度控制方法。通过采用单目相机、轮式里程计和ⅠMU 来观测移动机器人的运动，并利用非线性优化方法来实现多传感器信息的融合，以提高移动机器人运动状态的估计精度。同时，将运动状态估计信息引入基于增量式PⅠD（proportion integration differentiation，比例积分微分）控制策略的移动机器人速度控制系统，以实现移动机器人速度控制精度与稳健性的提升。

1 移动机器人运动状态优化估计模型

1.1 运动状态非线性优化方法

图1 移动机器人运动信息观测模型Fig.1 Observation model of mobile robot motion information

为保证移动机器人运动状态的估计效率，在单目相机采集到的视觉测量帧中提取视觉关键帧并进行视觉运动观测。同时根据各视觉关键帧所在时刻下的轮式里程计关键帧和ⅠMU 关键帧，采用构建滑动窗口的方法对各视觉关键帧间各传感器的运动观测残差进行非线性优化，以实现对单目相机、轮式里程计及ⅠMU运动观测信息的优化融合。将滑动窗口内各ⅠMU 关键帧运动状态、观测偏差及视觉特征逆深度作为非线性优化变量，构建移动机器人运动状态优化向量χ，表示为：及滑动窗口先验残差rP的移动机器人运动观测残差优化模型，可表示为：

根据移动机器人运动观测残差优化模型，构建关于运动状态优化增量δχ的优化函数增量方程，对移动机器人运动状态优化向量χ中各优化变量进行最小二乘优化，以实现各视觉关键帧间移动机器人运动状态的优化估计。优化函数增量方程为：

1.2 视觉运动约束

视觉运动约束是指根据视觉运动观测信息对移动机器人多传感器信息融合构建的优化约束条件。本文将移动机器人运动过程中单目相机观测到的Harris角点作为视觉特征。单目相机提取到的室内、外环境视觉特征如图2所示。

构建单目相机视觉特征观测模型，表示为：

1.3 轮式里程计运动约束

移动机器人的运动状态可根据轮式里程计观测到的驱动轮转速信息与移动机器人结构参数来计算。轮式里程计观测到的运动信息与真实运动信息之间因滑移现象而存在误差。在移动机器人运动过程中，轮式里程计的运动观测信息为：

式中：v～、ψ～ 为轮式里程计的速度观测值和角速度观测值；v、ψ为速度真实值和角速度真实值；bv、bψ为轮式里程计的速度观测偏差和角速度观测偏差；nv、nψ为轮式里程计的速度观测噪声和角速度观测噪声。

根据相邻轮式里程计关键帧Ok与Ok+1间的轮式里程计运动观测信息，得到其对移动机器人运动状态的约束为：

1.4 IMU运动约束

ⅠMU运动约束是指根据ⅠMU在视觉关键帧间的运动观测信息移动机器人的运动状态进行约束。ⅠMU运动观测信息为：

式中：a～、ω～ 分别为ⅠMU 的加速度观测值和角速度观测值；a、ω分别为加速度真实值和角速度真实值；ba、bω为ⅠMU的加速度观测偏差和角速度观测偏差；na、nω为ⅠMU的加速度观测噪声和角速度观测噪声；RWB为ⅠMU 本体坐标系至世界坐标系W 的旋转矩阵；gW为世界坐标系W下的重力加速度。

根据ⅠMU 预积分模型理论［8］，相邻ⅠMU 关键帧Bk与Bk+1间ⅠMU运动观测信息对移动机器人运动状态的约束为：

2 移动机器人速度控制系统

2.1 速度控制系统框架

移动机器人速度控制是指通过对移动机器人各驱动电机的电压进行控制，以实现其在期望速度及角速度下的稳定运动。为保证移动机器人速度控制的精度，通过融合视觉信息、轮式里程计信息和ⅠMU信息，利用运动状态优化估计模型实现其运动状态的优化估计，并以此为基础构建速度控制系统框架，如图3所示。

图3 移动机器人速度控制系统框架Fig.3 Framework of mobile robot speed control system

从图3中可以看出，移动机器人速度控制系统根据运动状态优化估计模型中实时更新的优化变量，实现移动机器人运动速度的实时估计，并结合增量式PⅠD 控制策略，完成对移动机器人驱动电机的实时控制。

2.2 速度控制策略

为实现对移动机器人运动速度的精确、实时估计，制定如下速度控制策略：当移动机器人运动状态优化变量更新时，将优化更新的当前运动速度vWBt进行坐标变换，实现对移动机器人中心坐标系下运动速度的估计；当移动机器人运动状态优化变量未更新时，根据轮式里程计的当前观测速度vOt及上一次优化更新的轮式里程计速度观测偏差bvp来估计移动机器人的运动速度。在移动机器人中心坐标系下的估计速度ve可表示为：

基于轮式移动机器人运动学模型，根据移动机器人左、右侧驱动轮间距d以及估计速度ve的纵向分量vex与切向分量vey，可计算两侧驱动轮的估计速度；根据移动机器人的期望速度vt与期望角速度ωt，可计算驱动轮的目标速度。结合移动机器人驱动轮的实时估计速度和目标速度，计算得到其左、右侧驱动电机的速度控制误差el和er，分别为：

为降低运动速度优化变量对速度控制精度的影响，在对各驱动轮电机转速进行增量式PⅠD控制的过程中，对滑动窗口中运动速度优化变量的估计增量Δvs进行补偿。移动机器人速度控制策略为：

式中：Ph、Ph-1分别为当前速度控制时刻h与上一速度控制时刻（h-1）的速度控制输出；KP、KⅠ与KD分别为比例增益、积分增益与微分增益。

3 试验验证与结果分析

为验证基于多传感器融合信息的移动机器人速度控制系统的实际性能，搭建如图4所示的移动机器人速度控制试验平台，并开展移动机器人速度估计精度验证试验及速度控制精度验证试验。该试验平台配备的单目相机、轮式里程计与ⅠMU 的主要性能参数如表1所示。

图4 移动机器人速度控制试验平台Fig.4 Mobile robot speed control test platform

表1 试验平台中各传感器的主要性能参数Table1 Main performance parameters of each sensor in the test platform

在速度估计精度验证试验中，移动机器人在恒定速度激励下匀速运动。基于采集的试验数据，分别采用本文的速度估计方法、轮式里程计速度估计方法和ⅤⅠNS-Mono视觉惯性位姿估计方法［12］对移动机器人的实时运动速度进行估计。根据移动机器人的实际运动距离及运动时间，计算得到其平均运动速度为0.445 m/s。将0.445 m/s 作为参考速度，对各速度估计方法的精度进行评估。不同方法估计得到的移动机器人运动速度及其均方根误差（root mean squared error，RMSE）分别如图5和图6所示。

图5 不同方法估计的移动机器人运动速度对比Fig.5 Comparison of mobile robot motion speed estimated by different methods

图6 不同方法估计的移动机器人运动速度均方根误差对比Fig.6 Comparison of RMSE of mobile robot motion speed estimated by different methods

在速度控制精度验证试验中，在室内、外环境下采用不同速度控制方法（本文速度控制方法和传统的基于轮式里程计信息的速度控制方法）对移动机器人进行速度控制，并对不同速度控制方法的控制精度进行比较。在指定的控制时间tc内，移动机器人在不同期望线速度vt下作直线运动，根据实际运动距离计算移动机器人线速度的平均估计误差Δvc，结果如表2所示。在指定的控制时间tc内，移动机器人在相同期望线速度vt=0.5 m/s、不同期望角速度ωt下作圆周运动，根据实际运动角度计算移动机器人角速度的平均估计误差Δωc，结果如表3所示。

表2 基于不同速度控制方法的移动机器人线速度平均估计误差对比Table 2 Comparison of average linear speed estimation errors of mobile robot based on different speed control methods 单位：m/s

表3 基于不同速度控制方法的移动机器人角速度平均估计误差对比Table 3 Comparison of average angular velocity estimation errors of mobile robot based on different speed control methods 单位：rad/s

分析表2和表3可知：基于轮式里程计信息的速度控制方法的线速度估计误差与角速度估计误差均较大，且其控制精度随地面摩擦性质不同呈现较大的随机差异性；本文速度控制方法的速度估计过程虽受到各种环境因素的影响，但因采用了多传感器融合方法，使得在速度估计上具有较好的稳健性，在不同环境下仍具有较高的精度与适应能力，其控制精度较基于轮式里程计信息的速度控制方法有显著提高。

4 结 论

针对传统移动机器人速度控制方法控制精度低和环境适应能力差的问题，提出了一种基于多传感器融合信息的移动机器人速度控制方法。该方法采用非线性优化方法对移动机器人的视觉信息、轮式里程计信息及ⅠMU 信息进行优化融合，并结合运动状态估计信息与增量式PⅠD控制策略对移动机器人的速度进行控制。试验结果表明，相较于其他速度估计方法，本文速度估计方法有效减小了速度估计误差；相较于基于轮式里程计信息的速度控制方法，本文速度控制方法在控制精度和稳健性方面有较大提升。