【机器人学】2-4.腕部偏置的6自由度机器人运动学逆解【附MATLAB代码】

鱼会上树HY

于 2025-05-29 08:36:38 发布

阅读量272

点赞数 10

分类专栏：机器人学文章标签：机器人

本文链接：https://blog.csdn.net/y12345655/article/details/148286331

版权

机器人学专栏收录该内容

24 篇文章

订阅专栏

1. 理论推导

则该腕部偏执的机器人M-DH参数

由于Pieper准则：当如下条件之一满足时，一个6自由度运动学结构具有闭合形式的运动学逆解。

三个连续的转动关节的轴相交于同一点（UR机器人），二、三个连续的转动关节的轴平行（工业机器人）。当机器人腕部偏执时，不符合Pieper准则，因此该机器人没有解析解。只有数值解法，而本文采用数值与解析结合的方式求解。将机器人转换为腕部无偏执情况求解析解，计算腕部坐标。在更新腕部坐标迭代求解。

2. matlab代码

function [resultAll,jointPose] = IKanalytic_offWrist(DH,position,attitude,flag,flag2)
% result = 1;
d3 = DH(1);
a5 = DH(2);
d6 = DH(3);
ZERO = 0.001;

if(flag==1)
   a5 = a5 + d6;
end
resultAll = [1,1,1,1,1,1,1,1];
jointPose = [0,0,0,0,0,0];

px = position(1);
py = position(2);
pz = position(3);
a = attitude;
%目标位置姿态矩阵
nx=a(1,1);ox=a(1,2);ax=a(1,3);
ny=a(2,1);oy=a(2,2);ay=a(2,3);
nz=a(3,1);oz=a(3,2);az=a(3,3);

% 求解q2
AA = (px^2+py^2+pz^2+d3^2-a5^2)/(2*d3);
if(abs(AA/sqrt(px^2+py^2))>=1)
    resultAll = [0,0,0,0,0,0,0,0];
    return;
end
q2_1 = atan2(AA,sqrt(px^2+py^2-AA^2))-atan2(py,-px);
q2_2 = atan2(AA,-sqrt(px^2+py^2-AA^2))-atan2(py,-px);
% 求解q3
% q3_1 = atan2(0,1)-atan2(-pz,-cos(q2_1)*px-sin(q2_1)*py);
% q3_2 = atan2(0,1)-atan2(-pz,-cos(q2_2)*px-sin(q2_2)*py);
% q3_3 = atan2(0,-1)-atan2(-pz,-cos(q2_1)*px-sin(q2_1)*py);
% q3_4 = atan2(0,-1)-atan2(-pz,-cos(q2_2)*px-sin(q2_2)*py);
q3_1 = -atan2(-pz,-cos(q2_1)*px-sin(q2_1)*py);
q3_2 = -atan2(-pz,-cos(q2_2)*px-sin(q2_2)*py);
q3_3 = pi-atan2(-pz,-cos(q2_1)*px-sin(q2_1)*py);
q3_4 = pi-atan2(-pz,-cos(q2_2)*px-sin(q2_2)*py);
% 求解q4
BB_1 = -sin(q2_1)*px+cos(q2_1)*py-d3;
CC_1 = cos(q2_1)*cos(q3_1)*px+sin(q2_1)*cos(q3_1)*py-sin(q3_1)*pz;
CC_2 = cos(q2_1)*cos(q3_3)*px+sin(q2_1)*cos(q3_3)*py-sin(q3_3)*pz;
if(abs(BB_1/a5)<=1||abs(CC_1/a5)<=1)
    q4_1 = atan2(BB_1,CC_1);
else
    resultAll(1:2) = 0;
    resultAll(5:6) = 0;
end

if(abs(BB_1/a5)<=1||abs(CC_2/a5)<=1)
    q4_2 = atan2(BB_1,CC_2);
else
    resultAll(3:4) = 0;
    resultAll(7:8) = 0;
end
q4_3 = q4_1;
q4_4 = q4_2;
% 求解q6
D_1 = -cos(q2_1)*sin(q3_1)*nx-sin(q2_1)*sin(q3_1)*ny-cos(q3_1)*nz;
D_2 = -cos(q2_1)*sin(q3_3)*nx-sin(q2_1)*sin(q3_3)*ny-cos(q3_3)*nz;
D_3 = -cos(q2_2)*sin(q3_2)*nx-sin(q2_2)*sin(q3_2)*ny-cos(q3_2)*nz;
D_4 = -cos(q2_2)*sin(q3_4)*nx-sin(q2_2)*sin(q3_4)*ny-cos(q3_4)*nz;

E_1 = cos(q2_1)*sin(q3_1)*ox+sin(q2_1)*sin(q3_1)*oy+cos(q3_1)*oz;
E_2 = cos(q2_1)*sin(q3_3)*ox+sin(q2_1)*sin(q3_3)*oy+cos(q3_3)*oz;
E_3 = cos(q2_2)*sin(q3_2)*ox+sin(q2_2)*sin(q3_2)*oy+cos(q3_2)*oz;
E_4 = cos(q2_2)*sin(q3_4)*ox+sin(q2_2)*sin(q3_4)*oy+cos(q3_4)*oz;

F_1 = cos(q2_1)*sin(q3_1)*ax+sin(q2_1)*sin(q3_1)*ay+cos(q3_1)*az;
F_2 = cos(q2_1)*sin(q3_3)*ax+sin(q2_1)*sin(q3_3)*ay+cos(q3_3)*az;
F_3 = cos(q2_2)*sin(q3_2)*ax+sin(q2_2)*sin(q3_2)*ay+cos(q3_2)*az;
F_4 = cos(q2_2)*sin(q3_4)*ax+sin(q2_2)*sin(q3_4)*ay+cos(q3_4)*az;

q6_1 = atan2(sqrt(D_1^2+E_1^2),F_1);
q6_2 = atan2(sqrt(D_2^2+E_2^2),F_2);
q6_3 = atan2(sqrt(D_3^2+E_3^2),F_3);
q6_4 = atan2(sqrt(D_4^2+E_4^2),F_4);
q6_5 = -q6_1;
q6_6 = -q6_2;
q6_7 = -q6_3;
q6_8 = -q6_4;

if(abs(sin(q6_1))<ZERO)
    resultAll(1) = 0;
    resultAll(2) = 0;
end
if(abs(sin(q6_2))<ZERO)
    resultAll(3) = 0;
    resultAll(4) = 0;
end
if(abs(sin(q6_3))<ZERO)
    resultAll(5) = 0;
    resultAll(6) = 0;
end
if(abs(sin(q6_4))<ZERO)
    resultAll(7) = 0;
    resultAll(8) = 0;
end
% 求解q7
q7_1 = atan2(E_1/sin(q6_1),D_1/sin(q6_1));
q7_2 = atan2(E_2/sin(q6_2),D_2/sin(q6_2));
q7_3 = atan2(E_3/sin(q6_3),D_3/sin(q6_3));
q7_4 = atan2(E_4/sin(q6_4),D_4/sin(q6_4));
q7_5 = q7_2;
q7_6 = q7_1;
q7_7 = q7_4;
q7_8 = q7_3;

% 求解theta5
G_1 = (cos(q2_1)*cos(q3_1)*cos(q4_1)-sin(q2_1)*sin(q4_1))*ax+(sin(q2_1)*cos(q3_1)*cos(q4_1)+cos(q2_1)*sin(q4_1))*ay-sin(q3_1)*cos(q4_1)*az;
G_3 = (cos(q2_2)*cos(q3_2)*cos(q4_3)-sin(q2_2)*sin(q4_3))*ax+(sin(q2_2)*cos(q3_2)*cos(q4_3)+cos(q2_2)*sin(q4_3))*ay-sin(q3_2)*cos(q4_3)*az;

H_1 = (-cos(q2_1)*cos(q3_1)*sin(q4_1)-sin(q2_1)*cos(q4_1))*ax+(-sin(q2_1)*cos(q3_1)*sin(q4_1)+cos(q2_1)*cos(q4_1))*ay+sin(q3_1)*sin(q4_1)*az;
H_3 = (-cos(q2_2)*cos(q3_2)*sin(q4_3)-sin(q2_2)*cos(q4_3))*ax+(-sin(q2_2)*cos(q3_2)*sin(q4_3)+cos(q2_2)*cos(q4_3))*ay+sin(q3_2)*sin(q4_3)*az;

q5_1 = atan2(H_1/sin(q6_1),G_1/sin(q6_1));
q5_2 = -q5_1;
q5_3 = atan2(H_3/sin(q6_3),G_3/sin(q6_3));
q5_4 = -q5_3;
q5_5 = atan2(H_1/sin(q6_5),G_1/sin(q6_5));
q5_6 = -q5_5;
q5_7 = atan2(H_3/sin(q6_7),G_3/sin(q6_7));
q5_8 = -q5_7;

jointPose2 = [ 
              q2_1,q3_1,q4_1,q5_1,q6_1,q7_1;     
              q2_1,q3_1,q4_1,q5_5,q6_5,q7_5;
              q2_1,q3_3,q4_2,q5_2,q6_2,q7_2;
              q2_1,q3_3,q4_2,q5_6,q6_6,q7_6;
              q2_2,q3_2,q4_3,q5_3,q6_3,q7_3;
              q2_2,q3_2,q4_3,q5_7,q6_7,q7_7;
              q2_2,q3_4,q4_4,q5_4,q6_4,q7_4;
              q2_2,q3_4,q4_4,q5_8,q6_8,q7_8;
             ]*180/pi;
if(flag2==1)
    jointPose = jointPose2;
    return
end
for j=1:8
    if(resultAll(j)==1)
        jointPose = jointPose2(j,:);
        return
    end
    if(j==8)  
        return
    end
end
end


function result = FKOffsetW(theta,DH,flag)
q = [0,theta]/180*pi;
d3 = DH(1);
a5 = DH(2);
d6 = DH(3);
d7 = DH(4);
if(flag==1)
    d6 = 0;
end
T12 = connectingRodTransfer([0, 0, 0, q(2)],0);
T23 = connectingRodTransfer([-pi/2, 0, d3, q(3)],0);
T34 = connectingRodTransfer([pi/2, 0, 0, q(4)],0);
T45 = connectingRodTransfer([0, a5, 0, q(5)],0);
T56 = connectingRodTransfer([-pi/2, 0, d6, q(6)],0);
T67 = connectingRodTransfer([pi/2, 0, d7, q(7)],0);
result =T12*T23*T34*T45*T56*T67;
end
function TArr = connectingRodTransfer(dh,theta,inv)
if nargin == 2
    inv=0;
end
ct = cos(dh(4)+theta);
st = sin(dh(4)+theta);
ca = cos(dh(1));
sa = sin(dh(1));
a = dh(2);
d = dh(3);
TArr = [    ct      -st     0       a;
            st*ca   ct*ca   -sa     -sa*d;
            st*sa   ct*sa   ca      ca*d;
            0       0       0       1   ];
if inv
    TArr=[TArr(1:3,1:3)' -TArr(1:3,1:3)'*TArr(1:3,4);0 0 0 1];
end
end

测试代码

clc;
clear;
format short

SingularityNum = 0;
overPlane = 0;
NANum = 0;
successNum = 0;
compNum = 10000;
NANumData = [];
iterNumContainer = [];
itrnum = 20;
ZERO = 1e-5;
d3 = 0.425;
d7 = 0.1026;
d6 = 0.106;
a5 = 0.393;
DH = [d3,a5,d6,d7];
tic;
for k=1:compNum
for i=1:6
    q(i) = rand()*2*pi;
end

T = offsetW(q*180/pi,DH,0);
position(1) = T(1,4) - T(1,3)*d7;
position(2) = T(2,4) - T(2,3)*d7;
position(3) = T(3,4) - T(3,3)*d7;
attitude = T(1:3,1:3);

[result,joint_re2] = IKanalytic_offWrist(DH,position,attitude,1,1);

if(norm(result)==0)
    SingularityNum = SingularityNum+1;
    continue;
end
delta = 10;
lastdelta = delta;
iterNum = 0;
for j = 1:2:8
    if(delta<=ZERO&&i<=itrnum)
        break;
    end
    if(result(j)==0)
        continue;
    end
    joint = joint_re2(j,:)/180*pi;
    for i=1:itrnum
         iterNum = iterNum+1;
         position3(1) =  T(1,4)-d6*sin(joint(6))*attitude(1,1)-d6*cos(joint(6))*attitude(1,2)- T(1,3)*d7;
         position3(2) =  T(2,4)-d6*sin(joint(6))*attitude(2,1)-d6*cos(joint(6))*attitude(2,2)- T(2,3)*d7;
         position3(3) =  T(3,4)-d6*sin(joint(6))*attitude(3,1)-d6*cos(joint(6))*attitude(3,2)- T(3,3)*d7;
        [result2,joint_re] = IKanalytic_offWrist(DH,position3,attitude,0,1);
        if(result2(j)==0)
            break;
        end
        joint = joint_re(j,:)/180*pi;
        T2 = offsetW(joint*180/pi,DH,0);
        delta = norm((T2(1:3,4)-T(1:3,4)));
        if(delta<ZERO)
            successNum = successNum+1;
            break
        end
    end
end
if(delta>ZERO&&i==itrnum)
    NANumData = [NANumData;q];
    NANum = NANum+1;
end
end
toc;
[SingularityNum compNum-successNum-SingularityNum-NANum NANum successNum]

随机生成10000组关节角，进行正解，然后进行逆解。结果为：