最优控制、轨迹优化相关笔记、感悟

写在前面

上学期和最近无聊在看一些最优控制、轨迹优化相关的内容，涉及到MIT Russ Tedrake教授的Underactuated Robotics，以及CMU Zachary Manchester教授的Optimal Control（两套课程其实挺像的），加起来估计也看了十多天了。其中后者已经有些忘记了，因此开此文来记录学习感悟，梳理脉络，方便自己日后快速捡起（或是仅仅当做回忆也可以hh）

两门课程的内容量都非常大，本人更多记录感悟和关键，不记录全部推导等细节（可能有重要简单的推导，再细节不如直接回去，前者看课程主页，后者可以参考这位大佬的笔记）。**本人作为本科生，基础薄弱、才疏学浅、囫囵吞枣、实践经验为0，如有偏颇，请大佬们不吝赐教。本文会出现一些突兀的英文或者注释了英文，主要还是因为本人才疏学浅，不知道如何翻译最为恰当或者觉得英文挺重要，请见谅请见谅。**此外，随着阅历的增加，我也会继续完善这一笔记（不过未来的工作多半是learning-based吧hh）

研究完这些课程，本人深感本科期间学校教学体系的薄弱，作为自称TOP3的学校、A+的学科，不但不涉及最优控制或者最优化，连基础的LQR也未曾提及，控制类的课程只讲了经典控制理论和现代控制理论（不含最优控制），必修的机器人课只讲的机械臂。剩下两门好不容易选上的选修课，在规划、控制、感知方面倒确实对我有一定启迪（非常感激提供优质课程的老师们），但更多知识还是得靠自己学和悟（以及和各位大佬们交流~），正如这篇笔记的来由。

机器人知识体系

个人认为，机器人相关的知识体系，可以分为感知、决策、规划、控制四块。

感知

感知主要涉及SLAM的知识，主要研究定位、建图等，或者是CV领域的识别、分割、重建、深度估计等工作，关于系统学习，本人只在几年前粗略看过高翔博士写过的视觉SLAM十四讲（写得真的很好），最近了解较少。有机会可以再稍微了解一下。

决策

决策方面，主要是得到规划的目标或者调整规控的参数，是智能性最高层的体现，涉及到理解，是通往AGI的必经之路。

之前一段时间，这方面有不少RL来超越传统方法的工作，但是个人认为这些工作相当于给出一个更完善合理的规则表，还不足以体现智能性。近期具身智能大火，有不少mobile manipulation的工作，比如“拿来一个苹果”的任务，可以分层为1.寻找苹果 2.获取抓取点 3.规划抓取轨迹并执行 4.返回等操作，其中可以利用LMM得到任务描述，利用抓取点生成网络生成抓取点，利用常规的轨迹规划器和控制器完成底层操作等。

也可以把中间2、3两个操作合并为一个技能，这个技能通过一个foundation model完成，foundation model一般通过一系列的仿真、实物、遥操作、视频的数据imitation learning得到（不用rl是因为动作维度高、精度要求高，rl近似暴力遍历的方式不容易学会，而IL可以方便scaling并且有强先验）。

scaling是否真能带来质变，或者还会有如何全新的范式？我还挺期待的。有空想再深入调研一下单独写一个感悟。

规划、控制

上面提到的3便是传统意义上机器人的规划和控制，在这种场景下，一个静止底盘上的机械臂，在无障碍物的空间运动，也不涉及被操作物体的姿态（不需要dexterous manipulation），算一个全驱动（Fully-actuated）的系统，这意味着每一时刻的每一关节的加速度都可以被电机输出的组合得到（如果力矩足够大），运行轨迹的控制也较为简单，正如每一个在工厂里努力工作的机械臂那样，这在上个世纪已经被研究透了。

因此，本文的规划和控制便是针对于一个欠驱动（Underactuated）的系统而展开，不涉及固定机械臂，也不涉及好用又常见的基于搜索、采样的规划方法（A*、JPS、RRT*、DWA等）。本文涉及的，正如Russ在课程主页上写的那样，是algorithms for walking, running, swimming, flying, and manipulation。是去控制BD的Atlas跑酷，利用好系统的dynamics，而不是像ASIMO那样设计巨大的脚，把人形机器人当做high gain的固定机械臂去控制，强行抵消dynamics；是控制灵巧手做复杂操作（dexterous manipulation）；或是给无人机作轨迹规划。

尽管如今已经有许多工作利用RL和Imitation Learning做出了非常惊艳的工作，比如ETH Marco Hutter带的RSL组的工作，让人感觉RL+足式（尤其是四足、宇树的人形这些腿很轻，impact小，sim-to-real方便的足式）是浑然天成，Diffusion Policy的工作，也是让人眼前一亮，又比如石冠亚老师组的工作……尽管倾向于做Learning方向的工作，也相信几百年后的机器人身上很少再有数学公式（个人认为要想达到甚至超越生物的灵活运动能力，就应该摒弃数学公式，用更复杂却清晰的方式表征规划和控制的策略），但本人认为还是很有必要学习以往的优化体系的知识。理清楚现在工作和以往工作的共同之处、差异之处、互补之处，方能对机器人领域有更为透彻的理解。

下面就正式开始笔记。

系统模型

根据牛顿力学（$F=ma$），机电系统一般都是二阶系统，也就是$\ddot{q}=f(q,\dot{q},u,t)$，一般可以进一步写为一个仿射：$\ddot{q}=f_1(q,\dot{q},t)+f_2(q,\dot{q},t)u$，如果是机械臂，可以再进一步写为$M(q)\ddot{q}+C(q,\dot{q})\dot{q}={\tau }_g(q)+Bu$。

系统所有的$q$张成configuration space，而所有的$q,v$张成state space。

系统的平衡点是所有状态量变化率为零（可以通过牛顿法等方法求零点），而稳定的平衡点会产生一个region of attraction（ROA），附近的点都会流动到自身，从而可以抗扰动。Russ建议通过图分析系统稳定性，下图便是Russ教程中多次出现的无阻尼倒立摆（undamped pendulum）的酷似一对眼睛的相轨迹图（phase portrait）。

一个控制算法的目的，就是改变相轨迹图的形状，让系统的状态随着相轨迹流动到自己想要的地方，如果系统是全驱动的，甚至可以直接擦除（抵消）系统的相轨迹图，画成自己喜欢的形状（正如上面提到的ASIMO一样），但是不推荐hh。

一个系统常常会被离散化，这会涉及多种方法。这方面Russ没有怎么强调，但是Zach强调了一节课，并认为前向欧拉（显式）糟糕透了，并提倡后向欧拉（隐式）和龙格库塔（RK4，显式）。这主要是因为前向欧拉可能会高估系统的加速度/能量，导致最后发散。

输入的离散化一般用一阶保持器。

系统稳定性分析

李雅普诺夫方法

这里稳定性分析一般就是李雅普诺夫方法（Lyapunov Approach），判断准则在现代控制理论的各类书籍里很常见了，就不赘述了。

渐进稳定（asymptotically stable）的条件便是在稳定的基础上，$V$（Lyapunov function）的导数负定（少了等号），说明系统状态在无穷久后趋于平衡点，指数稳定（exponentially stable）为$\dot{V}<-\alpha V$则更进一步，说明系统指数级收敛。

Russ还讲了系统的HJB方程（Hamilton-Jacobi-Bellman equations）是系统无数可能的$V$中的一个

李雅普诺夫函数的获取

对于一个线性系统$\dot{x}=Ax$，在现代控制理论中也提到过，可以假设$\exists P,V(x)=x^{T}Px,P>0$，那么$\dot{V}(x)=x^{T}PAx+x^T{A}^{T}Px<0$，此时任取$Q>0,PA+A^{T}P=-Q$求解即可知道结果

对于非线性系统，通过搜索sums of squares (SOS)，求解半定规划（semi-definite programming ，SDP）得到结果。具体来说，可以令$V$是一系列nonlinear basis functions的组合，如$V(x)={\alpha }_0+{\alpha }_1{x}_1+{\alpha }_2{x}_2+{\alpha }_3{x}_1{x}_2+{\alpha }_4{x}_1^2+...$，然后求解如下问题，
$$\begin{gathered} fin{d}_{\alpha },s.t.V(x)isSOS, \\ -\dot{V}isSOS. \end{gathered}$$
find操作可以通过添加任意简单约束，求解一个半定规划得到结果。
可以参考这位大佬的回答。

最优控制、轨迹优化

个人感觉求解控制器相比于求解轨迹优化问题有一定的区别，控制器/策略$\pi$是对于全部$x$的映射，而轨迹优化旨在给定初始值$x_0$，得到后续一系列的控制序列。

此外规划和控制也存在区别，控制是存在闭环的，可能是通过高频规划动作序列得到，可能是通过反馈得到，规划则是开环；控制已经具备参考轨迹（这意味着在轨迹附近线性化误差是很小的），而规划没有。

优化问题涉及到表述和求解两步，正如Russ说的"transcription"，我们需要用合适的数学语言，完成problem formulation，把目标、约束、动力学近似并表述成常见的形式，然后或是求出解析解，或是更多让求解器求数值解。

动态规划（Dynamic Programming， DP）

现给定一个最优控制的目标和约束如下，
$$\begin{gathered} \underset{u(t)}{\min }=J(x(t),u(t))={\int }_0^{t_f}l(x(t),u(t))dt+l_F(x(t),u(t)), \\ s.t.\dot{x}(t)=f(x(t),u(t)), \end{gathered}$$
对于这样的问题，如果能得到最优的cost-to-go function $J^{\ast }(x)$，便可以得到最优的策略${\pi }^{\ast }(x)$。运用DP状态转移的思想，不断对$J$进行价值迭代。对于离散情况${\hat{J}}^{\ast }(s_i)\leftarrow {\min }_a\left[l_d(s,a)+{\hat{J}}^{\ast }(f_d(s,a))\right]$，便可以最后得到$J^{\ast }(s)$，${\pi }^{\ast }(s)=argmi{n}_a[J^{\ast }(s)]$，穷举$a$得到最佳$a$；而对于连续情况，
$$\begin{gathered} J^{\ast }(x)=mi{n}_u\left[h{l}_c(x,u)+J^{\ast }(x+h{f}_c(x,u))\right] \\ \approx mi{n}_u\left[h{l}_c(x,u)+J^{\ast }(x)+\frac{\partial J^{\ast }}{\partial x}h{f}_c(x,u))\right], \end{gathered}$$
其中$h$为展开时间步长，那么可以得到HJB方程$0={\min }_u\left[l(x,u)+\frac{\partial J^{\ast }}{\partial x}f(x,u))\right]$，对这个方程右边对$u$求导为零，理论上可以得到$u^{\ast }$的表达式。

两位老师都讲到了DP，尤其是Russ。DP是一个优美的思想，是RL的基础，前面的公式也能看到RL的影子，如果$J,\pi$用网络表达，并优化迭代策略，将是一个完整的RL算法，但是本人目前还没有完全理清楚思路，使两者完全对应起来，也还没理清楚DP之于现在主流的最优控制、轨迹优化又何必要联系。对于小规模的离散状态，或是把低维连续系统离散化（生成一个mesh），$J$还是能迭代的，那如果维度上升，不管是存储还是计算量都是指数级上升的，这是不能承受的，不过如果是用网络拟合，是一个可行的方法。

Shooting Method

这是在Zach这边最早提到的确定性最优控制方法，不推荐使用。其主要的流程是构造拉格朗日函数，初始化$u$的轨迹，利用极小值原理（见后），求解得到${\lambda }_N$，随后rollout得到整条轨迹，再不断迭代直至收敛，迭代一般比较缓慢。

LQR

LQR，即linear quadratic regulator，线性二次型调节器（个人理解调节器是用于稳定至平衡点的控制器，可以无限长时域但有界cost-to-go）。一个线性系统，有二次cost function，无不等式约束，便是一个LQR标准的系统，可以写成只带线性约束的QP问题，并得到解析解。

也可以通过Riccati Recursion得到反馈控制律，与系统初始状态无关，只与系统特性有关，这样就白送了一个控制器，可以提前算好，塞进单片机。这一反馈控制律的推导，Zach是对于一个有限时域离散系统来讲的，通过观察KKT求解矩阵，逐步递推得到，和网上主流的推导方式类似，而Russ的推导是针对无限时域连续系统，推导过程中有一步说是easy to verify，是我没想明白，就放弃了。

对于这个算法，有人指出既然是反馈控制，就是个等价PD控制（用到了q、v作为反馈），调Q、R矩阵还不如调PID，对此本人认为在低维度的时候可能差异不大，但是在高维、系统动力学比较复杂的情况下，LQR比PID清晰许多，PID面对MIMO的情况调参容易头大，而LQR很可能是一套Q、R走天下。

TVLQR

个人理解，TVLQR（Time-varying LQR）就是用来跟TO产生的轨迹的，作为一个控制器与MPC并列，因为其具有反馈的效果，相比于开环控制会好很多，而其可以直接得到解析的控制律，又不需要像MPC那样高频滚动优化。（没用过，纯个人开环猜测，留坑）

根据Russ，还可以进一步通过finite-time reachability analysis 来分析跟轨迹的能控性、可行性。

MPC

个人理解，MPC一般就是带不等式约束的有限时域LQR，并辅之滚动时域优化（Receding-horizon/Rolling-horizon），每次优化完轨迹后执行第一步，需要有较高的运算频率，所以会把模型线性化并且不需要太长的视野，再求QP**（不知道这样的线性化是否和DDP、TVLQR一样每步执行一次，或是根据效率而定，留坑**）。和前面的TVLQR一样，MPC需要有参考轨迹，这条参考轨迹一般是通过低频的DDP/iLQR得到的，个人认为如果算力足够，完全可以高频求解轨迹，每次只执行一步。（没用过，纯个人开环猜测，留坑）

引入旋转的优化

Zach特别讲了旋转相关的事项，四元数的导数和优化和仅仅用欧拉角不太一样，主要原因是自身带约束/加法不封闭等（第二个原因个人感觉）。

看这部分的时候走神比较厉害，相当于也没怎么看，就简单抄一下大佬的笔记，相当于是进行了重参数化后，对参数$\varphi$优化，但仍然利用$q$表示，用$q(\varphi )$表示$\varphi \to q$，使系统降维能控。

cost函数$J$对$\varphi$求导，用一次链式法则；系统方程$f$因为要状态转移，需要构造额外的$q\to \varphi$的函数$g$，相当于$q$的逆过程
$$\begin{gathered} \delta q={\left[cos(\frac{\theta }{2})asin(\frac{\theta }{2})\right]}^T\approx {\left[1\frac{1}{2}\varphi \right]}^T \\ {q}^{\prime }=q\ast \delta q=L(q)({\left[10\right]}^T+\frac{1}{2}H\varphi )=q+\frac{1}{2}L(q)H\varphi \\ G(q)=\frac{\partial q}{\partial \varphi }=\frac{1}{2}L(q)H \\ \frac{\partial J}{\partial \varphi }=\frac{\partial J}{\partial q}\frac{\partial q}{\partial \varphi }=\frac{\partial J}{\partial q}G(q) \\ {\nabla }^{2}J(q)=G^T(q)\frac{{\partial }^{2}J}{\partial q^2}+I_3(\frac{\partial J}{\partial q}q) \\ \frac{\partial g}{\partial \varphi }=\frac{\partial g}{\partial f}\frac{\partial f}{\partial q}\frac{\partial q}{\partial \varphi }=\left[G^T(f(q))\frac{\partial f}{\partial q}G(q)\right] \end{gathered}$$

非线性优化的方法

相比于前面提到的控制器/轨迹跟踪器，这一节要讲的轨迹规划则没有参考可言，而系统的非线性让问题求解变得复杂，需要迭代寻优（或者通过前端搜索得到初始路径/轨迹、后端优化得到可行轨迹的方式）。

非线性轨迹优化主要有DDP/iLQR（间接法）、Direct Collocation（直接配点法）

DDP、iLQR方法基本一致（后者忽略了高阶项），是re-invent导致的。两者都存在前向rollout的过程，正因为这个过程，两者的动力学一直是可行的，这也意味着初始估计不能给动力学不可行的轨迹。而后者可以有粗糙的估计值，但是收敛前动力学是不可行的。前两者的速度较快，问题维度小，但是可能存在误差累计，此外前两者会免费得到一个TVLQR，不需要额外的跟踪器。

其中一般会同时优化$x$和$u$，这被Russ称为Direct Transcription，否则为Direct Shooting，只优化$u$，然后通过rollout得到$x$，显然后面的方法尽管减少了优化变量的维度，但是会导致精度的累积误差，同时还不能使用参考轨迹（搜索得到的轨迹、动捕得到的关节量等）

推导可以参考的地方很多，比如前面optimal control的笔记，或者这篇笔记

除了前面提到的方法，还有一类课上没讲但是一些地方见到很多的方法：Planning as Inference，可以记一下留个坑

微分平坦

微分平坦（Differential Flatness）是用系统少量的状态及其有限阶导数来表示全部的状态量和输入变量，对空间进行降维，方便优化问题求解。比如无人机的状态向量$X={\left[x,y,z,\varphi ,\theta ,\psi ,\dot{x},\dot{y},\dot{z},p,q,r\right]}^T$，可以选择$\sigma =[x,y,z,\psi {]}^T$作为平坦输出。

优化问题数值求解

问题表述完毕后，需要借助求解器求解。对于不同类型问题求解的trick，比如非线性优化里的一些warm up的手段，本人没有相关的研究，但猜测还是挺吃经验的，甚至会是一篇论文的重点。下面简单记录一下优化问题求解的方法。

无约束优化

在无约束的情况下，求极值就是对状态的导数求零点，求零点一般采用牛顿法（Newton’s Method，不断局部线性化迭代的求根方法）及其变体（阻尼牛顿法（Damped Newton’s Method）、拟牛顿法（Quasi-Newton Methods）、加上线搜索的牛顿法），牛顿法的简单推导如下，
$$f(x+\Delta x)\approx f(x)+\frac{\partial f}{\partial x}\Delta x=0⟹\Delta x=-(\frac{\partial f}{\partial x}{)}^{-1}f(x).$$
求极值便是把上式的$f$换成了$\nabla f$，二阶导带来了黑塞矩阵（Hessian Matrix ），相应地阻尼牛顿法通过正则化，保证黑塞矩阵正定、迭代收敛于极小值，而拟牛顿法针对黑塞矩阵逆矩阵不好求的问题，对其作近似。最简单的利用牛顿法的表达式如下，
$$\Delta x=-({\nabla }^{2}f{)}^{-1}\nabla f.$$
至此，很难不联想到learning领域常见的梯度下降法，其计算一般如下所示，
$$\Delta x=-\lambda \nabla f.$$

等式约束

对于如下优化问题，
$$\begin{gathered} \underset{x}{\min }f(x), \\ s.tc(x)=0, \end{gathered}$$
$f(x)$在无约束/自由的方向的梯度为0，不自由的方向则是$c(x)$的梯度方向，因为函数受限于$c(x)=0$的值是不能发生变化的，因此就可以得到KKT条件（带约束优化问题的一阶必要条件）。构造拉格朗日函数$L(x,\lambda )=f(x)+{\lambda }^{T}c(x)$，对两个变量求导得到如下方程：
$$\begin{gathered} c(x)=0, \\ \nabla f+(\frac{\partial c}{\partial x}{)}^T\lambda =0, \end{gathered}$$
利用牛顿法，得到进一步的迭代方程，
$$\begin{gathered} \frac{{\partial }^{2}L}{\partial x^2}\Delta x+(\frac{\partial c}{\partial x}{)}^T\Delta \lambda =-{\nabla }_{x}L(x,\lambda ), \\ \frac{\partial c}{\partial x}\Delta x=-c(x) \end{gathered}$$

也可以用高斯牛顿法，把$\frac{{\partial }^{2}L}{\partial x^2}$换成${\nabla }^{2}f(x)$，迭代起来更快

不等式约束

对于如下优化问题，
$$\begin{gathered} \underset{x}{\min }f(x), \\ s.tc(x)\ge 0, \end{gathered}$$
可以写出不等式的KKT条件:

Stationarity Condition 稳定性条件
$$\nabla f-(\frac{\partial c}{\partial x}{)}^T\lambda =0$$
Primal Feasibility Condition 原始可行性条件
$$c(x)\ge 0$$
Dual Feasibility Condition 对偶可行性条件
$$\lambda \ge 0$$
Complementary Slackness Condition 互补松弛条件
$${\lambda }^{T}c(x)=0$$
其中第四个条件就是说当在$c(x)>0$的范围内，梯度失去了约束，$\lambda =0$，

进一步的求解，有罚函数法、内点法、有效集法、增广拉格朗日法等，内容太多就不写了。

庞特里亚金极小值定理

庞特里亚金极小值定理（Pontryagin’s Minimum Principle, PMP），可以看作确定性最优控制问题解的一阶必要条件，能利用KKT条件推导得到。

强化学习与最优控制

在Russ看来，RL就是ML语境下的最优控制，Zach也有类似的观点。

本人也简单写一下自己的看法，在用到RL的任务中，没有明确的cost-to-go/value function，在model-free和部分model-based的情况下，也没有agent和环境的动力学方程，即使明确了，也会因为系统的非凸性，难以优化到较好的local-minima。

那么RL，不管是大多基于trail-and-error的online方法，还是大多融合了先验知识的offline方法，不管是基于policy gradient、有时候估计value function用于计算优势的PPO等方法，还是估计action-value function的value-based的SAC、DDPG等方法，不管是拟合网络与采样策略有关的on policy方法还是估计Q*直接对应到最优策略的off policy的方法，都是在尝试显式或隐式估计不同策略下环境的cost-to-go、agent内部以及与环境交互的动力学方程，得到在此基础上的最优策略（遵循HJB equation、沿着cost-to-go梯度走的最优策略），不断迭代得到实际的最优。

周期性+混合系统/足式

orbital stability

足式的运动，是一个周期性的过程，需要对一个震荡系统分析轨道稳定性（orbital stability）
$$\underset{\tau }{\min }||x(t)-x^{\ast }(\tau )||\to 0$$
为此，Russ引出极限环（Limit Cycle）并用庞加莱映射（Poincaré Map）分析Van der Pol Oscillator（一个稳定震荡的系统）的稳定性

关于找到limit cycle，就是在优化的时候增加首尾状态相等的约束

足式建模

如何去建模接触，对于仿真器，Russ提到可以用spring-damp model，允许陷进地里，有一个stiff的弹簧提供向上的力，Zach提到contact implicit method，把条件写成约束，把前向欧拉积分的过程通过KKT公式等价于优化的过程，约束可以是和地面接触力和脚的高度相乘必定是0这种。

但是对于TO来说，可能很难捕捉到接触的瞬间，需要hybrid modeling ，也可以说是event-based method，在我看来，就是设置好转换条件的函数，把切换模型的时刻作为约束，一个hybrid traj opt类似于下图公式（来自课程17章），而spring-damp model和contact implicit method，都是把这样的条件隐式表达，剔除离散的步骤。

models for walking

对于行走最简单的模型就是rimless wheel，只有轮子中心有质量 ，假设每次只有一条腿扎进斜坡里，只考虑建模接触的瞬间，不考虑建模摆腿，整个过程可以看作stance phase（相当于pendulum）+discrete impact event at “foot strike”，每一次impact会失去朝着地面的动量，但是角动量仍然保留，能量损失按下面的公式($$\alpha =0$$即可看作实心的wheel)：
$${\dot{\theta }}^{+}=cos(2\alpha ){\dot{\theta }}^{-}$$
Compass Gait更像一点，建模摆腿，三个质点；The Kneed Walker又增加了膝盖关节，五个质点。但前面两个系统的ROA很小，非常不容易稳定。

models for running

最常见的就是Spring-Loaded Inverted Pendulum (SLIP)模型，一个弹簧的倒立摆。（重点出现于Marc Raibert在1986写的《Legged Robot that Balance》一书）因为弹簧，所以触地瞬间能量不会损失，因为弹簧没有质量，摆腿时间忽略不计，控制量就是摆腿角度。个人认为这个就是给四足机器人的模型，双足很难做到摆腿时间忽略不计，而且要保持平衡比较麻烦。

对SLIP的控制

slip系统的控制可以用apex-to-apex的方式（apex为顶点，在这边表示最高点）表示，以动作序列为参数，利用Poincaré analysis：
$$x_p[n+1]=P(x_p[n],u[n])$$
一个特定的角度、弹性系数、前进速度对应了一个特定的mapping，通过数值计算得到曲线（如下图），随后找到平衡点

可以将$P$展开(但是我没有懂$P$怎么被表达然后被展开)，用LQR
$$x_p[n+1]=A{x}_p[n]+Bu[n]$$
Russ还讲了deadbeat control（当LQR control cost为0？在一个hop里准确到达指定平衡点？）以及触地角度和下次一次hop的高度的一一对应（因为遵循能量守恒？），以及根据deadbeat control在不平整开环执行动作，触地时的角度正好对应于当前deadbeat的角度（刚好在平衡点吗？这节有点云里雾里）

落足点还和前进速度的控制有关，如图

ZMP

ZMP即Zero Moment Point，零力矩点，一般用于规划双足机器人的轨迹。ZMP是地面上的一点，重力和惯性力对这一点的力矩，其水平分量为零，即整个系统对于这个点的前向、侧向的倾覆力矩为零。（重点出现于kajita的《Introduction to humanoid robotics》）

个人理解，当ZMP在支撑多边形内，机器人系统动态稳定，而静态稳定时（没有速度、加速度），CoG（Center of Gravity）在支撑多边形内，CoG的条件比ZMP更为苛刻，步态也更笨拙。

ZMP在机器人动态平衡时和 CoP（Center of Pressure）是同一个点，但不平衡时（何时为不平衡？个人理解就是要倾倒的时候）不相等，很多情况下两者其实也混为一谈。ZMP点由于计算比较容易，常用于行走步态的生成，而CoP则由于容易直接测量，更适合用于对机器人行走进行控制。实际使用过程中，通过控制策略调整ZMP和CoP的位置,使二者重合,实现机器人的动态稳定行走。

当ZMP在支撑多边形外，则更名为FZMP（fictitious ZMP），可以理解为ZMP作用在多边形的边缘，足底的力矩失效了，这时，ZMP无意义，机器人要沿着边缘倾倒。个人理解，实际使用过程中，通过看CoP到多边形边缘的距离来判断ZMP离开支撑多边形的风险。

ZMP作为机器人稳定行走的一个充分条件，使用范围有一定的限制：当足底打滑、地面不平、或者机器人上身与外界环境接触时，ZMP就不能应用。

可以参考1、2

简单的移动规划

对于ZMP相关的步态规划，简单的方式一般是先规划落足点（个人思路是先用用深度信息，利用Two-pass、RANSAC这种方法提取连通域，分割可行平面，然后提取凸包，如下图，最后启发式或者评分排序获得落足点），然后规划CoM（可以先规划CoP，然后可以由CoP的定义以及牛顿欧拉方程得到和CoM的关系，具体见下式粗略推导），最后计算关节。
$$\begin{gathered} m\ddot{x}=\sum f_{W_x}^{B_i}=f_{W_x}^{CoP}, \\ m\ddot{z}=\sum f_{W_z}^{B_i}-mg=f_{W_z}^{CoP}-mg, \\ I\ddot{\theta }=\sum \left[{}^B{p}_W^{B_i}\times f_W^{B_i}\right]=\left[{}^B{p}_W^{CoP}\times f_W^{CoP}\right]+{\tau }_W^{CoP}=\left[{}^B{p}_W^{CoP}\times f_W^{CoP}\right] \\ 消去f_W^{CoP}\Rightarrow (m\ddot{z}+mg)(x_{CoP}-x)=(z_{CoP}-z)m\ddot{x}-I\ddot{\theta } \end{gathered}$$
其中比如${}^B{p}_W^{B_i}$就表示总质心指向$B_i$部分质心的向量在世界坐标系下的坐标，进一步简化，令$\ddot{z}=\ddot{\theta }=0$可以得到ZMP equation：
$$\ddot{x}=g\frac{x-x_{cop}}{z-z_{cop}}$$
如果$z$保持恒定，那么就剩下一个简单的仿射关系。

扯远了，上面提到的规划有一些弊端，比如落足点规划、质心规划一般比较保守，偏向于运动学，不考虑动力学以及关节位置、力约束等。个人认为，如果是利用优化的手段得到的轨迹（之前看到过TAMOLS，我记得就是这样干的，标题《Terrain-aware motion optimization for legged systems》），则能解决上面的弊端，而且规划频率应该可以很高

Atlas的做法是offline的Trajectory Optimization和online的MPC，解算的时候同时考虑Momentum Dynamics（想象自己是个蛋）和kinematics

combinatorial optimization

落足点带来了一系列选择的问题，比如左脚要落到A区域还是B区域，这涉及到组合优化（combinatorial optimization）、混合整数规划（Mixed Integer Programming, MIP）

鸽了鸽了

MPC+WBC

在四足机器人控制领域最为常见的便是MPC+WBC的范式，一般是MPC计算出足端力的基础上，又用WBC算法计算出关节力矩（动力学）以及速度、位置（运动学）。这一内容Russ并没有在课堂上具体讲，但是值得一看。对于这一组合，自然有些疑问，比如：既然MPC是控制器，为什么还需要WBC呢，WBC优势在哪里？

答案自然在奠基性文章里得到，看了Sangbae Kim教授MIT Biomimetic Robotics Laboratory（Cheetah组）的文章（声称MPC+WBC相比于纯MPC把速度从2.45m/s提到了3.7m/s），ETH的RSL组好像没注意到论文，记忆里只有大名鼎鼎OCS2库。

先贴一张该系统的framework，正如作者说的WBC频率非常高，而MPC频率比较低，所以引入WBC，反过来讲WBC只考虑一个time step，所以MPC的长时域又弥补了这一缺点（还提到其他工作有MPC范式的WBC，200Hz，但是动态性表现不太好）

接下来对这两部分具体展开，具体可看文章

MPC

文章令MPC追踪给定指令的速度、角速度。把自己想成一个蛋（单刚体动力学），只考虑简单的整体外部的接触力（带摩擦锥约束）与整体位姿、速度、角速度之间的动力学关系，再进一步对模型化简，使其线性化，随后解一个Convex MPC的问题。

然后用Gait scheduler根据给定步态类型算好支撑相、摆动相信息，Step planner预先算好foothold（用启发式方法：肩膀位置项+Raibert heuristic项+转动项）

WBC

Whole-Body Control(WBC)是利用了机器人所有自由度进行的控制，与同时控制多个任务（可能具有不同优先级）密切相关。本质上就是解了一个QP。

考虑全身动力学，状态量里把在质心虚拟出六自由度关节，把浮动基系统当做固定基考虑。

考虑优先级，使用null space projection（解释可以参考这边，或者RSL的机器人学讲义44页，当然也可以直接我接下来写的），简单来讲考虑$Ax=0$，满足这一方程的解$x$（正交），就属于$A$矩阵的零空间。在机器人运动学里，考虑冗余的机械臂常见的公式：
$$\begin{gathered} w_1=J_1(q)\dot{q} \\ \dot{q}=J_1^{+}(q)w_1+N_1\dot{q_0} \end{gathered}$$
其中$q$是关节角，$w$通常是末端位姿的导数$\dot{x}$，这边表示成任务，$J$是雅克比矩阵，$J^{+}$是雅可比矩阵的伪逆，$N$便是$J$的零空间投影矩阵，满足$JN=0$，那么在这基础上取的任意$\dot{q_0}$，改变了$\dot{q}$但影响不到$\dot{x}$。这时候如果考虑第二个任务：
$$w_2=J_2^{+}\dot{q}=J_2^{+}(J_1^{+}(q)w_1+N_1\dot{q_0})$$
这个时候就可以进一步解出$\dot{q_0}$（这里还可以继续划分零空间，下面忽略了），并得到通解或迭代公式:
$$\begin{gathered} \dot{q_0}=(J_2{N}_1{)}^{+}(w_2-J_2{J}_1^{+}{w}_1) \\ \dot{q}=\sum _{i=1}^n{\overline{N}}_i\dot{q_i},\dot{q_i}=(J_i{\overline{N}}_i{)}^{+}(w_i-J_i\sum _{k=1}^{i-1}{\overline{N}}_k{\dot{q}}_k) \\ or:{\dot{q}}_i={\dot{q}}_{i-1}+(J_i{N}_{i-1}{)}^{+}(w_i-J_i{\dot{q}}_{i-1}) \end{gathered}$$
其中${\overline{N}}_i$是${\overline{J}}_i={\left[J_1^T\dots J_{i-1}^T\right]}^T$的零空间投影矩阵。

低优先级的任务放置在高优先级任务的零空间下进行计算，就不会影响到高优先级的结果。

基于这样的思想，利用上一层给定的条件，根据多个任务（文中是foot position、body position、 body orientation）计算雅克比矩阵，按优先级迭代求解增量，最后即可得到期望的关节角、角速度、角加速度（用PD feedback ，利用角度、角速度计算期望的角加速度）

然后引入松弛变量，在等式约束里松弛接触力和加速度，之后求解一个QP得到最终的加速度，并进一步计算得到各个关节力矩。

可以参考：陈不陈佬的文章，或者这篇

关于Learning-based

这边Russ提到了learning-based方法（即RL），认为随着仿真对环境的建模越来越好，learning的方法会逐渐取代model-based，ETH做狗的一大成功就是设计了各种terrain来训练，方便sim2real。他认为state estimation再model base是overly brittle，而pixel-to-torque是更为鲁棒。

在教材的首页里他还写道：I’m willing to bet that our views of intelligence in 10-20 years will look less like feedforward networks with a training mode and a test mode, and more like a system with dynamics that ebb and flow in a beautiful dance with streams of incoming data and the ever-changing dynamics of the environment. These systems will move more flexibly between perception, forward prediction / sequential decision making, storing and retrieving long-term memories, and taking action. Dynamical systems theory offers us a way to understand and harness the complexity of these systems that we are building.

希望这一天能早点到来。

人形机器人为什么屈膝

这是我自己提的一个问题。

对于古早的机器人如ASIMO来说，我认为屈膝很大一点可以避免逆运动学奇异点丢失自由度，对于现在的机器人来说，我认为屈膝的目的更多是为了方便重心稳定（对于ASIMO也是），也便于调整。调整就好比在ZMP的规划中需要让重心沿着轨迹移动（不屈膝应该不至于不可调整吧，毕竟行走就是优化问题）；让重心（z轴）稳定，可能是ZMP方程的简化假设（平地上质心高度不变）导致的，也可能为了防止impact带来的坏处，比如减少里程计因为impact导致的误差，或是对机械结构的损伤。也有可能屈膝是为了方便近似SLIP模型（更容易模仿一个弹簧）？

但仔细想想屈膝类似半蹲马步，耗能不会小，我不信要求直立的时候z轴不加约束，mpc解出来最优的情况是屈膝的。

有待进一步考证和思考。

其他相关

Hybrid Zero Dynamics（HZD）、divergent component of motion（DCM）、Spatially Quantized Dynamics（SQD）

随机最优控制、鲁棒控制

这两部分我基本上没怎么看，感觉自己不怎么会接触到，因为噪声这个东西在Learning中太常见了，添加噪声太方便了，学出来的网络一般都有鲁棒性。

随机最优控制在状态转移或状态观测时引入噪声，把原本的确定量建模为随机分布，在优化目标上加上期望。对于一个线性系统，根据分离原理（Separation Principle），随机最优控制就是带状态估计的最优控制。KF本质上就是多个高斯分布之间相乘得到的新的高斯分布，从而起到按置信度融合观测数据的效果，个人感觉这一个滤波的操作，可以放到这边处理，也可以直接放进感知部分。

不知道分离原理在非线性系统是否能进一步使用？毕竟EKF就是线性化之后的结果。

鲁棒控制主要是假设系统参数本身有不确定性，去求解一个minmax问题，即要在最差的情况下也能使系统取得一个最好的效果。

输出反馈

Russ讲Output Feedback这部分内容主要是提了下经典控制理论里反馈的思想，比如$$\dot{x}=Ax+Bu,y=Cx,u=-Ky$$。然后提到pixel2torque也是反馈思想的体现（还挺牵强的，不会有人拿控制理论去做pixel2torque的控制器吧）

模仿学习

与我之前前面提到的RL一样，模仿学习也是最近几年机器人领域热门的话题，而且ChatGPT的出现后，LMM、LLM飞速发展，具身智能的这一话题也逐渐热门，这两年RT系列、Diffusion Policy、ALOHA、UMI等工作的出现，也很难不让人去觉得突破性的foundation model即将到来，imitation learning式的方法也在不断证明自身的优越性（数据效率高、不需要sim-2-real、训练方式简洁暴力）。我想正是因为这个，Russ才在这门优化主导的课上讲IL（理论上IL针对的确实也是underactuated的情况，没有偏题hh）

我记得几年前，人们对IL的评价是只学到了成功的案例、没有掌握机理、在遇到out-of-distribution的情况下会暴走、不会超越人类；而RL的种种算法，就是通过trial-and-error，去把握实际的distribution，显得更有泛化能力、并且理论上能做得更好。如今看起来（我自己没有去试过）IL好像真有那么些泛化性在，而RL也没那么神（当然openai five这种游戏类的以及各种Locomotion的工作还是非常惊艳的）。没有trick、pretrain的情况下，RL有时候就像在暴力搜索，训练的曲线有时也并不稳定，很多时候需要非常大的并行数量才能走向成功。offline-RL有着美妙的愿景，有着IL的数据质量、RL的学习范式，但是目前好像没有爆火的工作，不知道未来会如何。现在最常见的还是pre-train-fine-tuning的范式吧。

Manipulation

尽管manipulation在Russ的另一门课里，但我认为既然IL提到了，就也得提一下manipulation，时间有限，大概看了看课程大纲，主要讲的是一些pick-and-place、grasp的kinematics、dynamics，点云配准、法线提取，然后基于learning的detection、segment、pose estimation、grasp selection，之后是经典的RL介绍，最后是Tactile Sensing这种新型感知方式 。看下来感觉就是说，manipulation这边，不管是现在end2end的IL，还是分模块的方法，看起来都可以learning主导，不过泛化到open world的能力，感觉得进一步观望，毕竟相比于locomotion，语义信息在这边显得更加重要。不过本身我对manipulation的研究还不是很熟，不知道传统方法现在还是否有应用价值（如SLAM之于navigation，又或者TO之于无人机轨迹规划）还是已经处于尴尬地位了呢？后续有机会再单开一个manipulation的笔记吧。

写不动了，先写到这吧
Russ的另一门课里，但我认为既然IL提到了，就也得提一下manipulation，时间有限，大概看了看课程大纲，主要讲的是一些pick-and-place、grasp的kinematics、dynamics，点云配准、法线提取，然后基于learning的detection、segment、pose estimation、grasp selection，之后是经典的RL介绍，最后是Tactile Sensing这种新型感知方式 。看下来感觉就是说，manipulation这边，不管是现在end2end的IL，还是分模块的方法，看起来都可以learning主导，不过泛化到open world的能力，感觉得进一步观望，毕竟相比于locomotion，语义信息在这边显得更加重要。不过本身我对manipulation的研究还不是很熟，不知道传统方法现在还是否有应用价值（如SLAM之于navigation，又或者TO之于无人机轨迹规划）还是已经处于尴尬地位了呢？后续有机会再单开一个manipulation的笔记吧。

写不动了，先写到这吧