多自由度机械臂运动学正-逆解|空间轨迹规划控制|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 l4x = (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_numth_(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<2for i=1:1:p_numTeaching.animate(th_(i,:));%.animate绘制图形drawnow                           %马上作图pause(1)endj=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 JointStatepoint_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,l3th_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*betareturn th_1, th_2, th_3def circle():#global point_num victor_i = np.array([1,0,0])victor_j = np.array([0,1,0])radiu = 3.5n = 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 = 0y_t = 0z_t = 0for 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)

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

MATLAB机器人机械臂运动学正逆解、动力学建模仿真与轨迹规划
MATLAB机器人机械臂运动学正逆解.动力学建模仿真与轨迹规划,雅克比矩阵求解.蒙特卡洛采样画出末端执行器工作空间基于时间最优的改进粒子群优化算法机械臂轨迹规划设计 ID:4610679190520 ...
【机器人学】冗余七自由度机械臂的解析解逆解算法
冗余七自由度机械臂的解析解逆解算法参考论文一论文二参考 -[1] An Analytical Solution for a Redundant Manipulator with Seven D ...
6轴机器人运动学逆解matlab,六轴机器人建模方法、正逆解、轨迹规划实例与Matalb Robotic Toolbox 的实现...
摘要本文主要是给大家一个系统的概念,如何用Matlab实现六轴机器人的建模和实现轨迹规划.以后将会给大家讲解如何手写正逆解以及轨迹插补的程序.程序是基于Matlab2016a,工具箱版本为Robot ...
Matlab机器人工具箱（3-1）：五自由度机械臂（正逆运动学）
01 正运动学:DH表示法 1955年, Denavit和Hartenberg在"ASME Journal of Applied Mechanic"发表了一篇论文,这篇论文介绍了一 ...
和ur的区别_UR机械臂simscape正逆解仿真
最近在看相关的课程,把作业做一下,还是蛮有意思的.(课程代码所以涉及版权问题,经过同意后我上传代码) UR机械臂的物理模型文件是根据SolidWorks插件simscape导出的xml文件,课程直接提 ...
UR机械臂simscape正逆解仿真
最近在看相关的课程,把作业做一下,还是蛮有意思的. UR机械臂的物理模型文件是根据SolidWorks插件simscape导出的xml文件,课程直接提供的,博客不便给出,清楚整个建模原理就行.但个人还 ...
Parallel Platform (Stewart Platform) 类型机械臂的正逆解 01
并联机器人相对于串联机器人有着不同的应用场景.本文主要是基于并联机械臂的基础控制算法做一些阐述.对于机械臂的架构通讯方式(RS232 RS485 Canbus Modbus),结构设计(球关节,uni ...
matlab机械臂运动,四自由度机械臂运动学分析及Matlab仿真.PDF
四自由度机械臂运动学分析及Matlab仿真 2013 年 3 月机械科学与技术 March 2013 32 3 Mechanical Science and Technology for Aeros ...
工业机械人运动学正逆解，简单粗暴！！！！！！
ur机械臂是六自由度机械臂,由D-H参数法确定它的运动学模型,连杆坐标系的建立如上图所示. 转动关节θi是关节变量,连杆偏移di是常数. 关节编号 α(绕x轴) a(沿x轴) θ(绕z轴) d(沿z轴 ...

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

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

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

最新文章

热门文章