四足機器人姿態控制（通過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倍。

具體效果的數據如下圖，仿真動畫在最前面。