軌迹跟蹤算法：L1非線性跟蹤算法示範(附matlab simple demo)

概覽

L1算法的核心公式隻有一個：

a s c m d = 2 V 2 L 1 sin ⁡ η a_{s_{\mathrm{cmd}}}=2 \frac{V^{2}}{L_{1}} \sin \eta ascmd​​=2L1​V2​sinη

其中符号解釋如下圖：

一句話解釋：L1跟蹤算法解出了一個橫向加速度作用于對象，輸入為對象目前的速度矢量、與對象相距L1的待跟蹤軌迹上的參考點。至于 η \eta η是根據參考點位置和對象位置以及對象速度矢量來計算的。用L1來做跟蹤的關鍵是如何求取待跟蹤軌迹上的一點，該點到對象的位置距離恰好為L1。

L1算法的橫向加速度作用于對象使對象做半徑為R的圓周運動向參考點靠近[1]，當跟蹤軌迹為直線時，L1算法與PID跟蹤基本一緻，當跟蹤軌迹為圓弧時，L1算法可自動調節系統阻尼，是以相較于PID跟蹤更優。

關于[1]的證明，我覺得與其說是證明，不如說L1算法的推導，作者是先有了想讓對象以半徑為R的圓周運動靠近參考點這麼個想法之後推導出了L1：

a = V 2 R = V 2 L 1 2 / s i n η = 2 V 2 L 1 sin ⁡ η a=\frac{V^2}{R}=\frac{V^2}{\frac{L_1}{2}/sin\eta}=2 \frac{V^{2}}{L_{1}} \sin \eta a=RV2​=2L1​​/sinηV2​=2L1​V2​sinη

Simple demo by Matlab

clc; clear; close all;
% 該檔案僅僅是比較simple的L1跟蹤示範，沒有考慮初始位置自身L1半徑的圓與待跟蹤軌迹無解的情況
%% parameters
V = 5;
L1 = 0.5;
[x0,y0,theta0] = deal(2,0.5,pi/3);  % 初始值需要保證在待跟蹤軌迹上存在參考點（具體來說：兩圓需要有交點）
deltaT = 0.02;
%% trace
figure();
syms x y;
trace = ezplot('x^2+y^2=5'); hold on;
start = scatter(x0,y0,80,"cyan",'filled','o','MarkerEdgeColor','k');
xlim([-4,4]); ylim([-4,4]);
%% loop
iterMax = 100;
[x,y,theta] = deal(x0,y0,theta0);
syms m1 m2;
for i = 1:iterMax
    h = solve(m1^2+m2^2==5,(m1-x)^2+(m2-y)^2==L1^2,m1,m2); % 求解可能的參考點 L1為fixed length
    s1 = double([h.m1(1),h.m2(1)]);
    s2 = double([h.m1(2),h.m2(2)]);
    t1 = acos(s1*[cos(theta),sin(theta)]'/norm(s1)/norm([cos(theta),sin(theta)])); % 求解參考點與V的夾角
    t2 = acos(s2*[cos(theta),sin(theta)]'/norm(s2)/norm([cos(theta),sin(theta)]));
    [eta,ind] = min([t1,t2]);  % 選夾角小的
    s = [s1;s2];
    ref = s(ind,:);
    vec1 = [ref - [x,y],0];
    vec2 = [cos(theta),sin(theta),0];
    tmp = cross(vec2,vec1);  % 用叉乘來判斷參考點在速度方向的右側還是左側
    a = abs(2*V^2/L1*sin(eta));
    if(tmp(3) > 0)
        theta_a = theta + pi/2;
    else
        theta_a = theta - pi/2;
    end
    % 二階積分器求下一時刻位置
    Vx = V*cos(theta); Vy = V*sin(theta);
    ax = a*cos(theta_a); ay = a*sin(theta_a);
    VxNext = Vx + ax*deltaT; VyNext = Vy + ay*deltaT;
    xNext = x + VxNext*deltaT; yNext = y + deltaT*VyNext;
    res = plot([x,xNext],[y,yNext],"Color",'r','LineWidth',2); drawnow;
    legend([trace,start,res],["待跟蹤軌迹","起始點","L1跟蹤軌迹"],'AutoUpdate','off','Location','best');
    x = xNext; y = yNext;
    tmp = acos([VxNext,VyNext]*[1,0]'/norm([VxNext,VyNext]));
    if(VyNext<0)
        theta = -tmp;
    else
        theta = tmp;
    end
end