六轴六自由度机械臂轨迹规划的matlab实现（基于速度雅各比矩阵方法）
六轴六自由度机械臂轨迹规划的matlab实现（基于速度雅各比矩阵方法）
六轴六自由度机械臂轨迹规划的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−t0L=4vmax△Tv2=v1+amax△Tamax=△T2v1vmax=v1+v2=2v1+amax△T=4v1L=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)=[JvJw]=[J1J2J3J4J5J6]Ji=[Zi−1×ri−1Zi−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−x
s=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
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-
- 六轴机器人建模方法、正逆解、轨迹规划实例与Matalb Robotic Toolbox 的实现
- 六轴机器人建模方法、正逆解、轨迹规划实例与Matalb Robotic Toolbox 的实现
- 基于MATLAB机器学习工具箱的SVM支持向量机实现方法——fitcsvm
- 基于模型的动态规划方法理论——策略迭代方法 and 值迭代方法（maze代码实现）
- 基于java.util.logging实现轻量级日志记录库（增加根据当前类class初始化，修复线程池模型(javaEE)下的堆栈轨迹顺序与当前调用方法不一致问题）
- 基于动态规划的矩阵连乘最优方法
- 动态规划之矩阵连乘问题Python实现方法
- matlab中实现矩阵删除一行或一列的方法
- 电机与拖动：异步交流电动机改变电压，转子电阻及频率的机械特性曲线（Matlab实现方法）
- 轨迹系列——Socket总结及实现基于TCP或UDP的809协议方法
- 【转载】基于Frenet优化轨迹的无人车动作规划方法
- 基于密度的空间的数据聚类方法DBSCAN(Density-Based Spatial Clustering of Applications with Noise)matlab实现
- 基于java.util.logging实现轻量级日志记录库（增加根据当前类class初始化，修复线程池模型(javaEE)下的堆栈轨迹顺序与当前调用方法不一致问题）
- 基于AR模型谱估计算法（Yule-Walker方法与Burg方法）的C++实现
- 基于混合TCP-UDP的HTTP协议实现方法
- 利用Windows Group实现基于角色的WCF接口方法调用授权
- 矩阵论基础 2.5 用Matlab实现矩阵的基本运算
- silverlight线程与基于事件驱动javascript引擎（实现轨迹回放功能）
- 基于Spring+Hibernate的通用增删改查的实现方法
- 两种方法实现动态轨迹