四足机器狗匀速行走的matlab模拟与仿真

目录1.程序整体架构2.参数配置模块2.1 机器狗物理尺寸参数2.2 步态参数2.3 步态相位3.运动轨迹生成3.1 身体水平位移3.2 身体垂直高度3.3 身体俯仰角与滚转角3.4 髋关节位置解算3.5 足端轨迹规划3.6 摆动相足端轨迹3.7 支撑相足端轨迹4.matlab完整程序1.程序整体架构程序采用模块化设计分为6大核心模块各模块独立解耦、依次执行架构清晰且易于扩展初始化与参数配置模块定义机器狗物理尺寸、步态参数、仿真环境参数轨迹预计算模块核心模块生成身体运动轨迹、足端摆线轨迹、髋关节 / 膝关节坐标逆运动学求解模块基于余弦定理计算腿部关节角度确定膝关节位置3D动画渲染模块实时绘制机器狗躯干、头部、四肢、地面实现动态可视化步态分析模块绘制足端轨迹、步态时序、身体姿态、运动速度曲线辅助函数模块封装长方体绘制、旋转渲染等通用图形函数。2.参数配置模块2.1 机器狗物理尺寸参数该部分定义机器狗的刚体结构尺寸属于运动学建模的基础几何参数所有长度单位均为米dog.body_length 0.6; % 躯干长 dog.body_width 0.25; % 躯干宽 dog.body_height 0.08; % 躯干厚 dog.body_z 0.35; % 躯干离地高度 dog.upper_leg 0.15; % 大腿长(L1) dog.lower_leg 0.15; % 小腿长(L2)其中躯干参数定义机器狗主体的长宽厚以及默认离地高度保证行走时躯干不触地腿部参数大腿与小腿等长0.15m是二连杆机构的核心长度直接决定逆运动学求解结果头部/尾巴参数仅用于动画渲染不影响运动控制。2.2 步态参数步态是四足机器人行走的灵魂程序采用真实犬类四拍步行步态核心参数定义gait.stride_length 0.14; % 步幅单步前进距离 gait.step_height 0.06; % 抬脚高度摆动期最大离地高度 gait.frequency 2.0; % 步频每秒行走步数 gait.speed 步态速度公式; % 匀速前进速度 gait.phase [0.5, 0.0, 0.75, 0.25]; % 四条腿相位差机器狗匀速前进速度由步幅和步频直接决定公式四足机器狗步行的核心是相位差控制保证同一时间只有一条腿处于摆动期实现稳定支撑。2.3 步态相位四足机器狗步行的核心是相位差控制保证同一时间只有一条腿处于摆动期实现稳定支撑。腿编号定义1左前(LF)、2 右前(RF)、3 左后(LH)、4右后(RH)相位数组[0.5, 0.0, 0.75, 0.25]对应四条腿的运动相位相位间隔25%确保机器狗始终有三条腿支撑地面避免倾倒。3.运动轨迹生成机器狗躯干做匀速直线运动 仿生姿态波动模拟真实动物行走时的身体起伏、俯仰和侧滚。3.1 身体水平位移其中t为当前仿真时间v为匀速速度保证机器狗沿X轴匀速前进。3.2 身体垂直高度真实动物行走时身体会轻微上下起伏程序采用二倍频正弦曲线模拟3.3 身体俯仰角与滚转角模拟行走时躯干的轻微俯仰和侧摆增强仿真真实性角度单位为弧度小角度姿态变化不会影响行走稳定性。3.4 髋关节位置解算髋关节固定在躯干上随躯干同步运动同时叠加姿态影响该步骤保证髋关节随躯干姿态实时调整符合刚体运动规律。3.5 足端轨迹规划足端轨迹是四足机器人行走的关键程序采用摆线轨迹分为摆动相抬腿迈步和支撑相触地支撑两个阶段25%时间摆动75%时间支撑是步行步态的标准划分。3.6 摆动相足端轨迹摆动相实现[抬脚→向前→落地]的平滑运动X轴采用线性插值Z轴采用半正弦曲线保证离地高度平滑3.7 支撑相足端轨迹支撑相腿始终触地为身体提供支撑力X轴匀速向后滑动Z轴保持0触地摆线轨迹的优势运动平滑、无加速度突变、落地冲击小是四足机器人步行步态的最优轨迹之一。4.matlab完整程序%% 机器狗仿真 - 真实步态匀速行走无障碍物 clear; clc; close all; %% 参数设置 % 机器狗尺寸 dog.body_length 0.6; % 躯干长 dog.body_width 0.25; % 躯干宽 dog.body_height 0.08; % 躯干厚 dog.body_z 0.35; % 躯干离地高度 dog.upper_leg 0.15; % 大腿长 dog.lower_leg 0.15; % 小腿长 dog.head_len 0.12; dog.head_h 0.08; dog.tail_len 0.15; % 步态参数walk步态四拍节奏单腿依次迈出 gait.stride_length 0.14; % 步幅 gait.step_height 0.06; % 抬脚高度 gait.frequency 2.0; % 步频 Hz gait.speed gait.stride_length * gait.frequency; % 匀速前进速度 % Walk步态相位真实犬类左后→左前→右后→右前均匀间隔25% % 腿编号1左前 2右前 3左后 4右后 gait.phase [0.5, 0.0, 0.75, 0.25]; % LF, RF, LH, RH % 腿的安装位置相对身体中心 leg_mount [ dog.body_length/2, dog.body_width/2; % 左前 dog.body_length/2, -dog.body_width/2; % 右前 -dog.body_length/2, dog.body_width/2; % 左后 -dog.body_length/2, -dog.body_width/2]; % 右后 % 仿真参数 sim.dt 0.01; sim.duration 8.0; sim.t 0:sim.dt:sim.duration; sim.N length(sim.t); ground_len 12; %% 预计算轨迹 fprintf(生成步态轨迹...\n); body_x zeros(sim.N, 1); body_z zeros(sim.N, 1); body_pitch zeros(sim.N, 1); body_roll zeros(sim.N, 1); foot_pos zeros(sim.N, 4, 3); % N x 4legs x [x,y,z] knee_pos zeros(sim.N, 4, 3); hip_pos zeros(sim.N, 4, 3); for n 1:sim.N t sim.t(n); % 身体匀速前进 bx gait.speed * t; body_x(n) bx; % 统计有几条腿在摆动期 → 计算身体上下起伏 phase_vals zeros(4,1); swing_flags zeros(4,1); for leg 1:4 phi mod(gait.frequency * t gait.phase(leg), 1.0); phase_vals(leg) phi; swing_flags(leg) phi 0.25; % 前25%为摆动相后75%为支撑相 end % 身体自然起伏步频的2倍频 body_bob 0.005 * sin(2 * pi * gait.frequency * 2 * t); body_z(n) dog.body_z body_bob; % 身体轻微俯仰和侧摆 body_pitch(n) 0.02 * sin(2 * pi * gait.frequency * t); body_roll(n) 0.015 * sin(2 * pi * gait.frequency * t pi/4); bz body_z(n); for leg 1:4 phi phase_vals(leg); % 髋关节位置跟随身体 hx bx leg_mount(leg, 1); hy leg_mount(leg, 2); hz bz; % 加入身体俯仰影响 hz hz leg_mount(leg, 1) * sin(body_pitch(n)); % 加入侧摆影响 hz hz leg_mount(leg, 2) * sin(body_roll(n)); hip_pos(n, leg, :) [hx, hy, hz]; % 足端轨迹摆线轨迹 % 支撑相phi: 0.25~1.0脚在地面上向后滑动 % 摆动相phi: 0~0.25脚抬起向前迈步 % 足端相对髋关节的纵向偏移 if phi 0.25 % 摆动相从后方到前方 swing_ratio phi / 0.25; foot_dx gait.stride_length * (-0.5 swing_ratio); % 抬脚高度半正弦曲线 foot_dz gait.step_height * sin(pi * swing_ratio); else % 支撑相从前方匀速滑到后方 stance_ratio (phi - 0.25) / 0.75; foot_dx gait.stride_length * (0.5 - stance_ratio); foot_dz 0; end fx hx foot_dx; fy hy; fz foot_dz; % 脚的z坐标相对地面 foot_pos(n, leg, :) [fx, fy, fz]; % 逆运动学求膝关节 % 从髋关节到足端的向量 dx_leg fx - hx; dz_leg fz - hz; leg_reach sqrt(dx_leg^2 dz_leg^2); L1 dog.upper_leg; L2 dog.lower_leg; leg_reach min(leg_reach, L1 L2 - 0.001); leg_reach max(leg_reach, abs(L1 - L2) 0.001); % 余弦定理求膝关节角 cos_knee (L1^2 L2^2 - leg_reach^2) / (2 * L1 * L2); cos_knee max(min(cos_knee, 1), -1); % 髋关节角度 alpha atan2(-dz_leg, dx_leg); % 髋到脚的角度 cos_hip (L1^2 leg_reach^2 - L2^2) / (2 * L1 * leg_reach); cos_hip max(min(cos_hip, 1), -1); beta acos(cos_hip); hip_angle alpha beta; kx hx L1 * cos(hip_angle); ky hy; kz hz - L1 * sin(hip_angle); % 确保膝关节向前弯曲前腿或向后弯曲后腿 if leg 2 % 前腿膝关节偏后 if kx hx hip_angle alpha - beta; kx hx L1 * cos(hip_angle); kz hz - L1 * sin(hip_angle); end else % 后腿膝关节偏前 if kx hx hip_angle alpha - beta; kx hx L1 * cos(hip_angle); kz hz - L1 * sin(hip_angle); end end knee_pos(n, leg, :) [kx, ky, kz]; end end %% 3D动画 fprintf(播放动画...\n); fig figure(Position, [80, 80, 1200, 650], Color, w); for n 1:3:sim.N clf; bx body_x(n); bz body_z(n); by 0; % ---------- 地面 ---------- [Xg, Yg] meshgrid(linspace(bx-3, bx5, 80), linspace(-1.5, 1.5, 30)); Zg zeros(size(Xg)); surf(Xg, Yg, Zg, FaceColor, [0.55 0.78 0.38], EdgeColor, none, FaceAlpha, 0.8); hold on; % 草地纹理点 rng(42); gx bx - 2 7*rand(60,1); gy -1.2 2.4*rand(60,1); plot3(gx, gy, zeros(60,1)0.001, ., Color, [0.4 0.65 0.25], MarkerSize, 4); % ---------- 身体长方体---------- Ry [cos(body_pitch(n)) 0 sin(body_pitch(n)); 0 1 0; -sin(body_pitch(n)) 0 cos(body_pitch(n))]; Rx [1 0 0; 0 cos(body_roll(n)) -sin(body_roll(n)); 0 sin(body_roll(n)) cos(body_roll(n))]; R Rx * Ry; draw_body_rotated(bx, by, bz, dog.body_length, dog.body_width, dog.body_height, R, [0.25 0.45 0.85]); % ---------- 头部 ---------- head_offset R * [dog.body_length/2 dog.head_len/2; 0; 0.03]; hx_h bx head_offset(1); hy_h by head_offset(2); hz_h bz head_offset(3); % 头部微微点头 head_nod 0.01 * sin(2*pi*gait.frequency*sim.t(n) pi/3); draw_box_centered(hx_h, hy_h, hz_h head_nod, dog.head_len, dog.body_width*0.6, dog.head_h, [0.3 0.55 0.92]); % 耳朵 ear_w 0.02; ear_h 0.04; for side [-1, 1] ex hx_h dog.head_len*0.2; ey hy_h side * dog.body_width * 0.25; ez hz_h head_nod dog.head_h/2; draw_box_centered(ex, ey, ez ear_h/2, ear_w, ear_w, ear_h, [0.2 0.35 0.75]); end % 鼻子 plot3(hx_h dog.head_len/2 0.01, hy_h, hz_h head_nod, ko, MarkerSize, 4, MarkerFaceColor, k); % 眼睛 for side [-1, 1] plot3(hx_h dog.head_len*0.3, hy_h side*0.03, hz_h head_nod 0.02, ... ko, MarkerSize, 3, MarkerFaceColor, [0.1 0.1 0.1]); end % ---------- 尾巴 ---------- tail_wag 0.08 * sin(2*pi*gait.frequency*1.5*sim.t(n)); tail_base [bx - dog.body_length/2; by; bz 0.02]; tail_tip [tail_base(1) - dog.tail_len*0.7; tail_base(2) tail_wag; tail_base(3) dog.tail_len*0.7]; tail_mid [(tail_base(1)tail_tip(1))/2; (tail_base(2)tail_tip(2))/2 tail_wag*0.3; (tail_base(3)tail_tip(3))/2 0.02]; plot3([tail_base(1), tail_mid(1), tail_tip(1)], ... [tail_base(2), tail_mid(2), tail_tip(2)], ... [tail_base(3), tail_mid(3), tail_tip(3)], ... -, Color, [0.2 0.35 0.75], LineWidth, 3); % ---------- 四条腿 ---------- leg_colors [0.2 0.4 0.78; 0.2 0.4 0.78; 0.2 0.4 0.78; 0.2 0.4 0.78]; for leg 1:4 hx_l hip_pos(n, leg, 1); hy_l hip_pos(n, leg, 2); hz_l hip_pos(n, leg, 3); kx_l knee_pos(n, leg, 1); ky_l knee_pos(n, leg, 2); kz_l knee_pos(n, leg, 3); fx_l foot_pos(n, leg, 1); fy_l foot_pos(n, leg, 2); fz_l foot_pos(n, leg, 3); % 大腿 plot3([hx_l, kx_l], [hy_l, ky_l], [hz_l, kz_l], -, ... Color, leg_colors(leg,:), LineWidth, 4); % 小腿 plot3([kx_l, fx_l], [ky_l, fy_l], [kz_l, fz_l], -, ... Color, leg_colors(leg,:)*0.85, LineWidth, 3.5); % 膝关节 plot3(kx_l, ky_l, kz_l, o, Color, leg_colors(leg,:)*0.7, ... MarkerSize, 5, MarkerFaceColor, leg_colors(leg,:)*0.7); % 脚掌 plot3(fx_l, fy_l, fz_l, o, Color, [0.15 0.15 0.15], ... MarkerSize, 5, MarkerFaceColor, [0.2 0.2 0.2]); end % ---------- 视角与显示 ---------- xlabel(X (m)); ylabel(Y (m)); zlabel(Z (m)); title(sprintf(机器狗匀速行走 | 速度: %.2f m/s | 时间: %.2f s, gait.speed, sim.t(n)), FontSize, 13); axis equal; axis([bx-1.5, bx2.0, -1.0, 1.0, -0.05, 0.7]); view([-35, 22]); grid on; light(Position, [bx3, -3, 5], Style, infinite); light(Position, [bx-2, 3, 3], Style, infinite, Color, [0.3 0.3 0.3]); lighting gouraud; set(gca, FontSize, 10); drawnow; pause(0.01); end fprintf(动画完成行走距离: %.2f m\n, body_x(end)); %% 步态分析图 figure(Position, [100,100,1000,600], Color, w); % 足端轨迹侧视图 subplot(2,2,1); leg_names {左前腿,右前腿,左后腿,右后腿}; colors lines(4); for leg 1:4 fx_rel squeeze(foot_pos(:,leg,1)) - body_x; fz_rel squeeze(foot_pos(:,leg,3)); plot(fx_rel, fz_rel, -, Color, colors(leg,:), LineWidth, 1.5); hold on; end xlabel(相对身体纵向位移 (m)); ylabel(离地高度 (m)); title(足端轨迹身体坐标系); legend(leg_names, Location, best); grid on; % 步态时序图 subplot(2,2,2); t_plot sim.t(1:min(sim.N, round(2/sim.dt))); for leg 1:4 phi_t mod(gait.frequency * t_plot gait.phase(leg), 1.0); stance phi_t 0.25; % 支撑相1 for k 1:length(t_plot) if stance(k) plot([t_plot(k), t_plot(k)], [leg-0.4, leg0.4], -, Color, colors(leg,:), LineWidth, 2); end hold on; end end yticks(1:4); yticklabels(leg_names); xlabel(时间 (s)); title(步态时序图色块支撑相); xlim([0, t_plot(end)]); grid on; % 身体高度变化 subplot(2,2,3); plot(sim.t, body_z, b-, LineWidth, 1.5); hold on; plot(sim.t, body_z dog.body_height/2, r--, LineWidth, 1); plot(sim.t, body_z - dog.body_height/2, r--, LineWidth, 1); xlabel(时间 (s)); ylabel(高度 (m)); title(身体质心高度变化); grid on; legend(质心, 躯干上/下沿); % 身体前进速度验证匀速 subplot(2,2,4); vx diff(body_x) / sim.dt; plot(sim.t(2:end), vx, g-, LineWidth, 1.5); hold on; yline(gait.speed, r--, sprintf(目标速度 %.3f m/s, gait.speed), LineWidth, 1.5); xlabel(时间 (s)); ylabel(速度 (m/s)); title(前进速度); grid on; ylim([0, gait.speed*1.5]); sgtitle(机器狗步态分析, FontSize, 14); %% 辅助函数 function draw_box_centered(cx, cy, cz, dx, dy, dz, color) x cx - dx/2; y cy - dy/2; z cz - dz/2; vertices [x,y,z; xdx,y,z; xdx,ydy,z; x,ydy,z; x,y,zdz; xdx,y,zdz; xdx,ydy,zdz; x,ydy,zdz]; faces [1 2 3 4; 5 6 7 8; 1 2 6 5; 2 3 7 6; 3 4 8 7; 4 1 5 8]; patch(Vertices, vertices, Faces, faces, FaceColor, color, EdgeColor, [0.1 0.1 0.1]*0.5, FaceAlpha, 0.95, LineWidth, 0.5); end function draw_body_rotated(cx, cy, cz, dx, dy, dz, R, color) % 生成以原点为中心的长方体顶点旋转后平移 verts_local [-dx/2, -dy/2, -dz/2; dx/2, -dy/2, -dz/2; dx/2, dy/2, -dz/2; -dx/2, dy/2, -dz/2; -dx/2, -dy/2, dz/2; dx/2, -dy/2, dz/2; dx/2, dy/2, dz/2; -dx/2, dy/2, dz/2]; verts_rot (R * verts_local); verts_rot(:,1) verts_rot(:,1) cx; verts_rot(:,2) verts_rot(:,2) cy; verts_rot(:,3) verts_rot(:,3) cz; faces [1 2 3 4; 5 6 7 8; 1 2 6 5; 2 3 7 6; 3 4 8 7; 4 1 5 8]; patch(Vertices, verts_rot, Faces, faces, FaceColor, color, EdgeColor, [0.1 0.1 0.3], FaceAlpha, 0.95, LineWidth, 0.5); end仿真图如下本程序为机械狗的行走数值模拟可以用于机械狗行走强化学习的训练样本数据。