目录
A、所有程序打包下载连接
https://download.csdn.net/download/panjinliang066333/91186145
一、逆解计算
1、逆解计算
（1）两自由度平台三维模型
平台由两个自由度由两个电动缸运动实现，两个自由度分别为俯仰运动、翻滚运动。固定点，为万向节平衡点和两个电动缸共同支撑上平面。
（2）两自由度平台结构简图
根据结构简图，进行数学建模
电动缸总行程为250，在行程为125的情况下，上平台的俯仰角和翻滚角都为0.
依次分别计算出：四个点A、B、a、b的坐标
D和d分别为O、O到AB、ab的距离
D=sqrt(L1 * L1 - (L2/2) * (L2/2));
A1=[-L2/2; D; 0];
A2=[L2/2; D; 0];
d=sqrt(S1 * S1 - (S2/2) * (S2/2));
a1=[-S2/2; d; 0];
a2=[S2/2; d; 0];
（3）Matlab逆解计算代码
syms L1 L2 ;
syms S1 S2 ;
syms H h;
syms A B C x y z ;
syms Tx Ty Tz T ;
syms A1 A2 a1 a2 ;
syms D d;
syms ML1 ML2 ;
%设备参数
L1=250;
L2=250*1.41421356;
S1=250;
S2=250*1.41421356;
H=746; %缸体长度
L=250; %电动缸行程
D=sqrt(L1 * L1 - (L2/2) * (L2/2));
A1=[-L2/2; D; 0];
A2=[L2/2; D; 0];
d=sqrt(S1 * S1 - (S2/2) * (S2/2));
a1=[-S2/2; d; 0];
a2=[S2/2; d; 0];
A=input('请输入俯仰角A:');
B=input('请输入翻滚角B:');
A=(A/180)*pi;
B=(B/180)*pi;
x=0;
y=0;
z=0;
z=H+L/2;
P=[x;y;z];
Tx=[1 0 0;
0 cos(A) -sin(A);
0 sin(A) cos(A)];
Ty=[cos(B) 0 sin(B);
0 1 0;
-sin(B) 0 cos(B)];
T=Tx * Ty;
ML1=sqrt((a1(1,1)-A1(1,1))^2+(a1(2,1)-A1(2,1))^2+(a1(3,1)-A1(3,1))^2);
ML2=sqrt((a2(1,1)-A2(1,1))^2+(a2(2,1)-A2(2,1))^2+(a2(3,1)-A2(3,1))^2);
%打印显示——根据下底板轴分布来确定
fprintf('1#电动缸长度是：%f \n',ML1);
fprintf('2#电动缸长度是：%f \n',ML2);
fprintf('1#电动缸当前行程：%f \n',ML1-H);
fprintf('2#电动缸当前行程：%f \n',ML2-H);
（4）测试计算
1、计算俯仰角10度
2、计算翻滚角15度
2、逆解计算代码优化和函数封装
（1）优化上述代码
先对上述代码进行优化，
优化上述并联结构两自由度逆解程序，主要改进包括：
1、使用更清晰的变量命名
2、添加注释说明
3、优化矩阵运算
4、增加输入验证
5、改进输出格式
优化后的代码
function NJ()
% 并联平台两自由度逆解计算程序
% 输入俯仰角和翻滚角，计算两个电动缸的伸长量
% 清空工作区
clearvars;
close all;
%clc;
%% 平台几何参数定义
% 上平台安装点参数
upper_platform.L1 = 250; % 上平台特征长度1 (mm)
upper_platform.L2 = 250*sqrt(2); % 上平台特征长度2 (mm)
upper_platform.H = 746; % 缸体长度 (mm)
upper_platform.stroke = 250; % 电动缸最大行程 (mm)
% 下平台安装点参数
lower_platform.S1 = 250; % 下平台特征长度1 (mm)
lower_platform.S2 = 250*sqrt(2); % 下平台特征长度2 (mm)
%% 安装点位置计算
% 计算上平台安装点位置
upper_platform.D = sqrt(upper_platform.L1^2 - (upper_platform.L2/2)^2);
upper_platform.A1 = [-upper_platform.L2/2; upper_platform.D; 0];
upper_platform.A2 = [ upper_platform.L2/2; upper_platform.D; 0];
% 计算下平台安装点位置
lower_platform.d = sqrt(lower_platform.S1^2 - (lower_platform.S2/2)^2);
lower_platform.a1 = [-lower_platform.S2/2; lower_platform.d; 0];
lower_platform.a2 = [ lower_platform.S2/2; lower_platform.d; 0];
%% 用户输入
% 获取用户输入的姿态角度（度）
while true
try
pitch_angle_deg = input('请输入俯仰角Pitch (度): ');
roll_angle_deg = input('请输入翻滚角Roll (度): ');
% 验证输入是否为数值
if ~isnumeric(pitch_angle_deg) || ~isnumeric(roll_angle_deg)
error('输入必须为数值');
end
% 转换为弧度
pitch_angle = deg2rad(pitch_angle_deg);
roll_angle = deg2rad(roll_angle_deg);
break;
catch
fprintf('输入无效，请重新输入数值\n');
end
end
%% 平台位置和姿态计算
% 平台中心位置（初始位置在中立点）
platform_center = [0; 0; upper_platform.H + upper_platform.stroke/2];
% 旋转矩阵
% 俯仰旋转矩阵 (绕X轴)
Rx = [1, 0, 0;
0, cos(pitch_angle), -sin(pitch_angle);
0, sin(pitch_angle), cos(pitch_angle)];
% 翻滚旋转矩阵 (绕Y轴)
Ry = [cos(roll_angle), 0, sin(roll_angle);
0, 1, 0;
-sin(roll_angle), 0, cos(roll_angle)];
% 组合旋转
R = Rx * Ry;
% 计算电动缸长度
actuator1_length = norm(rotated_a1 - upper_platform.A1);
actuator2_length = norm(rotated_a2 - upper_platform.A2);
% 计算电动缸行程（相对于中立位置）
actuator1_stroke = actuator1_length - upper_platform.H;
actuator2_stroke = actuator2_length - upper_platform.H;
%% 结果输出
fprintf('\n=== 计算结果 ===\n');
fprintf('电动缸1总长度: %.3f mm\n', actuator1_length);
fprintf('电动缸2总长度: %.3f mm\n', actuator2_length);
fprintf('------------------\n');
fprintf('电动缸1行程: %.3f mm\n', actuator1_stroke);
fprintf('电动缸2行程: %.3f mm\n', actuator2_stroke);
fprintf('==================\n');
% 检查是否超出行程限制
% max_stroke = upper_platform.stroke/2;
% if abs(actuator1_stroke) > max_stroke || abs(actuator2_stroke) > max_stroke
% fprintf('\n警告：电动缸行程超出限制 (±%.1f mm)！\n', max_stroke);
% end
% 明确定义中立位置和行程方向
neutral_length = upper_platform.H; % 中立位置长度
min_length = neutral_length - upper_platform.stroke; % 最短长度
max_length = neutral_length + upper_platform.stroke; % 最长长度
%$ 检查是否超出物理限制
if actuator1_length < min_length || actuator1_length > max_length || ...
actuator2_length < min_length || actuator2_length > max_length
fprintf('警告：电动缸长度超出物理限制 (%.1f mm ~ %.1f mm)！\n', min_length, max_length);
end
end（2）封装后代码
function [actuator_lengths, actuator_strokes] = parallel_platform_ik(pitch_angle_deg, roll_angle_deg)
% PARALLEL_PLATFORM_IK 并联平台两自由度逆运动学计算
%
% 输入参数：
% pitch_angle_deg : 俯仰角（度）
% roll_angle_deg : 翻滚角（度）
%
% 输出参数：
% actuator_lengths : 2x1向量，两个电动缸的总长度 [mm]
% actuator_strokes : 2x1向量，两个电动缸的行程（相对于中立位置）[mm]
%
% 示例：
% [lengths, strokes] = parallel_platform_ik(10, 5);
% fprintf('电动缸长度: %.2f mm, %.2f mm\n', lengths(1), lengths(2));
% fprintf('电动缸行程: %.2f mm, %.2f mm\n', strokes(1), strokes(2));
%% 平台几何参数定义
% 上平台安装点参数
upper_platform.L1 = 250; % 上平台特征长度1 (mm)
upper_platform.L2 = 250*sqrt(2); % 上平台特征长度2 (mm)
upper_platform.H = 746; % 缸体长度 (mm)
upper_platform.stroke = 250; % 电动缸最大行程 (mm)
% 下平台安装点参数
lower_platform.S1 = 250; % 下平台特征长度1 (mm)
lower_platform.S2 = 250*sqrt(2); % 下平台特征长度2 (mm)
%% 安装点位置计算
% 计算上平台安装点位置
upper_platform.D = sqrt(upper_platform.L1^2 - (upper_platform.L2/2)^2);
upper_platform.A1 = [-upper_platform.L2/2; upper_platform.D; 0];
upper_platform.A2 = [ upper_platform.L2/2; upper_platform.D; 0];
% 计算下平台安装点位置
lower_platform.d = sqrt(lower_platform.S1^2 - (lower_platform.S2/2)^2);
lower_platform.a1 = [-lower_platform.S2/2; lower_platform.d; 0];
lower_platform.a2 = [ lower_platform.S2/2; lower_platform.d; 0];
%% 输入验证
if ~isnumeric(pitch_angle_deg) || ~isnumeric(roll_angle_deg)
error('输入角度必须为数值');
end
%% 平台位置和姿态计算
% 将角度转换为弧度
pitch_angle = deg2rad(pitch_angle_deg);
roll_angle = deg2rad(roll_angle_deg);
% 平台中心位置（初始位置在中立点）
platform_center = [0; 0; upper_platform.H + upper_platform.stroke/2];
% 旋转矩阵
% 俯仰旋转矩阵 (绕X轴)
Rx = [1, 0, 0;
0, cos(pitch_angle), -sin(pitch_angle);
0, sin(pitch_angle), cos(pitch_angle)];
% 翻滚旋转矩阵 (绕Y轴)
Ry = [cos(roll_angle), 0, sin(roll_angle);
0, 1, 0;
-sin(roll_angle), 0, cos(roll_angle)];
% 组合旋转
R = Rx * Ry;
% 计算电动缸长度
actuator_lengths = [
norm(rotated_a1 - upper_platform.A1);
norm(rotated_a2 - upper_platform.A2)
];
% 计算电动缸行程（相对于中立位置）
actuator_strokes = actuator_lengths - upper_platform.H;
%% 行程限制检查（可选）
max_stroke = upper_platform.stroke;
if any(abs(actuator_strokes) > max_stroke)
warning('电动缸行程超出限制 (±%.1f mm)', max_stroke);
end
end（3）测试计算
1、计算俯仰角10度
[actuator_lengths, actuator_strokes] = parallel_platform_ik(10, 0)
2、计算翻滚角15度
[actuator_lengths, actuator_strokes] = parallel_platform_ik(0, 15)
二、正解计算
正解计算（即已知电动缸长度，求平台姿态）通常需要数值迭代方法，这里采用牛顿-拉夫森法（Newton-Raphson）进行求解
1、正解计算代码
function [pitch_angle_deg, roll_angle_deg] = parallel_platform_fk(L1_actual, L2_actual)
% PARALLEL_PLATFORM_FK 并联平台两自由度正运动学计算
%
% 输入参数：
% L1_actual : 电动缸1的实际长度 (mm)
% L2_actual : 电动缸2的实际长度 (mm)
%
% 输出参数：
% pitch_angle_deg : 俯仰角 (度)
% roll_angle_deg : 翻滚角 (度)
%
% 示例：
% [pitch, roll] = parallel_platform_fk(800, 820);
%% 平台几何参数（与逆解一致）
% 上平台安装点参数
upper.L1 = 250; % 上平台特征长度1 (mm)
upper.L2 = 250*sqrt(2); % 上平台特征长度2 (mm)
upper.H = 746; % 缸体长度 (mm)
upper.stroke = 250; % 电动缸最大行程 (mm)
% 下平台安装点参数
lower.S1 = 250; % 下平台特征长度1 (mm)
lower.S2 = 250*sqrt(2); % 下平台特征长度2 (mm)
%% 安装点位置计算
upper.D = sqrt(upper.L1^2 - (upper.L2/2)^2);
upper.A1 = [-upper.L2/2; upper.D; 0];
upper.A2 = [ upper.L2/2; upper.D; 0];
lower.d = sqrt(lower.S1^2 - (lower.S2/2)^2);
lower.a1 = [-lower.S2/2; lower.d; 0];
lower.a2 = [ lower.S2/2; lower.d; 0];
%% 初始猜测（中立位置）
pitch_angle = 0; % 俯仰角初始猜测 (rad)
roll_angle = 0; % 翻滚角初始猜测 (rad)
%% 牛顿-拉夫森法迭代求解
max_iter = 100; % 最大迭代次数
tolerance = 1e-6; % 收敛容差
iter = 0; % 迭代计数器
converged = false; % 收敛标志
% 中立位置平台中心高度
neutral_height = upper.H + upper.stroke/2;
while ~converged && iter < max_iter
% 计算当前姿态下的电动缸长度
[L1_calculated, L2_calculated] = compute_actuator_lengths(...
pitch_angle, roll_angle, upper, lower, neutral_height);
% 计算残差（实际长度与计算长度的差）
residual = [L1_actual - L1_calculated;
L2_actual - L2_calculated];
% 检查是否收敛
if norm(residual) < tolerance
converged = true;
break;
end
% 计算雅可比矩阵（数值近似）
delta = 1e-6; % 微小扰动
% 对俯仰角的偏导
[L1_pitch, L2_pitch] = compute_actuator_lengths(...
pitch_angle + delta, roll_angle, upper, lower, neutral_height);
J(1,1) = (L1_pitch - L1_calculated) / delta; % dL1/dPitch
J(2,1) = (L2_pitch - L2_calculated) / delta; % dL2/dPitch
% 对翻滚角的偏导
[L1_roll, L2_roll] = compute_actuator_lengths(...
pitch_angle, roll_angle + delta, upper, lower, neutral_height);
J(1,2) = (L1_roll - L1_calculated) / delta; % dL1/dRoll
J(2,2) = (L2_roll - L2_calculated) / delta; % dL2/dRoll
% 求解线性系统并更新角度
delta_angles = J \ residual;
pitch_angle = pitch_angle + delta_angles(1);
roll_angle = roll_angle + delta_angles(2);
iter = iter + 1;
end
%% 输出结果
if ~converged
warning('正解计算未收敛，结果可能不准确');
end
% 转换为角度输出
pitch_angle_deg = rad2deg(pitch_angle);
roll_angle_deg = rad2deg(roll_angle);
% 可选：打印迭代信息
fprintf('正解计算完成，迭代次数: %d\n', iter);
fprintf('最终残差: %.6f mm\n', norm(residual));
end2、正解计算测试1
测试代码
% 示例1：已知电动缸长度求姿态
%翻滚角-38度，两个电动缸长度
[pitch, roll] = parallel_platform_fk(763.086, 980.551);
fprintf('平台姿态: 俯仰角=%.2f°, 翻滚角=%.2f°\n', pitch, roll);
% 示例2：验证正逆解一致性
% 先给定角度计算长度
[lengths, ~] = parallel_platform_ik(10, 5);
% 再用长度反求角度
[pitch, roll] = parallel_platform_fk(lengths(1), lengths(2));
fprintf('验证结果应与输入角度(10°,5°)接近:\n');
fprintf('计算姿态: 俯仰角=%.4f°, 翻滚角=%.4f°\n', pitch, roll);
3、正解计算测试2
（1）俯仰角10度
（2）翻滚角15度
三、动画仿真运动
1、动画仿真实现功能
功能要求：
1、计算该运动平台在中立位置开始运动：俯仰角±15度范围内、翻滚角±35度范围内的工作空间。平台先完成俯仰运动（±15°）再完成翻滚运动（±35°）。
2、绘制出该两自由度并联运动平台三维结构图、并绘制出工作空间的三维曲面图形，图像动画显示。
3、生成相应的视频动画，视频保存在指定文件夹中。
2、动画仿真效果展示
平台运动模拟画面
电动缸数据
3、程序代码
%%俯仰和翻滚先后运动
%俯仰角±15度、翻滚角：±38度
function parallel_platform_sequential_motion()
% 主函数：先俯仰后翻滚的顺序运动
% 1. 初始化平台参数
[upper, lower] = init_platform_parameters();
% 2. 计算工作空间数据
[PITCH, ROLL, L1, L2, stroke_usage] = calculate_workspace(upper, lower);
% 3. 绘制工作空间曲面
plot_workspace_surfaces(PITCH, ROLL, L1, L2, stroke_usage);
% 4. 执行顺序运动动画（先俯仰后翻滚）
animate_sequential_motion(upper, lower);
end
function [upper, lower] = init_platform_parameters()
% 初始化平台几何参数
upper.L1 = 250;
upper.L2 = 250*sqrt(2);
upper.H = 746;
upper.stroke = 250;
lower.S1 = 250;
lower.S2 = 250*sqrt(2);
% 计算安装点位置
upper.D = sqrt(upper.L1^2 - (upper.L2/2)^2);
upper.A1 = [-upper.L2/2; upper.D; 0];
upper.A2 = [upper.L2/2; upper.D; 0];
lower.d = sqrt(lower.S1^2 - (lower.S2/2)^2);
lower.a1 = [-lower.S2/2; lower.d; 0];
lower.a2 = [lower.S2/2; lower.d; 0];
end
function [PITCH, ROLL, L1, L2, stroke_usage] = calculate_workspace(upper, lower)
% 计算工作空间数据
pitch_range = linspace(-15, 15, 30);
roll_range = linspace(-35, 35, 30);
[PITCH, ROLL] = meshgrid(pitch_range, roll_range);
L1 = zeros(size(PITCH));
L2 = zeros(size(PITCH));
for i = 1:numel(PITCH)
[L1(i), L2(i)] = compute_actuator_lengths(...
deg2rad(PITCH(i)), deg2rad(ROLL(i)), upper, lower);
end
stroke_usage = (L1 - upper.H) / (upper.stroke/2) * 100;
end
function [L1, L2] = compute_actuator_lengths(pitch, roll, upper, lower)
% 计算电动缸长度
platform_center = [0; 0; upper.H + upper.stroke/2];
Rx = [1 0 0; 0 cos(pitch) -sin(pitch); 0 sin(pitch) cos(pitch)];
Ry = [cos(roll) 0 sin(roll); 0 1 0; -sin(roll) 0 cos(roll)];
R = Rx * Ry;
rotated_a1 = R * lower.a1 + platform_center;
rotated_a2 = R * lower.a2 + platform_center;
L1 = norm(rotated_a1 - upper.A1);
L2 = norm(rotated_a2 - upper.A2);
end
function plot_workspace_surfaces(PITCH, ROLL, L1, L2, stroke_usage)
% 绘制工作空间曲面图
figure('Name', '工作空间分析', 'Color', 'white', 'Position', [100 100 1200 800]);
subplot(2,2,1);
surf(PITCH, ROLL, L1, 'EdgeColor', 'none');
title('电动缸1长度 (mm)');
xlabel('俯仰角 (°)'); ylabel('翻滚角 (°)'); zlabel('长度 (mm)');
colorbar; axis tight; view(3);
subplot(2,2,2);
surf(PITCH, ROLL, L2, 'EdgeColor', 'none');
title('电动缸2长度 (mm)');
xlabel('俯仰角 (°)'); ylabel('翻滚角 (°)'); zlabel('长度 (mm)');
colorbar; axis tight; view(3);
subplot(2,2,3);
surf(PITCH, ROLL, stroke_usage, 'EdgeColor', 'none');
title('电动缸行程使用率 (%)');
xlabel('俯仰角 (°)'); ylabel('翻滚角 (°)'); zlabel('使用率 (%)');
colorbar; caxis([-100 100]); view(3);
subplot(2,2,4);
contour(PITCH, ROLL, stroke_usage, 'LineWidth', 2);
title('工作空间边界');
xlabel('俯仰角 (°)'); ylabel('翻滚角 (°)');
colorbar; caxis([-100 100]);
hold on;
plot(0, 0, 'ro', 'MarkerSize', 10, 'LineWidth', 2);
text(0, 0, ' 中立位置', 'VerticalAlignment', 'bottom');
end
function animate_sequential_motion(upper, lower)
% 顺序运动动画：先俯仰后翻滚
fig = figure('Name', '平台顺序运动动画', 'Color', 'white', 'Position', [200 200 800 600]);
% 创建临时视频文件
try
%video_path = fullfile(tempdir, 'sequential_motion.mp4');
video_path = 'C:\Users\97205\Desktop\两自由度-Matlab\platform_animation.mp4'; % Windows示例
writerObj = VideoWriter(video_path, 'MPEG-4');
writerObj.FrameRate = 20;
open(writerObj);
catch
error('无法创建视频文件，请检查写入权限');
end
% ========== 第一阶段：俯仰运动 ==========
pitch_range = linspace(0, 15, 15); % 0°→15°
for pitch = [pitch_range, fliplr(pitch_range), -pitch_range, fliplr(-pitch_range)]
clf(fig);
plot_single_pose(upper, lower, pitch, 0); % 保持roll=0
title(sprintf('俯仰运动阶段\n俯仰角: %.1f°, 翻滚角: %.1f°', pitch, 0));
frame = getframe(fig);
writeVideo(writerObj, frame);
pause(0.05);
end
% ========== 第二阶段：翻滚运动 ==========
roll_range = linspace(0, 35, 20); % 0°→35°
for roll = [roll_range, fliplr(roll_range), -roll_range, fliplr(-roll_range)]
clf(fig);
plot_single_pose(upper, lower, 0, roll); % 保持pitch=0
title(sprintf('翻滚运动阶段\n俯仰角: %.1f°, 翻滚角: %.1f°', 0, roll));
frame = getframe(fig);
writeVideo(writerObj, frame);
pause(0.05);
end
close(writerObj);
fprintf('顺序运动动画已保存到: %s\n', video_path);
% 显示复合运动（可选）
figure('Name', '复合运动演示', 'Position', [300 300 800 600]);
for pitch = linspace(-15, 15, 30)
for roll = linspace(-35, 35, 30)
clf;
plot_single_pose(upper, lower, pitch, roll);
title(sprintf('复合运动演示\n俯仰角: %.1f°, 翻滚角: %.1f°', pitch, roll));
drawnow;
end
end
end
function plot_single_pose(upper, lower, pitch_deg, roll_deg)
% 绘制单个姿态
pitch = deg2rad(pitch_deg);
roll = deg2rad(roll_deg);
platform_center = [0; 0; upper.H + upper.stroke/2];
% 旋转矩阵
Rx = [1 0 0; 0 cos(pitch) -sin(pitch); 0 sin(pitch) cos(pitch)];
Ry = [cos(roll) 0 sin(roll); 0 1 0; -sin(roll) 0 cos(roll)];
R = Rx * Ry;
% 计算平台位置
lower_pts = [lower.a1, lower.a2, -lower.a1, -lower.a2];
lower_pts = R * lower_pts + repmat(platform_center, 1, 4);
rotated_a1 = R * lower.a1 + platform_center;
rotated_a2 = R * lower.a2 + platform_center;
% 绘制平台
fill3(lower_pts(1,[1 2 3]), lower_pts(2,[1 2 3]), lower_pts(3,[1 2 3]), 'b', 'FaceAlpha', 0.3);
hold on;
fill3(lower_pts(1,[3 4 1]), lower_pts(2,[3 4 1]), lower_pts(3,[3 4 1]), 'b', 'FaceAlpha', 0.3);
upper_pts = [upper.A1, upper.A2, -upper.A1, -upper.A2];
fill3(upper_pts(1,[1 2 3]), upper_pts(2,[1 2 3]), upper_pts(3,[1 2 3]), 'r', 'FaceAlpha', 0.3);
fill3(upper_pts(1,[3 4 1]), upper_pts(2,[3 4 1]), upper_pts(3,[3 4 1]), 'r', 'FaceAlpha', 0.3);
% 绘制电动缸
plot3([upper.A1(1), rotated_a1(1)], [upper.A1(2), rotated_a1(2)], [upper.A1(3), rotated_a1(3)], 'k', 'LineWidth', 3);
plot3([upper.A2(1), rotated_a2(1)], [upper.A2(2), rotated_a2(2)], [upper.A2(3), rotated_a2(3)], 'k', 'LineWidth', 3);
% 设置图形属性
axis equal; grid on;
xlabel('X (mm)'); ylabel('Y (mm)'); zlabel('Z (mm)');
view(30, 30);
xlim([-500 500]); ylim([-500 500]);
zlim([0, upper.H + upper.stroke]);
end四、工作空间分析与可视化
实现功能要求：给定几个姿态角度，得到上平台运动图像和电动缸长度
1、图像效果
平台结构
工作空间分析
2、程序代码
%% 主程序：工作空间分析与可视化
function parallel_platform_workspace_analysis()
% 清空工作区
clear; close all; clc;
%% 1. 平台几何参数定义
% 上平台安装点参数
upper.L1 = 250; % 上平台特征长度1 (mm)
upper.L2 = 250*sqrt(2); % 上平台特征长度2 (mm)
upper.H = 746; % 缸体长度 (mm)
upper.stroke = 250; % 电动缸最大行程 (mm)
% 下平台安装点参数
lower.S1 = 250; % 下平台特征长度1 (mm)
lower.S2 = 250*sqrt(2); % 下平台特征长度2 (mm)
% 计算安装点位置
[upper, lower] = calculate_platform_geometry(upper, lower);
%% 2. 工作空间计算
% 定义角度范围
pitch_range = linspace(-15, 15, 30); % 俯仰角范围 ±15°
roll_range = linspace(-35, 35, 30); % 翻滚角范围 ±35°
% 初始化存储矩阵
[PITCH, ROLL] = meshgrid(pitch_range, roll_range);
L1 = zeros(size(PITCH));
L2 = zeros(size(PITCH));
Z_pos = zeros(size(PITCH));
% 遍历所有角度组合
for i = 1:numel(PITCH)
% 计算当前姿态下的电动缸长度
[L1(i), L2(i)] = compute_actuator_lengths(...
deg2rad(PITCH(i)), deg2rad(ROLL(i)), upper, lower);
% 计算平台中心高度
Z_pos(i) = upper.H + upper.stroke/2;
end
%% 3. 绘制工作空间曲面
figure('Name', '工作空间分析', 'Color', 'white');
% 电动缸长度曲面
subplot(2,2,1);
surf(PITCH, ROLL, L1, 'EdgeColor', 'none');
title('电动缸1长度 (mm)');
xlabel('俯仰角 (deg)'); ylabel('翻滚角 (deg)'); zlabel('长度 (mm)');
colorbar; axis tight; view(3);
subplot(2,2,2);
surf(PITCH, ROLL, L2, 'EdgeColor', 'none');
title('电动缸2长度 (mm)');
xlabel('俯仰角 (deg)'); ylabel('翻滚角 (deg)'); zlabel('长度 (mm)');
colorbar; axis tight; view(3);
% 行程使用率曲面
subplot(2,2,3);
stroke_usage1 = (L1 - upper.H) / (upper.stroke/2) * 100;
surf(PITCH, ROLL, stroke_usage1, 'EdgeColor', 'none');
title('电动缸1行程使用率 (%)');
xlabel('俯仰角 (deg)'); ylabel('翻滚角 (deg)'); zlabel('使用率 (%)');
colorbar; caxis([-100 100]); view(3);
subplot(2,2,4);
stroke_usage2 = (L2 - upper.H) / (upper.stroke/2) * 100;
surf(PITCH, ROLL, stroke_usage2, 'EdgeColor', 'none');
title('电动缸2行程使用率 (%)');
xlabel('俯仰角 (deg)'); ylabel('翻滚角 (deg)'); zlabel('使用率 (%)');
colorbar; caxis([-100 100]); view(3);
%% 4. 绘制平台三维结构图
% 选择几个典型姿态进行可视化
demo_angles = [0 0; 15 0; 0 35; -10 -20]; % [pitch, roll]
figure('Name', '平台三维结构', 'Color', 'white', 'Position', [100 100 1200 800]);
for i = 1:4
subplot(2,2,i);
plot_platform_3d(upper, lower, demo_angles(i,1), demo_angles(i,2));
title(sprintf('姿态: Pitch=%.0f°, Roll=%.0f°', demo_angles(i,1), demo_angles(i,2)));
end
%% 5. 工作空间边界分析
figure('Name', '工作空间边界', 'Color', 'white');
% 计算边界条件
[boundary_pitch, boundary_roll] = find_workspace_boundary(pitch_range, roll_range, L1, L2, upper);
% 绘制工作空间边界
plot3(boundary_pitch, boundary_roll, zeros(size(boundary_pitch)), 'r-', 'LineWidth', 2);
hold on;
scatter3(PITCH(:), ROLL(:), zeros(numel(PITCH),1), 10, 'filled', 'MarkerFaceAlpha', 0.3);
xlabel('俯仰角 (deg)'); ylabel('翻滚角 (deg)');
title('工作空间边界 (红色)');
grid on; axis tight; view(2);
end
%% 几何计算函数
function [upper, lower] = calculate_platform_geometry(upper, lower)
% 计算上平台安装点位置
upper.D = sqrt(upper.L1^2 - (upper.L2/2)^2);
upper.A1 = [-upper.L2/2; upper.D; 0];
upper.A2 = [ upper.L2/2; upper.D; 0];
% 计算下平台安装点位置
lower.d = sqrt(lower.S1^2 - (lower.S2/2)^2);
lower.a1 = [-lower.S2/2; lower.d; 0];
lower.a2 = [ lower.S2/2; lower.d; 0];
end
%% 电动缸长度计算函数
function [L1, L2] = compute_actuator_lengths(pitch, roll, upper, lower)
% 平台中心位置（中立高度）
platform_center = [0; 0; upper.H + upper.stroke/2];
% 旋转矩阵
Rx = [1, 0, 0;
0, cos(pitch), -sin(pitch);
0, sin(pitch), cos(pitch)];
Ry = [cos(roll), 0, sin(roll);
0, 1, 0;
-sin(roll), 0, cos(roll)];
R = Rx * Ry;
% 计算旋转后的下平台安装点位置
rotated_a1 = R * lower.a1 + platform_center;
rotated_a2 = R * lower.a2 + platform_center;
% 计算电动缸长度
L1 = norm(rotated_a1 - upper.A1);
L2 = norm(rotated_a2 - upper.A2);
end
%% 工作空间边界计算函数
function [boundary_pitch, boundary_roll] = find_workspace_boundary(pitch_range, roll_range, L1, L2, upper)
% 找到达到行程限制的点
max_stroke = upper.stroke/2;
stroke1 = L1 - upper.H;
stroke2 = L2 - upper.H;
% 找出接近行程限制的边界点
threshold = 0.95; % 识别阈值为95%行程
boundary_idx = find(abs(stroke1) > max_stroke*threshold | abs(stroke2) > max_stroke*threshold);
% 提取边界角度
boundary_pitch = PITCH(boundary_idx);
boundary_roll = ROLL(boundary_idx);
% 对边界点进行排序（便于绘制闭合曲线）
[~, idx] = sort(atan2d(boundary_roll, boundary_pitch));
boundary_pitch = boundary_pitch(idx);
boundary_roll = boundary_roll(idx);
end
五、运动平台中心点工作空间计算
1、工作空间计算功能要求
该两自由度上平台中心点，在上述给定的俯仰角、翻滚角范围内，俯仰角和翻滚角以0.1的分辨率进行迭代，计算所有可能存在的空间位置，绘制出所有空间位置的坐标点图像。
2、工作空间图像效果
3、程序代码
%%绘制工作空间图像
%平台中心点，在上述给定的俯仰角、翻滚角范围内，俯仰角和翻滚角以0.1的分辨率进行迭代，
%计算所有可能存在的空间位置，绘制出所有空间位置的坐标点图像
function plot_highres_platform_workspace()
% 初始化平台参数
[upper, lower] = init_platform_parameters();
% 定义角度范围及分辨率（0.1°步长）
pitch_range = -15:0.1:15; % 俯仰角±15°，0.1°分辨率
roll_range = -35:0.1:35; % 翻滚角±35°，0.1°分辨率
% 预分配内存（估算点数）
num_points = length(pitch_range) * length(roll_range);
points = zeros(num_points, 3);
colors = zeros(num_points, 3); % 用于存储颜色数据
% 计算所有可能的空间位置
idx = 1;
for pitch = pitch_range
for roll = roll_range
% 计算平台中心位置（考虑高度变化）
platform_center = calculate_platform_center(...
deg2rad(pitch), deg2rad(roll), upper);
points(idx,:) = platform_center';
% 生成颜色映射（基于角度）
colors(idx,:) = [
(pitch - min(pitch_range))/range(pitch_range), ... % 红色分量表示俯仰
(roll - min(roll_range))/range(roll_range), ... % 绿色分量表示翻滚
0.5 ]; % 固定蓝色分量
idx = idx + 1;
end
end
% 绘制高分辨率点云图
figure('Name', '高分辨率平台工作空间', 'Color', 'white', 'Position', [100 100 1400 800]);
% 3D点云（使用scatter3提高渲染性能）
subplot(2,2,[1,3]);
scatter3(points(:,1), points(:,2), points(:,3), 6, colors, 'filled');
title(sprintf('平台中心点工作空间 (分辨率: 0.1°)\n点数: %d', size(points,1)));
xlabel('X (mm)'); ylabel('Y (mm)'); zlabel('Z (mm)');
axis equal; grid on; view(40, 30);
% 添加参考坐标系
hold on;
plot3([0 50], [0 0], [0 0], 'r-', 'LineWidth', 2); % X轴
plot3([0 0], [0 50], [0 0], 'g-', 'LineWidth', 2); % Y轴
plot3([0 0], [0 0], [0 50], 'b-', 'LineWidth', 2); % Z轴
text(50,0,0,'X','Color','r','FontWeight','bold');
text(0,50,0,'Y','Color','g','FontWeight','bold');
text(0,0,50,'Z','Color','b','FontWeight','bold');
% XY平面投影
subplot(2,2,2);
scatter(points(:,1), points(:,2), 6, colors, 'filled');
title('XY平面投影');
xlabel('X (mm)'); ylabel('Y (mm)');
axis equal; grid on;
% XZ平面投影
subplot(2,2,4);
scatter(points(:,1), points(:,3), 6, colors, 'filled');
title('XZ平面投影');
xlabel('X (mm)'); ylabel('Z (mm)');
axis equal; grid on;
% 显示工作空间统计信息
fprintf('=== 工作空间高分辨率分析 ===\n');
fprintf('角度范围: 俯仰 %.1f° 到 %.1f°, 翻滚 %.1f° 到 %.1f°\n',...
min(pitch_range), max(pitch_range), min(roll_range), max(roll_range));
fprintf('角度分辨率: 0.1°\n');
fprintf('总点数: %d\n', size(points,1));
fprintf('X范围: %.3fmm 到 %.3fmm\n', min(points(:,1)), max(points(:,1)));
fprintf('Y范围: %.3fmm 到 %.3fmm\n', min(points(:,2)), max(points(:,2)));
fprintf('Z范围: %.3fmm 到 %.3fmm\n', min(points(:,3)), max(points(:,3)));
% 保存数据到文件（可选）
save('platform_workspace_points.mat', 'points', 'pitch_range', 'roll_range');
end
function center = calculate_platform_center(pitch, roll, upper)
% 精确计算平台中心位置（考虑运动学模型）
% 中立位置高度
neutral_height = upper.H + upper.stroke/2;
% 旋转矩阵
Rx = [1 0 0; 0 cos(pitch) -sin(pitch); 0 sin(pitch) cos(pitch)];
Ry = [cos(roll) 0 sin(roll); 0 1 0; -sin(roll) 0 cos(roll)];
R = Rx * Ry;
% 平台坐标系法向量（旋转后）
normal_vector = R * [0; 0; 1];
% 高度修正（考虑平台倾斜）
height_correction = neutral_height * normal_vector(3);
% 平台中心位置 (考虑可能的XY偏移)
center = [
0.25 * neutral_height * sin(roll); % 经验公式-X偏移
0.25 * neutral_height * sin(pitch); % 经验公式-Y偏移
height_correction % 精确高度
];
end
function [upper, lower] = init_platform_parameters()
% 平台几何参数（与之前一致）
upper.L1 = 250;
upper.L2 = 250*sqrt(2);
upper.H = 746;
upper.stroke = 250;
lower.S1 = 250;
lower.S2 = 250*sqrt(2);
% 安装点计算
upper.D = sqrt(upper.L1^2 - (upper.L2/2)^2);
upper.A1 = [-upper.L2/2; upper.D; 0];
upper.A2 = [upper.L2/2; upper.D; 0];
lower.d = sqrt(lower.S1^2 - (lower.S2/2)^2);
lower.a1 = [-lower.S2/2; lower.d; 0];
lower.a2 = [lower.S2/2; lower.d; 0];
end