MATLAB Code For Inverse and Forward Kinematics (Newton-Raphson Method)

使用牛顿迭代法解决机器人逆运动学问题

• 问题阐述
  -四轴机器人的正向运动学
  -六轴机器人的逆向运动学
• MATLAB 代码

---

---

---

---

1. 四轴机器人的正向运动学

取θ1，θ2，θ3，θ4分别为pi*2/9，pi/6，pi*2/3，0；

通过正向运动学得出

2.六轴机器人的逆向运动学

1. 代码详情
   Exp_rot.m

function R= exp_rot(theta,what)
R=eye(3)+what*sin(theta)+what*what*(1-cos(theta));
end

w_hat.m
function R= w_hat(w)
R(1,2) = -w(3);
R(1,3) = w(2);
R(2,3) = -w(1);
R(2,1) = w(3);
R(3,1) = -w(2);
R(3,2) = w(1);
End

Exp_twist.m
function R =Exp_twist(T,theta)
v=T(1:3,1);
w=T(4:6,1);
what=w_hat(w);
if sum(w == [0;0;0])~= 3
R(1:3,4)= ((eye(3)-exp_rot(theta,what))*(cross(w,v))+(w*w’*v*theta));
else
R(1:3,4) = theta* v;
end
R(1:3,1:3)=exp_rot(theta,what);
R(4,1:3)=0;
R(4,4)=1;
end

adj.m
function out=adj(X)%%X为输入的SE（3）
p=X(1:3,4);
R=X(1:3,1:3);
out(1:3,1:3) = R;
out(1:3,4:6) = w_hat(p)*R;
out(4:6,1:3) = 0;
out(4:6,4:6) = R;
end

Jac.m
function R=Jac(T1,T2,T3,T4,T5,T6,theta)
%%c1(第一列)
R(1:6,1)=T1;
%%c2
theta1=theta(1,1);
R(1:6,2)=adj(Exp_twist(T1,theta1))*T2;
%%c3
theta2=theta(2,1);
R(1:6,3)=adj(Exp_twist(T1,theta1))*adj(Exp_twist(T2,theta2))*T3；
%%c4
theta3=theta(3,1);
R(1:6,4)=adj(Exp_twist(T1,theta1))..*adj(Exp_twist(T3,theta3))*T4;
%%c5
theta4=theta(4,1);
R(1:6,5)=adj(Exp_twist(T1,theta1))*….*adj(Exp_twist(T4,theta4))*T5;
%%c6
theta5=theta(5,1);
R(1:6,6)=adj(Exp_twist(T1,theta1))*……adj(Exp_twist(T5,theta5))*T6;
end

Vee.m
function out=Vee(R)
out(1:3,1)=R(1:3,4);
out(4,1)=R(3,2);
out(5,1)=R(1,3);
out(6,1)=R(2,1);
end
Psi.m
function R=Psi(T1,T2,T3,T4,T5,T6,theta,gz,gd)
R=Vee(logm(Exp_twist(T1,theta(1,1))..*Exp_twist(T6,theta(6,1))*gz(pinv(gd))));
end

New_Rap.m
function th=New_Rap(T1,T2,T3,T4,T5,T6,theta,gz,gd)
i=0;
error=10;
while error >0.01
J=Jac(T1,T2,T3,T4,T5,T6,theta);
J35=pinv(J);%%J35=J#
theta_new=theta-J35*Psi(T1,T2,T3,T4,T5,T6,theta,gz,gd);
error = abs(norm(Psi(T1,T2,T3,T4,T5,T6,theta,gz,gd)));
theta=theta_new;
i=i+1;
plot(i,error,’r-o’);
hold on;
end
th=theta;
display(th);
end
3. 总体运行
clear all
%%四轴twist
t1= [0;-200;0;0;0;1]
t2=[200;-200;0;0;0;1]
t3=[450;-200;0;0;0;1]
t4=[0;0;1;0;0;0]
%%四轴初始状态
gzero(1:3,1:3)=eye(3);
gzero(1:3,4)=[100;450;525];
gzero(4,1:3)=0;
gzero(4,4)=1;
%%求得逆向运动学所需gd
gd=Exp_twist(t1,pi/6)*Exp_twist(t2,-pi/20)*Exp_twist(t3,-pi/10)*Exp_twist(t4,pi/10)*gzero;
%%六轴twist
T1=[0;0;0;0;0;-1];
T2=[0;-491;0;-1;0;0];
T3=[0;-841;0;-1;0;0];
T4=[-841;0;0;0;1;0];
T5=[0;-841;350;-1;0;0];
T6=[-350;0;0;0;0;1];
%%六轴初始状态
gz(1:3,1:3)=eye(3);
gz(1:3,4)=[0;350;757];
gz(4,1:3)=0;
gz(4,4)=1;
%%theta角度初值
theta=[0;0;0;0;0;0];
%%获得迭代后角度
th=New_Rap(T1,T2,T3,T4,T5,T6,theta,gz,gd);
%%代入迭代后角度
final=Exp_twist(T1,th(1,1))*Exp_twist(T2,th(2,1))*Exp_twist(T3,th(3,1))*Exp_twist(T4,th(4,1))*Exp_twist(T5,th(5,1))*Exp_twist(T6,th(6,1))*gz;
%%显示结果
display(final);
display(gd);

4.迭代效果