添加轨迹运动_四足机器人学习笔记（2）轨迹优化基础

根据姜中和老师的综述，我在近期看了Matthew Kelly 的轨迹优化入门，我发现互联网中文圈儿里关于轨迹优化的介绍非常的少，可能这门课程对于已经在从事控制项目的大佬们来说是个微不足道的基础，其他项目的人隔行如隔山，很少接触到轨迹优化。反正这篇四十多页的英文讲义我看了好几遍才算是有点明白，因为讲义里大篇幅讲解样条差值，但是最让我难懂的地方其实还是在于差值（Spline Interpolation）前对结点（Knot Point）的计算。

b站里搬运的一个英文轨迹优化讲解Introduction to Trajectory Optimization也为我的学习提供了帮助。

1.轨迹优化问题简述

部分翻译参考了轨迹优化与直接配点法。

1.1问题简述

轨迹用来描述一个物体的运动过程，通常是关于时间的变量。轨迹优化一种用于寻找最佳轨迹选择的方法，通常是通过选择合适的系统输入或控制量，是系统完成期望的运动过程。在控制领域，轨迹优化近似于最优控制，但从更广的概念上来讲，轨迹优化更具一般性。

fig1.简单滑块移动示例的边界条件说明

如图1所示，一个滑块放置在光滑地面上，并受到一个水平方向的作用力。我们想要滑块在里的作用下，在1s的时间内从开始位置运动到指定位置，并刚好停在该处。这个问题存在着无数种可能的运动轨迹，进一步，我们希望从中确定一条最优的轨迹。

fig2.左边和右边分别为滑块移动的可行解和关于目标的最优解

我们可以把这个问题用数学语言形式化。系统满足如下的动力学方程：

边界约束为：

目标函数有不同的选择方式，以下给出了最小平方力和最小功两种目标：

学过自控的同学应该都在第十章最优控制里学过通过泛函法或者变分法（可以参考CSDN那篇）对这个问题进行求解，但是不是轨迹优化所关注的问题。

1.2梯形配点法（Trapezoidal Collocation）

下面将用梯形配点法来计算最优的滑块运动轨迹。将这个连续的问题进行离散化，将其转化为NLP（非线性规划）问题。首先将轨迹离散化，我们将得到一个有限的决策变量集。将整个时间分为N份，相对应的，位置x和速度v也分离成n个元素的数列：

这些配置点实际上就是最终转化的NLP问题优化变量，N表示离散的细化程度，N越大则离散化误差越小，但优化难度越大。接下来我们要把原始问题用这些配置点进行近似表示，其核心思想是两个配置点之间的状态变化等于系统动力学的积分，而积分在被积函数不确定时，用始末值计算梯形面积代替定积分：

其中

。我们把速度与作用力的动力学关系同样转化成上面的形式，忽略由梯形积分近似的误差，得到以下由配置点表示的约束方程：