CN112882479B - 一种基于内外角累积状态的路径规划方法 - Google Patents

Abstract

本发明公开一种基于内外角累积状态的路径规划方法，其特征在于，包括以下步骤：S1、机器人设定多状态程序结构体系，实现多种功能状态，包括势场避障状态、忽略障碍状态及沿墙行走状态；S2、当未陷入局部极小时，选择势场避障状态，机器人利用势场法避障；基于势场模型进行路径规划；S3、当机器人陷于局部极小点时，针对局部极小点的特征，选择忽略障碍状态或沿墙行走状态这两种辅助状态；两种辅助状态的选择，通过行进过程中的角度累计变化信息来进行判断；本发明提出的方法能够利用传感器信息在线解决势场法的局部极小问题，适用于初次通过且完全未知的复杂环境，具有较好的应用价值。

Description

一种基于内外角累积状态的路径规划方法

技术领域

本发明属于机器人技术领域，尤其涉及机器人路线规划技术领域，具体涉及一种基于内外角累积状态的路径规划方法。

背景技术

机器人的路线规划主要采用势场法，势场法最初是由khatib教授提出，通过引入广义势场的概念，在三维空间中构造人为势场，解决了机械臂运动规划问题。人为势场由目标点产生的引力场和周围障碍产生的斥力场叠加而成。在叠加势场的作用下，机器人只能沿着叠加势场的负梯度方向运动，运动过程中受到最终目标点的引力作用而靠近，同时又受到周围障碍的斥力作用而躲避，进而形成起始点与目标点之间的有效路径。但由于实际环境中障碍分布的复杂性，存在一些导致机器人无法脱离的局部极小区域，如图1所示。在起始点与目标点间存在L形障碍，根据周围环境生成的人工势场可知：若机器人沿着势场的负梯度方向运动，将进入凹形障碍物内部，此时因为斥引力大小相等方向相反，所以合力矢量无法为机器人提供下一步运动的方向和驱动力，导致容易出现运动停止或者产生运动震荡，最终无法到达目标点——即机器人困于局部极小点而无法到达目标，导致路径规划任务失败。

尽管势场法存在导致规划失败的局部极小问题；但相比于其他复杂路径规划算法，势场法具有规划复杂度低、实时性高等优点。很多专家都针对势场法存在的局部极小问题，提出了相应的改进策略，改进策略从整体上可分为两种：

算法一：依靠传感器采集的环境信息进行局部规划：这种局部规划方法，当陷入局部极小点后，依靠传感器采集的环境信息进行局部在线规划，多适用于未知环境。常见算法有随机搜索法、设置虚拟目标点法、沿墙算法。其中，随机搜索是模仿分子布尔运动实现避障，但效率较低；设置虚拟目标点法又包含设置虚拟目标及设置虚拟障碍法：设置虚拟目标点法主要是基于周围环境信息以选择合适的子目标，该算法中合理子目标点的选择尤为重要；设置虚拟障碍法则通过记忆当前位置，并将该位置用虚拟障碍填补以改变原有势场分布的方式引导机器人逃离局部极小点，该算法依赖机器人自身定位精度且在线计算量较大；沿墙(Bug)法利用沿墙方式逃离陷入局部极小的陷阱，算法的核心在于设计进入与脱离沿墙状态间的切换条件——如J.Antich提出的依据陷入局部极小后跟踪障碍的轮廓特征，J.Borenstein等人提出的基于机器人前进方向与目标角度的夹角关系特征，ZhuA,Yang SX等人提出在机器人与目标距离关系特征，分别实现沿墙状态的切换；但是，上述沿墙算法均存在路径非最优或使机器人死循环的不足。

算法二、依靠建图和全局规划思想进行整体规划。这种整体规划的方法，陷入局部极小点后依靠建图和全局规划思想进行整体规划，多适用于已知环境。其中，搜索法利用最佳优先、模拟退火、多势场切换等全局规划策略在陷入极小值点后规划路径；Wonyun Choi等人提出全局路径预规划与实时路径势场法相结合的方式，可在预测阶段提前避开局部极小点；Stentz A提出的DynamicA*算法，能够通过算法迭代在已知环境中产生较短的路径。Zhang T提出的依靠建图辅助规划路径的方法适用于未知环境但规划的路径基于实时建图，存在较大的时间和空间复杂度。

综上所述，第一类算法依靠传感器扫描周围环境，计算量较少，但是，规划出的路径因为信息太少导致路径不是最优。第二类算法，虽可实现较好的路径规划，但需要采集更多的环境信息且计算量较大。当机器人需要快速实现首次面对信息完全未知的场景时，上述算法虽能避开障碍到达目标点，但或多或少存在自身的局限性，因此，针对势场法路径规划中存在的陷入局部极小问题。

发明内容

发明目的：针对现有技术中存在问题或不足，本发明提供一种基于内外角累积状态的路径规划方法，以解决背景技术中势场法路径规划中存在的问题。

为实现上述发明目的，本发明的实施例提供一种基于内外角累积状态的路径规划方法，其特征在于，包括以下步骤：

S1、机器人设定多状态程序结构体系，实现多种功能状态，包括势场避障状态、忽略障碍状态及沿墙行走状态；

S2、当未陷入局部极小时，选择势场避障状态，机器人利用势场法避障；基于势场模型进行路径规划；

S3、当机器人陷于局部极小点时，针对局部极小点的特征，选择忽略障碍状态或沿墙行走状态这两种辅助状态；两种辅助状态的选择，通过行进过程中的角度累计变化信息来进行判断；

S3-1、当机器人陷于局部极小点时，首先判断前方是否有障碍；若无前方无障碍，选择忽略障碍状态，若前方有障碍，选择沿墙行走状态；

S3-2、若选择沿墙行走状态，判断是否绕过障碍，若绕过障碍，则进行步骤S2的势场避障状态，若未绕过障碍，继续选择沿墙行走状态。

进一步的，所述步骤S2中，基于势场模型进行路径规划，具体包括以下方法：

每个采样时刻，机器人线速度，角速度和前进的期望方向如下：

ω＝ω_max(θ_res-θ₀)

θ_exp＝θ_res

其中：υ_max是机器人最大线速度，ω_max是机器人最大偏转增益常数，F_r是障碍物给予机器人的斥力的大小，θ_rep-θ₀是合力方向与机器人前进方向的夹角，θ_rep-θ₀是斥力方向与机器人前进方向的夹角，θ_exp是机器人期望运动方向，θ_res是合力方向。

进一步的，所述忽略障碍状态，即奔向目标状态进行路径规划，具体包括以下方法：每个采样时刻，机器人线速度，角速度和前进的期望方向如下：

ν＝υ_max

0_exp＝θ_att

忽略障碍物对机器人的影响，机器人的运动方向只参考引力方向θ_att。

进一步的，所述沿墙行走状态进行路径规划，具体包括以下方法：每个采样时刻，机器人线速度，角速度和前进的期望方向如下：

υ＝υ_max(1-|cos(θ_rep-θ₀)|)

ω＝ω_max(θ_res-θ₀)+ω_d(d₀-d_s)

θ_exp＝θ_rep-α

其中，d₀-d_s为机器人与墙体的实测距离和预设机器人到墙标准距离的距离差，修正值ω_d(d₀-d_s)可以让机器人与墙体之间的距离保持在预设值，α为偏转角，α的理论值为90°，当以速度v靠近障碍并进入沿墙模式后，机器人以期望运动方向θ_exp为参照，沿墙行走并调整自身姿态与墙体平行。

进一步的，所述步骤S3中，累积角度计算的过程如下：

θ＝∑[θ(t)-θ(t-1)]，θ(t)-θ(t-1)＞β；

θ(t)为采样时间T＝t时的前进角，θ表示累积变化角，β为单位滤波角；当变化的差值大于β时，则将变化量计入累计角度。

进一步的，所述势场避障状态的结束条件为：条件1或条件2成立；其中，

条件1:|F_att+F_rep|≤ε；

条件2:|θ_att-θ_sum|≥90°；

其中，合力大小小于等于ε时，即条件1成立，说明机器人陷入局部极小点；若机器人陷入局部极小点后出现抖动和原地旋转情况，条件1无法检测到上述情况；则从机器人前进过程中斥引力与合力的变化出发，判断结束条件2，用于检测上述抖动和原地旋转情况是否发生，在T∈[0，t₁)时刻机器人正处于势场的避障状态，此时由于障碍物在探测范围以外，机器人的运动方向参考引力方向向目标点移动；在T∈[t₁，t₂)时刻随着机器人探测到周围障碍并逐渐逼近，斥力逐渐增大，合力的方向开始逐渐偏向斥力方向；针对在t₂时刻合力方向与引力方向徒增设计结束条件，在满足势场避障状态和忽略障碍状态结束条件时，随即进入沿墙模式。

优选的，所述忽略障碍状态，即忽略障碍斥力，沿着引力方向的直线运动；所述忽略障碍状态的结束条件为：条件3和条件4同时成立；

条件3：θ_{r_i}≠180°；

条件4：d_target＞s₁；

其中，θ_{r_i}为各障碍物给机器人的斥力方向，且没有正前方障碍给予的斥力，d_target为目标离机器人距离，s₁为安全距离；条件3和条件4同时成立时，表示遇到多个障碍形成局部极小的情况，此时机器人不需要转入沿墙行走状态，可通过继续直线行走奔向目标。

进一步的，所述沿墙行走状态，即沿着障碍物的边缘运动，所述沿墙行走状态的结束条件为：条件5、条件6、条件7和条件7同时成立；

条件5：θ_total≈90°；

条件6：|θ_outer|＞90°；

条件7：D_last＞D_now；

条件8：d_target＞s₁；

其中，外角θ_outer设置为当机器人处于沿墙状态时，以向障碍同侧旋转的转角；内角θ_inter设置为当机器人处于沿墙状态时，向障碍异侧旋转的转角；记内角θ_inter为正，外角θ_outer为负；相对累积角θ_total即为内角θ_inter和外角θ_outer的算数和；当θ_total＝0时，表示所经过路线内角外角正负相抵，当该路径点的机器人姿态与开始姿态相同时，即可将该路径点命名为“关键重置点”，D_last为上一个“关键重置点”与目标点间的垂直距离，D_now为当前“关键重置点”与目标点间的垂直距离；结束条件描述了相对累积角θ_total为0后，累积外角θ_outer绝对值大于90°且上一个“关键重置点”与目标点间的垂直距离(D_last)大于当前位置与目标点间的垂直距离(D_now)的情况，满足该结束条件就回到势场导航状态。

进一步的，所述势场局部极小点分成两种情况：(1)障碍物形成的局部极小点位于机器人与目标之间，这种情况下需要绕过该障碍物；(2)周边多个障碍物的合斥力造成了局部极小点，且机器人位置与目标位置间的连线上并不存在障碍物，只需沿着引力方向直线向目标运动即可；在陷入局部极小点后，机器人可以先直奔目标，在发现前方存在障碍物的情况下，再进行沿墙运动以绕过障碍物。

本发明的上述技术方案的有益效果如下：本发明一种基于内外角累积状态的路径规划方法，首先基于势场新模型提出了采用多状态程序结构法，定义了势场法避障状态、忽略目标状态和沿墙行走状态等三种状态；再融合了目标点法和Bug算法，基于运行过程中的机器人自身转动角度累计变化信息，设计了合理的状态转换条件；最后，在Webots机器人平台仿真实现了当机器人在势场法避障状态陷入局部极小点后的避障及逃脱，仿真实验结果表明，提出的方法能够利用传感器信息在线解决势场法的局部极小问题，适用于初次通过且完全未知的复杂环境，具有较好的应用价值。

附图说明

图1为本发明的背景技术中L形障碍周围势场图。

图2为本发明的实施例中状态间相互切换关系的流程图。

图3为本发明的实施例中避障时斥引力以及合力方向和大小变化图；其中，图3(a)为斥引力以及合力方向图，图3(b)为斥引力以及合力大小。

图4为本发明的实施例中狭窄通道产生的局部极小图。

图5为本发明的实施例中示例环境下理想路径规划图。

图6为本发明的实施例中主体坐标系和沿墙运动受力，其中，图6(a)为自制机器人SmartCar距离传感器分布图；图6(b)为沿墙行走状态下受力图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合具体实施例进行详细描述。

为解决未知环境下势场法的局部极小问题及保证算法实时性，本发明首先设定了基于多状态程序结构，包括势场法避障状态在内的多种功能状态；其次，针对局部极小点特征，选择了忽略障碍和沿墙行走两种辅助状态；最后，引入了行进过程中的角度累计变化信息来增强机器人的在各状态间切换的能力，实现了当势场法避障状态陷入局部极小点时的逃脱、避障前进等有效路径规划。

每个控制周期机器人通过传感器的实时数据和角度信息，对当前环境进行判断，决定运行状态；通过三种状态相互间的有效配合与转换，实现未知环境下势场法的路径规划，各状态之间的转换关系如图2所示，多状态程序结构体系直接面向底层控制，具有良好的实时性，可以方便地通过增加或减少状态对体系结构进行模块化的修改与调整；多状态程序结构体系间所涉及的功能状态及转换条件如下。

状态1：基于势场的避障状态

状态描述：当未陷入局部极小时，机器人利用势场法避障。

结束条件：1|2

条件1:|F_att+F_rep|≤ε

条件2:|θ_att-θ_sum|≥90°

本发明中所有结束条件中，“&”表示逻辑与，“|”表示逻辑或；条件1成立意味着合力大小接近于0(ε是一个很小的正数)，这是最常见的判断局部极小的条件，机器人陷入局部极小后常会出现抖动和原地旋转情况，而条件1却常常无法检测到上述情况，所以本发明从机器人前进过程中斥引力与合力的变化出发，设置结束条件2，用于检测上述情况是否发生，如图3所示，在T∈[0，t₁)时刻机器人正处于势场的避障状态，此时由于障碍物在探测范围以外，所以机器人的运动方向参考引力方向向目标点移动；在T∈[t₁，t₂)时刻随着机器人探测到周围障碍并逐渐逼近，斥力逐渐增大，合力的方向开始逐渐偏向斥力方向；针对在t₂时刻合力方向与引力方向徒增设计结束条件，在满足势场避障状态和忽略障碍状态结束条件时，随即进入沿墙模式。

状态2：忽略障碍状态：

状态描述：忽略障碍斥力，沿着引力方向的直线运动。

有两种环境会形成实际场景中势场局部极小点，第一种情况是障碍物形成的局部极小点位于机器人与目标之间，这种情况下需要绕过该障碍物，如图1所示；第二种情况则是周边多个障碍物的合斥力造成了局部极小点，但机器人位置与目标位置间的连线上并不存在障碍物，只需沿着引力方向直线向目标运动即可，如图4所示，因此，在陷入局部极小点后，机器人可以先直奔目标，在发现前方存在障碍物的情况下，再进行沿墙运动以绕过障碍物。

忽略障碍状态结束条件：条件3&条件4

条件3：θ_{r_i}≠180°

条件4：d_target＞s₁

其中，θ_{r_i}为各障碍物给机器人的斥力方向,其中没有正前方障碍给予的斥力，d_target为目标离机器人距离,s₁为安全距离。条件3和条件4同时成立即描述了遇到多个障碍形成局部极小的情况，此时机器人不需要转入沿墙行走状态，可通过继续直线行走奔向目标。

状态3：沿墙行走状态

状态描述：沿着障碍物的边缘运动。

沿墙行走状态结束条件：5&6&7&8

条件5：θ_total≈90°

条件6：|θ_outer|＞90°

条件7：D_last＞D_now

条件8：d_target＞s₁

当机器人处于沿墙状态时，以向障碍同侧旋转的转角定义为外角“θ_outer”，向障碍异侧旋转的转角定义为内角“θ_inter”，记内角为正，外角为负，相对累积角θ_total即为内角θ_inter和外角θ_outer的算数和；当θ_outer＝0时，表示所经过路线内角外角正负相抵。当该路径点的机器人姿态与开始姿态相同时，即可将该路径点命名为“关键重置点”，D_last为上一个“关键重置点”与目标点间的垂直距离，D_now为当前“关键重置点”与目标点间的垂直距离，结束条件描述了相对累积角θ_total为0后，累积外角θ_outer绝对值大于90°且上一个“关键重置点”与目标点间的垂直距离(D_last)大于当前位置与目标点间的垂直距离(D_now)的情况，满足该结束条件就回到势场导航状态。该结束条件具有通用一般性，可以有效应对任意形状的墙体障碍物，主要特征在于：在势场状态下，当机器人直面目标移动，当遇墙体避障进入沿墙模式时，设置初始状态(θ_inter＝θ_outer＝θ_total＝0)，机器人前进方向与墙面平行；此时如果能将机器人向障碍物偏转一定角度，即可重新直面目标。而在沿墙模式中，由于只有在墙体缺口处机器人才有可能向障碍物偏转外角θ_outer；因此，当机器人仅满足移动到在“关键重置点”时，不能退出沿墙模式，而是将机器人状态重置为初始状态(θ_inter＝θ_outer＝θ_total＝0)，将此刻环境当成新的未知环境重新规划导航，以此循环；唯有当机器人经过“关键重置点”，且当向障碍物偏转外角θ_outer＞90°时，方能同时满足条件5和条件6，即可认为机器人进入关键点，重新直面目标，结束沿墙模式。

实施例1：机器人模型

四轮移动机器人SmartCar仿照Poineer 3型机器人搭建，在其顶部安装的距离罗盘由数个测距传感装置组成，图6(a)描述了传感器的分布情况，距离罗盘可用于扫描周围环境信息，扫描的数据可以表示为(d_i，θ_i)，其中d_i，θ_i分别表示为以机器人为中心相对障碍的距离和方向，系统将在每个采样时间点对距离传感器从i＝1，...，N(N＝16)逆时针依次采样，图示坐标系定义为“主体坐标系”，仿真时以初始时刻的主体坐标系作为每次路径规划的“全局坐标系”。

实施例2：算法实现

1)势场避障状态

每个采样时刻，机器人线速度，角速度和前进的期望方向如下：

ω＝ω_max(θ_res-θ₀)

θ_exp＝θ_res

其中：υ_max是机器人最大线速度，ω_max是机器人最大偏转增益常数，F_r是障碍物给予机器人的斥力的大小，θ_rep-θ₀是合力方向与机器人前进方向的夹角，θ_rep-θ₀是斥力方向与机器人前进方向的夹角，θ_exp是机器人期望运动方向，θ_res是合力方向。

2)奔向目标状态

每个采样时刻，机器人线速度，角速度和前进的期望方向如下：

ν＝υ_max

θ_exp＝θ_att

忽略障碍物对机器人的影响，机器人的运动方向只参考引力方向θ_att。

3)沿墙行走状态

每个采样时刻，机器人线速度，角速度和前进的期望方向如下：

υ＝υ_max(1-|cos(θ_rep-θ₀)|)

ω＝ω_max(θ_res-θ₀)+ω_d(d₀-d_s)

θ_exp＝θ_rep-α

其中：d₀-d_s是机器人与墙体的实测距离和预设机器人到墙标准距离的距离差，修正值ω_d(d₀-d_s)可以让机器人与墙体之间的距离保持在预设值，α为偏转角(理论值为90°)，当以速度v靠近障碍并进入沿墙模式后，机器人以期望运动方向θ_exp为参照，沿墙行走并调整自身姿态与墙体平行，如图6(b)所示。

4)累积角度计算：

θ＝∑[θ(t)-θ(t-1)]，θ(t)-θ(t-1)＞β

式中，θ(t)为采样时间T＝t时的前进角，θ表示累积变化角，β为单位滤波角；当变化的差值大于β，则将变化量计入累计角度。

实施例3：环境实例

机器人路径规划过程如图5所示，机器人从起始点(S)受势力场影响向目标(T)出发，当遇到墙体时，根据状态切换关系，由于满足状态2结束条件机器人便进入沿墙状态——此时，机器人将车身调整至与墙相切方向(A点)，同时设置初始状态(θ_inter＝θ_outer＝θ_total＝0)，记录此时与目标之间的垂直距离D_A，此后,继续前进寻找“关键重置点”或满足当前状态的结束条件位置；在沿墙过程中，当机器人遇到转角时，由于检测到墙体在右侧且自身受斥力影响向左偏转，根据内角定义，该转角为内角(如A～B段内产生的转角)，此时机器人在B点时的自身角度参数分别为：θ_inter＝+90°，θ_outer＝0°，θ_total＝+90°；同理可得，C点θ_inter＝+180°，θ_outer＝0°，θ_total＝+180°；当机器人前进到D点时，自身角度参数分别为θ_inter＝+180°，θ_outer＝-90°，θ_total＝+90°，由于外角参数产生一定变化，需要计算自身与目标间的垂直距离D_D，由于D_D＞D_A，机器人将继续保持沿墙模式；当机器人到达E点时，自身角度参数分别为θ_inter＝+180°，θ_outer＝-180°，θ_total＝0°，此时自身角度参数满足“关键重置点”条件，机器人将状态重置为与A点相同初始状态θ_inter＝θ_outer＝θ_total＝0°，并计算与目标之间的垂直距离D_E，继续保持先前状态进行路径规划；当机器人探测到右侧障碍且自身向右偏转时，如在E～F段产生相应的外角时，机器人在F点时的自身角度参数为θ_inter＝0°，θ_outer＝-90°，θ_total＝-90°，累积外角达到-90°，，此时计算与目标之间的垂直距离D_F，由于D_F＜D_E，满足退出沿墙模式条件，机器人在F点重新回到受势场影响的路径规划，直到到达目标(T)位置。

本发明首先基于势场新模型提出了采用多状态程序结构法，定义了势场法避障状态、忽略目标状态和沿墙行走状态等三种状态；再融合了目标点法和Bug算法，基于运行过程中的机器人自身转动角度累计变化信息，设计了合理的状态转换条件；最后，在Webots机器人平台仿真实现了当机器人在势场法避障状态陷入局部极小点后的避障及逃脱，仿真实验结果表明，提出的方法能够利用传感器信息在线解决势场法的局部极小问题，适用于初次通过且完全未知的复杂环境，具有较好的应用价值。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (5)

1.一种基于内外角累积状态的路径规划方法，其特征在于，包括以下步骤：

S1、机器人设定多状态程序结构体系，实现多种功能状态，包括势场避障状态、忽略障碍状态及沿墙行走状态；

S2、当未陷入局部极小时，选择势场避障状态，机器人利用势场法避障；基于势场模型进行路径规划；所述基于势场模型进行路径规划，具体包括以下方法：

每个采样时刻，机器人线速度，角速度和前进的期望方向如下：

ω＝ω_max(θ_res-θ₀)

θ_exp＝θ_res

其中：v_max是机器人最大线速度，ω_max是机器人最大偏转增益常数，F_r是障碍物给予机器人的斥力的大小，θ_rep-θ₀是合力方向与机器人前进方向的夹角，θ_rep-θ₀是斥力方向与机器人前进方向的夹角，θ_exp是机器人期望运动方向，θ_res是合力方向；

S3、当机器人陷于局部极小点时，针对局部极小点的特征，选择忽略障碍状态或沿墙行走状态这两种辅助状态；两种辅助状态的选择，通过行进过程中的角度累计变化信息来进行判断；

其中，所述忽略障碍状态，即奔向目标状态进行路径规划，具体包括以下方法：每个采样时刻，机器人线速度，角速度和前进的期望方向如下：

v＝v_max

θ_exp＝θ_att

忽略障碍物对机器人的影响，机器人的运动方向只参考引力方向θ_att；

所述沿墙行走状态进行路径规划，具体包括以下方法：每个采样时刻，机器人线速度，角速度和前进的期望方向如下：

v＝v_max(1-|cos(θ_rep-θ₀)|)

ω＝ω_max(θ_res-θ₀)+ω_d(d₀-d_s)

θ_exp＝θ_rep-α

其中，d₀-d_s为机器人与墙体的实测距离和预设机器人到墙标准距离的距离差，修正值ω_d(d₀-d_s)可以让机器人与墙体之间的距离保持在预设值，α为偏转角，α的理论值为90°，当以速度v靠近障碍并进入沿墙模式后，机器人以期望运动方向θ_exp为参照，沿墙行走并调整自身姿态与墙体平行；

所述累积角度计算的过程如下：

θ＝∑[θ(t)-θ(t-1)]，θ(t)-θ(t-1)＞β；

θ(t)为采样时间T＝t时的前进角，θ表示累积变化角，β为单位滤波角；当变化的差值大于β时，则将变化量计入累计角度；

S3-1、当机器人陷于局部极小点时，首先判断前方是否有障碍；若无前方无障碍，选择忽略障碍状态，若前方有障碍，选择沿墙行走状态；

S3-2、若选择沿墙行走状态，判断是否绕过障碍，若绕过障碍，则进行步骤S2的势场避障状态，若未绕过障碍，继续选择沿墙行走状态。

2.根据权利要求1所述的一种基于内外角累积状态的路径规划方法，其特征在于，所述势场避障状态的结束条件为：条件1或条件2成立；其中，

条件1：|F_att+F_rep|≤ε；

条件2：|θ_att-θ_sum|≥90°；

其中，合力大小小于等于ε时，即条件1成立，说明机器人陷入局部极小点；若机器人陷入局部极小点后出现抖动和原地旋转情况，条件1无法检测到上述情况；则从机器人前进过程中斥引力与合力的变化出发，判断结束条件2，用于检测上述抖动和原地旋转情况是否发生，在T∈[0，t₁)时刻机器人正处于势场的避障状态，此时由于障碍物在探测范围以外，机器人的运动方向参考引力方向向目标点移动；在T∈[t₁，t₂)时刻随着机器人探测到周围障碍并逐渐逼近，斥力逐渐增大，合力的方向开始逐渐偏向斥力方向；针对在t₂时刻合力方向与引力方向徒增设计结束条件，在满足势场避障状态和忽略障碍状态结束条件时，随即进入沿墙模式。

3.根据权利要求1所述的一种基于内外角累积状态的路径规划方法，其特征在于，所述忽略障碍状态，即忽略障碍斥力，沿着引力方向的直线运动；所述忽略障碍状态的结束条件为：条件3和条件4同时成立；

条件3：θ_{r_i}≠180°；

条件4：d_target＞s₁；

其中，θ_{r_i}为各障碍物给机器人的斥力方向，且没有正前方障碍给予的斥力，d_target为目标离机器人距离，S₁为安全距离；条件3和条件4同时成立时，表示遇到多个障碍形成局部极小的情况，此时机器人不需要转入沿墙行走状态，可通过继续直线行走奔向目标。

4.根据权利要求1所述的一种基于内外角累积状态的路径规划方法，其特征在于，所述沿墙行走状态，即沿着障碍物的边缘运动，所述沿墙行走状态的结束条件为：条件5、条件6、条件7和条件8同时成立；

条件5：θ_total≈90°；

条件6：|θ_outer|＞90°；

条件7：D_last＞D_now；

条件8：d_target＞s₁；

其中，外角θ_outer设置为当机器人处于沿墙状态时，以向障碍同侧旋转的转角；内角θ_inter设置为当机器人处于沿墙状态时，向障碍异侧旋转的转角；记内角θ_inter为正，外角θ_outer为负；相对累积角θ_total即为内角θ_inter和外角θ_outer的算数和；当θ_total＝0时，表示所经过路线内角外角正负相抵，当该路径点的机器人姿态与开始姿态相同时，即可将该路径点命名为“关键重置点”，D_last为上一个“关键重置点”与目标点间的垂直距离，D_now为当前“关键重置点”与目标点间的垂直距离；结束条件描述了相对累积角θ_total为0后，累积外角θ_outer绝对值大于90°且上一个“关键重置点”与目标点间的垂直距离(D_last)大于当前位置与目标点间的垂直距离(D_now)的情况，满足该结束条件就回到势场导航状态。

5.根据权利要求1所述的一种基于内外角累积状态的路径规划方法，其特征在于，所述势场局部极小点分成两种情况：(1)障碍物形成的局部极小点位于机器人与目标之间，这种情况下需要绕过该障碍物；(2)周边多个障碍物的合斥力造成了局部极小点，且机器人位置与目标位置间的连线上并不存在障碍物，只需沿着引力方向直线向目标运动即可；在陷入局部极小点后，机器人可以先直奔目标，在发现前方存在障碍物的情况下，再进行沿墙运动以绕过障碍物。

Images