一种改进A-RRT的移动机器人路径规划方法

专利名称：一种改进A-RRT的移动机器人路径规划方法

专利类型：发明专利

专利申请号：CN202210644899.5

专利申请（专利权）人：安徽工业大学
权利人地址：安徽省马鞍山市花山区湖东路59号

专利发明（设计）人：徐向荣,李琦琦,李永港,游天涯

专利摘要：本发明公开了一种改进A‑RRT的移动机器人路径规划方法，属于机器人的运动规划技术领域。本发明的改进A‑RRT的移动机器人路径规划方法，采用融合人工势场的RRT*算法和膨胀半径的A‑RRT*算法，可以有效减小规划的路径代价，为移动机器人规划出一条优化的路径，并提高其运行的平稳性，完成移动操作机器人的地图构建与路径规划。与传统RRT、RRT*相比，改进后的算法能够在较短的时间内完成路径规划，路径的代价会更小，搜索效率高，具备更高的安全性，且构架保持了原有简洁的特点；将该算法应用在串联机器人的关节空间中，使串联机器人能够在工作空间中有障碍物的情况下得到一条安全的路径；引入基于Minimum Snap的AR‑RRT*算法路径平滑方法，最后引入动态窗口法对局部路径进行规划。

主权利要求：
1.一种改进A‑RRT的移动机器人路径规划方法，其特征在于，步骤为：S1、全局路径规划改进：
S1‑1、在RRT*算法中引入目标人工势场，由此来引导随机生长树的生长趋势向目标点方向生长；
S1‑2、引入膨胀半径，使得机器人在真实环境中移动时能够避开狭窄的移动路线，安全达到目标点；
S2、基于MinimumSnap轨迹规划的A‑RRT*算法路径平滑处理：S2‑1、控制移动机器人的加加加速度变化率最小，通过将四阶倒数的积分值作为代价函数，并且以最小化代价函数来实现损耗能量最小，通过对轨迹上的所有端点进行约束；
S2‑2、然后再根据代价函数和约束条件得到二次规划的一般形式，最终获得一条满足约束条件的光滑轨迹；
S3、局部路径规划的动态窗口法：采用动态窗口法进行规划，机器人在移动的过程中根据自身所处的环境信息从而做出相应的决策，完成局部调整动作。
2.如权利要求1所述的改进A‑RRT的移动机器人路径规划方法，其特征在于，所述步骤S1融合人工势场的RRT*算法中，RRT*算法新节点的生成步骤为：S1‑11：找到距离qrand最近的节点qnear，并连接qrand与qnear，以qnear为起点向qnear截取步长λ；
S1‑12：连接qnear与qgoal，以qnear为起点向终点qgoal截取步长kλ；
S1‑13：引入人工势场法的合力生成思想得到新节点qnew，连接qnew与qnear，如果qnew与qnear之间无障碍物，则将qnew加入随机生长树。
3.如权利要求2所述的改进A‑RRT的移动机器人路径规划方法，其特征在于，所述步骤S1中新节点qnew的公式表示为：设定向目标点qgoal方向扩展的节点q1，向随机点qrand方向扩展的节点为q2；
人工势场法的引力场函数为：
其中a为引力场系数，Xg为目标点的位置，引力Fatt(X)： 则目标点qgoal方向扩展的节点q1为：
其中k为目标点对移动机器人的引力系数，λ为扩展步长；
向随机点qrand方向扩展的节点为q2有：
最后加入目标引力势场后新节点qnew的数学表达式为：
4.如权利要求3所述的改进A‑RRT的移动机器人路径规划方法，其特征在于，所述步骤S1中引入膨胀半径后整体融合算法的具体流程如下：S1‑21：算法初始化，设置移动机器人的起始点与终点、障碍物的膨胀半径以及其他相关参数；
S1‑22：在空闲的状态空间中随机生成一个采样点qrand，遍历随机搜索树T中的所有节点，随机树中找到一个距离qrand最小的一个节点qnear，并连接qrand与qnear，且以qnear为起点向qrand方向扩展一个步长λ，然后连接qnear和终点qgoal，并以qnear为起点向目标点qgoal扩展kλ的长度，k为终点对移动机器人引力系数；
S1‑23：通过引入人工势场法的合力生成的方式得到新节点qnew，连接节点qnew和qnear并判断两点之间是否存在障碍物，如果存在放弃此次生长，反之将节点qnew加入随机搜索树；
S1‑24：搜索qnew固定范围内的节点，重新选择qnew的父节点，并对整个随机树进行剪枝；
S1‑25：判断qnew是否到达终点，若没达到则转步骤S1‑22；反之转步骤S1‑26；
S1‑26：从终点到起点遍历随机生长树，得到一条完整路径，并对该条路径进行平滑处理，得到最终路径，算法结束。
5.如权利要求3所述的改进A‑RRT的移动机器人路径规划方法，其特征在于，所述步骤S2中，在二维平面中两个端点之间的连续轨迹一般用n阶多项式表示，即其中P0,P1…Pn为多项式系数，i为多项式的阶数；
整个轨迹可以视为由N段轨迹组合而成：
加加加速度Snap(t)可表示为：
将式(0.6)代入式(0.5)，可表示出整段轨迹的MinimumSnap最小化目标函数定义为加加加速度变化率的平方在每段轨迹相应时间段内的积分，则通式可表示为:其中P是各段的参数矩阵，Q为权重矩阵，i和l为矩阵的行索引和列索引，索引从0开始；
整个轨迹的MinimumSnap最小化目标函数的表达式为:
6.如权利要求5所述的改进A‑RRT的移动机器人路径规划方法，其特征在于，所述步骤S2‑2中构建等式约束方程：S2‑2‑1连续性约束：
相邻轨迹连接处首尾两段轨迹满足位置、速度、加速度、加加速度相等，从而构建为连续性等式约束：其中：Pj,i，Pj+1,l分别表示为第j段轨迹和j+1段轨迹段首的多项式系数；
S2‑2‑2光滑性约束：
整段轨迹的每两段连接处的速度、加速度、加加速度都能够满足某一特定值，从而构建到光滑性等式约束：将式(0.9)、式(0.10)代入式(0.8)联合求解，分别对x轴和y轴求解基于MinimumSnap的目标函数，将计算得到的每段轨迹系数P代入各段状态方程中，而得到整段轨迹的每个方向的状态量。
7.如权利要求6所述的改进A‑RRT的移动机器人路径规划方法，其特征在于，所述步骤S3中动态窗口法运行步骤如下：S3‑1、速度采样，根据机器人速度、加速度限制及大减速下的安全限制，在机器人线速度和角速度空间进行采样(dx,dy,dθ)；
S3‑2、轨迹模拟，根据机器人的运动模型，对每一个采样速度进行计算，模拟在该采样速度下，机器人在一小段时间里的运动轨迹；
S3‑3、轨迹评价，使用评价函数对步骤S3‑2中模拟的轨迹进行评价打分，评价相关函数；
S3‑4、速度选择，根据评价函数选择得分最高的轨迹，该轨迹下的速度即为最优速度，下发速度至下位机；
S3‑5、判断是否到达目标，如果未到达跳转至步骤S3‑1继续执行，否则结束。 说明书 : 一种改进A‑RRT的移动机器人路径规划方法技术领域[0001] 本发明涉及机器人的运动规划技术领域，更具体地说，涉及一种改进A‑RRT的移动机器人路径规划方法。背景技术[0002] 移动机器人运动规划是机器人研究领域的常见问题，同时也是基础问题。其目的是为机器人规划出一条由起始位置到目标位置的安全路径，该路径要符合其在工作空间中的一些既定的约束条件。针对移动机器人的路径规划，已经有许多的算法被提出和应用，例如人工势场法、蚁群算法、A*算法和遗传算法等，但是，常见的这些算法在理论上都存在一定的不足。例如人工势场法存在局部最小值问题，A*算法应用在较大空间中，运算量会非常大，蚁群算法在求解过程中，会占据较大的计算量。[0003] 基于快速搜索随机树算法进行路径规划，是通过对状态空间中的采样点进行碰撞检测，对空间中障碍物不需要过于准确地建模，这样有利于机械臂在路径规划中的高维空间和复杂约束下的运动规划。[0004] 相比于以上的算法，传统的RRT算法的工作流程比较简单，以自由空间中一个初始点为根节点，采用随机采样的方法进行扩展，生成无碰撞的随机扩展树。当扩展树不停生长，在自由空间中逐步覆盖，最终会得到足够靠近终点的节点，从而生成从初始到目标的路径。该算法在理论上来说是简洁有效的，但是，由于其存在过多多于的搜索路径，使得其在时间和计算上的浪费。发明内容[0005] 1.要解决的技术问题[0006] 本申请旨在提供一种改进A‑RRT的移动机器人路径规划方法，其至少在一个方面比背景技术中说明的现有技术有利。[0007] 2.技术方案[0008] 本发明的目的通过以下技术方案实现：[0009] 一种改进A‑RRT的移动机器人路径规划方法，步骤为：[0010] S1、全局路径规划改进：[0011] S1‑1、在RRT*算法中引入目标人工势场，由此来引导随机生长树的生长趋势向目标点方向生长；[0012] S1‑2、引入膨胀半径，使得机器人在真实环境中移动时能够避开狭窄的移动路线，安全达到目标点；[0013] S2、基于MinimumSnap轨迹规划的A‑RRT*算法路径平滑处理：[0014] S2‑1、控制移动机器人的加加加速度变化率最小，通过将四阶倒数的积分值作为代价函数，并且以最小化代价函数来实现损耗能量最小，通过对轨迹上的所有端点进行约束；[0015] S2‑2、然后再根据代价函数和约束条件得到二次规划的一般形式，最终获得一条满足约束条件的光滑轨迹；[0016] S3、局部路径规划的动态窗口法：采用动态窗口法进行规划，机器人在移动的过程中根据自身所处的环境信息从而做出相应的决策，完成局部调整动作。[0017] 进一步的，步骤S1融合人工势场的RRT*算法中，RRT*算法新节点的生成步骤为：[0018] S1‑11：找到距离qrand最近的节点qnear，并连接qrand与qnear，以qnear为起点向qnear截取步长λ；[0019] S1‑12：连接qnear与qgoal，以qnear为起点向终点qgoal截取步长kλ；[0020] S1‑13：引入人工势场法的合力生成思想得到新节点qnew，连接qnew与qnear，如果qnew与qnear之间无障碍物，则将qnew加入随机生长树。[0021] 更进一步的，步骤S1中新节点qnew的公式表示为：[0022] 设定向目标点qgoal方向扩展的节点q1，向随机点qrand方向扩展的节点为q2；[0023] 人工势场法的引力场函数为：[0024] 其中a为引力场系数，Xg为目标点的位置，引力Fatt(X)：则目标点qgoal方向扩展的节点q1为：[0025][0026] 其中k为目标点对移动机器人的引力系数，λ为扩展步长；[0027] 向随机点qrand方向扩展的节点为q2有：[0028][0029] 最后加入目标引力势场后新节点qnew的数学表达式为：[0030][0031] 更进一步的，步骤S1中引入膨胀半径后整体融合算法的具体流程如下：[0032] S1‑21：算法初始化，设置移动机器人的起始点与终点、障碍物的膨胀半径以及其他相关参数；S1‑22：在空闲的状态空间中随机生成一个采样点qrand，遍历随机搜索树T中的所有节点，随机树中找到一个距离qrand最小的一个节点qnear，并连接qrand与qnear，且以qnear为起点向qrand方向扩展一个步长λ，然后连接qnear和终点qgoal，并以qnear为起点向目标点qgoal扩展kλ的长度，k为终点对移动机器人引力系数；[0033] S1‑23：通过引入人工势场法的合力生成的方式得到新节点qnew，连接节点qnew和qnear并判断两点之间是否存在障碍物，如果存在放弃此次生长，反之将节点qnew加入随机搜索树；[0034] S1‑24：搜索qnew固定范围内的节点，重新选择qnew的父节点，并对整个随机树进行剪枝；[0035] S1‑25：判断qnew是否到达终点，若没达到则转步骤S1‑22；反之转步骤S1‑26；[0036] S1‑26：从终点到起点遍历随机生长树，得到一条完整路径，并对该条路径进行平滑处理，得到最终路径，算法结束。[0037] 更进一步的，步骤S2中，在二维平面中两个端点之间的连续轨迹一般用n阶多项式表示，即[0038][0039] 其中P0,P1…Pn为多项式系数，i为多项式的阶数；(针对首末状态下约束位置、速度、加速度、加加加速度，需要构建7次8阶多项式的目标函数才能够满足方程半正定求解要求，因此i＝7)。[0040] 整个轨迹可以视为由N段轨迹组合而成：[0041][0042] 加加加速度Snap(t)可表示为：[0043][0044] 将式(0.6)代入式(0.5)，可表示出整段轨迹的MinimumSnap最小化目标函数定义为加加加速度变化率的平方在每段轨迹相应时间段内的积分，则通式可表示为:[0045][0046] 其中P是各段的参数矩阵，Q为权重矩阵，i和l为矩阵的行索引和列索引，索引从0开始；[0047] 整个轨迹的MinimumSnap最小化目标函数的表达式为:[0048][0049] 更进一步的，步骤S2‑2中构建等式约束方程：[0050] S2‑2‑1连续性约束：[0051] 相邻轨迹连接处首尾两段轨迹满足位置、速度、加速度、加加速度相等，从而构建为连续性等式约束：[0052][0053] 其中：Pj,i，Pj+1,l分别表示为第j段轨迹和j+1段轨迹段首的多项式系数；[0054] S2‑2‑2光滑性约束：[0055] 整段轨迹的每两段连接处的速度、加速度、加加速度都能够满足某一特定值，从而构建到光滑性等式约束：[0056][0057] 将式(0.9)、式(0.10)代入式(0.8)联合求解，分别对x轴和y轴求解基于MinimumSnap的目标函数，将计算得到的每段轨迹系数P代入各段状态方程中，而得到整段轨迹的每个方向的状态量。[0058] 更进一步的，步骤S3中动态窗口法运行步骤如下：[0059] S3‑1、速度采样，根据机器人速度、加速度限制及大减速下的安全限制，在机器人线速度和角速度空间进行采样(dx,dy,dθ)；[0060] S3‑2、轨迹模拟，根据机器人的运动模型，对每一个采样速度进行计算，模拟在该采样速度下，机器人在一小段时间里的运动轨迹；[0061] S3‑3、轨迹评价，使用评价函数对步骤(2)中模拟的轨迹进行评价打分，评价函数包含多个指标如：与全局路径的距离、障碍物的有无、行驶的朝向等；[0062] S3‑4、速度选择。根据评价函数选择得分最高的轨迹，该轨迹下的速度即为最优速度，下发速度至下位机。[0063] S3‑5、判断是否到达目标，如果未到达跳转至步骤S3‑1继续执行，否则结束。[0064] 3.有益效果[0065] 相比于现有技术，本发明的优点在于：[0066] (1)本发明的改进A‑RRT的移动机器人路径规划方法，采用融合人工势场的RRT*算法和膨胀半径的A‑RRT*算法，可以有效减小规划的路径代价，为移动机器人规划出一条优化的路径，并提高其运行的平稳性，完成移动操作机器人的地图构建与路径规划；[0067] (2)由于RRT*算法还是继承了传统RRT算法的空间搜索的随机性，从而导致RRT*算法在整个搜索过程中缺乏启发性以及导向性，因此本发明的改进A‑RRT的移动机器人路径规划方法引入目标引力势场，由此来引导随机生长树的生长趋势向目标点方向生长，解决了RRT*算法的缺少导向性的问题，所以很大程度上缩短了RRT*算法的搜索时间，该算法的引入提高了算法的规划效率。[0068] (3)传统的二维算法有时可能一味的追求达到目标点的路径最短，忽视了安全性。机器人在利用激光雷达进行建图时得到的为一个二维地图环境，由于在真实环境中有时候障碍物的形状是不规则的，从而导致机器人在通过一些狭窄环境中时会与障碍物发生碰撞，导致机器人的损坏，因此本发明的改进A‑RRT的移动机器人路径规划方法引入带有膨胀半径的算法，使得机器人在真实环境中移动时能够避开狭窄的移动路线，安全达到目标点；[0069] (4)本发明的改进A‑RRT的移动机器人路径规划方法中，A‑RRT*算法能够规划出一条从起点到目标点的有效离散空间路径点，得到的路径比较曲折且不光滑，移动机器人沿着这样的路径运动导致大量的能量损失，因此需要对所得到的路径进行平滑处理，使得处理后的路径各个节点之间的过渡更加的平滑，机器人在遇到拐点时生成一条适合机器人的光滑路径；基于MinimumSnap的轨迹优化算法是为了控制移动机器人的加加加速度变化率最小，该方法通过将四阶倒数的积分值作为代价函数，并且以最小化代价函数来实现损耗能量最小。通过对轨迹上的所有端点进行约束，然后再根据代价函数和约束条件得到二次规划的一般形式，最终获得一条满足约束条件的光滑轨迹。[0070] (5)本发明的改进A‑RRT的移动机器人路径规划方法中，局部路径规划需要考虑实时性，本发明采用动态窗口法进行规划，机器人在移动的过程中根据自身所处的环境信息从而做出相应的决策，完成局部调整动作。[0071] (6)本发明的改进A‑RRT的移动机器人路径规划方法，主要是一种在粒子滤波的基础上融合了Rao‑Blackwellized思想，并且针对传统RRT算法在实际操作中存在的一些问题，对其存在的问题进行改进，提出了基于膨胀半径的A‑RRT*算法；与传统RRT、RRT*相比，改进后的算法能够在较短的时间内完成路径规划，路径的代价会更小，搜索效率高，具备更高的安全性，且构架保持了原有简洁的特点；将该算法应用在串联机器人的关节空间中，使串联机器人能够在工作空间中有障碍物的情况下得到一条安全的路径；引入基于MinimumSnap的AR‑RRT*算法路径平滑方法，最后引入动态窗口法对局部路径进行规划。附图说明[0072] 图1为本发明方法的流程图；[0073] 图2为A‑RRT*节点扩展示意图；[0074] 图3改进前后路径对比图；[0075] 图4三种算法仿真效果图；[0076] 图5优化前后路径对比图；[0077] 图6动态窗口法流程。具体实施方式[0078] 为进一步了解本发明的内容，结合附图和实施例对本发明作详细描述。[0079] 实施例[0080] 本实施例的改进A‑RRT的移动机器人路径规划方法，具体的实施流程见图1，具体实施方式详述如下：[0081] 步骤1：全局路径规划的改进算法：[0082] 全局路径规划是指移动机器人在已知的环境中，机器人通过算法得到从起点到终点的一条安全无碰撞且最优的可行路径。应用于移动机器人的全局路径规划算法有很多种，本实施例提出了一种基于膨胀半径的A‑RRT*算法(AR‑RRT*)来完成移动机器人的全局路径规划，融合了人工势场的RRT*算法。[0083] 本方法是在RRT*算法的基础上，[0084] (1)引入目标引力势场(即人工势场的方法)，引导随机生长树的生长趋势向目标点方向生长，解决了RRT*算法的缺少导向性的问题，所以很大程度上缩短了RRT*算法的搜索时间，提高了算法的规划效率，A‑RRT*节点扩展如图2所示，改进后的RRT*算法新节点的生成步骤为：[0085] 1)找到距离qrand最近的节点qnear，并连接qrand与qnear，以qnear为起点向qnear截取步长λ。[0086] 2)连接qnear与qgoal，以qnear为起点向终点qgoal截取步长kλ。[0087] 3)引入人工势场法的合力生成思想得到新节点qnew，连接qnew与qnear，如果qnew与qnear之间无障碍物，则将qnew加入随机生长树。[0088] 由以上步骤可以看出，新节点qnew由任意节点qrand和终点qgoal共同决定，下面给出新节点qnew的公式表示：[0089] 设定向目标点qgoal方向扩展的节点q1，向随机点qrand方向扩展的节点为q2。人工势场法的引力场函数为： 其中a为引力场系数，Xg为目标点的位置，引力Fatt(X)： 则目标点qgoal方向扩展的节点q1为：[0090][0091] 其中k为目标点对移动机器人的引力系数，λ为扩展步长。[0092] 向随机点qrand方向扩展的节点为q2有：[0093][0094] 最后加入目标引力势场后新节点qnew的数学表达式为：[0095][0096] (2)引入膨胀半径[0097] 传统的二维算法有时可能一味的追求达到目标点的路径最短，从而忽视了移动机器人在真实环境下进行运动规划时最关键的一个问题(移动机器人的安全性)。机器人在利用激光雷达进行建图时得到的为一个二维地图环境，由于在真实环境中有时候障碍物的形状是不规则的，从而导致机器人在通过一些狭窄环境中时会与障碍物发生碰撞，导致机器人的损坏。因此引入带有膨胀半径的算法，使得机器人在真实环境中移动时能够避开狭窄的移动路线，安全达到目标点。效果如图3所示，其中图3(a)是未加入膨胀半径思想的路径图，规划出的路径直接通过狭窄通道，可能会造成机器人的损坏，图3(b)是加入膨胀半径思想的路径图，机器人绕开了狭窄通道，并且机器人与障碍物保有一定的距离，确保了机器人的安全性。[0098] 以上步骤说明采用融合人工势场的RRT*算法和膨胀半径的A‑RRT*算法，可以有效减小规划的路径代价，为移动机器人规划出一条优化的路径，并提高其运行的平稳性，完成移动操作机器人的地图构建与路径规划，由此来引导随机生长树的生长趋势向目标点方向生长，解决了RRT*算法的缺少导向性的问题，所以很大程度上缩短了RRT*算法的搜索时间，该算法的引入提高了算法的规划效率。[0099] 整体融合算法的具体流程如下：[0100] 1)算法初始化，设置移动机器人的起始点与终点、障碍物的膨胀半径以及其他相关参数。[0101] 2)在空闲的状态空间中随机生成一个采样点qrand，遍历随机搜索树T中的所有节点，随机树中找到一个距离qrand最小的一个节点qnear，并连接qrand与qnear，且以qnear为起点向qrand方向扩展一个步长λ，然后连接qnear和终点qgoal，并以qnear为起点向目标点qgoal扩展kλ的长度(k为终点对移动机器人引力系数)。[0102] 3)通过引入人工势场法的合力生成的方式得到新节点qnew，连接节点qnew和qnear并判断两点之间是否存在障碍物，如果存在放弃此次生长，反之将节点qnew加入随机搜索树。[0103] 4)搜索qnew固定范围内的节点，重新选择qnew的父节点，并对整个随机树进行剪枝。[0104] 5)判断qnew是否到达终点，若没达到则转步骤2；反之转步骤6。[0105] 6)从终点到起点遍历随机生长树，得到一条完整路径，并对该条路径进行平滑处理，得到最终路径，算法结束。[0106] 为了验证本实施例提出的基于膨胀半径的A‑RRT*算法的可行性与优越性，利用MATLAB在二维空间内绘制多种地图环境。各个地图都具有一定的特殊性，对于算法的性能来说也是一种考验，选择RRT、RRT*和AR‑RRT*这三种算法在MATLAB2019a中进行路径规划实验，并在算法耗时和路径长度方面进行对比。[0107] 如图4可知，普通RRT算法所得到的路径比较曲折，产生了较多的无用节点。RRT*算法虽然得到了相对较短的路径，但距离障碍物的距离太近且耗费了太多的节点。相对于前两种算法，基于膨胀半径的A‑RRT*算法得到的路径弯折较少，且距离障碍物保持一定的距离。[0108] 步骤2：基于MinimumSnap的AR‑RRT*算法路径平滑处理。[0109] 改进的AR‑RRT*算法能够规划出一条从起点到目标点的有效路径，但其得到的路径是由一系列离散的空间点组成，得到的路径比较曲折且并不光滑，导致大量的能量损失。因此需要对所得到的路径进行平滑处理，使得处理后的路径各个节点之间的过渡更加的平滑，机器人在遇到拐点时生成一条适合机器人的光滑路径。[0110] 基于MinimumSnap的轨迹优化算法是为了控制移动机器人的加加加速度变化率最小，该方法通过将四阶倒数的积分值作为代价函数，并且以最小化代价函数来实现损耗能量最小。通过对轨迹上的所有端点进行约束，然后再根据代价函数和约束条件得到二次规划的一般形式，最终获得一条满足约束条件的光滑轨迹。[0111] 在二维平面中两个端点之间的连续轨迹一般用n阶多项式表示，即[0112][0113] 其中P0,P1…Pn为多项式系数，i为多项式的阶数(针对首末状态下约束位置、速度、加速度、加加加速度，需要构建7次8阶多项式的目标函数才能够满足方程半正定求解要求。因此i＝7)。[0114] 整个轨迹可以视为由N段轨迹组合而成：[0115][0116] 加加加速度Snap(t)可表示为：[0117][0118] 将式(0.6)代入式(0.5)，可表示出整段轨迹的MinimumSnap最小化目标函数定义为加加加速度变化率的平方在每段轨迹相应时间段内的积分，则通式可表示为:[0119][0120] 其中P是各段的参数矩阵，Q为权重矩阵，i和l为矩阵的行索引和列索引，索引从0开始。[0121] 可得到整个轨迹的MinimumSnap最小化目标函数的表达式为:[0122][0123] 通过上式可知，轨迹规划问题建模成了一个数学意义上的二次规划(QuadraticProgramming，QP)问题，二次规划问题通过构建连续性等式约束和光滑性等式约束。[0124] 构建等式约束方程[0125] (1)连续性约束[0126] 相邻轨迹连接处首尾两段轨迹满足位置、速度、加速度、加加速度相等，从而构建为连续性等式约束：[0127][0128] 其中：Pj,i，Pj+1,l分别表示为第j段轨迹和j+1段轨迹段首的多项式系数。[0129] (2)光滑性约束[0130] 整段轨迹的每两段连接处的速度、加速度、加加速度都能够满足某一特定值，从而构建到光滑性等式约束：[0131][0132] 将式(0.9)、式(0.10)代入式(0.8)联合求解，分别对x轴和y轴求解基于MinimumSnap的目标函数，将计算得到的每段轨迹系数P代入各段状态方程中，而得到整段轨迹的每个方向的状态量。优化前后的对比图见图5所示，图5(a)为本文提出的改进AR‑RRT*规划出来的轨迹，可以看到虽然该算法能够规划一条从起始到目标点的有效路径，但得到的路径比较曲折不光滑，移动机器人按此路径运动，能量损失较大；添加MinimumSnap目标函数优化后的结果如图5(b)所示，可以看到，得到的曲线路径更加光滑，生成的是一条适合机器人运动的光滑路径。[0133] 步骤4：局部路径规划的动态窗口法[0134] 局部路径规划与全局路径规划不同的地方在于需要考虑到实时性，机器人在移动过程中需要根据自身所处的环境信息从而做出相应的决策。本实施例采用的是动态窗口法，该算法的实现流程见图6：[0135] 动态窗口法具体运行步骤如下：[0136] (1)速度采样，根据机器人速度、加速度限制及大减速下的安全限制，在机器人线速度和角速度空间进行采样(dx,dy,dθ)；[0137] (2)轨迹模拟，根据机器人的运动模型，对每一个采样速度进行计算，模拟在该采样速度下，机器人在一小段时间里的运动轨迹；[0138] (3)轨迹评价，使用评价函数对步骤(2)中模拟的轨迹进行评价打分，评价函数包含多个指标如：与全局路径的距离、障碍物的有无、行驶的朝向等；[0139] (4)速度选择。根据评价函数选择得分最高的轨迹，该轨迹下的速度即为最优速度，下发速度至下位机。[0140] (5)判断是否到达目标，如果未到达跳转至步骤(1)继续执行，否则结束。[0141] 本实施例采用动态窗口法进行规划，机器人在移动的过程中根据自身所处的环境信息从而做出相应的决策，完成局部调整动作。[0142] 本实施例的改进A‑RRT的移动机器人路径规划方法，并且针对传统RRT算法在实际操作中存在的一些问题，对其存在的问题进行改进，提出了基于膨胀半径的A‑RRT*算法，与传统RRT、RRT*相比，改进后的算法能够在较短的时间内完成路径规划，路径的代价会更小，搜索效率高，具备更高的安全性，且构架保持了原有简洁的特点；将该算法应用在串联机器人的关节空间中，使串联机器人能够在工作空间中有障碍物的情况下得到一条安全的路径；引入基于MinimumSnap的AR‑RRT*算法路径平滑方法，最后引入动态窗口法对局部路径进行规划。[0143] 以上示意性的对本发明及其实施方式进行了描述，该描述没有限制性，附图中所示的也只是本发明的实施方式之一，实际的结构并不局限于此。所以，如果本领域的普通技术人员受其启示，在不脱离本发明创造宗旨的情况下，不经创造性的设计出与该技术方案相似的结构方式及实施例，均应属于本发明的保护范围。

专利地区：安徽

专利申请日期：2022-06-09

专利公开日期：2024-07-26

专利公告号：CN114995431B