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

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

【技术实现步骤摘要】
一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法


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

技术介绍

[0002]移动机器人的运动规划有广泛的应用前景，如农药喷洒、快递分发、搜索救援等方面。机器人在执行任务时，往往需要在未知复杂场景中进行作业。外界扰动(风、人为干扰等)可能会让机器人偏离其期望运动轨迹并与障碍物发生碰撞，破坏机器人的机械电气部件。此外，期望运动轨迹越平滑，机器人底层控制器就越容易进行轨迹跟随。因此，在规划机器人的期望运动轨迹时，往往需要考虑规划出一条平滑且远离障碍物的轨迹，以获得更好的轨迹跟随效果并应对外界扰动。
[0003]移动机器人的轨迹规划问题至少涉及到时间、空间等维度，通常都选择将其构造成一个优化问题来解决。根据不同求解优化问题的方式可以将这些方法分类为：基于硬约束优化问题的方法、基于软约束优化问题的方法与基于搜索的运动原语的方法。1.现有的基于硬约束的优化问题的方法规划出来的轨迹不能保证远离障碍物，由于硬约束优化问题通过构造成二次规划问题来进行求解，而轨迹与障碍物的距离的约束很难表示成一个线性的约束条件。所以通常会考虑直接膨胀无人机来得到一条远离障碍物的轨迹，但是在狭窄通道容易出现问题无解从而规划失败。2.现有基于软约束的优化问题的方法规划出来的轨迹不能保证轨迹能完成满足所有约束。软约束方法本质就是尽可能去满足所有约束，对约束没有硬性保证，其求解出来的轨迹可能会违背某些约束(如超过最大速度/加速度限制、轨迹过障碍物等)。需要人为提前找出合理的参数或规划过程中在线迭代调整参数，容易造成求解时间不稳定。3.现有基于搜索的运动原语的方法需要构造搜索树才能进行下一步的搜索求解工作，搜索树的大小跟动作/ 状态空间离散的颗粒度成正比，且离散颗粒度又会直接影响到求解出来的轨迹的平滑度等性质。想得到一条较优的轨迹就需要较小的离散颗粒度，也就需要较大的内存存储搜索树以及较长的搜索时间，这对计算机的计算力和内存要求较高。
[0004]中国专利CN110275526A，公开日为2019.09.24，公开了一种基于改进遗传算法的移动机器人路径规划方法；在一定程度上提高了路径规划质量，但是其方法步骤复杂，计算繁琐。

技术实现思路

[0005]本专利技术为克服上述现有技术中的至少一个缺陷，提供一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，有效提高了鲁棒性。
[0006]为解决上述技术问题，本专利技术采用的技术方案是：一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，包括以下步骤：
[0007]S1.路径搜索：使用基于图搜索或基于采样的路径搜索算法找到一条当前位置到目标位置的路径，该路径没有考虑动力学约束，其中路径搜索算法包括 A*算法、RRT*算法；
[0008]在三维空间里进行规划时，需要将连续空间离散成栅格地图，在使用路径搜索算法搜索出从起点到目标点的一条最短路径。由于路径搜索只考虑机器人起点与终点的位置，并不会考虑诸如当前速度、加速度以及无人机的动力学约束，这会导致找出的路径违背机器人的动力学约束，如超过了机器人的最大加速度限制导致底层控制器无法进行轨迹跟随。因此需要后续的轨迹优化工作；
[0009]S2.生成飞行走廊：在上一步找出的路径上的每个离散点生成对应的飞行走廊，并删除相同的飞行走廊，只留下不同的飞行走廊；所述的飞行走廊为：栅格地图中包含该点的无障碍的区域所围成的凸六面体，该凸六面体的边只能与地图坐标系的某一轴平行。
[0010]对于路径上的离散出来的某一点，生成包含它的飞行走廊的过程为：在栅格地图中分别向六个方向进行拓展，每次只向该方向拓展一步，并且本次拓展需要在之前拓展的生成出来的临时飞行走廊上进行拓展；当某一方向上遇到障碍物或达到地图边界时，跳过该方向的拓展；最后当所有方向的都无法再拓展时，就停止拓展，此时得到的便是该点的飞行走廊。在每次方向上循环拓展使得飞行走廊能在每一个方向上都尽量均匀分布，且边长不会相差太大。
[0011]S3.生成轨迹：基于硬约束的二次规划问题的方法，在最小化轨迹的三次导数即机器人的加速度的变化率的平方的积分来获得平滑轨迹的方法的基础上，加入对膨胀区域的侵入程度L的惩罚项作为优化问题方程的一部分，其中侵入程度L代表障碍物距离。具体包括：
[0012]在飞行走廊内会有一条多项式/样条表示对应的轨迹；用三次均匀B-样条曲线代表对应的轨迹，每个飞行走廊内B-样条的控制点的个数根据处于该飞行走廊内的路径长度启发式确定，且至少为三个。
[0013]整条轨迹有N个控制点{q0，q1，q2，...，q
N-1
}，每个控制点q
i
是一个三维向量；有N+4个节点向量{t0，t1，t2，...，t
N+3
}，每个节点的间隔为轨迹的B-样条曲线f(t)的数学表达式如下：
[0014][0015]其中，有
[0016][0017]s(t)＝[1 u(t) u2(t)u3(t)][0018]Q
m
＝[q
m-3
q
m-2
q
m-1
q
m
][0019][0020]式中，f(t)是轨迹在时间维度t上的数学表达式；u(t)是对于轨迹在时间轴上的归一化处理，[t
m
，t
m+1
]是轨迹时间维度上的可行域；s(t)是一个代表时间维度的零次方到三次方的向量；Q
m
是待优化的某四个控制点；M4是轨迹f(t)表达式中的一个常量矩阵；
[0021]总共有P个飞行走廊，N个控制点将会根据路径在飞行走廊内的长度进行启发式分
配到P个飞行走廊中，第i个飞行走廊D
i
内的c
i
个控制点为：
[0022][0023]由于需要让轨迹经过当前位置和目标位置，需要将首尾各三个控制点固定在起点与目标点；把机器人建模为边长为R米的立方体，整个过程中要求无人机不能离开飞行走廊，且在每个轴上的边都尽量远离障碍物距离R米。
[0024]假设处于第0个飞行走廊的时间段为t∈[t3，t4]，飞行走廊D0在轴 l(l∈{x，y，z})方向上的最小最大取值分别为d
l，min
、d
l，max
，则该时间段内的速度曲线和加速度曲线的控制点分别为{V0，V1，V2}、{A0，A1}，需要满足 V
l，i
∈[v
l，min
，v
l，max
]、A
l，i
∈[a
l，min
，a
l，max
]，且有：
[0025][0026][0027]在该时间内的优化问题方程为：
[0028][0029]s.t.
[0030]V
l，i...

【技术保护点】

【技术特征摘要】
1.一种可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，包括以下步骤：S1.路径搜索：使用路径搜索算法找到一条当前位置到目标位置的路径；S2.生成飞行走廊：在上一步找出的路径上的每个离散点生成对应的飞行走廊，并删除相同的飞行走廊，只留下不同的飞行走廊；S3.生成轨迹：基于硬约束的二次规划问题的方法，在最小化轨迹的三次导数即机器人的加速度的变化率的平方的积分来获得平滑轨迹的方法的基础上，加入对膨胀区域的侵入程度L的惩罚项作为优化问题方程的一部分。2.根据权利要求1所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，所述的步骤S3具体包括：在飞行走廊内会有一条多项式/样条表示对应的轨迹；用三次均匀B-样条曲线代表对应的轨迹；整条轨迹有N个控制点{q0，q1，q2，...，q
N-1
}，每个控制点q
i
是一个三维向量；有N+4个节点向量{t0，t1，t2，...，t
N+3
}，每个节点的间隔为轨迹的B-样条曲线f(t)的数学表达式如下：其中，有s(t)＝[1 u(t) u2(t) u3(t)]Q
m
＝[q
m-3 q
m-2 q
m-1
q
m
]式中，f(t)是轨迹在时间维度t上的数学表达式；u(t)是对于轨迹在时间轴上的归一化处理，[t
m
，t
m+1
]是轨迹时间维度上的可行域；s(t)是一个代表时间维度的零次方到三次方的向量；Q
m
是待优化的某四个控制点；M4是轨迹f(t)表达式中的一个常量矩阵；总共有P个飞行走廊，N个控制点将会根据路径在飞行走廊内的长度进行启发式分配到P个飞行走廊中，第i个飞行走廊D
i
内的c
i
个控制点为：3.根据权利要求2所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，在所述的步骤S3中，由于需要让轨迹经过当前位置和目标位置，需要将首尾各三个控制点固定在起点与目标点；把机器人建模为边长为R米的立方体，整个过程中要求无人机不能离开飞行走廊，且在每个轴上的边都尽量远离障碍物距离R米。4.根据权利要求3所述的可保证远离障碍物的基于硬约束优化问题的机器人平滑轨迹规划方法，其特征在于，假设处于第0个飞行走廊的时间段为t∈[t3，t4]，飞行走廊D0在轴l(l∈{x，y，z})方向上的最小最大取值分别为d
l，min
、d
l，max
，则该时间段内的速度曲线和加速度曲线的控制点分别为{V0，V1，V2}、{A0，A1}，需要满足V
l，i
∈[v
l，min
，v
l，max

【专利技术属性】
技术研发人员：郑培炜，成慧，
申请(专利权)人：中山大学，
类型：发明
国别省市：