这个是本人在大三期间做的项目 ---- 基于MIT的Cheetah方案设计的十二自由度并联四足机器人，这个项目获得过两个国家级奖项和一个省级奖项。接下来我会将这个机器人的控制部分所有代码进行开源，并配有相关的教程博客，希望能够帮助到在学习相关领域知识或者进行项目开发的同学。

学习建议

机器人学导论是做机械臂，足式机器人绕不开的理论知识，本项目也是涉及到了机器人学导论前七章的大部分内容。
书籍： 《机器人学导论第四版》（中英均可，建议英文）
教学视频： B站林沛群系列教程
运动学教程
动力学教程
习题： 有余力者可以做课后习题，不过题量有点多，可以挑着做。林沛群的教程本身也是带习题的，不过国内的视频链接里没有找到，Coursera上面有，有条件的同学可以选择科学上网一下。

学习内容

以下的内容围绕本项目进行展开，所以不会系统地讲到每个知识点，系统学习请参考上面的学习建议。

1. 坐标系变换

坐标系转换在足式机器人中主要有两个方面的体现，一个是关节与关节之间的坐标系变换，一个是体坐标系（Body
Frame）与世界坐标系（World Frame）的变换。

关节坐标系变换

因为四足机器人的腿部是由数个刚体构成，刚体与刚体之间采用关节电机进行连接，所以彼此之间存在相互制约关系。为了更好地描述各个关节的位置，我们需要对每个连接处的关节构建坐标系。由于刚体长度一定，各个电机的角度也可以进行读取，所以各个变换矩阵T可以很轻易地计算得到，这里就不进行展开了。

B&W坐标系变换

由于对四足机器人的控制不止要控制它腿部位置，还要控制其运动，即世界坐标系下的坐标位置以及航向。所以需要用到体坐标系和世界坐标系的变换。

2. 正逆运动学

对于四足机器人的控制，正逆运动学主要用于表示关节角度和足端位置的关系。正运动学将关节电机的角度值转换为足端位置，用于后续的运动轨迹规划。逆运动学将足端位置指令转换为各个电机的角度指令，用于对各个电机进行位置控制。

正运动学

逆运动学

3. 轨迹规划

在四足机器人的控制中，主要有两种轨迹规划，一种是足端运动轨迹的规划，主要涉及运动机器人学导论相关知识；一种是机器人质心在世界坐标系下的运动轨迹规划，主要是结合其他传感器数据以及导航相关知识进行规划。这里只讨论第一种情况，即足端轨迹规划。

机器人学导论中主要谈及两种轨迹规划的方法：n次样条法和LSPB法（抛物线-直线-抛物线法），但由于足端的轨迹要求是平滑和运算简单，这两者都不太符合，所以我们采用的是贝赛尔曲线进行轨迹规划。

基于贝赛尔曲线，我们可以只通过很少的控制点，去生成复杂的平滑曲线。相比正弦等曲线，贝塞尔曲线的一阶导数和二阶导数计算量减少了许多，同时曲线也更加平滑，因此我们采用二阶贝塞尔曲线来规划足端运动轨迹。



得到了轨迹方程后，通过不断改变t值，可以得到n个离散点，再结合逆运动学知识，转换为各个关节角度，即可对足端位置进行控制。

4. 动力学

动力学是目前产品级四足机器人的灵魂，也是最难的一部分。基于运动学，可以精准控制四足机器人的足端位置，达到在平整路面运动的效果。但是一旦路面情况复杂起来，单运动学无法解决一切问题，需要结合动力学的知识，在四足机器人足端均接触地面的同时，整体获得足够的支撑力及驱动力，从而达到在复杂路况下运动的效果。

由于测试条件所限，该项目仅在仿真环境中实现了力位混控，在实体控制上仍用的是纯位控。

雅可比矩阵

在机器人学中，雅可比矩阵指的则是机器人末端(end-effector)的位姿（位置与姿态）与机器人各个关节位置值的偏微分关系。用于表征足端线速度与电机角速度之间的关系。

一般方法： 雅可比矩阵的计算，一般都是由末端位姿对驱动关节角度的求导，但对于并联结构末端位姿仍是闭链中非驱动关节角度的函数，所以会求导会得到关节角度之间的偏导项。偏导项可以通过对闭链约束的求导得到，代入即可得到结果。
取巧方法： 借助matlab工具对正运动学公式结果进行微分，即可求解出较为精准的雅可比矩阵J。

得到雅可比矩阵后，通过以下的换算公式即可得到足端线速度$v$与电机角速度$q$之间的关系。
$$\begin{gathered} v=J\ast q \\ q=J^{-1}\ast v \end{gathered}$$
静态下的力矩平衡：$F$是作用在末端执行器上的笛卡尔力-力矩矢量，$\tau$是关节力矩矢量
$$\tau =J^T\ast F$$

动力学计算

动力学主要分为两个问题：

1. 已知一个轨迹点$\Theta ,\dot{\Theta },\ddot{\Theta }$，求取对应的关节驱动力矩$\tau$，主要用于机器人控制。
2. 已知一组力矩$\tau$，求取机器人运动结果$\Theta ,\dot{\Theta },\ddot{\Theta }$，主要用于机器人仿真。

动力学求解方法：

1. 牛顿-欧拉法：外推法计算速度和加速度，内推法计算力和力矩
   
   
2. 拉格朗日方程：基于能量的求解方法
   $k(\Theta ,\dot{\Theta })$为动能之和，$u(\Theta )$为势能之和。
   
   对于本项目，主要是在MPC算法中的模型建立部分运动到了动力学的相关知识（引用了MIT开源的Cheetah项目中动力学模型），后续的文章会详细展开说明，这里就不再赘述了。