轨迹生成与优化

最新推荐文章于 2025-06-07 13:39:42 发布

原创最新推荐文章于 2025-06-07 13:39:42 发布 · 618 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#无人机 #算法 #c++

路径规划专栏收录该内容

3 篇文章

订阅专栏

1.什么什mini_snap

我们在一个有障碍物的环境中使用前段搜索算法（a star ， RRT ），找到了一条理想的轨迹路径，但是搜索出来的并不是的路径是没有考虑动力学约束的，因此轨迹会出现不光滑的点，如果轨迹是不光滑的不考虑动力学约束的，因此无人机会飞一下然后又停一下，这显然是我们所不希望的。

如果我们希望以轨迹中的一些航向点为基础，画出一条光滑的连接每一个航向点的轨迹，那么这条轨迹就很适合机器人区运动。

2.一段多项式轨迹

对于空间中的两个点，我们可以使用直线进行拟合，对于三个点我们可以使用二次函数来进行拟合，更广义一点来说，一次函数可以拟合两个点，二次函数可以拟合三个点，哪么N次函数可以拟合N+1 个参数点

如果我们现在知道了一条轨迹，使用一个五次多项式去拟合轨迹

$P(t) = p_0t^0 +p_1t^1 + p_2t^2 + p_5t^5$

其中上面有六个未知系数，如果我们能够算出这个方程，我们至少需要6个方程，一个很明显的思路就是我们取六个点进行拟合，在大多数情况下，我们不仅关心机器人的位置，对于它的速度加速度我们也同样很关心，因此如果我们选取头尾的位置以及速度和加速度，那我们也唯一确定这个多项式，况且无人机还能以我们想要的速度加速度开始和结束。这就是论文中所采用的思路。

3.多段轨迹多项式

如果是多段的轨迹我们又如何进行操作呢

论文中的思路是：作者提出了对于一个经过多个中间点点的轨迹，分别使用多个轨迹进行拟合，分别使用多个轨迹段进行拟合，也就是个对于上述的T0-T1-T2-T3 这条轨迹，我们分别分为三个轨迹段进行轨迹拟合，分别拟合T0-T1 ， T1-T2，T2-T3，每一小段再使用一段轨迹拟合的思路进行拟合。

但是与一段轨迹拟合又有一些区别，在多数情情况下，我们很难区设定机器人在经过中间点的速度，加速度，jerk等信息。于是我们就认为中间点的速度，加速度等信息是一个可以自由i变化的状态，那么对于这种中间状态不确定的问题，我们可以构建为一个优化问题，使其中间状态取得某一个值，我们制定的代价函数达到最小值，那么一方后面就不去设计无人机以什么样的状态通过中间点，另一方面还可以通过最优化代价函数得到一个最优的轨迹。

通俗点说就是，以上面的轨迹为例，我们已经知道了T0和T3的位置，速度，加速度等运动信息，并且我们还知道了T1, T2 的位置信息，而对于T2,T3的速度，加速度等信息，我们则认为他是一个变量，我们通过设置一个目标代价函数，找到这几个变量在取一定值时，使得我们设计的代价函数取得最小值，更通俗的说就是：就是通过代价函数来计算这几个变量，使得无人机耗能最少。

4 mini_snap 代价函数：

4.2 连续性约束、

首先我们需要限制中间点的航迹点在两端轨迹的结合处是左右两边的状态要连续，如果我们的目标是Jerk ，那么我们得抱枕接合处的三阶导数是连续的，因为我们需要对Kerk进行积分，那么多项式在求导三次之后必须要连续，所以要确保0阶 1阶 2 阶导数是光滑的，也就是保证求解到加速度函数依旧是光滑的，这一点就体现了多项式轨迹的优势，他是一个天然高阶光滑的函数，写成数学形式，就是

$P_m^k(T_m) = P_{m+1}^{k}(T_m)$

这个数学公式就是指在Tm时刻，轨迹段Pm和Pm+1 具有相同的k阶导数

4.3 微分约束

连续性约束保证了轨迹的光滑性，虽然限制了轨迹的光滑性，但是并没有限制轨迹必须经过某一些点，或者以一个确定状态下通过某一些点，也就是这些多项式一些是已知的，但是大多数情况下，我们都对航向点的位置是有限制的，并不能随意生成，所以还需要给中间点加一些限制，包括位置，速度，加速度， jerk， snap 等等的限制，

首先我们考虑一个很现实的问题，无人机的起点和终点我们是事先知道的，并且机器人以上面速度或者加速度开始，我们也是可以设置的，无人机的终点的速度加速度也是可以事先给定，无人机的中间点的位置我们也是事先给定的，但是呢，中间的速度，加速度我们没办法去约束，因为很难去确定机器人到达某一个点的速度或者加速度。所以对于中间点的速度或者加速度或者更高阶的jerk和snap 等，我们不对其进行微分限制，只让它们具有连续性约束，让后让优化器给它们分配最优的状态。

所以对于起始点的位置速度，加速度等我们使用确定的值。

中间点，我们使用位置进行限制。

最后构建了一个带约束的QP问题：