CN110032187B - 无人摩托静态障碍避障路径规划计算方法 - Google Patents

Abstract

本发明公开了一种无人摩托静态障碍避障路径规划方法，该方法包括以下步骤：建立无人摩托的平衡动力学模型与运动学模型；使用自适应分辨率的A*算法，在栅格地图上获取一条连接初始节点与目标节点的通路；结合平衡动力学模型与运动学模型，使用RRT算法对所述通路进行剪枝，生成一条无人摩托可执行的路径。该方法通过采用低精度地图，多层次展现环境，以及采用历史路径、历史地图的剪枝技术，极大降低算法的运行时间，实现路径规划中对于实时性的需求，当环境变化时也能够较快地适应，从而可以拓展为动态障碍物的避障算法。

Description

无人摩托静态障碍避障路径规划计算方法

技术领域

本发明涉及无人车智能决策规划与控制执行技术领域，特别涉及一种无人摩托静态障碍避障路径规划计算方法。

背景技术

随着交通需求的日益增长，驾驶成为日常生活必不可少的技能，同时也占据了人们大量的时间，可以预见在未来将会有大量驾驶工作的需求。各类汽车厂商以及高校均期望在数年时间之内研发出能够适应绝大多数路面条件的无人驾驶。而无人驾驶摩托车又是在无人驾驶中相对较新的领域，能够进行更为灵活的运动。摩托车作为一种特殊构型的机器人，兼备速度高、噪音小与野外通过性好的特性，其无人化在各领域均有较广应用前景。

无人摩托车中涉及到感知、规划、平衡控制，其中规划涉及到路径规划与轨迹规划，本发明提供了一种路径规划方法。路径规划是指车辆根据传感器接收到的，或事先在地图上采集到的信息，对于所即将采取的行进路径进行规划，寻求一条在两点之间符合约束条件的路径，约束条件可以为路径最短、机械功最小、可实现性、安全性、曲线平滑性等。

路径规划当中对于约束条件影响最大的即为障碍物，在本发明中假设障碍物均可以视作静态障碍物，也即障碍物均为静止不动的、具有一定几何形状的物体。根据障碍物进行避障操作是需要研究的目标。与一般车辆相比，摩托车重量较轻，加减速过程时间短，且面对障碍物时，摩托车体型更小，较容易穿越障碍物，从而具有更强的在野外运动能力。

目前应用较为广泛的传统路径规划算法为A*(A star)算法，其缺点在于存在大量不必要的冗余计算，实时性不高，远远达不到在线规划的要求。另外一类研究较为广泛的方法是RRT(Rapidly-exploring Random Trees，快速搜索随机树)法，缺点在于由于采用随机的策略，路径平滑性较差，另外产生大量不必要的随机树拓展节点导致收敛速度较慢，同样不利于实时性要求较高的场合。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一。

为此，本发明的目的在于提出一种无人摩托静态障碍避障路径规划计算方法，该方法能够保留其优越性能，且大大降低了算法运行时间，最终生成可执行的、完备的、实时规划的路径，极大地满足了实用需求。

为达到上述目的，本发明提出了无人摩托静态障碍避障路径规划计算方法,包括以下步骤：S1，建立无人摩托的平衡动力学模型与运动学模型；S2，使用自适应分辨率的A*算法，在栅格地图上获取一条连接初始节点与目标节点的通路；S3，结合所述平衡动力学模型与所述运动学模型，使用RRT算法对所述通路进行剪枝，生成一条无人摩托可执行的路径。

本发明实施例的无人摩托静态障碍避障路径规划计算方法，通过采用低精度地图，多层次展现环境，以及采用历史路径、历史地图的剪枝技术，极大降低算法的运行时间，实现路径规划当中对于实时性的需求，又由于实时性能够保障，当环境变化时也能够较快地适应，从而可以拓展为动态障碍物的避障算法；路径规划具有完备性，也即若存在从起点到终点的路径，则必定能够找到路径，否则将会返回规划失败的信息；拓展得到的路径完全基于车体的动力学特性，因而完全可实现，路径能够满足所有的约束条件；采用图搜索的方法，十分直观地表现出路径规划的过程和结果；由于路径是完备的且完全可实现，因此可以保证安全性，不会发生意外碰撞。

另外，根据本发明上述实施例的无人摩托静态障碍避障路径规划计算方法还可以具有以下附加的技术特征：

进一步地，在本发明的一个实施例中，所述无人摩托的动力学模型为：

其中，φ为无人摩托的车身倾斜角，δ为无人摩托的车把转向角，a为无人摩托重心与后轮落地点的水平距离，h为车身无倾斜时无人摩托重心到地面的距离，λ为无人摩托的前叉角，v_x为无人摩托后轮的前向速度，c为无人摩托拖尾，b为无人摩托轴距，g为重力加速度，s为拉普拉斯算子。

进一步地，在本发明的一个实施例中，所述运动模型为：

其中，v为车辆速度，θ为航向角，δ为无人摩托的车把转向角，c为无人摩托拖尾，λ为无人摩托的前叉角，φ为无人摩托的车身倾斜角，b为无人摩托轴距，

为在绝对坐标地图中的横向速度，

为在绝对坐标地图中的纵向速度，

为航向角速度。

进一步地，在本发明的一个实施例中，步骤S201，对所述栅格地图进行所述自适应分辨率；步骤S202，在粗精度地图上寻路，若寻路成功，进入下一步，否则当前地图没有所述初始节点与所述目标节点的通路；步骤S203，若路径途径模糊栅格，则将模糊栅格及其附近部分取出，进行局部重规划。

进一步地，在本发明的一个实施例中，在所述栅格地图上的栅格由0和1布尔数组成，其中，0为障碍物区域，1为空旷区域。

进一步地，在本发明的一个实施例中，所述步骤S2还包括：步骤S204，若所述局部重规划成功，则输出整条路径，结束算法，否则将模糊栅格值置为0，设置重规划标志；步骤S205，若存在所述重规划标志，则重新执行算法，若当前地图上无可行路径，则提高地图精度，进入所述步骤S202重新执行算法。

进一步地，在本发明的一个实施例中，所述自适应分辨率以清晰度作为评价标准，清晰度map_def为：

其中，地图中两种类型的栅格个数分别为num_int和num_dec，num_int为完全的障碍或完全无障碍，num_dec为其余格栅。

进一步地，在本发明的一个实施例中，所述步骤S3包括：步骤S301，初始化搜索树、历史路径和历史地图；步骤S302，若达到所述目标节点，则由所述搜索树追溯父节点，获取整条规划路径，结束算法，否则进入下一步；步骤S303，选取拓展节点，选取与所述拓展节点评估距离最近的节点，选取由最近的节点向所述拓展节点拓展的控制量，根据所述控制量从最近的节点到达新节点；步骤S304，若拓展成功，将所述新节点加入所述搜索树，更新所述历史路径和所述历史地图，否则以适中间隔值取所述控制量进行拓展，直至拓展成功，若拓展失败，则将所述最近节点的死亡次数设置为较大数值，并返回至所述步骤S302。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1是本发明一个实施例的启发式的传统路径规划方法实现过程的效果图；

图2是本发明一个实施例的快速搜索随机树法实现过程的效果图，(a)图和(b)图中的左侧均为生成的随机树，右侧均为最终规划得到的路径；

图3是本发明一个实施例的无人摩托静态障碍避障路径规划计算方法流程图；

图4是本发明一个实施例的不同清晰度的效果图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

首先，下面现对现有技术的技术方案和技术方案存在的缺陷进行描述。

1)启发式的传统路径规划方法

A*算法是一种图搜索法，A*算法需要在离散的栅格化地图中才能够使用，在基本的A*算法中，按照以下步骤进行全局路径规划：

步骤一：将初始节点置入开启列表；

步骤二：若开启列表中没有节点，则路径探索失败。否则进行下一步；

步骤三：选取当前节点。评估条件为开启列表中代价函数f(·)值最低的节点。对与当前节点相邻的节点做以下操作：

a)障碍或关闭列表中的节点，不操作；

b)不在开启列表中的节点，加入到开启列表。以当前节点作为父节点，计算其代价函数值；

c)已经在开启列表中，若以当前节点为父节点计算得到的代价函数值小于原值，则更新其父节点和代价函数值。

步骤四：当前节点置入关闭列表。若目标节点在开启列表中，则规划完毕。得到一条有效路径。否则回到步骤二。

如图1所示，左图中将其分为了100×100块栅格，寻找一条从左上角到右下角的路径。黑色部分为障碍物，灰色部分为关闭列表中的节点，而与灰色部分相邻的白色部分在开启列表中，其余的白色部分为尚未探索到的部分。右图则为实行算法所得到的最终规划的路径。但该方法路径的平滑性较差，栅格地图精度越高则求解效率越低，存在大量不必要的冗余计算，求解时间较长，长达数十秒乃至数分钟，实时性不高，不适用于实时性要求高的环境。

2)快速搜索随机树法

RRT(Rapidly-exploring Random Trees，快速搜索随机树)法使用随机方法以初始点为起点生成随机树。如图2所示，首先在地图中随机选取状态节点，随机状态节点选取后，选择最近的已生成节点向随机节点进行拓展延伸，当该随机树连接起点和终点时停止，反向追溯得到规划路径。

但该方法由于采用随机的策略，路径平滑性较差，另外产生大量不必要的随机树拓展节点导致收敛速度较慢。

本发明实施例基于以上的方法进行融合与改进，得到的一种新路径规划算法，采用降低地图精度的方式大量减少拓展节点，能够体现较大尺度上的障碍物密集程度，从而在一个更高的全局范围下决定路径朝向的趋势，有助于更快地收敛于最优路径，从而大大加快算法计算速度。并且由于融合了RRT中碰撞检测的方法，完全可以保证A*算法原有的有效性和完备性。

本发明实施例又利用车体自身的动力学特性进行节点拓展，使得路径更加平滑。同时为了加快算法速度，增大搜索树有效拓展，引入了历史路径与历史地图。历史路径用于记录当前搜索树已到达的节点位置，历史地图用于记录搜索树已探索过的位置。利用改进方法对于随机树进行剪枝操作，大大降低了路径规划算法所需时间。最终能够生成可执行的、完备的、实时规划的路径，极大地满足了实用需求。

下面参照附图对本发明实施例提出的无人摩托静态障碍避障路径规划计算方法实现过程进行详细说明。

图3是本发明一个实施例的无人摩托静态障碍避障路径规划计算方法流程图。

如图3所示，该无人摩托静态障碍避障路径规划计算方法包括以下步骤：

在步骤S1中，建立无人摩托的平衡动力学模型与运动学模型。

进一步地，在本发明的一个实施例中，无人摩托的动力学模型为：

其中，φ为无人摩托的车身倾斜角，δ为无人摩托的车把转向角，a为无人摩托重心与后轮落地点的水平距离，h为车身无倾斜时无人摩托重心到地面的距离，λ为无人摩托的前叉角，v_x为无人摩托后轮的前向速度，c为无人摩托拖尾，b为无人摩托轴距，g为重力加速度，s为拉普拉斯算子。

进一步地，在本发明的一个实施例中，运动模型为：

其中，v为车辆速度，θ为航向角，δ为无人摩托的车把转向角，c为无人摩托拖尾，λ为无人摩托的前叉角，φ为无人摩托的车身倾斜角，b为无人摩托轴距，

为在绝对坐标地图中的横向速度，

为在绝对坐标地图中的纵向速度，

为航向角速度。

具体而言，步骤1中动力学模型建立过程如下：

无人摩托的动力学模型为：

其中，φ为无人摩托的车身倾斜角，δ为无人摩托的车把转向角，a为无人摩托重心G与后轮落地点P1的水平距离，h为车身无倾斜时无人摩托重心到地面的距离，λ为无人摩托的前叉角，v_x为无人摩托后轮的前向速度，c为无人摩托拖尾，b为无人摩托轴距，g为重力加速度，s为拉普拉斯算子；

控制方式采用比例-微分与前馈控制相结合的控制律：

其中，δ_d为无人摩托的车把转向角指令，即控制器的输出，φ_d为无人摩托的目标车身倾斜角，即控制器的输入，

为关于φ的一阶导数，K₁为比例系数、K₂为微分系数、K₃为前馈系数；

车把转向角指令δ_d与实际车把转向角δ之间存在一个低通滤波环节，该环节为一个一阶惯性环节，T_s为时间常数；

控制系统整体控制过程可以表示为：

其中，M₁＝ahv_xs_λ，M₂＝hv_x ²s_λ-acgs_λ ²，M₃＝bh²，M₄＝bgh

状态方程可以简写为：

其中：

无人摩托的离散运动学模型为：

式中(x,y)为车体坐标位置，v为车辆速度，θ为航向角。δ_real为车辆实际转把转角，根据几何上的关系，其计算方式：

可得到运动学模型：

在步骤S2中，使用自适应分辨率的A*算法，在栅格地图上获取一条连接初始节点与目标节点的通路。

进一步地，步骤S2的具体实现过程为：

初始在一张栅格地图上使用A*算法寻路，栅格由0、1布尔数值所组成，0为障碍物区域(黑色部分)，不可通过；1为空旷区域(白色部分)。选取八邻域作为搜索的结构。

主体搜索步骤如下：

步骤S201，对栅格地图进行自适应分辨率。

步骤S202，在粗精度地图上寻路，若寻路成功，进入下一步，否则当前地图没有初始节点与目标节点的通路。

步骤S203，若路径途径模糊栅格，则将模糊栅格及其附近部分取出，进行局部重规划。

步骤S204，若局部重规划成功，则输出整条路径，结束算法，否则将模糊栅格值置为0，设置重规划标志。

步骤S205，若存在重规划标志，则重新执行算法，若当前地图上无可行路径，则提高地图精度，进入步骤S202重新执行算法。

可以理解的是，自适应分辨率以清晰度作为评价标准，清晰度map_def为：

其中，地图中两种类型的栅格个数分别为num_int和num_dec，如图4所示，num_int为完全的障碍或完全无障碍，num_dec为其余格栅。

进一步而言，步骤S202中的粗精度地图寻路算法、步骤S203中的局部重规划算法如下：

步骤S2001：初始化开启列表Open，将初始节点置入。初始化关闭列表Close，为空。

步骤S2002：若开启列表Open非空，找到Open列表中代价函数值最小的节点Finest并从Open列表中删除。否则跳转至步骤S205。

步骤S2003：若节点Finest不为目标节点，则将其每一个不在关闭列表Close中的邻域节点neibor置入Open列表，若已经存在则更新节点其历史代价值、估计代价值、总代价值Cost。若节点Finest为目标节点，跳转至步骤205。

步骤S2004：将节点Finest置入关闭列表。跳转至步骤S202。

步骤S2005：若已经到达目标节点，则由目标节点反向递推，得到完整路径。否则进入下一步。

步骤S2006：当前地图没有能够连接初始节点与目标节点的通路。

开启列表由五元数组构成，分别为节点位置、历史代价值、估计代价值、总代价值、序号。历史代价值Cost_his即为从初始点到当前节点的路径长度，估计代价值Cost_heu为：当前节点到目标点的欧氏距离。总代价值计算公式为：

Cost＝(Cost_his+Cost_heu)*(1+(1-value_cube))

其中：

当前栅格所占空间在原始地图中表示障碍物栅格的个数为num_obs，表示空旷栅格的个数为num_squ。

在步骤S3中，结合平衡动力学模型与运动学模型，使用RRT算法对通路进行剪枝，生成一条无人摩托可执行的路径。

进一步地，在本发明的一个实施例中，步骤S3包括：

步骤S301，初始化搜索树、历史路径和历史地图；

步骤S302，若达到目标节点，则由搜索树追溯父节点，获取整条规划路径，结束算法，否则进入下一步；

步骤S303，选取拓展节点，选取与拓展节点评估距离最近的节点，选取由最近的节点向拓展节点拓展的控制量，根据控制量从最近的节点到达新节点；

步骤S304，若拓展成功，将新节点加入搜索树，更新历史路径和历史地图，否则以适中间隔值取控制量进行拓展，直至拓展成功，若拓展失败，则将最近节点的死亡次数设置为较大数值，并返回至步骤S302。

具体地，步骤3的具体过程为：

结合了步骤S1中的动力学、运动学模型搜索树拓展方式。在步骤S2的规划基础之上，使用RRT算法得到最终可执行的规划路径，步骤如下：

步骤301：初始化搜索树、历史路径、历史地图。

步骤302：若已经达到目标节点，则由搜索树向父节点追溯，获取整条规划路径，结束算法。否则进入下一步。

步骤303：选取拓展节点node_aim，选取与拓展节点评估距离最近的节点node_close，选取由节点node_close向节点node_aim拓展的控制量control，从节点node_close以控制量control到达新节点node_new。

步骤304：若拓展成功，将新节点node_new加入搜索树，更新历史路径、历史地图。否则以适中间隔值取控制量control进行拓展，直到拓展成功。对于拓展失败的节点node_close的deathtime设置为较大数值。进入步骤302。

其中，设置距离代价Cost_dis、角度代价Cost_del、死亡节点代价Cost_death以及总代价Cost_total。

Cost_dis＝K_dis*Δdis

Cost_del＝K_del*Δdel

Cost_death＝K_death*deathtime²

Cost_total＝Cost_dis+Cost_del+Cost_death

其中，K_dis、K_del、K_death为系数。Δdis为节点到拓展节点距离；Δdel为当前节点朝向角与拓展节点的朝向角之差；deathtime为当前节点的死亡次数，也即节点拓展失败的次数，避免在同一节点上进行大量重复无必要的拓展。历史路径用于记录当前搜索树已到达的节点位置，历史地图用于记录搜索树已探索过的位置。

综上，根据本发明实施例提出的无人摩托静态障碍避障路径规划计算方法，通过采用低精度地图，多层次展现环境，以及采用历史路径、历史地图的剪枝技术，极大降低算法的运行时间，实现路径规划当中对于实时性的需求，又由于实时性能够保障，当环境变化时也能够较快地适应，从而可以拓展为动态障碍物的避障算法；路径规划具有完备性，也即若存在从起点到终点的路径，则必定能够找到路径，否则将会返回规划失败的信息；拓展得到的路径完全基于车体的动力学特性，因而完全可实现，路径能够满足所有的约束条件；采用图搜索的方法，十分直观地表现出路径规划的过程和结果；由于路径是完备的且完全可实现，因此可以保证安全性，不会发生意外碰撞。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (5)

1.一种无人摩托静态障碍避障路径规划计算方法，其特征在于，包括以下步骤：

S1，建立无人摩托的平衡动力学模型与运动学模型；

S2，使用自适应分辨率的A*算法，在栅格地图上获取一条连接初始节点与目标节点的通路，具体包括：步骤S201，对所述栅格地图进行所述自适应分辨率，其中，所述栅格地图上的栅格由0和1布尔数组成，0为障碍物区域，1为空旷区域，步骤S202，在粗精度地图上寻路，若寻路成功，进入下一步，否则当前地图没有所述初始节点与所述目标节点的通路，步骤S203，若路径途径模糊栅格，则将模糊栅格及其附近部分取出，进行局部重规划，步骤S204，若所述局部重规划成功，则输出整条路径，结束算法，否则将模糊栅格值置为0，设置重规划标志，步骤S205，若存在所述重规划标志，则重新执行算法，若当前地图上无可行路径，则提高地图精度，进入所述步骤S202重新执行算法；以及

S3，结合所述平衡动力学模型与所述运动学模型，使用RRT算法对所述通路进行剪枝，生成一条无人摩托可执行的路径。

2.根据权利要求1所述的无人摩托静态障碍避障路径规划计算方法，其特征在于，所述无人摩托的动力学模型为：

其中，φ为无人摩托的车身倾斜角，δ为无人摩托的车把转向角，a为无人摩托重心与后轮落地点的水平距离，h为车身无倾斜时无人摩托重心到地面的距离，λ为无人摩托的前叉角，v_x为无人摩托后轮的前向速度，c为无人摩托拖尾，b为无人摩托轴距，g为重力加速度，s为拉普拉斯算子。

3.根据权利要求1所述的无人摩托静态障碍避障路径规划计算方法，其特征在于，所述运动模型为：

其中，v为车辆速度，θ为航向角，δ为无人摩托的车把转向角，c为无人摩托拖尾，λ为无人摩托的前叉角，φ为无人摩托的车身倾斜角，b为无人摩托轴距，

为在绝对坐标地图中的横向速度，

为在绝对坐标地图中的纵向速度，

为航向角速度。

4.根据权利要求1所述的无人摩托静态障碍避障路径规划计算方法，其特征在于，所述自适应分辨率以清晰度作为评价标准，清晰度map_def为：

其中，地图中两种类型的栅格个数分别为num_int和num_dec，num_int为完全的障碍或完全无障碍，num_dec为其余格栅。

5.根据权利要求1所述的无人摩托静态障碍避障路径规划计算方法，其特征在于，所述步骤S3包括：

步骤S301，初始化搜索树、历史路径和历史地图；

步骤S302，若达到所述目标节点，则由所述搜索树追溯父节点，获取整条规划路径，结束算法，否则进入下一步；

步骤S303，选取拓展节点，选取与所述拓展节点评估距离最近的节点，选取由最近的节点向所述拓展节点拓展的控制量，根据所述控制量从最近的节点到达新节点；

步骤S304，若拓展成功，将所述新节点加入所述搜索树，更新所述历史路径和所述历史地图，否则以适中间隔值取所述控制量进行拓展，直至拓展成功，若拓展失败，则将所述最近节点的死亡次数设置为较大数值，并返回至所述步骤S302。

Images