基于Robotics Toolbox与Robotics System Toolbox 机器人正逆运动学解算、雅克比及其轨迹规划
主要区别：
robotics toolbox 主要用来建立机器人运动模型，进行空间规划，正逆运动求解的软件。
robotics system toolbox 是关于机器人系统MATLAB/Simulink与ROS的接口，以及常用的机器人算法。
————————————————————
机器人工具箱：
官网链接如下：https://petercorke.com/
————————————————————
机器人DH建模：
%% DH and Inertia Parameters of UR5
alpha = [0,pi/2,0,0,-pi/2,pi/2];
a = [0,0,.425,.39225,0,0];
d = [.08916,0,0,.10915,.09456,.0823];
offset = 0;
m = [3.7,8.393,2.275,1.219,1.219,.1879];
L1 = Link( 'd', d(1), 'a', a(1), 'alpha', alpha(1) ,'standard' );
L2 = Link( 'd', d(2), 'a', a(2), 'alpha', alpha(2) ,'standard' );
L3 = Link( 'd', d(3), 'a', a(3), 'alpha', alpha(3) ,'standard' );
L4 = Link( 'd', d(4), 'a', a(4), 'alpha', alpha(4) ,'standard' );
L5 = Link( 'd', d(5), 'a', a(5), 'alpha', alpha(5) ,'standard');
L6 = Link( 'd', d(6), 'a', a(6), 'alpha', alpha(6) ,'standard');
%%改进D-H模型
% 关节角 连杆偏距 连杆长度 连杆转角
% theta(i) d(i) a(i-1) alpha(i-1) offset
ML1=Link([0 d(1) a(1) alpha(1) 0 ],'modified');
ML2=Link([0 d(2) a(2) alpha(2) 0 ],'modified');
ML3=Link([0 d(3) a(3) alpha(3) 0 ],'modified');
ML4=Link([0 d(4) a(4) alpha(4) 0 ],'modified');
ML5=Link([0 d(5) a(5) alpha(5) 0 ],'modified');
ML6=Link([0 d(6) a(6) alpha(6) 0 ],'modified');
% robot_UR5 =SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','UR5');%SerialLink 类函数
robot_UR5=SerialLink([L1,L2,L3,L4,L5,L6],'name','UR5'); %SerialLink 类函数
robot_UR5.display(); %Link 类函数
robot_UR5.teach(); %可以自由拖动的关节角度
qz = [0 0 0 0 0 0] %零角度
qr = [0 1.57079632679490 -1.57079632679490 0 0 0] % 就绪状态，机器人伸直且垂直
qs = [ 0 0 -1.57079632679490 0 0 0] % 伸展状态，机器人伸直且水平
qn = [ 0 0.785398163397448 3.14159265358979 0 0.785398163397448 0] % 标准状态，机器人灵巧工作状态
不同位姿可视化：
qn = [ -pi/2 0 0 0 0 0];
qn = [ -pi/2 pi/3 0 0 0 0]
qn = [ -pi/2 pi/3 pi/3 0 0 0];
qn = [ -pi/2 pi/3 pi/3 -pi/3 0 0];
qn = [ -pi/2 pi/3 pi/3 -pi/3 pi/2 0];
qn = [ -pi/2 pi/3 pi/3 -pi/3 pi/2 pi/2];
正运动学：给定机器人关节变量的取值来确定末端执行器的位置和姿态。
robot_UR5.fkine(qn)
输入关节空间参数，输出齐次变换矩阵，即将此时此刻所处的关节参数转换为末端姿态和位姿参数。
ans =
0.5000 -0.8660 0 0.3506
0.8660 0.5000 0 -0.4376
0 0 1 0.4572
0 0 0 1
根据关节坐标θ计算其末端执行器坐标系的位置和姿态。
T06 = T01*T12*T23*T34*T45*T56;
qn = [ 0 0 0 0 pi/3 0];
T=robot_UR5.fkine(qn);
T =
0.5000 -0.8660 0 0.746
0 0 -1 -0.2037
0.8660 0.5000 0 0.1303
0 0 0 1
robot_UR5.ikine(T)
ans =
0.0000 0.0000 -0.0000 0.0000 1.0472 -0.0000
雅可比矩阵：
雅克比矩阵可以联系机器人末端操作空间速度和关节空间速度，已知关节空间速度，利用雅克比矩阵，可以得到操作空间的速度，再结合逆过程，就可以实现速度模式控制机器人。
%jacob0()求解的是将关节速度映射到世界坐标系中的末端执行器空间速度
jacob_o = robot_UR5.jacob0(qn)
jacob_o =
0.2037 0.2037 -0.0412 -0.0411 -0.0412 0
0.7460 0.7460 0.0000 0.0000 -0.0000 0
0.0000 0.0000 0.7460 0.3210 -0.0713 0
0 0 0 0 0 -0.8660
0 0 -1.0000 -1.0000 -1.0000 -0.0000
1.0000 1.0000 0.0000 0.0000 0.0000 0.5000
%jaconb()求解的是将关节速度映射到工具坐标系中的末端执行器空间速度
jacob_e = robot_UR5.jacobe(qn)
jacob_e =
0.1019 0.1019 0.6255 0.2574 -0.0823 0
-0.1764 -0.1764 0.4086 0.1961 0 0
-0.7460 -0.7460 0 0 0 0
0.8660 0.8660 0 0 0 0
0.5000 0.5000 0 0 0 1.0000
0.0000 0.0000 1.0000 1.0000 1.0000 0.0000
figure;plot_ellipse( jacob_o(1:3,:)*jacob_o(1:3,:)' ) % 线速度的可操作性椭球
figure;plot_ellipse( jacob_o(4:6,:)*jacob_o(4:6,:)' ) ;% 角速度的可操作性椭球
p560.maniplty(qn) ;% 可操作性度量
结果：Manipulability: translation 0.111181, rotation 2.44949
——————————————————
%% 轨迹规划
T0=robot_UR5.fkine(qz);%根据给定起始点，得到起始点位姿 ，T0 = transl(0.3,0,0.3)*rpy2tr(45,10,20);
T0 =
1 0 0 0.8173
0 0 -1 -0.2037
0 1 0 0.1715
0 0 0 1
T1=robot_UR5.fkine(qn);%根据给定终止点，得到终止点位姿 ，T1 = transl(0.6,0,0.6)*rpy2tr(45,20,25);
T1 =
0.9374 -0.2500 0.2424 0.3803
0.2665 0.0670 -0.9615 -0.3128
0.2241 0.9659 0.1294 0.76
0 0 0 1
q0=robot_UR5.ikine(T0);%根据起始点位姿，得到起始点关节角
q0 =
0 0 0 0 0 0
qf=robot_UR5.ikine(T1);%根据终止点位姿，得到终止点关节角
qf =
-0.1309 -0.1309 0.5236 0.7854 -1.0472 0.5236
m = 50 ;% 迭代次数
t = 0:0.05:60;
% 关节空间轨迹规划
% 已知初始和终止的关节角度，利用五次多项式来规划轨迹
[q ,qd, qdd]=jtraj(q0,qf,m); %五次多项式轨迹，得到关节角度，角速度，角加速度，50为采样点个数
T=robot_UR5.fkine(q);%根据插值，得到末端执行器位姿
T =
1 0 0 0.8173
0 0 -1 -0.2037
0 1 0 0.1715
0 0 0 1
i=1:6;
% plot(q(:,i)); title('运动轨迹');legend()
robot_UR5.plot(q,'trail','b-','movie','jtraj.gif')
% 笛卡尔空间轨迹规划
% 已知初始和终止的末端关节位姿，利用匀加速、匀减速运动来规划轨迹。
Tc = ctraj(T0, T1, length(t));
Tjtraj=transl(Tc); plot2(Tjtraj,'r');
figure;plot(t',transl(Tc));
figure;plot(t',tr2rpy(Tc));
% figure;qc = robot_UR5.ikine(Tc); robot_UR5.plot(qc,'trail','b-','movie','ctraj.gif')
————————————————————————
% 末端画圆
%确定画图平面
p0=[0,0,0]; p1=[0.3,0.3,0.3];p2=[-0.4,0.4,0.4];
X=p1-p0;B=p2-p0; Z=cross(X,B);Y=cross(Z,X);
x1=X/norm(X);y1=Y/norm(Y);z1=Z/norm(Z);
R=[x1.',y1.',z1.'];
Js=[R ,p0.'];Js=[Js;0 0 0 1];
r=norm(p1-p0);
%圆的轨迹
t=0:0.05:20;
[s,sd,~] = tpoly(0,2*pi,t);
q1=[-12.7,98.9,-107,158,-114,11];
k = q1;
vs =[]; % qs 关节速度
pd=[];%ps 笛卡尔坐标系下的坐标位置
for i = 1:length(s)
T=[1 0 0 r*cos(s(i));0 1 0 r*sin(s(i));0 0 1 0;0 0 0 1];
T1=Js*T;
q=robot_UR5.ikunc(T1,k); %求解逆运动
k=q;
pd=[pd;T1(1:3,4).'];
vs=[vs;q];
end
close all;
figure('NumberTitle', 'off', 'Name', '运动图像');
plot2(pd,'r','LineWidth',2);%圆的线宽
hold on;
robot_UR5.plot(vs,'fps',20);%输出运动图像
%% 蒙特卡洛方法工作空间分析
%% 为每一个关节设置旋转角度限制
q1max = 180.0;
q1min = -180.0;
q2max = -90.0;
q2min = 90.0;
q3max = -90.0;
q3min = 90.0;
q4max = -180.0;
q4min = 180.0;
q5max = -90.0;
q5min = 90.0;
q6max = -180.0;
q6min = 180.0;
N=2000; %随机取得的工作点个数
theta1=(q1min+(q1max-q1min)*rand(N,1))*pi/180; %随机取得关节1角度
theta2=(q2min+(q2max-q2min)*rand(N,1))*pi/180; %随机取得关节2角度
theta3=(q3min+(q3max-q3min)*rand(N,1))*pi/180; %随机取得关节3角度
theta4=(q4min+(q4max-q4min)*rand(N,1))*pi/180; %随机取得关节4角度
theta5=(q5min+(q5max-q5min)*rand(N,1))*pi/180; %随机取得关节5角度
theta6=(q6min+(q6max-q6min)*rand(N,1))*pi/180; %随机取得关节6角度
for n=1:1:N
q=[theta1(n),theta2(n),theta3(n),theta4(n),theta5(n),theta6(n)];
robot_UR5.plot(q);%动画显示
p=robot_UR5.fkine(q);
a=zeros(3,1);
a(:,1)=p(1,1,1).t;
plot3(a(1,1),a(2,1),a(3,1),'b.','MarkerSize',0.5);%画出落点
hold on;
end
把机器人工具箱的功能块添加到simulink的library browser里，首先在MATLAB\R2018b\toolbox\rvctools\simulink下建立一个脚本文件slblocks.m
function blkStruct = slblocks
%SLBLOCKS Defines a block library.
% Library's name. The name appears in the Library Browser's
% contents pane.
blkStruct.Name = ['Robot' sprintf('\n') 'ToolBox'];
% The function that will be called when the user double-clicks on
% the library's name. ;
blkStruct.OpenFcn = 'roblocks';
% The argument to be set as the Mask Display for the subsystem. You
% may comment this line out if no specific mask is desired.
% Example: blkStruct.MaskDisplay = 'plot([0:2*pi],sin([0:2*pi]));';
% No display for now.
% blkStruct.MaskDisplay = '';
% End of blocks
重要的指令
% 机器人工具箱 roblocks rtbdemo % sl_ctorque; % 加载计算力矩控制器
一些相关名词：
- SE(3):特殊欧式群
- se(3):特殊欧式群的李代数
- SO(3): 三维特殊正交群
- so(3): 三维特殊正交群的李代数
- T(3):三维移动群
- R: 旋转矩阵
————————————————————————————————
Robotics System Toolbox 机器人正逆运动学解算、雅克比及其轨迹规划
%% Set Up Robot Model
%调用importrobot从存储在统一机器人描述格式(URDF)文件中的描述生成一个rigidBodyTree模型。
%将DataFormat和Gravity属性设置为与Simscape一致。Simulink模型从仿真的工作空间访问该机器人模型。
addpath([pwd filesep 'Geometry']);UR5 = importrobot('sm_universalUR5.urdf');
UR5.DataFormat = 'column'; UR5.Gravity = [0, 0, -9.80665];
configuration = randomConfiguration(UR5);q = homeConfiguration(UR5);
showdetails(UR5)
% show(UR5,[0;-pi/6;-pi/2;0;0;0]);show(UR5,configuration);show(UR5,q);
% [UR5,metadata] = importrobot("ur5_robot",'DataFormat','column');
initialConfig=[0 ; 0 ; 0; 180; -90; 0]; initialConfig = pi/180*(initialConfig);
% initialConfig = [1.57 ; 0 ; 0; -1.57; 1.57; 0];% 圆轨迹时，ur5机器人的初始姿态
figure;show(UR5,initialConfig)
targetPosition = trvec2tform([(0.78) 0 0.3]);
正运动学：
getTransform |
Get transform between body frames |
randomConfiguration |
Generate random configuration of robot |
homeConfiguration |
Get home configuration of robot |
show |
Show robot model in figure |
transform = getTransform(UR5,q,'base_link','tool0')
齐次变换
transform =
-1.0000 0 -0.0000 0.8173
-0.0000 0.0000 1.0000 0.0055
0.0000 1.0000 -0.0000 -0.1915
0 0 0 1.0000
雅克比模块：
Matlab2022b 最新版退出机器人逆运动学设计工具箱：
MATLAB command prompt: Enter inverseKinematicsDesigner.
uniUR5e = loadrobot("universalUR5e");
robot=UR5;
% MATLAB命令提示符:输入inverseKinematicsDesigner
currentRobotJConfig = homeConfiguration(robot);
numJoints = numel(currentRobotJConfig);
endEffector = "tool0";
timeStep = 0.1; % seconds
toolSpeed = 0.1; % m/s
jointInit = currentRobotJConfig;
taskInit = getTransform(robot,jointInit,endEffector);
taskFinal = trvec2tform([0.4,0,0.6])*axang2tform([0 1 0 pi]);
%% 工作空间
distance = norm(tform2trvec(taskInit)-tform2trvec(taskFinal));
initTime = 0;
finalTime = (distance/toolSpeed) - initTime;
trajTimes = initTime:timeStep:finalTime;
timeInterval = [trajTimes(1); trajTimes(end)];
[taskWaypoints,taskVelocities] = transformtraj(taskInit,taskFinal,timeInterval,trajTimes);
tsMotionModel = taskSpaceMotionModel('RigidBodyTree',robot,'EndEffectorName','tool0');
tsMotionModel.Kp(1:3,1:3) = 0;
tsMotionModel.Kd(1:3,1:3) = 0;
q0 = currentRobotJConfig;
qd0 = zeros(size(q0));
% [tTask,stateTask] = ode15s(@(t,state) exampleHelperTimeBasedTaskInputs(tsMotionModel,timeInterval,taskInit,taskFinal,t,state),timeInterval,[q0; qd0]);
%% exampleHelperTimeBasedTaskInputs函数如下：
function stateDot = exampleHelperTimeBasedTaskInputs(motionModel, timeInterval, initialTform, finalTform, t, state)
[refPose, refVel] = transformtraj(initialTform, finalTform, timeInterval, t);
stateDot = derivative(motionModel, state, refPose, refVel);
end
%% 关节空间逆运动学
ik = inverseKinematics('RigidBodyTree',robot);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1 1 1 1 1 1];
initialGuess = jointInit;
jointFinal = ik(endEffector,taskFinal,weights,initialGuess);
wrappedJointFinal = wrapToPi(jointFinal);
ctrlpoints = [jointInit',wrappedJointFinal'];
jointConfigArray = cubicpolytraj(ctrlpoints,timeInterval,trajTimes);
jointWaypoints = bsplinepolytraj(jointConfigArray,timeInterval,1);
% 关节空间轨迹控制
jsMotionModel = jointSpaceMotionModel('RigidBodyTree',robot,'MotionType','PDControl');
q0 = currentRobotJConfig;
qd0 = zeros(size(q0));
% [tJoint,stateJoint] = ode15s(@(t,state) exampleHelperTimeBasedJointInputs(jsMotionModel,timeInterval,jointConfigArray,t,state),timeInterval,[q0; qd0]);
function stateDot = exampleHelperTimeBasedJointInputs(motionModel, timeInterval, configWaypoints, t, state)
% Use a B-spline curve to ensure the trajectory is smooth and moves
% through the waypoints with non-zero velocity
[qd, qdDot] = bsplinepolytraj(configWaypoints, timeInterval , t);
% Compute state derivative
stateDot = derivative(motionModel, state, [qd; qdDot]);
end
参考文献：
【1】https://blog.csdn.net/ooorczgc/article/details/125110656?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522167108649616800184174108%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=167108649616800184174108&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~hot_rank-3-125110656-null-null.142^v68^pc_new_rank,201^v4^add_ask,213^v2^t3_control1&utm_term=Matlab%E6%9C%BA%E5%99%A8%E4%BA%BA&spm=1018.2226.3001.4187
【2】 运动学 https://mp.weixin.qq.com/s/BOumG8LP8t89AMNSyUSQpA
【3】 动力学 https://mp.weixin.qq.com/s/opQcpv02sysIcz8seg3lkQ
【4】 路径规划 https://mp.weixin.qq.com/s/uNW7Kbsr1rxFT6jqI-rCiA
【6】https://blog.csdn.net/huangjunsheng123/article/details/110630665?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522167108972216800184170144%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=167108972216800184170144&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_ecpm_v1~hot_rank-6-110630665-null-null.142^v68^pc_new_rank,201^v4^add_ask,213^v2^t3_control1&utm_term=Matlab%E6%9C%BA%E5%99%A8%E4%BA%BA%E8%BF%90%E5%8A%A8%E5%AD%A6&spm=1018.2226.3001.4187
【7】https://zhuanlan.zhihu.com/p/545448973
【8】机器人逆运动学工具箱：https://ww2.mathworks.cn/help/robotics/ref/inversekinematicsdesigner-app.html
【9】 https://ww2.mathworks.cn/help/robotics/ug/plan-and-execute-trajectory-kinova-gen3.html
评论（0）
您还未登录，请登录后发表或查看评论