--------全文约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.”。
感谢雍洋同学对本次公开课内容的整理，同时感谢分享高朋杨硕博士的审核
