bsplinepolytraj
使用b樣條生成多項式軌跡。
% 生成一個分段三次b樣條軌跡
% control points---表示一組二維xy控制點，控制點構成直線段多邊形，b樣條根據這些控制點生成多邊形對應的曲線
% tInterVal---起始時間
% tSamples---按照一定週期構成的時間樣本
[q,qd,qdd,pp] = bsplinepolytraj(control points,tInterval,tSamples);
該函數輸出軌跡位置(q)、速度(qd)、加速度(qdd)、時間矢量(tvec)和多項式係數(pp)，多項式利用梯形速度實現路徑點。
%lesson_10_bsplinepolytraj 使用b樣條生成多項式軌跡
% [q,qd,qdd,pp] = bsplinepolytraj(control points,tInterval,tSamples)生成一個分段三次b樣條軌跡，
% 該軌跡落在由控制點定義的控制多邊形中。在tInterval中給定的起始時間和結束時間之間對彈道進行均勻採樣。
% 函數返回輸入時間樣本tSamples的位置、速度和加速度。該函數還返回多項式軌跡關於時間的分段多項式pp形式。
%使用bsplinepolytraj函數指定一組二維xy控制點。b樣條使用這些控制點在多邊形內創建軌跡。還給出了路點的時間點。
cpts = [1 4 4 3 -2 0; 0 1 2 4 3 1];
tpts = [0 5];
%計算b樣條軌跡
tvec = 0:0.01:5;
[q, qd, qdd, pp] = bsplinepolytraj(cpts,tpts,tvec);
figure
plot(cpts(1,:),cpts(2,:),'xb-')
hold all
plot(q(1,:), q(2,:))
xlabel('X')
ylabel('Y')
hold off
%繪製b樣條軌跡中每個元素的位置。這些軌跡是在時間上參數化的三次分段多項式。
figure
plot(tvec,q)
hold all
plot([0:length(cpts)-1],cpts,'x')
xlabel('t')
ylabel('Position Value')
legend('X-positions','Y-positions')
hold off
cubicpolytraj
生成三階多項式軌跡。
[q,qd,qdd,pp] = polytraj(wayPoints,timePoints,tSamples)
生成一個三階多項式，該多項式實現一組給定的輸入waypoint和相應的時間點。
該函數輸出給定時間樣本tSamples的位置q、速度qd和加速度qdd。該函數還返回多項式軌跡關於時間的分段多項式pp形式。
wpts = [1 4 4 3 -2 0; 0 1 2 4 3 1];
tpts = 0:5;
tvec = 0:0.01:5;
% 每個航路點的速度邊界條件，指定爲由“ VelocityBoundaryCondition”和n×p矩陣組成的逗號分隔對。
% 每行對應於軌跡中各個變量在所有p個航路點的速度
[q, qd, qdd, pp] = cubicpolytraj(wpts, tpts, tvec, 'VelocityBoundaryCondition',[1 0 -1 -1 0 0; 1 1 1 -1 -1 -1]);
plot(tvec, q)
hold all
plot(tpts, wpts, 'x')
xlabel('t')
ylabel('Positions')
legend('X-positions','Y-positions')
hold off
%您還可以驗證二維平面中的實際位置。將q向量和路徑點的單獨行繪製爲x和y位置。
figure
plot(q(1,:),q(2,:),'-b',wpts(1,:),wpts(2,:),'or')
xlabel('X')
ylabel('Y')
quinticpolytraj
生成五階多項式軌跡。
[q, qd, qdd, pp] = quinticpolytraj(wayPoints,timePoints,tSamples)
生成一個五階多項式，該多項式實現一組給定的輸入wayPoints和相應的時間點。
該函數輸出給定時間樣本tSamples的位置、速度和加速度。該函數還返回多項式軌跡關於時間的分段多項式pp形式。
wpts = [1 4 4 3 -2 0; 0 1 2 4 3 1];
tpts = 0:5;
tvec = 0:0.01:5;
% 每個航路點的速度邊界條件，指定爲由“ VelocityBoundaryCondition”和n×p矩陣組成的逗號分隔對。
% 每行對應於軌跡中各個變量在所有p個航路點的速度
[q, qd, qdd, pp] = quinticpolytraj(wpts, tpts, tvec,'VelocityBoundaryCondition',[1 0 -1 -1 0 0; 1 1 1 -1 -1 -1]);
plot(tvec, q)
hold all
plot(tpts, wpts, 'x')
xlabel('t')
ylabel('Positions')
legend('X-positions','Y-positions')
hold off
figure
plot(q(1,:),q(2,:),'.b',wpts(1,:),wpts(2,:),'or')
xlabel('X')
ylabel('Y')
rottraj
生成方向旋轉矩陣之間的軌跡。
[R,omega,alpha] = rottraj(r0,rF,tInterval,tSamples)
生成一個軌跡，該軌跡在r0和rF兩個方向之間插入點，這些點基於時間間隔和給定的時間樣本。
%定義兩個四元數路徑點在它們之間插入。
q0 = quaternion([0 pi/4 -pi/8],'euler','ZYX','point');
qF = quaternion([3*pi/2 0 -3*pi/4],'euler','ZYX','point');
tvec = 0:0.01:5;
%w方向角速度，方向角加速度
[qInterp1,w1,a1] = rottraj(q0,qF,[0 5],tvec);
plot(tvec,compact(qInterp1))
title('Quaternion Interpolation (Uniform Time Scaling)')
xlabel('t')
ylabel('Quaternion Values')
legend('W','X','Y','Z')
%在旋轉矩陣之間插入軌跡
r0 = [1 0 0; 0 1 0; 0 0 1];
rF = [0 0 1; 1 0 0; 0 0 0];
tvec = 0:0.1:1;
%生成軌跡。使用plotTransforms繪製結果。將旋轉矩陣轉換爲四元數並指定零平移。圖中顯示了座標系的所有中間旋轉。
%w方向角速度，方向角加速度
[rInterp1,w1,a1] = rottraj(r0,rF,[0 1],tvec);
rotations = rotm2quat(rInterp1);
zeroVect = zeros(length(rotations),1);
translations = [zeroVect,zeroVect,zeroVect];
plotTransforms(translations,rotations)
xlabel('X')
ylabel('Y')
zlabel('Z')
transformtraj
生成兩個齊次矩陣之間的軌跡。
[tforms,vel,acc] = transformtraj(T0,TF,tInterval,tSamples)
生成一個軌跡，該軌跡在兩個4×4的齊次變換T0和TF之間插入，其中的點基於時間間隔tInterval和給定的時間樣本tSamples。
% 從兩個方向和位置構建轉換。給出了插值的時間間隔和時間矢量。
% axang2tform---軸角轉換爲齊次變換矩陣
t0 = axang2tform([0 1 1 pi/4])*trvec2tform([0 0 0]);
tF = axang2tform([1 0 1 6*pi/5])*trvec2tform([1 1 1]);
tInterval = [0 1];
tvec = 0:0.01:1;
% 在點之間插入。使用plotTransforms繪製軌跡。將轉換轉換爲四元數旋轉和線性轉換。圖中顯示了座標系的所有中間變換。
[tfInterp, v1, a1] = transformtraj(t0,tF,tInterval,tvec);
rotations = tform2quat(tfInterp);
translations = tform2trvec(tfInterp);
plotTransforms(translations,rotations)
xlabel('X')
ylabel('Y')
zlabel('Z')
trapveltraj
生成帶有梯形速度剖面的軌跡。
[q,qd,qdd,tSamples,pp] = trapveltraj(wayPoints,numSamples)
通過給定的一組遵循梯形速度剖面的輸入wayPoints生成軌跡。該函數輸出給定時間內的位置q、速度qd和加速度qdd。該函數還返回多項式軌跡關於時間的分段多項式pp形式。
wpts = [0 45 15 90 45; 90 45 -45 15 90];
[q, qd, qdd, tvec, pp] = trapveltraj(wpts, 501);
subplot(2,1,1)
plot(tvec, q)
xlabel('t')
ylabel('Positions')
legend('X','Y')
subplot(2,1,2)
plot(tvec, qd)
xlabel('t')
ylabel('Velocities')
legend('X','Y')
%您還可以驗證二維平面中的實際位置。將q向量和路徑點的單獨行繪製爲x和y位置。
figure
plot(q(1,:),q(2,:),'-b',wpts(1,:),wpts(2,:),'or')
