CN106406338A - 一种基于激光测距仪的全向移动机器人的自主导航装置及其方法 - Google Patents

Abstract

本发明涉及移动机器人的自主导航的技术领域，更具体地，涉及一种基于激光测距仪的全向移动机器人的自主导航装置及其方法。一种基于激光测距仪的全向移动机器人的自主导航装置，包括测量移动机器人轮子的移动距离的编码器、测量移动机器人的旋转角速度和加速度的惯性测量单元，编码器和惯性测量单元分别连接控制器，还包括激光测距仪，激光测距仪连接上位机，上位机连接控制器，控制器连接执行单元。本发明利用激光测距仪对移动机器人进行定位，同时建立环境的二维平面地图，并根据目标任务进行自主导航，能在行进过程中自主避障的方法。

Description

一种基于激光测距仪的全向移动机器人的自主导航装置及其 方法

技术领域

本发明涉及移动机器人的自主导航的技术领域，更具体地，涉及一种基于激光测距仪的全向移动机器人的自主导航装置及其方法。

背景技术

智能移动机器人能在未知或部分未知的环境中自主运动，具备环境感知、行为规划、动态决策、执行决策等功能。机器人在自主运动过程中通常需要解决三个问题，即机器人的定位问题、机器人的任务规划问题以及机器人的行为规划问题。移动机器人在未知环境中由于缺乏先验地图信息，需要通过车载激光测距仪、单目、双目摄像机等传感器获取周围的环境信息，以构建环境地图并估计机器人当前的位置，即同步定位与建图(Simultaneous Localization and Mapping，SLAM)。基于所构建的地图信息，根据目标任务规划机器人的可行路径；机器人在沿着所规划的路径行进的过程中需要准确的定位信息，并根据实时位置信息和环境信息实现动态的行为决策，完成目标任务。

SLAM分为定位和建图两个方面，是移动机器人在未知环境中实现自主导航的核心技术。传统的航位推测法利用车载里程计等信息对机器人进行定位，但里程计存在累积误差，其定位准确率随着机器人的移动而降低。当环境地图已知时，可以将观测到的环境信息与已知的地图信息进行对比，从而实时修正机器人的位姿估计，降低定位的累计误差。而SLAM考虑的是环境信息未知的情况，一方面机器人的位姿估计需要先验地图信息，另一方面，地图的建立需要准确的机器人位姿信息，定位和建图相辅相承、互相影响，准确的定位提供准确的地图，准确的地图提供准确的定位。目前，SLAM以基于卡尔曼滤波和基于粒子滤波的SLAM方法为主，但随着机器人的移动以及地图的扩大，卡尔曼滤波器需要维持庞大的状态变量矩阵，而粒子滤波器的粒子数目也需要增加，因此基于卡尔曼滤波和粒子滤波的SLAM算法的计算复杂度随着机器人的移动而增加。同时，当机器人的运动学模型和观测模型均为非线性时，尤其当无法获得准确的观测模型时，将影响基于卡尔曼滤波和粒子滤波SLAM算法的准确性。本发明考虑基于迭代最近点算法(ICP)的SLAM算法，由于激光测距仪得到的点云数目有限，使用点匹配迭代计算方法的计算开销是固定的，相比基于卡尔曼滤波和粒子滤波的SLAM算法，ICPSLAM算法的计算复杂度降低，具有较好的实时性。

全局路径规划算法主要包括模糊规划算法、蚁群优化算法、粒子群算法、A*算法等方法，这些方法通常需要在一个确定的空间内对障碍物进行确切的建模和描述。此外，在环境趋于复杂或动态变化时，如何使机器人的路径全局最优或次优，如何避免所规划路径的震荡和死锁，这些仍是有待解决的问题。快速扩展随机树(RRT)算法是基于采样的单查询运动规划方法，通过状态空间的随机采样点，把搜索导向空白区域，从而找到一条从起始点到目标点的可行路径，适合复杂环境和变化场景的路径规划。

目前避障算法常见的有可视图法、人工势场法、向量场直方图法。本发明考虑使用基于障碍区域的避障算法，当移动机器人的期望速度落在建立的障碍区域时，对期望速度进行修正，在保证朝目标点前进的同时通过最小的修正量避开障碍物。

发明内容

本发明为克服上述现有技术所述的至少一种缺陷，提供一种基于激光测距仪的全向移动机器人的自主导航装置及其方法，利用激光测距仪对移动机器人进行定位，同时建立环境的二维平面地图，并根据目标任务进行自主导航，能在行进过程中自主避障的方法。

为解决上述技术问题，本发明采用的技术方案是：一种基于激光测距仪的全向移动机器人的自主导航装置，其中，包括测量移动机器人轮子的移动距离的编码器、测量移动机器人的旋转角速度和加速度的惯性测量单元，编码器和惯性测量单元分别连接控制器，还包括激光测距仪，激光测距仪连接上位机，上位机连接控制器，控制器连接执行单元。

本发明以三轮全向移动机器人为机器人运动平台，利用车载编码器和惯性测量单元获取移动机器人的位置信息和姿态信息(简称位姿信息)，同时由车载激光测距仪获取移动机器人各方向距离最近的障碍物的点云信息。使用迭代最近点算法(ICP)将机器人当前的观测数据与之前的观测数据进行匹配，以修正移动机器人的位姿估计，同时建立环境的平面地图。在已知环境地图的情况下，设定目标任务点，使用快速搜索随机树算法(RRT)规划可行路径，对移动机器人进行导航控制。在自主导航的过程中，机器人根据实时激光数据检测障碍物，进行局部路径规划，实现自主避障。

本发明中，车载编码器测量移动机器人轮子的移动距离，惯性测量单元测量移动机器人的旋转角速度和加速度，车载嵌入式控制器获取编码器和惯性测量单元的测量值，结合移动机器人的运动学模型计算其位姿信息，发送给上位机***；上位机***同时获取激光测距仪的激光点云信息，经过算法计算得到移动机器人的运动速度的期望值(包括平移速度和旋转速度)，并将运动速度期望值发送给车载嵌入式控制器；嵌入式控制器通过移动机器人运动学模型反解出车轮的运动速度，再应用PID算法对车轮的转速进行闭环控制。

进一步的，移动机器人底盘设有三个全向轮。优选的，所述的三个全向轮按等边三角形分布。

利用所述的基于激光测距仪的全向移动机器人的自主导航装置的控制方法，其中，包括以下步骤：

S1.同步定位与地图构建；

S2.路径规划；

S3.位置跟踪与自主避障。

本装置包括同步定位与建图，路径规划与自主避障三个主要技术模块。同步定位与建图采用ICP-SLAM算法，首先利用移动机器人的里程计和惯性传感器，并结合机器人的运动学模型预估移动机器人的位姿信息，之后使用ICP算法对激光点云信息和地图信息进行对比，对位姿进行修正，并更新地图信息。全局路径规划模块是在地图和初始位置已知的情况下，设定目标位置，使用RRT算法，在保证机器人不与墙壁等障碍物碰撞的前提下，寻找优化的可行路径。自主避障模块使用ICP算法得到准确的定位信息，并根据规划的全局路径计算期望的运动速度，同时利用激光点云信息判断前进方向上的障碍物，通过局部路径规划对期望的机器人运动速度进行动态矫正以实现机器人在行进中的自主避障。

本发明针对移动机器人在未知室内环境中基于激光测距仪的自主导航问题提出了较为完整的解决方案，具体包括了同时定位与建图、路径规划、位置跟踪以及避障算法。首先，通过编码器和惯性测量单元获取移动机器人的位姿信息，利用激光测距仪获取环境信息，使用ICP-SLAM算法进行同步定位与建图。然后根据目标任务使用RRT算法规划全局路径，并根据移动机器人的实时位置和环境信息设计避障策略，作局部路径规划。

与现有技术相比，有益效果是：本发明设计了将当前获取的激光点云信息与已构建地图进行匹配的全局ICP算法，以及将当前与上一时刻的激光点云信息进行匹配的局部CP算法，并综合全局ICP算法与局部ICP算法对移动机器人的位姿信息进行修正，提高了位姿估计的准确性，同时降低了匹配失败的情况。

设计了包括内环和外环两个部分的位置跟踪算法，其中，内环使用编码器和惯性测量单元预估移动机器人的位姿信息；在外环部分，利用内环预估的位姿信息，并结合激光点云与已知地图，对机器人的位姿信息进行进一步修正。基于内环和外环的位置跟踪策略，在确保定位准确的同时，提高了***的实时性，避免由于计算耗时造成的控制延迟。

针对移动机器人与障碍物之间的距离的四种不同情况，分别设计了相应的避障策略，使得移动机器人在躲避障碍物的同时能够尽量沿着最短的路径前进。

附图说明

图1为移动机器人***框图。

图2为三轮全向移动机器人的运动学模型示意图。

图3为ICP-SLAM算法流程图。

图4为位置跟踪与自主避障算法流程图。

图5为避障算法示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

如图1所示，一种基于激光测距仪的全向移动机器人的自主导航装置，其中，包括测量移动机器人轮子的移动距离的编码器、测量移动机器人的旋转角速度和加速度的惯性测量单元，编码器和惯性测量单元分别连接控制器，还包括激光测距仪，激光测距仪连接上位机，上位机连接控制器，控制器连接执行单元。

本实施例中，车载编码器测量移动机器人轮子的移动距离，惯性测量单元测量移动机器人的旋转角速度和加速度，车载嵌入式控制器获取编码器和惯性测量单元的测量值，结合移动机器人的运动学模型计算其位姿信息，发送给上位机***；上位机***同时获取激光测距仪的激光点云信息，经过算法计算得到移动机器人的运动速度的期望值(包括平移速度和旋转速度)，并将运动速度期望值发送给车载嵌入式控制器；嵌入式控制器通过移动机器人运动学模型反解出车轮的运动速度，再应用PID算法对车轮的转速进行闭环控制。

如图2所示，移动机器人底盘由三个全向轮按等边三角形分布构成，通过控制三个全向轮的转速可以实现全向移动机器人在平面上360°全方位的平移以及旋转。全向移动机器人的运动分为旋转和平移两部分。令三个车轮的速度分别为(v₁,v₂,v₃)，移动机器人体坐标系沿x轴和y轴的移动速度为(v_x,v_y)。在只有平移没有旋转的情况下，全向移动机器人的运动学方程为：

当只存在旋转，没有平移的时候，旋转角速度为w，则有：

v₁＝v₂＝v₃＝L*w. (2)

当同时存在旋转和平移时，各全向轮的速度为平移和旋转运动的速度叠加。若已知各全向轮的移动速度，可推导移动机器人的平移速度和旋转速度。由于平移和旋转运动的强耦合性，在控制过程中需尽量让机器人不同时进行平移和旋转运动，以提高移动机器人位姿估计的准确性。

同步定位与地图构建模块如图3所示，根据机载里程计和惯性传感器获取移动机器人的移动信息，结合运动学模型预估移动机器人的位姿信息。在当前已有的地图中获取预估位置周围的点云信息，作为虚拟的模型点集，同时使用激光测距仪获取的当前环境真实的点云信息作为数据点集，两者进行ICP算法匹配，得到位姿信息全局矫正值。另一方面使用上一次获取的激光点云信息和当前获取的点云信息作ICP匹配得到位姿信息的局部矫正值。根据匹配的误差值对两组矫正值做加权得到最后的矫正位姿信息。同时更新地图信息，加入当前的激光观测值。

迭代最近点算法是同步定位与地图构建模块的核心算法，其通过迭代循环寻找数据点集跟模型点集的最近点，构成匹配对，并求解数据点集相对于与模型点集的旋转矩阵R_k和平移向量T_k，使得每次迭代中相似性度量S_LTF最小。ICP及其各种改进归类可归为以下几个步骤：1.选择数据点集P和模型点集M的子集。2.寻找匹配点集对。3给每个匹配对赋予权重。4.剔除某些匹配对。5寻找几何变换最小化相似性度量S_LTF。具体步骤如下:

1：初始化旋转矩阵R₀和平移向量T₀，随机选取或者通过其它条件获得。

2：对数据点集P的每个点都在模型点集中找到最近的一个点构成匹配对，同时去掉错误的匹配对，构成匹配点集对:

N＝{(i,j)|x_i∈P,y_j∈M,y_j＝argmin(||(R_k-₁x_i+T_k-₁-y_j)||²)}. (3)

匹配距离平方之和为

3：寻找最优的几何变换(即旋转矩阵R_k和平移向量T_k)使得变换后的数据点集P’和模型点集M距离平方之和(即相似性度量)最小：

4：若达到满足停止条件或者迭代次数达到上限值匹配结束，否则返回步骤2。ICP算法的停止条件有两种：(1)标准差e＝S_LTS/|N_p|足够小；(2)前后两次的标准差变化|e-e'|/e足够小。

在步骤2中，寻找最近点的方法常用的有基于k-d树和基于Delaunary三角划分的最近点搜索算法。实验研究结果表明，k-d树适合高维点集的点搜索，而基于Delaunay三角化算法更适合于低维点集，故本发明考虑使用基于Delaunary三角划分的最近点搜索算法。该算法将散点集合划分成不均匀的三角形网格，且网格满足两个基本准则，其一是空圆特性，即在Delaunay三角形网中任一个三角形网格的外接圆范围内不会有其它点存在。其二是最大化最小角特性，即在散点集可能形成的三角剖分中，Delaunay三角剖分所形成的三角形的最小角最大。在ICP算法中，首先将模型点集M进行Delaunary三角划分，然后采用三角划分搜索策略找到数据点集P在模型点集中的最近点作为对应点，构成匹配对。

理想情况下，数据点集P和模型点集M能够完全匹配，若数据点集中一点x_p在模型点集M中找到对应点y_pm，则模型点集中y_pm在数据点集中找到的对应点也应该是x_p。可依此作为匹配对的评判准则。令数据点集中P_x1在模型点集中的对应点为M_y1，模型点集中M_y1的在数据点集的对应点为P_x2，P_x2的对应点为M_y2。当满足下列关系是，我们认为对应点的寻找是正确的，否则剔除该对应关系。

其中ε某个跟标准差相关的常数。

步骤3中的寻找最佳的几何变换常用方法有SVD奇异值分解，四元组，正交矩阵，双四元组方法等，本方案采用的是SVD分解的方法。

在同步定位与建图过程中，将预估的移动机器人的位姿信息作为ICP算法的初始估计，估计值与真值只有微小误差，可大大减小ICP算法的迭代次数，提高收敛速度。同时使用已有的地图上预估的移动机器人位置周围的点云信息作为模型点集，进一步减少模型点集的大小，提高ICP算法计算速度。

在已知环境地图和初始位置P_s的情况下，设定目标位置P_e，使用快速扩展随机树(RRT)算法进行全局路径规划。RRT算法是以状态空间中的一个初始点作为根节点，通过随机采样扩展的方式，逐渐增加叶节点，最终生成一棵随机扩展树。当随机树的叶节点中包含了目标点或目标区域中的点时，从初始点到目标点之间的一条以随机树的叶节点组成的路径就是规划路径。规划路径以一系列的离散节点形式给出，相邻节点间不存在障碍物，可根据规划的路径直接到达目标位置点。考虑到在实际环境中，由于移动机器人运动惯性以及未知障碍物的存在，移动机器人不能严格按照规划的路径运动。在以节点形式给出的规划路径中，移动机器人可将一个个节点作为子运动目标，根据实际的情况进行局部路径规划，减少由于偏离轨迹或未知障碍物等原因而需要重新规划全局路径的情况，提高了导航过程的鲁棒性。

考虑到移动机器人的体积大小，使用RRT算法寻找路径前先对环境地图进行膨胀处理，膨胀的区域称为碰撞区域S，从而将移动机器人的体积转移到障碍物中。在RRT算法过程中，将移动机器人看为质点处理，当代表移动机器人的质点位于碰撞区域时，在实际环境中，移动机器人已经与障碍物发生碰撞。

RRT算法假设搜索树为T，当前节点数目为N，其节点为ti_j(t_j)，j为当前节点编号，i为其父节点编号。以初始位置作为搜索树的根节点t₀₁，地图上两点构成的线段为L(p₁,p₂)。在地图空白区域产生随机点p(即当移动机器人位于随机点时不碰撞障碍物)。在目前已有的随机树中找到跟随机点最近的可直接相通的节点p_k，即节点和随机点之间没有障碍物遮挡且不存在碰撞区域。

将该节点作为父节点，沿父节点和随机点的连线方向扩展搜索树，产生新的子节点t_k(N+1)，且子节点和父节点的距离为步长s。

t_k(N+1)＝{t|t∈L(t_k,p),||t-t_k||＝s}. (9)

不断产生随机点扩展随机树，直到随机树的子节点到达目标点，从该节点及其父节点回溯即为找到的路径path＝(p₁,p₂,...,p_n),n为路径的节点数目。

通过RRT算法得到以一系列节点构成的路径，但往往不是最优路径，不相邻的节点存在直接到达的可能，可对其进一步优化。优化步骤如下：

从初始节点开始，判断节点i和节点i+2是否相通，即移动机器人可否直接从节点i直线运动到节点i+2；若可以则去掉节点i+1，下一步判断节点i和节点i+3；若不能直接相通，下一步判断节点i+1和节点i+3。以此类推。类似的也可同时从目标点位置开始判断。

位置跟踪与自主避障，该模块包括位置跟踪和避障两部分。位置跟踪得到准确的位姿信息作为避障算法的输入，避障算法依据规划路径和当前的位姿信息计算期望的移动速度，并结合激光传感器信息对期望速度进行修正，最后将修正的运动速度向移动机器人发送。如图4所示。

跟踪定位过程与定位建图过程类似，使用里程计和惯性传感器预估移动机器人的位姿，使用ICP算法对位姿信息进行修正。由于激光信息的获取频率最高只有10HZ，ICP算法在最差的情况计算频率为3HZ。为了保证对机器人的实时控制，设计了内环和外环两部分的跟踪算法。内环部分将航位推测法预估的位姿信息作为避障算法部分的输入，控制频率为15Hz，外环使用ICP算法对位姿进行修正，将修正的位姿信息作为避障算法部分的输入，最低控制频率为3HZ。如图3所示，在t时刻得到了移动机器人的预估位姿，包括世界坐标下x轴和y轴的位置以及移动机器人的偏航角X_t＝(x_t,y_t,α_t)，同时将获取的激光信息与已知地图的信息进行ICP算法匹配，此时将预估位姿X_t作为避障算法的输入。在t+1,t+2,…,t+k-1时刻，将当前时刻预估的位姿信息X_t+1,X_t+2,...,X_t+k-1作为避障算法的输入。在t+k时刻预估得到的移动机器人的位姿为X_t+k，此时t时刻开始进行运算的ICP算法完成匹配，得到t时刻的修正位姿考虑激光测距仪的采样频率，我们设定t时刻和t+k时刻的时间间隔需大于0.1s。由此移动机器人的修正位姿为：

我们将修正的位姿作为避障算法的输入，同时获取t+k时刻的激光信息，按照上述步骤再次进行ICP算法的匹配以修正机器人的位姿信息。

如图4所示，在获取了移动机器人的位姿信息后，避障算法依据实时环境信息作局部路径规划。首先确定当前的运动目标，由于规划路径以节点的形式给出，每一个节点即为一个子运动目标，当到达当前目标节点附近或判断移动机器人位置可越过当前目标节点沿直线运动到下一节点时，则作目标节点切换，也即将下一个路径节点作为目标节点。之后根据当前位置和目标节点位置计算移动机器人的期望运动方向，利用避障算法对运动方向进行修正，并将计算的平移与旋转速度发送给移动机器人控制其运动。

RRT算法规划了一条可行路径，但在实际控制过程中，由于移动机器人的运动学特性以及环境中存在未知障碍物等原因，移动机器人并不会完全按照预期的路径运动，需要根据当前的实时位姿信息、移动速度以及环境信息进行局部路径规划，以避开障碍物同时向目标靠近。

局部路径规划算法如图5所示。安全距离设定为d。令当前位置为S，目标位置为E，理想的运动方向为V_SE。利用上述信息预测下一个控制周期内的预测位置P。将避障过程分为以下四种情况：

(1)图5(a)所示，障碍物中与当前位置S距离最近的点为O1，如果两者距离SO1小于d，则表明移动机器人即将发生碰撞，此时不考虑目标点的位置，移动机器人向远离点O1的方向运动。

(2)若SO1大于d，则找到距离预测位置P最近的障碍物点O，以点O为圆心，d为半径建立障碍区域VO，即图5(b)中的阴影部分。若期望的运动方向V_SE没有落在该区域内，则表明机器人将不会跟该点碰撞，可以保持期望速度不变。

(3)如图5(c)所示，若V_SE落在障碍区域VO区间内，在有限时间内，移动机器人将发生碰撞，此时需要对速度方向进行修正。此时若点P位于障碍区域之外，寻找点S与圆VO的切点T，则最终的运动方向将为切线方向：

(4)如图5(d)所示，若点P位于障碍区域VO之内，此时速度V_SE的修正量为方向，修正后的速度为：

由于使用的激光测距仪的测量范围为240°，当机器人最终前进方向与当前方向相差较大时，令其做旋转运动，以使移动机器人能够充分测量到前进方向的环境信息，避免因检测不到障碍物而发生碰撞。在其余的情况下，控制移动机器人仅做平移运动，避免因同时发生旋转和平移运动而导致定位误差增加。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (9)

1.一种基于激光测距仪的全向移动机器人的自主导航装置，其特征在于，包括测量移动机器人轮子的移动距离的编码器、测量移动机器人的旋转角速度和加速度的惯性测量单元，编码器和惯性测量单元分别连接控制器，还包括激光测距仪，激光测距仪连接上位机，上位机连接控制器，控制器连接执行单元。

2.根据权利要求1所述的一种基于激光测距仪的全向移动机器人的自主导航装置，其特征在于：移动机器人底盘设有三个全向轮。

3.根据权利要求2所述的一种基于激光测距仪的全向移动机器人的自主导航装置，其特征在于：所述的三个全向轮按等边三角形分布。

4.利用权利要求3所述的基于激光测距仪的全向移动机器人的自主导航装置的控制方法，其特征在于，包括以下步骤：

S1.同步定位与地图构建；

S2.路径规划；

S3.位置跟踪与自主避障。

5.根据权利要求4所述的基于激光测距仪的全向移动机器人的自主导航装置的控制方法，其特征在于：所述的步骤S1中，根据编码器和惯性测量单元获取移动机器人的移动信息，结合运动学模型预估移动机器人的位姿信息；在当前已有的地图中获取预估位置周围的点云信息，作为虚拟的模型点集，同时使用激光测距仪获取的当前环境真实的点云信息作为数据点集，两者进行ICP算法匹配，得到位姿信息全局矫正值；另一方面使用上一次获取的激光点云信息和当前获取的点云信息作ICP匹配得到位姿信息的局部矫正值；根据匹配的误差值对两组矫正值做加权得到最后的矫正位姿信息；同时更新地图信息，加入当前的激光观测值。

6.根据权利要求5所述的基于激光测距仪的全向移动机器人的自主导航装置的控制方法，其特征在于：所述的步骤S1中，

迭代最近点算法是同步定位与地图构建模块的核心算法，其通过迭代循环寻找数据点集跟模型点集的最近点，构成匹配对，并求解数据点集相对于与模型点集的旋转矩阵R_k和平移向量T_k，使得每次迭代中相似性度量S_LTF最小；ICP及其各种改进归类可归为以下几个步骤：1.选择数据点集P和模型点集M的子集；2.寻找匹配点集对；3给每个匹配对赋予权重；4.剔除某些匹配对；5寻找几何变换最小化相似性度量S_LTF；具体步骤如下：

1：初始化旋转矩阵R₀和平移向量T₀，随机选取或者通过其它条件获得；

2：对数据点集P的每个点都在模型点集中找到最近的一个点构成匹配对，同时去掉错误的匹配对，构成匹配点集对:

N＝{(i,j)|x_i∈P,y_j∈M,y_j＝argmin(||(R_k-1x_i+T_k-1-y_j)||²)}. (3)

匹配距离平方之和为

$S_{lts}=\underset{(i,j)\∈N}{\Σ}{e}_i^2=\underset{(i,j)\∈N}{\Σ}\left|\right|y_j-R_{k-1}{x}_i-T_{k-1}|{|}^2.---\left(4\right)$

3：寻找最优的几何变换(即旋转矩阵R_k和平移向量T_k)使得变换后的数据点集P’和模型点集M距离平方之和(即相似性度量)最小：

$(R_k,T_k)=\underset{R,T}{\mathrm{argmin}}\underset{(i,j)\∈N}{\Σ}\left|\right|y_j-R_k{x}_i-T_k|{|}^2.---\left(5\right)$

$S_{LTS}=\underset{(i,j)\∈N}{\Σ}{e}_i^2=\underset{(i,j)\∈N}{\Σ}\left|\right|y_j-R_k{x}_i-T_k|{|}^2.---\left(6\right)$

4：若达到满足停止条件或者迭代次数达到上限值匹配结束，否则返回步骤2。ICP算法的停止条件有两种：(1)标准差e＝S_LTS/|N_p|足够小；(2)前后两次的标准差变化|e-e'|/e足够小；

在步骤2中，寻找最近点的方法常用的有基于k-d树和基于Delaunary三角划分的最近点搜索算法；实验研究结果表明，k-d树适合高维点集的点搜索，而基于Delaunay三角化算法更适合于低维点集，故考虑使用基于Delaunary三角划分的最近点搜索算法；该算法将散点集合划分成不均匀的三角形网格，且网格满足两个基本准则，其一是空圆特性，即在Delaunay三角形网中任一个三角形网格的外接圆范围内不会有其它点存在；其二是最大化最小角特性，即在散点集可能形成的三角剖分中，Delaunay三角剖分所形成的三角形的最小角最大；在ICP算法中，首先将模型点集M进行Delaunary三角划分，然后采用三角划分搜索策略找到数据点集P在模型点集中的最近点作为对应点，构成匹配对；

理想情况下，数据点集P和模型点集M能够完全匹配，若数据点集中一点x_p在模型点集M中找到对应点y_pm，则模型点集中y_pm在数据点集中找到的对应点也应该是x_p；可依此作为匹配对的评判准则；令数据点集中P_x1在模型点集中的对应点为M_y1，模型点集中M_y1的在数据点集的对应点为P_x2，P_x2的对应点为M_y2；当满足下列关系是，我们认为对应点的寻找是正确的，否则剔除该对应关系；

$\left\{\begin{array}{c}\left|\right|P_{x1}-P_{x2}\left|\right|<\mathrm{\&epsiv;}\\ \left|\right|M_{y1}-M_{y2}\left|\right|<\mathrm{\&epsiv;}\end{array}\right.,---\left(7\right)$

其中ε某个跟标准差相关的常数；

步骤3中的寻找最佳的几何变换常用方法有SVD奇异值分解，四元组，正交矩阵，双四元组方法等，本方案采用的是SVD分解的方法。

7.根据权利要求4所述的基于激光测距仪的全向移动机器人的自主导航装置的控制方法，其特征在于：所述的步骤S2中，

在已知环境地图和初始位置P_s的情况下，设定目标位置P_e，使用快速扩展随机树算法进行全局路径规划；RRT算法是以状态空间中的一个初始点作为根节点，通过随机采样扩展的方式，逐渐增加叶节点，最终生成一棵随机扩展树；当随机树的叶节点中包含了目标点或目标区域中的点时，从初始点到目标点之间的一条以随机树的叶节点组成的路径就是规划路径；规划路径以一系列的离散节点形式给出，相邻节点间不存在障碍物，可根据规划的路径直接到达目标位置点；考虑到在实际环境中，由于移动机器人运动惯性以及未知障碍物的存在，移动机器人不能严格按照规划的路径运动；在以节点形式给出的规划路径中，移动机器人可将一个个节点作为子运动目标，根据实际的情况进行局部路径规划，减少由于偏离轨迹或未知障碍物等原因而需要重新规划全局路径的情况，提高了导航过程的鲁棒性；

考虑到移动机器人的体积大小，使用RRT算法寻找路径前先对环境地图进行膨胀处理，膨胀的区域称为碰撞区域S，从而将移动机器人的体积转移到障碍物中；在RRT算法过程中，将移动机器人看为质点处理，当代表移动机器人的质点位于碰撞区域时，在实际环境中，移动机器人已经与障碍物发生碰撞；

RRT算法假设搜索树为T，当前节点数目为N，其节点为t_ij(t_j)，j为当前节点编号，i为其父节点编号；以初始位置作为搜索树的根节点t₀₁，地图上两点构成的线段为L(p₁,p₂)；在地图空白区域产生随机点p(即当移动机器人位于随机点时不碰撞障碍物)；在目前已有的随机树中找到跟随机点最近的可直接相通的节点p_k，即节点和随机点之间没有障碍物遮挡且不存在碰撞区域；

将该节点作为父节点，沿父节点和随机点的连线方向扩展搜索树，产生新的子节点t_k(N+1)，且子节点和父节点的距离为步长s；

t_k(N+1)＝{t|t∈L(t_k,p),||t-t_k||＝s}. (9)

不断产生随机点扩展随机树，直到随机树的子节点到达目标点，从该节点及其父节点回溯即为找到的路径path＝(p₁,p₂,...,p_n),n为路径的节点数目；

$\left\{\begin{array}{c}{p}_m=\left\{t_{ij}\right|p_{m-1}=t_{ki},1<m<n,t_{ij},t_{ki}\&Element;T\}\\ p_1=p_s\\ p_n=p_e\end{array}\right..---\left(10\right)$

通过RRT算法得到以一系列节点构成的路径，但往往不是最优路径，不相邻的节点存在直接到达的可能，可对其进一步优化；优化步骤如下：

从初始节点开始，判断节点i和节点i+2是否相通，即移动机器人可否直接从节点i直线运动到节点i+2；若可以则去掉节点i+1，下一步判断节点i和节点i+3；若不能直接相通，下一步判断节点i+1和节点i+3；以此类推；类似的也可同时从目标点位置开始判断。

8.根据权利要求4所述的基于激光测距仪的全向移动机器人的自主导航装置的控制方法，其特征在于：所述的步骤S3中，

位置跟踪得到准确的位姿信息作为避障算法的输入，避障算法依据规划路径和当前的位姿信息计算期望的移动速度，并结合激光传感器信息对期望速度进行修正，最后将修正的运动速度向移动机器人发送；

跟踪定位过程与定位建图过程类似，使用里程计和惯性传感器预估移动机器人的位姿，使用ICP算法对位姿信息进行修正；由于激光信息的获取频率最高只有10HZ，ICP算法在最差的情况计算频率为3HZ；为了保证对机器人的实时控制，设计了内环和外环两部分的跟踪算法；内环部分将航位推测法预估的位姿信息作为避障算法部分的输入，控制频率为15Hz，外环使用ICP算法对位姿进行修正，将修正的位姿信息作为避障算法部分的输入，最低控制频率为3HZ；在t时刻得到了移动机器人的预估位姿，包括世界坐标下x轴和y轴的位置以及移动机器人的偏航角X_t＝(x_t,y_t,α_t)，同时将获取的激光信息与已知地图的信息进行ICP算法匹配，此时将预估位姿X_t作为避障算法的输入；在t+1,t+2,…,t+k-1时刻，将当前时刻预估的位姿信息X_t+1,X_t+2,...,X_t+k-1作为避障算法的输入；在t+k时刻预估得到的移动机器人的位姿为X_t+k，此时t时刻开始进行运算的ICP算法完成匹配，得到t时刻的修正位姿考虑激光测距仪的采样频率，我们设定t时刻和t+k时刻的时间间隔需大于0.1s；由此移动机器人的修正位姿为：

${\hat{X}}_{t+k}={\tilde{X}}_t+X_{t+k}-X_t.---\left(11\right)$

我们将修正的位姿作为避障算法的输入，同时获取t+k时刻的激光信息，按照上述步骤再次进行ICP算法的匹配以修正机器人的位姿信息；

在获取了移动机器人的位姿信息后，避障算法依据实时环境信息作局部路径规划；首先确定当前的运动目标，由于规划路径以节点的形式给出，每一个节点即为一个子运动目标，当到达当前目标节点附近或判断移动机器人位置可越过当前目标节点沿直线运动到下一节点时，则作目标节点切换，也即将下一个路径节点作为目标节点；之后根据当前位置和目标节点位置计算移动机器人的期望运动方向，利用避障算法对运动方向进行修正，并将计算的平移与旋转速度发送给移动机器人控制其运动；

RRT算法规划了一条可行路径，但在实际控制过程中，由于移动机器人的运动学特性以及环境中存在未知障碍物等原因，移动机器人并不会完全按照预期的路径运动，需要根据当前的实时位姿信息、移动速度以及环境信息进行局部路径规划，以避开障碍物同时向目标靠近。

9.根据权利要求8所述的基于激光测距仪的全向移动机器人的自主导航装置的控制方法，其特征在于：

局部路径规划算法，安全距离设定为d；令当前位置为S，目标位置为E，理想的运动方向为V_SE；利用上述信息预测下一个控制周期内的预测位置P；将避障过程分为以下四种情况：

(1)障碍物中与当前位置S距离最近的点为O1，如果两者距离SO1小于d，则表明移动机器人即将发生碰撞，此时不考虑目标点的位置，移动机器人向远离点O1的方向运动；

(2)若SO1大于d，则找到距离预测位置P最近的障碍物点O，以点O为圆心，d为半径建立障碍区域VO，若期望的运动方向V_SE没有落在该区域内，则表明机器人将不会跟该点碰撞，可以保持期望速度不变；

(3)若V_SE落在障碍区域VO区间内，在有限时间内，移动机器人将发生碰撞，此时需要对速度方向进行修正；此时若点P位于障碍区域之外，寻找点S与圆VO的切点T，则最终的运动方向将为切线方向：

(4)若点P位于障碍区域VO之内，此时速度V_SE的修正量为方向，修正后的速度为：

由于使用的激光测距仪的测量范围为240°，当机器人最终前进方向与当前方向相差较大时，令其做旋转运动，以使移动机器人能够充分测量到前进方向的环境信息，避免因检测不到障碍物而发生碰撞；在其余的情况下，控制移动机器人仅做平移运动，避免因同时发生旋转和平移运动而导致定位误差增加。