🔥 作者：FrigidWinter

🔥 簡介：主攻機器人與人工智能領域的理論研究和工程應用，業余豐富各種技術堆疊，主要涉足：【機器人(ROS)】【機器學習】【深度學習】【計算機視覺】

🔥 專欄：

• 《機器人原理與技術》
• 《計算機視覺教程》
• 《機器學習》
• 《嵌入式系統》
• …

0 前言

本文基于差速輪式機器人模型做一個運動學應用，即控制機器人兩輪的速度差改變其運動軌跡，使機器人完成一個倒車入庫的動作，

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

1 什么是差速輪式機器人？

差速輪式機器人是輪式機器人的一種，如下圖所示，其特點是：兩輪只有繞軸的旋轉運動，而沒有其他速度分量，因此這種機器人不能橫向運動，想象家里的掃地機，路上開的汽車，他們都是差速結構，因此都不能“橫著開”！什么樣的機器人可以橫著開？這個需要裝配麥克納姆輪，以后有機會再說，

2 差速輪式機器人的運動學方程

差速輪式機器人的運動學方程如下：
p ˙ = [ x ˙ y ˙ θ ˙ ] = [ cos ? θ 0 sin ? θ 0 0 1 ] [ v ω ] = S ( q ) u \boldsymbol{\dot{p}}=\left[ \begin{array}{c} \dot{x}\\ \dot{y}\\ \dot{\theta}\\\end{array} \right] =\left[ \begin{matrix} \cos \theta& 0\\ \sin \theta& 0\\ 0& 1\\\end{matrix} \right] \left[ \begin{array}{c} v\\ \omega\\\end{array} \right] =S\left( \boldsymbol{q} \right) \boldsymbol{u} p˙?=???x˙y˙?θ˙????=???cosθsinθ0?001????[vω?]=S(q)u

本文不推導晦澀的公式，這里寫出來只是為了編程，

3 開始編程實作

3.1 機器人軌跡控制

首先，我們要確定兩個位置，一個是車位位置ref，一個是車的當前位置p

% 車位
xRef = ref(1);
yRef = ref(2);
thetaRef = ref(3);

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

接著，我們要計算二者的誤差

% 誤差
xErr = cos(theta) * (xRef - x) + sin(theta) * (yRef - y);
yErr = -sin(theta) * (xRef - x) + cos(theta) * (yRef - y);
thetaErr = thetaRef - theta;

然后我們期望把誤差降低

% 引數
Kpx = 3
Kpt = 3

% 輪子的線速度和角速度
v = Kpx * sqrt(xErr^2 + yErr^2);
w = Kpt * thetaErr;

接觸過控制理論的同學一定能看出這就是個P反饋控制器，接著把這個增量反饋出去

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

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

3.2 畫車位和車

車位很簡單，就是三條直線

annotation('line', [0.53,0.53], [0.35,0.5], 'Color','k');
annotation('line', [0.63,0.63], [0.35,0.5], 'Color','k');
annotation('line', [0.53,0.63], [0.35,0.35], 'Color','k');

車可以按喜好畫成各種樣子，本文用箭頭表示車的方向，

arrow = quiver(x, y, endPt(1) - x, endPt(2) - y, ...
       'MaxHeadSize',5.5,'AutoScaleFactor',1,'AutoScale','off', 'LineWidth', 1.5, 'color', color, ...
       'Marker', 'o', 'MarkerSize', 4, 'MarkerFaceColor',color);

效果如下

加點難度，把車位斜過來，也表現的很好！

3.3 制作動圖

接著看看如何制作Matlab演示影片，下面是完整代碼

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

% 畫車位
annotation('line', [0.5,0.5], [0.2,0.3], 'Color','k');
annotation('line', [0.63,0.63], [0.2,0.3], 'Color','k');
annotation('line', [0.5,0.63], [0.2,0.2], 'Color','k');

% 畫目標位置
plotPose(refPos);
grid on
hold on

% 畫初始位置
handler = plotPose(initPos);
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(stableProcess, movie(:, i));
end
hold off
close(stableProcess);

4 真車實戰

因為限制動圖大小為5M內，下面的實體抽調了大部分幀，