指数积形式的运动学建模

最新推荐文章于 2023-09-09 17:04:28 发布

小小蜗牛，大大梦想

最新推荐文章于 2023-09-09 17:04:28 发布

阅读量442

点赞数 1

文章标签：数学建模

本文链接：https://blog.csdn.net/a1233219/article/details/131206782

版权

在实际应用领域中，机械臂运动学标定研究大部分基于经典 DH 方法，原因是采用经典 DH 法建立的运动学模型含有最少的参数。然而，当相邻关节轴线接近平行或平行时，机械臂会出现奇异性问题。指数积形式的运动学建模是作为机械臂参数标定方法的另一种研究思路基础，且指数积方法优于其他方法。指数积方法就是建立在空间运动的李群、李代数理论的基础上，保证了几何参数和机械臂末端位姿间连续映射特性。

该方法的主要优点有：
1）实现了移动关节和转动关节统一描述，通用性好；
2）当相邻关节轴线接近平行时，由于李群和李代数之间的平滑映射，导致
基于指数积方法的运动学模型相对关节运动是光滑变化的，避免了奇异点的出现；
3）利用李代数 se(3) 的指数映射易于微分求导的性质，物理意义清晰、表达
形式简洁等。

Forward_kinematics_POE.m

function [Tout] = Forward_kinematics_POE(q)
%注：输入六个关节角为实际转动的关节角
%以下程序是根据指数积公式POE建立的正运动学
%CRP的零位关节角为[0;pi/2;0;0;pi/2;0]
%初始位形时，末端位姿M相对于基坐标系
M = [0 0 1 0.462;
     0 1 0 0;  
     -1 0 0 0.755; 
     0 0 0 1];
%相对基坐标系的螺旋轴
w1 = [0;0;1]; %沿关节轴正向的单位向量 相对于基坐标系
w2 = [0;1;0];
w3 = [0;1;0];
w4 = [1;0;0];
w5 = [0;1;0];
w6 = [1;0;0];

q1 = [0; 0; 0]; %qn是关节轴上任一点 坐标值在基坐标系中进行度量
q2 = [0.03; 0; 0.38] ;
q3 = [0.03; 0; 0.755] ;
q4 = [0.375; 0; 0.755] ;
q5 = [0.375; 0; 0.755] ;
q6 = [0.462; 0; 0.755] ;

v1 = -cross(w1,q1); %线速度
v2 = -cross(w2,q2);
v3 = -cross(w3,q3);
v4 = -cross(w4,q4);
v5 = -cross(w5,q5);
v6 = -cross(w6,q6);

R1 = [0 -w1(3) w1(2);w1(3) 0 -w1(1);-w1(2) w1(1) 0]; %角速度的反对称矩阵
R2 = [0 -w2(3) w2(2);w2(3) 0 -w2(1);-w2(2) w2(1) 0];
R3 = [0 -w3(3) w3(2);w3(3) 0 -w3(1);-w3(2) w3(1) 0];
R4 = [0 -w4(3) w4(2);w4(3) 0 -w4(1);-w4(2) w4(1) 0];
R5 = [0 -w5(3) w5(2);w5(3) 0 -w5(1);-w5(2) w5(1) 0];
R6 = [0 -w6(3) w6(2);w6(3) 0 -w6(1);-w6(2) w6(1) 0];

G1 = eye(3)*q(1) + (1-cos(q(1)))*R1 + (q(1)-sin(q(1)))*R1^2;  
G2 = eye(3)*q(2) + (1-cos(q(2)))*R2 + (q(2)-sin(q(2)))*R2^2;
G3 = eye(3)*q(3) + (1-cos(q(3)))*R3 + (q(3)-sin(q(3)))*R3^2;
G4 = eye(3)*q(4) + (1-cos(q(4)))*R4 + (q(4)-sin(q(4)))*R4^2;
G5 = eye(3)*q(5) + (1-cos(q(5)))*R5 + (q(5)-sin(q(5)))*R5^2;
G6 = eye(3)*q(6) + (1-cos(q(6)))*R6 + (q(6)-sin(q(6)))*R6^2;

Rot1 = eye(3) + sin(q(1))*R1 + (1-cos(q(1)))*R1^2;  %刚体转动的矩阵指数 exp([w]theta)
Rot2 = eye(3) + sin(q(2))*R2 + (1-cos(q(2)))*R2^2; 
Rot3 = eye(3) + sin(q(3))*R3 + (1-cos(q(3)))*R3^2;
Rot4 = eye(3) + sin(q(4))*R4 + (1-cos(q(4)))*R4^2; 
Rot5 = eye(3) + sin(q(5))*R5 + (1-cos(q(5)))*R5^2;
Rot6 = eye(3) + sin(q(6))*R6 + (1-cos(q(6)))*R6^2; 

eS1 = [Rot1 G1*v1 ;0 0 0 1];%刚体运动的矩阵指数4*4 exp([s]theta)
eS2 = [Rot2 G2*v2 ;0 0 0 1];
eS3 = [Rot3 G3*v3 ;0 0 0 1];
eS4 = [Rot4 G4*v4 ;0 0 0 1];
eS5 = [Rot5 G5*v5 ;0 0 0 1];
eS6 = [Rot6 G6*v6 ;0 0 0 1];

%末端执行器相对于第六关节法兰盘中心处的变换矩阵
% T67 = [1 0 0 -0.08824;
%        0 1 0 0.01657;
%        0 0 1 -0.42473;
%        0 0 0 1];

Tout = eS1*eS2*eS3*eS4*eS5*eS6*M; %全局POE
% Tout =  eS1*eS2*eS3*eS4*eS5*eS6*T01*T12*T23*T34*T45*T56 ;%*T67; %基于基坐标系的表示  局部POE




end

Inverse_kinematics_POE.m

function[q] = Inverse_kinematics_POE(q0,Td)
%此程序是逆运动学数值求解,采用的方法是基于物体雅可比矩阵的牛顿-拉夫森法
%初始化：已知目标矩阵Td，初始角度估计值q0，设定i=0
%如果初始估计值与真实值没有足够接近，则迭代过程可能不收敛
%解出来的角度与初始角度有关
theta = q0;
for i = 1:1000
   disp(i)
   T_sb = Forward_kinematics_POE(theta);
   T_bd = inv(T_sb)*Td;  %以e为底
   Vb_frame = logm(T_bd); %T_bd的矩阵对数
   v = Vb_frame(1:3,4);
   w_frame = Vb_frame(1:3,1:3);
   %速度旋量Vb 维度6*1 
   Vb = [w_frame(6);w_frame(7);w_frame(2);v];
   xitew = Vb(1:3);
   xitev = Vb(4:6);
   errorw = norm(xitew)
   errorv = norm(xitev)
   %如果满足设定的误差最小值则停止循环
   if errorw < 1e-8 || errorv < 1e-8   %if errorw < 1e-8 && errorv < 1e-8
       q = theta;
       break
   end
   %不满足条件则修改theta的值
   %计算当前角度的物体雅克比矩阵
   J = Jacoby_POE(theta);
   theta = theta + pinv(J)*Vb;
   for j = 1:6
       while theta(j) < -pi
           theta(j) = theta(j) + pi;
       end
       while theta(j) > pi
           theta(j) = theta(j) - pi;
       end
   end
end

end

Jacoby_POE.m

function [J] = Jacoby_POE(q)
%以下程序是物体雅可比矩阵的求解6*6矩阵
%参考现代机器人学5.1节,雅可比矩阵由正向运动学的指数积求得
%先求取空间雅可比矩阵再转换为物体雅可比
zer = zeros(3,3);
%初始位形时，末端位姿M相对于基坐标系
M = [0 0 1 0.462;
     0 1 0 0;  
     -1 0 0 0.755; 
     0 0 0 1];
%相对基坐标系的螺旋轴
w1 = [0;0;1]; %沿关节轴正向的单位向量 相对于基坐标系
w2 = [0;1;0];
w3 = [0;1;0];
w4 = [1;0;0];
w5 = [0;1;0];
w6 = [1;0;0];

q1 = [0; 0; 0]; %qn是关节轴上任一点 坐标值在基坐标系中进行度量
q2 = [0.03; 0; 0.38] ;
q3 = [0.03; 0; 0.755] ;
q4 = [0.375; 0; 0.755] ;
q5 = [0.375; 0; 0.755] ;
q6 = [0.462; 0; 0.755] ;

v1 = -cross(w1,q1); %线速度
v2 = -cross(w2,q2);
v3 = -cross(w3,q3);
v4 = -cross(w4,q4);
v5 = -cross(w5,q5);
v6 = -cross(w6,q6);

R1 = [0 -w1(3) w1(2);w1(3) 0 -w1(1);-w1(2) w1(1) 0]; %转轴的反对称矩阵
R2 = [0 -w2(3) w2(2);w2(3) 0 -w2(1);-w2(2) w2(1) 0];
R3 = [0 -w3(3) w3(2);w3(3) 0 -w3(1);-w3(2) w3(1) 0];
R4 = [0 -w4(3) w4(2);w4(3) 0 -w4(1);-w4(2) w4(1) 0];
R5 = [0 -w5(3) w5(2);w5(3) 0 -w5(1);-w5(2) w5(1) 0];
R6 = [0 -w6(3) w6(2);w6(3) 0 -w6(1);-w6(2) w6(1) 0];

G1 = eye(3)*q(1) + (1-cos(q(1)))*R1 + (q(1)-sin(q(1)))*R1^2;  
G2 = eye(3)*q(2) + (1-cos(q(2)))*R2 + (q(2)-sin(q(2)))*R2^2;
G3 = eye(3)*q(3) + (1-cos(q(3)))*R3 + (q(3)-sin(q(3)))*R3^2;
G4 = eye(3)*q(4) + (1-cos(q(4)))*R4 + (q(4)-sin(q(4)))*R4^2;
G5 = eye(3)*q(5) + (1-cos(q(5)))*R5 + (q(5)-sin(q(5)))*R5^2;
G6 = eye(3)*q(6) + (1-cos(q(6)))*R6 + (q(6)-sin(q(6)))*R6^2;

Rot1 = eye(3) + sin(q(1))*R1 + (1-cos(q(1)))*R1^2;  %刚体转动的矩阵指数
Rot2 = eye(3) + sin(q(2))*R2 + (1-cos(q(2)))*R2^2; 
Rot3 = eye(3) + sin(q(3))*R3 + (1-cos(q(3)))*R3^2;
Rot4 = eye(3) + sin(q(4))*R4 + (1-cos(q(4)))*R4^2; 
Rot5 = eye(3) + sin(q(5))*R5 + (1-cos(q(5)))*R5^2;
Rot6 = eye(3) + sin(q(6))*R6 + (1-cos(q(6)))*R6^2; 

eS1 = [Rot1 G1*v1 ;0 0 0 1];%刚体运动的矩阵指数
eS2 = [Rot2 G2*v2 ;0 0 0 1];
eS3 = [Rot3 G3*v3 ;0 0 0 1];
eS4 = [Rot4 G4*v4 ;0 0 0 1];
eS5 = [Rot5 G5*v5 ;0 0 0 1];
eS6 = [Rot6 G6*v6 ;0 0 0 1];


Tsb = eS1*eS2*eS3*eS4*eS5*eS6*M; %基于基坐标系的表示
%以上是正运动学的指数积公式

%以下是空间雅克比矩阵的求解
e_S1 = eS1;
e_S2 = eS1*eS2;
e_S3 = eS1*eS2*eS3;
e_S4 = eS1*eS2*eS3*eS4;
e_S5 = eS1*eS2*eS3*eS4*eS5;

e_S1_R = e_S1(1:3,1:3);
e_S2_R = e_S2(1:3,1:3);
e_S3_R = e_S3(1:3,1:3);
e_S4_R = e_S4(1:3,1:3);
e_S5_R = e_S5(1:3,1:3);

e_S1_P = e_S1(1:3,4);
e_S2_P = e_S2(1:3,4);
e_S3_P = e_S3(1:3,4);
e_S4_P = e_S4(1:3,4);
e_S5_P = e_S5(1:3,4);

e_S1_P_frame = [0 -e_S1_P(3) e_S1_P(2);e_S1_P(3) 0 -e_S1_P(1);-e_S1_P(2) e_S1_P(1) 0];
e_S2_P_frame = [0 -e_S2_P(3) e_S2_P(2);e_S2_P(3) 0 -e_S2_P(1);-e_S2_P(2) e_S2_P(1) 0];
e_S3_P_frame = [0 -e_S3_P(3) e_S3_P(2);e_S3_P(3) 0 -e_S3_P(1);-e_S3_P(2) e_S3_P(1) 0];
e_S4_P_frame = [0 -e_S4_P(3) e_S4_P(2);e_S4_P(3) 0 -e_S4_P(1);-e_S4_P(2) e_S4_P(1) 0];
e_S5_P_frame = [0 -e_S5_P(3) e_S5_P(2);e_S5_P(3) 0 -e_S5_P(1);-e_S5_P(2) e_S5_P(1) 0];

S1 = [w1;v1];
S2 = [w2;v2];
S3 = [w3;v3];
S4 = [w4;v4];
S5 = [w5;v5];
S6 = [w6;v6];

%伴随矩阵6*6
Ad_es1 = [e_S1_R  zer;e_S1_P_frame*e_S1_R e_S1_R];
Ad_es1es2 = [e_S2_R  zer;e_S2_P_frame*e_S2_R e_S2_R];
Ad_es1es2es3 = [e_S3_R  zer;e_S3_P_frame*e_S3_R e_S3_R];
Ad_es1es2es3es4 = [e_S4_R  zer;e_S4_P_frame*e_S4_R e_S4_R];
Ad_es1es2es3es4es5 = [e_S5_R  zer;e_S5_P_frame*e_S5_R e_S5_R];

Js1 = S1;
Js2 = Ad_es1*S2;
Js3 = Ad_es1es2*S3;
Js4 = Ad_es1es2es3*S4;
Js5 = Ad_es1es2es3es4*S5;
Js6 = Ad_es1es2es3es4es5*S6;

%空间雅克比的求解
Js = [Js1 Js2 Js3 Js4 Js5 Js6] ;%6*6
%物体雅克比的求解
Tbs = inv(Tsb);
R_Tbs = Tbs(1:3,1:3);
P_Tbs = Tbs(1:3,4);
P_Tbs_frame = [0 -P_Tbs(3) P_Tbs(2);P_Tbs(3) 0 -P_Tbs(1);-P_Tbs(2) P_Tbs(1) 0];
Ad_Tbs = [R_Tbs zer;P_Tbs_frame*R_Tbs R_Tbs];
Jb = Ad_Tbs * Js;


J = Jb;

end