[robot] review forward kinematics

本文介绍了DH参数在机器人运动学中的应用,包括如何通过DH参数确定机器人关节间的相对位置和姿态,给出了具体的数学表达式及计算实例。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

DH parameters

  1. find common normal of Zi and Zi+1.

  2. place new X axis Xi+1 along this common normal.

  3. d is the depth from the origin of Zi to the common normal along the Zi+1 axis

  4. θ rotates about previous Z axis to align its X with the new origin

  5. a is the distance along the rotated X axis.

  6. α rotates about the new X axis to put Z in its desired orientation.

Special Case: two Z axes are parallel.
choose any convenient d.

dh

example

00

00

example

00

00

dθaα
3θ10π/2
0-π/2+θ25.750
0θ3+π/23.3750
0θ40-π/2
4.125θ500
function [ pos ] = arm_forward_kinematics( theta1, theta2, theta3, theta4, theta5, g )
%The input to the function will be the joint
%    angles of the robot in radians, and the distance between the gripper pads in inches.
%    The output must contain 10 positions of various points along the robot arm as specified
%    in the question.
    % parameters
    a = 3;
    b = 5.75;
    c = 7.375;
    d = 4.125;
    e = 1.125;
    pos = zeros(10, 4);
    % transformation matrix A1
    A1 = compute_dh_matrix(0, -0.5*pi, a, theta1);

    % transformation matrix A2
    A2 = compute_dh_matrix(b, 0, 0, theta2-0.5*pi);
    % transformation matrix A4
    A3 = compute_dh_matrix(c, 0, 0, theta3+0.5*pi);
    % transformation matrix A4
    A4 = compute_dh_matrix(0, -0.5*pi, 0, theta4-0.5*pi);
    % transformation matrix A5
   %A5 = compute_dh_matrix(0, 0, d, theta5);
    pos(1,4) = 1;
    value = A1*([0,0,0,1]');
    pos(2,:) = value;
    value = A1*A2*([0,0,0,1]');
    pos(3,:) = value;
    value = A1*A2*A3*([0,0,0,1]');
    pos(4,:) = value;
    value = A1*A2*A3*A4*([0,0,0,1]');
    pos(5,:) = value;
    %
    A = A1*A2*A3*A4;
    %temp = A*([c+d,0,a+b,1]');
    temp = A * compute_dh_matrix(0,0,d-e,theta5);
    pos(6,:) = temp*[0,0,0,1]';
    temp = A* compute_dh_matrix(0.5*g, 0, d-e,theta5);
    pos(7,:) = temp*[0,0,0,1]';
    temp = A* compute_dh_matrix(-0.5*g, 0, d-e,theta5);
    pos(8,:) = temp*[0,0,0,1]';
    temp = A* compute_dh_matrix(0.5*g, 0, d,theta5);
    pos(9,:) = temp*[0,0,0,1]';
    temp = A* compute_dh_matrix(-0.5*g, 0, d,theta5);
    pos(10,:) =  temp*[0,0,0,1]';
    pos = pos(:,1:3);
end

function A = compute_dh_matrix(a, alpha, d, theta)
%% standard DH parameters
ctheta = cos(theta);
stheta = sin(theta);
salpha = sin(alpha);
calpha = cos(alpha);
A = [ctheta, -stheta*calpha, stheta*salpha, a*ctheta;
    stheta, ctheta*calpha, -ctheta*salpha, a*stheta;
    0,salpha, calpha,d;
    0,0,0,1];
end

example

-

00

function A = compute_dh_matrix(a, alpha, d, theta)
%% standard DH parameters
ctheta = cos(theta);
stheta = sin(theta);
salpha = sin(alpha);
calpha = cos(alpha);
A = [ctheta,-stheta,0,a;
    stheta*calpha,ctheta*calpha,-salpha,-salpha*d;
    stheta*salpha,ctheta*salpha,calpha,calpha*d;
    0,0,0,1];
end
>> theta1 = 15/180*pi;
>> theta2=-40/180*pi;
>> theta3=-50/180*pi;
>> theta4=pi/6;
>> theta5=70/180*pi;
>> theta5=25/180*pi;
>> theta5=70/180*pi;
>> theta6=25/180*pi;
>> t1 = compute_dh_matrix(0,0,0,theta1);
>> t2=compute_dh_matrix(0,-pi/2,220,theta2);
>> t3=compute_dh_matrix(430,0,-90,theta3);
>> t4=compute_dh_matrix(0,-pi/2,430,theta4);
>> t5=compute_dh_matrix(0,pi/2,0,theta5);
>> t6=compute_dh_matrix(0,-pi/2,0,theta6);
>> t1*t2*t3*t4 % frame{4} 下的原点在frame{0}下的坐标[699.88,322.12,276.40]

ans =

    0.1294    0.2241    0.9659  699.8767
   -0.4830   -0.8365    0.2588  322.1173
    0.8660   -0.5000   -0.0000  276.3987
         0         0         0    1.0000

>> t1*t2*t3*t4*t5*t6

ans =

    0.9575   -0.1992    0.2088  699.8767
   -0.2828   -0.7911    0.5424  322.1173
    0.0571   -0.5783   -0.8138  276.3987
         0         0         0    1.0000
<think>我们注意到用户在使用MATLAB进行三关节机械臂轨迹跟踪时,调用`forward_kinematics`函数出现'输出参数太多'的错误。这种错误通常发生在调用函数时,尝试接收比函数实际返回更多的输出参数。根据引用[1]中提到的MATLAB函数定义规则,以及引用[2]中关于逆运动学求解的代码示例,我们可以分析问题原因并给出解决方案。###错误原因分析在MATLAB中,当函数被调用时,如果调用语句中指定的输出参数数量超过了函数定义中实际返回的输出参数数量,就会产生"输出参数太多"的错误。例如,假设`forward_kinematics`函数定义为:```matlabfunctionpos=forward_kinematics(q,L1,L2,L3)```但调用时却尝试获取两个输出:```matlab[pos,joints]=forward_kinematics(qk,L1,L2,L3);%错误:函数只返回一个输出```###解决方案1.**检查函数定义**:确保函数声明中返回的输出参数数量与调用时要求的数量一致。2.**修改调用方式**:调整调用语句,使其与函数定义匹配。3.**修改函数定义**:如果实际需要多个输出,则修改函数定义以返回所需数量的输出。###具体步骤以您提供的`forward_kinematics`函数为例:####原函数定义```matlabfunction[pos,joints]=forward_kinematics(q,L1,L2,L3)%...函数体...end```####正确调用方式如果函数定义有两个输出参数(`pos`和`joints`),则调用时必须指定两个输出:```matlab[pos,joints]=forward_kinematics(q,L1,L2,L3);```####错误调用示例如果调用时只使用一个输出变量:```matlabpos=forward_kinematics(q,L1,L2,L3);%正确:只获取第一个输出```或者尝试获取三个输出:```matlab[pos,joints,other]=forward_kinematics(q,L1,L2,L3);%错误:函数只返回两个输出```###修改建议1.**检查函数文件位置**:确保调用的函数是您修改后的版本(MATLAB可能调用了其他路径的同名函数)。2.**检查函数定义**:在函数文件开头明确声明输出参数数量。3.**使用`narginchk`和`nargoutchk`**:在函数中添加参数数量检查,例如:```matlabfunction[pos,joints]=forward_kinematics(q,L1,L2,L3)narginchk(4,4);%检查输入参数数量nargoutchk(1,2);%检查输出参数数量%...函数体...end```###示例修正在您提供的原始代码中,`forward_kinematics`函数定义有两个输出参数:```matlabfunction[pos,joints]=forward_kinematics(q,L1,L2,L3)```因此,在调用此函数时,必须使用两个输出参数,如:```matlab%正确调用[x_act,y_act,z_act]=forward_kinematics(qk,L1,L2,L3);%错误!这里试图获取3个输出,但函数只返回2个%应改为:[pos,joints]=forward_kinematics(qk,L1,L2,L3);x_act=pos(1);y_act=pos(2);z_act=pos(3);```或者修改函数使其返回三个坐标值:```matlabfunction[x,y,z,joints]=forward_kinematics(q,L1,L2,L3)%...计算...x=end_effector(1);y=end_effector(2);z=end_effector(3);%...保留joints计算...end```###总结"输出参数太多"错误的根本原因是函数调用和定义之间的输出参数数量不匹配。请按照以下步骤解决:1.定位报错的调用语句2.检查函数定义文件中的输出参数声明3.调整调用方式或修改函数定义在机械臂控制代码中,特别注意`forward_kinematics`函数的调用位置(如动画循环和误差计算部分),确保每次调用都正确获取输出参数。§§相关问题§§1.MATLAB中如何调试"输入参数不足"的错误?2.如何避免MATLAB中的函数命名冲突?3.在机械臂控制中,还有哪些常见的函数调用错误?</think>## MATLAB调用forward_kinematics函数报错"输出参数太多"的解决方案 当调用`forward_kinematics`函数出现"输出参数太多"错误时,这是因为函数调用语句要求返回的参数数量超过了函数实际定义的输出参数数量。以下是详细的解决步骤: ### 错误原因分析 1. **函数定义输出参数不足**:函数只定义了`pos`作为输出,但调用时尝试获取多个输出 ```matlab function pos = forward_kinematics(q, L1, L2, L3) % 只定义了一个输出参数 ``` 2. **调用语句要求过多输出**: ```matlab % 错误调用 - 尝试获取两个输出 [x_act, y_act, z_act] = forward_kinematics(qk, L1, L2, L3); ``` ### 解决方案 #### 方法1:修改函数定义(推荐) 在函数定义中增加输出参数数量: ```matlab function [x, y, z, joints] = forward_kinematics(q, L1, L2, L3) % ... 原函数内容不变 ... x = end_effector(1); y = end_effector(2); z = end_effector(3); % joints 保持原样 end ``` #### 方法2:修改调用方式 如果不修改函数,调整调用方式: ```matlab % 正确调用方式1:只获取一个输出 pos = forward_kinematics(qk, L1, L2, L3); x_act = pos(1); y_act = pos(2); z_act = pos(3); % 正确调用方式2:使用结构体 result = forward_kinematics(qk, L1, L2, L3); x_act = result.x; y_act = result.y; z_act = result.z; ``` ### 完整修正示例 ```matlab %% 修正后的forward_kinematics函数 function [x, y, z, joints] = forward_kinematics(q, L1, L2, L3) q1 = q(1); q2 = q(2); q3 = q(3); % 关节位置计算 joint0 = [0; 0; 0]; joint1 = [0; 0; 0]; joint2 = [L1*cos(q1)*cos(q2); L1*sin(q1)*cos(q2); L1*sin(q2)]; joint3 = [L1*cos(q1)*cos(q2) + L2*cos(q1)*cos(q2+q3); L1*sin(q1)*cos(q2) + L2*sin(q1)*cos(q2+q3); L1*sin(q2) + L2*sin(q2+q3)]; end_effector = [L1*cos(q1)*cos(q2) + L2*cos(q1)*cos(q2+q3) + L3*cos(q1)*cos(q2+q3); L1*sin(q1)*cos(q2) + L2*sin(q1)*cos(q2+q3) + L3*sin(q1)*cos(q2+q3); L1*sin(q2) + L2*sin(q2+q3) + L3*sin(q2+q3)]; joints = [joint0, joint1, joint2, joint3, end_effector]; x = end_effector(1); y = end_effector(2); z = end_effector(3); end %% 修正后的调用代码 [pos, joints] = forward_kinematics(qk, L1, L2, L3); % 或者 [x_act, y_act, z_act, joints] = forward_kinematics(qk, L1, L2, L3); ``` ### 调试建议 1. **使用`nargout`检查输出数量**: ```matlab function varargout = forward_kinematics(q, L1, L2, L3) % ... 计算 ... outputs = {x, y, z, joints}; varargout = outputs(1:nargout); end ``` 2. **添加参数检查**: ```matlab function [x, y, z, joints] = forward_kinematics(q, L1, L2, L3) if nargout > 4 error('太多输出参数:最多支持4个输出'); end % ... 函数主体 ... end ``` ### 预防措施 1. **保持函数定义与调用一致**:定义和调用时使用相同数量的输出参数 2. **使用显式输出声明**:在函数开头明确指定输出参数 3. **文档化函数接口**:使用帮助注释说明输出参数 ```matlab % FORWARD_KINEMATICS 计算机械臂正运动学 % [x, y, z] = FORWARD_KINEMATICS(q, L1, L2, L3) 返回末端位置 % [x, y, z, joints] = ... 同时返回关节位置 ``` 通过以上修正,可以解决"输出参数太多"的错误,确保机械臂轨迹跟踪程序正常运行[^1][^2]。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

matdodo

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值