一种逆运动学分析的几何方法

1. 问题描述

如左图所示，是一个双足机器人两条腿的自由度和坐标系示意图。图中Y轴的正方向指向机器人的前方，X轴的正方向指向机器人的右方， Z轴则指上方，构成右手坐标系。

机器人的左右两条腿各有5个自由度，它们是:

髋关节Roll自由度(hip roll)，记为\(J_0^l, J_0^r\);
髋关节Pitch自由度(hip pitch)，记为\(J_1^l, J_1^r\);
膝关节Pitch自由度(knee pitch)，记为\(J_2^l, J_2^r\);
踝关节Pitch自由度(ankle pitch)，记为\(J_3^l, J_3^r\);
踝关节Roll自由度(ankle roll)，记为\(J_4^l, J_4^r\);

其中左上角标\(l,r\)分别表示左腿和右腿，后文中在没有特别说明的情况下，都是使用这种方式区分左右两条腿。这些自由度都是转动关节，我们以逆时针转动为正方向。其转轴方向均平行于Y轴和X轴并指向正方向，满足右手坐标，即右手抓握转轴，大拇指指向转轴方向，四指弯曲的方向为转动的正方向。

左右两条腿由一个连杆连接，称为腰(waist)，其长度称为臀宽记为\(w_{hip}\)。每条腿均有5个连杆，称为臀部(hip)、大腿(thigh)、小腿(shank)、脚踝(ankle)和脚(foot)。长度用符号\(l_{hip}、l_{thigh}、l_{shank}、l_{ankle}、l_{foot}\)表示。

该机器人的逆运动学分析可以描述为：已知腰部坐标\(\boldsymbol{P_w}\)姿态\(\boldsymbol{R_w}\)，左右两脚末端坐标\(\boldsymbol{P_l}, \boldsymbol{P_r}\)和姿态\(\boldsymbol{P_l}, \boldsymbol{R_r}\)求解各个关节的转角。其中，\(\boldsymbol{P}=[x, y, z]^T\)为世界笛卡尔坐标系下的坐标。姿态用欧拉角表示\(\boldsymbol{R}=[roll, pitch, yaw]^T\)，刚体先绕\(z\)轴转动\(yaw\)角度，再绕\(y\)轴转动\(pitch\)角度，最后绕\(x\)轴转动\(roll\)角度。

我们在计算过程中，都是先把欧拉角转换到旋转矩阵的。由于两者描述的是同一个事物，我们可以只用一个符号\(R\)表示。

2. 问题拆解

在问题描述中，我们已知了腰部和双脚末端在世界坐标系中的绝对位置和姿态。而求取各个关节的转角，只需要知道双脚末端相对于腰部的相对位置和姿态就可以了。也就是说，我们可以把机器人整个看作是一个刚体，腰部的位置和姿态(以后简称位姿)描述了机器人在空间中的位姿，而机器人的各种形态或者说是末端相对于腰部的位姿，完全由各个关节转角决定。整个解算过程可以分为两个部分：

求取两脚末端相对于腰部的位姿；
根据相对位姿关系求取各个关节转角。

3. 相对位姿求解

对于相对位姿的求解，首先需要根据腰部的位置\(\boldsymbol{P_w}\)和姿态\(\boldsymbol{R_w}\)，写出齐次矩阵的形式\(\boldsymbol{H_w} = \begin{bmatrix} \boldsymbol{R_w} & \boldsymbol{P_w} \\ \boldsymbol{0} & 1 \end{bmatrix}\)。此外，还有左右两脚末端的齐次矩阵\(\boldsymbol{H_{le}} = \begin{bmatrix} \boldsymbol{R_{le}} & \boldsymbol{P_{le}} \\ \boldsymbol{0} & 1 \end{bmatrix}\)， \(\boldsymbol{H_{re}} = \begin{bmatrix} \boldsymbol{R_{re}} & \boldsymbol{P_{re}} \\ \boldsymbol{0} & 1 \end{bmatrix}\)。

我们将机器人整体看作是一个刚体，整体相对于世界坐标系做一次\(\boldsymbol{H_w}\)的逆变换，就可以得到两脚末端相对于腰部的位姿关系： \(\boldsymbol{H_w}^{-1}\boldsymbol{H_{le}}, \boldsymbol{H_w}^{-1}\boldsymbol{H_{re}}\)。其中， \(\boldsymbol{H_w}^{-1} = \begin{bmatrix} \boldsymbol{R_{w}}^T & -\boldsymbol{R_{w}}^T\boldsymbol{P_{w}} \\ \boldsymbol{0} & 1 \end{bmatrix}\)。

从齐次矩阵中，我们可以得到双足末端相对于腰部的坐标\(\boldsymbol{P_{le,w}}=[x_{le,w}, y_{le,w}, z_{le,w}]^T\), \(\boldsymbol{P_{re,w}}=[x_{re,w}, y_{re,w}, z_{re,w}]^T\)。由于\(J_0^l, J_0^r\)直接与腰部固连在一起，只需要做一个简单的平移变换，就可以求取两脚末端相对于(\(J_0^l, J_0^r\))的位置关系 \(\boldsymbol{P_{le,J_0^l}}=[x_{le,w} + \frac{w_{hip}}{2}, y_{le,w}, z_{le,w}]^T\), \(\boldsymbol{P_{re,J_0^r}}=[x_{re,w} - \frac{w_{hip}}{2}, y_{re,w}, z_{re,w}]^T\)。

4. 关节转角求解

根据问题描述中机器人的自由度分布，可以发现不存在绕着\(z\)轴旋转的自由度，所以两脚相对于\(J_0^l, J_0^r\)的姿态中\(yaw=0\)。很多时候，为了能让机器人转向，人们会在臀部添加一个yaw自由度。对于这种情况，不需要进行复杂的计算，其相对姿态的\(yaw\)取值就是该自由度的转角值。所以总可以归结为5个自由度确定脚末端位姿的问题。

在5自由度关节转角的问题时，我们可以将之拆分为三个部分：

两脚踝pitch自由度\(J_3^l, J_3^r\)相对于\(J_0^l, J_0^r\)的位置坐标\(\boldsymbol{P_{J_3^l,J_0^l}}, \boldsymbol{P_{J_3^r,J_0^r}}\)。
根据\(\boldsymbol{P_{J_3^r,J_0^r}}\)求解髋关节和膝关节自由度转角。
根据脚末端姿态计算脚踝关节自由度转角。

求解第2个子问题的时候，实际上有一个假设:两脚末端的姿态与腰部的姿态一致。这样就可以先解决末端相对\(J_0^l, J_0^r\)的位置问题。第3个子问题，则解决了末端的姿态问题。由于踝关节的pitch和roll自由度的转轴不在一个平面上，所以仅通过调整这两个关节不仅影响了末端的姿态，还影响了其在空间中的位置。实现位置与姿态的解耦计算，提出了第1个子问题，先求取踝关节的相对位置以消除两个自由度转动过程中对末端位置的影响。

4.1 求取踝关节相对位置

假设脚末端相对于\(J_0^l, J_0^r\)的姿态为\(\boldsymbol{R_e}=[roll_e, pitch_e, 0]^T\)，我们需要\(\boldsymbol{J_4,J_5}\)自由度分别转动\(pitch_e\)和\(roll_e\)角度。

\(\boldsymbol{J_4}\)转动时，将会造成末端相对平移\(P_{J_4}=[0, (l_{ankle} + l_{foot})\sin{(pitch_e)}, (l_{ankle} + l_{foot})(1-\cos{(pitch_e)})]^T\)，

\(\boldsymbol{J_5}\)转动时，将会造成末端相对平移\(P_{J_5}=[-l_{foot}\cos{(pitch_e)}\sin{(roll_e)}, 0, l_{foot}\cos(pitch_e)(1-\cos{(roll_e)})]^T\)。

所以\(\boldsymbol{P_{J_3,J_0}} = \boldsymbol{P_{le,J_0}} - P_{J_4} - P_{J_5}\)，

4.2 求解髋关节与膝关节自由度转角

暂放两张图，后续解析