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

一.运动学正解已知两个关节的角度和以及连杆长度求足端的位置坐标。步骤分解建立坐标系以髋关节为原点向上为轴正方向向右为轴正方向。大腿的位置大腿长度为髋关节角度为逆时针为正。大腿末端膝关节的坐标小腿的位置小腿长度为膝关节角度为相对于大腿的旋转角度。脚底的坐标相对于膝关节脚底总坐标正解就是已知关节角度算末端的位置知道你的大腿和小腿弯了多少度计算脚能伸到哪里。二、逆运动学Inverse Kinematics问题已知足端的目标位置x, y和大小腿长度求两个关节的角度和。在计算过程中需要求得的角度还有()求膝关节角度θ2勾股定理计算从髋关节到脚底的距离设x,y坐标已知从足端坐标B到原点O 做一条线,设为图2中的L根据勾股定理可得或者如果两杆之和小于a或两杆差值大于a则目标位置不可达也就是说逆解存在的必要条件为。余弦定理求膝关节角度 θ₂将代入式移项并代入勾股定理解出最后求出求髋关节角度 θ1余弦定理已知三角形的三条边长可以使用反余弦函数求解三角形中某个角。反正切求得当x0时当x0时当x0时(自己画图理解下~)三、摆线运动曲线一个圆在直线上做纯滚动圆上边界固定一点所绘的轨迹叫摆线。设圆心半径为点初始位置为圆的正上方设圆转过的角度或则圆心水平位移距离为。机器人足端摆线方程为 步长足端起点-足端终点抬腿高度。摆动相周期内从0~faai为摆动相占整个周期的比例支撑相时 则为y高度始终不变为0四、代码实现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); % 角度范围 Ts2; fai0.5; % xs-35; % xf65; xs240; xf2-10; h50; 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_modTs*fai sigma2*pi*t_mod/fai/Ts; zep2h*(1-cos(sigma))/2zs; xep3(xf2-xs2)*(sigma-sin(sigma))/(2*pi)xs2; xep4(xs2-xf2)*(sigma-sin(sigma))/(2*pi)xf2; y3zep2; y4zs; end if t_modTs*fai t_modTs sigma2*pi*(t_mod-(Ts*fai))/fai/Ts; zep2h*(1-cos(sigma))/2zs; xep3(xs2-xf2)*(sigma-sin(sigma))/(2*pi)xf2; xep4(xf2-xs2)*(sigma-sin(sigma))/(2*pi)xs2; y3zs; y4zep2; end %plot([0, xep], [0, y], bo-, LineWidth, 2) %求φ角度 fail3 acos((xep3.^2 y3.^2 L.^2 - Ls.^2) /(2 *L*sqrt(xep3.^2y3.^2))); fail4 acos((xep4.^2 y4.^2 L.^2 - Ls.^2) /(2 *L*sqrt(xep4.^2y4.^2))); if xep30 sita3abs(atan(y3/xep3))-fail3; end if xep3 0 sita3pi-abs(atan(y3/xep3))-fail3; end if xep30 sita3pi-1.5707-fail3; end if xep40 sita4abs(atan(y4/xep4))-fail4; end if xep4 0 sita4pi-abs(atan(y4/xep4))-fail4; end if xep40 sita4pi-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)); Lx3L*cos(sita3); Ly3L*sin(-sita3); Lx4L*cos(sita4); Ly4L*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, Ly330, sprintf(小腿角度: %.1f°, fail3_deg), Color, r, FontSize, 12); text(Lx4, Ly430, 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