多自由度机械臂运动学正-逆解|空间轨迹规划控制|MATLAB仿真+实际机器调试_多自由度的实时规划算法

多自由度机械臂运动学正-逆解|空间轨迹规划控制|MATLAB仿真+实际机器调试

)

DH建模法可以参考这个博客：
还有《机器人》这本书，一定要理论实践相结合，理解后可以用几何法建模也可以用DH方法，几何法与解析法两者在运动学逆解中常用到。

%% 2020/9/25
clear;
clc;        %清屏
clear L     %清变量
Len_tool=0; %虚拟关节长度
% 度弧转换+弧度转换
Du=180/pi;
%% define the length of the Links 
d0_=55;
d1_=115;
d2_=90;
d3_3=100;
%% DH矩阵，DH建模法建立六轴机器人模型
%%          th    d   a      alpha
L(1) = Link([ 0,     5.5,  0,        pi/2   ], 'qlim','[-pi/4 pi/4]');    %定义连杆1，限制关节活动角度
L(2) = Link([ 0,     0,  11.5,        0      ], 'qlim','[0 pi]');         %定义连杆2，限制关节活动角度
L(3) = Link([ 0,     0,  9,        0      ], 'qlim','[0 pi]');            %虚拟关节
Teaching= SerialLink(L,'name','Robot');%连接连杆
%% 定义抓取终点（可运动学反解出二连杆相应位姿角度）
%q0 =[pi/6 pi/4 pi/3]
 q0=[0,0,0]
Teaching.plot(q0)  %画图,机器人处于初始位置
Teaching.fkine(q0)  %画图,机器人处于初始位置
hold on
% 旋转矩阵
th = [pi/6,pi/4,pi/3]
l1 = 5.5;
l2 = 11.5;
l3 = 9.0;
%  syms th1 th2 th3 th4 th5 l1 l2 l3 l4
 x = (l2*cos(th(2)) + l3*cos(th(2)+th(3)))*cos(th(1));
 y = (l2*cos(th(2)) + l3*cos(th(2)+th(3)))*sin(th(1));
 z = l1 + l2*sin(th(2)) + l3*sin(th(2)+th(3));
 T1 = [rotz(th(1)),zeros(3,1);
        zeros(1,3),1         ];
 T2 = transl(0,0,l1);
 T3 = [rotx(pi/2),zeros(3,1);
        zeros(1,3),1         ];
 T4 = [rotz(th(2)),zeros(3,1);
        zeros(1,3),1         ];
 T5 = transl(l2,0,0);
 T6 = [rotz(th(3)),zeros(3,1);
        zeros(1,3),1         ];
 T7 = transl(l3,0,0);
 T  = T1*T2*T3*T4*T5*T6*T7
 % 直接规划函数

%% 空间圆描述
n = [1 0 0]; %法向量n
radiu = 3.5; %圆的半径为1
c = [16 0 10.5]; %圆心的坐标
fai = (0*pi:1*pi/19:2*pi)'; %theta角从0到2*pi
a = cross(n,[1 0 0]); %n与i叉乘，求取a向量
if ~any(a) %如果a为零向量，将n与j叉乘
    a=cross(n,[0 1 0]);
end
b=cross(n,a); %求取b向量
a=a/norm(a); %单位化a向量
b=b/norm(b); %单位化b向量
%% 处理手法值得学习
c1=c(1)*ones(size(fai,1),1);
c2=c(2)*ones(size(fai,1),1);
c3=c(3)*ones(size(fai,1),1);
x=c1 + radiu*a(1)*cos(fai)+radiu*b(1)*sin(fai);%圆上各点的x坐标
y=c2 + radiu*a(2)*cos(fai)+radiu*b(2)*sin(fai);%圆上各点的y坐标
z=c3 + radiu*a(3)*cos(fai)+radiu*b(3)*sin(fai);%圆上各点的z坐标
plot3(x,y,z)
xlabel('x轴')
ylabel('y轴')
zlabel('z轴')
grid on 
 %% 运动学逆解
 % syms x y z 
%  x = T(1,4)
%  y = T(2,4)
%  z = T(3,4)
p_num = size(x,1);
th_ =zeros(p_num,3);
for i = 1:1:p_num
 th_(i,1) = atan2(y(i),x(i));
 R = sqrt(x(i)^2 + y(i)^2 +(z(i) - l1)^2 );
 gama = acos((l2^2 + l3^2 -R^2)/(2*l2*l3));
 Psa = -1;%位型判断，根据位姿判断
 th_(i,3) = Psa*(pi - gama);
 alpha = asin((z(i)-l1)/R);
 alpha2 = atan2(z(i) - l1,x(i)^2 + y(i)^2);
 beta = asin((l3*sin(gama))/R);
 th_(i,2) = alpha - Psa*beta;
end
q1 = [ th_(1,1), th_(1,2), th_(1,3)]
Teaching.plot(q1)  %画图,机器人处于初始位置
j=0;
  while j<2
   for i=1:1:p_num
        Teaching.animate(th_(i,:));%.animate绘制图形
        drawnow                           %马上作图
        pause(1)
   end
  j=j+1;
  end
 %% 逆解结束
 %姿态可用矩阵求得
% qth = [th1 th2 0 th3 th4]
% Teaching.fkine(qth)
% %% 正解
% T6 = rotz(th3)
% T7 = transl(d3,0,0)
% T8 = roty(-pi/2)
% T9 = transl(0,0,d4)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110

在Ros平台中实现对三连杆机械臂规划动作的实现：

#!/usr/bin/env python
from time import sleep, time
from math import pi
import threading
import math
from matplotlib import pyplot as plt
import numpy as np
import rospy
from std_msgs.msg import String
from opmlib.robot import Robot
from sensor_msgs.msg import JointState

point_num = 0
th = [pi/6,pi/4,pi/3]
l1 = 5.5
l2 = 11.5
l3 = 9.0
x = []
y = []
z = []
Ang1 = []
Ang2 = []
Ang3 = []
motion = [[0,0,0,0,0.5],
         [0,0,0.5*pi,0.5*pi,0.5],
	 [0,0,0.5*pi,0.5*pi,-0.1]]#forward
print("Ezekiel is ok")  # 
#rospy.init_node('talker', anonymous=True)
rospy.init_node('talker', anonymous=True)
pub1 = rospy.Publisher('/move_group/fake_controller_joint_states', JointState, queue_size=10)
rate = rospy.Rate(60) # 10hz
sleep(1.0)

def forward(th):
    global l1,l2,l3 
    x_ = (l2*math.cos(th[1]) + l3*math.cos(th[1]+th[2]))*math.cos(th[0])
    y_ = (l2*math.cos(th[1]) + l3*math.cos(th[1]+th[2]))*math.sin(th[0])
    z_ = l1 + l2*math.sin(th[1]) + l3*math.sin(th[1]+th[2])
    return x_,y_,z_

def fkine(Pos, Psa):
    global l1,l2,l3
    th_1 = math.atan2(Pos[1], Pos[0])
    R = math.sqrt(math.pow(Pos[0], 2)+ math.pow(Pos[1], 2) +math.pow(Pos[2] - l1, 2) )
    gama = math.acos((math.pow(l2, 2) + math.pow(l3, 2) -math.pow(R, 2))/(2*l2*l3))
    th_3 = Psa*(pi - gama)
    alpha = math.asin((Pos[2]-l1)/R)
    alpha2 = math.atan2(Pos[2] - l1, math.pow(Pos[0], 2)+ math.pow(Pos[1], 2))
    beta = math.asin((l3*math.sin(gama))/R)
    th_2 = alpha - Psa*beta
    return th_1, th_2, th_3

def circle():#
    global point_num 
    victor_i = np.array([1,0,0])
    victor_j = np.array([0,1,0])
    radiu = 3.5
    n = np.array([1,0,0]) #
    c = np.array([15, 0,11.5])
    fai = np.arange(0*pi,2*pi + 1*pi/100, 1*pi/100)
    a = np.cross(n,victor_i)
    if not np.any(a):
        a = np.cross(n,victor_j)
    b =  np.cross(n,a)
    a =  a/(np.linalg.norm(a))
    b =  b/(np.linalg.norm(b))
    point_num = np.size(fai)
    x_t = 0
    y_t = 0
    z_t = 0
    for i in range(0,point_num,1):
        x_t= c[0] + radiu*a[0]*math.cos(fai[i])+radiu*b[0]*math.sin(fai[i])
        y_t= c[1] + radiu*a[1]*math.cos(fai[i])+radiu*b[1]*math.sin(fai[i]) 
        z_t= c[2] + radiu*a[2]*math.cos(fai[i])+radiu*b[2]*math.sin(fai[i]) 
        x.append(x_t)
        y.append(y_t)
        z.append(z_t)

def arm_op(Apos):
  	jointState_msg = JointState()
   	jointState_msg.name = ['joint1','joint2','joint3','joint4','joint_gripper']
	jointState_msg.position = [Apos[0], 0.5*pi-Apos[1], -Apos[2], 0.5*pi - Apos[3], Apos[4]]	
	rospy.loginfo(jointState_msg)
        pub1.publish(jointState_msg)
circle()#calculaion of traj	
for j in range(0,point_num,1):
    position = np.array([x[j],y[j],z[j]])
    th1, th2, th3 = fkine(position, -1)
    Ang1.append(th1)
    Ang2.append(th2)
    Ang3.append(th3)
t = range(0,point_num,1)
plt.figure(12)
plt.plot(t,Ang1,'bo',t, Ang2,'r--',t, Ang3,'b',label='parametric curve')
plt.show()
# manipulator init
OP_ = np.array([0, 0 ,0 ,0.5*pi ,0.3 ])
arm_op(OP_)
sleep(2.0)
OP_ = np.array([0, 0.25*pi, 0, 0, 0.3])
arm_op(OP_)
sleep(2.0)

OP_ = np.array([Ang1[0], Ang2[0], Ang3[0], 0, 0.3])
arm_op(OP_)
sleep(6.0)

for k in range(0,point_num,1):
     Ang_ctrl = [Ang1[k], Ang2[k], Ang3[k], 0, 0.3]
     arm_op(Ang_ctrl)
     sleep(0.01)
while not rospy.is_shutdown():
	print("Ezekiel is ok") 
	for k in range(0,point_num,1):
		     Ang_ctrl = [Ang1[k], Ang2[k], Ang3[k], 0, 0.3]
		     arm_op(Ang_ctrl)
		     sleep(0.005)

       

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120