PUMA机械臂matlab仿真正逆解与路径规划

一苓二肆2026-01-20 16:03

注明：

项目链接 https://github.com/howard789/puma560_trajectory_plan

机器人工具箱 https://petercorke.com/toolboxes/robotics-toolbox/

一，正逆解运算

二，新添代码

puma560_gui_sliders.m 正解

Matlab 复制代码 
function puma560_gui_sliders()
% PUMA560 6关节滑块GUI：拖动滑块实时更新机械臂，并显示末端位姿
% 依赖：项目已 addpath(genpath('demo'), genpath('my_trajectory_plan'))
% 复用：demo/show_forward.m （你已验证能输入角度并画出机械臂）

clc;

% 确保能找到 show_forward
if exist('show_forward','file') ~= 2
    addpath(genpath('demo'), genpath('my_trajectory_plan'));
end

% 初始角度（度）
q0 = [30 25 -90 50 70 -80];

% UI
fig = uifigure('Name','PUMA560 Slider GUI', 'Position',[100 100 860 520]);

% 角度显示区
txt = uitextarea(fig, 'Position',[520 20 320 480], 'Editable','off', 'FontName','Consolas');

% 滑块区域
panel = uipanel(fig, 'Title','Joint Angles (deg)', 'Position',[20 20 480 480]);

labels = {'J1','J2','J3','J4','J5','J6'};

% 你可以按需要改范围（度）
% 建议先给个宽范围，后面再按你PUMA560实际限位收紧
mins = [-180 -180 -180 -180 -180 -180];
maxs = [ 180  180  180  180  180  180];

sl = gobjects(1,6);
ed = gobjects(1,6);

for i=1:6
    y = 430 - (i-1)*70;

    uilabel(panel, 'Text', labels{i}, 'Position',[10 y 30 22]);

    sl(i) = uislider(panel, ...
        'Limits',[mins(i) maxs(i)], ...
        'Value', q0(i), ...
        'Position',[50 y+10 320 3]);

    ed(i) = uieditfield(panel, 'numeric', ...
        'Value', q0(i), ...
        'Position',[390 y 70 22], ...
        'Limits',[mins(i) maxs(i)]);
end

% 一个"重置"按钮
uibutton(panel, 'Text','Reset', 'Position',[10 20 80 30], ...
    'ButtonPushedFcn', @(~,~)resetAll());

% 一个"复制当前角度"按钮
uibutton(panel, 'Text','Copy q', 'Position',[110 20 80 30], ...
    'ButtonPushedFcn', @(~,~)copyQ());

% 画图窗口（单独开一个 figure 更清爽）
robotFig = figure('Name','PUMA560 View', 'NumberTitle','off');
movegui(robotFig, 'northeast');

% 绑定回调：拖动滑块实时更新；在编辑框输入数字也更新
for i=1:6
    sl(i).ValueChangingFcn = @(src,evt)onSliderChanging(i, evt.Value);
    sl(i).ValueChangedFcn  = @(src,evt)updateAll(); % 松手后再完整更新一次（更稳）
    ed(i).ValueChangedFcn  = @(src,evt)onEditChanged(i, src.Value);
end

% 首次显示
updateAll();

%% ---------- nested functions ----------
    function q = getQ()
        q = arrayfun(@(k) sl(k).Value, 1:6);
    end

    function setQ(q)
        for k=1:6
            sl(k).Value = q(k);
            ed(k).Value = q(k);
        end
    end

    function onSliderChanging(idx, val)
        % 滑块拖动时：同步编辑框并快速刷新
        ed(idx).Value = val;
        quickUpdate();
    end

    function onEditChanged(idx, val)
        % 编辑框改值：同步滑块并刷新
        sl(idx).Value = val;
        updateAll();
    end

    function resetAll()
        setQ(q0);
        updateAll();
    end

    function copyQ()
        q = getQ();
        s = sprintf('[%.2f %.2f %.2f %.2f %.2f %.2f]', q);
        clipboard('copy', s);
        uialert(fig, ['已复制到剪贴板: ' s], 'Copied');
    end

    function quickUpdate()
        % 拖动时的"快速更新"：为了不卡，尽量少做事
        q = getQ();
        updateRobotPlot(q);
        updateText(q, []);
    end

    function updateAll()
        q = getQ();
        T = updateRobotPlot(q);
        updateText(q, T);
    end

    function T = updateRobotPlot(q_deg)
        % 关键：复用你项目的 show_forward
        % 你的 show_forward 目前是"显示正解矩阵并显示 robot 状态"
        % 如果 show_forward 能返回 T，就拿它；否则我们只显示文字"请改 show_forward 返回T"
        figure(robotFig); %#ok<LFIG>
        try
            % 如果你已经把 show_forward 改成 function T=show_forward(joints)
            T = show_forward(q_deg);
        catch
            % 若 show_forward 没返回值，也不影响"画图实时变化"
            show_forward(q_deg);
            T = [];
        end
        drawnow limitrate;
    end

    function updateText(q_deg, T)
        % 更新右侧文本区域
        lines = {};
        lines{end+1} = sprintf('q(deg) = [%.2f %.2f %.2f %.2f %.2f %.2f]', q_deg);
        lines{end+1} = '';

        if isempty(T)
            lines{end+1} = 'T: (show_forward 未返回 T，若要显示T请把 show_forward 改成 function T=show_forward(joints))';
        else
            p = T(1:3,4);
            R = T(1:3,1:3);
            lines{end+1} = 'T =';
            lines{end+1} = mat2str(T, 4);
            lines{end+1} = '';
            lines{end+1} = sprintf('p = [%.4f, %.4f, %.4f]^T', p(1), p(2), p(3));
            lines{end+1} = 'R =';
            lines{end+1} = mat2str(R, 4);
        end

        txt.Value = lines;
    end
end

puma560_gui_ik_pose.m 逆解

Matlab 复制代码 
function puma560_gui_ik()
% 末端位姿(滑块) -> inverse_kinematics(T) -> 8组解 -> 选择显示
% 依赖：
%   - demo/show_forward.m   (用来画机械臂)
%   - inverse_kinematics.m  (你贴的函数，输入4x4齐次矩阵，返回8组rad)

clc;

% 路径
if exist('show_forward','file') ~= 2 || exist('inverse_kinematics','file') ~= 2
    addpath(genpath('demo'), genpath('my_trajectory_plan'));
end

if exist('inverse_kinematics','file') ~= 2
    error('找不到 inverse_kinematics.m，请确认已 addpath 到工程目录。');
end
if exist('show_forward','file') ~= 2
    error('找不到 show_forward.m，请确认 demo 目录已加入路径。');
end

% 初始末端位姿（建议你选一个"肯定可达"的）
pose0.xyz = [-0.2, -0.2, 0.2];      % x y z  (单位按你工程：多半是米)
pose0.rpy = [0, 90, 0];             % roll pitch yaw (deg), ZYX

% UI
fig = uifigure('Name','PUMA560 IK GUI', 'Position',[100 100 1020 560]);

panel = uipanel(fig,'Title','End-Effector Pose (xyz + rpy)', 'Position',[20 20 640 520]);
txt = uitextarea(fig,'Position',[680 20 320 520], 'Editable','off', 'FontName','Consolas');

uilabel(panel,'Text','选择解:', 'Position',[10 30 60 22]);
dd = uidropdown(panel,'Items',{'1'},'Value','1','Position',[70 30 80 22]);

btnPlay = uibutton(panel,'Text','轮播', 'Position',[170 30 70 26]);
btnStop = uibutton(panel,'Text','停止', 'Position',[250 30 70 26], 'Enable','off');
btnCopyT = uibutton(panel,'Text','复制T', 'Position',[340 30 70 26]);

% 机器人显示窗口
robotFig = figure('Name','PUMA560 IK View','NumberTitle','off');
movegui(robotFig,'northeast');

% sliders
names = {'x','y','z','roll','pitch','yaw'};
xyzMin = [-0.6, -0.6, -0.2];
xyzMax = [ 0.6,  0.6,  0.8];
rpyMin = [-180, -180, -180];
rpyMax = [ 180,  180,  180];
mins = [xyzMin, rpyMin];
maxs = [xyzMax, rpyMax];
vals0 = [pose0.xyz, pose0.rpy];

sl = gobjects(1,6);
ed = gobjects(1,6);

for i=1:6
    y = 450 - (i-1)*70;
    uilabel(panel,'Text',names{i}, 'Position',[10 y 60 22]);

    sl(i) = uislider(panel, ...
        'Limits',[mins(i) maxs(i)], ...
        'Value', vals0(i), ...
        'Position',[70 y+10 420 3]);

    ed(i) = uieditfield(panel,'numeric', ...
        'Value', vals0(i), ...
        'Limits',[mins(i) maxs(i)], ...
        'Position',[510 y 110 22]);

    sl(i).ValueChangingFcn = @(src,evt)onChanging(i, evt.Value);
    sl(i).ValueChangedFcn  = @(src,evt)updateAll();
    ed(i).ValueChangedFcn  = @(src,evt)onEdit(i, src.Value);
end

dd.ValueChangedFcn = @(~,~)updateRobotFromSelected();
btnPlay.ButtonPushedFcn = @(~,~)startPlay();
btnStop.ButtonPushedFcn = @(~,~)stopPlay();
btnCopyT.ButtonPushedFcn = @(~,~)copyT();

% timer for autoplay
timerObj = timer('ExecutionMode','fixedSpacing','Period',1.0,'TimerFcn',@(~,~)tick());

% 缓存当前解（避免每次点下拉都重新算）
Qdeg_cache = [];
valid_cache = false;
T_cache = eye(4);

updateAll();

%% -------- nested --------
    function pose = getPose()
        v = arrayfun(@(k) sl(k).Value, 1:6);
        pose.xyz = v(1:3);
        pose.rpy = v(4:6);
    end

    function onChanging(idx, val)
        ed(idx).Value = val;
        quickUpdate();
    end

    function onEdit(idx, val)
        sl(idx).Value = val;
        updateAll();
    end

    function T = poseToT(pose)
        x = pose.xyz(1); y = pose.xyz(2); z = pose.xyz(3);
        r = deg2rad(pose.rpy(1));
        p = deg2rad(pose.rpy(2));
        yaw = deg2rad(pose.rpy(3));

        Rz = [cos(yaw) -sin(yaw) 0;
              sin(yaw)  cos(yaw) 0;
              0         0        1];
        Ry = [cos(p) 0 sin(p);
              0      1 0;
             -sin(p) 0 cos(p)];
        Rx = [1 0      0;
              0 cos(r) -sin(r);
              0 sin(r)  cos(r)];
        R = Rz*Ry*Rx;   % ZYX

        T = [R [x;y;z]; 0 0 0 1];
    end

    function quickUpdate()
        % 拖动时也更新，但少做点事（不弹窗/不花里胡哨）
        updateAll(true);
    end

    function updateAll(isFast)
        if nargin < 1, isFast = false; end

        pose = getPose();
        T = poseToT(pose);

        % 调你工程的解析逆解：返回 cell 里装 8 组 joints（rad）
        [valid, result_list] = inverse_kinematics(T);

        T_cache = T;
        valid_cache = logical(valid);

        if ~valid_cache || isempty(result_list)
            Qdeg_cache = [];
            dd.Items = {'1'};
            dd.Value = '1';
            if ~isFast
                txt.Value = buildTextNoSolution(T, pose);
            end
            return;
        end

        % cell -> 8x6 rad
        Qrad = cell2mat(result_list(:));      % 8x6
        Qdeg_cache = rad2deg(Qrad);

        % 更新下拉
        dd.Items = arrayfun(@num2str, 1:size(Qdeg_cache,1), 'UniformOutput', false);
        if ~ismember(dd.Value, dd.Items)
            dd.Value = dd.Items{1};
        end

        if ~isFast
            txt.Value = buildText(T, pose, Qdeg_cache);
        end

        updateRobotFromSelected();
    end

    function updateRobotFromSelected()
        if isempty(Qdeg_cache), return; end
        idx = str2double(dd.Value);
        idx = max(1, min(idx, size(Qdeg_cache,1)));
        q_deg = Qdeg_cache(idx,:);

        figure(robotFig); %#ok<LFIG>
        show_forward(q_deg);     % 你工程的 FK 可视化（输入"度"）
        title(sprintf('IK Solution %d: [%.1f %.1f %.1f %.1f %.1f %.1f] deg', ...
            idx, q_deg(1),q_deg(2),q_deg(3),q_deg(4),q_deg(5),q_deg(6)));
        drawnow limitrate;
    end

    function lines = buildTextNoSolution(T, pose)
        lines = {};
        lines{end+1} = 'No IK solution (valid=0).';
        lines{end+1} = sprintf('xyz = [%.3f %.3f %.3f]', pose.xyz(1),pose.xyz(2),pose.xyz(3));
        lines{end+1} = sprintf('rpy = [%.1f %.1f %.1f] deg (ZYX)', pose.rpy(1),pose.rpy(2),pose.rpy(3));
        lines{end+1} = '';
        lines{end+1} = 'T =';
        lines{end+1} = mat2str(T, 4);
    end

    function lines = buildText(T, pose, Qdeg)
        lines = {};
        lines{end+1} = 'Target pose:';
        lines{end+1} = sprintf('xyz = [%.3f %.3f %.3f]', pose.xyz(1),pose.xyz(2),pose.xyz(3));
        lines{end+1} = sprintf('rpy = [%.1f %.1f %.1f] deg (ZYX)', pose.rpy(1),pose.rpy(2),pose.rpy(3));
        lines{end+1} = '';
        lines{end+1} = 'T =';
        lines{end+1} = mat2str(T, 4);
        lines{end+1} = '';
        lines{end+1} = sprintf('Found %d solutions:', size(Qdeg,1));
        for k=1:size(Qdeg,1)
            q = Qdeg(k,:);
            lines{end+1} = sprintf('%2d) [%.1f %.1f %.1f %.1f %.1f %.1f] deg', ...
                k, q(1),q(2),q(3),q(4),q(5),q(6));
        end
    end

    function startPlay()
        btnPlay.Enable = 'off';
        btnStop.Enable = 'on';
        if strcmp(timerObj.Running,'off'), start(timerObj); end
    end

    function stopPlay()
        btnPlay.Enable = 'on';
        btnStop.Enable = 'off';
        if strcmp(timerObj.Running,'on'), stop(timerObj); end
    end

    function tick()
        if isempty(dd.Items), return; end
        cur = str2double(dd.Value);
        nxt = cur + 1;
        if nxt > numel(dd.Items), nxt = 1; end
        dd.Value = dd.Items{nxt};
        updateRobotFromSelected();
    end

    function copyT()
        clipboard('copy', mat2str(T_cache, 6));
        uialert(fig, '已复制当前 T 到剪贴板（mat2str 格式）', 'Copied');
    end
end

draw_shapes_with_puma560.m 路径规划

Matlab 复制代码 
function draw_shapes_with_puma560_onefig()
% 单窗口：机械臂动画 + 末端轨迹实时绘制（circle/rectangle/triangle/star）
% 依赖：inverse_kinematics(T)  -> [valid, result_list] (8组rad)
%       show_forward(q_deg)    -> 绘制机械臂（输入度）

clear; clc; close all;
addpath(genpath('demo'), genpath('my_trajectory_plan'));

assert(exist('inverse_kinematics','file')==2, '找不到 inverse_kinematics.m');
assert(exist('show_forward','file')==2, '找不到 show_forward.m');

%% ====== 选择图形 ======
shape = "triangle";   % "circle" | "rectangle" | "triangle" | "star"

%% ====== 轨迹参数（你主要调这些）======
N = 30;                        % 点数（越大越顺）
dt = 0.02;                      % 帧间隔（秒）
center = [0.25, 0.00, 0.35];    % 图形中心
scale = 0.1;                   % 图形尺度
yaw_deg = 0;
pitch_deg = 90;
roll_deg = 0;
close_loop = true;

%% ====== 初始关节角（用于连续选解）======
q_prev_deg = [30 25 -90 50 70 -80];
q_prev_rad = deg2rad(q_prev_deg);

%% ====== 生成目标路径点 ======
P = generate_shape_points(shape, N, center, scale, close_loop);

T_list = cell(size(P,1),1);
for k = 1:size(P,1)
    T_list{k} = make_T_from_xyz_rpy(P(k,:), [roll_deg pitch_deg yaw_deg]);
end

%% ====== 逆解并选连续解 ======
Q_deg = zeros(size(P,1), 6);

for k = 1:size(P,1)
    Tk = T_list{k};

    [valid, result_list] = inverse_kinematics(Tk);
    if ~valid || isempty(result_list)
        error('点 %d 无解：xyz=[%.3f %.3f %.3f]，请调 center/scale/姿态', ...
            k, P(k,1),P(k,2),P(k,3));
    end

    Qrad_all = cell2mat(result_list(:));    % 8x6
    idx = select_closest_solution(Qrad_all, q_prev_rad);

    q_sel_rad = Qrad_all(idx,:);
    q_prev_rad = q_sel_rad;

    Q_deg(k,:) = rad2deg(q_sel_rad);
end

%% ====== 单窗口动画：边动边画轨迹 ======
fig = figure('Name',"PUMA560 Draw: "+shape,'NumberTitle','off');
ax = axes('Parent',fig);
grid(ax,'on'); axis(ax,'equal');
xlabel(ax,'X'); ylabel(ax,'Y'); zlabel(ax,'Z');
view(ax, 3);

% 先画"目标轨迹虚线"（可选，不想要就注释）
hold(ax,'on');
plot3(ax, P(:,1), P(:,2), P(:,3), '--', 'LineWidth', 1);

% 实时轨迹线（从空开始）
hTrace = plot3(ax, nan, nan, nan, 'LineWidth', 2);  % 不指定颜色，遵守你的偏好

% 用于缓存已经走过的点（这里用目标点作为轨迹；如果你想用"实际FK点"，我也能改）
xt = nan(size(P,1),1);
yt = nan(size(P,1),1);
zt = nan(size(P,1),1);

% 动画循环
for k = 1:size(Q_deg,1)
    % 1) 在同一个 figure 里画机器人
    % show_forward 内部可能会 clear/重画当前figure，
    % 为了让轨迹线不丢：我们每次 show_forward 后再把轨迹线画回去。
    figure(fig);
    show_forward(Q_deg(k,:));     % 你的函数：输入"度"
    hold on;

    % 2) 更新轨迹点
    xt(k) = P(k,1);
    yt(k) = P(k,2);
    zt(k) = P(k,3);

    set(hTrace, 'XData', xt(1:k), 'YData', yt(1:k), 'ZData', zt(1:k));

    title(sprintf('Shape: %s | step %d/%d', shape, k, size(Q_deg,1)));

    drawnow;
    pause(dt);
end

disp('完成：单窗口边动边画轨迹。');

end

%% ====== 辅助函数 ======
function P = generate_shape_points(shape, N, center, scale, close_loop)
cx = center(1); cy = center(2); cz = center(3);

switch lower(shape)
    case "circle"
        t = linspace(0, 2*pi, N);
        P = [cx + scale*cos(t(:)), cy + scale*sin(t(:)), cz + zeros(N,1)];

    case "rectangle"
        w = scale*2;
        h = scale*1.2;
        v = [ ...
            cx-w/2, cy-h/2, cz;
            cx+w/2, cy-h/2, cz;
            cx+w/2, cy+h/2, cz;
            cx-w/2, cy+h/2, cz];
        P = polyline_resample(v, N, close_loop);

    case "triangle"
        R = scale*1.2;
        ang = deg2rad([90, 210, 330]);
        v = [cx+R*cos(ang(:)), cy+R*sin(ang(:)), cz+zeros(3,1)];
        P = polyline_resample(v, N, close_loop);

    case "star"
        Rout = scale*1.4;
        Rin  = scale*0.55;
        ang0 = deg2rad(90);
        v = zeros(10,3);
        for i=1:10
            a = ang0 + (i-1)*pi/5;
            r = Rout;
            if mod(i,2)==0, r = Rin; end
            v(i,:) = [cx + r*cos(a), cy + r*sin(a), cz];
        end
        P = polyline_resample(v, N, close_loop);

    otherwise
        error('未知 shape：%s', shape);
end
end

function P = polyline_resample(vertices, N, close_loop)
V = vertices;
if close_loop
    V = [V; V(1,:)];
end
seg = diff(V,1,1);
L = sqrt(sum(seg.^2,2));
s = [0; cumsum(L)];
total = s(end);
su = linspace(0, total, N)';

P = zeros(N,3);
for dim=1:3
    P(:,dim) = interp1(s, V(:,dim), su, 'linear');
end
end

function T = make_T_from_xyz_rpy(xyz, rpy_deg)
x = xyz(1); y = xyz(2); z = xyz(3);
roll = deg2rad(rpy_deg(1));
pitch= deg2rad(rpy_deg(2));
yaw  = deg2rad(rpy_deg(3));

Rz = [cos(yaw) -sin(yaw) 0;
      sin(yaw)  cos(yaw) 0;
      0         0        1];
Ry = [cos(pitch) 0 sin(pitch);
      0          1 0;
     -sin(pitch) 0 cos(pitch)];
Rx = [1 0 0;
      0 cos(roll) -sin(roll);
      0 sin(roll)  cos(roll)];

R = Rz*Ry*Rx;
T = [R [x;y;z]; 0 0 0 1];
end

function idx = select_closest_solution(Qrad_all, q_prev_rad)
diff = Qrad_all - q_prev_rad;
d = sqrt(sum(diff.^2,2));
[~, idx] = min(d);
end
相关推荐
Frank_refuel2 小时前
C++之继承
开发语言·c++
sunfove2 小时前
Python 自动化实战：从识图点击、模拟真人轨迹到封装 EXE 全流程教学
开发语言·python·自动化
傻啦嘿哟2 小时前
Python网页自动化操作全攻略：从入门到实战
开发语言·python·自动化
筱歌儿2 小时前
TinyMCE-----word表格图片进阶版
开发语言·javascript·word
黎雁·泠崖3 小时前
Java面向对象：对象数组进阶实战
java·开发语言
%xiao Q3 小时前
GESP C++四级-216
java·开发语言·c++
西红市杰出青年3 小时前
Python异步----------信号量
开发语言·python
a程序小傲3 小时前
蚂蚁Java面试被问：向量数据库的相似度搜索和索引构建
开发语言·后端·python·架构·flask·fastapi
w***76553 小时前
JS vs jQuery：核心差异解析
开发语言·javascript·jquery
热门推荐
01GitHub 镜像站点02OpenCode 入门教程：介绍 · 安装 · 配置第三方 API (如 Claude)032025 Telegram 最新免费社工库机器人（LetsTG可[特殊字符]）搭建指南（含 Python 脚本）04Linux下V2Ray安装配置指南05安娜的档案(Anna’s Archive) 镜像网站/国内最新可访问入口（持续更新）06UV安装并设置国内源07AI 规范驱动开发“三剑客”深度对比：Spec-Kit、Kiro 与 OpenSpec 实战指南08Claude Code Skills 实用使用手册09BongoCat - 跨平台键盘猫动画工具10网站改了域名，如何查找？