八自由度四足机器人运动学正解及逆解(附代码)

已知两个关节的角度（θ₁和θ₂），求脚底的位置坐标（x, y）。

阿阿阿阿阿里郎

1578人浏览 · 2025-05-24 23:19:29

阿阿阿阿阿里郎 · 2025-05-24 23:19:29 发布

一.运动学正解

已知两个关节的角度（ $\theta _{1}$ 和 $\theta_{2}$ ），以及连杆长度，求足端的位置坐标 $\left \{ x,y \right \}$ 。

步骤分解：
建立坐标系：以髋关节为原点，向上为 $y$ 轴正方向，向右为 $x$ 轴正方向。
大腿的位置：
- 大腿长度为 $l_{1}$ ，髋关节角度为 $\theta_{1}$ ，逆时针为正。
- 大腿末端（膝关节）的坐标：
  $x_{1}=l_{1}\cdot \cos\theta _{1}$ $y_{1}=l_{1}\cdot \sin\theta _{1}$
小腿的位置：
- 小腿长度为 $l_{2}$ ，膝关节角度为 $\theta _{2}$ （相对于大腿的旋转角度）。
- 脚底的坐标（相对于膝关节）：
  - $x_{2}=l_{2}\cdot \cos\left \{ \theta _{1}+\theta _{2} \right \}$ $y_{2}=l_{2}\cdot \sin\left \{ \theta _{1}+\theta _{2} \right \}$
脚底总坐标：
$x=l_{1}\cdot \cos\theta _{1}+l_{2}\cdot \cos\left \{ \theta _{1}+\theta _{2} \right \}$ $y=l_{1}\cdot \sin\theta _{1}+l_{2}\cdot \sin\left \{ \theta _{1}+\theta _{2} \right \}$
正解就是已知关节角度，算末端的位置，知道你的大腿和小腿弯了多少度，计算脚能伸到哪里。

二、逆运动学（Inverse Kinematics）

问题：已知足端的目标位置（x, y）和大小腿长度，求两个关节的角度（ $\theta _{1}$ 和 $\theta_{2}$ ）。在计算过程中需要求得的角度还有：( $\varphi\,, \beta\,,\gamma$ )
求膝关节角度θ2：
勾股定理：计算从髋关节到脚底的距离：
- 设x,y坐标已知，从足端坐标B到原点O 做一条线,设为 $a$ （图2中的L），根据勾股定理可得：
  或者
  - 如果 $a>L1+L2$ （两杆之和小于a）或 $a<|L1-L2|$ （两杆差值大于a），则目标位置不可达，也就是说逆解存在的必要条件为： $|L_{1}-L_{2}|\leqslant a\leqslant L_{1}+L_{2}$ 。
余弦定理：求膝关节角度 θ₂：
- $a^2 = L_1^2 + L_2^2 - 2 \cdot L_1 \cdot L_2 \cdot \cos(\beta)$
- 将 $\beta = 180^\circ - \theta_2$ 代入式： $a^2 = L_1^2 + L_2^2 - 2 \cdot L_1 \cdot L_2 \cdot \cos(180^\circ - \theta_2)$
移项并代入勾股定理解出：
最后求出：
求髋关节角度 θ1：
余弦定理，已知三角形的三条边长，可以使用反余弦函数求解三角形中某个角。
反正切求得： $\gamma =\arctan({\frac{y}{x}})$

当x>0时： $\theta_{1}= \arctan({\frac{y}{x}}) -\varphi$
当x<0时： $\theta_{1}=\pi -\varphi -\arctan({\frac{y}{x}})$
当x=0时： $\theta_{1}=\frac{\pi }{2}-\varphi$ (自己画图理解下~)

三、摆线运动曲线

一个圆在直线上做纯滚动，圆上边界固定一点 $P$ 所绘的轨迹，叫摆线。

设圆心半径为 $R$ ，点 $P$ 初始位置为圆的正上方，设圆转过的角度 $\theta$ 或 $t$ ，则圆心水平位移距离为 $R\theta$ 。

$P=\left\{\begin{matrix}x=R(\theta -sin\theta ) \\ y=R(1 - cos\theta ) \end{matrix}\right.$

机器人足端摆线方程为：步长 $L$ =足端起点-足端终点 $=x_{s}-x_{f}$ ，抬腿高度 $h$ 。

摆动相周期内 $\theta$ 从0~ $2\pi$ ， $\theta(t) =\frac{2\pi t}{T_{swing}}$ ， $T_{swing}=faai\cdot T$ ，faai为摆动相占整个周期 $T$ 的比例

$\left\{\begin{matrix}x(\theta )=\frac{L}{2\pi }(\theta -sin\theta )+x_{s} \\ y(\theta )=\frac{h}{2}(1-cos\theta ) \end{matrix}\right.$

支撑相时则为，y高度始终不变为0

$x(\theta )=\frac{L}{2\pi }(\theta -sin\theta )+x_{f}$

四、代码实现

4.1 运动学逆解

/*默认速度 步长 设置*/
float l1 = 200; //大腿长(mm)
float l2 = 160; //小腿长(mm)
float h = 30;  //抬腿高度 设置
float xf = 40; //xf为终点足端坐标
float xs = -20; // xs起始足端坐标

float Ts = 1;          //周期
float faai = 0.5;      //占空比
float speed = 0.025; //步频调节

//=============一些中间变量=============
float t = 0;//时间变量
float init_x = 0;
float init_y = -290;//腿高度
float x1 = init_x;//腿1x坐标
float y1 = init_y;//腿1y坐标
float x2 = init_x;
float y2 = init_y;
float x3 = init_x;
float y3 = init_y;
float x4 = init_x;
float y4 = init_y;

float ges_x_1 = 0;
float ges_x_2 = 0;
float ges_x_3 = 0;
float ges_x_4 = 0;

float ges_y_1 = init_y;
float ges_y_2 = init_y;
float ges_y_3 = init_y;
float ges_y_4 = init_y;


void calculateLegAngles(float x, float y, float* hip_angle, float* knee_angle) {
    x = -x;
    //求解θ2
    *knee_angle = acos((x * x + y * y - l1 * l1 - l2 * l2) / (2 * l1 * l2));
    求解φ
    float fai = acos((l1 * l1 + x * x + y * y - l2 * l2) / (2 * l1 * sqrt(x * x + y * y)));
    求解θ1
    if (x > 0) {
        *hip_angle = abs(atan(y / x)) - fai;
    } else if (x < 0) {
        *hip_angle = pi - abs(atan(y / x)) - fai;
    } else {
        *hip_angle = pi - 1.5707 - fai;
    }
    //转为角度
    *knee_angle = 180 * *knee_angle / pi;
    *hip_angle = 180 * *hip_angle / pi;
}

void IK(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
    float hip_angle1, knee_angle1;
    float hip_angle2, knee_angle2;
    float hip_angle3, knee_angle3;
    float hip_angle4, knee_angle4;

    calculateLegAngles(x1, y1, &hip_angle1, &knee_angle1);
    calculateLegAngles(x2, y2, &hip_angle2, &knee_angle2);
    calculateLegAngles(x3, y3, &hip_angle3, &knee_angle3);
    calculateLegAngles(x4, y4, &hip_angle4, &knee_angle4);


    angle_output(hip_angle1, knee_angle1, hip_angle2, knee_angle2, 
                 hip_angle3, knee_angle3, hip_angle4, knee_angle4);
}

4.2 步态生成代码

//默认高度参数
#define BASE_HEIGHT 280
#define HEIGHT_MIN 170
#define HEIGHT_MAX 330
#define X_ZERO 50

/*默认速度 步长 设置*/
float l1 = 200; //大腿长(mm)
float l2 = 160; //小腿长(mm)
float h = 30;  //抬腿高度 设置
float xf = 40; //xf为终点足端坐标
float xs = -20; // xs起始足端坐标

float Ts = 1;          //周期
float faai = 0.5;      //占空比
float speed = 0.025; //步频调节


void GaitGenerator::trot(float t, float direction, four_leg::LegAngles& leg_angles,
                         StableHeight& stable_adjust) {
    // 根据方向设置步态参数
    float xf = (direction > 0) ? 30.0f : 20.0f;//终点坐标
    float xs = (direction > 0) ? 10.0f : -20.0f;//起点坐标
    int sign = (direction > 0) ? 1 : -1;
    
    float sigma, zep, xep_b, xep_z = 0;
    
    if (t < config_.Ts * config_.faai) {
        sigma = 2 * M_PI * t / (config_.faai * config_.Ts);
        zep = config_.h * (1 - cos(sigma)) / 2;
        xep_b = (xf - xs) * ((sigma - sin(sigma)) / (2 * M_PI)) + xs;
        xep_z = (xs - xf) * ((sigma - sin(sigma)) / (2 * M_PI)) + xf;

        // 左前腿
        LeftFront.y = BASE_HEIGHT - zep ;
        // 右前腿
        RightFront.y = BASE_HEIGHT;
        // 右后腿
        RightBack.y = BASE_HEIGHT- zep;
        // 左后腿
        LeftBack.y = BASE_HEIGHT ;
        
        
        // X坐标设置
        LeftFront.x = -xep_z * sign + X_ZERO;
        RightFront.x = -xep_b * sign + X_ZERO;
        RightBack.x = -xep_z * sign - X_ZERO;
        LeftBack.x = -xep_b * sign - X_ZERO;
    }
    else if (t >= config_.Ts * config_.faai && t < config_.Ts) {
        sigma = 2 * M_PI * (t - config_.Ts * config_.faai) / (config_.faai * config_.Ts);
        zep = config_.h * (1 - cos(sigma)) / 2;
        xep_b = (xf - xs) * ((sigma - sin(sigma)) / (2 * M_PI)) + xs;
        xep_z = (xs - xf) * ((sigma - sin(sigma)) / (2 * M_PI)) + xf;

        // 左前腿
        LeftFront.y = BASE_HEIGHT;
        // 右前腿
        RightFront.y = BASE_HEIGHT - zep;
        // 右后腿
        RightBack.y = BASE_HEIGHT;
        // 左后腿
        LeftBack.y = BASE_HEIGHT - zep;
       
         
        // X坐标设置
        LeftFront.x = -xep_b * sign + X_ZERO; //sign为前进后退标志位
        RightFront.x = -xep_z * sign + X_ZERO;
        RightBack.x = -xep_b * sign - X_ZERO;
        LeftBack.x = -xep_z * sign - X_ZERO;
    }
    
    // 计算逆运动学
    kinematics_.IK(
        LeftFront.x, LeftFront.y,
        RightFront.x, RightFront.y,      
        RightBack.x, RightBack.y,
        LeftBack.x, LeftBack.y,    
        leg_angles
    );
}

4.3 MATLAB单腿运动可视化

% 设置参数
L = 200;          % 腿的长度
Ls =160;          %小腿长度
Tmax = 100; % 动画帧数
theta_range = linspace(0, 2*pi,Tmax); % 角度范围
theta_s_range= linspace(0, 2*pi,Tmax); % 角度范围

Ts=2;
fai=0.5;
% xs=-35;
% xf=65;
xs2=40;
xf2=-10;
h=50;
zs=-290;
% 创建图形窗口
figure;
set(gcf, 'Position', [100, 100, 1000, 800]); % 增大窗口尺寸以便显示更多信息

for t= 0:0.04:5
   t_mod = mod(t, 1); % 取模，使 t 在 0 到 1 之间循环
   if t_mod<Ts*fai 
       sigma=2*pi*t_mod/fai/Ts;
       zep2=h*(1-cos(sigma))/2+zs;
       xep3=(xf2-xs2)*(sigma-sin(sigma))/(2*pi)+xs2;
       xep4=(xs2-xf2)*(sigma-sin(sigma))/(2*pi)+xf2;
       y3=zep2;
       y4=zs;
   end
   if  t_mod>Ts*fai && t_mod<Ts 
       sigma=2*pi*(t_mod-(Ts*fai))/fai/Ts;
       zep2=h*(1-cos(sigma))/2+zs;
       xep3=(xs2-xf2)*(sigma-sin(sigma))/(2*pi)+xf2;
       xep4=(xf2-xs2)*(sigma-sin(sigma))/(2*pi)+xs2;
       y3=zs;
       y4=zep2;
   end
   
   %plot([0, xep], [0, y], 'bo-', 'LineWidth', 2)
   %求φ角度
   fail3= acos((xep3.^2 + y3.^2 + L.^2 - Ls.^2) /(2 *L*sqrt(xep3.^2+y3.^2)));
   fail4= acos((xep4.^2 + y4.^2 + L.^2 - Ls.^2) /(2 *L*sqrt(xep4.^2+y4.^2)));
   if xep3>0
       sita3=abs(atan(y3/xep3))-fail3;
   end
   if xep3 <0
       sita3=pi-abs(atan(y3/xep3))-fail3;
   end 
   if xep3==0
       sita3=pi-1.5707-fail3;
   end
   
      if xep4>0
       sita4=abs(atan(y4/xep4))-fail4;
   end
   if xep4 <0
       sita4=pi-abs(atan(y4/xep4))-fail4;
   end 
   if xep4==0
       sita4=pi-1.5707-fail4;
   end

   %求θ2
   knee_angle3 = acos((xep3.^2 + y3.^2 - L.^2 - Ls.^2) /(2 *L*Ls));
   knee_angle4 = acos((xep4.^2 + y4.^2 - L.^2 - Ls.^2) /(2 *L*Ls));
   
   Lx3=L*cos(sita3);
   Ly3=L*sin(-sita3);
   Lx4=L*cos(sita4);
   Ly4=L*sin(-sita4);
   
   clf; % 清空当前图形
   hold on;
   plot([0, Lx3], [0, Ly3], 'bo-', 'LineWidth', 20)
   plot([Lx3,xep3],[Ly3,y3],'ro-','LineWidth',15);
   plot([0, Lx4], [0, Ly4], 'bo-', 'LineWidth', 20)
   plot([Lx4,xep4],[Ly4,y4],'ro-','LineWidth',15); 
   
   center3=[xep3,y3];
   center4=[xep4,y4];
   viscircles(center3, 40, 'Color', 'g', 'LineWidth', 2);
   viscircles(center4, 40, 'Color', 'g', 'LineWidth', 2);
   
   % ================ 新增内容：显示角度和坐标信息 ================
   % 转换角度为度数
   sita3_deg = rad2deg(sita3);
   sita4_deg = rad2deg(sita4);
   fail3_deg = rad2deg(knee_angle3);
   fail4_deg = rad2deg(knee_angle4);
   
   % 在关节位置显示角度信息
   text(0, -20, sprintf('大腿角度: %.1f°', sita3_deg), 'Color', 'b', 'FontSize', 12);
   text(0, -40, sprintf('大腿角度: %.1f°', sita4_deg), 'Color', 'b', 'FontSize', 12);
   
   text(Lx3, Ly3+30, sprintf('小腿角度: %.1f°', fail3_deg), 'Color', 'r', 'FontSize', 12);
   text(Lx4, Ly4+30, sprintf('小腿角度: %.1f°', fail4_deg), 'Color', 'r', 'FontSize', 12);
   
   % 在足端显示坐标信息
   text(center3(1)+20, center3(2)+20, sprintf('足端: (%.1f, %.1f)', center3(1), center3(2)), 'Color', 'k', 'FontSize', 12);
   text(center4(1)+20, center4(2)+20, sprintf('足端: (%.1f, %.1f)', center4(1), center4(2)), 'Color', 'k', 'FontSize', 12);
   
   % 在顶部显示时间信息
   text(50, 0, sprintf('时间: %.2f s', t), 'Color', 'm', 'FontSize', 14, 'FontWeight', 'bold');
   
   % 添加图例说明
   legend({'大腿', '小腿', '足端'}, 'Location', 'southeast');
   % ========================================================
   
        % 添加标签和标题
    axis([-100, 300, -440, 20]);
    xlabel('X轴');
    ylabel('Z轴');
    title('四足机器人腿部运动动画');
     axis equal;
    % 暂停一小段时间，以便观察动画效果
    pause(0.01);
end