基于超声波传感器的巡检机器人导航纠偏研究

张磊，刘义亭，2*，陈光明，李佩娟

(1.南京工程学院，江苏南京，211167;2.东南大学，江苏南京，210096;3.南京农业大学，江苏南京，210031;4.江苏省智能化农业装备重点实验室，江苏南京，210031)

随着智慧农业系统地推进，人们可以使用各类传感器以及自动化技术对果蔬的培育采摘进行远程监测，像温度、二氧化碳浓度、肥料、水、通风等农作物生产环境因素都可以得到很好的控制。在室内农业中运用全过程智能化系统，对智慧农业的建设有着重要意义。导航定位[1]是巡检机器人完成室内农业中各种任务的基础，但在室内农业种植区的环境下，蔬菜架之间的环境极为相似，如采用单一激光雷达[2]信息作为里程计，则会遇到“长廊效应”的问题，导致巡检机器人导航[3]过程中的定位精度产生很大误差，出现与蔬菜架碰撞的可能。

目前，巡检机器人的导航方式主要有激光导航、视觉导航、磁条导航和惯性导航等。李照等[4]提出基于模糊纠偏控制的磁导航方式，对自动寻迹的控制方式进行优化，使得系统具有较强的抗干扰能力，但其导航过程中没有考虑定位精度因此纠偏误差较大。袁斌等[5]在惯性导航过程中对惯性元件得到的姿态进行卡尔曼最优估计，使用PID 控制调节自动导引运输车(automated guided vehicle，AGV)位姿，由于惯性元件存在误差累积，会影响到导航系统下的加速度值，AGV 长时间的运动导致定位产生很大的漂移。徐伟锋[6]提出一种基于RBF 神经网络的煤矿巡检机器人自主导航轨迹纠偏控制器，提升自主导航运行控制的快速响应性，但其导航过程采用单一里程计，长时间的累积误差无法更新。

针对上述方法存在的缺陷，为确保室内农业环境中巡检机器人自主导航[7]的平稳运行，提高定位精度，首先对巡检机器人的位姿信息进行扩展卡尔曼滤波[8]，将融合后的位姿用于其导航定位，之后将超声波传感器的量测数据结合融合后的位姿，根据巡检机器人与左右墙体之间的相对位置关系判断车体是否偏移，达到对导航轨迹纠偏[9]的目的。

该巡检机器人由硬件部分和远程控制部分组成。机器人硬件部分包括移动底盘，驱动模块，环境感知模块和运动控制模块，移动底盘采用两轮差速模型，驱动模块包括驱动电路以及两个带编码器的直流减速电机，环境感知模块搭载了TIM571激光雷达、TL720D 惯性测量单元和超声波传感器，运动控制模块包括GK5000工控机和STM32单片机控制板;远程控制部分为一台安装了ubuntu系统的计算机，通过局域网络发送给机器人终端移动控制命令。图1为巡检机器人的结构框架图。

图1 巡检机器人结构框架图Figure 1 Frame diagram of inspection robot structure

巡检机器人采用两轮差速底盘结构，控制板和驱动板之间采用串口通讯的方式，通过两个自带编码器的直流减速电机驱动。远程控制端将指令通过局域网传输给巡检机器人上的工控机，单片机控制板接收来自工控机的指令，并转换成PWM 信号给电机驱动板控制左右电机转动以使机器人完成移动的命令，到达指定位置。激光雷达用于巡检机器人感知环境周围的障碍物和构建地图，通过发射和接收激光束，分析激光遇到目标对象后的折返时间，计算出到目标对象的相对距离，并利用此过程中收集到的目标对象表面激光点的二维坐标、反射率和纹理等信息，快速得到出被测目标的几何构型。惯性测量单元是通过加速度计和陀螺仪测量运动载体方位与姿态的装置，由三个加速度计和三个陀螺仪组成，分别沿着空间三个相互垂直的方向轴。超声波传感器是将超声波信号转换成电信号的传感器，可以测量出运动载体当前部位与障碍物的距离。

巡检机器人自主导航过程中，需要借助自身携带的各种传感器确定自身的位姿，传感器主要用于感知外部环境[10]。姿态解算[11]是巡检机器人导航的基础，通过编码器可以获得轮式里程计的测量数据，由于室内农业地面凹凸不平以及车轮存在打滑，定位误差会随着移动距离增加，影响长期精度。惯性传感器积分获得速度与位置，但积分会造成误差的累积，因此无法单独用于精确定位。由于单一传感器估计存在累积误差[12]无法消除，长时间位置精度误差较大，因此本文利用扩展卡尔曼滤波进行多传感器数据融合[13]，提高巡检机器人的导航定位精度[14]。

2.1 机器人运动学模型

本文研究巡检机器人在二维平面内的导航姿态，图2为巡检机器人差速模型示意图，由于受所处环境以及传感器量测噪声[15]影响，其编码器所解算的位置与实际值存在误差，并且随着运行时间增加，累积的误差无法消除，需要对巡检机器人建立恰当的运动模型，可以更加准确的模拟两轮差速巡检机器人的运动过程。

图2 巡检机器人差速模型Figure 2 Differential velocity model of inspection robot

如图2所示，Vl和Vr分别代表巡检机器人左右轮速，Vx为巡检机器人导航正方向的速度，θ为巡检机器人导航方向与水平方向X轴的夹角，L为巡检机器人左右轮距。由于室内农业地面环境凹凸不平，巡检机器人左右两轮打滑的情况取决于车轮表面的摩擦系数以及地面的状况，因此假设巡检机器人在室内农业环境中以理想情况运行，车轮与地面不发生打滑的情况。巡检机器人的导航位姿包含Δx，Δy以及Δθ，根据两轮差速运动模型，可得其运动学公式如式(1)～式(5)所示。

式中:Δx——巡检机器人X方向上的位移量，m;

Δy——巡检机器人Y方向上的位移量，m;

Δθ——巡检机器人姿态角，rad;

Vθ——巡检机器人逆时针旋转的角速度，rad/s;

dt——两次计算的时间差，s。

两轮差速底盘由两个带有电机的动力轮分布于底盘左右两侧，左右两轮独立控制速度，巡检机器人在根据路径规划导航的过程中，通过对其左右车轮的速度控制可以实现正方向上前进速度以及转向角的改变，从而实现巡检机器人导航纠偏控制。其速度控制公式如式(6)、式(7)所示。

式中:Vx——巡检机器人正方向的前进速度，m/s;

L——巡检机器人左右轮距，m。

2.2 扩展卡尔曼滤波算法

由于巡检机器人在室内农业环境的导航过程中，单独依靠轮式里程计并不能做到精确定位以及精准控制纠偏角度，因此需要利用惯性测量单元得到的姿态角不断修正轮式里程计，减少累计角度偏差。简单的卡尔曼滤波通常用在线性高斯系统中，在假设高斯和线性情况下预测系统状态信息和观测模型，但现实中的巡检机器人由于在运行过程中受到噪声的影响，表现为非线性的运动模型和测量模型。扩展卡尔曼滤波正是通过局部线性解决非线性的问题，首先对非线性的预测方程和观测方程进行求导，利用泰勒级数展开将非线性函数近似线性化处理，然后采用卡尔曼滤波的框架进行滤波，完成对目标的状态估计。

当前状态的概率的概率分布是关于上一状态和将要执行的控制量的二元函数，再叠加一个高斯噪声，测量值同样是关于当前状态的函数叠加高斯噪声。系统可为非线性系统，扩展卡尔曼滤波假设系统状态转移概率和测量概率分别由非线性函数g和h控制，从而得到系统的状态预测方程和测量方程

式中:x t——t时刻的状态;

x t-1——t-1时刻的状态;

u t——t时刻的控制;

εt——均值为0，方差为Rt的运动噪声，Rt表示运动噪声从控制空间向状态空间的映射;

z t——t时刻的观测值;

δt——期望为0，方差为Qt的测量噪声。

扩展卡尔曼滤波利用一阶泰勒展开的方法对非线性函数作线性化处理，线性化通过一个在高斯函数的均值处与非线性函数相切的线性函数去近似。函数g的状态用其后验均值μt-1处的值近似，加μt-1和ut处与g相切的斜率做泰勒展开，将g线性化，得

式中:Gt——雅克比矩阵，n×n矩阵，它的值取决于ut和μt-1;

n——状态的维数。

对测量函数h线性化，在处进行泰勒展开，得到扩展卡尔曼滤波分为预测和更新的两个部分，主要由式(10)～式(14)组成。

状态预测

测量更新

K——卡尔曼增益，它表示对预测值的修正程度，卡尔曼增益越小，表示越相信运动模型，对预测均值的修正程度越小，反之亦然;

μt——所求t时刻状态的最优后验估计;

z t——真实的测量;

σt——后验状态的协方差;

I——单位矩阵。

扩展卡尔曼滤波的主要过程就是式(10)～式(14)不断迭代的过程，由上一时刻的后验状态bel(x t-1)和当前时刻的控制可以得到当前时刻的预测状态，再由当前时刻的测量去修正预测的状态，得到当前最终的后验估计bel(x t)，如此不断重复迭代。基于运动模型和测量模型，结合扩展卡尔曼滤波的实现过程，设计扩展卡尔曼滤波器估计机器人的位姿，扩展卡尔曼滤波的运动方程是非线性的。将轮式里程信息与惯性测量单元所得的巡检机器人姿态角做扩展卡尔曼滤波融合，初始化状态x0及协方差矩阵P0，根据控制参数进行状态更新，计算预测状态的均值

式中:v t——从控制空间到状态空间的转换需要的雅可比矩阵;

w t——圆弧模型的控制量;

θt-1——t-1时刻的姿态角。

式中:θzt——t时刻惯性测量单元的角度信息。

迭代计算预测状态的均值，得到对轮式里程计和惯性测量单元融合后的巡检机器人姿态角。用惯性测量单元修正轮式里程计的姿态角的结果如图3所示，实线表示轮式里程计的角度，其与真实角度值(虚线)存在一定误差，粗黑线表示经过扩展卡尔曼滤波融合后的姿态角，结果可见融合惯性测量单元信息有效减小了移动机器人工作过程中的角度误差。

图3 角度修正效果图Figure 3 Angle correction effect chart

2.3 自主导航纠偏控制

该巡检机器人导航纠偏方法是依据超声波传感器采集的与障碍物距离来确定巡检机器人在导航过程中的角度偏差，扩展卡尔曼滤波融合后的位姿为巡检机器人导航以及纠偏提供精确的定位信息。在巡检机器人的一侧前后等距分别安装超声波传感器，并配有激光雷达和惯性测量单元辅助纠偏，通过对左右轮速的控制达到巡检机器人姿态的改变，纠偏控制的流程图如图4所示。

图4 纠偏控制流程图Figure 4 Flow chart of rectification control

利用激光雷达采集巡检机器人周围的环境特征，通过上文的扩展卡尔曼滤波算法融合轮式里程计和惯性测量单元的数据，建立系统方程，获得一个初步融合后的预测的机器人位姿，实现巡检机器人的路径规划和自主避障。在巡检机器人的导航过程中判断左右两侧前后超声波与墙壁距离关系以及巡检机器人融合后的位姿。若左右两侧前后超声波传感器距离相等，则判断此时巡检机器人与左右障碍平面平行不需要纠偏;Vrz为巡检机器人的导航转弯速度，若左右两侧前后超声波传感器距离不相等，巡检机器人融合后的姿态角在1 s时间内的波动小于Vrz，则认为巡检机器人当前车体发生了偏移，暂停导航发布需要调整的角度给底盘，订阅调整后的位姿信息启动导航;若左右两侧前后超声波传感器距离不相等并且巡检机器人融合后的姿态角在1 s时间内的波动大于Vrz，判断车体当前正在转弯不需要纠偏。本次试验中Vrz设置为0.2 rad/s，即巡检机器人姿态角在1 s时间内的波动小于导航转弯速度，视为巡检机器人发生车体偏移的情况。

以巡检机器人车体前进方向和逆时针旋转为正方向，对于车体右方墙体，其纠偏控制的角度

式中:x1——巡检机器人车身右前侧超声波传感器距离右侧墙面的距离，m;

x2——巡检机器人车身右后侧超声波传感器距离右侧墙面的距离，m;

d1——右侧前后两超声波传感器的安装距离，m。

对于巡检机器人左方墙体，其纠偏控制的角度

式中:x3——巡检机器人车身左前侧超声波传感器距离左侧墙面的距离，m;

x4——巡检机器人车身左后侧超声波传感器距离左侧墙面的距离，m;

d2——左侧前后两超声波传感器的安装距离，m。

3.1 试验环境搭建

为了验证本文算法的性能，基于机器人操作系统在Gazebo仿真环境下对巡检机器人的本体和环境进行了仿真的搭建，如图5和图6所示。

图5 巡检机器人本体仿真的搭建Figure 5 Building of inspection robot body simulation

图6 仿真环境的搭建Figure 6 Building of simulation environment

图5中，选用了两轮差速模型作为巡检机器人的移动底盘，本体搭载了二维激光雷达和惯性测量单元，车体左侧一前一后相等距离分别安装了一个超声波传感器。其中，二维激光雷达获取巡检机器人外部环境特征用于仿真环境的建图和导航避障算法的实现，惯性测量单元可以实时提供巡检机器人的姿态角，并使用扩展卡尔曼滤波算法完成轮式里程计和姿态角的数据融合，使巡检机器人的导航过程中定位更精确，减小与墙体碰撞概率。超声波传感器可以获得巡检机器人当前位姿与墙壁的量测距离，从而判断巡检机器人的导航轨迹是否需要纠偏。图6为室内农业仿真环境的搭建，室内的中间为蔬菜架，房间的四周放置了三处堆放杂物的货架模拟真实环境，巡检机器人需要在与墙面平行的状态下完成对蔬菜架的拍摄，以监测蔬菜的生长环境。

本试验采用激光雷达导航的方式，使用Dijkstra算法对巡检机器人进行路径规划，从而完成起点到终点的导航过程。图7为导航环境的可视化界面，给定了一个经过转弯的目标点，在图中虚线线段为Dijkstra算法规划处的全局路径，在其导航过程中，使用本文算法判断巡检机器人与墙壁的平行关系，以达到纠偏的目的。在遇到转弯的情况下，监听惯性测量单元发布的姿态角变动范围控制巡检机器人底盘不做位姿的调整。

图7 导航轨迹Figure 7 Navigation track

3.2 试验数据及分析

对于巡检机器人在gazebo搭建的仿真环境导航，使用本文算法的效果如图8和图9所示，图8为巡检机器人左侧前后两个超声波在导航过程中与墙壁的距离，图中可以看出，1号区域的有两个时刻前后超声波的数据偏差很大，对应图7中的1号区域，这是由于巡检机器人导航过程中路径规划出的路径与墙面不平行，产生的车体转向，此时需要对车体进行纠偏的控制。2号区域有着很大的波动，并且前后两个超声波的数据一起下降，对应图7中的2号区域，这是在转弯的时候，墙角有障碍物的存在，虽然前后超声波的数据偏差很大，但是不需要对车体进行纠偏控制。

图8 左侧超声波数据Figure 8 Left ultrasonic data

图9为巡检机器人导航过程中使用扩展卡尔曼滤波融合后的姿态角，圆点为巡检机器人纠偏后的姿态角，1号区域中导航轨迹使得巡检机器人的车体发生了转向，使用本文算法成功判断并且对其进行了三次纠偏，使得车身与墙平行，车身初始角度为0 rad，三次纠偏后的车身角度分别为-0.022 6 rad，0.002 1 rad，-0.009 4 rad。2号区域为巡检机器人导航过程中转弯的过程，车体的姿态角变化较大，因此不对其进行纠偏处理，转弯后的车体朝向与转弯前的车体朝向发生改变，由0 rad变为-π/2 rad，在后续的导航过程中，经过了四次的纠偏，纠偏后的车身角度分别为-1.583 9 rad，-1.537 7 rad，-1.605 2 rad，-1.586 0 rad，车体对导航轨迹发生的转向进行了成功的纠偏。由此可得，本文基于超声波传感器的导航纠偏在不发生转弯的情况下平均误差为0.011 3 rad，在发生一次转弯的情况下纠偏的平均误差为0.023 9 rad。

图9 扩展卡尔曼滤波融合后的姿态角Figure 9 Attitude angle after extended Kalman filter fusion

首先对巡检机器人两轮差速模型进行运动学建模，使用扩展卡尔曼滤波算法对轮式里程计和惯性测量单元的偏航角进行数据融合，在此基础上将超声波的量测数据结合巡检机器人的位姿信息，比较导航过程中一侧前后超声波传感器与墙壁距离关系以及惯性测量单元所测得的偏航角变动幅度，从而根据巡检机器人与左右墙体之间的相对位置关系判断出是否需要纠偏，根据一侧前后超声波传感器的数据计算出需要纠偏的姿态角度。本试验在Gazebo上搭建巡检机器人和仿真环境，通过Matlab画出巡检机器人角度变动的曲线，根据仿真曲线对比分析本文纠偏算法的可行性。试验表明，本设计基于超声波传感器的导航纠偏在不发生转弯的情况下平均误差为0.011 3 rad，在发生一次转弯的情况下纠偏的平均误差为0.023 9 rad，可以极大减少巡检机器人在室内农业环境导航运行中发生碰撞的概率，在导航过程中实时检测巡检机器人与墙面的平行关系，有效提高巡检机器人的工作性能。

猜你喜欢里程计位姿卡尔曼滤波室内退化场景下UWB双基站辅助LiDAR里程计的定位方法导航定位学报(2022年5期)2022-10-13一种单目相机/三轴陀螺仪/里程计紧组合导航算法中国惯性技术学报(2019年1期)2019-05-21基于模板特征点提取的立体视觉里程计实现方法传感器与微系统(2018年7期)2018-08-29基于递推更新卡尔曼滤波的磁偶极子目标跟踪北京航空航天大学学报(2017年9期)2017-12-18基于共面直线迭代加权最小二乘的相机位姿估计光学精密工程(2016年5期)2016-11-07基于CAD模型的单目六自由度位姿测量光学精密工程(2016年4期)2016-11-07大角度斜置激光惯组与里程计组合导航方法导航定位与授时(2016年6期)2016-03-16小型四旋翼飞行器位姿建模及其仿真湖北工业大学学报(2016年5期)2016-02-27基于模糊卡尔曼滤波算法的动力电池SOC估计电源技术(2016年9期)2016-02-27基于扩展卡尔曼滤波的PMSM无位置传感器控制电源技术(2015年1期)2015-08-22