基于融合定位的室外作业机器人实时全覆盖路径规划方法

本发明属于机器人，具体涉及室外作业机器人路径规划方法。

背景技术：

1、目前，以割草机器人、药物喷洒机器人和农业收割机器人等为代表，作业时需要对室外工作空间进行完整覆盖，移动机器人系统往往需要人工干预而无法实现自主运动控制，即相关机器人综合感知、运行速度有待提高。

2、传统装备制造及新型机器人公司都关注定位与实时运动规划的研发。出于成本考虑与技术门槛，主流的覆盖规划方法，主要在工作空间边界埋设电线、磁条或设立地标等，对边界进行识别，并主要通过在边界范围内随机游走或以某种固定形式移动实现对工作空间的全覆盖。这种通过埋线或地标实现的边界识别对于室外往往较大的工作区域而言往往成本较高，且管线存在老化现象，需要定期维护和更换，这对于用户而言并不友好。此外，由于没有对周围环境的感知模块，机器人往往只能随机游走并通过碰撞传感器实现对工作空间的覆盖，效率相对较低。

3、在同样需要用到全覆盖路径规划的扫地机器人产业由于竞争激烈，产品迭代十分迅速，目前的主打产品已经能够基于碰撞监测、红外、激光或超声雷达等传感器，或通过双目视觉或深度相机实现在室内场景的精确定位或构建地图，进而通过智能的实时路径规划实现全覆盖和避障功能。但是，由于室外场景纹理较少、室外的工作空间往往相对较大，进行全局建图和特征匹配对机器人的算力要求极高；室外场景纹理信息不够充足且光照变化比较明显，使得机器人的精确定位非常困难，机器人无法实现对环境和自身状态的准确感知，自然也无法据此实现实时全覆盖路径规划。因此，在扫地机器人中使用的定位和路径规划算法并不能简单地迁移到室外场景。

技术实现思路

1、本发明的目的在于提供一种安全可靠、路径优化的室外作业机器人实时全覆盖路径规划方法。

2、本发明提供的室外作业机器人实时全覆盖路径规划方法，包括采用gps、imu和双目相机三类传感器对机器人位置进行融合定位，提高系统整体的定位精度；根据双目立体视觉建图原理得到任意空间点相对于机器人本身的位置；将各个空间点以点云的形式整合，得到周围环境的点云地图，通过转换得到八叉树地图；采用基于分解的全覆盖路径规划算法进行全覆盖路径规划；最后通过pid和lqr等算法控制机器人沿着参考路径移动，实现对工作空间的全覆盖；其整体架构参见图1；具体步骤为：

3、步骤(一)采用三种传感器进行融合定位

4、具体地，考虑到室外环境的需求，机器人系统采用gps、imu和双目相机三类传感器进行融合定位。其中，双目相机能够实现对周围环境的感知，便于建立局部地图，同时还能将视频输入交付给语义处理模块，基于深度学习识别图像中的障碍物和动态物体，从而为后续的定位、路径规划和避障提供必要的信息。imu作为低成本的定位设备，能够在短时间内给出精度较高的位姿数据。gps定位精度为米级，但能够直接给出绝对位置，不会产生累计误差。

5、三类传感器的融合定位能够使不同传感器的优劣势互补，得到更高也更鲁棒的定位性能。双目视觉里程计可通过图像对周围环境建模并根据三角测量实现对自身位姿的估计，但精度易受光照和动态物体的影响，通过imu可以一定程度上弥补此类情况下的定位误差，这也是多传感器融合定位系统中非常经典的组合。双目相机和imu都通过计算不同采样点间的相对位姿来计算机器人的状态，但由于每次估计都存在一定误差，经过多次计算之后得到的绝对位姿将因为出现累计误差产生较大的偏移，而这可以通过gps给出的绝对位置进行纠正。gps根据设备成本的不同在室外环境可以实现厘米级到米级的定位精度，但是由于室外环境往往还存在遮挡和天气等因素的干扰，单纯的gps定位无法保证做到全程覆盖，且低成本的gps往往只能精确到米级，所以通过双目相机和imu的补充能够提高系统整体的定位精度。

6、通过三类传感器的组合，定位模块能够各取所长，以较低的成本实现相对较高的定位精度。下面将对三种传感器的测量模型进行简单介绍，并在之后给出本发明采用的低耦合融合定位方案。

7、(1)双目相机

8、视觉slam由于成本较激光雷达低且包含大量的语义信息，随着slam(实时定位与建图)的发展在学界的热度已经逐渐超越激光slam。近年来视觉slam方案不断迭代，并表现出不弱于激光雷达的精度和鲁棒性，而其中双目相机由于性能相对均衡，广泛地被各类移动机器人搭载。

9、通过对图像提取特征点，可以对相邻帧的特征点进行特征匹配，进而通过dlt(直接线性变换)等方法求解出相邻两帧间的相对位姿，多个帧间的相对位姿叠加，即可得到当前的绝对位姿，实现双目视觉定位。

10、(2)imu惯导

11、imu通常包含加速度计和陀螺仪，能够以较高的频率给出设备的平移加速度和旋转角速度，将每个帧测量得到的加速度和角速度乘以采样周期即可得到两个帧间的相对位姿。和双目相机类似，imu给出的帧间相对位姿往往具有较高的精度，但是由于长时间的累计对误差的放大效应，长时间运行之后得到的绝对位姿相对较高。

12、(3)gps卫星定位

13、gps是一种通过卫星信号确定地球上任意位置的定位系统。围绕地球轨道运行的卫星不断发射包含自身位置和时间戳信息的信号，接收器在接收到来自多个卫星的信号后即可根据卫星信号的传播时间以光速确定卫星的距离，通过接收至少来自三颗卫星的信号即可计算出接收器相对于各个卫星的相对位置。

14、由于gps给出的是绝对位置，不需要经过对相对位置的积分过程，因此并不存在累计误差。通过增加接收的频段，接收器能够实现更加精确的定位信息，但是此类高精度接收器受限于高成本并没有被包括割草机器人、药物喷洒机器人和收割机器人等为代表的需要对室外工作空间进行完整覆盖的移动机器人广泛采用—通过与imu和双目相机等低成本传感器进行融合定位，系统能够实现更高的定位精度和鲁棒性。

15、在通过以上定位方法获得来自三类传感器给出的位姿信息后，需要将数值不同的信息进行整合，本发明采用的是松耦合方案，亦即通过ekf(extended kalman filter,扩展卡尔曼滤波)实现对三个位姿信息的加权，得到更加精确的定位。ekf假设所有的测量都是以真值为均值的高斯分布，来自三个传感器的信息可以通过不同的计算方式得到对状态的估计，通过对三个估计的方差不断进行更新，可以对基于三个传感器的测量进行加权，从而得到理论上最优的估计值。关于ekf的详细迭代过程，可以参考图2所示。

16、具体而言，对于任意的移动机器人系统，其在时刻k处的状态可以用xk表示，通常xk为由机器人的空间坐标p＝(x,y,z)t、姿态o＝(r,p,y)t和速度v＝(vx,vy,vz)t构成的向量，亦即xk＝(p,o,v)t。在进行位姿估计之前，需要事先建立系统的运动模型xk＝f(xk-1,uk-1)，其中uk-1表示系统在k时刻的控制命令，通常为加速度a＝(ax,ay,az)和角加速度ω＝(ωr,ωp,ωy)，可以表示为uk-1＝(a,ω)t。

17、对于绝大多数移动机器人系统，运动模型xk＝f(xk-1,uk-1)并非线性的，因此需要线性化为xk＝axk-1+buk-1的形式。

18、ekf假设基于运动模型和观测模型得到的位姿信息都包含高斯噪声，于是对于包含l套传感器的系统，位姿估计可以用以下数学公式进行建模。

19、

20、

21、在根据上述过程完成对移动机器人系统的建模之后，不断根据此线性化的运动模型在上一帧的状态基础上预测当前时刻的状态xk和相应的协方差矩阵

22、pk：pk＝apk-1at+q                (2)

23、并通过测量值和线性模型给出的估计值计算时刻k下的卡尔曼增益kk：

24、kk＝pkht(hpkht+r)-1              (3)

25、最后通过卡尔曼增益分配来自不同传感器以及运动模型的观测值的权重得到当前的状态估计xk(其中表示基于运动模型得到的状态估计)：

26、

27、这里给出的预测和更新过程都仅限于单个传感器，在本系统中，由于主控能够同时接收到来自三个传感器的信息，因此需要建立三套预测模型，每当接收到来自某个传感器的信号输入时，就基于此输入和对应的预测模型对当前状态进行更新，最终实现融合定位。

28、步骤(二)局部建图，得到八叉树地图(octomap)；

29、为了实现对周围环境的感知，并进一步为路径规划服务，在完成对机器人自身的定位之后，还需要建立地图。对于室外场景，因为大范围的地图信息需要耗费大量的存储空间，如果要求机器人对整个工作空间建图将大大提高硬件成本，所以此处采用的是局部建图方案。

30、双目立体视觉建图的原理参见图3，由于双目相机视差的存在，同一个空间点在两个摄像头的成像位置是不一致的，据此可以计算出此空间点距离两个摄像头光心中点的距离。如图3所示，对于空间中的某个点p，其在双目相机的左右两个成像平面上投影得到的投影位置分别为pl和pr，对应的坐标为xl和xr，根据三角形的相似关系可以得到点p距离相机光心距离的计算公式为：

31、

32、由此即可得到任意空间点相对于机器人本身的位置，将各个空间点以点云的形式整合起来，得到周围环境的点云地图。在通过双目相机得到的稠密点云地图基础上，通过简单的转换得到八叉树地图(octomap)，见图4所示。具体而言，点云数据是在三维坐标系中的空间点集合，通过在空间中用一个尽量大的方格包含所有的空间点，之后再不断在已有方格的基础上划分为8个子方格(方格的最小大小可以手动指定，决定了八叉树地图的精度)，并根据方格内是否包含空间点确定此方格是否被占据，将每个被占据的方格保留即可得到八叉树地图。

33、步骤(三)全覆盖路径规划，即根据局部地图以及历史信息给出遍历工作空间的参考路径；

34、针对应用场景，实现全覆盖路径规划(cpp，coverage path planning)，亦即在工作空间中确定某个参考路径，使该路径在避开障碍物的情况下经过空间中的所有点。考虑到覆盖场景的通用性，本发明采用的cpp算法必须是实时的，即进行路径规划前并不存在先验的全局地图，而需要通过前端给出的局部地图进行规划。

35、八叉树地图能够用指定大小的小方格对周围的环境进行建模，据此机器人可以实时感知到附近的地形和障碍物信息，并在相对平坦的可通行区域进行路径规划，此时的问题就转化为二维栅格地图上的cpp问题。通常，cpp问题分为分解和遍历两个步骤，具体而言，是将目标空间划分为多个便于覆盖的子空间，之后用z字形或回字形路径分别进行覆盖。

36、目前主流的基于分解的全覆盖路径规划算法，包括梯形分解法、牛耕法和基于morse的分解方法[1]，其中梯形分解法和牛耕法都仅适用于障碍物为凸多边形的场景，而基于morse的分解方法则可以解决任意边界为可微曲线的障碍物的全覆盖路径规划问题。本发明即采用基于morse的分解方法，参见图5所示；

37、morse分解方法的核心在于morse函数的设计，通过morse曲线扫描整个目标空间，即可得到多个关键点(morse曲线与障碍物的切点)，在经过关键点时morse曲线将会被障碍物划分为多段，进而产生多个候选子区域(如图5中的单元2和单元3)，而离开障碍物时同样因为经过关键点多段morse曲线将合并为一段，完成子区域的生成并产生新的候选子区域(如图5中的单元4)。此处采用的是最简单也最常见的morse函数表示为：

38、h(x,y)＝x                                (6)其他可用的morse函数，如：

39、h(x,y)＝x2+y2                               (7)

40、对于边界不规则的情况，可以将机器人的运动拆分为垂直于扫描方向运动以及沿着边界运动两种固定模式。当路径覆盖过程中遇到边界或障碍物时即沿着边界或障碍物的边缘运动(直到运动距离在扫描方向上的投影长度等于预设的扫描间距)，如此，即实现对任意不规则场景的路径全覆盖。参见图6所示。

41、步骤(四)在基于上述方法得到参考路径之后，即可通过pid和lqr[2]等算法控制机器人沿着参考路径移动，实现对工作空间的全覆盖。

42、与现有技术相比，本发明优点

43、本发明通过使用双目相机以及imu和gps等传感器进行融合定位，实现室外场景的精确定位，并据此建立局部地图以便于进行实时全覆盖路径规划，能够很好地完成户外工作空间全覆盖任务。

44、本发明构建了以割草机器人、药物喷洒机器人和收割机器人等为代表的需要对室外工作空间进行完整覆盖的移动机器人的智能化方案，相较于目前以手动控制以及通过埋线或路标划定边界后随机游走等方法而言，能够极大地降低人工成本，并拥有较高的工作效率。