四足机器人运动学(机器人姿态与足端位置，欧拉角和旋转矩阵)控制思路

四足机器人运动学

通过改变足端位置改变机器人位姿

机器人姿态与足端位置

通过改变四条腿的关节角度实现机器人在三个平移和三个旋转方向的小幅运动

如图，给机器人定义两个坐标系：

{b}：机身坐标系，以机身的形心为原点

{s}：世界坐标系，以地面上的X 点为原点，便于计算X点即$P_0$

机器人的四个足端分别为$P_0$ $P_1$ $P_2$ $P_3$
这四个点对于{b}的坐标分别为：${p_b}_0$ ${p_b}_1$ ${p_b}_2$ ${p_b}_3$
这四个点对于{s}的坐标分别为：${p_s}_0$ ${p_s}_1$ ${p_s}_2$ ${p_s}_3$

当机器人姿态变化时，${p_b}_0$ ${p_b}_1$ ${p_b}_2$ ${p_b}_3$ 会发生变化，且希望机器人相对于地面足端不移动：${p_s}_0$ ${p_s}_1$ ${p_s}_2$ ${p_s}_3$ 不变

控制目的：需要通过关节电机的转动实现机器人姿态控制，就是需要求解每个关节转动的角度。

实现思路：在已知${p_s}_0$ ${p_s}_1$ ${p_s}_2$ ${p_s}_3$ 的情况下求${p_b}_0$ ${p_b}_1$ ${p_b}_2$ ${p_b}_3$ ，再通过逆运动学求解各个关节角度

1. 点的坐标变换

   变换点的参考坐标系=>左乘齐次变换矩阵(相关讲解参考主页上一篇)

2. 计算${T_s}_b^{-1}$
   根据齐次变换矩阵的可逆性： 可以先求${T_s}_b$

此处要将旋转矩阵${R_s}_b$ 转换为欧拉角(具有直观的几何意义，便于生成姿态命令)

欧拉角概念:刚体分别绕x,y,z轴旋转的角度

旋转矩阵转换为欧拉角：

假设初始状态下{s}和{b}重合，所以${R_s}_b$ =$I$ ,(此处使用$Y-P-R$ 顺序的欧拉角，即物体先绕{b}的z轴旋转，在绕新坐标系{b’}的y轴旋转，再绕新坐标系{b’‘}的x轴旋转得到最终坐标系{b’‘’}),因为是相对物体坐标系旋转，所以依次右乘旋转矩阵实现旋转矩阵的变换

此处初始状态下，{s}和{b}平行，所以旋转矩阵${R_s}_b$=$I$ ，最终旋转矩阵改用欧拉角表示，${-p_b}_0$(0)+$p_d$ 表示两坐标系原点坐标向量$\overrightarrow{O{O}^{\prime }}$在{s}下平移了$p_d$

再使用
求得${T_s}_b^{-1}$

4. 最后使用单腿逆运动学求解关节角度
   （正/逆运动学可能会在下一篇写吧…//）