CN100491084C - 一种基于二元环境信息的移动机器人局部路径规划方法 - Google Patents

Abstract

一种基于二元环境信息的移动机器人局部路径规划方法步骤如下：(1)计算目标点在局部路径规划坐标系下的坐标；(2)计算无障碍时的默认规划路径，包括默认输出曲率为S₀和默认输出弧长L₀；(3)中间变量，即左右两侧避障弧段及局部路径规划参数初始化；(4)根据敏感器输出，获取二元环境信息描述，并对障碍进行分类；(5)选取当前最优弧段；(6)计算障碍与步骤(5)的当前最优弧段的相对位置关系，并对障碍进行重新分类；(7)更新左右避障弧段；结束上述工作后，返回步骤(4)重新比较和选取当前最优弧段，进入下一次循环；(8)输出局部路径规划结果。本发明简单、高效，易于实现，满足了移动机器人连续行走的规划实时性要求。

Description

一种基于二元环境信息的移动机器人局部路径规划方法

技术领域

本发明涉及一种局部路径规划方法，适用于一类轮式行走移动机器人的局部路径规划。

背景技术

移动机器人路径规划的任务是在有障碍物的工作环境中寻找一条从给定起点到终点适当的运动路径，使机器人在运动过程中能安全无碰地绕过所有障碍物。移动机器人路径规划主要解决三个问题：(1)使机器人能从初始点运动到目标点；(2)用一定的算法使机器人能绕开障碍物并且经过某些必须经过的点；(3)在完成以上任务的前提下尽量优化机器人运行轨迹。

根据工作环境路径规划模型可分为两种：基于模型的全局路径规划，作业环境的全部信息已知，又称静态或离线路径规划；基于传感器的局部路径规划，作业环境信息全部未知或部分未知，又称动态或在线路径规划。

局部路径规划只需要距离移动机器人较近的障碍物信息。机器人在移动过程中，根据传感器的信息来不断地更新其内部的环境表示，从而确定出自身在地图中的当前位置及周围局部范围内的障碍物分布情况，并在此基础上，以最终到达目标点为目的，规划出一条局部最优路径。

局部路径规划方法的设计或选择与机器人的移动性能、环境基本特征、机器人传感器配置、局部规划实时性要求等均密切相关，目前还不存在一个通用的适用于任何条件的局部路径规划方法。

本发明的适用对象为一类轮式移动机器人，也可称为智能车辆，其特点为：(1)移动装置采用轮式结构，具备沿直线或弧线方向前进或后退及原地转弯能力，但在移动过程中，为保持运动的连续和连贯性，通常要求其移动路径为连续的曲线，且曲线上任一点的曲率大于某定值，避免以折线形式行走；(2)该移动机器人的主要行驶区域以较平坦的开阔地面为主，并分布有大小不同的障碍，即，运动环境由障碍物和自由空间组成，且障碍物的数量及大小均有限；(3)机器人所配置的传感器可以感知一定范围内的障碍信息，包括障碍的大小及与机器人的相对位置；(4)机器人的局部路径规划实时性要求较高，应具备在连续行进过程中避过主要障碍的能力。

目前应用较多的局部路径规划方法包括：人工势场法、模糊逻辑算法、神经网络法、遗传算法和栅格法等。这些方法在应用中都存在不同的缺点，人工势场法仅考虑目标点和障碍对路径规划的影响，规划输出的路径多表现为折线形式，对机器人的移动能力有较高的要求；模糊逻辑算法考虑了汽车驾驶的经验，更适合应用于存在移动障碍等条件的复杂环境，对于一般二元环境来说显得较为复杂；栅格法等全局规划方法也可经改造后应用于局部路径规划，但对传感器要求较高，并且计算量较大，实时性不强。

绝大多数的算法研究中，均更多地关注于规划路径的生成过程，而对规划过程中障碍信息的判断和处理不详，而在实际应用中，算法的设计和编程时，如何更加高效地富有条理地处理障碍信息，也是提高程序执行效率、使代码更加简洁易懂、便于维护所必须考虑的问题。

因此，有必要设计一种简单高效且易于编程实现的局部路径规划算法，在满足大范围较平坦地形的基本假设前提下，可以规划移动机器人绕开障碍，连续、平滑地(即避免折线和后退，仅沿连续圆滑曲线前进)行进到达目标点。

发明内容

本发明的技术解决问题：克服现有技术的不足，提供一种简单、高效，易于实现的基于二元环境信息的移动机器人局部路径规划方法，满足了移动机器人连续行走的规划实时性要求。

本发明的技术解决方案：一种基于二元环境信息的移动机器人局部路径规划方法，其特征在于步骤如下：

(1)计算目标点在局部路径规划坐标系下的坐标(x₀，y₀)；

(2)计算无障碍时的默认规划路径，包括默认输出曲率为S₀和默认输出弧长L₀；

(3)中间变量，即左右两侧避障弧段及局部路径规划参数初始化

定义左避障弧段为从左侧避过当前目标点方向上的障碍所需的最小曲率，即最靠右侧的弧段，右避障弧段为从右侧避过当前障碍所需的最大曲率，即最靠近左侧的弧段，左右避障弧段的弧长分别为L_l、L_r，左右避障弧段的曲率分别为S_l、S_r，初始条件下，两弧段均与步骤(2)中计算出的默认规划弧段重合，即有S_l＝S_r＝S₀、L_l＝L_r＝L₀；

局部路径规划的参数为：移动机器人的长度a、宽度b，移动机器人所能实现的最小转弯半径R_min或最大曲率S_max、局部路径规划有效距离D，R_min和S_max均为标量，且满足S_max＝1/R_min，局部路径规划的输出弧段曲率限制在[-S_max，S_max]区间内，D＝min(D_s，D_i)，其中D_s为环境感知有效距离，可根据敏感器配置情况和运动控制精度确定，D_t为目标点距离

(4)根据敏感器输出，获取二元环境信息描述，并对二元环境信息中的障碍信息进行初始分类

a.将二元环境中的障碍信息以x_fi，x_ri，y_li，y_ri的形式给出，分别对应每个障碍的前边界X坐标、后边界X坐标、左边界Y坐标、右边界Y坐标，i＝1，2，…,n，n为障碍总数；

b.根据障碍与左右避障弧段的相对位置关系，将障碍分为四类，即未知障碍、左侧障碍、右侧障碍、可忽略障碍；未知障碍即障碍与左右避障弧段的相对位置关系未知；左侧障碍即障碍位于右避障弧段左侧；右侧障碍即障碍位于左避障弧段右侧；可忽略障碍包括两类情况，其一为障碍位于左右避障弧段之间，则此时可认为该障碍已经被避过，可不予考虑，其二为障碍到移动机器人的距离大于有效距离，不会影响本次路径规划结果的执行，因此在当前局部路径规划过程中可不予考虑；

c.将所有的障碍初始分类定义为未知障碍；

(5)选取当前最优弧段

比较左右避障弧段，从其中选取曲率与默认规划路径的曲率最接近的，视为当前的最优弧段，最优弧段的曲率和弧长分别为S_c和L_c，即比较|S_l-S₀|和|S₀-S_r|，如果|S_l-S₀|≤|S₀-S_r|，则令S_c＝S_l、L_c＝L_l，否则令S_c＝S_r、L_c＝L_r；

(6)计算障碍与步骤(5)的当前最优弧段的相对位置关系，并对障碍进行重新分类

顺序读取障碍的信息，如果是未知障碍且障碍与移动机器人的距离大于规划有效距离D，则直接将其重分类为可忽略障碍；否则计算障碍与当前最优弧段的相对位置关系，判断障碍在最优弧段的左侧、或右侧，或落在最优弧段上，如果障碍落在最优弧段上，则中断步骤(6)进入步骤(7)，否则对障碍进行重新分类，然后读取下一障碍信息；如果全部障碍信息都已读取完毕，仍没有因为障碍在弧段上而中断，则当前最优弧段上没有障碍，为可行规划弧段，可直接转入步骤(8)输出规划结果；

(7)更新左右避障弧段

当前最优弧段为左避障弧段时，则更新左避障弧段；当前最优弧段为右避障弧段时，则更新右避障弧段，在更新左右避障弧段之后，障碍不再落在弧段上，对于左避障弧段，此时障碍在弧段右侧，对于右避障弧段，此时障碍在弧段左侧，然后再根据障碍的类别属性，对当前障碍进行重新归类；

结束上述工作后，返回步骤(5)重新选取最优弧段；

(8)输出局部路径规划结果

步骤(6)中若遍历所有障碍后，没有障碍落在当前最优弧段上，则当前最优弧段为可行规划弧段，将曲率S_c和弧长L_c作为当前最优规划路径输出给导航与控制系统，并结束当前局部路径规划进程。

本发明与现有技术相比的优点在于：相比于已知的各局部路径规划方法，本发明主要针对大范围平坦地形的基本假设，忽略复杂起伏地形对移动机器人的影响，用尽可能少的数据量描述环境信息，因此更简单，所需的规划时间更短，可以满足移动机器人连续行走的规划实时性要求；环境信息数据量需求的降低，使得移动机器人无需配备复杂的多敏感器即可满足局部路径规划的环境感知需求，大大降低了硬件配置要求；对于多障碍的环境，对障碍进行简单的分类处理，提高了程序的执行效率，特别易于工程实现。

附图说明

图1本发明的局部路径规划执行周期；

图2为本发明的局部路径规划方法流程图；

图3为本发明的局部路径规划坐标系示意图；

图4这本发明的无障碍时的默认规划路径计算；

图5为本发明中的左右避障弧段示意图；

图6为本发明的障碍与弧段的相对位置关系；

图7为本发明的左避障弧段的计算；

图8为本发明通过多次调用局部路径规划算法完成避障连续行走到达目标点。

具体实施方式

在移动机器人自主行走过程中，需要每隔一定时间执行一次局部路径规划方法，即本发明生成当前局部最优路径，提供给导航与控制系统。

本发明局部路径规划方法进程的执行周期可以分为两种方式：定周期执行和变周期执行。定周期执行时局部路径规划进程频率较高，以尽可能准确获取环境信息、减少误判为首要目标；变周期执行时局部路径规划进程频率动态变化，以成功避过已知障碍为首要目标。可以根据应用对象的不同选择不同的形式。如果敏感器感知的环境信息不够准确(如存在一定的误判概率、障碍位置的计算精度欠佳等)或障碍数较多，需要频繁地进行环境数据采集，局部路径规划的频率较高，则宜采用固定周期；如果敏感器感知范围有限(如相机视场角小于180度)，且计算环境信息时没有综合历史数据(即每次的局部路径规划均只以避过当前能够感知的障碍为主要任务)，则采用固定周期可能会形成障碍的感知盲区，造成避障动作不完整等结果，使得避障失败，因此宜选用变周期。

定周期或变周期的选择可以通过输出的局部最优路径弧段的弧长来体现，即导航与控制系统可以根据输出的弧长来确定局部路径规划进程的调用频率和具体时刻。设移动机器人的行驶速度为V，敏感器数据采集及处理的总时间为t，局部路径规划的时间相对于敏感器数据处理的时间可以忽略不计，输出弧长为L，为实现移动机器人的连续行走，则两次调用局部路径规划进程的最大行驶里程间隔应选择为L-Vt，即，从上次调用局部路径规划进程时刻起，移动机器人行驶里程达到L-Vt以前，即应启动下一次的局部路径规划。当L为定值时，局部路径规划为定周期执行(严格来说应是定距离，即局部路径规划进程的启动不是根据行驶时间确定，而是根据行驶里程数确定，实际的运动过程中，移动机器人的速度并不是严格恒定的，因此局部路径规划的周期也不是严格一致的)；当L为变值时，局部路径规划为变周期执行。

如图1所示，图中在点1处启动第n次局部路径规划，到点2处接收到局部路径规划的输出，从点2起至点4为止，移动机器人按照第n次局部路径规划输出的曲率S和弧长L行进(同理，点1到点2路段，移动机器人是按照第n-1次局部路径规划输出行进的)，并在点3处启动第n+1次局部路径规划。1、2和3、4的距离均为Vt，1、4的距离为L_n(第n次局部路径规划的输出L)，3、6的距离为L_n+1(第n+1次规划输出L)，2、3段距离为L-2Vt。由图可知，规划输出的弧长应能使得在启动下一次局部路径规划以前接收到当前的路径规划结果。因此，局部路径规划的最小输出弧长为2Vt，且(L-2Vt)/L越大，即2、3的距离占1、4距离的比重越大，移动机器人越能充分地执行局部路径规划的输出，降低路径规划与实际运动之间的误差。当目标点与移动机器人的距离小于2Vt时，则无需进行局部路径规划。

本发明的局部路径规划方法的主要实现过程如图2所示：

(1)计算目标点在局部路径规划坐标系下的坐标。

在移动机器人的行进过程中，可能存在一个或多个目标点，机器人需按照操作者的全局规划，顺序地到达或接近到达全部或部分目标点。但对于局部路径规划来说，某一时刻可以只考虑移动方向上距当前位置最近的一个目标点，该目标点可以在敏感器的有效感知范围之外。

通常在全局路径规划时，目标点是在导航坐标系下描述的，如地球惯性坐标系、东北天坐标系等；为便于完成局部路径规划，建立局部路径规划坐标系，该坐标系为瞬时坐标系，坐标原点位于当前规划时刻的移动机器人质心在水平面上的投影点上，X轴指向移动机器人的前进方向，Y轴位于水平面上，指向移动机器人左侧，Z轴满足右手螺旋定则。

导航坐标系的定义不同，则从导航坐标系到局部路径规划坐标系的转换关系不尽相同，因此本发明不再给出具体的转换公式，后续的局部路径规划步骤中，所有的计算均将在局部路径规划坐标系下完成。

在局部路径规划过程中，根据环境基本假设，地形较为平坦，可简化为二维空间，因此，只需要计算目标点的X和Y方向坐标即可，Z坐标可不予考虑。

局部路径规划坐标系的定义如图3所示，图中下方矩形代表移动机器人外轮廓，假设右上角标志的点1为目标点，则可以得到该点在局部路径规划坐标系X轴和Y轴方向上的坐标。

(2)计算无障碍时的默认规划路径。

根据目标点在局部路径规划坐标系下的坐标(即目标点与移动机器人的相对位置关系)，可以求得无障碍时的默认输出规划路径。

如图4所示，默认输出规划路径位于过局部路径规划坐标系的原点和目标点，并与X轴相切的圆上。若目标点位于X轴上，则默认输出规划路径从弧段退化为线段。该规划路径的曲率为该圆对应的曲率，并满足左转为正、右转为负。

设默认输出曲率为S₀，目标点坐标为(x₀，y₀)，则有： $S_0=2y_0/(x_0^2+y_0^2).$

默认输出弧长L₀的选择需根据实际情况综合考虑，应满足如下几个条件：

(a)如前所述，局部路径规划的输出弧长应大于2Vt；

(b)默认输出弧长应不大于敏感器环境感知范围，即，默认输出弧长对应弧段的安全性可以通过当前敏感器的数据充分感知，如，若相机的有效视场距离为5m，则默认输出弧长也应不大于5m；

(c)默认输出弧长不宜过大，相隔一个默认输出弧长距离的两个位置点上所获取的环境数据应具有较大的重合，以尽可能准确地把握环境信息。

若选择定周期局部路径规划，默认输出弧长即为最终的输出弧长，后面各步骤中均不再对输出弧长进行调整；若选择变周期局部路径规划，则默认输出弧长实际上是最小输出弧长，步骤(7)中可能会根据障碍信息增大规划弧长的输出值。

(3)局部路径规划中间变量及参数的初始化

为完成路径规划，本发明定义了左右两侧避障弧段，左避障弧段为从左侧避过当前目标点方向上的障碍所需的最小曲率(即最靠右侧)的弧段，右避障弧段为从右侧避过当前障碍所需的最大曲率(即最靠近左侧)的弧段。如图5所示，对于图中的障碍，弧段1即为左避障弧段，弧段2即为右避障弧段。左右避障弧段的弧长L_l、L_r和曲率S_l、S_r均在步骤(7)中动态计算，最终输出的局部最优规划弧段也从这两个弧段中选取。初始条件下，两弧段均与步骤(2)中计算出的默认规划弧段重合，即有S_l＝S_r＝S₀、L_l＝L_r＝L₀。

在局部路径规划后续步骤中还需要用到如下参数：移动机器人的长度a、宽度b，移动机器人所能实现的最小转弯半径R_min或最大曲率S_max，局部路径规划有效距离D。

R_min和S_max均为标量，且满足S_max＝1/R_min，是移动机器人移动性能的重要参数，对于轮式机器人来说，受到转向轮转角范围的限制，如果希望各转向轮的转向均与该轮的瞬时前进方向一致，则其能够实现的转弯半径存在最小值R_min，更小的转弯半径需要通过原地转弯或差速转弯实现，通常这在移动机器人的连续行驶中是需要尽量避免的，因此需要将局部路径规划的输出弧段曲率限制在[-S_max，S_max]区间内。

受敏感器感知能力的限制，移动机器人获得的局部环境信息的范围是有限的，为了保证局部路径规划的准确性和安全性，移动机器人一定距离以外的环境数据可以认为是可信度不高的或对当前运动没有根本影响的，均可忽略不计，记这个距离为局部路径规划有效距离D。可选取D＝min(D_s，Dt)，其中D_s为环境感知有效距离，可根据敏感器配置情况和运动控制精度确定，D_t为目标点距离因为虽然通常情况下可以把目标点选取在环境感知有效范围之外，但在运动末段，目标点距离可能会小于环境感知有效距离，此时目标点后方的障碍已经不是局部路径规划所需要关心的了。

(4)根据敏感器输出，获取二元环境信息描述，并将对二元环境信息进行初始分类。

所谓二元环境，即假设环境由自由空间和障碍组成，在本规划算法中，自由空间即指平坦路面，移动机器人可在自由空间内实现直线或弧线行进；障碍则包括一切移动机器人无法在其上穿越的空间，如具有一定体积的石头、坑、陡坡等。

本发明局部路径规划方法中采用矩形区域简化描述障碍信息，矩形的各边分别平行于局部路径规划坐标系的X轴和Y轴，如图3所示。对于不规则形状的障碍可以视作是多个矩形障碍重叠形成。因此为描述当前二元环境信息，只需要记录下所有障碍的四个顶点在局部路径规划坐标系下的坐标即可，即设障碍总数为n，环境信息描述为各点的左前点坐标(x_lfi，y_lfi)、右前点坐标(x_rfi，y_rfi)、左后点坐标(x_lri，y_lri)、右后点坐标(x_rri，y_rri)，其中，i＝1，2，…，n。可进一步简化为采用障碍的前后X坐标和左右Y坐标(x_fi，x_ri，y_li，y_ri)描述，如图3所示。

根据环境基本假设，在局部路径规划过程中不必考虑复杂地形和大范围大面积障碍等情况对规划的影响，该环境描述方法对环境进行了最大程度的简化，从而可以降低对敏感器硬件配置的要求。一般地，在环境特征明显、安装位置合理的前提下，仅靠一架相机，经过适当的视觉图像处理，即可获得满足本局部路径规划算法的环境描述信息。环境信息的具体生成过程因敏感器配置的不同而不同，但只要满足前述的数据形式，便可用于本局部路径规划算法，因此环境数据生成的具体实现方式不在本算法的考虑范围之内。

在发明的本局部路径规划方法中，为了便于管理，对所有障碍都添加了分类标记，将其划分为不同的类别，并在后续步骤中进行不同的处理。根据障碍与左右避障弧段的相对位置关系，可以将障碍分为四类，即未知障碍、左侧障碍、右侧障碍、可忽略障碍。未知障碍即障碍与左右避障弧段的相对位置关系未知；左侧障碍即障碍位于右避障弧段左侧；右侧障碍即障碍位于左避障弧段右侧；可忽略障碍包括两类情况，其一为障碍位于左右避障弧段之间，则此时可认为该障碍已经被避过，可不予考虑，其二为障碍到移动机器人的距离大于有效距离D，不会影响本次路径规划结果的执行，因此在当前局部路径规划过程中可不予考虑。具体的障碍与弧段位置关系判断和分类将在步骤(6)中完成，初始条件下，所有的障碍均属于未知障碍。

步骤(1)到(4)为局部路径规划方法的准备阶段。

(5)选取当前最优弧段。

比较左右避障弧段(如前所述，初始状态下两弧段重合，但在后面的步骤(7)中经动态计算重新赋值后，两者即可能不再相同)，从其中选取曲率与默认规划路径的曲率最接近的，视为当前的最优弧段，记其曲率和弧长为S_c和L_c。即比较|S₁-S₀|和|S₀-S_r|，如果|S_l-S₀|≤|S₀-S_r|，则令S_c＝S_l、L_c＝L_l，否则令S_c＝S_r、L_c＝L_r。

需要注意的是，上述比较的一个前提条件是左避障弧段和右避障弧段均未超出移动机器人的最小转弯半径限制，即max(|S_l|，|S_r|)≤S_max，若左右避障弧段中有一个超出了该限制，则直接选取未超限的弧段为当前最优弧段，若两弧段均超限，则意味着当前环境中的障碍已经遮挡了移动机器人的所有前进方向，因此直接中断当前局部路径规划进程，并返回错误信息。

(6)计算障碍与当前最优弧段的相对位置关系并对障碍进行重新分类。

如前所述，本算法中将障碍分为四类，即未知障碍、左侧障碍、右侧障碍、可忽咯障碍。根据左侧障碍的定义，左侧障碍位于右避障弧段的左侧，而在步骤(7)中右避障弧段的更新为单向的向右侧搜索，即新的右避障弧段只可能出现在原右避障弧段的右侧，一旦确定某障碍为左侧障碍，无论右避障弧段在规划过程中是否发生了更新，该障碍与右避障弧段的相对位置方向都是唯一确定的，始终位于右避障弧段的左侧。因此，在当前最优弧段为右避障弧段时，无需重新计算左侧障碍与当前最优弧段的相对位置关系。同理，在当前最优弧段为左避障弧段时，无需重新计算右侧障碍与当前最优弧段的相对位置关系。根据可忽略障碍的定义，该障碍始终位于左右两侧避障弧段中间或规划有效距离之外，同样无需计算可忽略障碍与当前最优弧段的相对位置关系。

综上，当前最优弧段为左避障弧段时，仅比较未知障碍和左侧障碍与弧段的相对位置关系；当前最优弧段为右避障弧段时，仅比较未知障碍和右侧障碍与弧段的相对位置关系。

顺序读取障碍的信息。如果是未知障碍且障碍与移动机器人的距离大于规划有效距离，则直接将其重分类为可忽略障碍；否则计算障碍与当前最优弧段的相对位置关系，判断障碍在最优弧段的左侧、或右侧，或落在最优弧。段上。如果障碍在最优弧段上，则中断步骤(6)进入步骤(7)；否则对障碍进行重新分类，然后读取下一障碍信息，如果全部障碍信息都已读取完毕，仍没有因为障碍在弧段上而中断，则当前最优弧段上没有障碍，为可行规划弧段，可直接转入步骤(8)输出规划结果。

本步骤的实现可以具体分解如下：

(a)顺序读取障碍信息(x_f，x_r，y_l，y_r)，如果不存在障碍，或全部障碍信息都已读取完毕，则当前最优弧段为可行规划弧段，可直接转入步骤(8)输出规划结果。

i.当前最优弧段为左避障弧段：

如果障碍为右侧障碍或可忽略障碍，重新执行(a)读取下一障碍信息；

如果障碍为未知障碍或左侧障碍，转入(b)。

ii.当前最优弧段为右避障弧段：

如果障碍为左侧障碍或可忽略障碍，重新执行(a)读取下一障碍信息；

如果障碍为未知障碍或右侧障碍，转入(b)。

(b)若障碍为未知障碍，判断其与移动机器人的距离是否大于规划有效距离D，如果大于D，则直接将其重分类为可忽略障碍，然后返回(a)读取下一障碍信息。

已知障碍的边界坐标为(x_f，x_r，y_l，y_r)，分别代表前后边X坐标和左右边的Y坐标，弧段的曲率和弧长为S_c和L_c，移动机器人的长度为a、宽度为b，规划有效距离为D。判断未知障碍是否在规划有效距离之外的公式为：

如果y_l·y_r≤0，则障碍在X轴上，障碍到移动机器人的距离D_ob＝x_r；如果y_r>0，则障碍在机器人的左侧， $D_{\mathrm{ob}}=\min (x_r,\sqrt{{(x_r-a/2)}^2+{(y_r-b/2)}^2});$

如果y_l<0，则障碍在机器人的右侧， $D_{\mathrm{ob}}=\min (x_r,\sqrt{{(x_r-a/2)}^2+{(y_l+b/2)}^2}).$

若D_ob>D，则障碍为可忽略障碍。

(c)根据计算判断当前读取的障碍与当前最优弧段的位置关系，即障碍是在弧段的左侧、弧段的右侧、还是落在弧段上。

如图6所示，由于移动机器人是有其体积的，因此机器人在沿当前最优弧段行进时所覆盖的区域不是一条弧线而是一段圆环，圆环内环和外环的圆心均与当前最优弧段的圆心重合。已知当前最优弧段为S_c，则弧段半径为R_c＝1/S_c，圆心坐标为(0，R_c)。运动区域圆环的内径为|R_c|-b/2，外径为

(或者可以将外径的值近似简化为|R_c|+b/2)。如果障碍的任何一点落在此环形区域内都会影响移动机器人沿该弧段前进，应视作障碍在弧段上。

可以根据机器人的当前最优弧段将步骤(c)分成三种情况：

i.当前最优弧段为直线段

随着S_c趋近于0，R_c＝1/S_c趋于无穷大，因此当|S_c|≤δ_c时，可以认为移动机器人的行走路径为直线段，其中δ_c为一小量。此时有：

如果y_r≥b/2，障碍在弧段左侧；

如果y_l≤-b/2，障碍在弧段右侧；

如果上述两条都不满足，障碍在弧段上。

ii.当前最优弧段为左转弯的弧段(即弧段圆心在+Y轴上)

当S_c>δ_c时，圆心在+Y轴上。障碍完全落在内环以内(障碍的右前点到圆心距离小于内径)，则障碍在弧段左侧；障碍完全落在外环以外(障碍的左后点到圆心距离大于外径)，则障碍在弧段右侧；否则障碍在弧段上。即：

如果 $\sqrt{{\left(x_f\right)}^2+{(y_r-R_c)}^2}\&le;\left|R_c\right|-b/2,$ 障碍在弧段左侧；

如果 $\sqrt{{\left(x_r\right)}^2+{(y_l-R_c)}^2}\&GreaterEqual;\sqrt{{(\left|R_c\right|+b/2)}^2+{(a/2)}^2},$ 障碍在弧段右侧；

如果上述两条都不满足，障碍在弧段上。

iii.当前最优弧段为右转弯的弧段(即弧段圆心在-Y轴上)

当S_c<-δ_c时，圆心在-Y轴上。障碍完全落在外环以外(障碍的右后点到圆心距离大于外径)，则障碍在弧段左侧；障碍完全落在内环以内(障碍的左前点到圆心距离小于内径)，则障碍在弧段右侧；否则障碍在弧段上。即：

如果 $\sqrt{{\left(x_r\right)}^2+{(y_r-R_c)}^2}\&GreaterEqual;\sqrt{{(\left|R_c\right|+b/2)}^2+{(a/2)}^2},$ 障碍在弧段左侧；

如果 $\sqrt{{\left(x_f\right)}^2+{(y_l-R_c)}^2}\&le;\left|R_c\right|-b/2,$ 障碍在弧段右侧；

如果上述两条都不满足，障碍在弧段上。

(d)根据(c)的计算结果，如果障碍在当前最优弧段上，则当前最优弧段已不可行，中断步骤(6)进入步骤(7)重新计算和选取左右避障弧段；否则(障碍在弧段的左侧或在弧段的右侧)进入(e)。

(e)根据当前最优弧段是左避障弧段还是右避障弧段、当前障碍原来所属类别、以及(c)的计算结果，对当前障碍进行重新分类。

i.当前最优弧段为左避障弧段：

对于未知障碍，若障碍在左避障弧段左侧，则其必然也在右避障弧段的左侧，故重分类为左侧障碍；若障碍在左避障弧段右侧，则按定义自然属于右侧障碍。

对于左侧障碍，若障碍在左避障弧段左侧，分类不发生变化；若障碍在左避障弧段的右侧，根据分类定义，障碍同时也在右避障弧段的左侧，因此重分类为可忽略障碍。

ii.当前最优弧段为右避障弧段：

对于未知障碍，若障碍在右避障弧段左侧，则按定义自然属于左侧障碍；若障碍在右避障弧段右侧，则其必然也在左避障弧段的右侧，故重分类为右侧障碍。

对于右侧障碍，若障碍在右避障弧段左侧，根据分类定义，障碍同时也在左避障弧段的右侧，因此重分类为可忽略障碍；若障碍在右避障弧段的右侧，分类不发生变化。

步骤(e)可归纳如表1。

表1障碍分类

(f)返回(a)，读取下一障碍信息。

(7)更新左右避障弧段

当计算结果表明某障碍在当前最优弧段上时，步骤(6)中断，进入步骤(7)，根据该障碍信息更新左右避障弧段。

如果当前最优弧段为左避障弧段，证明原左避障弧段无效，需继续向左侧寻找新的左避障弧段，即计算避过当前障碍左侧边缘点所需的最小曲率和弧长，并将结果赋值给左避障弧段，右避障弧段保持不变；如果当前最优弧段为右避障弧段，证明原右避障弧段无效，需继续向右侧寻找新的右避障弧段，即计算避过当前障碍右侧边缘点所需的最大曲率和弧长，并将结果赋值给右避障弧段，左避障弧段保持不变。

即，当前最优弧段为左避障弧段时，则更新左避障弧段；当前最优弧段为右避障弧段时，则更新右避障弧段。

(a)更新左避障弧段

已知落在当前最优弧段上的当前障碍的边界坐标为(x_f，x_r，y_l，y_r)，移动机器人的长度为a、宽度为b，规划有效距离为D，要求障碍与左避障弧段保持 $d=\sqrt{{(a/2)}^2+{(b/2)}^2}$ 的安全距离，左避障弧段的计算如图7所示。图中给出了两种不同的情况，对于左侧所示的障碍，其左避障弧段应为一个左转弯的弧段；对于右侧所示的障碍，其左避障弧段应为一个右转弯的弧段。两种情况可根据障碍左边界的坐标进行区分。

i.左避障弧段为一个向左转弯的弧段

设圆心位置为(0，R_l)，当障碍的左边界满足y_l+d≥0，有：

$\sqrt{{\left(x_r\right)}^2+{(y_l-R_l)}^2}=R_l+d$

$S_l=\frac{1}{R_l}=\frac{2(y_l+d)}{{\left(x_r\right)}^2+{\left(y_l\right)}^2-d^2}$

ii.左避障弧段为一个向右转弯的弧段

当障碍的左边界满足y_l+d<0，有：

$\sqrt{{\left(x_f\right)}^2+{(y_l-R_l)}^2}=|R_l+d|$

$S_l=\frac{1}{R_l}=\frac{2(y_l+d)}{{\left(x_f\right)}^2+{\left(y_l\right)}^2-d^2}$

上述两个S_l的计算公式可以统一为：

$S_l=\frac{1}{R_l}=\frac{2(y_l+d)}{{\left(x_i\right)}^2+{\left(y_l\right)}^2-d^2}$

式中当y_l+d≥0时x_i＝x_r，当y_l+d<0时x_i＝x_f。

如果局部路径规划算法采用变周期执行，则还需要对左避障弧段的弧长进行更新。弧长计算公式为：

$L_l=\min [D,\max (L_l,\left|R_l{\sin }^{-1}\frac{x_f}{R_l+d}\right|)]$

(b)更新右避障弧段

同理可得右避障弧段的更新公式为：

$S_r=\frac{1}{R_r}=\frac{2(y_r-d)}{{\left(x_i\right)}^2+{\left(y_r\right)}^2-d^2}$

式中当y_r-d≥0时x_i＝x_f，当y_r-d<0时x_i＝x_r。相应的弧长计算公式为：

$L_r=\min [D,\max (L_r,\left|R_r{\sin }^{-1}\frac{x_f}{R_r-d}\right|)]$

(c)特殊情况的处理

在计算得到的曲率S_l或S_r趋近于零时，计算会出现半径无穷大，此时可将上面的弧长计算公式改为：

L＝min[D，max(L，x_f)]

在更新左右避障弧段之后，障碍不再落在弧段上，对于左避障弧段，此时障碍在弧段右侧，对于右避障弧段，此时障碍在弧段左侧，于是可以根据障碍的类别属性，按表1的规则对当前障碍进行重新归类。

结束上述工作后，返回步骤(5)重新比较和选取当前最优弧段，进入下一次循环。

(8)局部路径规划结果输出

步骤(6)中若遍历所有障碍后，没有障碍落在当前最优弧段上，则当前最优弧段为可行规划弧段，将曲率S_c和弧长L_c作为当前最优规划路径输出给导航与控制系统，并结束当前局部路径规划进程。

导航与控制系统每隔一定周期调用局部路径规划算法，获取局部最优规划路径，并控制运动系统沿该路径行进，经过若干个周期后即可逐步接近以致最终到达目标点的容许误差范围之内，如图8所示。

由上述步骤可以看出，本发明局部路径规划方法的实现十分简单，基本上完全依靠几何计算即可完成完整的规划计算过程，规划时间可以在很短的时间内完成，从而满足移动机器人连续行走的规划实时性要求；对于多障碍的环境，在步骤(6)中并不针对每条当前最优路径都遍历所有的障碍，而是一旦发现某障碍落在当前弧段即刻做出反应，更新左右避障弧段后再重新比较障碍与弧段的关系，从而可以极大地提高程序的执行效率；整个规划算法流程简单明晰，易于理解，很方便工程编程；本局部路径规划算法输出的规划路径均为与当前移动机器人前进方向相切的弧段，从而使得移动机器人的完整运动轨迹可以形成一条连续的圆滑的曲线，便于运动控制的实现。

以中国空间技术研究院自主研发的月面巡视探测器原理样机为例进一步说明本发明的具体实现过程：

该原理样机以月面环境为假想背景。已知的月面环境为较为平坦的松软土壤，并分布一定概率的障碍，满足本局部路径规划算法的基本假设。该原理样机为六轮摇臂悬架式结构，六轮驱动，前后四个角轮提供转向，可以实现前进、后退、原地转弯、行进中Ackerman转弯。用于局部路径规划的主要传感器为一对安装于车体前方的避障相机。

样机运动控制模块中要求的最小转弯半径为1.5米，样机的长宽比为1000mm×800mm，自主行走时样机的行驶速度约为20～25mm/s。避障相机的有效可视范围约为3～5米，两台避障相机能够提供的总视场角约为100度，且车上不提供保留历史环境数据的功能，因此选择选用变周期模式的局部路径规划算法，即，输出规划弧长L为变值，并设定样机自上次启动局部路径规划进程起，行驶距离达到0.8L时启动下一次局部路径规划。同时针对原理样机的情况，对本局部路径规划算法部分内容进一步做了简化。

本发明局部路径规划方法的具体实现如下：

(1)计算目标点在局部路径规划坐标系下的坐标。

月面巡视探测器原理样机的导航坐标系为月面坐标系，其XOY平面与局部路径规划坐标系的XOY平面平行，因此坐标转换为简单的水平平移加Z轴旋转，此项坐标转换由导航与控制系统完成计算，并在调用局部路径规划算法时一并输入。

(2)计算无障碍时的默认规划路径。

设默认输出曲率为S₀，目标点坐标为(x₀，y₀)，有：

$S_0=2y_0/(x_0^2+y_0^2)$

原理样机的移动速度较低，计算2Vt小于0.1m，但由于自主行走过程中，遥端可能没有详细的环境信息反馈，要求无障碍情况下局部路径规划的间隔不宜过长，因此选取L₀为0.5m。

(3)局部路径规划算法中间变量及参数的初始化。

左右避障弧段的弧长L_l、L_r和曲率S_l、S_r初始条件下取S_l＝S_r＝S0、L_l＝L_r＝L₀。

取样机的长度a＝1、宽度b＝0.8，移动机器人所能实现的最小转弯半径R_min＝1.5为最大曲率S_max＝0.667，局部路径规划环境感知有效距离D_s＝3。局部路径规划的输出弧段曲率限制在[-0.667，0.667]区间内。

局部路径规划有效距离 $D=\min (D_s,\sqrt{x_0^2+y_0^2}).$

(4)根据敏感器输出，获取二元环境信息描述，并进行初始障碍分类。

原理样机的视觉模块完成相机拍摄图像的采集和处理，并根据局部路径规划算法的数据输入要求，将环境中的障碍信息以(x_fi，x_ri，y_li，y_ri)的形式提供给局部路径规划算法，其中i＝1，2，…，n，n为障碍总数，x_fi，x_ri，y_li，y_ri分别对应每个障碍的前边界X坐标、后边界X坐标、左边界Y坐标、右边界Y坐标。初始条件下，所有的障碍均分类为未知障碍。

(5)选取当前最优弧段。

记当前最优弧段的曲率和弧长为S_c和L_c，比较左右避障弧段，如果|S_l-S₀|≤|S₀-S_r|，则令S_c＝S_l、L_c＝L_l，否则令S_c＝Sr、L_c＝L_r。

(6)计算障碍与当前最优弧段的相对位置关系并对障碍进行重新分类。

顺序读取障碍信息(x_f，x_r，y_l，y_r)，若为未知障碍，判断是否在规划有效距离之外：

如果y_l·y_r≤0，则障碍在X轴上，障碍到原理样机的距离D_ob＝x_r；

如果y_r>0，则障碍在样机的左侧， $D_{\mathrm{ob}}=\min (x_r,\sqrt{{(x_r-a/2)}^2+{(y_r-b/2)}^2});$

如果y_l<0，则障碍在样机的右侧， $D_{\mathrm{ob}}=\min (x_r,\sqrt{{(x_r-a/2)}^2+{(y_l+b/2)}^2}).$

若D_ob>D，则障碍为可忽略障碍。

由于原理样机的行驶环境为松软月壤，存在一定的滑移，定位精度相对有限，对样机行进的覆盖环状区域的外廓要求可以不必十分精确，因此将运动区域圆环的外径近似简化为|R_c|+b/2，内径仍为|R_c|-b/2。如果障碍的任何一点落在此环形区域内都会影响移动机器人沿该弧段前进，应视作障碍在弧段上。

取δ_c＝0.01，当|S_c|≤δ_c时，可以认为样机的行走路径为直线段，此时有：

如果y_r≥b/2，障碍在弧段左侧；

如果y_l≤-b/2，障碍在弧段右侧；

如果上述两条都不满足，障碍在弧段上。

当S_c>δ_c时，圆心在+Y轴上：

如果 $\sqrt{{\left(x_f\right)}^2+{(y_r-R_c)}^2}\&le;\left|R_c\right|-b/2,$ 障碍在弧段左侧；

如果 $\sqrt{{\left(x_r\right)}^2+{(y_l-R_c)}^2}\&GreaterEqual;\left|R_c\right|+b/2,$ 障碍在弧段右侧；

如果上述两条都不满足，障碍在弧段上。

当S_c<-δ_c时，圆心在-Y轴上：

如果 $\sqrt{{\left(x_r\right)}^2+{(y_r-R_c)}^2}\&GreaterEqual;\left|R_c\right|+b/2,$ 障碍在弧段左侧；

如果 $\sqrt{{\left(x_f\right)}^2+{(y_l-R_c)}^2}\&le;\left|R_c\right|-b/2,$ 障碍在弧段右侧；

如果上述两条都不满足，障碍在弧段上。

若障碍不在弧段上，按表1的原则对障碍进行重新分类，若刚好落在弧段之上，则当前最优弧段已不可行，需中断步骤(6)进入步骤(7)重新计算和选取左右避障弧段。或者，当遍历所有障碍后，没有障碍落在当前最优弧段上，则当前最优弧段为可行规划弧段，可直接转入步骤(8)输出规划结果。

(7)更新左右避障弧段。

如前所述，基于样机的滑移现象和定位精度的考虑，障碍与避障弧段的安全距离的选取在

的基础上略放宽，取d＝0.8。左避障弧段的计算如图7所示。记曲率计算公式为：

$S_m=\frac{2(y_m\&PlusMinus;d)}{{\left(x_n\right)}^2+{\left(y_m\right)}^2-d^2}$

当y_m±d≥0时x_n＝x_f，否则x_n＝x_r。

以上对应左避障弧段时将下标_m替换为_l，±取正；对应右避障弧段时将下标_m替换为_r，±取负。

弧长计算公式简化为：

L＝min[D，max(L，x_l)]

如果当前最优弧段为左避障弧段，则仅更新左避障弧段，右避障弧段保持不变；如果当前最优弧段为右避障弧段，则仅更新右避障弧段，左避障弧段保持不变。

在更新左右避障弧段之后，障碍不再落在弧段上，对于左避障弧段，此时障碍在弧段右侧，对于右避障弧段，此时障碍在弧段左侧，于是可以根据障碍的类别属性，按表1的规则对当前障碍进行重新归类。

结束上述工作后，返回步骤(4)重新比较和选取当前最优弧段，进入下一次循环。

(8)局部路径规划结果输出。

步骤(6)中若遍历所有障碍后，没有障碍落在当前最优弧段上，则当前最优弧段为可行规划弧段，将曲率S_c和弧长L_c作为当前最优规划路径输出给导航与控制系统，并结束当前局部路径规划进程。

上述步骤基本上与前文所述的本局部路径规划方法一致，仅在细节上做了微小调整，仿真和内外场实验均证明了，当障碍距离样机过近或距离目标点过近，由于原理样机敏感器环境采集视场和运动特性的限制，无法实现自主的避障动作，其他一般情况下，通过连续多次的局部路径规划，探测器原理样机均可根据规划算法的输出，在避过障碍的同时尽可能近地到达目标点。

Claims (15)

1、一种基于二元环境信息的移动机器人局部路径规划方法，其特征在于步骤如下：

(1)计算目标点在局部路径规划坐标系下的坐标x₀、y₀；

(2)计算无障碍时的默认规划路径，包括默认输出曲率为S₀和默认输出弧长L₀；

(3)中间变量，即左右两侧避障弧段及局部路径规划参数初始化

定义左避障弧段为从左侧避过当前目标点方向上的障碍所需的最小曲率，即最靠右侧的弧段，右避障弧段为从右侧避过当前障碍所需的最大曲率，即最靠近左侧的弧段，左右避障弧段的弧长分别为L_l、L_r，左右避障弧段的曲率分别为S_l、S_r，初始条件下，两弧段均与步骤(2)中计算出的默认规划弧段重合，即有S_l＝S_r＝S₀、L_l＝L_r＝L₀；

局部路径规划的参数为：移动机器人的长度a、宽度b，移动机器人所能实现的最小转弯半径R_min或最大曲率S_max、局部路径规划有效距离D，R_min和S_max均为标量，且满足 $S_{\max }=1/R_{\min }^{*},$ 局部路径规划的输出弧段曲率限制在[-S_max，S_max]区间内，D＝min(D_s，D_l)，其中D_s为环境感知有效距离，可根据敏感器配置情况和运动控制精度确定，D_l为目标点距离

(4)根据敏感器输出，获取二元环境信息描述，并对二元环境信息中的障碍信息进行初始分类

a.将二元环境中的障碍信息以x_fi，x_rl，y_h，y_ri的形式给出，分别对应每个障碍的前边界X坐标、后边界X坐标、左边界Y坐标、右边界Y坐标，i＝1，2，…，n，n为障碍总数；

b.根据障碍与左右避障弧段的相对位置关系，将障碍分为四类，即未知障碍、左侧障碍、右侧障碍、可忽略障碍；未知障碍即障碍与左右避障弧段的相对位置关系未知；左侧障碍即障碍位于右避障弧段左侧；右侧障碍即障碍位于左避障弧段右侧；可忽略障碍包括两类情况，其一为障碍位于左右避障弧段之间，则此时可认为该障碍已经被避过，可不予考虑，其二为障碍到移动机器人的距离大于有效距离，不会影响本次路径规划结果的执行，因此在当前局部路径规划过程中可不予考虑；

c.将所有的障碍初始分类定义为未知障碍；

(5)选取当前最优弧段

比较左右避障弧段，从其中选取曲率与默认规划路径的曲率最接近的，视为当前的最优弧段，最优弧段的曲率和弧长分别为S_c和L_c，即比较|S_l-S₀|和|S₀-S_r|，如果|S_l-S₀|≤|S₀-S_r|，则令S_c＝S_l、L_c＝L_l，否则令S_c＝S_r、L_c＝L_r；

(6)计算障碍与步骤(5)的当前最优弧段的相对位置关系，并对障碍进行重新分类

顺序读取障碍的信息，如果是未知障碍且障碍与移动机器人的距离大于规划有效距离D，则直接将其重分类为可忽略障碍；否则计算障碍与当前最优弧段的相对位置关系，判断障碍在最优弧段的左侧、或右侧，或落在最优弧段上，如果障碍落在最优弧段上，则中断步骤(6)进入步骤(7)，否则对障碍进行重新分类，然后读取下一障碍信息；如果全部障碍信息都已读取完毕，仍没有因为障碍在弧段上而中断，则当前最优弧段上没有障碍，为可行规划弧段，可直接转入步骤(8)输出规划结果；

(7)更新左右避障弧段

当前最优弧段为左避障弧段时，则更新左避障弧段；当前最优弧段为右避障弧段时，则更新右避障弧段，在更新左右避障弧段之后，障碍不再落在弧段上，对于左避障弧段，此时障碍在弧段右侧，对于右避障弧段，此时障碍在弧段左侧，然后再根据障碍的类别属性，对当前障碍进行重新归类；

结束上述工作后，返回步骤(5)重新选取最优弧段；

(8)输出局部路径规划结果

步骤(6)中若遍历所有障碍后，没有障碍落在当前最优弧段上，则当前最优弧段为可行规划弧段，将曲率S_c和弧长L_c作为当前最优规划路径输出给导航与控制系统，并结束当前局部路径规划进程。

2、根据权利要求1所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：所述步骤(1)中的局部路径规划坐标系的坐标原点位于当前规划时刻的移动机器人质心在水平面上的投影点上，X轴指向移动机器人的前进方向，Y轴位于水平面上，指向移动机器人左侧，Z轴满足右手螺旋定则。

3、根据权利要求1所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：所述步骤(2)中的默认输出曲率 $S_0={2y}_0/(x_0^2+y_0^2),$ (x₀，y₀)为目标点坐标。

4、根据权利要求1所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：所述步骤(2)中的默认输出弧长L₀的选择原则：

(1)局部路径规划的默认输出弧长L₀应大于2Vt，其中Vt为图像处理时间内移动机器人移动的距离；

(2)默认输出弧长L₀应不大于敏感器环境感知范围，即默认输出弧长L₀对应弧段的安全性可以通过当前敏感器的数据充分感知；

(3)相隔一个默认输出弧长L₀距离的两个位置点上所获取的环境数据应具有重合性，以便准确地把握环境信息。

5、根据权利要求1所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：所述的局部路径规划采用定周期执行，或变周期执行两种执行方式，所述步骤(2)中对于定周期局部路径规划，默认输出弧长为最终的输出弧长；对于变周期局部路径规划，则默认输出弧长为最小输出弧长，步骤(7)中还会根据障碍信息增大规划弧长的输出值。

6、根据权利要求1所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：所述步骤(6)中判断未知障碍是否在规划有效距离D之外的方法为：

如果y_l·y_r≤0，则障碍在X轴上，障碍到移动机器人的距离D_ob＝x_r；

如果y_r>0，则障碍在机器人的左侧， $D_{\mathrm{ob}}=\min (x_r,\sqrt{{(x_r-a/2)}^2+{(y_r-b/2)}^2});$

如果y_l<0，则障碍在机器人的右侧， $D_{\mathrm{ob}}=\min (x_r,\sqrt{{(x_r-a/2)}^2+{(y_l+b/2)}^2})$

若D_ob>D，则障碍为可忽略障碍，

(x_f，x_r，y_l，y_r)为已知障碍的边界坐标，分别代表前后边X坐标和左右边的Y坐标，弧段的曲率和弧长为S_c和L_c，a、b分别为移动机器人的长度和宽度。

7、根据权利要求1所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：所述步骤(6)判断障碍与弧段的相对位置关系，即障碍在弧段左侧、在弧段右侧还是落在弧段之上，其判断方法为：

(1)当|S_c|≤δ_c时，移动机器人的行走路径为直线段，此时有：

如果y_r≥b/2，障碍在弧段左侧；

如果y_l≤-b/2，障碍在弧段右侧；

如果上述两条都不满足，障碍在弧段上；

(2)当S_c>δ_c时，圆心在+Y轴上：

如果 $\sqrt{{\left(x_f\right)}^2+{(y_r-R_c)}^2}\&le;\left|R_c\right|-b/2,$ 障碍在弧段左侧；

如果 $\sqrt{{\left(x_r\right)}^2+{(y_l-R_c)}^2}\&GreaterEqual;\left|R_c\right|+b/2,$ 障碍在弧段右侧；

如果上述两条都不满足，障碍在弧段上；

(3)当S_c<-δ_c时，圆心在-Y轴上：

如果 $\sqrt{{\left(x_r\right)}^2+{(y_r-R_c)}^2}\&GreaterEqual;\left|R_c\right|+b/2,$ 障碍在弧段左侧；

如果 $\sqrt{{\left(x_f\right)}^2+{(y_l-R_c)}^2}\&le;\left|R_c\right|-b/2,$ 障碍在弧段右侧；

如果上述两条都不满足，障碍在弧段上；

其中δ_c为预先设置的一小量，(x_f，x_r，y_l，y_r)为已知障碍的边界坐标，分别代表前后边X坐标和左右边的Y坐标，S_c和L_c分别为最优弧段的曲率和弧长，a、b分别为移动机器人的长度和宽度，Rc为弧段半径。

8、根据权利要求1所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：所述步骤(6)中的障碍分类原则为：

a.当前最优弧段为左避障弧段时：

对于未知障碍，若障碍在左避障弧段左侧，则其必然也在右避障弧段的左侧，故重分类为左侧障碍；若障碍在左避障弧段右侧，则按定义自然属于右侧障碍；

对于左侧障碍，若障碍在左避障弧段左侧，分类不发生变化；若障碍在左避障弧段的右侧，根据分类定义，障碍同时也在右避障弧段的左侧，因此重分类为可忽略障碍；

b.当前最优弧段为右避障弧段时：

对于未知障碍，若障碍在右避障弧段左侧，则按定义自然属于左侧障碍；若障碍在右避障弧段右侧，则其必然也在左避障弧段的右侧，故重分类为右侧障碍；

对于右侧障碍，若障碍在右避障弧段左侧，根据分类定义，障碍同时也在左避障弧段的右侧，因此重分类为可忽略障碍；若障碍在右避障弧段的右侧，分类不发生变化；

将上述原则归纳为下列的表1

表1

9、根据权利要求8所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：所述步骤(7)更新左右避障弧段后再根据障碍的类别属性，按照上述表1对当前障碍进行重新归类。

10、根据权利要求1所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：所述步骤(7)中更新左避障弧段曲率的计算公式为：

$S_l=\frac{1}{R_l}=\frac{2(y_l+d)}{{\left(x_i\right)}^2+{\left(y_l\right)}^2-d^2}$

式中当y_l+d≥0时x_i＝x_r，当y_l+d<0时x_i＝x_f，a为移动机器人的长度、b为宽度， $d=\sqrt{{(a/2)}^2+{(b/2)}^2},$ R_l为左避障弧段圆心的Y坐标，其绝对值为左避障弧段至圆心的半径，D为规划有效距离，(x_f，x_r，y_l，y_r)为已知障碍的边界坐标；

如果局部路径规划中采用变周期执行，则还需要对左避障弧段的弧长进行更新，弧长计算公式为：

$L_l=\min [D,\max (L_l,\left|R_l{\sin }^{-1}\frac{x_f}{R_l+d}\right|)].$

11、根据权利要求1所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：所述步骤(7)中更新右避障弧段曲率的计算公式为：

$S_r=\frac{1}{R_r}=\frac{2(y_r-d)}{{\left(x_i\right)}^2+{\left(y_r\right)}^2-d^2}$

式中当y_r-d≥0时x_i＝x_f，当y_r-d<0时x_i＝x_r，a为移动机器人的长度、b为宽度， $d=\sqrt{{(a/2)}^2+{(b/2)}^2},$ R_r为右避障弧段圆心的Y坐标，其绝对值为右避障弧段至圆心的半径，D为规划有效距离，(x_f，x_r，y_l，y_r)为已知障碍的边界坐标；

如果局部路径规划中采用变周期执行，则还需要对右避障弧段的弧长进行更新，弧长计算公式为：

$L_r=\min [D,\max (L_r,\left|R_r{\sin }^{-1}\frac{x_f}{R_r-d}\right|)].$

12、根据权利要求10或11所述的基于二元环境信息的移动机器人局部路径规划方法，其特征在于：在计算得到的曲率S_l或S_r趋近于零时，计算会出现半径无穷大，则左避障弧段和右避障弧段弧长的计算公式改为：

L＝min[D，max(L,x_f)]。

Images