?

?Figure. Several possible path shapes for a single joint

五次多項(xiàng)式曲線(quintic polynomial)

?

$$\theta(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$$

?　　考慮邊界條件：

$$\begin{align*}?
\theta_0&=a_0\\
\theta_f&=a_0+a_1t+a_2{t_f}^2+a_3{t_f}^3+a_4{t_f}^4+a_5{t_f}^5\\
\dot{\theta_0}&=a_1\\
\dot{\theta_f}&=a_1+2a_2t_f+3a_3{t_f}^2+4a_4{t_f}^3+5a_5{t_f}^4\\
\ddot{\theta_0}&=2a_2\\
\ddot{\theta_f}&=2a_2+6a_3{t_f}+12a_4{t_f}^2+20a_5{t_f}^3\\
\end{align*}$$

?　　這6組約束構(gòu)成了一個(gè)6個(gè)未知數(shù)的線性方程組，可以求出系數(shù)為：

$$\begin{align*}?
a_0&=\theta_0\\
a_1&=\dot{\theta_0}\\
a_2&=\frac{\ddot{\theta_0}}{2}\\
a_3&=\frac{20\theta_f-20\theta_0-(8\dot{\theta_f}+12\dot{\theta_0})t_f-(3\ddot{\theta_0}-\ddot{\theta_f}){t_f}^2}{2{t_f}^3}\\
a_4&=\frac{30\theta_0-30\theta_f+(14\dot{\theta_f}+16\dot{\theta_0})t_f+(3\ddot{\theta_0}-2\ddot{\theta_f}){t_f}^2}{2{t_f}^4}\\
a_5&=\frac{12\theta_f-12\theta_0-(6\dot{\theta_f}+6\dot{\theta_0})t_f-(\ddot{\theta_0}-\ddot{\theta_f}){t_f}^2}{2{t_f}^5}
\end{align*}$$

?

?　　在MATLAB機(jī)器人工具箱中函數(shù)tpoly可以用于計(jì)算并生成機(jī)器人單軸的五次多項(xiàng)式軌跡曲線。當(dāng)$t \in [0,T]$時(shí)，五次多項(xiàng)式曲線以及其一階導(dǎo)數(shù)、二階導(dǎo)數(shù)都是連續(xù)光滑的多項(xiàng)式曲線：

$$\begin{align*}?
S(t)&=At^5+Bt^4+Ct^3+Dt^2+Et+F\\
\dot{S}(t)&=5At^4+4Bt^3+3Ct^2+2Dt+E\\
\ddot{S}(t)&=20At^3+12Bt^2+6Ct+2D
\end{align*}$$

　　根據(jù)約束條件

　　可以寫(xiě)出矩陣方程如下：

　　利用MATLAB提供的左除（反除）操作符，可以方便的求解線性方程組：Ax=b → x=A\b（表示矩陣A的逆乘以b)

　　?tpoly.m主要內(nèi)容如下：

%TPOLY Generate scalar polynomial trajectory% [S,SD,SDD] = TPOLY(S0, SF, T, SD0, SDF) as above but specifies initial % and final joint velocity for the trajectory and time vector T.function [s,sd,sdd] = tpoly(q0, qf, t, qd0, qdf)if isscalar(t)t = (0:t-1)';elset = t(:);endif nargin < 4qd0 = 0;endif nargin < 5qdf = 0;endtf = max(t);% solve for the polynomial coefficients using least squaresX = [0 0 0 0 0 1tf^5 tf^4 tf^3 tf^2 tf 10 0 0 0 1 05*tf^4 4*tf^3 3*tf^2 2*tf 1 00 0 0 2 0 020*tf^3 12*tf^2 6*tf 2 0 0];coeffs = (X \ [q0 qf qd0 qdf 0 0]')';% coefficients of derivatives coeffs_d = coeffs(1:5) .* (5:-1:1);coeffs_dd = coeffs_d(1:4) .* (4:-1:1);% evaluate the polynomialsp = polyval(coeffs, t);pd = polyval(coeffs_d, t);pdd = polyval(coeffs_dd, t);

　　在MATLAB中輸入下面命令生成從位置0運(yùn)動(dòng)到1的五次多項(xiàng)式曲線（時(shí)間步數(shù)為50步）：

>> [s, sd, sdd] = tpoly(0, 1, 50);

　　其位置、速度、加速度曲線如下圖所示：

　　雖然這三條曲線都是連續(xù)且光滑的，但卻存在一個(gè)很實(shí)際的問(wèn)題。從速圖曲線中可以看出在t=25時(shí)速度達(dá)到最大值，沒(méi)有勻速段，其它時(shí)刻速度都小于最大值。平均速度除以最大速度的值為：mean(sd) / max(sd) =?0.5231，即平均速度只有最大速度的一半左右，速度利用率較低。對(duì)于大多數(shù)實(shí)際伺服系統(tǒng)，電機(jī)的最大速度一般是固定的，因此希望速度曲線在最大速度的時(shí)間盡可能長(zhǎng)。

梯形速度曲線 / Linear segment with parabolic?blend (LSPB) trajectory?

?　　高次多項(xiàng)式軌跡曲線的計(jì)算量比較大，我們也可以考慮用直線段來(lái)構(gòu)造簡(jiǎn)單的軌跡曲線，但是在不同直線段的交接處會(huì)發(fā)生速度跳變的情況（位移曲線不光滑），如果用拋物線（parabolic blend）進(jìn)行拼接就可以得到光滑的軌跡。如下圖所示，單軸從$t_0$開(kāi)始勻加速運(yùn)動(dòng)（位移曲線為拋物線）；$t_b$時(shí)刻達(dá)到最大速度，進(jìn)行勻速直線運(yùn)動(dòng)（位移曲線為直線段）；從$t_f-t_b$時(shí)刻開(kāi)始進(jìn)行勻減速運(yùn)動(dòng)，$t_f$時(shí)刻減速為零并到達(dá)目標(biāo)位置。曲線關(guān)于時(shí)間中點(diǎn)$t_h$對(duì)稱，由于這種軌跡的速度曲線是梯形的，因此也稱為梯形速度（trapezoidal velocity trajectory）曲線，在電機(jī)驅(qū)動(dòng)器中被廣泛使用。

Figure. Linear segment with parabolic blends

　　MATLAB機(jī)器人工具箱中函數(shù)lspb可以用于計(jì)算并生成梯形速度曲線，下面的命令生成從位置0運(yùn)動(dòng)到1的梯形速度軌跡曲線，時(shí)間步數(shù)為50步，最大速度為默認(rèn)值：

>> [s, sd, sdd] = lspb(0, 1, 50);

　　另外也可以指定最大速度（In fact the velocity cannot be chosen arbitrarily, too high or toolow a value for the maximum velocity will result in an infeasible trajectory?）：

>> s = lspb(0, 1, 50, 0.025); >> s = lspb(0, 1, 50, 0.035);

　　下圖a是默認(rèn)最大速度的曲線，圖b是指定不同速度的對(duì)比。

??　　lspb.m的主要內(nèi)容如下：

%LSPB Linear segment with parabolic blend % % [S,SD,SDD] = LSPB(S0, SF, M) is a scalar trajectory (Mx1) that varies % smoothly from S0 to SF in M steps using a constant velocity segment and % parabolic blends (a trapezoidal velocity profile). Velocity and % acceleration can be optionally returned as SD (Mx1) and SDD (Mx1) % respectively. % % [S,SD,SDD] = LSPB(S0, SF, M, V) as above but specifies the velocity of % the linear segment which is normally computed automatically.function [s,sd,sdd] = lspb(q0, q1, t, V)if isscalar(t)t = (0:t-1)';elset = t(:);endtf = max(t(:));if nargin < 4 % if velocity not specified, compute itV = (q1-q0)/tf * 1.5;elseV = abs(V) * sign(q1-q0); % 判斷實(shí)際速度符號(hào)if abs(V) < abs(q1-q0)/tferror('V too small');elseif abs(V) > 2*abs(q1-q0)/tferror('V too big');endendif q0 == q1 % 目標(biāo)位置和起始位置相同 s = ones(size(t)) * q0;sd = zeros(size(t));sdd = zeros(size(t));returnendtb = (q0 - q1 + V*tf)/V; % 計(jì)算勻加減速段時(shí)間a = V/tb;s = zeros(length(t), 1);sd = s;sdd = s;for i = 1:length(t)tt = t(i);if tt <= tb % 勻加速段 % initial blends(i) = q0 + a/2*tt^2;sd(i) = a*tt;sdd(i) = a;elseif tt <= (tf-tb) % 勻速段% linear motions(i) = (q1+q0-V*tf)/2 + V*tt;sd(i) = V;sdd(i) = 0else % 勻減速段% final blends(i) = q1 - a/2*tf^2 + a*tf*tt - a/2*tt^2;sd(i) = a*tf - a*tt;sdd(i) = -a;endend

?

?多自由度軌跡規(guī)劃?

?　　機(jī)器人工具箱中的函數(shù)mtraj可以在內(nèi)部調(diào)用單自由度軌跡生成函數(shù)，來(lái)生成多個(gè)軸的運(yùn)動(dòng)軌跡。mtraj第一個(gè)參數(shù)為單自由度軌跡生成函數(shù)的句柄，q₀和q_f分別為起始和目標(biāo)點(diǎn)的坐標(biāo)（是一個(gè)多維向量）。

function [S,Sd,Sdd] = mtraj(tfunc, q0, qf, M)if ~isa(tfunc, 'function_handle')error('first argument must be a function handle');endM0 = M;if ~isscalar(M)M = length(M);endif numcols(q0) ~= numcols(qf)error('must be same number of columns in q0 and qf')ends = zeros(M, numcols(q0));sd = zeros(M, numcols(q0));sdd = zeros(M, numcols(q0));for i=1:numcols(q0)% for each axis[s(:,i),sd(:,i),sdd(:,i)] = tfunc(q0(i), qf(i), M);end

　　mtraj可以調(diào)用tpoly或lspb，在50步內(nèi)生成從(0, 2)運(yùn)動(dòng)到(1, -1)的軌跡。返回值x是一個(gè)50×2的矩陣，每一列代表一個(gè)軸的數(shù)據(jù)，每一行代表一個(gè)時(shí)間點(diǎn)。

>> x = mtraj(@tpoly, [0 2], [1 -1], 50); >> x = mtraj(@lspb, [0 2], [1 -1], 50);
>> plot(x)

　　在指定的時(shí)間內(nèi)x₁從0運(yùn)動(dòng)到1，x₂從2運(yùn)動(dòng)到-1：

?

?

?

參考：

V-rep學(xué)習(xí)筆記：Reflexxes Motion Library 4

多軸插補(bǔ)為什么普遍使用梯形速度曲線？

工業(yè)機(jī)器人運(yùn)動(dòng)軌跡規(guī)劃方法簡(jiǎn)述

Introduction to Robotics - Mechanics and Control. Chapter 7 Trajectory generation

Robotics, vision and control fundamental algorithms in MATLAB?Chapter 3 Time and Motion

轉(zhuǎn)載于:https://www.cnblogs.com/21207-iHome/p/7843517.html

總結(jié)

以上是生活随笔為你收集整理的机器人中的轨迹规划（Trajectory Planning ）的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。