第七章 智能机器人自主导航与路径规划

1. 主动灯塔法 主动灯塔是可以很可靠地被检测到的信号发射源，将该信 号进行最少的处理就可以提供精确的定位信息。 2. 路标导航定位法 路标导航定位法是利用环境中的路标，给移动机器人提供 位置信息。路标分为人工路标和自然路标。 3. 地图匹配法 基于地图的定位方法称为地图匹配法。机器人运用各种传 感器(如超声波传感器、激光测距仪、视觉系统等)探测环境 来创建它所处的局部环境地图，然后将此局部地图与存储在 机器人中的己知的全局地图进行匹配。如果匹配成功，机器 人就计算出自身在该环境中的位置。
7.1.1 导航系统分类 对于不同的室内与室外环境、结构化与非结构化环境，机 器人完成准确的自身定位后，常用的导航方式主要有磁导航、 惯性导航、视觉导航、卫星导航等。 1. 磁导航 磁导航是在路径上连续埋设多条引导电缆，分别流过不同 频率的电流，通过感应线圈对电流的检测来感知路径信息。 2. 惯性导航 惯性导航是利用陀螺仪和加速度计等惯性传感器测量移动 机器人的方位角和加速率，从而推知机器人当前位置和下一 步的目的地。 3. 视觉导航 依据环境空间的描述方式，可将移动机器人的视觉导航方 式划分为三类。
7.1.2 导航系统体系结构
智能机器人的导航系统是一个自主式智能系统，其主要任 务是如何把感知、规划、决策和行动等模块有机地结合起来。 下图给出了一种智能机器人自主导航系统的控制结构。
摄像头 红外测距 接受任务
( xc , yc ,c )
目标位姿
全局路径 规划
初始位姿
( x0 , y0 , 0 )
7.3.3基于概率的绝对定位
概率定位中最重要的马尔可夫定位和蒙特卡罗定位。马尔 可夫定位和蒙特卡罗定位不仅能够实现全局定位和局部位置 跟踪，而且能够解决机器人的“绑架”问题。 机器人“绑架”问题是指，由于机器人容易与外界发生碰 撞而使机器人在不知情(里程计没有记录)的情况下发生移动。
1. 马尔可夫定位（Markov Localization，ML） 尔可夫定位的基本思想是：机器人不知道它的确切位置， 而是知道它可能位置的信度（Belief，即机器人在整个位置 空间的概率分布，信度值之和为1）。马尔可夫定位的关键 之处在于信度值的计算。当机器人收到外界传感器信息或者 利用编码器获得机器人移动信息时，基于马尔可夫假设和贝 叶斯规则，每个栅格的信度值被更新。
测量位姿、速度
里程计
7.2 环境地图的表示
构造地图的目的是用于绝对坐标系下的位姿估计。地图的 表示方法通常有四种：拓扑图、特征图、网格图及直接表征 法(Appearance based methods)。不同方法具有各自的特点 和适用范围，其中特征图和网格图应用较为普遍。
7.2.1
拓扑图
1）基本思想 地铁、公交路线图均是典型的拓扑地图实例，其中停靠站 为节点，节点间的通道为边。在一般的办公环境中，拓扑单 元有走廊和房间等，而打印机、桌椅等则是功能单元。连接 器用于连接对应的位置，如门、楼梯、电梯等。 2）特点 拓扑图把环境建模为一张线图表示，忽略了具体的几何特征 信息，不必精确表示不同节点间的地理位置关系，图形抽象， 表示方便。
7.3 定位
定位是确定机器人在其作业环境中所处位置。机器人可以 利用先验环境地图信息、位姿的当前估计以及传感器的观测 值等输入信息，经过一定处理变换，获得更准确的当前位姿。 移动机器人定位方式有很多种，常用的可以采用里程计、 摄像机、激光雷达、声纳、速度或加速度计等。 从方法上来分，移动机器人定位可分为相对定位和绝对定 位两种。
第7章 智能机器人自主导航与路 径规划
7.1 概述
导航，最初是指对航海的船舶抵达目的地进行的导引过程。 这一术语和自主性相结合，已成为智能机器人研究的核心和 热点。 Leonard和Durrant-Whyte将移动机器人导航定义为三个子 问题： （1）“Where am I？” ——环境认知与机器人定位； （2）“Where am I going？”—— 目标识别； （3）“How do I get there？”——路径规划。 为完成导航，机器人需要依靠自身传感系统对内部姿态和 外部环境信息进行感知，通过对环境空间信息的存储、识别、 搜索等操作寻找最优或近似最优的无碰撞路径并实现安全运 动。
1）系统误差 系统误差在很长的时间内不会改变，和机器人导航的外界 环境并没有关系，主要由下列因素引起： （1）驱动轮直径不等； （2）驱动轮实际直径的均值和名义直径不等； （3）驱动轮轴心不重合； （4）驱动轮间轮距长度不确定； （5）有限的编码器测量精度； （6）有限的编码器采样频率。
2）非系统误差 非系统误差是在机器人和外界环境接触过程中，由于外界 环境不可预料特性引起的。主要误差来源如下： （1）轮子打滑； （2）地面不平； （3）地面有无法预料的物体（例如石块）； （4）外力作用和内力作用； （5）驱动轮和地板是面接触而不是点接触。
对于机器人定位来说，非系统误差是异常严重的问题，因 为它无法预测并导致严重的方向误差。 非系统误差包括方向误差和位置误差。考虑机器人的定位 误差时，方向误差是主要的误差源。机器人导航过程中小的 方向误差会导致严重的位置误差。 轮子打滑和地面不平都能导致严重的方向误差。在室内环 境中，轮子打滑对机器人定位精度的影响要比地面不平对定 位精度影响要大，因为轮子打滑发生的频率更高。
7.2.4 直接表征法 1）基本思想 通过记录来自不同位ห้องสมุดไป่ตู้及方向的环境外观感知数据，这些 图像中包括了某些坐标、几何特征或符号信息，利用这些数 据作为在这些位置处的环境特征描述。 直接表征法与识别拓扑位置所采用的方法原理上是一样的， 差别仅在于该法试图从所获取的传感器数据中创建一个函数 关系以便更精确地确定机器人的位姿。 2）特点 直接表征法数据存贮量大，环境噪声干扰严重，特征数据 的提取与匹配困难，其应用受到一定限制。
2. 蒙特卡罗定位（Monte-Carlo Localization，MCL） 基于马尔可夫定位方法，Dellaert等人提出了蒙特卡罗定 位方法。MCL 也称为粒子滤波（Particle Filter）。 MCL的主要思想是用N个带有权重的离散采样 来表示后验概率密度 。其中 是机器人在t时刻的一个可能状态； 是一个非负的参数称 为权重，表示t时刻机器人的状态为 的概率也就是 ，且 。
4. GPS定位 GPS是适用于室外移动机器人的一种全局定位系统，它是 一种以空间卫星为基础的高精度导航与定位系统，是由美国 国防部批准研制，为海、陆、空三军服务的一种新的军用卫 星导航系统，该系统由三大部分构成：GPS卫星星座(空间部 分)，地面监控部分(控制部分)和GPS信号接收机(用户部分)。 GPS系统能够实施全球性、全天候、实时连续的三维导航定 位服务。
（1）基于地图的导航：是完全依靠在移动机器人内部预先 保存好的关于环境的几何模型、拓扑地图等比较完整的信息， 在事先规划出的全局路线基础上，应用路径跟踪和避障技术 来实现的； （2）基于创建地图的导航：是利用各种传感器来创建关于 当前环境的几何模型或拓扑模型地图，然后利用这些模型来 实现导航； （3）无地图的导航：是在环境信息完全未知的情况下，可 通过摄像机或其他传感器对周围环境进行探测，利用对探测 的物体进行识别或跟踪来实现导航。 4．卫星导航 移动机器人通过安装卫星信号接收装置，可以实现自身定 位，无论其在室内还是室外。
7.2.2 特征图 1）基本思想 结构化环境中，最常见的特征是直线段、角、边等。 这些特征可用它们的颜色、长度、宽度、位置等参数表示。 基于特征的地图一般用式（7.1）的特征集合表示：
（7.1）
其中 是一个特征(边、线角等)，n是地图中的特征总 数。、
2）特点
特征法定位准确，模型易于由计算机描述和表示，参 数化特征也适用于路径规划和轨迹控制，但特征法需要 特征提取等预处理过程，对传感器噪声比较敏感，只适 于高度结构化环境。
3. 卡尔曼滤波定位（Kalman Filter， KF） kalman滤波器是一个最优化自回归数据处理算法。基本思 想是采用信号和噪声空间状态模型，结合当前时刻的观测值 和前一时刻的估计值来更新对状态变量的估计，从而得到当 前时刻的估计值。对于非线性估计问题，可以通过线性近似 去解决。相应的方法有EKF(Extended Kalman Filter)、UKF (Unscented Kalman Filter)等。
7.3.1相对定位
相对定位又称为局部位置跟踪，要求机器人在已知初始位 置的条件下通过测量机器人相对于初始位置的距离和方向来 确定当前位置，通常也称航迹推算法。 相对定位只适于短时短距离运动的位姿估计，长时间运动 时必须应用其它的传感器配合相关的定位算法进行校正。
1. 里程计法 里程计法是移动机器人定位技术中广泛采用的方法之一。 在移动机器人的车轮上安装光电编码器，通过编码器记录的 车轮转动圈数来计算机器人的位移和偏转角度。 里程计法定位过程中会产生两种误差。
3）误差补偿 机器人定位过程中，需要利用外界的传感器信息补偿误差。 因此利用外界传感器定位机器人时，主要任务在于如何提取 导航环境的特征并和环境地图进行匹配。在室内环境中，墙 壁、走廊、拐角、门等特征被广泛地用于机器人的定位研究。
2. 惯性导航定位法 惯性导航定位法是一种使用惯性导航传感器定位的方法。 它通常用陀螺仪来测量机器人的角速度，用加速度计测量机 器人的加速度。对测量结果进行一次和二次积分即可得到机 器人偏移的角度和位移，进而得出机器人当前的位置和姿态。
7.3.2 绝对定位
绝对定位又称为全局定位，要求机器人在未知初始位置的 情况下确定自己的位置。主要采用导航信标、主动或被动标 识、地图匹配、卫星导航技术或概率方法进行定位，定位精 度较高。这几种方法中，信标或标识牌的建设和维护成本较 高，地图匹配技术处理速度慢，GPS只能用于室外，目前精 度还很差，绝对定位的位置计算方法包括三视角法、三视距 法、模型匹配算法等。
传感器
超声测距
激光雷达 特征提取 构造地图
局部路径 规划
地图匹配 参考轨迹 生成器
(v1 , v2 ) ( xt , yt ,t , vt )
车体运动 控制器
u (v1 , v2 )
车体动力 学补偿
ˆ1 , y ˆ2 ,ˆ) (x
实时数据处理 （信息融合）
位姿估计
车体 运动学
( x, y , , v )
式中： 度)。
称为系统的运动模型(状态转移先验密
2)更新 通过观测模型利用新的观测信息更新系统的状态，即通过 如下公式计算后验概率密度 ： （7.3） 式中： 称为系统的观测模型(观测密度)。 当机器人获得编码器信息或者利用外界传感器感知环境后， 马尔可夫定位算法必须对所有的栅格进行计算，因此需要大 量的计算资源和内存，导致定位处理的实时性很差。
MCL包括4个阶段：初始化，采样阶段，权重归一化和输出 阶段。采样阶段是MCL的核心，它包括重采样、状态转移和 权重计算3步。实际上MCL是按照提议密度分布抽取采样，然 后利用权重来补偿提议密度分布与后验密度分布 之 间的差距。 当机器人发生“绑架”时，要估计的后验密度 与提 议密度分布的错位很大，在 取值较大区域的采样数 很少，需要大量的采样才能较好地估计后验密度。
7.2.3
网格图 1）基本思想 网格图把机器人的工作空间划分成网状结构，网格中的每 一单元代表环境的一部分，每一个单元都分配了一个概率值， 表示该单元被障碍物占据的可能性大小。 2）特点 网格法是一种近似描述，易于创建和维护，对某个网格的 感知信息可直接与环境中某个区域对应，机器人对所测得的 障碍物具体形状不太敏感，特别适于处理超声测量数据。但 当在大型环境中或网格单元划分比较细时，网格法计算量迅 速增长，需要大量内存单元，使计算机的实时处理变得很困 难。
根据初始状态概率分布 和观测数据 估计系统的当前状态 ，其中 表示机器人的位 姿(由位置和方向组成)。从统计学的观点看， 的估计是一 个贝叶斯滤波问题，可以通过估计后验密度分布 来 实现。贝叶斯滤波器假设系统是一个马尔可夫过程， 可以通过以下2步算得。
1)预测 通过运动模型预测系统在下一时刻的状态，即通过如下公 式计算先验概率密度 ： （7.2）