0 前言

硬核，這年頭機器人都開始自學「倒車入庫」了這篇文章本質上屬於機器人的鎮定問題。本文仍基於差速輪式機器人模型完成一個軌跡跟蹤應用，使機器人在儘可能短的時間內跟蹤上愛心軌跡。

模擬效果動圖如下所示，看完本文相信你也可以做到！(文末有實際機器人執行效果圖！)

在這裡插入圖片描述

1 什麼是軌跡跟蹤？

軌跡跟蹤以穩定性和快速性為核心，基於時間微分約束(如速度、加速度)，規劃路徑點間的運動狀態，軌跡規劃賦予路徑規劃時間資訊。簡單來說：就是控制機器人用最短的時間跟上期望的軌跡。

和上一期的鎮定問題做個對比，鎮定問題是設計一個控制律在有限時間內使誤差衰減到0，當前位姿穩定在參考位姿上。

這兩次博文討論的問題表面上輕鬆簡單，但背後的應用極為廣泛，常見於智慧車競賽的循線、平衡等賽題，學術界方面也有相當的論文研究這兩個問題，感興趣的同學可以繼續深入瞭解，我把相關的參考論文放在文章最後。

2 開始程式設計實現

2.1 選擇軌跡

R = 5.0;
T = 40.0;
w = 2 * pi / T;

ref = @(t) [R*(2*sin(w*t)-sin(2*w*t)), R*(2*cos(w*t)-cos(2*w*t))];

列印出來看看
在這裡插入圖片描述

2.2 編寫軌跡跟蹤演演算法

function dpdt = wmrTrackOde45Func(t, p, ref)

獲取機器人當前狀態

% robot state
x = p(1);
y = p(2);
theta = p(3);

獲取參考位姿資訊

% reference path
pRef = ref(t);
xRef = pRef(1);
yRef = pRef(2);
vRef = 1;
wRef = 0;

獲取目標向量

% predict reference path
targetVec = [xRef - x;
             yRef - y]; 
if targetVec(2) > 0
    thetaRef = acos(targetVec(1) / sqrt(targetVec(1)^2 + targetVec(2)^2));
else
    thetaRef = - acos(targetVec(1) / sqrt(targetVec(1)^2 + targetVec(2)^2));
end

控制器引數設定

% controller parameters
Kx = 15;
Ky = 25;
Kt = 10;

控制

% controller
ex = cos(theta) * (xRef - x) + sin(theta) * (yRef - y);
ey = -sin(theta) * (xRef - x) + cos(theta) * (yRef - y);
u  = [ vRef * cos(thetaRef - theta) + Kx * ex;
       wRef + vRef*(Ky * ey + Kt * sin(thetaRef - theta))];

v = u(1);
w = u(2);

dpdt = [v*cos(theta); v*sin(theta); w];
end

然後把這個函數封裝成ode45的被調函數，讓Matlab幫我們迭代計算即可。

2.3 測試跟蹤效能

其中綠色是機器人初始位置，藍色是機器人實際軌跡。
在這裡插入圖片描述
再來看看誤差曲線，大概在5s的時候各引數全部跟蹤上預期曲線，這個效果可以通過調節控制器引數來觀察變化，加深對控制作用的理解，上文程式碼中的控制器是我個人整定的，大家可以按需調整。

對其他軌跡的效果怎麼樣呢？看看下面圓軌跡的跟蹤效果，即使初始位置很偏也沒有任何問題。
在這裡插入圖片描述

2.4 製作動圖

figure(1)
trackProcess = VideoWriter('video/trackProcess.avi');
open(trackProcess);
movie = moviein(t);

% 畫參考軌跡
plot(refPos(:,1),refPos(:,2), "--b", 'LineWidth', 1);
grid on
hold on

% 畫初始位置
handler = plotPose(initPos, 0.8, 0.3, 'r', size);

for i=1:length(t)
    delete(handler);
    handler = plotPose(actualPos(i,:));
    plot(actualPos(i,1), actualPos(i,2), 'Marker', '.', 'color', [0,0.5,0]);
    movie(:, i) = getframe;
	writeVideo(trackProcess, movie(:, i));
end
hold off
close(trackProcess);

3 真車實戰

由於限制5M，只放出部分動圖，後續補充完整視訊。
在這裡插入圖片描述

4 參考文獻

希望做相關研究的同學可以進一步閱讀下面的文獻

[1]楊士超. 非完整輪式移動機器人的反饋鎮定和軌跡跟蹤控制研究[D]. 長安大學.
[2]Kanayama Y , Kimura Y , Miyazaki F , et al. A stable tracking control method for an autonomous mobile robot[C]// IEEE International Conference on Robotics & Automation. IEEE, 1991.

硬核，機器人只需5秒就跟蹤上了愛心曲線

目錄