CN111551178B - 一种基于最短路径的分段轨迹时间规划方法 - Google Patents

Abstract

本发明公开了一种基于最短路径的分段轨迹时间规划方法，涉及移动机器人的轨迹规划，包括：在搜索到最短可行通道的基础上，对轨迹曲线中每一段轨迹的时间进行规划，使用极少数的点在可行通道内部规划出一条最短的路径，计算全局路线在每一个凸安全节点上的最短路径；计算凸安全节点之间的欧式距离得到各段曲线的时间比例，将欧式距离除以移动机器人的移动速度得到各段曲线的时间，得到最优的时间分配。本发明的算法可以快速、自动地对分段轨迹的时间进行分配，并且使用该算法分配各段轨迹的时间在一定程度上提升了轨迹的质量，更加有利于移动机器人的运动。

Description

一种基于最短路径的分段轨迹时间规划方法

技术领域

本发明涉及移动机器人的轨迹规划，具体涉及一种基于最短路径的分段轨迹时间规划方法。

背景技术

目前，移动机器人的轨迹规划问题对机器人的运动时间进行规划是必不可少的一个环节，在对轨迹进行优化时即可以综合考虑时间、路径和能量的影响，对时间要求不是很严格时，也可以先对各曲线段的时间进行规划，再对轨迹的路径或能量等进行优化。

当移动机器人的运动环境较复杂时，通常使用多个曲线段来拟合轨迹，如果各段曲线的时间比例与各段路径比例的差别较大，基于硬约束轨迹规划算法对能量等进行优化的轨迹会在个别区域出现明显的绕长路问题，因此，合理设置各段曲线的时间比例，对部分轨迹规划算法优化的轨迹质量极其重要。

发明内容

本发明的主要目的在于提供一种基于最短路径的分段轨迹时间规划方法。

本发明采用的技术方案是：一种基于最短路径的分段轨迹时间规划方法，包括：

在搜索到最短可行通道的基础上，对轨迹曲线中每一段轨迹的时间进行规划，使用极少数的点在可行通道内部规划出一条最短的路径，计算全局路线在每一个凸安全节点上的最短路径；

计算凸安全节点之间的欧式距离得到各段曲线的时间比例，将欧式距离除以移动机器人的移动速度得到各段曲线的时间，得到最优的时间分配。

进一步地，所述基于最短路径的分段轨迹时间规划方法包括以下步骤：

S1，设置目标函数；

S2，设置等式约束；

S3，设置不等式约束；

S4，创建映射矩阵；

S5，计算二次规划的H矩阵；

S6，求解优化问题；

S7，计算各段时间。

更进一步地，所述步骤S1包括：

假设有n个凸安全节点，则有2n个坐标点，用集合{p₁，p₂，...，p_2n}表示，由于该方法是在二维平面中进行规划，每一个坐标点p_i都是二维的，用p_i＝[x_i1，y_i1]表示，并且用P表示二次规划问题中的系数矩阵；设坐标点p_i与系数矩阵P的转换关系如下：

其余点依次类推，

从而，

进一步得，

整个系数矩阵P由XY坐标表示为：

构造映射矩阵M为：

映射矩阵M乘以系数矩阵P得到前后坐标点的坐标差值：

欧式距离的平方由如下式子构建：

目标函数J的表达式如下：

转化为二次规划QP问题的目标函数表达式如下：

J＝P^THP，H＝M^TM。

更进一步地，所述步骤S2包括：

位置连续约束条件将前一凸安全节点的后一坐标点和后一凸安全节点的前一坐标点约束为同一位置，等式约束条件为：

P_j2＝P_(j+1)1，j＝1，2......，3；

将等式约束转化为矩阵表达式：

A_eqP＝b_eq

其中，A_eq表示线性等式约束的系数矩阵，P为目标函数的系数矩阵，b_eq表示线性等式约束的右端向量。

更进一步地，所述步骤S3包括：

位置约束将P_i约束在相应的凸安全节点内，构造相应的不等式约束为：

A_laP≤b_lq

其中，A_lq表示线性不等式约束的系数矩阵，P为目标函数的系数矩阵，b_lq表示线性不等式约束的右端向量。

更进一步地，所述步骤S6包括：

目标函数J的优化目标为最小化各个坐标点的欧式距离，建立优化模型如下：

minJ＝P^TM^TMP

s.t.A_eqP＝b_eq，A_lqP≤b_lq

求解这个非线性优化模型，得到最优解P^*，P^*转化为由离散坐标点

表示的一条最短路径；

s.t表示约束。

更进一步地，所述步骤S7包括：

计算

中每个凸安全节点包含的两个坐标点的欧式距离，将这个距离除以移动机器人的移动速度，即可得到每一段贝塞尔曲线的最优时间T^*。

本发明的优点：

本发明的算法可以快速、自动地对分段轨迹的时间进行分配，并且使用该算法分配各段轨迹的时间在一定程度上提升了轨迹的质量，更加有利于移动机器人的运动。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1是本发明实施例的凸安全节点分布示意图；

图2是本发明实施例的约束前一凸安全节点的后一坐标点和后一凸安全节点的前一坐标点为同一位置示意图；

图3是本发明实施例的最短路径示意图；

图4是本发明实施例的基于最短路径的分段轨迹时间规划方法流程图；

图5是本发明实施例的算法在实际应用中的可视化效果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

参考图1至图4，如图1至图4所示，一种基于最短路径的分段轨迹时间规划方法，包括：

在搜索到最短可行通道的基础上，对轨迹曲线中每一段轨迹的时间进行规划，使用极少数的点在可行通道内部规划出一条最短的路径，计算全局路线在每一个凸安全节点上的最短路径；

计算凸安全节点之间的欧式距离得到各段曲线的时间比例，将欧式距离除以移动机器人的移动速度得到各段曲线的时间，得到最优的时间分配。

所述基于最短路径的分段轨迹时间规划方法包括以下步骤：

S1，设置目标函数；

S2，设置等式约束；

S3，设置不等式约束；

S4，创建映射矩阵；

S5，计算二次规划的H矩阵；

S6，求解优化问题；

S7，计算各段时间。

本发明的算法可以快速、自动地对分段轨迹的时间进行分配，并且使用该算法分配各段轨迹的时间在一定程度上提升了轨迹的质量，更加有利于移动机器人的运动。

所述步骤S1包括：

假设有n个凸安全节点，则有2n个坐标点，用集合{p₁，p₂，...，p_2n}表示，由于该方法是在二维平面中进行规划，每一个坐标点p_i都是二维的，用p_i＝[x_i1，y_i1]表示，并且用P表示二次规划问题中的系数矩阵；下面以图1(a)举例，图中凸安全节点为4个，因此

n＝4，用符号j表示第j个凸安全节点，设坐标点p_i与系数矩阵P的转换关系如下：

其余点依次类推，

从而，

进一步得，

整个系数矩阵P由XY坐标表示为：

构造映射矩阵M为：

映射矩阵M乘以系数矩阵P得到前后坐标点的坐标差值：

欧式距离的平方由如下式子构建：

目标函数J的表达式如下：

转化为二次规划QP(二次规划问题是指目标函数为二次函数，并且约束为线性形式的问题)问题的目标函数表达式如下：

J＝P^THP，H＝M^TM。

所述步骤S2包括：

位置连续约束条件将前一凸安全节点的后一坐标点和后一凸安全节点的前一坐标点约束为同一位置，等式约束条件为：

P_j2＝P_(j+1)1，j＝1，2......，3；

将等式约束转化为矩阵表达式：

A_eqP＝b_eq

其中，A_eq表示线性等式约束的系数矩阵，P为目标函数的系数矩阵，b_eq表示线性等式约束的右端向量。

所述步骤S3包括：

位置约束将P_i约束在相应的凸安全节点内，构造相应的不等式约束为：

A_lqP≤b_lq

其中，_lq表示线性不等式约束的系数矩阵，P为目标函数的系数矩阵，b_lq表示线性不等式约束的右端向量。

所述步骤S6包括：

目标函数J的优化目标为最小化各个坐标点的欧式距离，建立优化模型如下：

minJ＝P^TM^TMP

s.t.A_eqP＝b_eq，A_lqP≤b_lq

求解这个非线性优化模型，得到最优解P^*，P^*转化为由离散坐标点

表示的一条最短路径；

s.t表示约束，各个参数的计算和含义参考步骤S1到步骤S5。

所述步骤S7包括：

计算

中每个凸安全节点包含的两个坐标点的欧式距离，将这个距离除以移动机器人的移动速度，即可得到每一段贝塞尔曲线的最优时间T^*。

值得说明的一点是，上述例子中节点个数为4个，而实际使用中该算法的节点个数是任意个数的，只需更改坐标点个数、参数j的取值范围和其余相应参数即可。

本发明的创新点在于针对分段轨迹曲线的各段时间比例分配不合理时，对轨迹光滑度影响较大的问题，提出一种基于最短路径的分段轨迹时间规划方法。本专利在基于图论的轨迹规划算法构建的双层图基础上，使用极少数的点在可行通道内部规划出一条最短的路径，计算这些点之间的欧式距离即可得到各段曲线的时间比例，将欧式距离除以移动机器人的移动速度也可以得到各段曲线的时间，其中本发明的凸安全节点由双层图或者类似方法构建。

本发明使用一种简单的算法对轨迹曲线中每一段轨迹的时间进行规划，当分段轨迹的时间比例设置不合理时，轨迹曲线的质量较差，所以本专利在搜索到最短可行通道的基础上，计算全局路线在每一个凸安全节点上的最短路径，然后除以机器人移动的速度得到最优的时间分配。所以本发明求解最优时间分配问题的根本在于求解最短路线问题，为了方便起见，这条最短路线不需要增加任何机器人的运动学和动力学约束。

本发明的方案是在已经搜索出可行通道，并且得到各部分通道的最大膨胀区域的基础上进行的。以图1(a)为例，设图中的8个坐标点为{p₁，p₂，......，p₈}，起始坐标为P₁，终点坐标为P₈，图中的可行通道为点P₁到P₈的区域，图中的每一段可行通道的最大膨胀区域，这里称之为凸安全节点，图中有四个凸安全节点，每个凸安全节点包含两个坐标点，如p₁和p₂属于同一个凸安全节点，而p₇和p₈属于同一个凸安全节点。设每个坐标点的坐标为p_i＝(x_i，y_i)，i＝1，2，......8，按顺序连接p₁到p₈得到一条折线路径，如图1(b)所示，当约束前一凸安全节点的后一坐标点和后一凸安全节点的前一坐标点为同一位置时，将会出现图2的效果。

在实际使用中，只需要保持各段曲线的时间比例不变即可，当整体时间过短时，移动机器人的速度跟不上时，将无法求解出轨迹，因此，在保持时间比例不变的基础上，可以对整体的时间进行上下调整，使得整段轨迹都满足移动机器人的运动学和动力学约束；

本发明的算法在实际应用中的可视化效果参考图5，如图5所示，在该应用中本发明的算法计算得到的分段轨迹时间为：

{11.8，13.2，12.6，9.1，86，12.1，12.1，9.1，9.1，9.1，7.0，15.2，15.2，8.4，84，84，22.6}s。

本发明使用一种简单的算法对轨迹曲线中每一段轨迹的时间进行规划，当分段轨迹的时间比例设置不合理时，轨迹曲线的质量较差，在搜索到最短可行通道的基础上，计算全局路线在每一个凸安全节点上的最短路径，然后除以机器人移动的速度得到最优的时间分配。

所以本专利求解最优时间分配问题的根本在于求解最短路线问题，为了方便起见，这条最短路线不需要增加任何机器人的运动学和动力学约束。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于最短路径的分段轨迹时间规划方法，其特征在于，包括：

在搜索到最短可行通道的基础上，对轨迹曲线中每一段轨迹的时间进行规划，使用极少数的点在可行通道内部规划出一条最短的路径，计算全局路线在每一个凸安全节点上的最短路径；

计算凸安全节点之间的欧式距离得到各段曲线的时间比例，将欧式距离除以移动机器人的移动速度得到各段曲线的时间，得到最优的时间分配；

所述基于最短路径的分段轨迹时间规划方法包括以下步骤：

S1，设置目标函数；

S2，设置等式约束；

S3，设置不等式约束；

S4，创建映射矩阵；

S5，计算二次规划的H矩阵；

S6，求解优化问题；

S7，计算各段时间；

所述步骤S1包括：

假设有n个凸安全节点，则有2n个坐标点，用集合{p₁，p₂，...，p_2n}表示，由于该方法是在二维平面中进行规划，每一个坐标点p_i都是二维的，用p_i＝[x_i1，y_i1]表示，并且用P表示二次规划问题中的系数矩阵；设坐标点p_i与系数矩阵P的转换关系如下：

其余点依次类推，

从而，

进一步得，

整个系数矩阵P由XY坐标表示为：

构造映射矩阵M为：

映射矩阵M乘以系数矩阵P得到前后坐标点的坐标差值：

欧式距离的平方由如下式子构建：

目标函数J的表达式如下：

转化为二次规划QP问题的目标函数表达式如下：

J＝P^THP，H＝M^TM；

所述步骤S2包括：

位置连续约束条件将前一凸安全节点的后一坐标点和后一凸安全节点的前一坐标点约束为同一位置，等式约束条件为：

P_j2＝P_(j+1)1，j＝1，2......，3；

将等式约束转化为矩阵表达式：

A_eqP＝b_eq

其中，A_eq表示线性等式约束的系数矩阵，P为目标函数的系数矩阵，b_eq表示线性等式约束的右端向量；

所述步骤S3包括：

位置约束将P_i约束在相应的凸安全节点内，构造相应的不等式约束为：

A_lqP≤b_lq

其中，A_lq表示线性不等式约束的系数矩阵，P为目标函数的系数矩阵，b_lq表示线性不等式约束的右端向量；

所述步骤S6包括：

目标函数J的优化目标为最小化各个坐标点的欧式距离，建立优化模型如下：

min J＝P^TM^TMP

s.t.A_eqP＝b_eq，A_lqP≤b_lq

求解这个非线性优化模型，得到最优解P^*，P^*转化为由离散坐标点

表示的一条最短路径；

s.t表示约束；

所述步骤S7包括：

计算

和每个凸安全节点包含的两个坐标点的欧式距离，将这个距离除以移动机器人的移动速度，即可得到每一段贝塞尔曲线的最优时间T^*。

Images