四足机器人——3自由度机械臂正逆解（肘式+膝式）

四足机器人——3自由度机械臂正逆解（肘式+膝式）

项目介绍

最近在做一个12自由度四足机器人的项目，每条腿3个自由度。涉及到足端位姿关节逆解的问题，于是进行了简单的推导，并且编写了Matlab仿真程序，有需要的童鞋可以点击下载。
四足机器人腿步有肘式和膝式之分，很好理解，顾名思义。因为想要探索一下前肘后膝四足机器人和双肘式、双膝式的不同，因此推导了两种腿部关节的逆解。为了简单起见，没有用到机器人学复杂的坐标转换，只需要基础的初高中几何知识就可以理解。

说明：以下推导x,y,z值取正方向为正值，反方向为负值；角度值逆时针为正值，顺时针为负值。
O、A、B、C 表示点，a、b、c、l 表示边长，q1、q2、q3表示角度。

肘式

正解推导（已知关节角度求坐标）
$$Ax=0$$$$Ay=a\ast sin(q3)$$$$Az=-a\ast cos(q3)$$
$$Bx=-b\ast sin(-q2)$$$$By=[b\ast cos(-q2)+a]\ast sin(q3)$$$$Bz=-[b\ast cos(-q2)+a]\ast cos(q3)$$
$$Cx=-b\ast sin(-q2)+c\ast cos(-q2-q1)$$$$Cy=[b\ast cos(-q2)+a+c\ast sin(-q2-q1)]\ast sin(q3)$$$$Cz=-[b\ast cos(-q2)+a+c\ast sin(-q2-q1)]\ast cos(q3)$$
逆解推导(已知坐标求关节角度)
$$q3=arctan(-y/z)$$$$l=\sqrt{(x^2+(y-a\ast sin(q3){)}^2+(z+a\ast cos(q3){)}^2)}$$$$q2=-arccos((b^2+l^2-c^2)/(2\ast b\ast l))+arcsin(x/l)$$$$q1=pi/2-arccos((b^2+c^2-l^2)/(2\ast b\ast c))$$

膝式

正解推导（已知关节角度求坐标）
$$Ax=0$$$$Ay=a\ast sin(q3)$$$$Az=-a\ast cos(q3)$$
$$Bx=b\ast sin(q2)$$$$By=[b\ast cos(q2)+a]\ast sin(q3)$$$$Bz=-[b\ast cos(q2)+a]\ast cos(q3)$$
$$Cx=b\ast sin(q2)-c\ast cos(q2+q1)$$$$Cy=[b\ast cos(q2)+a+c\ast sin(q2+q1)]\ast sin(q3)$$$$Cz=-[b\ast cos(q2)+a+c\ast sin(q2+q1)]\ast cos(q3)$$
逆解推导(已知坐标求关节角度)
$$q3=arctan(-y/z)$$$$l=\sqrt{(x^2+(y-a\ast sin(q3){)}^2+(z+a\ast cos(q3){)}^2)}$$$$q2=arccos((b^2+l^2-c^2)/(2\ast b\ast l))-arcsin(-x/l)$$$$q1=arccos((b^2+c^2-l^2)/(2\ast b\ast c))-pi/2$$