CN106569496A - 一种运动路径的规划方法 - Google Patents

Abstract

本发明涉及机器人路径规划技术领域，尤其涉及一种运动路径的规划方法，包括：建立牵引车拖动拖车的系统模型并提取拖挂式移动机器人的状态信息；以邻近点为出发点根据一预设的步长拓展新节点；对切向拟合形成的期望路径段进行路径跟踪并计算新节点的状态信息；判断路径跟踪的过程是否满足预设的夹角约束条件，并将满足夹角约束条件的新节点和新节点的状态信息添加入一节点树中，随后返回，直至新节点为目标点时形成包括一完整路径的完整节点树为止；对完整路径进行优化形成一优化路径，并将优化路径作为拖挂式移动机器人的运动路径；上述技术方案能够对拖挂式移动机器人进行路径规划，避免牵引车和拖车折叠、挤压的现象发生。

Description

一种运动路径的规划方法

技术领域

本发明涉路径规划技术领域，尤其涉及一种运动路径的规划方法。

背景技术

路径规划是指机器人从所处的环境中自动搜索出一条从初始状态到目标状态的避碰、最优或次优路径。通常选用的实现方法包括人工势场法、遗传算法、快速扩展随机树算法(Rapid-exploring Random Tree，简称RRT)等。路径规划的研究通常应用于单体机器人系统中，由于拖挂式机器人系统在约束和控制条件上的复杂性，一般的路径规划算法无法满足实际需求。快速扩展随机树算法是一种随机釆样的典型树结构算法，采用特定的增量方式进行构造，其基本思想是由控制理论决定随机树的增长方式，通过在状态空间随机采样状态点将搜索导向空白区域，逐步缩短随机状态点与树的期望节点即规划目标点间的距离，从而找到一条连接起始点与目标点的规划路径。这种规划方式抛弃状态空间对障碍物精确定义的要求，选用碰撞检测函数来判定系统每个位形与障碍物的关系，可以简化对空间的建模，搜索速度快，不会出现栅格法、人工势场法等算法中易出现的维数灾难问题。因此用于解决复杂环境下复杂系统的路径规划问题。

拖挂式机器人系统根据连接方式可分为离轴式和连轴式，根据运动特性又可分为正车和倒车，由于系统结构的复杂性，不同工况下系统的路径跟踪控制方法都各不相同，尤其针对离轴式倒车系统，通常容易发生牵引车和拖车折叠、挤压的现象。

发明内容

针对上述问题，本发明提出了一种运动路径的规划方法，应用于拖挂式移动机器人，所述拖挂式移动机器人包括一牵引车和一拖车，所述牵引车牵引所述拖车向一目标点运动；其中，所述规划方法包括：

步骤S1：建立所述牵引车拖动所述拖车的系统模型并提取所述拖挂式移动机器人的初始的状态信息；

步骤S2：选取一随机方向点，并选取最靠近所述随机方向点的节点作为一邻近点，以所述邻近点为出发点根据一预设的步长拓展新节点；

步骤S3：对所述邻近点和所述新节点进行切向拟合形成一期望路径段，并对所述期望路径段进行路径跟踪并计算所述新节点的所述状态信息；

步骤S4：判断所述路径跟踪的过程是否满足预设的夹角约束条件，并将满足夹角约束条件的新节点和所述新节点的所述状态信息添加入一节点树中，随后返回所述步骤S2，直至所述新节点为所述目标点时形成包括一完整路径的完整节点树为止；

步骤S5：对所述完整路径进行优化形成一优化路径，并将所述优化路径作为所述拖挂式移动机器人的所述运动路径。

上述的规划方法，其中，所述步骤S5中，确定所述完整路径中最优的路径的具体方法是：

对每个所述完整路径中的所述节点进行正向优化操作，以对所述节点进行筛选，并以筛选后的所述节点形成的所述完整路径作为所述拖挂式移动机器人的所述最优路径。

上述的规划方法，其中，所述正向优化为依据所述节点拓展的正向顺序进行的节点优化操作。

上述的规划方法，其中，进行所述步骤S5之前还需要将所述完整节点树中未形成所述完整路径的节点删除。

上述的规划方法，其中，所述邻近点下形成有多个子节点树；所述步骤S2还包括一击中次数测试的步骤：

步骤S21：对每个所述邻近点进行所述击中次数测试，并记录击中每个所述邻近点的次数；

步骤S22：判断击中所述邻近点的次数是否超出一预设的效率阀值；

若是，则删除所述邻近点及所述邻近点下的所有所述子节点树；

若否，则执行所述步骤S3。

上述的规划方法，其中，所述状态信息包括所述牵引车和拖车的位置信息。

上述的规划方法，其中，所述步骤S4中，判断所述路径跟踪的过程是否符合所述夹角约束条件的方式具体为：

判断所述路径跟踪的过程中所述牵引车和所述拖车形成的车体夹角是否总是小于一预设的约束角，并在所述路径跟踪的过程中所述车体夹角总是小于所述约束角时判断所述路径跟踪的过程满足所述夹角约束条件。

上述的规划方法，其中，所述步骤S4中：

判断所述路径跟踪的过程是否符合所述夹角约束条件的同时还对所述拖挂式移动机器人的工作环境进行碰撞检测；

于所述路径跟踪的过程中所述车体夹角总是小于所述约束角且所述工作环境无碰撞阻挡时，则将所述新节点和所述新节点的所述状态信息添加入所述节点树中。

有益效果：本发明提出的一种运动路径的规划方法应用于拖挂式移动机器人，能够对拖挂式移动机器人进行路径规划，避免牵引车和拖车折叠、挤压的现象发生。

附图说明

图1为本发明一实施例中运动路径的规划方法的步骤流程图；

图2为本发明一实施例中拖挂式移动机器人的结构示意图；

图3为本发明一实施例中拟合的期望路径的示意图；

图4为针对图3所示的期望路径进行路径跟踪的结果；

图5为本发明一实施例中可行路径的示意图；

图6为图5所示的可行路径优化后的结果；

图7为本发明一实施例中路径搜索的结果；

图8为图7中所示的路径搜索结果的优化结果；

图9为本发明一实施例中路径搜索的结果；

图10为图10中所示的路径搜索结果的优化结果；

图11为本发明一实施例中拖车双轮中点轨迹线20次计算结果的汇总图；

图12为本发明一实施例中100次计算样本的统计信息；

图13为本发明一实施例中搜索时间的统计结果；

图14为本发明一实施例中路径长度的统计结果。

具体实施方式

下面结合附图和实施例对本发明进行进一步说明。

在一个较佳的实施例中，如图1所示，提出了一种运动路径的规划方法，可以应用于拖挂式移动机器人，拖挂式移动机器人可以包括如图2所示的一牵引车和一拖车，牵引车牵引拖车向一目标点运动；该规划方法可以包括：

步骤S1：建立牵引车拖动拖车的系统模型并提取拖挂式移动机器人的初始的状态信息；

步骤S2：选取一随机方向点，并选取最靠近随机方向点的节点作为一邻近点，以邻近点为出发点根据一预设的步长拓展新节点；

步骤S3：对邻近点和新节点进行切向拟合形成一期望路径段，并对期望路径段进行路径跟踪并计算新节点的状态信息；

步骤S4：判断路径跟踪的过程是否满足预设的夹角约束条件，并将满足夹角约束条件的新节点和新节点的状态信息添加入一节点树中，随后返回步骤S2，直至新节点为目标点时形成包括一完整路径的完整节点树为止；

步骤S5：对完整路径进行优化形成一优化路径，并将优化路径作为拖挂式移动机器人的运动路径。

具体地，每个期望路径段的起始点可以为所选择的邻近点；由于采用切向拟合形成期望路径段，因此所选择的邻近点和每个拓展的新节点只会形成一个期望路径段，并且在每次执行步骤S2时，拓展的新节点数量只有一个，多次循环以后能够在节点树中拓展多个新节点；状态信息可以包括拖车和牵引车分别的方位角，角速度，线速度，加速度等；但这些都是优选的情况，不应视为是对本发明的限制。

在一个较佳的实施例中，步骤S5中，确定完整路径中最优的路径的具体方法是：

对每个完整路径中的节点进行正向优化操作，以对节点进行筛选，并以筛选后的节点形成的完整路径作为拖挂式移动机器人的最优路径。

上述实施例中，优选地，正向优化为依据节点拓展的正向顺序进行的节点优化操作。

在一个较佳的实施例中，进行步骤S5之前还需要将完整节点树中未形成完整路径的节点删除。

在一个较佳的实施例中，邻近点下形成有多个子节点树；步骤S2还包括一击中次数测试的步骤：

步骤S21：对每个邻近点进行击中次数测试，并记录击中每个邻近点的次数；

步骤S22：判断击中邻近点的次数是否超出一预设的效率阀值；

若是，则删除邻近点及邻近点下的所有子节点树；

若否，则执行步骤S3。

在一个较佳的实施例中，状态信息包括牵引车和拖车的位置信息。

在一个较佳的实施例中，步骤S4中，判断路径跟踪的过程是否符合夹角约束条件的方式具体为：

判断路径跟踪的过程中牵引车和拖车形成的车体夹角是否总是小于一预设的约束角，并在路径跟踪的过程中车体夹角总是小于约束角时判断路径跟踪的过程满足夹角约束条件。

在一个较佳的实施例中，步骤S4中：

判断路径跟踪的过程是否符合夹角约束条件的同时还对拖挂式移动机器人的工作环境进行碰撞检测；

于路径跟踪的过程中车体夹角总是小于约束角且工作环境无碰撞阻挡时，则将新节点和新节点的状态信息添加入节点树中。

具体地，下面将给出本发明详细的计算和分析的例子：

1)路径跟踪控制方法

牵引车(根据系统动力源，统称机器人为牵引车)的姿态可描述为(x₁,y₁,θ₁)，拖车的姿态可描述为(x₂,y₂,θ₂)。设牵引车前轮与车体纵轴的夹角，即车轮转向角为α，逆时针为正。设牵引车的线速度(前进速度)和角速度分别为u₁和ω₁(或)，拖车的线速度和角速度分别为u₂和ω₂(或)。根据图1坐标系，可得系统运动方程：

-c₁ω₁cos(θ₁-θ₂)] (1)

根据拖车和牵引车的连接几何关系可得：

x₂＝x₁-c₁cosθ₁-l₂cosθ₂

y₂＝y₁-c₁sinθ₁-l₂sinθ₂ (2)

以带一节拖车的单体两轮机器人系统模型为研究对象。

1.离轴式和连轴式正车路径跟踪

根据系统连接方式的不同，拖挂式机器人系统可分为离轴式(c₁＞0)和连轴式(c₁＝0)，两种连接方式在正车驱动中的运动方程相同，故可用相同的控制方法进行路径跟踪。

设期望路径为ρ_d(x_d,y_d),x_d和y_d三阶可导且三阶导数有界，期望路径的曲率满足k_d≤1/r_1min，若牵引车的控制率取为：

u₁＝u_1d

其中，u₁、ω₁为牵引车的线速度和角速度。

e_x＝(x_d-x)cosθ+(y_d-y)sinθ

e_y＝-(x_d-x)sinθ+(y_d-y)cosθ

e_θ＝θ_d-θ

则牵引车的运动将渐近收敛到期望路径ρ_d(x_d,y_d)，其中，期望线速度u_1d，期望角速度ω_d，误差e_x、e_y、e_θ满足式(4)，系数k₃,k₄＞0。

2.离轴式倒车系统路径跟踪控制方程

若拖车当作“虚拟机器人”，则倒车运动的控制率为和(分别为拖车的线速度和角速度)，当它们满足如下条件(拖车的姿态角θ₂初值的取值范围为[-π,π])：

根据运动学方程可反推得到离轴式倒车系统的控制方程为：

3.连轴式倒车系统路径跟踪控制方程

对于连轴式系统，c₁＝0，根据模型可推得该系统下运动学方程为：

根据上式的第4式，取控制率的u₁为

则

使即可推得该系统的控制方程为：

2)基于路径跟踪控制的改进RRT算法

引入样条拟合控制方法，在节点拓展时，根据T_near点拖车或牵引车的方位角信息与T_new点的坐标值，进行两点间的切向拟合，形成至T_new的期望路径，如图3所示，根据系统当前工况(正、倒车、离轴式、连轴式)，选择对应的控制方程(如上)，针对期望路径进行路径跟踪(如图4)。若得到的跟踪结果满足牵引车与拖车夹角的安全性要求，则进一步进行碰撞检测等，反之进行重新搜索。

本发明采用节点击中机制，避免因加入路径跟踪控制的夹角约束条件造成的搜索失败情况，具体操作为：记录每次拓展所选择T_near点以及其击中次数，根据多次测试，选择合理的效率阀值(亦为节点最大子节点数)，在击中次数超出效率阀值的情况时，删除T_near点以及其子树(子树由于控制约束，一般无法继承至T_near的父项)。同时在每次一次拓展失败时，尝试将T_near更改为其父节点进行重新拓展。

根据上述内容，整理得到基于路径跟踪的改进RRT算法流程：

其中步骤1为记录系统初始状态，包括牵引车和拖车的初始位置和初始角度(x₁₀,y₁₀,z₁₀)，(x₂₀,y₂₀,z₂₀)，步骤2-9为节点搜索和拓展检测。步骤10-19为拓展检测的具体方法，其中对所选择的T_near进行击中次数检测，超出设定的阀值则删减树中的T_near点以及其子树，否则，进行样条曲线控制方法拟合期望路径，计算该曲线的一至三阶导数，并根据当前拖挂式系统的工况选择合适的路径跟踪控制方程(如第上节)，进行路径跟踪(步骤14)，跟踪的结果满足系统安全夹角要求dθ_max＝|θ₁-θ₂|＜θ_require，则进行碰撞检测，检测通过则在树上添加新节点。当T_new为T_goal，终止搜索，对搜索结果进行优化处理(优化前后的路径如图5和图6所示)，并根据优化结果进行重新拟合和跟踪。

3)根据贪心思想进行结果优化。

由于加入了路径跟踪控制方程，一般采用上述方法随机搜索得到的计算结果可直接用于实际操作，属于可行解，但不是最优解。参考贪心思想，对路径主要节点进行优化，优化方法可分为正向优化与反向优化两种，针对此系统问题的控制约束条件，一般采用正向优化方法。

步骤1为对Tree进行剪枝处理，留下主干节点(T₀，T₁，T₂，……，T_n)。步骤2-10为从T_i点和该点系统状态，依次尝试连接拓展T_n，T_n-1，……，T_i+1，直至拓展成功，若T_j为T_n则结束优化。优化结果如图12所示。

注：由贪心思想得到的T_i，由于改变了T_i点原有的系统状态，无法保证T_n，T_n-1，……，T_i+1中存在成功拓展点，若没有则需要进行退回处理，当T_i退回至T₀时必有成功拓展点T₁，因此，此优化方法是封闭有解的。

4)仿真验证

图7和图9分别为两个连轴式倒车系统算例的计算结果，输入数据包括起始点T₀(1.5,0)，起始点朝向A₀＝180°目标点T_goal(18,0)，目标点朝向A_goal＝160°,牵引车初始速度u₀＝-1，初始角速度w₀＝0.6，系统安全夹角阀值为60°，搜索步长取为1.5m，目标点随机概率取30％。图7和图9为拓展树搜索结果，图8为图7剪枝以及优化之后的结果，图10为图9剪枝以及优化之后的结果(其中包括牵引车、拖车路径轨迹线，轮廓点轨迹线、以及轮印轨迹线)。路径行进时间为20.736s，牵引车与拖车最大夹角为47.01°，满足安全阀值60°的要求。

图11为取20次计算结果中拖车双轮中点的轨迹线汇总图。

图12-图14是根据当前系统输入参数和环境模型，进行100次重复计算的统计结果。

表1 100次重复计算统计结果

经过仿真试验和结果分析，可知，在有解前提下，改进路径规划方法的搜索成功率近似为100％，极大地提高了原RRT算法的稳定性，同时由于加入了夹角约束、控制理论和优化方法，保证了搜索结果的可行性、安全性和较优性。在搜索耗时上，高于一般的RRT算法，但可通过适当降低跟踪控制精度的方法提高搜索效率，亦可用于实时系统。

综上所述，本发明提出了一种运动路径的规划方法，应用于拖挂式移动机器人,包括：建立牵引车拖动拖车的系统模型并提取拖挂式移动机器人的状态信息；选取一随机方向点，并选取最靠近随机方向点的节点作为一邻近点，以邻近点为出发点根据一预设的步长拓展新节点；对邻近点和新节点进行切向拟合形成一期望路径段，并对期望路径段进行路径跟踪并计算新节点的状态信息；判断路径跟踪的过程是否满足预设的夹角约束条件，并将满足夹角约束条件的新节点和新节点的状态信息添加入一节点树中，随后返回，直至新节点为目标点时形成包括一完整路径的完整节点树为止；对完整路径进行优化形成一优化路径，并将优化路径作为拖挂式移动机器人的运动路径；上述技术方案能够对拖挂式移动机器人进行路径规划，避免牵引车和拖车折叠、挤压的现象发生。

通过说明和附图，给出了具体实施方式的特定结构的典型实施例，基于本发明精神，还可作其他的转换。尽管上述发明提出了现有的较佳实施例，然而，这些内容并不作为局限。

对于本领域的技术人员而言，阅读上述说明后，各种变化和修正无疑将显而易见。因此，所附的权利要求书应看作是涵盖本发明的真实意图和范围的全部变化和修正。在权利要求书范围内任何和所有等价的范围与内容，都应认为仍属本发明的意图和范围内。

Claims (8)

1.一种运动路径的规划方法，应用于拖挂式移动机器人，所述拖挂式移动机器人包括一牵引车和一拖车，所述牵引车牵引所述拖车向一目标点运动；其特征在于，所述规划方法包括：

步骤S1：建立所述牵引车拖动所述拖车的系统模型并提取所述拖挂式移动机器人的初始的状态信息；

步骤S2：选取一随机方向点，并选取最靠近所述随机方向点的节点作为一邻近点，以所述邻近点为出发点根据一预设的步长拓展新节点；

步骤S3：对所述邻近点和所述新节点进行切向拟合形成一期望路径段，并对所述期望路径段进行路径跟踪并计算所述新节点的所述状态信息；

步骤S4：判断所述路径跟踪的过程是否满足预设的夹角约束条件，并将满足夹角约束条件的新节点和所述新节点的所述状态信息添加入一节点树中，随后返回所述步骤S2，直至所述新节点为所述目标点时形成包括一完整路径的完整节点树为止；

步骤S5：对所述完整路径进行优化形成一优化路径，并将所述优化路径作为所述拖挂式移动机器人的所述运动路径。

2.根据权利要求1所述的规划方法，其特征在于，所述步骤S5中，确定所述完整路径中最优的路径的具体方法是：

对每个所述完整路径中的所述节点进行正向优化操作，以对所述节点进行筛选，并以筛选后的所述节点形成的所述完整路径作为所述拖挂式移动机器人的所述最优路径。

3.根据权利要求2所述的规划方法，其特征在于，所述正向优化为依据所述节点拓展的正向顺序进行的节点优化操作。

4.根据权利要求1所述的规划方法，其特征在于，进行所述步骤S5之前还需要将所述完整节点树中未形成所述完整路径的节点删除。

5.根据权利要求1所述的规划方法，其特征在于，所述邻近点下形成有多个子节点树；所述步骤S2还包括一击中次数测试的步骤：

步骤S21：对每个所述邻近点进行所述击中次数测试，并记录击中每个所述邻近点的次数；

步骤S22：判断击中所述邻近点的次数是否超出一预设的效率阀值；

若是，则删除所述邻近点及所述邻近点下的所有所述子节点树；

若否，则执行所述步骤S3。

6.根据权利要求1所述的规划方法，其特征在于，所述状态信息包括所述牵引车和拖车的位置信息。

7.根据权利要求1所述的规划方法，其特征在于，所述步骤S4中，判断所述路径跟踪的过程是否符合所述夹角约束条件的方式具体为：

判断所述路径跟踪的过程中所述牵引车和所述拖车形成的车体夹角是否总是小于一预设的约束角，并在所述路径跟踪的过程中所述车体夹角总是小于所述约束角时判断所述路径跟踪的过程满足所述夹角约束条件。

8.根据权利要求1所述的规划方法，其特征在于，所述步骤S4中：

判断所述路径跟踪的过程是否符合所述夹角约束条件的同时还对所述拖挂式移动机器人的工作环境进行碰撞检测；

于所述路径跟踪的过程中所述车体夹角总是小于所述约束角且所述工作环境无碰撞阻挡时，则将所述新节点和所述新节点的所述状态信息添加入所述节点树中。