基于激光与视觉融合的复合机器人高精度导航系统*

张智博,邹喜华,邓 果

(1.西南交通大学信息科学与技术学院,成都 611756;2.成都锦西数智科技有限公司,成都 611756)

0 引言

随着工业和制造业等领域的工作量和复杂程度逐渐上升,用机器人代替人工来完成更加精确繁重的重复性任务的趋势日益明显,并且对于机器人自主化的需求也更加迫切。相比于AGV(automated guided vehicle)和机械臂的单一功能,集成了两者特性的复合机器人则更具柔性化、自主化。此类机器人自主化和智能化的重要标志就是能够进行自主稳定的导航。导航涉及地图建立、定位以及路径规划等技术。机器人导航技术的发展经历了固定轨迹的磁条导航[1-2]和基于机器视觉的色带[3-5]、图案导航[6]、反光板导航[7-8]、惯性导航[9-10]、视觉导航[11]、能够灵活规划路径的激光导航[12-13],到目前的多传感器融合的导航方式[14-15],每种导航方式都有其优点。磁条、色带和图案等方式的导航轨迹固定,作业过程比较稳定,只需要考虑停障或避障问题,且系统较为简单,但是灵活性和工作效率相对较低,仅仅适用于少量特定的场合。目前应用最广泛的是SLAM技术,即利用激光雷达等传感器来构建完整的环境地图并用于自主导航。目前大多数成熟的机器人产品使用单线激光雷达导航的方式,但是由于信息源单一,随着机器人被逐渐应用在越来越复杂的场景,其定位精度不高且对于灵活多变的环境应对能力不足的缺点就凸显了出来,无法达到更加可靠、精准、稳定的使用要求。

本文针对复合机器人高精度自主导航定位的需求,设计了激光雷达导航与视觉高精度定位融合的导航定位系统,使最终的导航定位精度稳定在了10 mm以内。

1 复合机器人系统

1.1 软硬件系统框架

基于Ubuntu系统和ROS(robot operating system)平台,建立复合机器人系统框架。如图1所示,系统分硬件层和软件层两个部分。

图1 软硬件系统框架

机器人硬件方面采用两轮差速型底盘,英伟达NX工控机、倍加福单线激光雷达、览沃固态激光雷达、Realsense深度相机、六轴机械臂、IMU。机器人实物如图2所示。激光雷达、相机、IMU等传感器的数据在工控机中处理,工控机通过CAN通信方式给机器人底盘下发速度指令,机器人底盘控制板将工控机发下来的速度指令通过运动学模型分解为左右轮电机的转速,进而发送到相应的电机驱动器,电机驱动器利用PID算法控制电机的转速。同时,电机驱动器实时读取编码器的数据,解算出机器人实际的运动速度和运动里程,并按照特定格式反馈给工控机。工控机通过TCP通信的方式给机械臂发送移动指令,并接收机械臂反馈的结果,最终实现整个系统的正常运行。

图2 机器人实物图

软件部分主要包括激光雷达、IMU等传感器驱动程序、激光雷达导航、视觉高精度定位与机械臂抓取部分。在启动传感器驱动后首先加载目标点进行激光雷达导航,在到达指定目标点后执行视觉高精度定位,定位完成后导航系统控制机械臂模块对物品进行识别抓取,抓取完成转运至下一目标点并放置物品。

1.2 导航定位系统

导航定位系统包括激光雷达导航、视觉高精度定位部分,具体结构如图3所示。这里由于里程计的准确度密切影响激光雷达导航的重定位精度,且室内轮式机器人容易出现打滑现象,单独依靠编码器解算的轮式里程计容易引起较大误差,所以使用扩展卡尔曼滤波算法对轮式里程计与IMU进行融合来保证里程计的准确性[16]。另外复合机器人载有机械臂,比普通搬运机器人高出许多,单独使用二维激光雷达不足以躲避不规则动态障碍物。如图3所示,由于二维激光雷达的安装高度较低,只能检测到桌子的四条桌腿,而在桌面高度很容易与桌面发生碰撞。三维雷达就能够检测到桌子的全貌,所以增加固态三维雷达来提高避障的准确性。整个融合式导航定位系统的工作原理详见后文。

图3 导航定位系统框架

2 高精度自主导航定位系统

本文技术方案中的高精度自主导航定位系统分为两个阶段,第一阶段为激光雷达导航,主要作用为构建环境地图并根据实时定位以及扫描的障碍物信息自主规划路径并移动到目标点,且能够实时避障;第二阶段为视觉高精度定位,针对激光雷达导航精度不足的问题进行位姿调整,使最终定位精度达到10 mm以内。

2.1 激光雷达导航

激光雷达导航部分包括SLAM建图、重定位、路径规划与运动控制,根据ROS分布式架构,对激光雷达导航系统进行模块化开发。

2.1.1 SLAM建图

室内机器人执行导航任务的前提是有一个准确的地图,这里使用Cartographer来构建环境地图。Cartographer是一套基于图优化的算法,分前端和后端两个部分。算法以机器人各时刻的位姿为节点,以节点间的运动作为约束边来进行图优化,并且能够进行回环检测。

Cartographer前端引入子图(Submap)概念,负责子图的生成,并通过Scan-to-map激光帧与子图匹配代替常用的Scan-to-scan帧间匹配的方法,不断执行激光与子图匹配,当新的激光帧观测不到当前子图之外的新信息时再开始创建新的子图,然后由所有子图构成全局地图,过程如图4所示。所有激光帧对应的机器人全局位姿和所有子图对应的全局位姿通过Scan-to-map的匹配方式进行关联,从而构成位姿图[17]。

图4 Cartographer地图结构

随着地图规模逐渐增大,累积误差就会越来越大,这时引入回环检测,可以检测到机器人在此之前是否到过此地,即位姿闭环,能够在一定程度上消除累积误差。具体方式为将当前激光数据与所有子图进行匹配,若当前激光帧位姿和某个子图中的一个位姿距离较近,则将当前激光帧与这个子图进行匹配,若匹配程度复合要求,则认为成功检测到了回环。并且为了提高搜索效率,Cartographer采用了分支定界的方法,每来一个新的激光帧,都对其进行回环检测,若激光帧被确定为闭环,则将此闭环约束加到全局建图的约束中,对子图在世界坐标系下的位姿和激光帧在世界坐标系下的位姿和进行一次全局优化,最终得到优化后的全局地图。

2.1.2 重定位

基于已知地图的重定位使用AMCL(自适应蒙特卡洛)算法实现,其主要负责根据里程计、激光雷达信息来解算机器人在环境地图中的实时位姿。AMCL是一种基于粒子滤波并且增加自适应方法来对粒子进行采样的重定位算法,考虑到机器人在地图中的初始位姿是未知的,首先通过高斯随机采样的方式将粒子分布在整个地图中,然后通过状态转移方程即机器人运动学方程得到下一时刻粒子的建议分布,并将激光雷达观测信息带入粒子状态,从而得到每个粒子位置的观测信息。然后将观测信息与机器人所处的地图进行比较,并根据二者的匹配程度给每个粒子打分,添加权重。然后进行迭代,将匹配得分小于设定阈值的粒子丢弃,为保持粒子数量并将注意力转移到粒子得分较高的地方,按高斯分布在剩余粒子周围重新生成部分新的粒子,迭代完成后用粒子的权值表示状态变量的概率分布,即机器人最终的位姿。

2.1.3 路径规划与运动控制

机器人路径规划通常使用A*算法,其在地图中对于每个搜索到的节点通过式(1)所示的总代价估计函数式进行估计来得到最优位置,再从这个位置继续搜索,直到到达目标点[18]。总代价估计函数表达式为:

f(n)=g(n)+h(n)

(1)

式中:f(n)是节点n从起点到目标点的代价估计函数,g(n)是状态空间中从起始节点到当前节点的实际代价,h(n)是从当前节点到目标点的最佳路径的代价。

但是传统A*算法规划的路径不够平滑,会导致机器人频繁转向,所以这里使用三阶贝塞尔曲线对其进行改进。

由选取的n+1个控制点构成的n阶贝塞尔曲线的表达式可由式(2)表示:

(2)

式中:j为控制参数,Pi为控制点坐标,Bi,n(j)为伯恩斯坦多项式,其表达式为:

(3)

那么三阶贝塞尔曲线的参数方程可表示为:

P(j)=P0(1-j)3+3P1(1-j)2j+3P2(1-j)j2+P3j3

(4)

式中:Pi=(xi,yi)为控制点的坐标。

局部路径规划模块负责根据已有的全局路径规划出一段在机器人一定范围内的路径,计算并下发速度指令,即运动控制。使得机器人能够按照规划的路径运行,并实时地进行避障。本文采用DWA算法,其主要通过在速度空间(v,ω)中采样多组速度值,并模拟机器人在这些速度下一定时间内的轨迹[19],如图5所示,在得到多组轨迹后,使用式(5)所示的评价函数对这些轨迹进行评价,选取最优轨迹所对应的速度来驱动机器人运动。

图5 动态窗口采样轨迹

F(v,ω)=α·velocity(v,ω)+β·dist(v,ω)+γ·angle(v,ω)

(5)

式中:v为机器人的线速度,ω为机器人的角速度,velocity(v,ω)为用来评价模拟轨迹的机器人速度,dist(v,ω)用来评价此条模拟轨迹上与障碍物的距离,angle(v,ω)用来评价若机器人在以该速度行驶的情况下,模拟轨迹终点与目标点相差的角度,α、β、γ分别为3个函数对应的权重。

2.2 视觉高精度定位

本文选择二维码作为视觉标志物,根据二维码大小可得到二维码在相机坐标系下的实时位姿,经测试其测量误差在1 mm以内。二维码视觉高精度定位流程如图6所示,主要过程为通过相机实时检测二维码,并获取其姿态信息,然后转换为两坐标系之间的偏移旋转量,最后通过相机运动使二者重合,达到高精度定位的目的。

图6 二维码视觉高精度定位流程

相机调整完成后与二维码坐标系的相对位置关系如图7所示,上下分别为相机坐标系与二维码坐标系,二者坐标轴重合,表明定位成功。

图7 相机与二维码相对位置关系

2.3 激光雷达导航与视觉高精度定位融合

激光雷达导航受限于激光雷达精度、地图质量和机器人底盘硬件精度等多种因素,其定位精度一般在2～5 cm,有些甚至在5～10 cm范围,所以单独依赖激光雷达导航无法达到有些场景下10 mm的定位需求。本文将激光导航与二维码视觉高精度定位融合,第一阶段采用激光雷达导航,在自主导航至目标点附近后通过在目标点附近设置标志物再进行第二阶段的二维码视觉高精度定位,使定位精度达到10 mm以内。

具体实现方式如图8所示,由于机器人底盘高度原因,选择将相机安装到底盘正后方,使用相机检测二维码只能获取二维码与相机坐标系的相对位置,所以目标是在激光雷达导航完成后将相机坐标系与二维码坐标系尽量重合。其中XOY为二维码坐标系,L为机器人底盘坐标系与相机坐标系之间的距离,为固定值,相机坐标系原点在二维码坐标系下的坐标x,y和偏角θ已知,调整流程分旋转-平移-旋转3步,那么根据式(6)～式(8)即可求出机器人需要移动的距离和旋转角度。且为了便于控制旋转和平移量,使用IMU数据积分量作为反馈,从而构成闭环,待精度达到要求范围内结束调整,激光雷达导航与二维码视觉融合高精度导航定位的整个流程完成。

图8 融合定位流程

第一次旋转角度:

(6)

平移距离:

(7)

第二次旋转角度:

(8)

3 系统实现与功能测试

3.1 激光雷达导航系统测试

机器人硬件搭建完成后在ROS下进行软件部分的开发,ROS能够很好地支持Python、C++等编程语言进行开发,且提供了RVIZ等可视化工具,为机器人研究提供了很大便利。

使用Cartographer算法构建的实验所用环境地图与真实环境如图9所示。

(a) 环境地图 (b) 真实场景图9 环境地图与真实场景

进而,通过控制机器人运动记录下其在地图中几个点的坐标作为目标点,编写程序使其能够在目标点之间循环导航,通过ROS下的可视化工具RVIZ观察机器人运行情况,静态导航过程如图10所示。

图10 导航示意图

在两个导航点之间放置一把椅子作为动态障碍物,观察机器人的动态避障情况,如图11所示。

(a) 二维雷达下的障碍 (b) 三维雷达下的障碍

图11a中二维激光雷达仅能检测到椅子的四条腿,而图11b显示三维激光雷达点云能够检测到椅子全貌。机器人成功规划出了一条绕过障碍物的路径(弯曲线条),且机器人在运动过程中一直与障碍物保持着一定的距离。如图11d所示,到达目标点后激光轮廓依然与地图轮廓保持着较好的重合度,表明机器人定位准确。

3.2 激光雷达与二维码视觉融合导航定位测试

通过在一个目标点处地面上放置二维码,并在一阶段激光雷达导航完成后执行二维码视觉高精度定位模块进行位姿调整,测试定位精度。激光雷达导航结束与二维码视觉高精度定位结束后由相机检测数据计算得到的机器人定位精度对比如表1所示。由这些实验数据可得二维码视觉高精度定位后的平均精度为5.1 mm,且相较于一阶段激光雷达导航的精度提升了82.7%,达到了预期效果。

表1 二维码视觉高精度定位前后精度对比 (mm)

机器人导航至目标点并执行完视觉高精度定位后,既可执行对接转运,也可进行机械臂识别抓取。对自动抓取转运进行测试,如图12a所示,机械臂抓取成功并调整好姿态后,导航系统继续前往下一个目标点进行放置,如图12b所示。

(a) 抓取物品 (b) 放置物品图12 机械臂抓取与转运放置

4 结束语

本文主要研究了室内场景下复合机器人高精度导航定位问题,将激光雷达与视觉高精度定位融合形成两阶段导航系统,同时增加了三维激光雷达进行避障,有效提升了定位精度,并兼顾了稳定性与精确性。经实验证明设计的导航定位系统定位精度更高,且能够安全躲避不规则障碍物,为复合机器人自动搬运等应用场景提供了精确稳定的方案。