轮腿机器人-五连杆正运动学解算

1.五连杆正运动学分析

如图所示为五连杆结构图，其中A，E为机器人腿部控制的两个电机，θ₁,θ₄可以通过电机的编码器测得。五连杆控制任务主要关注机构末端C点位置，其位置用直角坐标表示为(C_x,C_y)，极坐标系用(L₀,θ₀)表示。
根据上述五连杆结构图可以列出以下等式：
{ B x + L 2 ∗ c o s ( θ 2 ) = D x + L 3 ∗ c o s ( θ 3 ) B y + L 2 ∗ s i n ( θ 2 ) = D y + L 3 ∗ s i n ( θ 3 )

$$\begin{equation} \begin{cases} B_{x}+L_{2}*{\color{Green} cos(\theta _{2})} =D_{x}+L_{3}*{\color{Green} cos(\theta _{3})} \\ B_{y}+L_{2}*{\color{Orange} sin(\theta _{2})} =D_{y}+L_{3}*{\color{Orange} sin(\theta _{3})} \end{cases} \tag{1} \end{equation}$$ {Bx​+L2​∗cos(θ2​)=Dx​+L3​∗cos(θ3​)By​+L2​∗sin(θ2​)=Dy​+L3​∗sin(θ3​)​​(1)​
对公式(1)移项，并在等式两边进行平方有：
$$\begin{array}{c}\{\begin{array}{l}(B_x+L_2\ast cos({\theta }_2)-D_x{)}^2=(L_3\ast cos({\theta }_3){)}^2\\ (B_y+L_2\ast sin({\theta }_2)-D_y{)}^2=(L_3\ast sin({\theta }_3){)}^2\end{array}\end{array}\text{\hspace{1em}(2)}$$
将平方展开有：
$$\begin{array}{c}\{\begin{array}{l}(B_x-D_x{)}^2+2\ast (B_x-D_x)\ast L_2\ast cos({\theta }_2)+(L_2\ast cos({\theta }_2){)}^2=(L_3\ast cos({\theta }_3){)}^2\\ (B_y-D_y{)}^2+2\ast (B_y-D_y)\ast L_2\ast sin({\theta }_2)+(L_2\ast sin({\theta }_2){)}^2=(L_3\ast sin({\theta }_3){)}^2\end{array}\end{array}\text{\hspace{1em}(3)}$$
对公式(3)内部两个等式相加并移项有：
$$\begin{array}{c}K\ast sin({\theta }_2)+M\ast cos({\theta }_2)=C\end{array}\text{\hspace{1em}(4)}$$

{ K = 2 ∗ ( B y − D y ) ∗ L 2 M = 2 ∗ ( B x − D x ) ∗ L 2 P = 2 ∗ [ ( L 3 ) 2 − ( L 2 ) 2 ] L B D = ( B x − D x ) 2 + ( B y − D y ) 2 C = P − ( L B D ) 2

$$\begin{cases} K=2*(B_{y} - D_{y})*L_{2} \\M=2*(B_{x}-D_{x})*L_{2} \\P=2*[(L_{3})^{2}-(L_{2})^{2}] \\L_{BD}=\sqrt{(B_{x}-D_{x})^{2}+(B_{y} - D_{y})^{2}} \\C=P-(L_{BD} )^2 \end{cases}$$ ⎩ ⎨ ⎧​K=2∗(By​−Dy​)∗L2​M=2∗(Bx​−Dx​)∗L2​P=2∗[(L3​)2−(L2​)2]LBD​=(Bx​−Dx​)2+(By​−Dy​)2 ​C=P−(LBD​)2​
使用二倍角法对公式(4)进一步化简，已知：
$$\{\begin{array}{l}tan\frac{\theta }{2}=\frac{sin(\theta )}{1+cos(\theta )}\\ cos(\theta )=co{s}^2\frac{\theta }{2}-si{n}^2\frac{\theta }{2}=2\ast co{s}^2\frac{\theta }{2}-1\\ co{s}^2\frac{\theta }{2}-si{n}^2\frac{\theta }{2}=1\end{array}$$
当$1+cos(\theta )\ne 0$，对公式(4)进行如下变化，其中$\tau =1+cos(\theta )$:
$$\begin{array}{c}\frac{\tau }{2}\ast (\frac{2\ast K\ast sin({\theta }_2)}{\tau }+\frac{2\ast M\ast cos({\theta }_2)}{\tau }-\frac{2\ast C}{\tau })=0\end{array}\text{\hspace{1em}(5)}$$
使用二倍角对公式(5)进行展开并进行化简得：
$$\begin{array}{c}\frac{1+cos({\theta }_2)}{2}\ast [(C-M)\ast ta{n}^2\frac{{\theta }_2}{2}+2\ast K\ast tan(\frac{{\theta }_2}{2})+(M+C)]\end{array}\text{\hspace{1em}(6)}$$
根据公式(6)得到了一个关于$tan(\frac{{\theta }_2}{2})$的一元二次方程，其求根判别式为：
$$△=(2\ast K{)}^2-4\ast (C-M)\ast (M+C)=4(K^2+M^2-C^2)$$
当$△\ge 0$时，可以解出${\theta }_2$:
$${\theta }_2=2\ast arctan(\frac{K\pm \sqrt{(K^2+M^2-C^2)}}{M-C})$$
通过${\theta }_1$即可解算出$C$点的直角坐标有：
$$\begin{array}{c}\{\begin{array}{l}{C}_x=L_1\ast cos({\theta }_1)+L_2\ast cos({\theta }_2)\\ C_y=L_1\ast sin({\theta }_1)+L_2\ast sin({\theta }_2)\end{array}\end{array}\text{\hspace{1em}(7)}$$
进一步推导得到极坐标为：
$$\begin{array}{c}\{\begin{array}{l}{L}_0=\sqrt{(C_x-L_5{)}^2+C_y^2}\\ {\theta }_0=arctan\frac{C_y}{C_x-\frac{L_5}{2}}\end{array}\end{array}\text{\hspace{1em}(8)}$$

2.参考文献

https://zhuanlan.zhihu.com/p/613007726
[1]于红英,唐德威,王建宇.平面五杆机构运动学和动力学特性分析[J].哈尔滨工业大学学报,2007(06):940-943.
[2]谢惠祥.四足机器人对角小跑步态虚拟模型直觉控制方法研究[D].国防科学技术大学,2015.