继续更新，这两天继续死磕Gazebo仿真。还是拿本人之前的公布的开源项目pigot四足机器人作为仿真对象，研究四足机器人的姿态控制算法仿真。将建模与仿真过程的经验教训和相关资料进行总结（如有兴趣，推荐看一看文末的参考资料）
这一阶段的工作如下：
1、 编写四足机器人的姿态控制算法。主要是运动学求解。
2、仿真模型的轨迹规划改进。目标点之间改为三次样条插值，让路径更加平滑。
3、搭建多机器人联合仿真模型。为了测试平衡控制算法，加入了一个实验台。
四足机器人的姿态控制
我采用如下思路控制机器人的姿态：
· 给定机器人的姿态欧拉角RPY
· 求出机器人姿态欧拉角与各腿位置间的关系。这里十分类似于并联机器人的逆解，求解一个向量方程即可[1]。
· 将各腿位置代入机器人单腿的运动学逆解，即可得到各关节角。参考串联机器人的逆解求解思路[2]。
留坑：我已经推导出了完整的姿态控制算法，包括单腿逆解和整体逆解。限于知乎编辑器对公式的支持不太好，可以直接看代码文件”kinematics_algorithm”中的leg_ikine()函数。我将会在CSDN或者其它地方发布，之后再在此更新。
仿真模型的轨迹规划改进
这一块琢磨得不深。之前是直接在各个目标点之间进行线性插值，现在加入了一个样条插值函数[3]，试验了一下，发现轨迹确实平滑得多。但四足轨迹规划最关键的步态算法还并没有涉及，看到一些步态规划的文章，比如摆线步态，准备看懂之后再改进改进。
Gazebo多机器人联合仿真
这一块是坑区。首先我想要搭建一个姿态仿真的测试平台，按道理来说应该是一个并联机器人。然而众所周知，ROS仿真所广泛使用的urdf格式是不支持闭链结构的。经过一系列探索，有如下两个解决方案：
1、利用Gazebo新版所推荐使用的sdf格式进行建模。
传统的urdf格式存在很多问题，例如不支持闭链结构、不包含世界模型等。而在Gazebo新版本中，正在推广sdf格式，解决了上述问题。但是，国内外所能找到的大多数教程、包括多数官方教程中能找到的参考资料太少。我做了一些尝试，发现利用sdf建立并联机器人(闭链结构)的模型没有问题。但ros_control控制插件只兼容了urdf格式，意味着要想控制sdf格式的机器人，必须自己编写插件。这方面现在还在扫盲过程中，只有以后再探索。以下资料值得参考，看最后一位网友给出的自己的代码文件：
２.论坛网友给出的方法，本人尚未测试是否可行。
于是我只能搭建了一个串联型平台。串联平台的参数很难调，在文末视频中可以看到其刚度比较差，晃得厉害。
四足机器人姿态控制仿真平台
下面又遇到了如何将两个机器人加入同一个模型中的坑。最后的解决方法是采用不同的命名空间，论坛网友的解决方法经测试很好用(在最后一个回答中)：
仿真效果
利用姿态控制算法，可以更加精确地控制机器人的站立动作（例如站立舞蹈）。输入imu采集的姿态信号后，还可以做姿态补偿与校正。控制示例见视频：
ROS四足机器人姿态控制仿真
Github地址
后续计划
1、四足机器人步态算法改进与仿真环境改进
2、抗扰动平衡算法研究与实现
3、头部机械臂的实现
4、SLAM、自主导航等的实现
精选参考资料
[1] 四足机器人姿态控制（通过IMU）
[2] SCalf液压驱动四足机器人的机构设计与运动分析[D]. 山东大学, 2013.
[4] ROS、Gazebo官方问答论坛
