CN112327853B - 一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法 - Google Patents

Abstract

本发明属于移动机器人运动规划领域，更具体地，涉及一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法。基于硬约束的二次规划问题的方法，在最小化轨迹的三次导数的平方的积分来获得平滑的轨迹的方法上加上了对膨胀区域侵入的惩罚项，作为优化问题方程的一部分，以此来得到一条平滑安全且尽量远离障碍物的轨迹。即便在狭窄的通道场景也能保证有解，提高了原先方法的鲁棒性。

Description

一种可保证远离障碍物的基于硬约束优化问题的机器人平滑 轨迹规划方法

技术领域

本发明属于移动机器人运动规划领域，更具体地，涉及一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法。

背景技术

移动机器人的运动规划有广泛的应用前景，如农药喷洒、快递分发、搜索救援等方面。机器人在执行任务时，往往需要在未知复杂场景中进行作业。外界扰动(风、人为干扰等)可能会让机器人偏离其期望运动轨迹并与障碍物发生碰撞，破坏机器人的机械电气部件。此外，期望运动轨迹越平滑，机器人底层控制器就越容易进行轨迹跟随。因此，在规划机器人的期望运动轨迹时，往往需要考虑规划出一条平滑且远离障碍物的轨迹，以获得更好的轨迹跟随效果并应对外界扰动。

移动机器人的轨迹规划问题至少涉及到时间、空间等维度，通常都选择将其构造成一个优化问题来解决。根据不同求解优化问题的方式可以将这些方法分类为：基于硬约束优化问题的方法、基于软约束优化问题的方法与基于搜索的运动原语的方法。1.现有的基于硬约束的优化问题的方法规划出来的轨迹不能保证远离障碍物，由于硬约束优化问题通过构造成二次规划问题来进行求解，而轨迹与障碍物的距离的约束很难表示成一个线性的约束条件。所以通常会考虑直接膨胀无人机来得到一条远离障碍物的轨迹，但是在狭窄通道容易出现问题无解从而规划失败。2.现有基于软约束的优化问题的方法规划出来的轨迹不能保证轨迹能完成满足所有约束。软约束方法本质就是尽可能去满足所有约束，对约束没有硬性保证，其求解出来的轨迹可能会违背某些约束(如超过最大速度/加速度限制、轨迹过障碍物等)。需要人为提前找出合理的参数或规划过程中在线迭代调整参数，容易造成求解时间不稳定。3.现有基于搜索的运动原语的方法需要构造搜索树才能进行下一步的搜索求解工作，搜索树的大小跟动作/ 状态空间离散的颗粒度成正比，且离散颗粒度又会直接影响到求解出来的轨迹的平滑度等性质。想得到一条较优的轨迹就需要较小的离散颗粒度，也就需要较大的内存存储搜索树以及较长的搜索时间，这对计算机的计算力和内存要求较高。

中国专利CN110275526A，公开日为2019.09.24，公开了一种基于改进遗传算法的移动机器人路径规划方法；在一定程度上提高了路径规划质量，但是其方法步骤复杂，计算繁琐。

发明内容

本发明为克服上述现有技术中的至少一个缺陷，提供一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，有效提高了鲁棒性。

为解决上述技术问题，本发明采用的技术方案是：一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，包括以下步骤：

S1.路径搜索：使用基于图搜索或基于采样的路径搜索算法找到一条当前位置到目标位置的路径，该路径没有考虑动力学约束，其中路径搜索算法包括 A*算法、RRT*算法；

在三维空间里进行规划时，需要将连续空间离散成栅格地图，在使用路径搜索算法搜索出从起点到目标点的一条最短路径。由于路径搜索只考虑机器人起点与终点的位置，并不会考虑诸如当前速度、加速度以及无人机的动力学约束，这会导致找出的路径违背机器人的动力学约束，如超过了机器人的最大加速度限制导致底层控制器无法进行轨迹跟随。因此需要后续的轨迹优化工作；

S2.生成飞行走廊：在上一步找出的路径上的每个离散点生成对应的飞行走廊，并删除相同的飞行走廊，只留下不同的飞行走廊；所述的飞行走廊为：栅格地图中包含该点的无障碍的区域所围成的凸六面体，该凸六面体的边只能与地图坐标系的某一轴平行。

对于路径上的离散出来的某一点，生成包含它的飞行走廊的过程为：在栅格地图中分别向六个方向进行拓展，每次只向该方向拓展一步，并且本次拓展需要在之前拓展的生成出来的临时飞行走廊上进行拓展；当某一方向上遇到障碍物或达到地图边界时，跳过该方向的拓展；最后当所有方向的都无法再拓展时，就停止拓展，此时得到的便是该点的飞行走廊。在每次方向上循环拓展使得飞行走廊能在每一个方向上都尽量均匀分布，且边长不会相差太大。

S3.生成轨迹：基于硬约束的二次规划问题的方法，在最小化轨迹的三次导数即机器人的加速度的变化率的平方的积分来获得平滑轨迹的方法的基础上，加入对膨胀区域的侵入程度L的惩罚项作为优化问题方程的一部分，其中侵入程度L代表障碍物距离。具体包括：

在飞行走廊内会有一条多项式/样条表示对应的轨迹；用三次均匀B-样条曲线代表对应的轨迹，每个飞行走廊内B-样条的控制点的个数根据处于该飞行走廊内的路径长度启发式确定，且至少为三个。

整条轨迹有N个控制点{q₀，q₁，q₂，...，q_N-1}，每个控制点q_i是一个三维向量；有N+4个节点向量{t₀，t₁，t₂，...，t_N+3}，每个节点的间隔为

轨迹的B-样条曲线f(t)的数学表达式如下：

其中，有

s(t)＝[1 u(t) u²(t)u³(t)]

Q_m＝[q_m-3q_m-2q_m-1q_m]

式中，f(t)是轨迹在时间维度t上的数学表达式；u(t)是对于轨迹在时间轴上的归一化处理，[t_m，t_m+1]是轨迹时间维度上的可行域；s(t)是一个代表时间维度的零次方到三次方的向量；Q_m是待优化的某四个控制点；M₄是轨迹f(t)表达式中的一个常量矩阵；

总共有P个飞行走廊，N个控制点将会根据路径在飞行走廊内的长度进行启发式分配到P个飞行走廊中，第i个飞行走廊D_i内的c_i个控制点为：

由于需要让轨迹经过当前位置和目标位置，需要将首尾各三个控制点固定在起点与目标点；把机器人建模为边长为R米的立方体，整个过程中要求无人机不能离开飞行走廊，且在每个轴上的边都尽量远离障碍物距离R米。

假设处于第0个飞行走廊的时间段为t∈[t₃，t₄]，飞行走廊D₀在轴 l(l∈{x，y，z})方向上的最小最大取值分别为d_l，min、d_l，max，则该时间段内的速度曲线和加速度曲线的控制点分别为{V₀，V₁，V₂}、{A₀，A₁}，需要满足 V_l，i∈[v_l，min，v_l，max]、A_l，i∈[a_l，min，a_l，max]，且有：

在该时间内的优化问题方程为：

s.t.

V_l，i∈[v_l，min，v_l，max]

A_l，i∈[a_l，min，a_l，max]

L_l，0，L_l，1∈[0，R]；

式中，J是待求解的优化方程，是关于轨迹三次导数的积分与飞行走廊侵入程度的求和；V_i是速度B-样条上的控制点，A_i是加速度B-样条上的控制点；l 代表x、y、z三个轴上的某一维，v_l，min代表l轴上的最小速度，v_l，max代表l 轴上的最大速度，a_l，min代表l轴上的最小加速度，a_l，max代表l轴上的最大加速度；区间

代表控制点在l轴上的可行域； L_l，0、L_l，1是侵入程度L向量的在l轴上的第0维和第1维；R是机器人建模成立方体的边长。

上述优化问题是一个典型的二次规划问题，可以在多项式时间内高效求解。优化问题中的待优化变量L是本发明的关键所在，其能保证在优化问题有解的情况下求解出一条尽量远离障碍物的轨迹。优化问题方程中的参数ω₀和ω₁是用于调节轨迹平滑度和远离障碍物程度的两个评价指标的权衡系数。

在本发明中，关键在于待优化目标函数中加入对障碍物的距离的惩罚项以及约束条件中使用惩罚项扩大可行域的构造形式，以此保证求解出的轨迹能在满足约束的前提下在平滑度与膨胀区域侵入程度(障碍物距离)两个指标上通过权重参数动态调整。本发明是基于硬约束的二次规划问题的方法，在最小化轨迹的三次导数(机器人的加速度的变化率)的平方的积分来获得平滑的轨迹的方法上加上了对膨胀区域侵入的惩罚项。通常情况下，轨迹规划得到的轨迹越平滑，机器人底层控制器就越容易进行轨迹跟随。这里的平滑指的是跟随该轨迹需要的加速度变化率，故通过最小化轨迹的三次导数获得平滑的轨迹。为了保证轨迹的安全性，需要保证整条轨迹上都没有障碍物，目前基于硬约束的方法通过在不满足安全约束的轨迹中间加上一个安全的航点来迭代求解出安全轨迹或在无障碍区域内生成飞行走廊并将轨迹通过飞行走廊约束在无障碍物区域内来求解安全轨迹。本发明也是利用飞行走廊作为约束来求解无障碍的轨迹，但原先的基于飞行走廊这种做法只是通过简单膨胀障碍物或膨胀无人机来保障轨迹远离障碍物，当环境中有狭窄通道时，会有无解的情况出现。本发明通过对这种方法进行改进，加入了对膨胀区域的侵入程度L(障碍物距离)的惩罚项作为优化问题方程的一部分，以此来得到一条平滑安全且尽量远离障碍物的轨迹。即便在狭窄的通道场景也能保证有解，提高了原先方法的鲁棒性。

与现有技术相比，有益效果是：

1.比现有的基于硬约束的优化问题的方法，本发明能得到一条远离障碍物的轨迹且不会有在可解情况下发生无解的情况；

2.相比现有基于软约束的优化问题的方法，本发明能得到一条满足全部约束的轨迹，而不会像软约束一样可能得到不满足约束的解；

3.相比现有基于搜索的运动原语的方法，本发明不需要较多的内存来存储搜索树，对计算机的内存压力不会太大。

附图说明

图1是本发明加入惩罚项L后规划出来的轨迹示意图。

图2是直接膨胀无人机后安全空间小于无人机导致无解的情况示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。

一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，包括以下步骤：

S1.路径搜索：使用基于图搜索或基于采样的路径搜索算法找到一条当前位置到目标位置的路径，该路径没有考虑动力学约束，其中路径搜索算法包括 A*算法、RRT*算法；

在三维空间里进行规划时，需要将连续空间离散成栅格地图，在使用路径搜索算法搜索出从起点到目标点的一条最短路径。由于路径搜索只考虑机器人起点与终点的位置，并不会考虑诸如当前速度、加速度以及无人机的动力学约束，这会导致找出的路径违背机器人的动力学约束，如超过了机器人的最大加速度限制导致底层控制器无法进行轨迹跟随。因此需要后续的轨迹优化工作；

S2.生成飞行走廊：在上一步找出的路径上的每个离散点生成对应的飞行走廊，并删除相同的飞行走廊，只留下不同的飞行走廊；所述的飞行走廊为：栅格地图中包含该点的无障碍的区域所围成的凸六面体，该凸六面体的边只能与地图坐标系的某一轴平行。

对于路径上的离散出来的某一点，生成包含它的飞行走廊的过程为：在栅格地图中分别向六个方向进行拓展，每次只向该方向拓展一步，并且本次拓展需要在之前拓展的生成出来的临时飞行走廊上进行拓展；当某一方向上遇到障碍物或达到地图边界时，跳过该方向的拓展；最后当所有方向的都无法再拓展时，就停止拓展，此时得到的便是该点的飞行走廊。在每次方向上循环拓展使得飞行走廊能在每一个方向上都尽量均匀分布，且边长不会相差太大。

S3.生成轨迹：基于硬约束的二次规划问题的方法，在最小化轨迹的三次导数即机器人的加速度的变化率的平方的积分来获得平滑轨迹的方法的基础上，加入对膨胀区域的侵入程度L的惩罚项作为优化问题方程的一部分，其中侵入程度L代表障碍物距离。具体包括：

在飞行走廊内会有一条多项式/样条表示对应的轨迹；用三次均匀B-样条曲线代表对应的轨迹，每个飞行走廊内B-样条的控制点的个数根据处于该飞行走廊内的路径长度启发式确定，且至少为三个。

整条轨迹有N个控制点{q₀，q₁，q₂，...，q_N-1}，每个控制点q_i是一个三维向量；有N+4个节点向量{t₀，t₁，t₂，...，t_N+3}，每个节点的间隔为

轨迹的B-样条曲线f(t)的数学表达式如下：

其中，有

s(t)＝[1 u(t) u²(t)u³(t)]

Q_m＝[q_m-3q_m-2q_m-1q_m]

式中，f(t)是轨迹在时间维度t上的数学表达式；u(t)是对于轨迹在时间轴上的归一化处理，[t_m，t_m+1]是轨迹时间维度上的可行域；s(t)是一个代表时间维度的零次方到三次方的向量；Q_m是待优化的某四个控制点；M₄是轨迹f(t)表达式中的一个常量矩阵；

总共有P个飞行走廊，N个控制点将会根据路径在飞行走廊内的长度进行启发式分配到P个飞行走廊中，第i个飞行走廊D_i内的c_i个控制点为：

由于需要让轨迹经过当前位置和目标位置，需要将首尾各三个控制点固定在起点与目标点；把机器人建模为边长为R米的立方体，整个过程中要求无人机不能离开飞行走廊，且在每个轴上的边都尽量远离障碍物距离R米。

下面以处于第0个飞行走廊的时间段t∈[t₃，t₄]来进行举例，飞行走廊D₀在轴l(l∈{x，y，z})方向上的最小最大取值分别为d_l，min、d_l，max，则该时间段内的速度曲线和加速度曲线的控制点分别为{V₀，V₁，V₂}、{A₀，A₁}，需要满足 V_l，i∈[v_l，min，v_l，max]、A_l，i∈[a_l，min，a_l，max]，且有：

在该时间内的优化问题方程为：

s.t.

V_l，i∈[v_l，min，v_l，max]

A_l，i∈[a_l，min，a_l，max]

L_l，0，L_l，1∈[0，R]；

式中，J是待求解的优化方程，是关于轨迹三次导数的积分与飞行走廊侵入程度的求和；V_i是速度B-样条上的控制点，A_i是加速度B-样条上的控制点；l 代表x、y、z三个轴上的某一维，v_l，min代表l轴上的最小速度，v_l，max代表l 轴上的最大速度，a_l，min代表l轴上的最小加速度，a_l，max代表l轴上的最大加速度；区间

代表控制点在l轴上的可行域； L_l，0、L_l，1是侵入程度L向量的在l轴上的第0维和第1维；R是机器人建模成立方体的边长；

上述优化问题是一个典型的二次规划问题，可以在多项式时间内高效求解。优化问题中的待优化变量L是本发明的关键所在，其能保证在优化问题有解的情况下求解出一条尽量远离障碍物的轨迹。优化问题方程中的参数ω₀和ω₁是用于调节轨迹平滑度和远离障碍物程度的两个评价指标的权衡系数。从图1可以看出，本发明通过加入一个膨胀区域侵入程度(障碍物距离)L惩罚项，可以在飞行走廊小于机器人大小时给予一定惩罚，使得优化问题有解。

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

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (9)

1.一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，包括以下步骤：

S1.路径搜索：使用路径搜索算法找到一条当前位置到目标位置的路径；

S2.生成飞行走廊：在上一步找出的路径上的每个离散点生成对应的飞行走廊，并删除相同的飞行走廊，只留下不同的飞行走廊；

S3.生成轨迹：基于硬约束的二次规划问题的方法，在最小化轨迹的三次导数即机器人的加速度的变化率的平方的积分来获得平滑轨迹的方法的基础上，加入对膨胀区域的侵入程度L的惩罚项作为优化问题方程的一部分；所述的步骤S3具体包括：

在飞行走廊内会有一条多项式/样条表示对应的轨迹；用三次均匀B-样条曲线代表对应的轨迹；

整条轨迹有N个控制点{q₀，q₁，q₂，...，q_N-1}，每个控制点q_i是一个三维向量；有N+4个节点向量{t₀，t₁，t₂，...，t_N+3}，每个节点的间隔为

轨迹的B-样条曲线f(t)的数学表达式如下：

其中，有

s(t)＝[1 u(t) u²(t)u³(t)]

Q_m＝[q_m-3q_m-2q_m-1q_m]

式中，f(t)是轨迹在时间维度t上的数学表达式；u(t)是对于轨迹在时间轴上的归一化处理，[t_m，t_m+1]是轨迹时间维度上的可行域；s(t)是一个代表时间维度的零次方到三次方的向量；Q_m是待优化的某四个控制点；M₄是轨迹f(t)表达式中的一个常量矩阵；

总共有P个飞行走廊，N个控制点将会根据路径在飞行走廊内的长度进行启发式分配到P个飞行走廊中，第i个飞行走廊D_i内的c_i个控制点为：

2.根据权利要求1所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，在所述的步骤S3中，由于需要让轨迹经过当前位置和目标位置，需要将首尾各三个控制点固定在起点与目标点；把机器人建模为边长为R米的立方体，整个过程中要求无人机不能离开飞行走廊，且在每个轴上的边都尽量远离障碍物距离R米。

3.根据权利要求2所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，假设处于第0个飞行走廊的时间段为t∈[t₃，t₄]，飞行走廊D₀在轴l(l∈{x，y，z})方向上的最小最大取值分别为d_l，min、d_l，max，则该时间段内的速度曲线和加速度曲线的控制点分别为{V₀，V₁，V₂}、{A₀，A₁}，需要满足V_l，i∈[v_l，min，v_l，max]、A_l，i∈[a_l，min，a_l，max]，且有：

在该时间内的优化问题方程为：

s.t.

V_l，i∈[v_l，min，v_l，max]

A_l，i∈[a_l，min，a_l，max]

L_l，0，L_l，1∈[0，R]；

式中，J是待求解的优化方程，是关于轨迹三次导数的积分与飞行走廊侵入程度的求和；V_i是速度B-样条上的控制点，A_i是加速度B-样条上的控制点；l代表x、y、z三个轴上的某一维，v_l，min代表l轴上的最小速度，v_l，max代表l轴上的最大速度，a_l，mmin代表l轴上的最小加速度，a_l，max代表l轴上的最大加速度；区间

代表控制点在l轴上的可行域；L_l，0、L_l，1是侵入程度L向量的在l轴上的第0维和第1维；R是机器人建模成立方体的边长。

4.根据权利要求3所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，每个飞行走廊内B-样条的控制点的个数根据处于该飞行走廊内的路径长度启发式确定，且至少为三个。

5.根据权利要求3所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，在所述的步骤S1中，使用基于图搜索或基于采样的路径搜索算法找到一条当前位置到目标位置的路径。

6.根据权利要求5所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，所述的路径搜索算法包括A*算法、RRT*算法。

7.根据权利要求3所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，所述的飞行走廊为：栅格地图中包含离散点的无障碍的区域所围成的凸六面体，该凸六面体的边只能与地图坐标系的某一轴平行。

8.根据权利要求7所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，对于路径上的离散出来的某一点，生成包含它的飞行走廊的过程为：在栅格地图中分别向六个方向进行拓展，每次只向该方向拓展一步，并且本次拓展需要在之前拓展的生成出来的临时飞行走廊上进行拓展；当某一方向上遇到障碍物或达到地图边界时，跳过该方向的拓展；最后当所有方向的都无法再拓展时，就停止拓展，此时得到的便是该点的飞行走廊。

9.根据权利要求8所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，在每次方向上循环拓展使得飞行走廊能在每一个方向上都均匀分布。

Images