警用巡逻机器人导航系统设计及关键技术研究

■ 文/广州高新兴机器人有限公司 刘彪 柏林 周科

1 引言

警用巡逻机器人系统中，导航系统构建是一项核心技术，是赋予机器人感知和行动能力的关键，如何提高警用巡逻机器人的自主导航能力，对环境的适应能力是实现警用巡逻机器人在未知环境中能够可靠而灵活移动的关键问题。这就要求警用巡逻机器人必须解决环境模型/地图、环境感知和分析环境、在环境中的相对（或绝对）位置信息和规划并执行到达工作目的地等关键技术问题。

2警用巡逻机器人导航系统框架模型及软件架构设计

警用巡逻机器人的导航系统架构是指如何把感知、建模、规划、决策、行动等多种模块有机地结合起来，从而在动态环境中，完成目标任务的一个或多个机器人的结构框架。警用巡逻机器人由于所处环境的复杂和易变，既需要对周围环境的变化快速的实时响应，又需要按部就班的执行已经制定好的“巡逻任务”，既要保证对环境变化的敏感性又要确保目标的实现效率。

警用巡逻机器人导航软件架构决定了整个机器人导航系统的鲁棒性、适应性以及运行效率等。由于警用巡逻机器人的导航框架模型是基于理想情况下所做的数学分析模型，在警用巡逻机器人导航框架实施的过程中还需要考虑以下三个主要问题：环境的复杂性和环境模型的误差、环境的不可预知性、对环境感知不精确带来的不稳定性。

图1 警用巡逻机器人导航系统框架模型

对于警用巡逻机器人较为复杂环境下的应用，需要在较高级的决策层面获得较好的目的性和效率；在较低级的反应层面获得较好的环境适应能力、鲁棒性和实时性。因此，警用巡逻机器人导航架构采用四层架构，即用户层、自主规划决策层、行为层和执行控制层四个层次。用户层主要处理用户与机器人的交互；主要用于传递给用户必要的信息并接受用户的指令；自主规划决策层完成一些高层的自主决策；行为层包括避碰、保护、扰动、逃离等一些行为，可以不在上层的控制下自主执行。执行控制层则是把传感器的非符号化数据转变为符号化数据供上层读取，或者用自动控制理论和方法高速地控制执行器的运作。

3 警用巡逻机器人环境表达与建模

3.1环境模型

环境建模，是指根据已知的环境信息，通过提取和分析相关特征，将其转换成机器人可以理解的特征空间。目前，警用巡逻机器人采用以激光雷达为主，结合惯性导航及卫星导航等多种传感器的信息融合起来的Lidar SLAM，通过处理来自不同传感器的信息冗余、互补，构成一个覆盖一定空间和时间的检测系统。当前2D SLAM问题面临的主要难点有：控制量和观测量都存在误差、制图和定位两个部分相互依赖、闭环检测。

图2 警用巡逻机器人在示范场所巡逻的环境

除SLAM问题本身的难点外，在室外警用巡逻机器人应用SLAM还面临着一些特殊的问题：

1）动态环境：无人车行驶的环境中充满了行人、车辆等动态物体，而SLAM数学模型中定义的地图是静态的；

2）室外场景的结构性很差：相比于SLAM通常应用的室内场景，室外比较少有清晰的墙壁、走廊这些特征；

3）室外环境复杂：这点显而易见，想要从环境中提取有效的特征进而用于定位和建图是一件非常困难的事情。

3.2 激光SLAM建图

本文使用的警用巡逻机器人采用2D激光雷达SLAM方法构建二维地图，其方法是将SLAM问题转化为图模型，再将图模型的优化转化为非线性最小二乘问题，并在这个过程中引入了两种约束，它们分别是：

1）运动模型约束

2）观测模型约束

图3 SLAM地图创建过程运动模型约束与观测模型约束

其中红色的箭头对应了运动模型约束，它们是由警用巡逻机器人的位移产生的；黑色虚线对应了观测模型约束，他们是由外部传感器的观测所产生的。这些约束构成了警用巡逻机器人位姿节点xt之间的约束边，将它们综合到一起就得到优化目标函数：

调用Ceres、g2o、SPA等后端优化库进行优化，下图是采用此方案在示范应用场所创建的SLAM地图，其中左侧是卫星拍摄的中心花坛轮廓图，右图为2D激光雷达创建的地图，二者具有高度的相似性。

图4 卫星地图与激光雷达创建的地图对比

4 警用巡逻机器人自定位技术

4.1警用巡逻机器人的运动模型

机器人运动学模型是机器人运动分析、运行轨迹规划和控制实现的主要依据，借助于动力学模型可以通过仿真对机器人的运动动态性能进行分析。机器人机械系统的约束形式和机理千差万别，但是他们的共同本质是使系统的位置、速度等运动学要素必须满足一定的条件，这种条件可以描述为：

其中，为速度，I为系统所受约束的数目。警用巡逻机器人本体采用双轮差速驱动平台，在平面中建立平面全局参考坐标系和局部参考坐标系之间的关系。将平面上任意一个惯性基点定义为从某个原点O开始的全局参考坐标系，并定义机器人底盘上的任一点O（一般选两轮连线的中点）作为它的位置参考点。在全局参考坐标系上O的位置由坐标（x，y）确定，全局坐标与局部坐标之间的角度差设为θ，设计机器人的姿态描述为这三个元素的向量zg，用正交旋转矩阵来描述全局坐标系到局部参考坐标系的映射 。

可以使用正交旋转矩阵将全局参考坐标系的运动映射到局部参考坐标系中，这样给定在全局参考坐标系中的某个速度就可以得到机器人在局部参考坐标系的运动分量。设机器人沿局部参考坐标系+XR运动，此时右轮，左轮。得到差动驱动机器人的运动学方程：

设警用巡逻机器人的两轮之间的轴间距为d，轮半径为r，两轮连线的中点为M，其坐标为（xM，yM），两轮的旋转角速度分别为ωl，ωr，那么得到平台的运动学模型和约束方程：

4.2警用巡逻机器人自定位技术

警用巡逻机器人自定位与环境建模问题是紧密相关的。环境模型的准确性依赖于定位精度，而定位的实现又离不开环境模型。警用巡逻机器人的自定位问题可以从两方面得到解决：

1）位姿跟踪，在警用巡逻机器人初始位姿已知的情况下，利用自身携带的多种内部传感器，依据警用巡逻机器人的运动学模型通过测量相对于机器人初始位姿的距离和方向来确定当前的位姿；

2）全局搜索定位，如果机器人的初始位姿未知,或者在跟踪过程中出现了“绑架”现象，则机器人依靠内部传感器估计自身运动的同时，使用外部传感器感知环境，对获得的信息进行分析提取环境特征并保存，在下一步通过对环境特征的比较对自身位置进行校正。

5 全局路径规划和局部避障规划

5.1 全局路径动态规划

路径规划是警用巡逻机器人导航中最重要的任务之一，对警用巡逻机器人路径规划系统的主要要求是：

1）在环境地图中寻找一条路径，保证警用巡逻机器人沿该路径移动时不与外界发生碰撞；

2）能够处理用传感器感知的环境模型中的不确定因素和路径执行中出现的误差；

3）通过使警用巡逻机器人避开外界物体而使其对机器人传感器感知范围的影响降到最小；

4）能够按照需要找到最优路径。

常用的路径寻路算法包括两个大类：基于静态数据的寻路算法以及基于实时数据的寻路算法。静态数据寻路算法是以历史数据为基础，在地图网络环境不变的情况下，计算出最优路径的方法。静态数据寻路算法过程简单，但需要一次计算就可以得到最优路径，但是该算法缺乏实时性的信息采集和反馈，不适合应用于复杂多变的环境。

D＊（Dstar，Dynamic A＊）首先根据有限的地图信息和假定，将所有未知位置视为无障碍，进行从目标点到起始点的后向搜索（Backward Search）。搜索过程中维护和更新一些重要的信息，通过Backpointer记录路径，然后机器人开始沿此路径移动。D＊的主要缺点是大量的复杂运算和大量的数据维护，规划出来的路线沿障碍物边沿，以及由于地图像素、障碍物不规则以及地图膨胀等导致的规划路径不平滑等问题，本文从减少计算量和数据量，以及路线平滑角度着手对D＊算法进行优化：

1）通过网络的粗粒化减少有向图的顶点数量；

2）数据更新过程中，添加状态标志，当标志未发生变化时，数据不进行更新；

3）使用最小二乘法对规划路线进行曲线拟合平滑过滤掉不平滑或折线。

5.2 局部避障规划

机器人在获得目的地信息后，首先经过全局路径规划规划出一条大致可行的路线，然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略。本文使用的警用巡逻机器人采用局部避障的动态窗口法（Dynamic Window Approach, DWA）进行局部规划避障，其原理主要是在速度空间（v，w）中采样多组速度，并模拟这些速度在一定时间内的运动轨迹，再通过一个评价函数对这些轨迹进行评价，选取最优轨迹对应的速度来驱动机器人运动，其主要特征是依据警用巡逻机器人的加减速性能限定速度采样空间在一个可行的动态范围内。

1）警用巡逻机器人运动轨迹模型

假设警用巡逻机器人的轨迹是一段一段的圆弧或者直线（旋转速度为0时），一对（vt，ωt）就代表一个圆弧轨迹（假设不是全向运动机器人），它做圆弧运动的半径为，r=v/ω。当旋转速度ω不等于0时，机器人坐标为：

2）速度采样

确定警用巡逻机器人的轨迹模型后，根据速度就可以推算出轨迹。因此只需采样很多速度，就可以推算机器人轨迹，然后评价这些轨迹是否最优。在速度集合（vt，ωt）的空间中存在无穷多组速度集合，但受机器人本身限制和环境约束，可以采样的速度是有限的，在有限的集合中评价最优的轨迹是可能的，这些限制包括：

a）警用巡逻机器人受自身最大速度最小速度的限制；

b）警用巡逻机器人受电机性能的影响，由于电机力矩有限，存在最大的加减速限制，因此警用巡逻机器人轨迹前向模拟的周期内，存在一个动态窗口，在该窗口内的速度是机器人能够实际达到的速度；

c）基于警用巡逻机器人安全的考虑，为了能够在碰到障碍物前停下来，因此在最大减速条件下，速度有一个范围。

3）评价函数

当采样速度完成后，对每个速度所形成轨迹进行评价，采用的评价函数如下：

5.3 巡逻路线

机器人处于巡逻模式下其运动规划应涵盖所要工作的全部区域任务点，其可能存在运动路径起点既是终点的情况，因此单纯使用智能算法并不能完成全局巡逻规划，需要借助人工规划和人工示教相结合的方式才能生成包含全部任务点的巡逻路线，机器人的巡逻路线是一条闭合的路线，涵盖了所有需要巡逻的区域。

6 警用巡逻机器人轨迹跟踪

路径跟踪是要让机器人沿一条预先指定的路径移动，预先指定的路径即是前面的巡逻路线，警用巡逻机器人采用Pure Pursuit轨迹跟踪方法来完成巡逻路线的轨迹跟踪。Pure Pursuit是Follow the Carrot的改进版本，它们二者十分相似，主要区别在于Follow the Carrot方法是机器人沿直线趋向于Carrot点，而Pure Pursuit方法是以去圆弧的方式趋向于Carrot点。使得警用巡逻机器人所走的圆弧是唯一的，它保证警用巡逻机器人在每一时刻的前进方向都是圆弧的切线方向。这种沿圆弧运动的方式使得机器人的转向更加平滑，减少了震荡的可能性。

7结语

目前，根据已知地图的警用巡逻机器人导航研究己有许多成功实例，然而在大多数情况下，警用巡逻机器人所处的环境是未知和动态变化的，因而警用巡逻机器人在未知环境下SLAM则成为机器人自定位领域的热点。常用的SLAM技术主要有基于激光传感器的SLAM和基于视觉传感器的vSLAM。当前，vSLAM技术在理论上还不成熟，在实际应用中也存在很大困难，但为了使警用巡逻机器人具有更高的自主导航能力以及环境适应能力，解决vSLAM技术存在的疑难问题并使其成功地应用于警用巡逻机器人导航系统，这必然是未来警用巡逻机器人自定位的主流趋势之一。

[1] 郑宏，王景川，陈卫东.基于地图的移动机器人自定位与导航系统[J].机器人,2007,29(4),397-402.

[2] 刘彪.室外监控机器人的微小型组合导航系统设计[D].哈尔滨工程大学,2011.

[3] Newcombe, R.A., et al. KinectFusion∶ Realtime dense surface mapping and tracking.2011, IEEE. p. 127-136.

[4] Georg Klein and David Murray. Parallel Tracking and Mapping for Small AR Workspaces, In Proc. International Symposium on Mixed and Augmented Reality (ISMAR'07, Nara).

[5] D. Scaramuzza, F. Fraundorfer. Visual Odometry∶ Part I∶ The First 30 Years and Fundamentals[J].IEEE Robotics &Automation Magazine, Vol.18, issue 4, 2011.

[6]F. Fraundorfer and D. Scaramuzza.Visual Odometry ∶ Part II∶ Matching, Robustness,Optimization, and Applications[J]. IEEE Robotics & Automation Magazine, vol.19,no. 2, pp. 78-90, 2012.