CN116608877A - 基于rrt算法的仿生模拟植物向光性全局路径规划方法 - Google Patents

Abstract

本发明在原始RRT算法基础上提出了一种仿生植物向光性的全局路径搜索方法，对算法随机扩展导向性差、算法收敛速度慢、狭窄区域中搜索效率低、规划的路径曲折等方面进行优化。本发明中的仿生设计模拟植物的向光性，使随机树选取拓展节点时在以目标点为圆心的一系列同心圆上，能加快随机树向目标点拓展的速度提高算法收敛效率。在随机树向目标点拓展过程中为了提高安全性对障碍物进行处理，合并距离较近的障碍物，并膨胀障碍物提高安全性。其次设计自适应步长，随机树根据障碍物的疏密程度改变步长以更好适应复杂环境。对生成的路径提取关键点后对路径进行曲线优化使路径平滑满足智能车的行驶要求。最后利用MATLAB进行仿真分析验证算法的有效性。

Description

基于RRT算法的仿生模拟植物向光性全局路径规划方法

技术领域

本发明属于及智能驾驶汽车和移动机器人的全局路径规划技术领域，具体涉及一种基于RRT算法的仿生模拟植物向光性全局路径规划方法。

背景技术

随着人工智能、5G技术的发展世界迎来了无人化、智能化发展浪潮，无人工厂、无人汽车、智能机器人等正在越来越多的出现在生活中。汽车作为生活中重要的代步工具也日益电子化、智能化。打造智慧交通有利于缓解交通压力、提高通勤效率、降低交通事故的发生。智能驾驶技术主要包括环境感知、导航定位、路径规划与控制决策等。路径规划是智能驾驶中重要的一环，其对智能驾驶技术的发展具有重大意义。

如今全球各大科研机构和汽车制造商对于智能驾驶技术的研究如火如荼，路径规划是其中不可或缺的一环，而且路径规划效果的优良直接关系到智能车行驶的安全性和舒适性。路径规划指在已知环境下通过算法规划出一条安全、可行的无碰撞路径。路径规划可以分为全局规划和局部规划，全局规划是已知环境中地图信息、静态障碍物信息、起点和终点信息，在地图上规划出一条可以规避静态障碍物的可行路径；局部路径规划主要针对动态避障，其利用激光雷达、摄像头等传感器感知环境中的动态障碍物进行实时避障。

RRT算法是经典的路径规划算法，其已被广泛的研究，RRT算法常被应用于机器人、无人机和无人车的路径规划中。

发明内容

本发明在传统的RRT算法的基础上提出改进，使得算法在静态环境下规划效率更高、路径更加安全合理。主要设计目标在于可在已知地图中高效规划出一条安全、无碰撞的可行路径。其基于改进RRT算法的全局路径规划方法，对算法随机扩展导向性差、算法收敛速度慢、狭窄区域中搜索效率低、规划的路径曲折等方面进行优化。

本发明所进行的工作至少包括：第一，总结前人及原RRT算法的可行性与不足之处，针对RRT算法随机拓展导向性差、收敛速度差等问题提出了一种仿生RRT算法(B-RRT,Bionics-Rapidly-exploring Random Tree)，该算法可提高路径规划效率和降低路径曲折度。第二，对环境中的障碍物进行预处理，合并距离较近的障碍物并膨胀障碍物，避免采样点紧贴障碍物，从而提高路径安全性。第三，提取随机树路径中关键点，对路径重连和曲线优化。最后利用MATLAB进行仿真实验，验证算法的有效性。

本发明解决其技术问题具体采用的技术方案是：

一种基于RRT算法的仿生模拟植物向光性全局路径规划方法，其特征在于，包括以下步骤：

步骤一：获取障碍物坐标信息、智能车运动学模型、地图信息、初始化位置信息和获取目标点信息；

步骤二：对环境中障碍物进行预处理，对距离较近的障碍物进行合并及膨胀障碍物；

步骤三：获取采样点并判断采样点可行性；

步骤四：判断障碍物疏密，采用基于安全性评估的可变步长RRT拓展随机树；

步骤五：拓展随机树并判断是否到达目标点，如未到达则回到步骤三，如到达则执行下一步骤；

步骤六：连线目标点到终点的可行路径，并提取路径中的关键点，进行路径重连接；

步骤七：路径优化、平滑路径、完成路径规划。

进一步地，在步骤一当中，智能车通过包括相机、激光雷达、IMU、GPS进行环境建图或直接获取地图信息；

初始化智能车自身位置和地图中障碍物信息，在已知地图中静态障碍物信息是已知的，将其看成矩形，设某一障碍物其横、纵坐标最大、最小值分别为x_max，x_min，y_max，y_min，则其四个顶点坐标为A(x_min,y_min)，B(x_max,y_min)，C(x_max,y_max)，D(x_min,y_max)，建立车辆运动学模型并简化，将运动学约束融合进路径规划过程中，使规划的路径满足汽车的行驶要求：

并由此整合得到阿克曼转向无人车的运动学方程。

进一步地，在步骤二中，对环境进行预处理，设无人车的长为a，宽为b；设障碍物均为矩形障碍物，若相邻两个障碍物之间的距离小于等于S₁，则填补该空缺，将其合并为同一障碍物；将障碍物表面距离小于S₂的区域设定为不可行区域，新节点无法在该区域生成，即认为该区域为不可通行区域，将距离大于等于S₂，小于等于S₃的区域设定为限制通行区域，新节点在该区域有一定概率生成，即认为该区域为有限通行区域，其中S₁、S₂、S₃公式如下：

S₁＝k·b

其中，k为大于1的安全系数；

在各个区域获取采样点的概率P如下式，其中p_o为设定的概率，k为采样点与障碍物的距离：

进一步地，在步骤三当中，采用B-RRT模仿植物生长的特性，将目标点类比为光源，障碍物稀疏区域类比空旷地带，在以目标点为圆心的一系列同心圆弧上选取随机节点，随机节点选取公式如下式所示：

采样半径r：

其中，q_rand(x,y)为待选取的随机点坐标；n₁、n₂是相互独立的0～1的随机数； 分别为目标点的横坐标和纵坐标，R为地图内两点最远的距离，若是矩形地图，则为矩形对角线距离，m为1～2的系数；

由于随机节点选取公式下采样的随机点会出现在地图外，因此需要剔除掉出现在地图外的采样点，而后进行重新采样。

进一步地，在步骤四当中，基于安全性评估的可变步长RRT，在探查到周围环境障碍物较少时，则增大随机树扩展的步长，根据周围环境障碍物多少来确定步长；

通过多次探索，记录探索节点与最近节点连线和障碍物发生碰撞的次数，根据碰撞发生次数的多寡定性该范围的障碍物疏密，进而调整步长的大小；

随机采样点的选取公式：

其中，q_rand(x,y)为随机采样节点坐标，n₁、n₂是互不相关的0～1的随机数，X_max、Y_max分别是地图的长和宽，n是区别于n₁、n₂的0～1的随机数，P是选取目标点为采样节点的概率值，q_goal(x,y)是目标点的坐标。

进一步地，在步骤五中，判断本次采样点与目标点的距离是否在规定阈值之内，如果是直接连接目标点与采样点，路径搜索完成；否则返回步骤三再次拓展随机树。

进一步地，在步骤六当中，设提取的路径点为{P_k,k＝1,2,···,n}，从节点P₁开始，连接节点P₁、P₃，若线段P₁P₃不与障碍物发生碰撞，即线段P₁P₃到障碍物的距离大于预设阈值，则继续连接P₁、P₄，若线段P₁P₄不与障碍物发生碰撞，则继续连接P₁、P₅，以此类推，直到连接P₁、P_k，而线段P₁P_k经过障碍物，则将P_k-1与P₁连接起来，同时删除中间冗余点，更新路径；从节点P_k-1重复上述操作，直到路径中没有冗余转折点，记录这些路径节点即为路径关键点。

进一步地，在步骤七中，采用贝塞尔曲线进行路径优化。

本发明及其优选方案在原始RRT算法基础上提出了一种仿生植物向光性的全局路径搜索方法，对算法随机扩展导向性差、算法收敛速度慢、狭窄区域中搜索效率低、规划的路径曲折等方面进行优化。本发明中的仿生设计模拟植物的向光性，使随机树选取拓展节点时在以目标点为圆心的一系列同心圆上，能加快随机树向目标点拓展的速度提高算法收敛效率。在随机树向目标点拓展过程中为了提高安全性对障碍物进行处理，合并距离较近的障碍物，并膨胀障碍物提高安全性。其次设计自适应步长，随机树根据障碍物的疏密程度改变步长以更好适应复杂环境。对生成的路径提取关键点后对路径进行曲线优化使路径平滑满足智能车的行驶要求。最后利用MATLAB进行仿真分析验证了算法的有效性。

附图说明

下面结合附图和具体实施方式对本发明进一步详细的说明：

图1为本发明实施例障碍物坐标示意图；

图2本发明实施例无人车二轮运动学模型示意图；

图3本发明实施例环境地图障碍预处理示意图；

图4本发明实施例可变步长探索节点示意图；

图5本发明实施例算法仿真对比示意图；

图6本发明实施例算法仿真时间对比示意图；

图7本发明实施例判断是否到达目标点示意图；

图8本发明实施例局部目标点选取示意图；

图9本发明实施例B-RRT算法全局路径规划与路径优化示意图；

图10本发明实施例方法路径规划流程图。

具体实施方式

为让本专利的特征和优点能更明显易懂，下文特举实施例，作详细说明如下：

应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本说明书使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

以下通过一个具体实施例对本发明方案做进一步的介绍，包括以下步骤：

步骤一：获取障碍物坐标信息、智能车运动学模型、地图信息、初始化位置信息和目标点信息；

智能汽车通过相机、激光雷达、IMU、GPS等传感器进行环境建图或者直接获取地图信息。初始化自身位置和地图中障碍物信息，在已知地图中静态障碍物信息是已知的，将其看成矩形设某一障碍物其横、纵坐标最大、最小值分别为x_max，x_min，y_max，y_min，则其四个顶点坐标为A(x_min,y_min)，B(x_max,y_min)，C(x_max,y_max)，D(x_min,y_max)，如图1所示。其次建立车辆运动学模型并简化如图2所示。将运动学约束融合进路径规划过程中，使规划的路径满足汽车的行驶要求，以下的运动学模型及其公式将详细说明。

其中，(x_f,y_f)、(x_r,y_r)分别是前、后轴中心位置，即简化后的前、后轮位置(后文统一之称为前轮、后轮)，v_f、v_r分别是前、后轮线速度，δ是前轮转向角，是无人车行驶的航向角，l是无人车的轴距，R是无人车转弯半径。

根据正交分解，速度关系为：

由几何关系，可得横摆角速度为：

整合得到阿克曼转向无人车的运动学方程：

步骤二：对环境中障碍物进行预处理，对距离较近的障碍物进行合并膨胀；

对环境进行预处理，特别是环境中的障碍物，设无人车的长为a，宽为b。设障碍物均为矩形障碍物，若相邻两个障碍物之间的距离小于等于S₁，则填补该空缺，将其合并为同一障碍物，如图3(a)所示。为了避免随机树采样点紧贴障碍物，增大智能车的危险系数，故而对障碍物进行膨胀。将障碍物表面距离小于S₂的区域设定为不可行区域，新节点无法在该区域生成，即认为该区域为不可通行区域，将距离大于等于S₂，小于等于S₃的区域设定为限制通行区域，新节点在该区域有一定概率生成，即认为该区域为有限通行区域，其示意图如图3(b)所示，其中S₁、S₂、S₃公式如下：

S₁＝k·b

其中，k为大于1的安全系数。

在各个区域获取采样点的概率P如下式，其中p_o为人为设定的概率，k为采样点与障碍物的距离。基于此公式就可以限制进一步提高路径的安全性。

步骤三：获取采样点并判断采样点可用性；

本发明提出的B-RRT模仿了植物生长的特性，将目标点类比为光源，障碍物稀疏区域类比空旷地带。进一步来说，在选取随机节点时，令其在以目标点为圆心的一系列同心圆弧上，其中B-RRT算法的随机节点选取公式如下式。

上式中采样半径r：

其中，q_rand(x,y)为待选取的随机点坐标；n₁、n₂是相互独立的0～1的随机数； 分别为目标点的横坐标和纵坐标，R为地图内两点最远的距离，若是矩形地图，则为矩形对角线距离，m为1～2的系数。在该公式下采样的随机点会出现在地图外，因此需要剔除掉不符合规划要求的采样点，而后进行重新采样。

步骤四：判断障碍物疏密，选择合适步长拓展随机树；

本发明根据相关思想提出基于安全性评估的一种可变步长RRT，如图4所示，在探查到周围环境障碍物较少时，可以适当增大随机树扩展的步长，根据周围环境障碍物多少来确定步长的优化改进，也即是可变步长RRT，接下来对算法进行详细说明，图4中黑色小圆代表以扩展出的树节点，黑色矩形代表障碍物，三角形代表目标点，虚线框所包围的区域代表以离采样节点(图中未表示出来)最近点为中心的探索范围，白色圆点代表随机探索节点。通过多次探索，记录探索节点与最近节点连线和障碍物发生碰撞的次数，根据碰撞发生次数的多寡定性该范围的障碍物疏密，进而调整步长的大小。

采用可变步长RRT进行路径规划时，随机树扩展步长时刻变化，在该仿真实验中为简单记选取离散步长在1、1.5、2间变化。其主要思想是在障碍物密集的区域步长稍小，可避免新生节点碰撞障碍物而舍弃；在障碍物稀疏的区域步长稍大，从而加快随机树的扩展速度，使算法效率提高。障碍物的疏密通过对最近节点上下左右各2m的范围内进行探索，总探索十次，记录没有障碍物碰撞的次数，其次数若大于6，步长取为2m；若大于3小于等于6，步长取为1.5m；若小于等于3，步长取为1m。如下式：

其中B随机树拓展步长，n为对最近节点周边探索的碰撞次数。

在提高算法的效率加快随机树向终点拓展的研究中不少研究者提出了目标偏向策略，广义上讲即是随机树在拓展时选取采样点的分布上不是在整个环境中均匀取值，而是人为增大在某区域选取采样点概率，加快随机树向目标点拓展。在算法中目标偏向具体是指有一定概率取到以终点为采样点，随机树可以直接向目标点拓展，加快算法收敛速度。原RRT算法随机采样点的选取公式：

q_rand(x,y)＝(n₁×X_max,n₂×Y_max)

其中，q_rand(x,y)为随机采样节点坐标，n₁、n₂是互不相关的0～1的随机数，X_max、Y_max分别是地图的长和宽。

而本实施例改进的目标偏向RRT算法其随机采样点的选取公式：

其中，n是区别于n₁、n₂的0～1的随机数，P是选取目标点为采样节点的概率值，在该仿真实验中P取0.2，q_goal(x,y)是目标点的坐标，其他的符号含义同上。

为了对比目标偏向RRT算法和B-RRT算法，本实施例对两者进行仿真实验，图5即为仿真对比图，通过观察仿真图可以明显看到目标偏向RRT的采样点较为稀疏，算法在路径规划后期可以依靠一定概率选取目标点作为随机树的扩展方向，解决了路径规划到目标点附近徘徊不前的问题。而B-RRT的随机采样点在离目标点较近区域分布密集，在离目标点较远区域分布较稀疏，正是这样的采样特性同样可以令随机树优先向目标点扩展。为了能够清楚体现目标偏向RRT算法与B-RRT算法的差异，分别对各算法仿真100次，记录所用时间，均保留两位小数，图6是两种RRT算法得到路径规划各自所需花费的时间对比图。

由仿真数据可知目标偏向RRT平均用时为0.89，其方差为0.04196，B-RRT平均用时为0.53，其方差为0.0088，可得知B-RRT用时比目标偏向RRT下降40％，此外B-RRT算法的稳定性有一定提升，该改进使得算法效率显著提升。

步骤五：拓展随机树并判断是否到达目标点，如果未到达目标点则回到步骤三，如果到达则执行下一步骤；

判断本次采样点与目标点的距离是否在规定阈值之内，如果是直接连接目标点与采样点，路径搜索完成，如图7所示；否则返回步骤三再次拓展随机树。

步骤六：连线目标点到终点的可行路径，并提取路径中的关键点，进行路径重连接；

在该步骤中B-RRT算法已经规划出全局路径，随机树连接从起点到目标点，如图8蓝色拓展树；而后需要提取该全局路径中的路径节点，设提取的路径点为{P_k,k＝1,2,···,n}，从节点P₁开始，连接节点P₁、P₃，若线段P₁P₃不与障碍物发生碰撞，即线段P₁P₃到障碍物的距离大于预设阈值，则继续连接P₁、P₄，若线段P₁P₄不与障碍物发生碰撞，则继续连接P₁、P₅，以此类推，直到连接P₁、P_k，而线段P₁P_k经过障碍物，则将P_k-1与P₁连接起来，同时删除中间冗余点，更新路径；从节点P_k-1重复上述操作，直到路径中没有冗余转折点，记录这些路径节点即为路径关键点。

图8中隐去了采样点，仅保留了蓝色的原始规划路径，绿色路径为根据关键点保留的路径，红色星号*为提取完毕的路径关键点，红色五角星为全局目标点。可以看到，显然最终保留的路径比初始路径的路径总长度有明显的降低，且转折点大量减少，原始路径转折点有49个，而删除中间冗余点后，最终的路径转折点为7个，降幅达86％。然而路径转折点虽已减少但是路径不够平滑难以满足智能车的驾驶要求，下一步骤将对绿色路径进行曲线优化。

步骤七：路径优化、平滑路径、完成路径规划，并在MATLAB中可视化全局路径；

在上一步骤中提取了全局路径的节点，根据规则进一步提取全局路径关键点，在一定程度上优化了路径，但是重连接的路径没有曲线优化，路径不够平滑不满足智能汽车的行驶要求。为此引入贝塞尔曲线进行路径优化，经过贝塞尔曲线优化后的路径光滑，满足汽车的行驶要求。在优化路径过程中不但考虑全局路径关键点使路径可以避开障碍物，而且加入动力学约束将汽车的转向、加速度等考虑其中。经过曲线优化后的路径才真正满足汽车行驶要求，如图9(b)为在MATLAB中可视化的经过贝塞尔曲线优化后的全局路径，图9(a)中黑色的曲线为规划的原始全局路径，虚线为经过提取路径关键后形成的路径。从左右对比中可以看到优化后的路径一定程度上贴合红色的全局路径，同时兼顾汽车的行驶和避障，特别在转向中避免大曲率急转。故而本发明改进的算法其优化后的路径具有很好的实用价值，其整体流程如图10所示。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

本专利不局限于上述最佳实施方式，任何人在本专利的启示下都可以得出其它各种形式的基于RRT算法的仿生模拟植物向光性全局路径规划方法，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本专利的涵盖范围。

Claims (8)

1.一种基于RRT算法的仿生模拟植物向光性全局路径规划方法，其特征在于，包括以下步骤：

步骤一：获取障碍物坐标信息、智能车运动学模型、地图信息、初始化位置信息和获取目标点信息；

步骤二：对环境中障碍物进行预处理，对距离较近的障碍物进行合并及膨胀障碍物；

步骤三：获取采样点并判断采样点可行性；

步骤四：判断障碍物疏密，采用基于安全性评估的可变步长RRT拓展随机树；

步骤五：拓展随机树并判断是否到达目标点，如未到达则回到步骤三，如到达则执行下一步骤；

步骤六：连线目标点到终点的可行路径，并提取路径中的关键点，进行路径重连接；

步骤七：路径优化、平滑路径、完成路径规划。

2.根据权利要求1所述的基于RRT算法的仿生模拟植物向光性全局路径规划方法，其特征在于：

在步骤一当中，智能车通过包括相机、激光雷达、IMU、GPS进行环境建图或直接获取地图信息；

初始化智能车自身位置和地图中障碍物信息，在已知地图中静态障碍物信息是已知的，将其看成矩形，设某一障碍物其横、纵坐标最大、最小值分别为x_max，x_min，y_max，y_min，则其四个顶点坐标为A(x_min,y_min)，B(x_max,y_min)，C(x_max,y_max)，D(x_min,y_max)，建立车辆运动学模型并简化，将运动学约束融合进路径规划过程中，使规划的路径满足汽车的行驶要求：

并由此整合得到阿克曼转向无人车的运动学方程。

3.根据权利要求1所述的基于RRT算法的仿生模拟植物向光性全局路径规划方法，其特征在于：

在步骤二中，对环境进行预处理，设无人车的长为a，宽为b；设障碍物均为矩形障碍物，若相邻两个障碍物之间的距离小于等于S₁，则填补该空缺，将其合并为同一障碍物；将障碍物表面距离小于S₂的区域设定为不可行区域，新节点无法在该区域生成，即认为该区域为不可通行区域，将距离大于等于S₂，小于等于S₃的区域设定为限制通行区域，新节点在该区域有一定概率生成，即认为该区域为有限通行区域，其中S₁、S₂、S₃公式如下：

S₁＝k·b

其中，k为大于1的安全系数；

在各个区域获取采样点的概率P如下式，其中p_o为设定的概率，k为采样点与障碍物的距离：

4.根据权利要求1所述的基于RRT算法的仿生模拟植物向光性全局路径规划方法，其特征在于：

在步骤三当中，采用B-RRT模仿植物生长的特性，将目标点类比为光源，障碍物稀疏区域类比空旷地带，在以目标点为圆心的一系列同心圆弧上选取随机节点，随机节点选取公式如下式所示：

采样半径r：

其中，q_rand(x,y)为待选取的随机点坐标；n₁、n₂是相互独立的0～1的随机数； 分别为目标点的横坐标和纵坐标，R为地图内两点最远的距离，若是矩形地图，则为矩形对角线距离，m为1～2的系数；

由于随机节点选取公式下采样的随机点会出现在地图外，因此需要剔除掉出现在地图外的采样点，而后进行重新采样。

5.根据权利要求4所述的基于RRT算法的仿生模拟植物向光性全局路径规划方法，其特征在于：

在步骤四当中，基于安全性评估的可变步长RRT，在探查到周围环境障碍物较少时，则增大随机树扩展的步长，根据周围环境障碍物多少来确定步长；

通过多次探索，记录探索节点与最近节点连线和障碍物发生碰撞的次数，根据碰撞发生次数的多寡定性该范围的障碍物疏密，进而调整步长的大小；

随机采样点的选取公式：

其中，q_rand(x,y)为随机采样节点坐标，n₁、n₂是互不相关的0～1的随机数，X_max、Y_max分别是地图的长和宽，n是区别于n₁、n₂的0～1的随机数，P是选取目标点为采样节点的概率值，q_goal(x,y)是目标点的坐标。

6.根据权利要求5所述的基于RRT算法的仿生模拟植物向光性全局路径规划方法，其特征在于：在步骤五中，判断本次采样点与目标点的距离是否在规定阈值之内，如果是直接连接目标点与采样点，路径搜索完成；否则返回步骤三再次拓展随机树。

7.根据权利要求1所述的基于RRT算法的仿生模拟植物向光性全局路径规划方法，其特征在于：

在步骤六当中，设提取的路径点为{P_k,k＝1,2,···,n}，从节点P₁开始，连接节点P₁、P₃，若线段P₁P₃不与障碍物发生碰撞，即线段P₁P₃到障碍物的距离大于预设阈值，则继续连接P₁、P₄，若线段P₁P₄不与障碍物发生碰撞，则继续连接P₁、P₅，以此类推，直到连接P₁、P_k，而线段P₁P_k经过障碍物，则将P_k-1与P₁连接起来，同时删除中间冗余点，更新路径；从节点P_k-1重复上述操作，直到路径中没有冗余转折点，记录这些路径节点即为路径关键点。

8.根据权利要求1所述的基于RRT算法的仿生模拟植物向光性全局路径规划方法，其特征在于：在步骤七中，采用贝塞尔曲线进行路径优化。