四、机器人的正逆运动学及轨迹规划

单腿建模

*规定z轴沿关节轴的i指向
*规定x轴沿两关节公垂线从i指向i+1
*规定y轴按右手定则

单腿坐标系变换关系参数表

连杆	${\theta }_i$	$d_i$	$a_i$	${\alpha }_i$
L1	${\theta }_1$	0	$a_1$	$\frac{pi}{4}$
L2	${\theta }_2$	0	$a_2$	0
L3	${\theta }_3$	0	$a_3$	0

• ${\theta }_i$为x轴间夹角
• $d_i$为沿z轴，x轴的间距
• $a_i$为沿x轴，z轴的间距
• $r_i$ 为z轴间夹角

则$T_i^{i-1}=R_z({\theta }_i)\ast trans(0,0,d_i)trans(a_i,0,0)R_x(d_i)$

$T_2^1=\left[\begin{array}{cccc}cos{\theta }_1& -sin{\theta }_1& 0& 0\\ sin{\theta }_1& cos{\theta }_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\ast \left[\begin{array}{cccc}1& 0& 0& a_1\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\ast \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& -1& 0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cccc}cos{\theta }_1& 0& sin{\theta }_1& a_1\ast cos{\theta }_1\\ sin{\theta }_1& 0& -cos{\theta }_1& a_1\ast sin{\theta }_1\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

$T_3^2=\left[\begin{array}{cccc}cos{\theta }_2& -sin{\theta }_2& 0& a_2\ast cos{\theta }_2\\ sin{\theta }_2& cos{\theta }_2& 0& a_2\ast sin{\theta }_2\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$T_4^2=\left[\begin{array}{cccc}cos{\theta }_3& -sin{\theta }_3& 0& a_3\ast cos{\theta }_3\\ sin{\theta }_3& cos{\theta }_3& 0& {\theta }_3\ast sin{\theta }_3\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

正运动学

通过${\theta }_1,{\theta }_2,{\theta }_3$求解足尖坐标$(x,y,z)$

$T_4^1=T_2^1\ast T_3^2\ast T_3^4=\left[\begin{array}{cccc}-& -& -& a_{1}cos{\theta }_1+a_{2}cos{\theta }_{1}cos{\theta }_2+a_{3}cos{\theta }_{1}cos{\theta }_{2}cos{\theta }_3-a_{3}cos{\theta }_{1}sin{\theta }_{2}sin{\theta }_3\\ -& -& -& a_{1}sin{\theta }_1+a_{2}sin{\theta }_{1}cos{\theta }_2+a_{3}sin{\theta }_{1}cos{\theta }_{2}cos{\theta }_3-a_{3}sin{\theta }_{1}sin{\theta }_{2}sin{\theta }_3\\ -& -& -& a_{2}sin{\theta }_2+a_{3}cos{\theta }_{2}sin{\theta }_3+a_{3}sin{\theta }_{2}cos{\theta }_3\\ 0& 0& 0& 1\end{array}\right]$

则
$x=a_{1}cos{\theta }_1+a_{2}cos{\theta }_{1}cos{\theta }_2+a_{3}cos{\theta }_{1}cos{\theta }_{2}cos{\theta }_3-a_{3}cos{\theta }_{1}sin{\theta }_{2}sin{\theta }_3$

$y=a_{1}sin{\theta }_1+a_{2}sin{\theta }_{1}cos{\theta }_2+a_{3}sin{\theta }_{1}cos{\theta }_{2}cos{\theta }_3-a_{3}sin{\theta }_{1}sin{\theta }_{2}sin{\theta }_3$

$z=a_{2}sin{\theta }_2+a_{3}cos{\theta }_{2}sin{\theta }_3+a_{3}sin{\theta }_{2}cos{\theta }_3$

逆运动学

已知足尖坐标$p(x,y,z)$

设$T_4^1=\left[\begin{array}{cccc}-& -& -& x\\ -& -& -& y\\ -& -& -& z\\ 0& 0& 0& 1\end{array}\right]$

由 $T_4^1=T_2^1\ast T_3^2\ast T_3^4$ 有 $(T_2^1{)}^{-1}\ast T_4^1=T_3^2\ast T_3^4$

得 $xcos{\theta }_1-a_1+ysin{\theta }_1=a_{2}cos{\theta }_2+a_{3}cos({\theta }_2+{\theta }_3)$

$z=a_{2}sin{\theta }_2+a_{3}sin({\theta }_2+{\theta }_3)$

$xsin{\theta }_1-ycos{\theta }_1=0$

解得

${\theta }_1=atan(y/x)$

${\theta }_3=\mp acon(\frac{M^2+z^2-a_2^2-a_3^2}{2a_2{a}_3})$

${\theta }_2=\frac{pi}{2}-acos\frac{z{a}_{3}cos{\theta }_3+z{a}_2-M{a}_{3}sin{\theta }_3}{M^2+z^2}$

注：$M=xcos{\theta }_1+ysin{\theta }_1-a_1$

在MATALB中验证正确性：

伪代码：
1 穷举${\theta }_1,{\theta }_2,{\theta }_3$在$(-\frac{pi}{4},\frac{pi}{4})$的所有状态
2 将所有状态正解为坐标$p_i(x_i,y_i,z_i)$
3 将所有$p_i(x_i,y_i,z_i)$逆解为${\theta }_1^{\ast },{\theta }_2^{\ast },{\theta }_3^{\ast }$
4 将${\theta }_1^{\ast },{\theta }_2^{\ast },{\theta }_3^{\ast }$与${\theta }_1,{\theta }_2,{\theta }_3$进行比较，误差范围在$\mp 0.01$视为正确
5 将正确的${\theta }_1,{\theta }_2,{\theta }_3$在图1显示，否则在图2显示；将单腿运动范围在图3显示

结果：

图1：正确的${\theta }_1,{\theta }_2,{\theta }_3$

图2：错误的${\theta }_1,{\theta }_2,{\theta }_3$图3：单腿运动范围

从验证结果可以看到，该逆解公式在${\theta }_1,{\theta }_2,{\theta }_3∊(-\frac{pi}{4},\frac{pi}{4}）$范围内完全正确，图3可对后面的单腿轨迹规划作一定参考

单腿轨迹规划

在单腿向前运动的一个周期T中，在$(0,\frac{T}{2})$用抛物线形成的拱形轨迹，在$(\frac{T}{2},T)$的轨迹为直线段。$s$为步长，$h$为迈步高度
$$y=\{\begin{array}{ll}-\frac{s^2}{4}\ast x^2+h& x∊(0,\frac{T}{2})\\ \frac{s}{2}-x& x∊(\frac{T}{2},T)\end{array}$$

通过运动学逆解将轨迹转化为三个关节坐标，实现单腿运动规划

前进的三角步态生成

三角步态特点：L1 L3 R2同相 R1 R3 L2 同相； (L1 L3 R2) 超前 (R1 R3 L2) T/2；同一组的三个足始终呈三角形向前运动

由以上特点建立以下单组腿的位置关系

设足尖1对坐标系1的坐标为$p_1(x_0,y_0)$

足尖1对坐标系0的坐标为$p_1^{\ast }(x_0,y_0+b_2)$

足尖2对坐标系0的坐标为$p_2^{\ast }(x_0+c_1,y_0+b_2-c_2-c_3)$

足尖3对坐标系0的坐标为$p_3^{\ast }(x_0-c_1,y_0+b_2-c_2-c_3)$

足尖2对坐标系2的坐标为$p_2(-\frac{\sqrt{2}}{2}(x_0+y_0-b_1+b_2+b_3+c_1-c_2-c_3),-\frac{\sqrt{2}}{2}(-x_0+y_0+b_1+b_2+b_3-c_1-c_2-c_3))$

足尖3对坐标系3的坐标为$p_3(-\frac{\sqrt{2}}{2}(x_0-y_0+b_1-b_2-b_3-c_1+c_2+c_3),-\frac{\sqrt{2}}{2}(x_0+y_0+b_1+b_2+b_3-c_1-c_2-c_3))$

即我们可以通过只对足尖1进行轨迹规划，通过以上关系求出在三角形约束条件下的足尖2和足尖3的轨迹

伪代码
1 规划足尖1运动轨迹$p_1(x_1,y_1,z_1)$
2 通过上述关系求出足尖2和足尖3运动轨迹$p_2(x_2,y_2,z_2)$和$p_3(x_3,y_3,z_3)$
3 将三个足尖轨迹在图1中显示
4 将轨迹坐标逆解为角度${\theta }_1,{\theta }_2,{\theta }_3$
5 将${\theta }_1,{\theta }_2,{\theta }_3$正解为$p_1^{\ast }$、$p_2^{\ast }$和$p_3^{\ast }$，并与$p_1$、$p_2$和$p_3$进行对比，在误差允许范围内视正确，否则错误
6 将错误个数输出，将转化的${\theta }_1,{\theta }_2,{\theta }_3$输出

结果：

图1：足尖轨迹

最后将输出的${\theta }_1,{\theta }_2,{\theta }_3$作为输入，在仿真环境中进行验证