机器人工具实战——任意轨迹的仿真（仅限于关节角）

本文介绍了一种方法，通过自定义函数getq和q2animation实现机器人关节角在预设轨迹中的变化，并以动画形式展示。文章讨论了如何将轨迹拟合为多项式，并限制最高项次数，同时指出程序可以进一步改进以适应不同机械臂参数。虽然能显示关节角变化，但无法获取速度和加速度信息。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

通过之前的学习，我们知道 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