221 lines
7.6 KiB
Matlab
221 lines
7.6 KiB
Matlab
function quadruped_leg_visualization()
|
||
% 四足机器人单腿运动学可视化
|
||
|
||
% 清除之前的图形
|
||
close all;
|
||
clc;
|
||
|
||
% 定义腿部参数 (单位: 米)
|
||
leg_params.upper_leg_length = 0.15;
|
||
leg_params.lower_leg_length = 0.15;
|
||
leg_params.hip_offset_x = 0.05;
|
||
leg_params.hip_offset_z = -0.02;
|
||
leg_params.hip_yaw_offset = 0.01;
|
||
leg_params.knee_sign = 1; % 1或-1，取决于机械设计
|
||
|
||
% 目标位置 (在髋关节坐标系中)
|
||
target_pos = [0.2, 0.1, -0.25]; % x, y, z
|
||
|
||
% 计算逆运动学 (右腿hip_yaw_sign=1，左腿=-1)
|
||
joint_angles = leg_inverse_kinematics(target_pos, leg_params, 1);
|
||
|
||
% 显示计算结果
|
||
disp('关节角度 (弧度):');
|
||
disp(['髋关节偏航 (hip yaw): ', num2str(joint_angles(1)), ' rad']);
|
||
disp(['髋关节滚动 (hip roll): ', num2str(joint_angles(2)), ' rad']);
|
||
disp(['髋关节俯仰 (hip pitch): ', num2str(joint_angles(3)), ' rad']);
|
||
disp(['膝关节 (knee): ', num2str(joint_angles(4)), ' rad']);
|
||
|
||
disp('关节角度 (度):');
|
||
disp(['髋关节偏航 (hip yaw): ', num2str(rad2deg(joint_angles(1))), '°']);
|
||
disp(['髋关节滚动 (hip roll): ', num2str(rad2deg(joint_angles(2))), '°']);
|
||
disp(['髋关节俯仰 (hip pitch): ', num2str(rad2deg(joint_angles(3))), '°']);
|
||
disp(['膝关节 (knee): ', num2str(rad2deg(joint_angles(4))), '°']);
|
||
|
||
% 可视化腿部
|
||
visualize_leg(joint_angles, leg_params, target_pos);
|
||
end
|
||
|
||
function joint_angles = leg_inverse_kinematics(target_pos, leg_params, hip_yaw_sign)
|
||
% 逆运动学计算函数
|
||
|
||
% 从参数字典中提取参数
|
||
L1 = leg_params.upper_leg_length;
|
||
L2 = leg_params.lower_leg_length;
|
||
offset_x = leg_params.hip_offset_x;
|
||
offset_z = leg_params.hip_offset_z;
|
||
yaw_offset = leg_params.hip_yaw_offset;
|
||
knee_sign = leg_params.knee_sign;
|
||
|
||
% 第一步：计算髋关节偏航角 (hip yaw)
|
||
x = target_pos(1);
|
||
y = target_pos(2);
|
||
z = target_pos(3);
|
||
|
||
x = x - offset_x;
|
||
z = z - offset_z;
|
||
|
||
% 使用迭代法处理yaw offset
|
||
theta_yaw = atan2(hip_yaw_sign * y, x);
|
||
for i = 1:3
|
||
% 计算yaw旋转后的位置
|
||
x_rot = x * cos(theta_yaw) + hip_yaw_sign * y * sin(theta_yaw);
|
||
y_rot = -x * sin(theta_yaw) + hip_yaw_sign * y * cos(theta_yaw);
|
||
|
||
% 考虑yaw offset
|
||
x_rot = x_rot - yaw_offset * (1 - cos(theta_yaw));
|
||
y_rot = y_rot + yaw_offset * sin(theta_yaw);
|
||
|
||
theta_yaw = atan2(hip_yaw_sign * y_rot, x_rot);
|
||
end
|
||
|
||
% 第二步：计算髋关节滚转角 (hip roll) 和俯仰角 (hip pitch)
|
||
x_rot = x * cos(theta_yaw) + hip_yaw_sign * y * sin(theta_yaw) - yaw_offset;
|
||
z_rot = z;
|
||
|
||
% 计算髋关节滚转角
|
||
L = sqrt(x_rot^2 + z_rot^2);
|
||
theta_roll = atan2(z_rot, x_rot);
|
||
|
||
% 计算髋关节俯仰角和膝关节角度
|
||
D = (L^2 - L1^2 - L2^2) / (2 * L1 * L2);
|
||
D = max(min(D, 1.0), -1.0); % 确保数值在有效范围内
|
||
|
||
theta_knee = knee_sign * acos(D);
|
||
|
||
alpha = atan2(L2 * sin(theta_knee), L1 + L2 * cos(theta_knee));
|
||
theta_pitch = alpha + theta_roll;
|
||
|
||
% 返回关节角度 (hip_yaw, hip_roll, hip_pitch, knee)
|
||
joint_angles = [theta_yaw, theta_roll, theta_pitch, theta_knee];
|
||
end
|
||
|
||
function visualize_leg(joint_angles, leg_params, target_pos)
|
||
% 可视化腿部运动学
|
||
|
||
% 提取参数
|
||
L1 = leg_params.upper_leg_length;
|
||
L2 = leg_params.lower_leg_length;
|
||
offset_x = leg_params.hip_offset_x;
|
||
offset_z = leg_params.hip_offset_z;
|
||
yaw_offset = leg_params.hip_yaw_offset;
|
||
|
||
% 提取关节角度
|
||
theta_yaw = joint_angles(1);
|
||
theta_roll = joint_angles(2);
|
||
theta_pitch = joint_angles(3);
|
||
theta_knee = joint_angles(4);
|
||
|
||
% 创建图形窗口
|
||
figure('Name', '四足机器人单腿运动学可视化', 'NumberTitle', 'off', 'Position', [100, 100, 1200, 800]);
|
||
|
||
% 1. 3D视图
|
||
subplot(2,2,[1,3]);
|
||
hold on;
|
||
grid on;
|
||
axis equal;
|
||
view(3);
|
||
xlabel('X (m)');
|
||
ylabel('Y (m)');
|
||
zlabel('Z (m)');
|
||
title('3D视图');
|
||
|
||
% 绘制坐标系
|
||
plot3([0, 0.1], [0, 0], [0, 0], 'r', 'LineWidth', 2); % X轴
|
||
plot3([0, 0], [0, 0.1], [0, 0], 'g', 'LineWidth', 2); % Y轴
|
||
plot3([0, 0], [0, 0], [0, 0.1], 'b', 'LineWidth', 2); % Z轴
|
||
|
||
% 计算关节位置
|
||
% 髋关节位置 (考虑了offset)
|
||
hip_pos = [offset_x, 0, offset_z];
|
||
|
||
% 应用yaw旋转
|
||
R_yaw = [cos(theta_yaw), -sin(theta_yaw), 0;
|
||
sin(theta_yaw), cos(theta_yaw), 0;
|
||
0, 0, 1];
|
||
|
||
% 应用roll旋转 (在yaw旋转后的坐标系中)
|
||
R_roll = [cos(theta_roll), 0, sin(theta_roll);
|
||
0, 1, 0;
|
||
-sin(theta_roll), 0, cos(theta_roll)];
|
||
|
||
% 应用pitch旋转
|
||
R_pitch = [1, 0, 0;
|
||
0, cos(theta_pitch), -sin(theta_pitch);
|
||
0, sin(theta_pitch), cos(theta_pitch)];
|
||
|
||
% 计算大腿末端位置
|
||
upper_leg_end = hip_pos + (R_yaw * R_roll * R_pitch * [L1; 0; 0])';
|
||
|
||
% 计算小腿末端位置
|
||
R_knee = [1, 0, 0;
|
||
0, cos(theta_knee), -sin(theta_knee);
|
||
0, sin(theta_knee), cos(theta_knee)];
|
||
|
||
lower_leg_end = upper_leg_end + (R_yaw * R_roll * R_pitch * R_knee * [L2; 0; 0])';
|
||
|
||
% 绘制连杆
|
||
plot3([hip_pos(1), upper_leg_end(1)], ...
|
||
[hip_pos(2), upper_leg_end(2)], ...
|
||
[hip_pos(3), upper_leg_end(3)], 'b-o', 'LineWidth', 3, 'MarkerSize', 8);
|
||
|
||
plot3([upper_leg_end(1), lower_leg_end(1)], ...
|
||
[upper_leg_end(2), lower_leg_end(2)], ...
|
||
[upper_leg_end(3), lower_leg_end(3)], 'r-o', 'LineWidth', 3, 'MarkerSize', 8);
|
||
|
||
% 绘制目标位置
|
||
plot3(target_pos(1), target_pos(2), target_pos(3), 'g*', 'MarkerSize', 15);
|
||
|
||
% 绘制yaw offset
|
||
plot3([hip_pos(1), hip_pos(1)+yaw_offset*sin(theta_yaw)], ...
|
||
[0, yaw_offset*cos(theta_yaw)], ...
|
||
[hip_pos(3), hip_pos(3)], 'm--', 'LineWidth', 2);
|
||
|
||
% 设置视图范围
|
||
axis_range = max([L1+L2, abs(offset_x)+0.1, abs(offset_z)+0.1]);
|
||
xlim([-axis_range, axis_range]);
|
||
ylim([-axis_range, axis_range]);
|
||
zlim([-axis_range, axis_range]);
|
||
|
||
% 2. X-Y平面视图
|
||
subplot(2,2,2);
|
||
hold on;
|
||
grid on;
|
||
axis equal;
|
||
xlabel('X (m)');
|
||
ylabel('Y (m)');
|
||
title('X-Y平面视图');
|
||
|
||
plot([0, 0.1], [0, 0], 'r', 'LineWidth', 2); % X轴
|
||
plot([0, 0], [0, 0.1], 'g', 'LineWidth', 2); % Y轴
|
||
|
||
plot([hip_pos(1), upper_leg_end(1)], [hip_pos(2), upper_leg_end(2)], 'b-o', 'LineWidth', 3, 'MarkerSize', 8);
|
||
plot([upper_leg_end(1), lower_leg_end(1)], [upper_leg_end(2), lower_leg_end(2)], 'r-o', 'LineWidth', 3, 'MarkerSize', 8);
|
||
plot(target_pos(1), target_pos(2), 'g*', 'MarkerSize', 15);
|
||
plot([hip_pos(1), hip_pos(1)+yaw_offset*sin(theta_yaw)], [0, yaw_offset*cos(theta_yaw)], 'm--', 'LineWidth', 2);
|
||
|
||
xlim([-axis_range, axis_range]);
|
||
ylim([-axis_range, axis_range]);
|
||
|
||
% 3. X-Z平面视图
|
||
subplot(2,2,4);
|
||
hold on;
|
||
grid on;
|
||
axis equal;
|
||
xlabel('X (m)');
|
||
ylabel('Z (m)');
|
||
title('X-Z平面视图');
|
||
|
||
plot([0, 0.1], [0, 0], 'r', 'LineWidth', 2); % X轴
|
||
plot([0, 0], [0, 0.1], 'b', 'LineWidth', 2); % Z轴
|
||
|
||
plot([hip_pos(1), upper_leg_end(1)], [hip_pos(3), upper_leg_end(3)], 'b-o', 'LineWidth', 3, 'MarkerSize', 8);
|
||
plot([upper_leg_end(1), lower_leg_end(1)], [upper_leg_end(3), lower_leg_end(3)], 'r-o', 'LineWidth', 3, 'MarkerSize', 8);
|
||
plot(target_pos(1), target_pos(3), 'g*', 'MarkerSize', 15);
|
||
|
||
xlim([-axis_range, axis_range]);
|
||
ylim([-axis_range, axis_range]);
|
||
|
||
% 添加图例
|
||
legend({'大腿', '小腿', '目标位置', 'Yaw偏移'}, 'Location', 'best');
|
||
end |