基于Matlab的机械臂逆运动学3D建模点位对齐问题求助

算力阿峰

2026-6-27

机械足逆运动学Matlab 3D可视化点位对齐问题

背景与现有Python实现

已完成机械足逆运动学数学推导，现有Python版3D可视化代码如下：

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.widgets import Slider, RadioButtons
import numpy as np
import math


def calculate_line(length, angle, axis):
    # Convert angle to radians
    angle_rad = np.radians(angle)
    # Calculate coordinates of the line based on the selected axis
    if axis == 'XY':
        x = [0, length * np.cos(angle_rad)]
        y = [0, length * np.sin(angle_rad)]
        z = [0, 0]
    elif axis == 'XZ':
        x = [0, length * np.cos(angle_rad)]
        y = [0, 0]
        z = [0, length * np.sin(angle_rad)]
    elif axis == 'YZ':
        x = [0, 0]
        y = [0, length * np.cos(angle_rad)]
        z = [0, length * np.sin(angle_rad)]
    return x, y, z


coax =35
femur = 120
tibia = 140

def update(val):
    rz= rh_slider.val
    
    fz=fz_slider.val
    fy=fy_slider.val
    fx=fx_slider.val
    b1=fy
    b2=coax
    a1=rz
    
    ax.clear()
    ax.plot([0, 0], [0,0], [0, rz ], color='green', label='')
    ax.plot([0, 300], [0,0], [rz, rz ], color='green', label='robot length')
    ax.plot([300, 300], [0,0], [0, rz ], color='green', label='')
    ax.plot([0, 0], [0,150], [rz, rz ], color='green', label='robot width')
    ax.plot([0, 0], [0,150], [rz, rz ], color='green', label='')
    ax.plot([0, 300], [150,150], [rz, rz ], color='green', label='')
    ax.plot([300, 300], [150,0], [rz, rz ], color='green', label='')
    
    print(f'a1 is {a1}')
    c=math.sqrt(pow(a1,2)+pow(fy,2))
    print(f'c is {c}')
    A1=math.degrees(math.asin(a1/c))
    print(f'A1 is {A1}')
    B1=math.degrees(math.asin(b1/c))
    print(f'B1 is {B1}')
    # Top triangle
    a2=math.sqrt(pow(c,2)-pow(b2,2))
    print(f'a2 is {a2}')
    A2=math.degrees(math.asin(a2/c))
    print(f'A2 is {A2}')
    B2=math.degrees(math.asin(b2/c))
    print(f'B2 is {B2}')
    coaxra=int(B1+A2)
    print(f'B1+A2 is {coaxra}')
    z=a1
    if a1 > femur+tibia :
        exit
    x, y, z = calculate_line(coax, dega[coaxra], "YZ")
    ax.plot([0,x[1]], [0,y[1]], [rz,z[1]+rz], color='green', label='coax')
    lx=x[1]
    ly=y[1]+coax
    lz=z[1]
    fa=coaxra+90
    print(f'fa is {fa}')
    x, y, z = calculate_line(a2, dega[fa], "YZ")
    ax.plot([lx,x[1]], [ly,y[1]+fy], [lz,z[1]], color='blue', label='Femur + tibia')
    ax.set_xlim(-400, 400)
    ax.set_ylim(-400, 400)
    ax.set_zlim(-100, 400)
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')


fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

dega=[]
cc=270
for i in range(361):
        dega.append(cc)
        cc=cc-1
        if cc<1:
          cc=360

fz_slider = Slider(plt.axes([0.1, 0.08, 0.65, 0.03]), 'fZ', -200, 200, valinit=0)
fy_slider = Slider(plt.axes([0.1, 0.04, 0.65, 0.03]), 'fY', -200, 200, valinit=coax)
fx_slider = Slider(plt.axes([0.1, 0.12, 0.65, 0.03]), 'fX', -200, 200, valinit=0)
rh_slider = Slider(plt.axes([0.1, 0.15, 0.65, 0.03]), 'RH', 0, 250, valinit=150)

fz_slider.on_changed(update)
fy_slider.on_changed(update)
fx_slider.on_changed(update)
rh_slider.on_changed(update)

plt.show()

其中dega数组用于修正角度方向。

迁移需求与问题

需将上述模型迁移至Matlab 3D空间，实现通过滑块控制机械足端点位置，并获取对应伺服角度。当前在存储前一段连杆终点作为下一段起点时出现点位对齐问题，对应Python代码行：

lx=x[1]
ly=y[1]+coax
lz=z[1]

已知连杆参数：coax=35、femur=120、tibia=140。

解决方案与Matlab实现

问题根源

Python代码中ly=y[1]+coax属于错误偏移：calculate_line返回的是相对于当前起点的坐标偏移，连杆终点应直接是起点加偏移量，无需额外叠加连杆长度。

完整Matlab代码

% 连杆参数定义
coax = 35;
femur = 120;
tibia = 140;

% 生成角度修正数组dega（与Python逻辑一致）
dega = zeros(1, 361);
cc = 270;
for i = 1:361
    dega(i) = cc;
    cc = cc - 1;
    if cc < 1
        cc = 360;
    end
end

% 创建可视化窗口与3D轴
fig = figure('Position', [100, 100, 800, 600]);
ax = axes('Parent', fig, 'Projection', '3d', 'Position', [0.1, 0.2, 0.8, 0.7]);
xlim(ax, [-400, 400]);
ylim(ax, [-400, 400]);
zlim(ax, [-100, 400]);
xlabel(ax, 'X');
ylabel(ax, 'Y');
zlabel(ax, 'Z');
grid(ax, 'on');

% 创建控制滑块
slider_rh = uicontrol('Parent', fig, 'Style', 'slider', 'Position', [100, 10, 600, 20], ...
    'Min', 0, 'Max', 250, 'Value', 150, 'Callback', @update_plot);
uicontrol('Parent', fig, 'Style', 'text', 'Position', [50, 10, 40, 20], 'String', 'RH');

slider_fx = uicontrol('Parent', fig, 'Style', 'slider', 'Position', [100, 40, 600, 20], ...
    'Min', -200, 'Max', 200, 'Value', 0, 'Callback', @update_plot);
uicontrol('Parent', fig, 'Style', 'text', 'Position', [50, 40, 40, 20], 'String', 'fX');

slider_fy = uicontrol('Parent', fig, 'Style', 'slider', 'Position', [100, 70, 600, 20], ...
    'Min', -200, 'Max', 200, 'Value', coax, 'Callback', @update_plot);
uicontrol('Parent', fig, 'Style', 'text', 'Position', [50, 70, 40, 20], 'String', 'fY');

slider_fz = uicontrol('Parent', fig, 'Style', 'slider', 'Position', [100, 100, 600, 20], ...
    'Min', -200, 'Max', 200, 'Value', 0, 'Callback', @update_plot);
uicontrol('Parent', fig, 'Style', 'text', 'Position', [50, 100, 40, 20], 'String', 'fZ');

% 初始化绘图
update_plot([]);

% 核心更新函数
function update_plot(~)
    % 获取滑块值
    rz = slider_rh.Value;
    fx = slider_fx.Value;
    fy = slider_fy.Value;
    fz = slider_fz.Value;
    
    b1 = fy;
    b2 = coax;
    a1 = rz;
    
    % 清除当前轴内容
    cla(ax);
    
    % 绘制机器人基座框架
    plot3(ax, [0, 0], [0, 0], [0, rz], 'g');
    plot3(ax, [0, 300], [0, 0], [rz, rz], 'g', 'DisplayName', 'robot length');
    plot3(ax, [300, 300], [0, 0], [0, rz], 'g');
    plot3(ax, [0, 0], [0, 150], [rz, rz], 'g', 'DisplayName', 'robot width');
    plot3(ax, [0, 300], [150, 150], [rz, rz], 'g');
    plot3(ax, [300, 300], [150, 0], [rz, rz], 'g');
    legend(ax, 'Location', 'best');
    
    % 逆运动学计算
    c = sqrt(a1^2 + fy^2);
    A1 = rad2deg(asin(a1 / c));
    B1 = rad2deg(asin(b1 / c));
    
    a2 = sqrt(c^2 - b2^2);
    A2 = rad2deg(asin(a2 / c));
    B2 = rad2deg(asin(b2 / c));
    
    coaxra = round(B1 + A2);
    coaxra = mod(coaxra - 1, 360) + 1; % 确保索引在1-360范围内
    
    % 绘制coax连杆：明确起点与终点
    [x_coax, y_coax, z_coax] = calculate_line(coax, dega(coaxra), 'YZ');
    start_coax = [0, 0, rz];
    end_coax = start_coax + [x_coax(2), y_coax(2), z_coax(2)];
    plot3(ax, [start_coax(1), end_coax(1)], ...
        [start_coax(2), end_coax(2)], ...
        [start_coax(3), end_coax(3)], 'g', 'DisplayName', 'coax');
    
    % 绘制femur+tibia连杆：以coax终点为起点
    fa = coaxra + 90;
    fa = mod(fa - 1, 360) + 1; % 确保索引合法
    [x_ft, y_ft, z_ft] = calculate_line(a2, dega(fa), 'YZ');
    start_ft = end_coax;
    end_ft = start_ft + [x_ft(2), y_ft(2), z_ft(2)];
    plot3(ax, [start_ft(1), end_ft(1)], ...
        [start_ft(2), end_ft(2)], ...
        [start_ft(3), end_ft(3)], 'b', 'DisplayName', 'Femur + tibia');
    
    % 输出伺服角度（可根据需求调整输出位置）
    disp(['Coax伺服角度: ', num2str(dega(coaxra)), '°']);
    disp(['Femur伺服角度: ', num2str(dega(fa)), '°']);
    
    % 重置轴范围
    xlim(ax, [-400, 400]);
    ylim(ax, [-400, 400]);
    zlim(ax, [-100, 400]);
end

% 坐标计算函数（与Python逻辑一致，返回相对偏移）
function [x, y, z] = calculate_line(length, angle, axis)
    angle_rad = deg2rad(angle);
    x = [0, 0];
    y = [0, 0];
    z = [0, 0];
    
    switch axis
        case 'XY'
            x(2) = length * cos(angle_rad);
            y(2) = length * sin(angle_rad);
        case 'XZ'
            x(2) = length * cos(angle_rad);
            z(2) = length * sin(angle_rad);
        case 'YZ'
            y(2) = length * cos(angle_rad);
            z(2) = length * sin(angle_rad);
    end
end

关键修正点

连杆坐标传递逻辑：直接将前一段连杆的终点坐标作为下一段的起点，不再添加额外偏移，确保点位完全对齐。
角度索引合法性：使用mod运算确保角度数组索引始终在1-360范围内，避免越界错误。
明确坐标关系：区分起点坐标与相对偏移量，绘图时通过起点加偏移计算终点，逻辑更清晰。

内容的提问来源于stack exchange，提问作者Fulcrum Mason

基于Matlab的机械臂逆运动学3D建模点位对齐问题求助

背景与现有Python实现 #

迁移需求与问题 #

解决方案与Matlab实现 #

问题根源 #

完整Matlab代码 #