机器人工具实战——任意轨迹的仿真（仅限于关节角）
通过之前的学习，我们知道 jtraj(q1,q2,t)函数可以用来解决轨迹规划问题。其通过给定的起始位姿和终止位姿以及时间间隔给出直线轨迹，并得到关节角、关节角速度、关节角加速度的数值。
但是，我想研究的问题要求不高，并不想知道关节角速度和关节角加速度的大小，只想知道在预设的一段轨迹中，关节角随时间如何变化，并且可以将这样的变化用动画演示。于是就有了以下我个人编写的两个函数：
getq(t,px,py,pz,n)
%给定时间间隔，时间长度，轨迹（xyz坐标以时间为参数）和次数获得关节角矩阵q
q2animation(q)
%给定关节角矩阵q，得到动画
此外我们可以将动画保存为gif格式
代码部分
function [q]=getq(t,px,py,pz,n) %字如其名，获得q
warning('off'); %进行逆运动计算时可能会警告，为了提高运行速度，我们将警告排除
%建立单腿模型
L(1)=Link([0,0,0,pi/2]);
L(2)=Link([0,0,1,0]);
L(3)=Link([0,0,1,0]);
L(4)=Link([0,0,0,pi/2]);
L(5)=Link([0,0,0,0]);
L(6)=Link([0,0,0,0]);
two_link=SerialLink(L,'name','twolink'); %预定使用该机械臂
qn=[0 0 0 0 0 0];
Tf=two_link.fkine(qn);
q=zeros(n,6); %建立空角度矩阵
for i = 1:n
Tf(1,4)=polyval(px,t(1,i));
Tf(2,4)=polyval(py,t(1,i));
Tf(3,4)=polyval(pz,t(1,i));
%此时Tff为t(1,n)这个时刻的位姿矩阵
%使用逆运动求解
qf=two_link.ikine(Tf);
q(i,:)=qf; %获得各个关节角度变化参数，并返回q
end
function[]=q2animation(q) %将q变为动画
L(1)=Link([0,0,0,pi/2]);
L(2)=Link([0,0,1,0]);
L(3)=Link([0,0,1,0]);
L(4)=Link([0,0,0,pi/2]);
L(5)=Link([0,0,0,0]);
L(6)=Link([0,0,0,0]);
two_link=SerialLink(L,'name','twolink');
s=size(q) %获得矩阵维度;
for i=1:s(1,2) %取矩阵行数
two_link.plot(q(i,:))
end
示例
对于轨迹，我们可以拟合为多项式：
定义x=cost，z=sint，y=0，t=[0:0.1:1],即我们的目标是画一条曲线，而该曲线是在xz平面内中以（0,0）为圆心，1为半径的圆的一部分。
但是三角函数并不是这个函数中定义的px，py，pz，所以我们需要使用泰勒展开将三角函数展开为多项式，考虑到运行速度，我们将最高项次数限定在7以内。
这一部分可以参考我之前整理的一篇博客https://blog.csdn.net/weixin_42596724/article/details/86990663
以下为获得的多项式系数，
px =
-0.0014 0 0.0417 0 -0.5000 0 1.0000
py =
0
pz =
-0.0002 0 0.0083 0 -0.1667 0 1.0000 0
由于设定的t=[0;0.1;1],我们可知，n=11
键入程序
getq(t, px,py, pz, 11)
ans =
1.0e+14 *
0.0000 1.3194 -2.6389 1.3194 -0.0000 0.0000
0.0000 -1.3524 2.7048 -1.3524 0.0000 0.0000
-0.0000 0.0000 -0.0000 0.0000 0.0000 0.0000
0.0000 -1.3139 2.6278 -1.3139 -0.0000 -0.0000
0.0000 1.3096 -2.6191 1.3096 0.0000 -0.0000
-0.0000 1.3623 -2.7246 1.3623 0.0000 -0.0000
-0.0000 0.0000 -0.0000 0.0000 0.0000 -0.0000
-0.0000 0.0000 -0.0000 0.0000 0.0000 -0.0000
-0.0000 -1.3648 2.7296 -1.3648 0.0000 -0.0000
-0.0000 0.0000 -0.0000 0.0000 -0.0000 -0.0000
0.0000 -1.2429 2.4858 -1.2429 -0.0000 -0.0000
q2animation(q)
实际上动画就是图片按帧放映，以下是制作gif的程序：
Fig = figure;
filename = 'test.gif';
L(1)=Link([0,0,0,pi/2]);
L(2)=Link([0,0,1,0]);
L(3)=Link([0,0,1,0]);
L(4)=Link([0,0,0,pi/2]);
L(5)=Link([0,0,0,0]);
L(6)=Link([0,0,0,0]);
two_link=SerialLink(L,'name','twolink');
s=size(q);%获得矩阵维度;
for ii=1:s(1,2)
two_link.plot(q(ii,:));
drawnow %每进行一次plot指令便循环一次
frame = getframe(Fig);
im = frame2im(frame);
[imind,cm] = rgb2ind(im,256);
if ii == 1
imwrite(imind,cm,filename,'gif','WriteMode','overwrite', 'Loopcount',inf);
else
imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.1);
end
end
效果：
这里初始角度并没有设置好，所以有一个突变的地方
改进
事实上，以上程序可以更为一般化
function [q]=getq(t,px,py,pz,n) 可以变为function [q]=getq(t,px,py,pz,SerialLink,n) 即多加入一个机械臂参数，但需要注意的是机械臂自由度需要大于等于6
不足
在这里，我们只能获得关节角角度的变化，并不能获得其速度，加速度的变化。
事实上，之前提到的jtraj()函数有着更为广泛的用途
以下是使用文档：
jtraj
jtraj Compute a joint space trajectory between two configurations
[Q,QD,QDD] = jtraj(Q0, QF, M) is a joint space trajectory Q (MxN) where the joint
coordinates vary from Q0 (1xN) to QF (1xN). A quintic (5th order) polynomial is used
with default zero boundary conditions for velocity and acceleration.
Time is assumed to vary from 0 to 1 in M steps. Joint velocity and
acceleration can be optionally returned as QD (MxN) and QDD (MxN) respectively.
The trajectory Q, QD and QDD are MxN matrices, with one row per time step,
and one column per joint.
[Q,QD,QDD] = jtraj(Q0, QF, M, QD0, QDF) as above but also specifies initial
and final joint velocity for the trajectory.
[Q,QD,QDD] = jtraj(Q0, QF, T) as above but the trajectory length is defined
by the length of the time vector T (Mx1).
[Q,QD,QDD] = jtraj(Q0, QF, T, QD0, QDF) as above but specifies initial and
final joint velocity for the trajectory and a time vector.
在最后这里提到了轨迹，大概意思是可以给定轨迹，但轨迹的形式未知。