[发明专利]一种移动型机器人安全运动轨迹的实时规划方法

权利要求书

1.一种移动型机器人安全运动轨迹的实时规划方法，其特征在于，包括以下步骤：

S1.拓扑路径搜索，包括地图栅格化、基于采样的路径搜索和拓扑路径选取三个步骤，得到优化后能量最小的路径作为整个拓扑路径搜索过程的最终轨迹；

S2.根据步骤S1选取的最终轨迹作为初始轨迹进行局部梯度信息的构建；

S3.进行轨迹优化，基于软约束的优化方法，考虑轨迹的平滑性、安全性和动力学约束三个指标，通过加权得到整体轨迹的优化方程。

2.根据权利要求1所述的移动型机器人安全运动轨迹的实时规划方法，其特征在于，所述的地图栅格化是指使用适合的栅格大小对全局3D地图进行栅格化，即将连续的感知三维空间转化成离散的栅格地图，其最小单位为一个栅格大小，使得生成的栅格地图在仅丢失少量精度的情况下能够大大减少地图信息的计算量。

3.根据权利要求1所述的移动型机器人安全运动轨迹的实时规划方法，其特征在于，所述的基于采样的路径搜索算法是指根据机载计算平台的负载大小确定最大承受的采样点数量，接着使用这个最大采样点数量对栅格地图进行采样，每个采样点都会跟与之相邻的采样点进行相连；其中，两个采样点之间的连线不能穿过障碍物，否则该两个采样点被视为不可见，即使两点之间的距离最短，也不能相连；通过上述操作，最终得到多条由机器人当前位置到目标导航点的路径。

4.根据权利要求3所述的移动型机器人安全运动轨迹的实时规划方法，其特征在于，拓扑路径选取是指对多条从机器人当前位置指向目标导航点的路径进行重采样，两两路径的采样点之间没有存在障碍物，即表示这两条路径是拓扑相等的，则删除相对来说更长的路径，通过这一步操作，即可得到多条拓扑不等的路径；，通过对多条拓扑不等的路径分别进行均匀B样条优化，以得到平滑且连续的轨迹。

5.根据权利要求4所述的移动型机器人安全运动轨迹的实时规划方法，其特征在于，所述的均匀B样条优化具体包括以下步骤：

对初始轨迹进行N个点的均匀重采样，可以得到轨迹的控制点序列为{Q₀，Q₁，Q₂，...，Q_N-2，Q_N-1}，其中，每一个控制点Q_i均为三维空间中的一个位置点；

对轨迹进行K次均匀B样条优化，则有N+K个节点向量{u₀，u₁，u₂，...，u_N+K-2，u_N+K-1}，其中每一个节点的前后间隔相同，即u₀-u₁＝u₁-u₂＝u₂-u₃＝...＝u_N+K-2-u_N+K-1；得到优化轨迹的数学表达式如下：

其中，N_i，k(u)为k次均匀B样条优化的基函数，其公式定义为：

选择一条优化后能量最小的路径作为整个拓扑路径搜索过程的最终轨迹，也是下一步局部梯度信息构建过程和轨迹优化过程的初始轨迹。

6.根据权利要求5所述的移动型机器人安全运动轨迹的实时规划方法，其特征在于，局部梯度信息的构建的过程包括：

首先通过对初始轨迹进行相同步长的重采样，得到轨迹上一系列均匀的采样点；

根据无人机大小以及飞行安全距离，预先设置一个距离阈值；

接着依次遍历整个轨迹上的采样点，确定与其距离最近的障碍物表面的点，若其距离小于所设置的距离阈值，则保留两个采样点以及其距离大小；

当轨迹上的采样点遍历结束，可以得到一个包含多个采样点对以及其距离大小的集合，是下一步中对轨迹进行进一步优化所用到的局部梯度信息。