四足机器人单腿逆运动学解-几何方法

古月居《四足机器人控制与仿真入门》学习笔记，视频链接： link

视角分解

静止状态

上图描述了一个四足机器人右前腿静止状态下的的后视图和右视图。O点为该腿的坐标系原点，P点为腿的末端点。共有3个自由度即3个旋转关节，3个关节连杆第一段关节长度为h,第二段关节长度为hu,第三段关节长度为hl。三个旋转角从上到下分别定义为gamma,alpha,beta。

运动状态

上图描述了一个四足机器人右前腿运动状态下的的后视图和右视图。规定关节顺时针旋转为正方向。

运动学逆解

单腿运动学逆解的目标是给定足末端点的坐标（x,y,z），求解能够达到这一目标的各关节角度，即：
$$\begin{gathered} (x,y,z)\to (gamma,alpha,beta) \\ s.t.h,hu,hl已知 \end{gathered}$$

求解gamma角

设dyz为从O点到P点的线段在yz平面上的投影，则dyz长度为：
$$dyz=\sqrt{y^2+z^2}$$
设lyz为顶点距足端点在yz平面上投影的距离，长度为：
$$lyz=\sqrt{dy{z}^2-h^2}$$
则
gamma_yz = -arctan(y/z)
gamma_h_offset = -arctan(h/lyz)
最终可得：
gamma = gamma_yz - gamma_h_offset

求解beta角

将上述关节投影在yz平面上可得如下几何关系：

其中
$$lx{z}^{\prime }=\sqrt{ly{z}^2+x^2}$$
于是可得如下方程组：
$$\{\begin{array}{c}(hu+n{)}^2+m^2=lx{z}^{\prime 2}\\ n^2+m^2=h{l}^2\end{array}$$
联立解得：$n=\frac{(lx{z}^{\prime 2}-h{l}^2-h{u}^2)}{2\ast hu}$
于是可得beta=-arccos(n/hl)

求解alpha角

同样几何关系描述如下：

易得
alpha_xz’ = -arctan(x/lyz)
alpha_off = arccos( (hu+n) / lxz’ )
则：
alpha = alpha_xz’+alpha_off

matlab代码实现

输入为足末端点坐标（x,y,z），输出为三个关节的角度
函数如下：

function [gamma,alfa,beta] = xyz(x,y,z)
h=0.15;
hu=0.35;
hl=0.382;
dyz=sqrt(y.^2+z.^2);
lyz=sqrt(dyz.^2-h.^2);
gamma_yz=-atan(y/z);
gamma_h_offset=-atan(h./lyz);
gamma=gamma_yz-gamma_h_offset;
%
lxzp=sqrt(lyz.^2+x.^2);
n=(lxzp.^2-hl.^2-hu.^2)/(2*hu);
beta=-acos(n/hl);

%
alfa_xzp=-atan(x/lyz);
alfa_off=acos((hu+n)/lxzp);
alfa=alfa_xzp+alfa_off;

%输出角度为弧度
end

课程链接：四足机器人控制与仿真入门