Open imuncle opened 4 years ago
看了《机器人学》后颇有感触，觉得我之前使用的几何法解角度实在是费劲，也就这小狗一条腿就三个自由度，要是六轴机械臂，用几何法解起来就很费神了（感兴趣可以随便翻一本机器人运动学的书，一般都会用几何法推一遍）。忽的我又想起来大一时线性代数老师说的一句名言：万物皆矩阵。
在机器人学中，一般用四维齐次变换矩阵表示坐标系的旋转和平移：
$$A=\left[ \begin{array}{ccc} { } & { } & { } &{ p_x }\\ { } & {R} & {}&{ p_y } \\ {} & {} & {}&{ p_z }\\ {0}&{0}&{0}&{1} \end{array}\right]$$
左上角的R为身体相对于地面的旋转矩阵，表示姿态（使用RPY表示方法）：
$$\begin{align} R &=\operatorname{rotx}(R) \cdot \operatorname{roty} (P) \cdot \operatorname{rotz} (Y)\\ &=\left[ \begin{array}{ccc} {1} & {0} & {0} \\ {0} & {\mathrm{cR}} & {-\mathrm{s} R} \\ {0} & {\mathrm{s} R} & {\mathrm{c} R}\end{array}\right] \left[ \begin{array}{ccc} {c P} & {0} & {s P} \\ {0} & {1} & {0} \\ {-s P} & {0} & {c P}\end{array}\right] \left[ \begin{array}{ccc} {\mathrm{c} Y} & {-\mathrm{s} Y} & {0} \\ {\mathrm{s} Y} & {\mathrm{c} Y} & {0} \\ {0} & {0} & {1}\end{array}\right] \end{align}$$
右上角的1×3矩阵表示坐标的平移向量。
四足机器人的抽象模型如下：
四足机器人的身体经过旋转和平移变换后到达绿色虚线位置。
我们的目标时求解腿部末端的位置向量，即图中的向量$\overrightarrow{A'B}$，显然有以下等式：
$$\overrightarrow{OO'}+\overrightarrow{O'A'}+\overrightarrow{A'B}=\overrightarrow{OB}$$
其中B点的坐标是我们手动指定的，而四足机器人的相关结构参数都是已知条件，所以上面等式中的$\overrightarrow{OB}$向量和$\overrightarrow{OO'}$都是已知量，而向量$\overrightarrow{O'A'}$有以下关系：
$$\overrightarrow{O'A'} = R\cdot\overrightarrow{O'A}$$
其中R是身体的变换矩阵，这也是我们手动指定的，与姿态角及位移有关。
于是根据上述关系我们就可以求出目标向量$\overrightarrow{A'B}$。
为考虑更加普遍的四足机器人结构，现将四足机器人的结构修改如下：
我自己的四足与上面的不同点在于我的机器人d长度为0，即我的相当于一个腕关节，但大多数的机器人d并不为零。
所建立的D-H坐标如上图所示，y轴未画出，可使用右手螺旋定则确定y轴方向，当然这里的分析计算根本用不到y轴。
很容易得出，上述狗腿的D-H参数如下：
单个关节的D-H变换矩阵为：
$$\begin{align}^{i-1}Ai&=T{z,d}T{z,\theta}T{x,a}T_{x,\alpha} \\ &=\left[\begin{array}{ccc} {1}&{0}&{0}&{0}\\ {0}&{1}&{0}&{0}\\ {0}&{0}&{1}&{d_i}\\ {0}&{0}&{0}&{1} \end{array}\right] \left[\begin{array}{ccc} {\cos\theta_i}&{-sin\theta_i}&{0}&{0}\\ {sin\theta_i}&{\cos\theta_i}&{0}&{0}\\ {0}&{0}&{1}&{0}\\ {0}&{0}&{0}&{1} \end{array}\right] \left[\begin{array}{ccc} {1}&{0}&{0}&{a_i}\\ {0}&{1}&{0}&{0}\\ {0}&{0}&{1}&{0}\\ {0}&{0}&{0}&{1} \end{array}\right] \left[\begin{array}{ccc} {1}&{0}&{0}&{0}\\ {0}&{\cos\alpha_i}&{-\sin\alpha_i}&{0}\\ {0}&{\sin\alpha_i}&{\cos\alpha_i}&{0}\\ {0}&{0}&{0}&{1} \end{array}\right] \\ &=\left[\begin{array}{ccc} {\cos\theta_i}&{-cos\alpha_i\sin\theta_i}&{\sin\alpha_i\sin\theta_i}&{a_i\cos\theta_i}\\ {\sin\theta_i}&{\cos\alpha_i\cos\theta_i}&{-\sin\alpha_i\cos\theta_i}&{a_i\sin\theta_i}\\ {0}&{\sin\alpha_i}&{\cos\alpha_i}&{d_i}\\ {0}&{0}&{0}&{1} \end{array}\right] \end{align} $$
根据齐次变换规律，可以得到四足机器人腿部正运动学方程为：
$$^{0}T_3=^{0}A_1^{1}A_2^{2}A_3 \tag{1}$$
将上面D-H参数表中的数据代入方程可以计算得到：
$$\begin{align} \left[\begin{array}{ccc} {n_x}&{o_x}&{a_x}&{p_x}\\ {n_y}&{o_y}&{a_y}&{p_y}\\ {n_z}&{o_z}&{a_z}&{p_z}\\ {0}&{0}&{0}&{1} \end{array}\right]= \left[\begin{array}{ccc} {c1c{23}}&{-c1s{23}}&{-s_1}&{l_1c_1c_2-ds_1+l_2c1c{23}}\\ {s1c{23}}&{-s1s{23}}&{c_1}&{l_1s_1c_2+dc_1+l_2s1c{23}}\\ {-s{23}}&{-c{23}}&{c_1}&{-l_1s_2-l2s{23}}\\ {0}&{0}&{0}&{1} \end{array}\right]\\ 其中c_i=\cos\theta_i; s_i=sin\thetai; c{ij}=\cos(\theta_i=\thetaj); s{ij}=\sin(\theta_i+\theta_j) \end{align} $$
根据正运动学方程，若已知支撑腿的各关节旋转角度，即可求得支撑腿末端在坐标系中的位姿，但我们的需求是反过来的，先知道腿末端的位置来求解各关节的旋转角度，所以我们需要进行逆运动学分析。
因为四足机器人每条腿只有三个自由度，可以运用递推逆变换法求逆解。
将式（1）等式左乘矩阵逆得：
$$^{0}A_1^{-1} \cdot^{0}T_3=^{1}A_2^{2}A_3$$
代入具体参数可得到如下矩阵：
$$\begin{align} \left[\begin{array}{ccc|c} {n_xc_1+n_ys_1}&{o_xc_1+o_ys_1}&{a_xc_1+a_ys_1}&{p_xc_1+p_ys_1}\\ {-n_z}&{-o_z}&{-a_z}&{-p_z}\\ {n_yc_1-n_xs_1}&{o_yc_1-o_xs_1}&{a_yc_1-a_xs_1}&{p_yc_1-p_xs1}\\ \hline {0}&{0}&{0}&{1} \end{array}\right]= \left[\begin{array}{ccc|c} {c{23}}&{-s_{23}}&{0}&{l2c{23}+l_1c2}\\ {s{23}}&{c_{23}}&{0}&{l2s{23}+l_1s_2}\\ {0}&{0}&{1}&{d}\\ \hline {0}&{0}&{0}&{1} \end{array}\right] \end{align} $$
由上面两个矩阵右上角三个元素一一相等可得到三元一次方程组：
$$p_xc_1+p_ys_1=l2c{23}+l_1c_2\tag{2}$$ $$-p_z=l2s{23}+l_1s_2\tag{3}$$ $$p_yc_1-p_xs_1=d\tag{4}$$
由式（4）可以求得：
$$\theta_1=arc\tan(p_y,p_x)-arc\tan(\frac{d}{\sqrt{p_x^2+p_y^2}},\pm\sqrt{1-\frac{d^2}{p_x^2+p_y^2}})$$
联合式（2）和式（3）可以求得：
$$\cos\theta_3=\frac{p_x^2+p_y^2+p_z^2-d^2-l_1^2-l_2^2}{2l_1l_2}$$ $$\theta_3=\pm arc\tan(\sqrt{1-\cos^2\theta_3},\cos\theta_3)$$ $$\theta_2=arc\tan[-rl_2s_3-p_z(l_1+l_2c_3),r(l_1+l_2c_3)-p_zl_2s_3]$$
说实话，推了一遍之后感觉最烦的还是逆运动学分析那里，求三个关节角度是需要一定技巧的。另外手动矩阵运算太累人了，稍不注意就乘错了。
最后谈一下四足机器人的腿末端运动轨迹的规划问题吧，经过和大佬们的交流以及上网查询资料后，四足机器人的腿末端运动轨迹应该采用复合摆线形式，这里给出一种修改后的摆线函数：
$$x(t)=S[\frac{t}{T}-\frac{1}{2\pi}\sin(2\pi\frac{t}{T})]$$ $$z(t)=H[\frac{1}{2}-\frac{1}{2}\cos(2\pi\frac{t}{T})]$$
OK，就这样吧，等下次有新东西了再来更这个系列。
看了《机器人学》后颇有感触，觉得我之前使用的几何法解角度实在是费劲，也就这小狗一条腿就三个自由度，要是六轴机械臂，用几何法解起来就很费神了（感兴趣可以随便翻一本机器人运动学的书，一般都会用几何法推一遍）。忽的我又想起来大一时线性代数老师说的一句名言：万物皆矩阵。
身体姿态表示
在机器人学中，一般用四维齐次变换矩阵表示坐标系的旋转和平移：
$$A=\left[ \begin{array}{ccc} { } & { } & { } &{ p_x }\\ { } & {R} & {}&{ p_y } \\ {} & {} & {}&{ p_z }\\ {0}&{0}&{0}&{1} \end{array}\right]$$
左上角的R为身体相对于地面的旋转矩阵，表示姿态（使用RPY表示方法）：
$$\begin{align} R &=\operatorname{rotx}(R) \cdot \operatorname{roty} (P) \cdot \operatorname{rotz} (Y)\\ &=\left[ \begin{array}{ccc} {1} & {0} & {0} \\ {0} & {\mathrm{cR}} & {-\mathrm{s} R} \\ {0} & {\mathrm{s} R} & {\mathrm{c} R}\end{array}\right] \left[ \begin{array}{ccc} {c P} & {0} & {s P} \\ {0} & {1} & {0} \\ {-s P} & {0} & {c P}\end{array}\right] \left[ \begin{array}{ccc} {\mathrm{c} Y} & {-\mathrm{s} Y} & {0} \\ {\mathrm{s} Y} & {\mathrm{c} Y} & {0} \\ {0} & {0} & {1}\end{array}\right] \end{align}$$
右上角的1×3矩阵表示坐标的平移向量。
单腿末端位置向量的求解
四足机器人的抽象模型如下：
四足机器人的身体经过旋转和平移变换后到达绿色虚线位置。
我们的目标时求解腿部末端的位置向量，即图中的向量$\overrightarrow{A'B}$，显然有以下等式：
$$\overrightarrow{OO'}+\overrightarrow{O'A'}+\overrightarrow{A'B}=\overrightarrow{OB}$$
其中B点的坐标是我们手动指定的，而四足机器人的相关结构参数都是已知条件，所以上面等式中的$\overrightarrow{OB}$向量和$\overrightarrow{OO'}$都是已知量，而向量$\overrightarrow{O'A'}$有以下关系：
$$\overrightarrow{O'A'} = R\cdot\overrightarrow{O'A}$$
其中R是身体的变换矩阵，这也是我们手动指定的，与姿态角及位移有关。
于是根据上述关系我们就可以求出目标向量$\overrightarrow{A'B}$。
腿部正、逆运动学分析
D-H坐标建立
为考虑更加普遍的四足机器人结构，现将四足机器人的结构修改如下：
我自己的四足与上面的不同点在于我的机器人d长度为0，即我的相当于一个腕关节，但大多数的机器人d并不为零。
所建立的D-H坐标如上图所示，y轴未画出，可使用右手螺旋定则确定y轴方向，当然这里的分析计算根本用不到y轴。
很容易得出，上述狗腿的D-H参数如下：
正运动学方程
单个关节的D-H变换矩阵为：
$$\begin{align}^{i-1}Ai&=T{z,d}T{z,\theta}T{x,a}T_{x,\alpha} \\ &=\left[\begin{array}{ccc} {1}&{0}&{0}&{0}\\ {0}&{1}&{0}&{0}\\ {0}&{0}&{1}&{d_i}\\ {0}&{0}&{0}&{1} \end{array}\right] \left[\begin{array}{ccc} {\cos\theta_i}&{-sin\theta_i}&{0}&{0}\\ {sin\theta_i}&{\cos\theta_i}&{0}&{0}\\ {0}&{0}&{1}&{0}\\ {0}&{0}&{0}&{1} \end{array}\right] \left[\begin{array}{ccc} {1}&{0}&{0}&{a_i}\\ {0}&{1}&{0}&{0}\\ {0}&{0}&{1}&{0}\\ {0}&{0}&{0}&{1} \end{array}\right] \left[\begin{array}{ccc} {1}&{0}&{0}&{0}\\ {0}&{\cos\alpha_i}&{-\sin\alpha_i}&{0}\\ {0}&{\sin\alpha_i}&{\cos\alpha_i}&{0}\\ {0}&{0}&{0}&{1} \end{array}\right] \\ &=\left[\begin{array}{ccc} {\cos\theta_i}&{-cos\alpha_i\sin\theta_i}&{\sin\alpha_i\sin\theta_i}&{a_i\cos\theta_i}\\ {\sin\theta_i}&{\cos\alpha_i\cos\theta_i}&{-\sin\alpha_i\cos\theta_i}&{a_i\sin\theta_i}\\ {0}&{\sin\alpha_i}&{\cos\alpha_i}&{d_i}\\ {0}&{0}&{0}&{1} \end{array}\right] \end{align} $$
根据齐次变换规律，可以得到四足机器人腿部正运动学方程为：
$$^{0}T_3=^{0}A_1^{1}A_2^{2}A_3 \tag{1}$$
将上面D-H参数表中的数据代入方程可以计算得到：
$$\begin{align} \left[\begin{array}{ccc} {n_x}&{o_x}&{a_x}&{p_x}\\ {n_y}&{o_y}&{a_y}&{p_y}\\ {n_z}&{o_z}&{a_z}&{p_z}\\ {0}&{0}&{0}&{1} \end{array}\right]= \left[\begin{array}{ccc} {c1c{23}}&{-c1s{23}}&{-s_1}&{l_1c_1c_2-ds_1+l_2c1c{23}}\\ {s1c{23}}&{-s1s{23}}&{c_1}&{l_1s_1c_2+dc_1+l_2s1c{23}}\\ {-s{23}}&{-c{23}}&{c_1}&{-l_1s_2-l2s{23}}\\ {0}&{0}&{0}&{1} \end{array}\right]\\ 其中c_i=\cos\theta_i; s_i=sin\thetai; c{ij}=\cos(\theta_i=\thetaj); s{ij}=\sin(\theta_i+\theta_j) \end{align} $$
根据正运动学方程，若已知支撑腿的各关节旋转角度，即可求得支撑腿末端在坐标系中的位姿，但我们的需求是反过来的，先知道腿末端的位置来求解各关节的旋转角度，所以我们需要进行逆运动学分析。
逆运动学分析
因为四足机器人每条腿只有三个自由度，可以运用递推逆变换法求逆解。
将式（1）等式左乘矩阵逆得：
$$^{0}A_1^{-1} \cdot^{0}T_3=^{1}A_2^{2}A_3$$
代入具体参数可得到如下矩阵：
$$\begin{align} \left[\begin{array}{ccc|c} {n_xc_1+n_ys_1}&{o_xc_1+o_ys_1}&{a_xc_1+a_ys_1}&{p_xc_1+p_ys_1}\\ {-n_z}&{-o_z}&{-a_z}&{-p_z}\\ {n_yc_1-n_xs_1}&{o_yc_1-o_xs_1}&{a_yc_1-a_xs_1}&{p_yc_1-p_xs1}\\ \hline {0}&{0}&{0}&{1} \end{array}\right]= \left[\begin{array}{ccc|c} {c{23}}&{-s_{23}}&{0}&{l2c{23}+l_1c2}\\ {s{23}}&{c_{23}}&{0}&{l2s{23}+l_1s_2}\\ {0}&{0}&{1}&{d}\\ \hline {0}&{0}&{0}&{1} \end{array}\right] \end{align} $$
由上面两个矩阵右上角三个元素一一相等可得到三元一次方程组：
$$p_xc_1+p_ys_1=l2c{23}+l_1c_2\tag{2}$$ $$-p_z=l2s{23}+l_1s_2\tag{3}$$ $$p_yc_1-p_xs_1=d\tag{4}$$
由式（4）可以求得：
$$\theta_1=arc\tan(p_y,p_x)-arc\tan(\frac{d}{\sqrt{p_x^2+p_y^2}},\pm\sqrt{1-\frac{d^2}{p_x^2+p_y^2}})$$
联合式（2）和式（3）可以求得：
$$\cos\theta_3=\frac{p_x^2+p_y^2+p_z^2-d^2-l_1^2-l_2^2}{2l_1l_2}$$ $$\theta_3=\pm arc\tan(\sqrt{1-\cos^2\theta_3},\cos\theta_3)$$ $$\theta_2=arc\tan[-rl_2s_3-p_z(l_1+l_2c_3),r(l_1+l_2c_3)-p_zl_2s_3]$$
结尾
说实话，推了一遍之后感觉最烦的还是逆运动学分析那里，求三个关节角度是需要一定技巧的。另外手动矩阵运算太累人了，稍不注意就乘错了。
最后谈一下四足机器人的腿末端运动轨迹的规划问题吧，经过和大佬们的交流以及上网查询资料后，四足机器人的腿末端运动轨迹应该采用复合摆线形式，这里给出一种修改后的摆线函数：
$$x(t)=S[\frac{t}{T}-\frac{1}{2\pi}\sin(2\pi\frac{t}{T})]$$ $$z(t)=H[\frac{1}{2}-\frac{1}{2}\cos(2\pi\frac{t}{T})]$$
OK，就这样吧，等下次有新东西了再来更这个系列。