Stewart并联结构两自由度正逆解计算和工作空间仿真
目录
A、所有程序打包下载连接
一、逆解计算
1、逆解计算
（1）两自由度平台三维模型
（2）两自由度平台结构简图
（3）Matlab逆解计算代码
（4）测试计算
2、逆解计算代码优化和函数封装
（1）优化上述代码
（2）封装后代码
（3）测试计算
二、正解计算
1、正解计算代码
2、正解计算测试1
3、正解计算测试2
（1）俯仰角10度
（2）翻滚角15度
三、动画仿真运动
1、动画仿真实现功能
2、动画仿真效果展示
3、程序代码
四、工作空间分析与可视化
1、图像效果
2、程序代码
五、运动平台中心点工作空间计算
1、工作空间计算功能要求
2、工作空间图像效果
3、程序代码
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 truetrypitch_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;catchfprintf('输入无效，请重新输入数值\n');endend%% 平台位置和姿态计算% 平台中心位置（初始位置在中立点）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_lengthfprintf('警告：电动缸长度超出物理限制 (%.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) < toleranceconverged = 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/dPitchJ(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/dRollJ(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 ~convergedwarning('正解计算未收敛，结果可能不准确');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);
endfunction [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];
endfunction [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);endstroke_usage = (L1 - upper.H) / (upper.stroke/2) * 100;
endfunction [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);
endfunction 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');
endfunction 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);catcherror('无法创建视频文件，请检查写入权限');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=0title(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=0title(sprintf('翻滚运动阶段\n俯仰角: %.1f°, 翻滚角: %.1f°', 0, roll));frame = getframe(fig);writeVideo(writerObj, frame);pause(0.05);endclose(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;endend
endfunction 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:4subplot(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_rangefor 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;endend% 绘制高分辨率点云图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');
endfunction 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 % 精确高度];
endfunction [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