优雅的求解带肘偏执的七自由度机械臂封闭逆解

最近做带偏置的七轴机械臂逆解算法，经过好几个晚上的学习，基本上算是完成了，还有一些细节有待完善，比如08组解，臂角解算出来目前还没有时间做，后面在逐步完善吧。该版本给出了一组逆解并用于验证了这组逆解的正解的正确。
版本：MATLAB 2019 a
工具：实时脚本

目录
1. 机械臂构型与坐标说明
2.解算函数部分
（1）RTB工具性可视化
        a. SDH 参数求解
        b. MDH 参数求解
    （2）正运动学求解
        a.SDH正运动学
        b.MDH正运动学
    （3）逆运动学解算
        1）求解初始逆解
        2）求解期望逆解
附录：MATLAB函数库
clc; 
clear all;

%% 带轴偏置机械臂运动学解算
%  Refer: [1]《肘部偏置七自由度机械臂运动学研究》————哈尔滨工业大学
%         [2]《冗余空间机器人操作臂：运动学、轨迹规划及控制》 徐文福著 2017.11
%         [3]《Introduction to Robotics:Mechanics and Control》 John J. Craig
%             Third Edition  2014
%  Methods: 臂型角 phi
%  Date:2019/11/29
%  Update:2019/12/07
%  Author:EasyR
%  Input: 末端位置和姿态[px,py,pz,r,p,y];
%         臂型角phi;
%  Output: 关节角θi

1. 机械臂构型与坐标说明



2.解算函数部分
（1）RTB工具性可视化
% Structure RTB
% Structure Parameters
Dbs=0.45; Dse=1.5; Dmm=0.3; Dew=1.5; Dwt=0.45;
a. SDH 参数求解


% theta1=-pi/2; theta2=0; theta3=pi; theta4=0; theta5=pi; theta6=0; theta7=pi/2;
% alpha1=-pi/2; alpha2=pi/2; alpha3=-pi/2; alpha4=pi/2; alpha5=-pi/2; alpha6=pi/2; alpha7=0; 
% 
% % Input vectors
% th=[theta1 theta2 theta3 theta4 theta5 theta6 theta7]';
% alp=[alpha1 alpha2 alpha3 alpha4 alpha5 alpha6 alpha7]';
% d=[Dbs 0 Dse Dmm Dew 0 Dwt]';
% % DH       th     d     a     alpha
% L1 = Link([th(1), d(1), 0, alp(1)  0], 'standard');
% L2 = Link([th(2), d(2), 0, alp(2)  0], 'standard');
% L3 = Link([th(3), d(3), 0, alp(3)  0], 'standard');
% L4 = Link([th(4), d(4), 0, alp(4)  0], 'standard');
% L5 = Link([th(5), d(5), 0, alp(5)  0], 'standard');
% L6 = Link([th(6), d(6), 0, alp(6)  0], 'standard');
% L7 = Link([th(7), d(7), 0, alp(7)  0], 'standard');
% 
% RobotSeven = SerialLink([L1 L2 L3 L4 L5 L6 L7]);
% RobotSeven.name = 'RobotSevenR';
% RobotSeven.comment = 'easyR';
% RobotSeven.display();
% % Forward Pose Kinematics
% % subplot(2,1,1);
% RobotSeven.teach(th');
% RobotSeven.plot(th');
b. MDH 参数求解
   
% Input vectors
d2=[Dbs 0 Dse Dmm Dew 0 Dwt]';
th2=[0 0 0 0 0 0 0 ]';
a2=[0 0 0 0 0 0 0]';
alp2=[0 -pi/2 pi/2 -pi/2 pi/2 -pi/2 pi/2 ]';
% Structure RTB
% DH       thi      di   ai-1   alphai-1
L1 = Link([th2(1), d2(1), a2(1), alp2(1)], 'modified');
L2 = Link([th2(2), d2(2), a2(2), alp2(2)], 'modified');
L3 = Link([th2(3), d2(3), a2(3), alp2(3)], 'modified');
L4 = Link([th2(4), d2(4), a2(4), alp2(4)], 'modified');
L5 = Link([th2(5), d2(5), a2(5), alp2(5)], 'modified');
L6 = Link([th2(6), d2(6), a2(6), alp2(6)], 'modified');
L7 = Link([th2(7), d2(7), a2(7), alp2(7)], 'modified');%坐标建到tool

RobotSeven = SerialLink([L1 L2 L3 L4 L5 L6 L7]);
RobotSeven.name = 'RobotSevenR';
RobotSeven.comment = 'easyR';
RobotSeven.display();
% RobotSeven.plot(th2'); 
% RobotSeven.teach(th2')
（2）正运动学求解
a.SDH正运动学
%% 略

b.MDH正运动学

%% 计算工具端的位置和姿态
dh=myfunMDHTable(th2,d2,a2,alp2);
[T,T10,T20,T30,T40,T50,T60,T70]=myfunTransMatrix(dh);% 输出末端pos


（3）逆运动学解算
%% 采用臂角计算的方式解算七轴，当d4=0时构成零偏置机械臂
%  input：输入位置点坐标和姿态[px,py,pz,r,p,y] ,6 by 1 matrix
%  outPut：输出关节角q,column vector 7by1
%  update:
%  state:


% 将结构参数在body coordinate中表示成矢量形式
lbs0=[0 0 Dbs]';
lse3=[0 0 Dse]';
lmm3=[0 Dmm 0]';
lew5=[0 0 Dew]';
lwt7=[0 0 Dwt]';

%——————————————————————————————————————————————————————————————————————————————————————————————%
  %矢量封闭
；
% 从点S指向W的矢量xsw0
%% 验算上述表达式正确性
%
R30=T30(1:3,1:3);  
T53=T(:,:,4)*T(:,:,5); R53=T53(1:3,1:3);
T75=T(:,:,6)*T(:,:,7); R75=T75(1:3,1:3);
R70=T70(1:3,1:3)  ;

r70=R30*R53*R75  ;
x70=lbs0+R30*((lse3+lmm3)+R53*(lew5+R75*lwt7)) ; %验证正确

;
%% 验算上述表达式正确性
%
xsw0=x70-lbs0-R70*lwt7 ;    %验算正确
% 由上述方程推导可以看出，当末端pos确定后，xsw0随之确定

%————————————————————————————————————————————————————————————————————————————————————————————%
=+(1-cos)  %绕任意直线sw旋转角度的坐标变换矩阵，3 by 3
，表示当=0时的变换矩阵
引入臂型角求解过程：
1）得到th3=0,phi=phi0时臂型；
2）肘部关节绕直线sw转动phi，得到最终臂型；
解算逆解过程：
1）解算th3=0,phi=phi0的逆解；初始逆解Initial
2）解算绕直线sw转动phi的逆解；期望逆解Expect
% solve the Initial ik
%已知工具端位姿[px,py,pz]和[r,p,y]
% 根据末端位姿求解 from base to tool的变换矩阵R70_Ecpect
% syms px py pz r p y 
px=2.44; py=0.3; pz=0.132; r=-pi; p=pi/4; y=pi;   %用于检验带值解算正确性
% R70_Expect = myfunRPY2R(y,p,r)     
R70_Expect =angle2dcm(r,p,y,'zyx');   %由欧拉角转化为旋转矩阵
1）求解初始逆解

%计算xsw0_Expect的矢量
xsw0_ExpectVector=[px;py;pz]-lbs0-R70_Expect*lwt7 ;       
Xsw0_Expect=norm(xsw0_ExpectVector);    %求取sw长度

%
%计算th4_In
dsw0_Initial2=Xsw0_Expect^2-Dmm^2;
temp4=(dsw0_Initial2-Dse^2-Dew^2)/(2*Dse*Dew);
th4_In=acos(temp4);    %两组解，一个正一个负
th4_In_1=th4_In;
th4_In_2=-th4_In;
% xsw0_Initial=R30*(lse3+lmm3+R53*lew5);

%
%计算th1_In和th2_In
=
syms th1_In th2_In pi
t10_in=myfunMatrixTrans(th1_In,0.45,0,0);
R100=t10_in(1:3,1:3);
t21_in=myfunMatrixTrans(th2_In,0,0,-pi/2);
R210=t21_in(1:3,1:3);
t32_in=myfunMatrixTrans(0,1.5,0,pi/2); %when alp3=0 or pi，Horizontal stretch；
R320=t32_in(1:3,1:3);
R300=R100*R210*R320;
%当末端关节固定时，肘部关节变换矩阵R43是一个定值，这样R430=R43
% xsw0_Expect=R300*(lse3+lmm3+R43*lew4)
lew4=[0; -Dew; 0];    %将ew转化为在肘部关节坐标{
     4}中的表达便于计算
t43=myfunMatrixTrans(th4_In_1,0,0,-pi/2);
R43=t43(1:3,1:3);
lsw3_In=lse3+lmm3+R43*lew4;
% xsw0_Temp=R300*lsw3_In
% xsw0_ExpectVector
% lsw3_In

%
%解算Th2_In
a=lsw3_In(3);b=-lsw3_In(1);c=xsw0_ExpectVector(3);
d=sqrt(a^2+b^2-c^2);
th2_In_1=atan2(b,a)+atan2(d,c);
th2_In_2=atan2(b,a)-atan2(d,c);

%
%解算Th1_In
a_temp=cos(th2_In_1)*lsw3_In(1)+sin(th2_In_1)*lsw3_In(3);
b_temp=lsw3_In(2);
c1_temp=xsw0_ExpectVector(2);
c2_temp=xsw0_ExpectVector(1);
num1=a_temp*c1_temp-b_temp*c2_temp;
num2=a_temp*c2_temp+b_temp*c1_temp;
den12=a_temp^2+b_temp^2;
th1_In_1 = vpa(atan2((num1/den12),(num2/den12)),6) ; %两组解之一

a_temp_2=cos(th2_In_2)*lsw3_In(1)+sin(th2_In_2)*lsw3_In(3);
b_temp=lsw3_In(2);
c1_temp=xsw0_ExpectVector(2);
c2_temp=xsw0_ExpectVector(1);
num1_2=a_temp_2*c1_temp-b_temp*c2_temp;
num2_2=a_temp_2*c2_temp+b_temp*c1_temp;
den12_2=a_temp_2.^2+b_temp^2;
th1_In_2 = atan2((num1_2/den12_2),(num2_2/den12_2)) ;%两组解之二

%
%解算th5_In、th6_In、th7_In

%前四个角已知情况下，根据R47对应项相等分别求解腕部关节角度
R300_Temp=subs(R300,[th1_In th2_In],[th1_In_1 th2_In_1]); %替换参数
R47_Temp=(R43.')*(R300_Temp.')*R70_Expect;

syms th5_In th6_In th7_In
T45=myfunMatrixTrans(th5_In,1.50,0, pi/2);
T56=myfunMatrixTrans(th6_In,0.00,0,-pi/2);
T67=myfunMatrixTrans(th7_In,0.45,0, pi/2);
T47=T45*T56*T67;
R47_Temp_1=T47(1:3,1:3);


%为方便求解，将R47_temp分解为不同量用Wij（i=1,2,3;j=1,2,3）表示
W21=R47_Temp(2,1);
W22=R47_Temp(2,2);
W23=R47_Temp(2,3);
W13=R47_Temp(1,3);
W33=R47_Temp(3,3);

th6_In_1=acos(-W23);   %两组解之一
th6_In_2=-th6_In_1 ;  %两组解之二
th7_In_1=atan2(-W22,W21);
th5_In_1=atan2( W33,W13);


2）求解期望逆解
求解思想：在求解完成的条件下的初始逆解后，然后绕sw轴转动角度获取新的关节解算值作为机械臂的期望逆解

% syms phi
phi=1.2;
[u0,usw0] = myfunVector2USkew(xsw0_ExpectVector);   %求取位姿已知下转轴sw的单位反对称矩阵，即 usw0X
As= usw0*R300_Temp;
Bs=-usw0*usw0*R300_Temp;
Cs= (u0*u0.')*R300_Temp;

R30_Expect=As*sin(phi)+Bs*cos(phi)+Cs;   %期望臂角phi下变换矩阵R30
% vpa(R30_Expect,6)
M31=R30_Expect(3,1);
M32=R30_Expect(3,2);
M33=R30_Expect(3,3);
M13=R30_Expect(1,3);
M23=R30_Expect(2,3);

%求解期望臂角下的关节逆解th1 th2 th3 th4
th1_E=atan2(M23,M13);
th2_E1=acos(M33);
th2_E2=-acos(M33);
th3_E=atan2(M32,-M31);
th4_E1=th4_In;
th4_E2=-th4_In;

%求解期望臂角下关节角th5 th6 th7
R47_Expect=(R43.')*(R30_Expect.')*R70_Expect;

th6_E1=acos(-W23);     %两组解之一
th6_E2=-acos(-W23);      %两组解之二
th7_E1=atan2(-W22,W21);
th5_E1=atan2( W33,W13);

disp('输出其中一组解用于验证运动学正解：');
t1=vpa(th1_E,10)
t2=vpa(th2_E1,10)
t3=vpa(th3_E,10)
t4=vpa(th4_E1,10)
t5=vpa(th5_E1,10)
t6=vpa(th6_E1,10)
t7=vpa(th7_E1,10)


t=[t1 t2 t3 t4 t5 t6 t7]';
dht=myfunMDHTable(t,d2,a2,alp2);
t70=myfunTransMatrix(dht);
pt=t70(1:3,4) ;%输出正解的末端位置

Rt=t70(1:3,1:3);Rt=double(Rt);   %转化为双精度double计算
[r1,r2,r3]=dcm2angle(Rt);  %输出正解的欧拉角

disp('输出正解末端位姿[px py pz r p y]:');
[px py pz r p y]



附录：MATLAB函数库
function dh=myfunMDHTable(q,d,a,alpha)
%Method:MDH
%Goal:compute transform matrix
%Author:easyR
%Date:2019/1/25
%main function is assamble the MDH table for computing
%Variable:q
%q：column vector ,7 by 1
%d: column vector ,7 by 1
%a: column vector ,7 by 1
%alpha: column vector ,7 by 1
q_1=q(1);q_2=q(2);q_3=q(3);q_4=q(4);q_5=q(5);q_6=q(6);q_7=q(7);
d_1=d(1);d_2=d(2);d_3=d(3);d_4=d(4);d_5=d(5);d_6=d(6);d_7=d(7);
a_1=a(1);a_2=a(2);a_3=a(3);a_4=a(4);a_5=a(5);a_6=a(6);a_7=a(7);
alpha_1=alpha(1);alpha_2=alpha(2);alpha_3=alpha(3);alpha_4=alpha(4);alpha_5=alpha(5);alpha_6=alpha(6);alpha_7=alpha(7);
dh=[q_1     q_2     q_3     q_4     q_5     q_6  q_7;
    d_1     d_2     d_3     d_4     d_5     d_6  d_7;
    a_1     a_2     a_3     a_4     a_5     a_6  a_7;
    alpha_1 alpha_2 alpha_3 alpha_4 alpha_5 alpha_6 alpha_7
    ]';

end

%%
%——————————————————————————————————————————————————————————————————————————————————————————————————%
function [T]=myfunMatrixTrans(theta,d,a,alpha)
%Method:MDH
%Goal:compute transform matrix
%Author:easyR
%Date:2019/1/25
T=[ 
                   cos(theta),          -sin(theta),          0,           a;
        sin(theta)*cos(alpha),cos(theta)*cos(alpha),-sin(alpha),-sin(alpha)*d;
        sin(theta)*sin(alpha),cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d;
                            0,                    0,          0,            1
       ];
end

%————————————————————————————————————————————————————————————————————————————————————————————————%
%%
function [T,T10,T20,T30,T40,T50,T60,T70]=myfunTransMatrix(dh)
%Method:MDH
%Goal:compute transform matrix
%Author:easyR
%Date:2019/1/25
%dh：MDH Table
%T:Transform Matrix from endpoint to basement
for k=1:7
    for i=1:k
        T(:,:,k)=myfunMatrixTrans( dh(i,1),dh(i,2),dh(i,3),dh(i,4));
    end
end
% disp('display each transform matrix Tn:');
%%
% transform matrix
T10=(T(:,:,1));T21=(T(:,:,2));T32=(T(:,:,3));T43=(T(:,:,4));T54=(T(:,:,5));T65=(T(:,:,6));T76=(T(:,:,7));
T20=T10*T21;T30=T20*T32;T40=T30*T43;T50=T40*T54;T60=T50*T65;T70=T60*T76;
end

%————————————————————————————————————————————————————————————————————————————————————————————————%

% function [PseInvA]=myfunPseudo(A)
% %compute pseudo inverse matrix of A which det(A)=0
% PseInvA = (inv((A'*A)))*A'
% 
% end

%————————————————————————————————————————————————————————————————————————————————————————————————%
%%
% function R=myfunRPY2R(alpha,beta,gama)
% %input:欧拉角RPY
% %ouput:旋转矩阵 3by3
% %Note:注意YPR分别表示ZYX的转角，对应gama beta alpha
% 
% R=[cos(alpha)*cos(beta) cos(alpha)*sin(beta)*sin(gama)-sin(alpha)*cos(gama) cos(alpha)*sin(beta)*cos(gama)+sin(alpha)*sin(gama);...
%     sin(alpha)*cos(beta) sin(alpha)*sin(beta)*sin(gama)+cos(alpha)*cos(gama) sin(alpha)*sin(beta)*cos(gama)-cos(alpha)*sin(gama);...
%     -sin(beta) cos(beta)*sin(gama) cos(beta)*cos(gama)];
% 
% end

%————————————————————————————————————————————————————————————————————————————————————————————————%
%%
% function rpy=myfunR2RPY(r)
% %input:旋转矩阵 3by3
% %ouput:欧拉角RPY
% 
% beta=atan2(-r(3,1),sqrt(r(1,1).^2+r(2,1).^2));
% 
% if -pi/2 <beta && beta<= pi/2
%     alpha=atan2(r(2,1),r(1,1));
%     gama=atan2(r(3,2),r(3,3));
%     
% elseif pi/2 <beta && beta<= 3*pi/2
%     alpha=atan2(-r(2,1),-r(1,1));
%     gama=atan2(-r(3,2),-r(3,3));
% end
% 
% rpy=[gama,beta,alpha];  %XYZ分别对应alpha beta gama角度RPY
% 
% end

%———————————————————————————————————————————————————————————————————————————————————————————————%

function [u,Us]=myfunVector2USkew(v)
%input:输入任意3D向量V,3 by 1
%ouput:输出其单位反对称矩阵 Us

n=sqrt(v(1)^2+v(2)^2+v(3)^2);
u=[v(1)/n;v(2)/n;v(3)/n];
Us=[   0  -u(3)   u(2);
    u(3)     0   -u(1);
   -u(2)   u(1)     0];

end

%————————————————————————————————————————————————————————————————————————————————————————————————%

输入与结果：

结果：