公开课精华|机器人的带约束轨迹规划
本文章总结于大疆前技术总监，目前在卡内基梅隆大学读博的杨硕博士在深蓝学院的关于机器人的带约束轨迹规划的公开课演讲内容。
--------全文约5000字--------
笔者不是机器人领域的，因此特地去了解了一下杨硕博士，深感佩服，不仅是他的履历，更多的是他关于学术上的至臻的追求和执着的探索。本次公开课可以算是一个机器人轨迹规划的入门介绍，同时也能够激励我们，在科研学习中，仰望星空与脚踏实地。
整个公开课分为以下几个部分：
1、复杂机器人运动轨迹规划的定义和其中的挑战
2、基础知识
3、轨迹规划方法之一：Differential Dynamic Programming
4、轨迹规划方法之二：Direct Collocation
5、轨迹规划方法之三：Planning As Inference
6、相关的论文和学习资料
复杂机器人运动轨迹规划的定义和其中的挑战
什么是复杂机器人？
复杂的机器人系统往往为高自由度、欠驱动、非线性系统。所谓欠驱动系统，是输入比要控制的量少的一类典型系统。在机器人世界中，存在着许许多多欠驱动的机器人，如多足机器人等。而如何通过关节的运动实现实现机器人特定肢体的位移是轨迹规划的目标。
其中一个比较大的挑战是，对每个关节进行独立的PID控制不一定能够实现整体的轨迹执行目标，同时复杂机器人系统会发生和环境的交互，特别是摩擦力和碰撞，而且轨迹规划和控制都必须通过机载计算机实时进行。
杨硕博士举了几个复杂机器人轨迹规划的例子。
第一个是：猴子机器人。
该机器人有3个关节，晃动过程中，只有两个关节有驱动。这是一个非常典型的欠驱动系统。只控制两个关节，并不一定能实现整体的移动抓杆的目标。因此要考虑整个系统的轨迹规划，轨迹控制器就需要考虑如何通过控制两个关节的电机，考虑上机器人自身的动力学，来实现整个机器人的移动目标。
第二个是：机械臂。
左图是通过控制上方机械臂的两个关节，来通过摩擦力使轮子转动。右图是通过控制上方的机械臂，来慢慢放平下方的立方体。
第三个是：波士顿动力的人形机器人。
杨硕博士的观察和理解是波士顿动力机器人采用Direct Collocation作为机器人的运动规方法，在线生成轨迹和处理约束。
通过以上三个例子，我们大概可以知道复杂机器人的轨迹规划领域是想要关注什么样的机器人。
如何定义轨迹规划，如下图所示：
上图是一个六足机器人，想要让这个机器人从A点移动到B点，如何通过控制每个关节的控制量，同时满足动力学方程和额外的约束，以满足上述的移动目标。
而这里的约束可以是：机器人的关节有位置、速度、力矩的限制；机器人多个关节的运动可能会被限制耦合在一起的限制；机器人一段时间内消耗的能量为定值的限制。
轨迹规划的要求是：
1、实时。解算运行2-5秒时长的轨迹的求解速度必须小于0.5秒甚至达到50Hz，这样才能做MPC（MPC是模型预测控制）。
2、尽量精确地符合约束。所有的等式约束不能有较大的违反值。
3、最好可以解出反馈控制器。反馈控制器可以提高控制稳定性。
基础知识
基础知识分为三个部分进行讲解：动力学方程、线性化、LQR(线性二次型调节器)。
首先是动力学方程，我们利用牛顿定律，将物体的力、位移、速度、加速度等变量构建成方程的形式。
最后我们都可以将系统的动力学方程写成如上
的形式。后续我们并不会很关注如何获取这个系统动力学方程，而是更多关注有了这个方程之后，我们可以做什么。
当然杨硕博士也举了三个动力学方程的例子，来展示动力学方程具体是怎么获得的。
第一个是下落的球：
这两个例子不用细说，大家仔细看一下题目和公式，有高中物理学知识就能看明白。
第二个是倒立摆小车：
这个时候，想写成一个比较好看的微分表达式还是有些难度的，但是仍然是可以写出的。我们推荐密歇根的一个教学网站(https://ctms.engin.umich.edu/CTMS/index.php?example=InvertedPendulum§ion=SystemModeling)，上面有这道题的详细推导过程。
第三个是多足机器人：
这是一个更复杂的系统，我们要考虑机器人系统的位置p、角度是θ、每个关节的角度q以及地面对每个脚尖的反作用力。这个动力学方程就会有很高的维度，分析起来就会有一定的难度。关于这个，可以具体查看Dai,Hongkai的论文(Dai, Hongkai, Andrés Valenzuela, and Russ Tedrake. "Whole-body motion planning with centroidal dynamics and full kinematics." 2014 IEEE-RAS International Conference on Humanoid Robots. IEEE, 2014.)。
第二个基础知识是线性化：
>>点击查看大图<<
对于
我们将右边一阶泰勒展开，如上图，然后进行误差小量代换，就得到
. 参考点为
.
以之前的倒立摆为例，之前倒立摆的动力学方程可以看到，方程里存在非线性的平方项，因此是非线性的。我们在它的不稳定平衡点，即竖直点做一个线性化，即状态取[0,0,pi,0]，此时
，把这个小量等式代入动力学方程，化简就可以得到如下图的线性化方程，原方程中的二次项因为sinpi = 0而消失。接方程组整理可以得到线性化的动力学方程形式，此处需要读者自己动手带进去算算。
第三个基础知识是二次型调节器(LQR):
这一部分如果没有相关背景知识的学生可能听起来非常吃力，笔者推荐一个博客，大家可以对为什么要做LQR以及LQR的直观的motivation做法有个简单认识：
https://blog.csdn.net/tauyangdao/article/details/108058222#31__53
在不指定优化标准的前提下，控制领域中的“最优”体现在“输出能够完全跟踪控制，即在每一时刻输出量与控制量完全一致”。实际过程并不是这样完美的过程，每一时刻都会存在误差。退而求其次，追求在整个工作时间的范围内误差最小,与轨迹误差类似，我们研究状态误差。
因此，把整个工作时间内每一时刻状态的误差都累加起来，只要累加值更小，便会更加接近系统性能的期望。
由上我们通过一系列推导，可以定义一个代价函数：
这个代价函数就将所有的误差累加起来，我们去最小化这个代价函数，即化为如下的优化问题：
这就是著名的连续时间无穷时域线性二次型调节器。
上述的优化问题的解如下：
>>点击查看大图<<
最优的u就是一个关于x的反馈形式。
上述是无穷时域的问题，当转换到有穷时域，即代价函数变为：
对于有穷时域的优化问题，解如下：
>>点击查看大图<<
如上图所示，和无穷时域不同的是，有穷时域的半正定矩阵S是关于t的函数，最优解u关于x的反馈系数也是关于t的函数。
对于离散化的有穷时域的优化问题：
>>点击查看大图<<
可以看出来最优解的形式上和连续时间的优化问题是有很大的相似性。
我们对上述离散时间有穷时域线性二次型调节器做一个总结：
>>点击查看大图<<
直观点以倒立摆为例，我们希望通过不断控制F，使得小车在时间T内，小车到达x0位置，并在那里保持稳定。离散时间有穷时域线性二次型调节器可以以上图所示，在时刻0的时候，位置处于x，通过控制外力
，在时刻T，到达x0，
。
轨迹规划方法之一：Differential Dynamic Programming
微分动态轨迹规划可以这么理解，上一节讲的是二次型调节器，基于的是在某个点对f函数进行泰勒线性展开，那在轨迹规划中，在整条轨迹上，我们在轨迹的每个点都各自线性展开，这样就可以保持足够的精度。这就是动态。
我们基于上一节的离散线性二次型调节器重新定义一个类似的问题，这个问题正好反过来，初始时间是在x0，通过控制外力
，在时刻T，使得x在xd，即
。
则这种情况下，我们的优化问题变成：
注意到在第N项中，我们期望
是等于
d的。
如本节开始所述，因为非线性系统的线性化精度依赖于参考点，且偏离参考点太远则线性化的方程会不准确。所以我们需要沿着轨迹进行重复线性化。如下图所示：
>>点击查看大图<<
我们每移动一点，就进行一次线性化，直到到达目标。此时优化的条件函数中的A和B是时变的，即随着位置的变化而变化。
值得一提的是Differential Dynamic Programming (DDP)、Sequential Linear Quadratic (SLQ)和 Iterative Linear Quadratic Regular (iLQR)三个是等价的，都是微分动态规划。
以iLQR对上述的微分动态规划做一个总结。
对于一个非线性系统，我们希望规划它的轨迹使得它从某个起始状态运动到某个目标状态，并且我们希望得到反馈控制器。我们首先根据经验和对模型的理解设定一个初始控制器，然后用这个控制器生成初始的轨迹。接着重复进行沿轨迹线性化、解LQR、用LQR的解更新初始的控制器的过程。
>>点击查看大图<<
以上图为例，我们构造一个初始的控制器，通过系统动力学方程，初始状态和代价函数，我们就可以依据初始控制器一步一步解出在该控制器下的轨迹，然后我们按照这个轨迹走一会，开始循环构造局部的LQR问题，即我们在轨迹上进行局部的线性化，接触LQR的解，更新控制器(外力)，所以这是一个不断修正的过程，最后达到xd。
通过仿真，我们举例如下图，通过不断修正，实时规划路径，最终到达目标。
对于之前所说的猴子机器人，我们可以让初始控制器设为重力，然后通过微分动态规划，不断修正控制器，即每个关节的力，来实现机器人抓取的功能。
微分动态规划的优点有：能获得最优轨迹，也能获得最优的反馈控制器；局部LQR问题的求解可以并行化，能达到非常高的求解速度；据说（我自己并没有实验验证过），比其他方法有更好的数值精度。
微分动态规划的缺点有：需要较好的初始化轨迹；比较难考虑额外的约束。
轨迹规划方法之二：Direct Collocation
直接配点法，放弃获得反馈控制器，而是将轨迹上每一时刻的状态和控制量看做一个非线性优化问题的决策变量，通过成熟的非线性优化领域的技术来处理约束。我们定义如下图所示的整个轨迹中的所有状态和所有控制，然后定义代价函数和约束，来求解这样的优化问题。
以六足机器人为例，我们将所有的每个时刻的状态都看作决策变量，如下图xn表示在第n时刻的决策变量，如身体的位置，身体的角度，速度，角速度，每个关节的角度等等，然后所有时刻就有N个xn这样的决策变量。
代价函数可以设置为系统速度限制或者关节速度限制等等。直接配点法关键在于约束条件。接下来我们介绍一些常见的约束。
约束一：机器人的起始姿态和终止姿态是给定的，这两个姿态由其他的基于地形的优化算法得到。我们可以给定机器人周围的地形图，优化在地形图上的站立姿态。
约束二：足尖受到的地面作用力（或者说机器人施加给地面的推力）必须处在一个摩擦锥内，这样才能避免足端打滑。
约束三：机器人运动过程中，足在swing和stance两个模式间切换。Swing mode：足在空中移动，足上的受力必为0；Stance mode：足支撑在地面上，足的运动速度必为0。这两个模式导致了足在不同时刻受到不同的约束。
约束四：对于运动轨迹上相邻的两个点，两者的差必然等于动力学方程在两个时刻之间的积分量，如下式所示。这也是直接配点法的最核心的约束。
在实际过程中，我们把轨迹表示成分段多项式，决策变量不是轨迹上的点，而是多项式的系数，这样可以大大减少问题的维度，也自然地保证了运动的平滑性，如下图所示。
将分段配点法应用到六足机器人，可以将决策变量维度和约束维度大大简化，计算时间也有很大的减少。
直接配点法的优点：可以处理任意高精度的系统动力学方程；可以处理非常复杂的约束。
直接配点法的缺点：只能解出运动轨迹，不能获得反馈控制器；非线性优化算法对于有些问题可能非常低效。
轨迹规划方法之三：Planning As Inference
Planning As Inference，规划即推理，是近年来发展的新轨迹规划方法，是现代应用深度增强学习进行机器人控制的主要理论基础之一，它将轨迹规划看做最大后验概率估计问题，建立最优控制和概率估计之间的联系。
我们将直接配点法的概念推广为条件概率分布，如上图所示，按照笔者的理解，我们把离散的值推广到概率领域，我们就可以将一个优化问题推广到回归问题上，这种解决回归问题的方式，我们就可以使用最大后验概率估计，当然也就可以使用深度学习等方式去做。
我们把轨迹规划看作最大后验概率估计，对于比较一般性的概率分布，最大后验概率估计必须要通过统计学的方法获得，但是如果概率分布都为高斯分布，我们可以用均值和方差来参数化高斯分布，从而得到概率分布的解析表达式。
因子图(factor graph)是一种常用的处理多个随机变量间的高斯分布的工具，因此我们引入因子图，如下图所示，因子图中的圆圈表示随机变量，圆点表示因子。
因子图中的每一个因子都是一个高斯分布，因为每个因子都是条件概率，所以所有因子总共的概率为因子的积。由下图中的推导，由于是高斯分布，我们可以把这个情况下的最大后验概率转化成一个矩阵的最小二乘问题。这个最小二乘问题可以通过QR分解来快速求解。
我们可以将因子图应用于最优控制，我们把LQR问题表示成因子图的形式，即将LQR中的系统动力学方程和代价函数都转化成条件概率分布，写成因子图的形式，然后我们就可以通过这个因子去求解最优反馈控制器。
值得一提的是，Planning As Inference的方法，目前的研究工作比较初步。其他已有的工作没有太多考虑约束， 主要讨论将概率分布推广到无模型、非高斯分布的情况。目前杨硕博士在进行有模型、高斯分布下如何考虑更多不同类型的约束的研究。
相关的论文和学习资料
最后，杨硕博士推荐了一些学习资料，分别是：
1、关于直接配点法
推荐MIT的一个课程：Underactuated Robotics.
2、 关于微分动态规划
推荐”Control-limited differential dynamic programming”这篇论文。以及https://studywolf.wordpress.com/2016/02/03/the-iterative-linear-quadratic-regulator-method/ 这份教材。
3、关于因子图
推荐”Factor graphs and GTSAM: A hands-on introduction.”、”Factor graphs for robot perception.”这两篇文章。
4、杨硕博士的关于如何使用因子图求解LQR问题的论文：”Equality constrained linear optimal control with factor graphs.”。
感谢雍洋同学对本次公开课内容的整理，同时感谢分享嘉宾杨硕博士的审核