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

最新推荐文章于 2024-12-01 11:24:10 发布

童年吹梦

最新推荐文章于 2024-12-01 11:24:10 发布

阅读量1.1k

点赞数

分类专栏：四足机器人控制与仿真入门文章标签：机器人 matlab

本文链接：https://blog.youkuaiyun.com/qq_46304090/article/details/126450272

版权

四足机器人控制与仿真入门专栏收录该内容

4 篇文章

订阅专栏

这篇博客介绍了如何使用Matlab编程实现四足机器人的站立姿态解算，通过几何方法参数化并计算各足端在世界坐标系中的位置，包括腿部坐标转换和脚点计算。读者将学习到关键的参数定义、姿态计算公式以及实际的脚部位置计算函数，适合对机器人控制感兴趣的开发者和研究者。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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

参数定义

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

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

姿态解算

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