六轴六自由度机械臂轨迹跟踪的matlab实现(基于速度雅各比矩阵方法)

  • 六轴六自由度机械臂轨迹跟踪的matlab实现(基于速度雅各比矩阵方法)
    • 1.轨迹跟踪的控制结构图设计
    • 2.系统的输入:轨迹规划
    • 3.被控对象:速度雅各比矩阵
    • 4.控制器:等速率趋近的滑模控制器
    • 5.结果展示

六轴六自由度机械臂轨迹跟踪的matlab实现(基于速度雅各比矩阵方法)

对于六轴六自由度机械臂进行轨迹规划,并针对其设计滑模控制器,实现机械臂的末端轨迹跟踪。(完整代码链接见文章末尾)
本文所用机械臂为innfos-Gluon-6L3,通过standard DH方法建模得到参数如下:

本文利用速度雅各比矩阵的方法来实现轨迹跟踪。这一方法的优点在于可以完全避免逆运动学求解,更加节省时间。

1.轨迹跟踪的控制结构图设计


控制系统的输入,同样也是系统的期望输出,是机械臂的目标位姿xd=(x,y,z,α,β,γ)x_d=(x,y,z,\alpha,\beta,\gamma)xd​=(x,y,z,α,β,γ)。其刚好对应机械臂的六个自由度。前三者是机械臂末端坐标系相对于世界坐标系的位置,后三者是机械臂末端坐标系相对于世界坐标系的旋转角度。每一时刻的机械臂期望位姿都是已知的,它是通过轨迹规划得到的,这一点将在后文详细讲解。
系统的控制器采用滑模控制器,其输入为位姿误差e=xd−xe=x_d-xe=xd​−x,输出为角度增量q˙\dot qq˙​。
系统的被控对象是利用速度雅各比矩阵来建模的,其关系为v=J(q)q˙v=J(q)\dot qv=J(q)q˙​。其中J(q)J(q)J(q)为速度雅各比矩阵。
系统的输出为机械臂的实际末端位置x=∫vdtx=\int vdtx=∫vdt。

2.系统的输入:轨迹规划

为了使得机械臂的末端运动平滑,常对机械臂进行规划,对末端速度、加速度等进行一定约束。
工业上常使用七段式S型曲线来进行轨迹规划,其示意图图如下:

其有七个等间距时间段,分别为加加速段、加速度恒定段、加减速段、匀速段、减减速段、加速度恒定段、减加速段。
各段速度的计算表达式如下,其中JJJ为加加速度。

式中的vmaxv_{max}vmax​等参数的计算推导如下:
△T=ti+1−ti=tf−t07L=4vmax△Tv2=v1+amax△Tamax=2v1△Tvmax=v1+v2=2v1+amax△T=4v1L=16v1△T→v1=L16△T\bigtriangleup T=t_{i+1}-t_{i}=\frac{t_f-t_0}{7} \\ L=4v_{max} \bigtriangleup T \\ v_2=v_1+a_{max}\bigtriangleup T\\ a_{max}=\frac{2v_1}{\bigtriangleup T} \\ v_{max}=v_1+v_2=2v_1+a_{max}\bigtriangleup T=4v_1\\ L=16v_1 \bigtriangleup T \rightarrow v_1=\frac{L}{16\bigtriangleup T} △T=ti+1​−ti​=7tf​−t0​​L=4vmax​△Tv2​=v1​+amax​△Tamax​=△T2v1​​vmax​=v1​+v2​=2v1​+amax​△T=4v1​L=16v1​△T→v1​=16△TL​
所以最终只需知道末端位移LLL和所需时间TTT,即可计算得到整个轨迹规划曲线。其matlab代码实现如下。
其中的x_sx\_sx_s、y_sy\_sy_s、z_sz\_sz_s分别为每时的机械臂末端位置,即期望期望位姿x_dx\_dx_d的前三行。x_dx\_dx_d的后三行为末端位置的姿态信息,在本次仿真中我们默认机械臂的姿态始终为0,所以x_dx\_dx_d的后三行总是为0。

%% 0.设定初始参数
% xyz_start=[0,-120.02,533.96];       %轨迹起点(关节角为0)的末端坐标,单位mm;
xyz_start=[0,-120.02,533.96];
xyz_end=[-30,-45,385];     %轨迹终点的末端坐标;
T=10;       %完成轨迹规划的时间;%% 1.轨迹规划
L=sqrt((xyz_end(1)-xyz_start(1))^2+(xyz_end(2)-xyz_start(2))^2+(xyz_end(3)-xyz_start(3))^2);
dt=T/7;        %每段的时间长度
v1=L/(16*dt);   %第一次加速度拐点
J=2*v1/(dt*dt); %加加速度
amax=dt*J;      %最大加速度
v2=v1+dt*amax;  %第二次加速度拐点
vmax=v2+v1;     %第三次速度拐点t1 = 1*dt;
t2 = 2*dt;
t3 = 3*dt;
t4 = 4*dt;
t5 = 5*dt;
t6 = 6*dt;
t7 = 7*dt;t=0:0.1:T;vt1=1/2*J*t.^2.*(t>=0 & t<t1);
vt2=(v1+amax*(t-t1)).*(t>=t1 & t<t2);
vt3=(vmax-1/2*J*(t3-t).^2).*(t>=t2 & t<t3);
vt4=vmax.*(t>=t3 & t<t4);
vt5=(vmax-1/2*J*(t-t4).^2).*(t>=t4 & t<t5);
vt6=(v2-amax*(t-t5)).*(t>=t5 & t<t6);
vt7=(1/2*J*(t7-t).^2).*(t>=t6 & t<t7);vt=vt1+vt2+vt3+vt4+vt5+vt6+vt7;     %各时刻速度S=zeros(1,length(t));       %各时刻位移
for i=2:length(t)S(i)=trapz(t(1:i),vt(1:i));
end %各时刻xyz的位移
x_s=xyz_start(1)+(xyz_end(1)-xyz_start(1))/L*S;
y_s=xyz_start(2)+(xyz_end(2)-xyz_start(2))/L*S;
z_s=xyz_start(3)+(xyz_end(3)-xyz_start(3))/L*S;%各时刻xyz轴的速度分量
v_x=(xyz_end(1)-xyz_start(1))/L*vt;
v_y=(xyz_end(2)-xyz_start(2))/L*vt;
v_z=(xyz_end(3)-xyz_start(3))/L*vt;

3.被控对象:速度雅各比矩阵

速度雅各比矩阵方法的关系表达式如下:
v=J(q)q˙v=J(q)\dot qv=J(q)q˙​
前者vvv是末端执行器的速度,后者q˙\dot qq˙​是关节角速度。表达式的物理意义是:当关节角度发生一个微小的变化△q\bigtriangleup q△q,末端执行器也会相应产生一个微小的位姿变化△x\bigtriangleup x△x。
速度的雅各比矩阵的求解方法有多种,如1.向量积方法 2.微分法 等等…
本文采用向量积的方法,求解方法如下。

J(q)=[JvJw]=[J1J2J3J4J5J6]Ji=[Zi−1×ri−1Zi−1]=[Zi−1×(Pn−Pi−1)Zi−1]J(q)=\begin{bmatrix}J_v\\J_w\end{bmatrix}=\begin{bmatrix}J_1&J_2&J_3&J_4&J_5&J_6\end{bmatrix}\\ J_i=\begin{bmatrix}Z_{i-1} \times r_{i-1}\\Z_{i-1}\end{bmatrix}=\begin{bmatrix}Z_{i-1} \times (P_n-P_{i-1})\\Z_{i-1}\end{bmatrix} J(q)=[Jv​Jw​​]=[J1​​J2​​J3​​J4​​J5​​J6​​]Ji​=[Zi−1​×ri−1​Zi−1​​]=[Zi−1​×(Pn​−Pi−1​)Zi−1​​]
本文采用的机械臂有6个关节角,因此其速度雅各比矩阵有6列,分别为JiJ_iJi​。本文的机械臂有6个自由度,因此对应的矩阵为6行。
每个雅各比矩阵分量JiJ_iJi​的后三行为{i−1}\{i-1\}{i−1}坐标系相对于世界坐标系的ZZZ轴分量;分量JiJ_iJi​的前三行为Zi−1Z_{i-1}Zi−1​与ri−1r_{i-1}ri−1​的差乘,ri−1r_{i-1}ri−1​是末端坐标系与{i−1}\{i-1\}{i−1}坐标系的相对位置在世界坐标系中的表示。
这种方法求解只适用于standard DH方法建模的模型,若使用modify DH方法建模,则需对上式的下标做一定修改。
此方法的matlab代码实现如下:

function [ J ] = Jacob_cross_SDH( q )
%JACOB_CROSS_SDH 函数摘要
%   输入q0为逼近角,单位为弧度,矩阵大小1*6;
%   输出J为速度雅各比矩阵,矩阵大小6*6;
%   说明:利用向量积的方法求解系统的雅各比矩阵,方法1和方法2任选一种
%   说明:此求解方法基于SDH参数建模,若MDH方法建模,需进行一定的下标改动d=[105.03,0,0,75.66,80.09,44.36];
a=[0,-174.42,-174.42,0,0,0];
alp=[pi/2,0,0,pi/2,-pi/2,0];
offset=[0,-pi/2,0,-pi/2,0,0];
thd=q+offset;% 求各个关节间的变换矩阵
T0=trotz(0)*transl(0,0,0)*trotx(0)*transl(0,0,0);
T1=trotz(thd(1))*transl(0,0,d(1))*trotx(alp(1))*transl(a(1),0,0);
T2=trotz(thd(2))*transl(0,0,d(2))*trotx(alp(2))*transl(a(2),0,0);
T3=trotz(thd(3))*transl(0,0,d(3))*trotx(alp(3))*transl(a(3),0,0);
T4=trotz(thd(4))*transl(0,0,d(4))*trotx(alp(4))*transl(a(4),0,0);
T5=trotz(thd(5))*transl(0,0,d(5))*trotx(alp(5))*transl(a(5),0,0);
T6=trotz(thd(6))*transl(0,0,d(6))*trotx(alp(6))*transl(a(6),0,0);% 求各个关节相对于惯性坐标系的变换矩阵
T00 = T0;
T01 = T1;
T02 = T1*T2;
T03 = T1*T2*T3;
T04 = T1*T2*T3*T4;
T05 = T1*T2*T3*T4*T5;
T06 = T1*T2*T3*T4*T5*T6;% 求各个关节相对于末端坐标系的变换矩阵
T06 = T1*T2*T3*T4*T5*T6;
T16 = T2*T3*T4*T5*T6;
T26 = T3*T4*T5*T6;
T36 = T4*T5*T6;
T46 = T5*T6;
T56 = T6;% 提取各变换矩阵的旋转矩阵
R00 = t2r(T00);
R01 = t2r(T01);
R02 = t2r(T02);
R03 = t2r(T03);
R04 = t2r(T04);
R05 = t2r(T05);
R06 = t2r(T06);% 取旋转矩阵第3列,即Z轴方向分量
Z0 = R00(: , 3);
Z1 = R01(: , 3);
Z2 = R02(: , 3);
Z3 = R03(: , 3);
Z4 = R04(: , 3);
Z5 = R05(: , 3);
Z6 = R06(: , 3);%% Method.1
% 求末端关节坐标系相对于前面各个坐标系的位置,即齐次变换矩阵的第四列
% pi6为坐标系i和末端坐标系的相对位置在坐标系i下的表示
P06 = T06(1:3, 4);
P16 = T16(1:3, 4);
P26 = T26(1:3, 4);
P36 = T36(1:3, 4);
P46 = T46(1:3, 4);
P56 = T56(1:3, 4);
P66 = [0; 0; 0];% 使用向量积求出雅可比矩阵
% R0i为坐标系0到坐标系i的旋转矩阵
% R0i*Pi6指坐标系i和末端坐标系的相对位置在0坐标系下的表示
J1 = [cross(Z0, R00*P06); Z0];
J2 = [cross(Z1, R01*P16); Z1];
J3 = [cross(Z2, R02*P26); Z2];
J4 = [cross(Z3, R03*P36); Z3];
J5 = [cross(Z4, R04*P46); Z4];
J6 = [cross(Z5, R05*P56); Z5];%% Method.2% % pi为坐标系i与世界坐标系0的相对位置
% p0=transl(T00);
% p1=transl(T01);
% p2=transl(T02);
% p3=transl(T03);
% p4=transl(T04);
% p5=transl(T05);
% p6=transl(T06);
%
% % p6-pi为i坐标系指向末端坐标系的向量
% % p6-pi即为末端坐标系与i坐标系相对位置在世界坐标系中的表示
% % Ji=[Jv;Jw]    对应六自由度的速度分量和旋转分量
% J1 = [cross(Z0, p6-p0); Z0];
% J2 = [cross(Z1, p6-p1); Z1];
% J3 = [cross(Z2, p6-p2); Z2];
% J4 = [cross(Z3, p6-p3); Z3];
% J5 = [cross(Z4, p6-p4); Z4];
% J6 = [cross(Z5, p6-p5); Z5];J = [J1, J2, J3, J4, J5, J6];end

4.控制器:等速率趋近的滑模控制器

控制器的输入为位姿误差e=xd−xe=x_d-xe=xd​−x,输出为关节角的增量q˙\dot qq˙​,因此控制器满足关系:
u=q˙=f_SMC(e)u=\dot q=f\_SMC(e)u=q˙​=f_SMC(e)
为此需求设计滑模控制器f_SMCf\_SMCf_SMC。
设计滑模面:
s=cee=xd−xs=ce\\ e=x_d-x s=cee=xd​−x
设计趋近率为等速趋近率:
s˙=−ξsgns\dot s=-\xi sgns s˙=−ξsgns
推导得到控制器输出uuu:
s˙=ce˙=−ξsgnse˙=x˙d−x˙=−1cξsgnsv=vd+1cξsgnsu=q˙=J−1(q)v=J−1(vd+1cξsgns)\dot s=c\dot e=-\xi sgns\\ \dot e=\dot x_d-\dot x=-\frac{1}{c}\xi sgns\\ v=v_d+\frac{1}{c}\xi sgns\\ u=\dot q=J^{-1}(q)v=J^{-1}(v_d+\frac{1}{c}\xi sgns) s˙=ce˙=−ξsgnse˙=x˙d​−x˙=−c1​ξsgnsv=vd​+c1​ξsgnsu=q˙​=J−1(q)v=J−1(vd​+c1​ξsgns)
matlab代码实现如下:

dth = [0; 0; 0; 0; 0; 0];
th = [0; 0; 0; 0; 0; 0];x=[xyz_start';0;0;0];        %其实时刻的位姿lamda=1;       %阻尼矩阵的系数k = 0.1;
ita = 0.0002;
c = 5;e = [0; 0; 0; 0; 0; 0];
de = [0; 0; 0; 0; 0; 0];
for i = 1 : length(t)xd=[x_s(i);y_s(i);z_s(i);0;0;0];     %期望位姿dxd=[v_x(i);v_y(i);v_z(i);0;0;0]; %期望速度q=th(:, i);Jac = Jacob_cross_SDH(q');   %求解当前角度下的雅可比矩阵e(:, i) = xd - x(:,i);      %误差s = c*e(:, i);      %滑模面v=dxd + (1/c)*ita*sign(s);    %机械臂的末端实际速度de(:, i) = dxd - v;     %误差的微分dth(:, i) = inv(Jac+lamda.*diag(ones(1,6)))*v;      %关节角的增量th(:, i + 1) = th(:, i) + dth(:, i)*0.1;    %下一时刻的关节角度x(:, i+1) = x(:, i) + v*0.1;    %机械臂末端实际位姿
end

更多更高级的滑模控制器设计请点击博主的控制器设计github仓库。

5.结果展示

设定轨迹跟踪起始点和终点:
[0−120.02533.96]→[−30−45385]\begin{bmatrix}0\\-120.02\\533.96\end{bmatrix} \rightarrow \begin{bmatrix}-30\\-45\\385\end{bmatrix} ⎣⎡​0−120.02533.96​⎦⎤​→⎣⎡​−30−45385​⎦⎤​
10s10s10s内的末端轨迹位置在各坐标轴的映射:

10s10s10s内系统跟踪末端轨迹位置的误差:

10s10s10s内各个关节角的角度:


源代码下载链接:https://github.com/Fantasty9413/Trajectory-tracking-

【机械臂】六轴六自由度机械臂轨迹跟踪的matlab实现(基于速度雅各比矩阵方法)相关推荐

  1. 空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂(双臂)轨迹跟踪控制matlab仿真程序

    空间机械臂Matlab/Simulink仿真程序自由漂浮空间机械臂(双臂)轨迹跟踪控制matlab仿真程序,含空间机器人动力学模型,PD控制程序,带仿真结果,可供二次开发学习 ID:672006146 ...

  2. 六轴多关节机器人跟四轴机器人差别在哪里呢

    六轴机器人功能多,应用范围广阔,更换不同的夹持工具可以应付各种各样的工业生产活动,那么这么高大上的设备,在我们要改造自动化的时候,是不是一定要用六轴的关节机械臂呢 答案显然是不一定的,除了关节机械臂, ...

  3. 三轴加速度传感器和六轴惯性传感器_六轴加速度传感器Beacon

    六轴加速度传感器Beacon 一.产品简介: 六轴加速度传感器Beacon是重庆金瓯科技发展有限责任公司基于Beacon技术研发的新一代传感器产品.该产品可以将采集的加速度数据信息通过Beacon广播 ...

  4. 六轴机械臂下位机(arduino)+上位机(ROS+Moveit)---(一)机械臂硬件

    六轴机械臂下位机(arduino)+上位机(ROS+Moveit)---(一)机械臂硬件 机械部分 机械臂制作时的注意点!!!(坑) 零件的3D打印 控制器接线问题 机械部分 六轴机械臂在工业领域的运 ...

  5. 大象机器人推出史上最紧凑的六自由度机械臂-mechArm

    2020年,秉持"Enjoy Robots World"的愿景和使命,在保留大部分工业型机器人功能的前提下,大象机器人与M5stack 强强联合共同出品了myCobot --全球最 ...

  6. 基于蚁群算法的六轴机械臂路径规划(运动学模型建立)

    机器人运动学模型的建立 1 D-H参数法建立坐标系 2 机器人运动学分析 2.1 运动学正解 2.2 运动学逆解 3 机器人的轨迹仿真 1 D-H参数法建立坐标系 代码: clear; clc; %建 ...

  7. 资深老师傅讲解六轴机器人机械臂的特征和优缺点!

    资深老师傅讲解六轴机器人机械臂的特征和优缺点! 六轴关节式机器人机械手臂使用旋转轴(或者叫活关节)进行装载.卸载和后处理工作.它使用一直线轴重新定位,可以做出灵活得像人类一样的动作.其具有六轴自由度, ...

  8. 桌面六轴机械臂myCobot初体验

    MyCobot是一款入门级的六自由度机械臂,目前是6轴机械臂中价格和性能十分优良的机械臂,本文的主题主要以myCobot 280 M5Stack的操作体验来记录一下使用这款机械臂的过程.以python ...

  9. 六轴机械臂mycobot初体验

    MyCobot是一款入门级的六自由度机械臂,目前是6轴机械臂中价格和性能十分优良的机械臂,本文的主题主要以myCobot 280 M5Stack的操作体验来记录一下使用这款机械臂的过程.以python ...

  10. Kinect XBOX 360和六轴机械臂的实时映射

    Kinect XBOX 360和六轴机械臂的实时映射 ROS与UR5机械臂的通讯 Kinect1 进行人体骨骼追踪 骨骼追踪 Kinect得到的数据控制UR5机械臂运动 总结 时间:2019-06 地 ...

最新文章

  1. 计算最长的字符串长度
  2. JZOJ 5925. 【NOIP2018模拟10.25】naive 的瓶子
  3. python类的专有方法_Python——类的专有方法
  4. 大文本存mysql怎么建索引_如何正确合理的建立MYSQL数据库索引
  5. 利用递归遍历文件夹和文件存入TreeView
  6. centos so查看_照片信息查看器app安卓下载-照片信息查看器app下载v1.1.0 安卓版
  7. python读取mat文件报错【NotImplementedError: Please use HDF reader for matlab v7.3 files】
  8. 送书 |《Python大数据与机器学习实战》
  9. PCB SI9000阻抗计算引擎Web方式实现方法
  10. vc使用nmake时报错
  11. opencv如何隐藏窗口-cvNameWindow创建窗口的时候会创建两个窗口,一个主窗口,一个子窗口。
  12. python 时频图_python,地震波形、时频图、频谱图计算和显示软件
  13. python 批量打印PDF
  14. (性能测试压箱底知识点)LR性能测试---结果(样例)分析
  15. snmpwalk与snmpget的区别
  16. 原生JS实现弹幕的简单操作速成
  17. js返回上一页,下一页
  18. CISP证书价值如何
  19. 用Qemu搭建x86学习环境
  20. vs2017打包所有文件到一个.exe或.msi 默认安装位置,加入桌面与菜单快捷方式

热门文章

  1. 计算机硬件故障诊断的原则,计算机硬件常见故障诊断和维护.doc
  2. 计算机术语宏是什么意思,宏(计算机术语)
  3. vb视屏教程计算机二级b,计算机二级vb教程|二级vb视频教程|计算机二级视频教程|快速通过计算机二级VB|猎豹网校视频...
  4. 基于GIS+BIM技术的高速公路征地拆迁管理系统应用
  5. 一天一看————以太网交换机的基本原理与配置!
  6. visio中绘制空间坐标系,visio导入科学图形包
  7. QT资源库中图片的调用
  8. 30 个数据工程必备的Python 包
  9. OpenJudge NOI题库 入门 116题 (一)
  10. 4am永远 鼠标按键设置_罗技的MX Master鼠标是苹果Mac系统下最好的鼠标