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

1.五连杆正运动学分析

如图所示为五连杆结构图，其中A，E为机器人腿部控制的两个电机，θ₁,θ₄可以通过电机的编码器测得。五连杆控制任务主要关注机构末端C点位置，其位置用直角坐标表示为(C_x,C_y)，极坐标系用(L₀,θ₀)表示。
根据上述五连杆结构图可以列出以下等式：
$$\begin{array}{c}\{\begin{array}{l}{B}_x+L_2\ast cos({\theta }_2)=D_x+L_3\ast cos({\theta }_3)\\ B_y+L_2\ast sin({\theta }_2)=D_y+L_3\ast sin({\theta }_3)\end{array}\end{array}\text{\hspace{1em}(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)}$$

$$\{\begin{array}{l}K=2\ast (B_y-D_y)\ast L_2\\ M=2\ast (B_x-D_x)\ast L_2\\ P=2\ast [(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{array}$$
使用二倍角法对公式(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.