双足机器人的运动学分析

1. 引言

双足机器人的一个主要研究方向就是模拟人类下肢的行走运动。 为了简化，通常忽略上肢运动对下肢运动的影响。 因此参考文献[1]，本文把机器人描述为由躯干、大腿、小腿和脚构成的七连杆机构。

机器人的运动问题主要涉及到以下两个方面的基本问题：
1. 正运动学问题:根据机器人各个杆件的几何参数和关节的运动情况，确定机器人的各部分相对于惯性坐标系的位置和姿态。
2. 逆运动学问题:给定机器人相对于惯性坐标系的位姿，和各个杆件的几何参数，求解各个关节的转角。

为简化求解过程，做出如下假设：
1. 髋关节的三个自由度轴线相交于髋关节中心，踝关节的两个自由度轴线相交于踝关节中心；
2. 行走时，两个髋关节连线与地面平行，躯干保持竖直；
3. 行走时，两个脚始终与地面平行。

2. 机器人的位置与姿态描述

如图1(1)所示，将机器人的结构简化为七连杆结构，包括躯干、大腿、小腿、脚。 其中\(O_{l_0},O_{r_0}\)分别为左右两脚掌的中心， 左右腿的踝关节、膝关节、髋关节中心依次记为\(O_{l_1},O_{l_2},O_{l_3},O_{r_1},O_{r_2},O_{r_3}\)。 脚的高度为\(d_0\)，小腿长度为\(d_1\)，大腿长度为\(d_2\)，两个髋关节之间距离为\(d_3\)，躯干的长度为\(d_4\)。 在两脚中心和各关节中心分别建立局部坐标系，其中\(O_{l_0}\)系和\(O_{r_0}\)系各轴与一惯性坐标系\(O\)始终保持平行。 机器人处于竖直状态时，各局部坐标系坐标轴与\(O\)系坐标轴平行， \(x\)轴垂直直面向外，\(y\)轴水平向右，\(z\)轴竖直向上。

图 1 (1) 机器人的七连杆结构及坐标系分布	图 1 (2) 机器人自由度分布

仿照人类两腿的设置，一般双足机器人的每条腿有六个自由度。 其中踝关节有俯仰和滚转两个自由度，膝关节有一个俯仰自由度，髋关节则有三个自由度。 一般情况下，机器人的各个关节都是由电机驱动的，每个自由度都需要一个电机。 为了分析方便，通常认为踝关节的两个转轴相交于一点，髋关节的三个转轴相交于一点， 我们定义机器人的自由度分布如图1(2)所示。 其中\(θ_{l_0}\),\(θ_{r_0}\),\(θ_{l_1}\),\(θ_{r_1}\)分别为左右腿踝关节的滚转和俯仰自自由度； \(θ_{l_2}\),\(θ_{r_2}\)分别为左右腿膝关节的俯仰自由度； \(θ_{l_3}\),\(θ_{r_3}\),\(θ_{l_4}\),\(θ_{r_4}\),\(θ_{l_5}\),\(θ_{r_5}\)分别为左右腿髋关节的滚转、俯仰和偏航自由度。 约定逆时针方向为正方向。 此外，由于机械上的限制，我们假设各关节的转动范围如下:

3. 正运动学模型

3.1 两腿间的运动学约束

假设\(O_{\mathcal{L}_i}\)在惯性坐标系\(O\)中的位置矢量为 \(\boldsymbol{r_{\mathcal{L}_i}}=[{x_{\mathcal{L}_i}},{y_{\mathcal{L}_i}},{z_{\mathcal{L}_i}}]^T\)， 其中\(\mathcal{L}=l,r\)分别表示左腿或者右腿的设置；\(i=0,1,2\)。

机器人在水平地面上行走时，通常限定躯干保持竖直，因此可以限定:

• 两个髋关节连线与地面平行： $$z_{r_3}-z_{l_3}=0$$
• 两个髋关节之间距离恒定： $$\left\| \boldsymbol{r_{r_3}} - \boldsymbol{r_{l_3}} \right\| = d_3^2 $$ $$(x_{r_3}-x_{l_3})^2+(y_{r_3}-y_{l_3})^2=d_3^2$$
• 躯干保持竖直： $$\theta_{l_1} + \theta_{l_2} + \theta_{l_4}=0$$ $$θ_{r_1}+θ_{r_2}+θ_{r_4}=0$$ $$θ_{l_0}+θ_{l_3}=0$$ $$θ_{r_0}+θ_{r_3}=0$$

3.2 单腿正运动学模型

设左腿各关节转角为\(θ_{l_0}~θ_{l_5}\)，右腿各关节转角为\(θ_{r_0}~θ_{r_5}\)。 矩阵\([∙]_\mathcal{L}\)中的\(c_i\)和\(s_i\)分别表示\(\cos{\theta_{\mathcal{L}_i}}\)和\(\sin{\theta_{\mathcal{L}_i}}\) 则单腿各局部坐标系间的正运动学关系如下：

\(O_{\mathcal{L}_1}\)系相对于\(O_{\mathcal{L}_0}\)系先绕\(x\)轴转动\(θ_{\mathcal{L}_0}\)， 再绕\(y\)轴转动\(θ_{\mathcal{L}_1}\)，则\(O_{\mathcal{L}_1}\)系相对于\(O_{\mathcal{L}_0}\)系的齐次变换矩阵如下： $$H_{{O_{\mathcal{L}_0}}{O_{\mathcal{L}_1}}} = \begin{bmatrix} c_1 & 0 & s_1 & 0 \\ s_0s_1 & c_0 & -s_0c_1 & 0 \\ -c_0s_1 & s_0 & c_0c_1 & d_0 \\ 0 & 0 & 0 & 1 \end{bmatrix}_\mathcal{L}$$

\(O_{\mathcal{L}_2}\)系相对于\(O_{\mathcal{L}_1}\)系绕\(y\)轴转动\(θ_{\mathcal{L}_2}\)有齐次变换矩阵如下: $$H_{{O_{\mathcal{L}_1}}{O_{\mathcal{L}_2}}} = \begin{bmatrix} c_2 & 0 & s_2 & 0 \\ 0 & 1 & 0 & 0 \\ -s_2 & 0 & c_2 & d_1 \\ 0 & 0 & 0 & 1 \end{bmatrix}_\mathcal{L}$$ $$H_{{O_{\mathcal{L}_0}}{O_{\mathcal{L}_2}}} = \begin{bmatrix} c_{1+2} & 0 & s_{1+2} & s_1d_1 \\ s_0s_{1+2} & c_0 & -s_0c_{1+2} & -s_0c_1d_1 \\ -c_0s_{1+2} & s_0 & c_0c_{1+2} & d_0 + c_0c_1d_1 \\ 0 & 0 & 0 & 1 \end{bmatrix}_\mathcal{L}$$

\(O_{\mathcal{L}_3}\)系相对于\(O_{\mathcal{L}_2}\)系先绕\(y\)轴转动\(θ_{\mathcal{L}_4}\)， 再绕\(x\)轴转动\(θ_{\mathcal{L}_3}\)，最后绕\(z\)轴转动\(θ_{\mathcal{L}_5}\)，得到齐次变换矩阵如下: $$H_{{O_{\mathcal{L}_2}}{O_{\mathcal{L}_3}}} = \begin{bmatrix} c_4c_5 + s_4s_3s_5 & -c_4s_5 + s_4s_3c_5 & s_4s_3 & 0 \\ c_3s_5 & c_3c_5 & -s_3 & 0 \\ -s_4c_5 + c_4s_3s_5 & s_4s_5 + c_4s_3c_5 & c_4c_3 & d_2 \\ 0 & 0 & 0 & 1 \end{bmatrix}_\mathcal{L}$$ $$H_{{O_{\mathcal{L}_0}}{O_{\mathcal{L}_3}}} = \begin{bmatrix} c_5c_{1+2+4}+ s_3s_5s_{1+2+4} & -s_5c_{1+2+4} + s_3c_5s_{1+2+4} & c_{1+2}s_4s_3 + s_{1+2}c_4c_3 &s_{1+2}d_2+s_1d_1 \\ s_0c_5s_{1+2+4}-s_0s_3s_5c_{1+2+4}+c_0c_3s_5 & -s_0s_5s_{1+2+4}-s_0s_3c_5c_{1+2+4}+c_0c_3c_5 & s_0s_{1+2}s_4s_3 - c_0s_3-s_0c_{1+2}c_4c_3 & -s_0c_{1+2}d_2-s_0c_1d_1 \\ -c_0c_5s_{1+2+4}+c_0s_3s_5c_{1+2+4}+s_0c_3s_5&c_0s_5s_{1+2+4}+c_0s_3c_5c_{1+2+4}+s_0c_3c_5&-c_0s_{1+2}s_4s_3-s_0s_3+c_0c_{1+2}c_4c_3 & c_0c_{1+2}d_2+d_0+c_0c_1d_1 \\ 0 & 0 & 0 & 1 \end{bmatrix}_\mathcal{L}$$

3.3 两腿间的运动学关系

根据图1(1)所示，点\(O_{r_3}\)相对于\(O_{l_3}\)系的坐标为\([0, d_3, 0]^T\)， 则点\(O_{r_3}\)相对于\(O_{l_0}\)系的坐标可以计算如下： $$\begin{bmatrix}r_{{O_{l_0}}{O_{r_3}}} \\ 1 \end{bmatrix} = H_{{O_{l_0}}{O_{l_3}}}\begin{bmatrix}0 \\ d_3 \\ 0 \\ 1 \end{bmatrix} = \begin{bmatrix} d_3(-s_5c_{1+2+4}+s_3c_5s_{1+2+4})+s_{1+2}d_2+s_1d_1 \\ d_3(-s_0s_5s_{1+2+4}-s_0s_3c_5c_{1+2+4}+c_0c_3c_5)-s_0c_{1+2}d_2-s_0c_1d_1 \\ d_3(c_0s_5s_{1+2+4}+c_0s_3c_5c_{1+2+4}+s_0c_3c_5)+c_0c_{1+2}d_2+d_0+c_0c_1d_1 \\ 1 \end{bmatrix}_l = \begin{bmatrix} -s_5d_3+s_{1+2}d_2+s_1d_1 \\ c_5d_3-s_0(c_{1+2}d_2+c_1d_1) \\ d_0+c_0(c_1d_1+c_{1+2}d_2) \\ 1 \end{bmatrix}_l$$

同理，点\(O_{l_3}\)相对于\(O_{r_3}\)系的坐标为\([0, -d_3, 0]^T\)， 则点\(O_{l_3}\)相对于\(O_{r_0}\)系的坐标可以计算如下： $$\begin{bmatrix}r_{{O_{r_0}}{O_{l_3}}} \\ 1 \end{bmatrix} = H_{{O_{r_0}}{O_{r_3}}}\begin{bmatrix}0 \\ -d_3 \\ 0 \\ 1 \end{bmatrix} = \begin{bmatrix} -d_3(-s_5c_{1+2+4}+s_3c_5s_{1+2+4})+s_{1+2}d_2+s_1d_1 \\ -d_3(-s_0s_5s_{1+2+4}-s_0s_3c_5c_{1+2+4}+c_0c_3c_5)-s_0c_{1+2}d_2-s_0c_1d_1 \\ -d_3(c_0s_5s_{1+2+4}+c_0s_3c_5c_{1+2+4}+s_0c_3c_5)+c_0c_{1+2}d_2+d_0+c_0c_1d_1 \\ 1 \end{bmatrix}_r = \begin{bmatrix} s_5d_3+s_{1+2}d_2+s_1d_1 \\ -c_5d_3-s_0(c_{1+2}d_2+c_1d_1) \\ d_0+c_0(c_1d_1+c_{1+2}d_2) \\ 1 \end{bmatrix}_r$$

4. 逆运动学模型

此时的逆运动学问题简化为给定两个踝关节\(r_{l_1}\),\(r_{r_1}\)和髋关节\(r_{l_3}\),\(r_{r_3}\)的位置， 以及摆动脚的偏航角，求\(θ_{\mathcal{L}_i}\)的取值，其中\(\mathcal{L}=l,r\)分别表示左腿或者右腿的设置； \(i=0,1,2,3,4,5\)。根据正运动学关系我们知道： $$\boldsymbol{r_{L_3}-r_{L_1}}= \begin{bmatrix}x_(\mathcal{L}_3\mathcal{L}_1 )\\y_{\mathcal{L}_3\mathcal{L}_1}\\z_{\mathcal{L}_3\mathcal{L}_1}\end{bmatrix}= \begin{bmatrix}s_{1+2}d_2+s_1d_1 \\ -s_0(c_{1+2}d_2+c_1d_1 ) \\ c_0(c_{1+2}d_2+c_1d_1)\end{bmatrix}_\mathcal{L}$$ $$\frac{y_{\mathcal{L}_3\mathcal{L}_1}}{z_{\mathcal{L}_3\mathcal{L}_1}}= \left . -\frac{s_0(c_{1+2}d_2+c_1d_1)}{c_0(c_{1+2}d_2+c_1d_1)}\right | _\mathcal{L} = -\tan{\theta_{\mathcal{L}_0}}$$ $$θ_{\mathcal{L}_0}=\arctan{-\frac{y_{\mathcal{L}_3\mathcal{L}_1}}{z_{\mathcal{L}_3\mathcal{L}_1}}}, θ_{\mathcal{L}_3} = -θ_{\mathcal{L}_0}$$

由上式得： $$d_1\sin{\theta_{\mathcal{L}_1}} + d_2\sin{\left(\theta_{\mathcal{L}_1}+\theta{\mathcal{L}_2}\right)} = x_{\mathcal{L}_3\mathcal{L}_1}$$ $$d_1\cos{\theta_{\mathcal{L}_1}} + d_2\cos{\left(\theta_{\mathcal{L}_1}+\theta{\mathcal{L}_2}\right)} = z_{\mathcal{L}_3\mathcal{L}_1} / \cos{\theta_{\mathcal{L}_0}}$$ 对上式的求解可以转化为平面二连杆逆运动学问题，如图2所示。

图 2 平面二连杆的逆运动学

则有 $$\alpha=\arccos{\left(\frac{d_1^2+d_2^2-r^2}{2d_1d_2} \right)}, r^2=x_{\mathcal{L}_3\mathcal{L}_1}^2+\left(z_{\mathcal{L}_3\mathcal{L}_1}/\cos{\theta_{\mathcal{L}_0}}\right)^2$$ $$\theta_{\mathcal{L}_2} = \pi - \alpha$$ $$\beta=\arccos{\left(\frac{r^2+d_1^2-d_2^2}{2d_1r} \right)}, \phi=\arctan{\left(\frac{z_{\mathcal{L}_3\mathcal{L}_1}/\cos{\theta_{\mathcal{L}_0}}}{x_{\mathcal{L}_3\mathcal{L}_1}} \right)}$$ $$\theta_{\mathcal{L}_1}=\frac{\pi}{2}-\phi+\beta$$ $$\theta_{\mathcal{L}_4}=-\left(\theta_{\mathcal{L}_1} + \theta_{\mathcal{L}_2}\right)$$ 髋关节的偏航角可以通过\(O_{r_3}\)相对于\(O_{l_3}\)系的坐标以及\(O_{l_3}\)相对于\(O_{r_3}\)系的坐标求得 $$\boldsymbol{r_{r_3}-r_{l_3}}=\begin{bmatrix} x_{r_3l_3} \\ y_{r_3l_3} \\ z_{r_3l_3} \end{bmatrix} =\begin{bmatrix} -s_5d_3 \\ c_5d_3 \\0 \end{bmatrix}_l, \boldsymbol{r_{l_3}-r_{r_3}}=\begin{bmatrix} x_{l_3r_3} \\ y_{l_3r_3} \\ z_{l_3r_3} \end{bmatrix} =\begin{bmatrix} s_5d_3 \\ -c_5d_3 \\0 \end{bmatrix}_r$$ $$\frac{x_{r_3l_3}}{y_{r_3l_3}} = -\frac{\sin{θ_{l_5}}d_3}{\cos{θ_{l_5}}d_3}=-\tan{θ_{l_5}}$$ $$\frac{x_{l_3r_3}}{y_{l_3r_3}} = -\frac{\sin{θ_{r_5}}d_3}{\cos{θ_{r_5}}d_3}=-\tan{θ_{r_5}}$$ $$θ_{l_5}=\arctan{\frac{x_{r_3l_3}}{y_{r_3l_3}}}$$ $$θ_{r_5}=\arctan{\frac{x_{l_3r_3}}{y_{l_3r_3}}}$$

结论

给定两个踝关节和髋关节在惯性坐标系下的坐标后，只要满足水平行走的运动学约束，理论上就可以求得各个关节的转角。