四足机器人姿态控制（通过IMU）

　　    先上效果图，webots下仿真，四足机器人在转动平台上保持机身水平。

    下面是姿态解算方法和控制策略：

如下图所示，给定机器人在世界坐标系的机身位置O’(x’,y’,z’)和航海角（roll、pitch、yaw），以及足部末端在世界坐标系下的位置P₁、P₂、P₃、P₄点的坐标。我们需要求解的是P₁、P₂、P₃、P₄点在机身坐标系Q下的坐标值，进一步可以通过前一节的足端运动学逆解求解电机关节角度。

以解算P₁坐标为例，首先计算机身目标位置变换的RPY航海角变换矩阵，其中的参数roll、pitch、yaw用R、P、Y简化表示。可得到变换矩阵A：

 

 其他腿的逆解过程与此相同，在此不再赘述，将求解出的机身坐标系下的足末端坐标，代入足端逆解公式即公式可求解出电机的控制角度。

调节方法使用PID调节，由于考虑参数选定的简单性，使用纯P调节，通过多次仿真实验，发现K_P = 0.2时调节效果较好。如公式（3-12），其中R_imu、P_imu为IMU（惯性检测单元）测量得到的机身与地面的横滚角与俯仰角差值，每次检测都补偿差值的0.2倍。

具体效果的数据如下图，仿真动画在最前面。