CN112432648A - 一种移动型机器人安全运动轨迹的实时规划方法 - Google Patents

Abstract

本发明涉及一种移动型机器人安全运动轨迹的实时规划方法，包括：S1.拓扑路径搜索，包括地图栅格化、基于采样的路径搜索和拓扑路径选取三个步骤，得到优化后能量最小的路径作为整个拓扑路径搜索过程的最终轨迹；S2.根据选取的最终轨迹作为初始轨迹进行局部梯度信息的构建；S3.进行轨迹优化，基于软约束的优化方法，考虑轨迹的平滑性、安全性和动力学约束三个指标，通过加权得到整体轨迹的优化方程。本发明在优化轨迹远离障碍物方面以及算法实时性方面更优；本发明使用的拓扑路径搜索算法能够缓解常见软约束优化方法的局部极小值问题。相对于一些专门为软约束优化方法缓解局部极小值方案，该发明在计算复杂度与整体轨迹的鲁棒性上更优。

Description

一种移动型机器人安全运动轨迹的实时规划方法

技术领域

本发明属于移动型机器人运动规划领域，更具体地，涉及一种移动型机器人安全运动轨迹的实时规划方法。

背景技术

移动型机器人的轨迹规划问题一般会将其数学化为一种可优化问题来求解，主要分成基于硬约束的优化问题和基于软约束的优化问题：

现有的基于硬约束的优化方法通常分两步：首先对传感器感知到的地图中以机器人当前位置点为起点进行膨胀，得到凸可飞行区域空间作为优化问题的硬约束条件。再通过二次规划方法对分段多项式轨迹进行优化求解，得到一条在凸可飞行区域空间约束下的最小化能量的轨迹解。基于硬约束的优化方法中的第一步：对于真实场景中的复杂环境，很难构建理想的凸可飞行空间。若使用长方体进行膨胀构建，其对可飞行区域的覆盖程度较低，会大大限制机器人的移动空间，甚至导致问题无解而规划不出运动轨迹；而若使用凸多面体进行膨胀构建，其相对于长方体膨胀在计算复杂度上会大大增加，难以在有限的机载计算资源下高效运行。基于硬约束的优化方法中的第二步：若考虑到机器人运动轨迹的安全性，需要机器人的运动轨迹尽可能地远离障碍物，则需要在硬约束条件中加入轨迹与障碍物的距离，其往往需要使用二次项来数学表示。而硬约束条件中存在二次及其以上的多项式会导致整个二次规划问题的求解时间大大增加。另外，分段多项式轨迹中的时间分配选择会直接影响到机器人运动轨迹的结果。

现有的基于软约束的优化方法通常将轨迹生成问题描述成非线性优化问题，在优化方程中会考虑到轨迹平滑性，运动安全性，机器人运动可行性等因素，构建带有轨迹平滑性惩罚项、对障碍物距离惩罚性以及线加速度或角加速度惩罚项的目标方程，使用基于梯度的优化方法对初始轨迹进行优化，即最小化目标方程的值，从而得到一条平滑、远离障碍物且满足机器人运动模型的轨迹。但是，基于软约束的优化方法在使用基于梯度的优化求解方法时，在遇到一些梯度突变的飞行场景时，由于周围点的梯度相差非常大，在初始轨迹穿过梯度突变的障碍物时，难以使其优化到偏离该障碍物，即陷入了局部极小值，导致无法求解得到一条平滑安全的运动轨迹。而在一些缓解局部极小值的研究方法中，通过一般的随机抽样策略或是基于采样的路径搜索算法，无法在复杂的3D空间中快速得到局部最优的初始路径，会导致算法的效率大大减少。

发明内容

本发明为克服上述现有技术中的至少一个缺陷，提供一种移动型机器人安全运动轨迹的实时规划方法，在优化轨迹原理障碍物方面以及实时性方面更优。

为解决上述技术问题，本发明采用的技术方案是：一种移动型机器人安全运动轨迹的实时规划方法，包括以下步骤：

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

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

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

在本发明中，提出的是基于拓扑路径搜索算法与基于软约束优化方法相结合的机器人路径规划方法。由于目前硬约束优化方法中硬约束条件很难在关于移动型机器人与障碍物位置之间的距离用一次线性方程来表示，一般至少为二次，而硬约束条件为存在二次及其以上约束项的优化问题的求解速度会大大下降，因此，基于软约束的优化方法，相对于硬约束的优化方法来说，为实时性更好。同时，该方法能够在前期的拓扑路径搜索上避免局部极小值问题，同时能在有限采样点上实现多个初始路径的合并优化，避免了大量额外的计算量损耗和提高了整体算法的鲁棒性，并使得最终求解得到的轨迹在能量最小化指标上更优。同时，本发明用于软约束优化的梯度信息，不是基于全局地图来构建完整的梯度信息图，而是仅仅根据初始路径和与其接近的障碍物来构建局部的梯度信息图，使得整体算法的计算复杂度更小。

进一步的，所述的地图栅格化是指使用适合的栅格大小对全局3D地图进行栅格化，即将连续的感知三维空间转化成离散的栅格地图，其最小单位为一个栅格大小，使得生成的栅格地图在仅丢失少量精度的情况下能够大大减少地图信息的计算量。具体来讲，地图栅格化相当于将有深度摄像机感知到密集型像素级别的点云地图信息，通过提取一个栅格体积大小的像素点信息进行加和取平均，将计算得到的深度值来替换对应栅格体积下包含的像素点的深度值，去除了冗余的地图信息，在仅丢失少量地图信息精度的情况下，既缓解了机载计算平台的数据储存压力，又减少了后续路径搜索步骤的计算量。

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

进一步的，拓扑路径选取是指对多条从机器人当前位置指向目标导航点的路径进行重采样，两两路径的采样点之间没有存在障碍物，即表示这两条路径是拓扑相等的，则删除相对来说更长的路径，通过这一步操作，即可得到多条拓扑不等的路径；，通过对多条拓扑不等的路径分别进行均匀B样条优化，以得到平滑且连续的轨迹，可用于判断其能量消耗。

进一步的，所述的均匀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样条优化的基函数，其公式定义为：

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

进一步的，局部梯度信息的构建的过程包括：

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

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

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

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

在本发明中，通过拓扑路径搜索步骤得到的一条初始轨迹，其在不考虑机器人速度，机器人动力学约束以及环境不确定性干扰等条件下，是关于位置点的局部最优轨迹。同时，该轨迹是三维地图上的一条从机器人当前位置到目标导航点的无碰撞轨迹，而且是通过多条均匀B样条优化后的拓扑轨迹中以能量最小指标筛选出来的，可以避免轨迹陷入局部极小值问题。因此，借助这样一条无碰撞的且能量最小的局部最优初始轨迹来构建局部梯度信息，能够更好地反映到所使用的局部梯度信息，有利于接下来结合轨迹平滑性，运动安全性，机器人运动可行性等因素进行进一步的轨迹优化。

进一步的，所述的轨迹优化具体包括：

对初始轨迹进行N个点的均匀重采样，可以得到轨迹的控制点序列为{Q₀，Q₁，Q₂，...，Q_N-2，Q_N-1}，可以得到其对应的关于速度，加速度以及能量的轨迹序列，其公式如下：

基于软约束的优化方法，主要考虑了轨迹的平滑性、安全性以及动力学约束性三个指标，通过加权得到整体轨迹的优化方程，数学表达式为：

其中，λ₁，λ₂，λ₃分别对应平滑性惩罚项，安全性惩罚项，动力学约束性惩罚项的加权系数；J₁表示轨迹平滑性的惩罚项；J₂表示轨迹安全性的惩罚项；J₃表示动力学约束惩罚项。

进一步的，轨迹平滑性的惩罚项J₁是关于加速度A和能量J在整个预测序列中的二范数之和，限制了机器人在轨迹上关于加速度和能量消耗上的变化率过大，以导致机器人运动过程中运动不稳定或不流畅，其公式如下：

进一步的，轨迹安全性的惩罚项J₂是关于轨迹与障碍物距离在局部梯度信息中的累加和，主要考虑了机器人运动的安全距离，限制了机器人在轨迹上与障碍物太靠近或直接碰上障碍物，以避免因环境不确定因素，如刮风，地面凹陷等等，造成机器人撞上障碍物而导航任务失败，其公式如下：

J₂＝∑f_c(Q_i)

其中，f_c是对Q_i在局部梯度信息上的累加，即促使Q_i在对临近障碍物的距离小于安全距离的情况下，优化Q_i点远离障碍物，其公式如下：

式中，d_i表示控制点Q_i与障碍物的最小距离；d_safe表示机器人与障碍物的最小安全距离。

进一步的，动力学约束惩罚项J₃是限制机器人在速度和加速度上不超过最大速度和最大加速度，不低于最小速度和最小加速度，以保证机器人在跟随规划轨迹上符合对应的运动学约束，其公式如下：

J₃＝∑f_d(V_i)+∑f_d(A_i)

其中，f_d表示对控制点对应的速度或加速度在超过最大值或低于最小值时的惩罚项，其公式如下：

式中，a₁，b₁，c₁，a₂，b₂，c₂表示常量，用于构建二元一次方程组，根据机器人的运动学模型进行调整；x_min，x_max分别表示控制点对应的速度或加速度的最小值和最大值。

与现有技术相比，有益效果是：本发明提出的一种移动型机器人安全运动轨迹的实时规划方法，相比于现有的基于硬约束优化的方法，本发明在优化轨迹远离障碍物方面以及算法实时性方面更优。相比于现有的基于软约束优化的方法，本发明使用的拓扑路径搜索算法能够缓解常见软约束优化方法的局部极小值问题。相对于一些专门为软约束优化方法缓解局部极小值方案，该发明在计算复杂度与整体轨迹的鲁棒性上更优。同时，本发明用于软约束优化的梯度信息，不是基于全局地图来构建完整的梯度信息图，而是仅仅根据初始路径和与其接近的障碍物来构建局部的梯度信息图，使得整体算法的计算复杂度更小。

附图说明

图1是本发明拓扑路径搜索的示例图。

具体实施方式

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

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

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

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

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

在本发明中，提出的是基于拓扑路径搜索算法与基于软约束优化方法相结合的机器人路径规划方法。由于目前硬约束优化方法中硬约束条件很难在关于移动型机器人与障碍物位置之间的距离用一次线性方程来表示，一般至少为二次，而硬约束条件为存在二次及其以上约束项的优化问题的求解速度会大大下降，因此，基于软约束的优化方法，相对于硬约束的优化方法来说，为实时性更好。同时，该方法能够在前期的拓扑路径搜索上避免局部极小值问题，同时能在有限采样点上实现多个初始路径的合并优化，避免了大量额外的计算量损耗和提高了整体算法的鲁棒性，并使得最终求解得到的轨迹在能量最小化指标上更优。同时，本发明用于软约束优化的梯度信息，不是基于全局地图来构建完整的梯度信息图，而是仅仅根据初始路径和与其接近的障碍物来构建局部的梯度信息图，使得整体算法的计算复杂度更小。

地图栅格化是指使用适合的栅格大小对全局3D地图进行栅格化，即将连续的感知三维空间转化成离散的栅格地图，其最小单位为一个栅格大小，使得生成的栅格地图在仅丢失少量精度的情况下能够大大减少地图信息的计算量。具体来讲，地图栅格化相当于将有深度摄像机感知到密集型像素级别的点云地图信息，通过提取一个栅格体积大小的像素点信息进行加和取平均，将计算得到的深度值来替换对应栅格体积下包含的像素点的深度值，去除了冗余的地图信息，在仅丢失少量地图信息精度的情况下，既缓解了机载计算平台的数据储存压力，又减少了后续路径搜索步骤的计算量。

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

拓扑路径选取是指对多条从机器人当前位置指向目标导航点的路径进行重采样，两两路径的采样点之间没有存在障碍物，即表示这两条路径是拓扑相等的，则删除相对来说更长的路径，通过这一步操作，即可得到多条拓扑不等的路径；，通过对多条拓扑不等的路径分别进行均匀B样条优化，以得到平滑且连续的轨迹，可用于判断其能量消耗。

具体的，均匀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样条优化的基函数，其公式定义为：

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

另外，局部梯度信息的构建的过程包括：

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

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

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

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

在本发明中，通过拓扑路径搜索步骤得到的一条初始轨迹，其在不考虑机器人速度，机器人动力学约束以及环境不确定性干扰等条件下，是关于位置点的局部最优轨迹。同时，该轨迹是三维地图上的一条从机器人当前位置到目标导航点的无碰撞轨迹，而且是通过多条均匀B样条优化后的拓扑轨迹中以能量最小指标筛选出来的，可以避免轨迹陷入局部极小值问题。因此，借助这样一条无碰撞的且能量最小的局部最优初始轨迹来构建局部梯度信息，能够更好地反映到所使用的局部梯度信息，有利于接下来结合轨迹平滑性，运动安全性，机器人运动可行性等因素进行进一步的轨迹优化。

具体的，轨迹优化具体包括：

对初始轨迹进行N个点的均匀重采样，可以得到轨迹的控制点序列为{Q₀，Q₁，Q₂，...，Q_N-2，Q_N-1}，可以得到其对应的关于速度，加速度以及能量的轨迹序列，其公式如下：

基于软约束的优化方法，主要考虑了轨迹的平滑性、安全性以及动力学约束性三个指标，通过加权得到整体轨迹的优化方程，数学表达式为：

其中，λ₁，λ₂，λ₃分别对应平滑性惩罚项，安全性惩罚项，动力学约束性惩罚项的加权系数。

J₁表示轨迹平滑性的惩罚项，是关于加速度A和能量J在整个预测序列中的二范数之和，限制了机器人在轨迹上关于加速度和能量消耗上的变化率过大，以导致机器人运动过程中运动不稳定或不流畅，其公式如下：

J₂表示轨迹安全性的惩罚项，是关于轨迹与障碍物距离在局部梯度信息中的累加和，主要考虑了机器人运动的安全距离，限制了机器人在轨迹上与障碍物太靠近或直接碰上障碍物，以避免因环境不确定因素，如刮风，地面凹陷等等，造成机器人撞上障碍物而导航任务失败，其公式如下：

J₂＝∑f_c(Q_i)

其中，f_c是对Q_i在局部梯度信息上的累加，即促使Q_i在对临近障碍物的距离小于安全距离的情况下，优化Q_i点远离障碍物，其公式如下：

式中，d_i表示控制点Q_i与障碍物的最小距离；d_safe表示机器人与障碍物的最小安全距离。

J₃表示动力学约束惩罚项，是限制机器人在速度和加速度上不超过最大速度和最大加速度，不低于最小速度和最小加速度，以保证机器人在跟随规划轨迹上符合对应的运动学约束，其公式如下：

J₃＝∑f_d(V_i)+∑f_d(A_i)

其中，f_d表示对控制点对应的速度或加速度在超过最大值或低于最小值时的惩罚项，其公式如下：

式中，a₁，b₁，c₁，a₂，b₂，c₂表示常量，用于构建二元一次方程组，根据机器人的运动学模型进行调整；x_min，x_max分别表示控制点对应的速度或加速度的最小值和最大值。

本发明提出的一种移动型机器人安全运动轨迹的实时规划方法，相比于现有的基于硬约束优化的方法，本发明在优化轨迹远离障碍物方面以及算法实时性方面更优。相比于现有的基于软约束优化的方法，本发明使用的拓扑路径搜索算法能够缓解常见软约束优化方法的局部极小值问题。相对于一些专门为软约束优化方法缓解局部极小值方案，该发明在计算复杂度与整体轨迹的鲁棒性上更优。同时，本发明用于软约束优化的梯度信息，不是基于全局地图来构建完整的梯度信息图，而是仅仅根据初始路径和与其接近的障碍物来构建局部的梯度信息图，使得整体算法的计算复杂度更小。

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

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

Claims (10)

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所述的移动型机器人安全运动轨迹的实时规划方法，其特征在于，局部梯度信息的构建的过程包括：

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

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

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

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

7.根据权利要求6所述的移动型机器人安全运动轨迹的实时规划方法，其特征在于，所述的轨迹优化具体包括：

对初始轨迹进行N个点的均匀重采样，可以得到轨迹的控制点序列为{Q₀，Q₁，Q₂，...，Q_N-2，Q_N-1}，可以得到其对应的关于速度，加速度以及能量的轨迹序列，其公式如下：

基于软约束的优化方法，主要考虑了轨迹的平滑性、安全性以及动力学约束性三个指标，通过加权得到整体轨迹的优化方程，数学表达式为：

其中，λ₁，λ₂，λ₃分别对应平滑性惩罚项，安全性惩罚项，动力学约束性惩罚项的加权系数；J₁表示轨迹平滑性的惩罚项；J₂表示轨迹安全性的惩罚项；J₃表示动力学约束惩罚项。

8.根据权利要求7所述的移动型机器人安全运动轨迹的实时规划方法，其特征在于，轨迹平滑性的惩罚项J₁是关于加速度A和能量J在整个预测序列中的二范数之和，限制了机器人在轨迹上关于加速度和能量消耗上的变化率过大，以导致机器人运动过程中运动不稳定或不流畅，其公式如下：

9.根据权利要求7所述的移动型机器人安全运动轨迹的实时规划方法，其特征在于，轨迹安全性的惩罚项J₂是关于轨迹与障碍物距离在局部梯度信息中的累加和，主要考虑了机器人运动的安全距离，限制了机器人在轨迹上与障碍物太靠近或直接碰上障碍物，以避免因环境不确定因素，造成机器人撞上障碍物而导航任务失败，其公式如下：

J₂＝∑f_c(Q_i)

其中，f_c是对Q_i在局部梯度信息上的累加，即促使Q_i在对临近障碍物的距离小于安全距离的情况下，优化Q_i点远离障碍物，其公式如下：

式中，d_i表示控制点Q_i与障碍物的最小距离；d_safe表示机器人与障碍物的最小安全距离。

10.根据权利要求7所述的移动型机器人安全运动轨迹的实时规划方法，其特征在于，动力学约束惩罚项J₃是限制机器人在速度和加速度上不超过最大速度和最大加速度，不低于最小速度和最小加速度，以保证机器人在跟随规划轨迹上符合对应的运动学约束，其公式如下：

J₃＝∑f_d(V_i)+∑f_d(A_i)

其中，f_d表示对控制点对应的速度或加速度在超过最大值或低于最小值时的惩罚项，其公式如下：

式中，a₁，b₁，c₁，a₂，b₂，c₂表示常量，用于构建二元一次方程组，根据机器人的运动学模型进行调整；x_min，x_max分别表示控制点对应的速度或加速度的最小值和最大值。

Images