人工智能與智能系統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. 作為本部分知識的應用，慣性導航中旋轉插值和角速度積分都用到正交旋轉矩陣法和四元數法，兩種方法的結果等價，但四元數法的公式更簡潔，計算更有效，

轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/423380.html

標籤：其他

上一篇：裴蜀定理

下一篇：SpringBoot整合Elasticsearch之索引，映射，檔案，搜索的基本操作案例分析