人工智慧與智慧系統2-> 機器人學2 | 時間與運動

《Robotics, Vision and Control》學習到第三章，我才發現這本書是有配套視訊的，第二章看的好辛苦，很多地方生硬理解了一下，現在打算把視訊再好好看一看，作為補充，也會記錄筆記。

本系列參考資料：

1. 《Robotics, Vision and Control》

2. B站公開課：

   1. 臺灣交通大學機器人學公開課
   2. Peter Corke 配套視訊

3. 老黃的《機器人技術基礎》課程講解及PPT

4. N多有關機器人學的大佬的部落格

   推薦一個部落格：MATLAB RTB常用公式彙整（三），我參考其修正了一些我的筆記。

目錄

• 00 概述
• 01 軌跡
  • 01-1 平滑一維軌跡
  • 01-2 多維軌跡
  • 01-3 多段軌跡
    • 一維
    • 多維
    • Matlab實現
  • 01-4 三維空間姿態插值
    • matlab實現
  • 01-5 笛卡爾運動
• 02 時變座標系
  • 02-1 旋轉座標系
    • 概念
    • matlab實現
    • 進一步
  • 02-2 增量運動
    • 概念推導
    • matlab實現
  • 02-3 慣性導航
• 03 簡單總結 | Review

00 概述

前一部分我們學習了二維三維空間中的物體位姿描述，這一部分我們主要研究物體位姿隨時間變化的描述情況，為後面機器人底盤、導航、定位做鋪墊。

01 軌跡

路徑：從初始位姿->最終位姿的一個圖形。

軌跡：路徑+特定時間屬性；軌跡的重要特徵是平滑，位置和姿態需要隨時間流暢地變化。

01-1 平滑一維軌跡

一維代表著一條直線，我們可以用一個常見的實函式就能表示出位姿隨時間的變化規律。我們以五次多項式函式為例：

其軌跡方程、導數及二階導如下：

\[S(t) = At^5+Bt^4+Ct^3+Dt^2+Et+F\\ S'(t) = 5At^4+4Bt^3+3Ct^2+2Dt+E\\ S''(t)= 20At^3+12Bt^2+6Ct+2D\\ t\in[0,T] \]

給出這樣的方程，我們就確定了位置、速度、加速度的邊界。我們把t=0和t=T帶入上面三個式子，可以得到一個矩陣。

\[\left[\begin{matrix} s_0 \\ s_T \\ s'_0 \\ s'_T \\ s''_0 \\ s''_T \end{matrix}\right] = \left[\begin{matrix} 0 & 0 & 0 & 0 & 0 & 1 \\ T^5 & T^4 & T^3 & T^2 & T & 1 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 5T^4 & 4T^3 & 3T^2 & 2T & 1 & 0 \\ 0 & 0 & 0 & 2 & 0 & 0 \\ 20T^3 & 12T^2 & 6T & 2 & 0 & 0 \end{matrix}\right] \cdot \left[\begin{matrix} A \\ B \\ C \\ D \\ E \\ F \end{matrix}\right] \]

s = tpoly(0, 1, 50);
%生成一個五次多項式軌跡，0~1，分50步
%輸出：
s = 50×1    
    0.0021
    0.0048
    0.0091
    0.0152
    0.0233
    0.0336
    0.0461
    0.0611
    0.0785
    0.0982
    ...
plot(s)
% 繪製軌跡曲線，如下：
[s, sd, sdd] = tpoly(0,1,50);
% 輸出速度和加速度的情況
% 如果想對速度加速度輸出
subplot(3,2,1);plot(s1);grid on;xlabel('時間');ylabel('s');
subplot(3,2,3);plot(sd1);grid on;xlabel('時間');ylabel('sd');
subplot(3,2,5);plot(sdd1);grid on;xlabel('時間');ylabel('sdd');

% 設定0~1,50步,初速度0.5,末速度為0
s1 = tpoly(0,1,50,0.5,0);
plot(s1)
% 如果想輸出速度加速度，同上

1. 從軌跡上看，這種情況會使得運動存在一種往復的現象，即存在一些點超過終點(一維)，比如影像中的峰值為5，超出了我們0~1的範圍。

2. 從速度上看，第一種情況(以0為初速度)，會在25s取得速度最大值(我們可以用plot繪製出來)，我們可以用下面這段程式碼計算平均速度：

   mean(sd)/max(sd)
   % 輸出：
   ans = 0.5231
   % 即平均速度為最大速度的52.31%
   

而我們知道，真正的機器人關節是有額定速度的，我們一般會希望機器人(關節)儘快到達指定的位置，即使其在速度較大的時間佔比儘可能大。

一種解決方法是採用混合曲線軌跡：

s2 = lspb(0, 1, 50)
% 軌跡由一條線段（代表勻速運動）與兩條拋物線組成
plot(s2)

可以預見，這種運動的速度曲線是一種梯形，所以也成為梯形軌跡，普遍應用於工業馬達驅動領域。

梯形軌跡的速度雖然光滑，但是加速度不光滑。

上面的lspb是預設速度，我們可以自己設定引數，給直線段一個速度(當然，速度值不是任意選擇的)：

% 我們先返回當前圖的直線段速度作為參考
max(sd)
% 輸出：
ans = 0.0306
% 再設定新引數0.025，不可行的引數會返回錯誤
s2 = lspb(0, 1, 50, 0.025);

01-2 多維軌跡

從前一節可以知道，我們描述一個物體大多是需要一個多維座標向量；我們描述物體的運動，也需要對從初始位姿向量到末態初始位姿向量的多維平滑運動的描述。

在RTB中將01-1的標量軌跡擴充套件成多維向量還是很簡單的，即使用 mtraj 函式。

x = mtraj(@tpoly, [0 2], [1 -1], 50);
% %分50個時間步從(0,2)運動到(1,-1)，根據50×2的矩陣，生成一個二變數的標量軌跡
% x = mtraj(@lspb, [0 2], [1 -1], 50);
plot(x);
% 其實不標也可,橫軸是時間，縱軸是各個維度x的標量
xlabel('t')
ylabel('Xn')

% 對於三維位姿，如果是三變數，如下，意義類推
x = mtraj(@tpoly, [0 2 1], [1 -1 0], 50);

如果採用lspb函式則為下圖：

對於三維空間中的位姿問題，可以先用如下方法將位姿齊次矩陣T轉換為一個六維向量：

x = [transl(T); tr2rpy(T)'];
% transl實現平移操作 tr2rpy是位姿齊次矩陣T->卡爾丹角(描述旋轉角度)

然後再產生相應的軌跡。但是對於描述姿態的三角度插值存在一定的限制。 這點後面再提。

01-3 多段軌跡

機器人應用中經常需要平滑地沿一條路徑運動，並不停頓地經過一個或多箇中間點。

我們可以對問題進行具體化，即我們需要讓軌跡通過可以確定這條路徑的N個點Xn(位姿向量)，即劃分為N-1個運動區間。

下面這幾段原文翻譯並不好，網上博主大多也只是做了記錄，但是其實不耽誤有工科基礎的同學看懂其中意思，我理解、整理如下：

一維

先放上原文圖(有了電子書嘻嘻)：

順著上面說的意思，舉個例子(如上面電子書的圖3.4)，我們考慮一個一維案例，畢竟從01-2我們可以看出多維只是一維的組合而已，對每個維度就是一個一維運動。

這個案例需要我們從X1到達X4，我們選取的路徑決定點(中間點)為X1、X2、X3、X4，注意這裡的選取並不代表我們一定要完全通過這些點，我們需要綜合所有的需求規劃出合理的方案。附上一維圖片：

最簡來講，我們會依次連線X1-X2-X3-X4，但明顯我們的速度將會變得不連續：因為X2甚至在X4的右側，我們如果勾勒書圖3.4的折線內容，會出現速度的突變。

為了實現速度連續，我們需要捨棄掉一些預定的中間點位置(實際上從計算方法\數值分析的角度很好理解)，採用直線和曲線擬合的方式，如書上曲線直線組合的方式。這樣書上的那個圖就會很好理解：

1. 從X1按要求的加速度（不一定恆定）加速至勻速速度；
2. 注意這個勻速速度是可以根據需要設定的；
3. 勻速運動到適宜時刻，即按照預定直線還需tacc/2時間的時刻，即圖中首段直線結束時刻；
4. 開始減速再反向加速的過程，圖中tacc時間對應過程；
5. 重複上述類似過程。

我們也可以算出第四步的平均加速度

\[a = \frac{v_{k+1}-v_k}{t_{acc}} \]

如果這個維度對應的軸最大加速度已知，就可以計算出最小的tacc。

多維

下面我們進一步學習一下多維的情況：

如上文所說，多維只不過是將一維組合在一起。而將一維組合在一起需要考慮的最重要的問題就是，我們需要讓不同維度上的點同時到達下一目標點Xk。

解決方法就是考慮簡單的短板效應：我們考慮從Xk到Xk+1需要時間最久的那個維度的運動。根據額定速度和該維度距離計算出具體時間，再進一步確定各維度實際所需的速度。

Matlab實現

% mstraj()可以根據中間點矩陣生成一個多段多軸軌跡
via = [4,1; 4,4; 5,2; 2,5];
q = mstraj(via, [2, 1], [], [4, 1], 0.05, 0);
% 引數：via中間點，最大速度向量，每段的時間向量，起點，取樣時間間隔，加速時間，會輸出一個以取樣點分行的位置矩陣
plot(q)

可以看到，上面的程式碼我們沒有設定加速時間，如果設定加速時間 tacc 會使上面的折線變得圓潤一些。

% mstraj()可以根據中間點矩陣生成一個多段多軸軌跡，要與生成五次多項式的mtraj區分開
q = mstraj(via, [2, 1], [], [4, 1], 0.05, );

值得注意的是，這個mstraj()引數很豐富，比如可以傳入起點和終點的速度，tacc可以是一個向量，設定每一次速度改變的時間。mstraj是對向量代表的位姿進行插值，我們的例子是笛卡爾座標系中的向量，也可以適用於三向量表示法的向量。

但是對於姿態旋轉，將會有更優的插值方式：

01-4 三維空間姿態插值

typora中使用上下標：

1. 上標： ^內容
2. 下標：_內容

在機器人學中，我們經常需要對姿態進行插值。比如，我們對於末端執行器從位姿ξ₀改變到ξ₁，我們就需要某個連續的函式ξ(s) = σ(ξ₀, ξ₁, s) (s∈[0, 1])；這個函式的邊界條件易知為σ(ξ₀, ξ₁, 0) = ξ₀，σ(ξ₀, ξ₁, 1) = ξ₁。

這裡必須強調這個函式必須是光滑地經過一系列中間位姿的，如何實現取決於ξ的表示方法(上一部分講的)。

複習插值：

在離散資料的基礎上補插連續函式，使得這條連續曲線通過全部給定的離散型資料點。

如果ξ的表示方式為正交旋轉矩陣，如果使用簡單的線性插值，插值得到的矩陣不正交。

複習矩陣正交：

正交矩陣滿足--列向量2範數為1，&& 列向量之間正交。

三角度表示法可以實現線性插值：

\[\sigma(\Gamma_0,\Gamma_1,s) = (1-s)\Gamma_0+s\Gamma_1\\ \xi\sim\Gamma\in{}S^3 \]

但根據上一部分的內容可以知道，三角度表示法存在奇點。

單位四元數的插值法與三角度類似，見matlab表示法。

四元插值法是通過使用球形線性插值(slerp)來實現的，其中單位四元數在一個四維超球面上沿一個大圓路徑運動，在三維空間就表現為繞固定軸的轉動。

matlab實現

%%三角度表示法
% 定義兩個姿態，分別為初態和末態
R0=rotz(-30/pi*180)*roty(30/pi*180);
R1=rotz(30/pi*180)*roty(30/pi*180);
%
% 得到等價橫滾-俯仰-偏航角
rpy0 = tr2rpy(R0);
rpy1 = tr2rpy(R1);
% 分50個時間步在它們之間生成一條軌跡
rpy = mtraj(@tpoly,rpy0,rpy1,50);
tranimate(rpy2tr(rpy));
view([-48.02 30.83]);% 調整視角使看的更清楚
% 動圖目前不會儲存，下面是效果圖：

%%單位四元數表示：
% 我們使用上面三角度的位姿，先轉換為四元數
q0 = UnitQuaternion(R0);
q1 = UnitQuaternion(R1);
% 對它們進行插值
q = interp(q0, q1,[0:49]'/49);
% 檢視一下插值情況
about(q)
% 動畫：
tranimate(q.T)
% 效果圖如下：

01-5 笛卡爾運動

這一部分我們來研究研究齊次變換矩陣的插值。

這一問題來自於我們有時會有一個需求是：要在SE(3)中生成兩位姿之間的光滑路徑，這個過程會同時涉及位置、姿態的變化。機器人學中這通常被稱為笛卡爾運動。

我們結合matlab實操來理解：

% 建立始末位姿：
T0 = transl(0.4, 0.2, 0)*trotx(180);
T1 = transl(-0.4, -0.2, 0.3)*troty(90)*trotz(-90);
% 這裡這些角度的問題：
% trotx這些函式沒有'deg'說明都應該是弧度制，這裡只是為了讓旋轉程度更大

工具箱函式trinterp提供了沿路徑單位化距離s∈[0, 1]中的位姿插值。比如：中間位姿：

trinterp(T0, T1, 0.5)
% 輸出為：
ans = 4×4    
         0    1.0000         0         0
         0         0   -1.0000         0
   -1.0000         0         0    0.1500
         0         0         0    1.0000

其中平移部分我們採用的是線性插值，旋轉部分採用四元數插值法interp進行球形插值。

上述兩個位姿之間五十個分步的軌跡可以用如下方法進行插值：

Ts = trinterp(T0, T1, [0:49]/49);
%生成每個時間步的齊次變換矩陣
% 引數分別對應起點/終點的位姿，0到1線性變化的路徑長度。

about(Ts)
% 輸出：
Ts [double] : 4x4x50 (6.4 kB)
% 這是每個時間分步對應的齊次變換矩陣(50個4×4矩陣)
% 我們可以檢視第一個點的齊次變換
Ts(:, :, 1)
% 輸出：
ans = 4×4    
    1.0000         0         0    0.4000
         0   -1.0000         0    0.2000
         0         0   -1.0000         0
         0         0         0    1.0000
% 可以通過動畫來直觀展示：
tranimate(Ts)
% 它將展現出座標系從位姿T1平移、旋轉到位姿T2的變化過程。
% 該軌跡的平移部分由以下方式獲得：
 P = transl(Ts)
% 輸出軌跡的笛卡爾位置座標，可以對照一下我們的始末位姿。每一行對應每一時間分步上的位置向量。
P = 50×3    
    0.4000    0.2000         0
    0.3837    0.1918    0.0061
    0.3673    0.1837    0.0122
    0.3510    0.1755    0.0184
    0.3347    0.1673    0.0245
    0.3184    0.1592    0.0306
    0.3020    0.1510    0.0367
    0.2857    0.1429    0.0429
    0.2694    0.1347    0.0490
    0.2531    0.1265    0.0551
    ...

% 我們可以繪製出位置的變化：注意是位置的變化
plot(P)

另外我們還可以來看看橫滾-俯仰-偏擺格式化的姿態變化曲線。

rpy = tr2rpy(Ts);
plot(rpy);

藍：x翻滾；紅色：y俯仰；黃色：z偏擺

可以看到，位置變化光滑且為線性，而姿態變化不光滑也不是線性的。非線性是因為角度表示法對於線性表示的四元數進行了非線性變換。不光滑是因為遇到了奇點。

plot還是直接連線得到的，我們可以用tpoly和lspb或者ctraj來把上面的折線平順處理。這樣做不會改變軌跡，但很明顯速度和加速度會變得合理很多。

Ts1 = trinterp(T0, T1, lspb(0,1,50));
P1 = transl(Ts1)
%%
% 或是：Ts = ctraj(T0,T1,50);始末位姿+分步數量
plot(P1)

rpy1 = tr2rpy(Ts1)
plot(rpy1)

02 時變座標系

上面我們研究了物體隨時間變化的旋轉和平移的軌跡問題，下面我們研究軌跡的速度。平移速度很好說，我們具體討論旋轉速度。

02-1 旋轉座標系

概念

我們考慮一個物體的旋轉的時候，我們一般會用角速度向量ω=(ω_x, ω_y, ω_z)來衡量，從大物上或許知道角速度是一個軸，是一個瞬時轉動軸，向量長度表示繞該軸的旋轉率。

聯想到了上一部分學習的繞定軸旋轉。

力學裡有一個時變旋轉矩陣微分表示式：

R(t)是標準正交旋轉矩陣，左側是角速度

\[\dot{R}(t) = S(\omega)R(t)\\ \dot{R}(t)\in{}SO(2)或 SO(3)\\ \]

S(ω）是一個斜對稱矩陣，三維情況下具體形式如下：

\[S(\omega)= \left[\begin{matrix} 0 & -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{matrix}\right] \]

matlab實現

% 要得到S(ω)矩陣
S = skew([1 2 3])
% 輸出：
S = 3×3    
     0    -3     2
     3     0    -1
    -2     1     0
% vex可以逆解S，得到角速度向量
vex(S)
% 輸出：
ans = 3×1    
     1
     2
     3

進一步

我們可以試著理解一下左側：

\[\dot{R}\approx \frac{R(t+\delta_t)-R(\delta_t)}{\delta_t}\\ \Rightarrow R(t+\delta_t)\approx{}\delta_t\dot{R}+R(t)\\ \Rightarrow R(t+\delta_t)\approx{}\delta_t S(\omega)R(t)+R(t)\approx(\delta_tS(\omega)+I_{3×3})R(t) \]

這表示了正交旋轉矩陣是如何作為一個角速度的函式變化的。

02-2 增量運動

概念推導

順著上面的推導繼續思考，我們考慮一個微小的旋轉R₀->R₁，得到：

\[R_1\approx(\delta_tS(\omega)+I_{3×3})R_0\\ \Downarrow\\ \delta_tS(\omega)=R_1R_0^T-I_{3×3}\\ \Downarrow\\ \delta_\theta = vex(R_1R_0^T-I_{3×3}) \]

δ_θ=δ_tω是一個三維向量，單位是角度，表示一個繞世界座標系xyz三軸的無窮小轉動。

下面說明無窮小的角度變化的乘法是可交換的：

delta1 = rotx(0.001*180/pi)*roty(0.002*180/pi)*rotz(0.003*180/pi)
% 輸出：
delta1 = 3×3    
    1.0000   -0.0030    0.0020
    0.0030    1.0000   -0.0010
   -0.0020    0.0010    1.0000


%%交換一下順序看看：
delta2 = roty(0.002*180/pi)*rotx((0.001*180/pi)*rotz(0.003*180/pi))
% 輸出：
delta2 = 3×3    
    1.0000   -0.0030    0.0020
    0.0030    1.0000   -0.0010
   -0.0020    0.0010    1.0000
% 我們使用上面推導的公式，就能算出矩陣對應的微小旋轉角0.001，0.002，0.003.
vex(delta1 - eye(3,3))'
% 輸出：
ans = 1×3    
    0.0010    0.0020    0.0030

對於兩個差異極小的位姿ξ₀，ξ₁，可以用一個六維向量來表示差異：

\[\delta = \Delta(\xi_0,\xi_1)=(\delta_d,\delta_\theta)--(3.9) \]

由位移增量和旋轉增量組成；這個差異δ很明顯可以用空間速度✖δ_t來計算，這個空間速度將在後面《速度關係》這一部分來學習。現在我們考慮位姿用齊次變換矩陣的形式表示：

\[δ = \Delta(T_0,T_1)= \left[\begin{matrix} t_1-t0\\ vex(R_1R_0^T-I_{3×3}) \end{matrix}\right]--(3.10)\\\\ PS：T_0=(R_0, t_0); T_1=(R_1,t_1) \]

工具箱中可以用tr2delta求解T。

我們考慮一下上上個公式(3.9)的逆運算：

\[\xi = \Delta^{-1}(\delta)\\ T = \left[\begin{matrix} S\delta_\theta & \theta_d\\ 0_{3×1} & 0 \end{matrix}\right] + I_{4×4} \]

逆運算可以用delta2tr求解。

matlab實現

% 我們先來確定始末姿態T0，T1
T0 = transl(1,2,3)*trotx(1*180/pi)*troty(1*180/pi)*trotz(1*180/pi);
T1 = T0 * transl(0.01,0.02,0.03)*trotx(0.001)*troty(0.002)*trotz(0.003)
% 輸出：
T1 = 4×4    
    0.2889   -0.4547    0.8425    1.0191
    0.8372   -0.3069   -0.4527    1.9887
    0.4644    0.8361    0.2920    3.0301
         0         0         0    1.0000
% (3.10)中▲的計算是由tr2delta完成的
d = tr2delta(T0, T1);
d' % 轉置而已
% 輸出：
ans = 1×6    
    0.0100    0.0200    0.0300    0.0010    0.0020    0.0030
%% 我們嘗試一下逆變換，用齊次變換作用於T0，看一看得到的結果與設定末姿態T1的差別
delta2tr(d)*T0
ans = 4×4    
    0.2903   -0.4521    0.8434    1.0100
    0.8376   -0.3061   -0.4524    2.0200
    0.4627    0.8378    0.2898    3.0300
         0         0         0    1.0000
% 看起來十分接近，誤差來源於位移量不是無窮小。

有關tr2delta和delta2tr的解釋：

tr2delta：

d = tr2delta（T0， T1）將齊次變換轉換為差分運動(差分是離散化的微分)，描述從姿態 T0 到 T1 的無窮小變化。將會輸出一個六維向量d =（dx， dy， dz， dRx， dRy， dRz），是平均空間速度乘以時間的近似值。

delta2tr：

將差分運動轉換為齊次變換。

02-3 慣性導航

我感覺這一部分可以單出一部分來講，畢竟《慣性導航》可以單獨出書來講的。這裡就僅僅作為了解。

慣性導航系統是一個“黑匣子”，由一臺計算機和運動感測器組成，輸入加速度、角速度等自身測量值，經過運算得到相對於慣性參考系（宇宙）的速度、方向、位置等位姿。

無需外部訊號輸入，所以它非常適合用於潛艇、航天器和彈道導彈。

1. 與其原理相近的是內耳中起平衡作用的感測器。
2. 整合了各種感測器的系統叫做慣性測量單元IMU
3. 早期慣導和現代捷聯式慣導設計原理稍有不同
4. 基本思想就是基於慣導系統的高工作取樣頻率，得到角速度、加速度等進行積分計算，得到速度、位置、角度變化等等，來確定當前位姿。

03 簡單總結 | Review

效仿第一部分，通過本部分的學習，我們也需要總結出幾個巨集觀的印象，以便於後續的深入學習：

1. 建立機器人運動的軌跡，軌跡的重要特點是光滑，時間和姿態要隨時間連續變化。
2. 旋轉的速度研究，我們研究正交旋轉矩陣關於時間導數，我們建立了與力學的角速度速度的聯絡
3. 作為本部分知識的應用，慣性導航中旋轉插值和角速度積分都用到正交旋轉矩陣法和四元數法。兩種方法的結果等價。但四元數法的公式更簡潔，計算更有效。