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

学习建议

机器人学导论是做机械臂，足式机器人绕不开的理论知识，本项目也是涉及到了机器人学导论前七章的大部分内容。
书籍： 《机器人学导论第四版》（中英均可，建议英文）
教学视频： B站林沛群系列教程

B&W坐标系变换

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

2. 正逆运动学

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

正运动学

逆运动学

3. 轨迹规划

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

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

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



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

4. 动力学

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

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

雅可比矩阵

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

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

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

动力学计算

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

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

动力学求解方法：

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