多自由度机械臂运动学正-逆解|空间轨迹规划控制|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)
在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)
