四足机器人站立姿态解算-几何方法

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

参数定义

定义机器人足端在长度方向的距离为l，在宽度方向的距离为w，两前足基座原点之间的距离为b。

O为地面坐标系原点，位于足端位置的对角线交点；
O’为身体部分几何中心。

姿态解算

提示：未作特殊说明情况下，以下向量均表示在世界坐标系下的向量，即O为坐标原点
对于1号腿，由几何关系易得：
$$\overrightarrow{O{B}_1}+\overrightarrow{B_1{A}_1}=\overrightarrow{O{O}^{\prime }}+\overrightarrow{O^{\prime }{A}_1}$$
由于$\overrightarrow{O^{\prime }{A}_1}$为世界坐标系下的向量，可以根据机体坐标系下的向量乘以旋转矩阵得到，即
$$\overrightarrow{O^{\prime }{A}_1}=R\cdot {\overrightarrow{O^{\prime }{A}_1}}_{机体}$$
则
$$\overrightarrow{A_1{B}_1}=-\overrightarrow{O{O}^{\prime }}-R\cdot {\overrightarrow{O^{\prime }{A}_1}}_{机体}+\overrightarrow{O{B}_1}$$
由于在一条腿上，坐标原点位于关节位置，即A点，因此向量AB即代表B点在该坐标系下的坐标(x,y,z)，原因可见上一篇文章四足机器人单腿逆运动学解-几何方法
同理可得四条腿足端坐标公式：
$$\overrightarrow{A_n{B}_n}=-\overrightarrow{O{O}^{\prime }}-R\cdot {\overrightarrow{O^{\prime }{A}_n}}_{机体}+\overrightarrow{O{B}_n}(n=1,2,3,4)$$

matlab程序

输入为机器狗期望的位置和姿态，输出为四个脚的位置坐标。

function [rb_x,rb_y,rb_z,rf_x,rf_y,rf_z,lb_x,lb_y,lb_z,lf_x,lf_y,lf_z] = pose_control(row,pitch,yaw,pos_x,pos_y,pos_z)
b=0.4;
l=0.8;
w=0.7;
h=0.732;
R=row*pi/180   ;
P=pitch*pi/180 ;
Y=yaw*pi/180   ;
pos=[pos_x,pos_y,pos_z]';
rotx=([[ 1,        0,         0      ]
       [ 0,        cos(R),   -sin(R) ]
       [ 0,        sin(R),    cos(R) ]]);
roty=([[ cos(P),   0,        -sin(P) ]
       [ 0,        1,         0      ]
       [ sin(P),   0,         cos(P) ]]);
rotz=([[ cos(Y),  -sin(Y),    0      ]
       [ sin(Y),   cos(Y),    0      ]
       [ 0,        0,         1      ]]);
rot_mat = rotx * roty * rotz;
%结构参数
  body_struct = ([[ l / 2,  -b / 2,  h]     
                 [  l / 2, b / 2,   h]     
                 [ -l / 2,  b / 2,    h]     
                 [ -l / 2, -b / 2,   h]])'; 
             
 footpoint_struct = ([[ l/2,   -w/2,  0]
                     [  l/2,   w/2,  0]
                     [  -l/2,    w/2,  0]
                     [  -l/2,    -w/2,  0]])';
leg_pose = zeros(3, 4);
for i= 1:4
    leg_pose(:,i) =  pos + rot_mat * body_struct(:, i) - footpoint_struct(:, i);
end

 rb_x = (leg_pose(1, 1));
 rb_y = -(leg_pose(2, 1));
 rb_z = -(leg_pose(3, 1));
 rf_x = (leg_pose(1, 2));
 rf_y = (leg_pose(2, 2));
 rf_z = -(leg_pose(3, 2));
 lb_x = (leg_pose(1, 3));
 lb_y = (leg_pose(2, 3));
 lb_z = -(leg_pose(3, 3));
 lf_x = (leg_pose(1, 4));
 lf_y = -(leg_pose(2, 4));
 lf_z = -(leg_pose(3, 4));
end

参考文献：
四足机器人的站立姿态控制原理