時間與運動
上一章主要介紹了在二維和三維空間中如何描述位姿。這一章則將上章拓展到對象，其位姿是時間的函數。
分兩個部分，第一個部分主要討論如何產生一系列短暫的位姿序列，一條足夠光滑的從起始位姿到終止位姿的軌跡；
第二部分主要討論位姿改變速率的概念，即其導數，以及如何將從機械學角度比如速度和角速度等方面將這些概念
聯繫起來。
軌跡
注意路徑（path）和軌跡（trajectory）的區別，路徑是指從A到B的空間概念，而軌跡是有特定時間點的一條路徑。
軌跡的一個重要特徵就是光滑（包括位置和姿態都關於時間是光滑的）。
接下來討論如何在一維空間中產生光滑的軌跡，然後擴展到多維空間，然後擴展到分線段的軌跡。
一維光滑軌跡
我們從一個時間函數開始討論。這個函數的特點是其初始值和終止值是給定的，並且它是光滑的。光滑在這裏意味着其一階微分是連續的。
多項式函數就是這樣一個函數。我們經常使用五次多項式，其加速度是光滑的三次曲線，加加速度是光滑的拋物線。Matlab機器人工具箱提供了函數tpoly來產生 一個五次多項式軌跡：
s=tpoly(0,1,50);
產生一個0到1之間的五次軌跡，時間間隔是50次。
我們也可以得到其速度和加速度曲線：
[s,sd,sdd]=tpoly(0,1,50);
其中sd，sdd就分別是速度和加速度。可以用plot(s),plot(sd)等將這些曲線繪製出來
也可以在tpoly函數中給出初始值和終止值：
s=tpoly(0,1,50,0.5,0);其中0.5就是速度初始值，0是速度終止值。
但是這樣產生的軌跡其速度分配不合理，平均速度只有峯值的百分之52，這就說明，對於大多數時候，速度都遠比峯值少。
實際的機器人的速度都會被很好的定義，以讓其儘可能多地在最大速度下運行，我們希望速度曲線在頂部的時候儘量平坦，而不是出現一些山峯。一個廣泛使用的是組合軌跡：其速度是由一個常數和多項式組成的加速和減速階段。我們可以在matlab中用lspb()函數來產生這種軌跡：
s=lspb(0,1,50);
它的參數的意義和tpoly一樣，其軌跡是一段線性直線段和拋物線組合起來的，和tpoly一樣，也可以得到其速度和加速度曲線：
[s,sd,sdd]=lspb(0,1,50);
lspb函數有第四個參數，是給定速度峯值的：
s=lspb(0,1,50,0.025)；
s=lspb(0,1,50,0.035);最後繪製的軌跡及其速度，加速度曲線如下圖所示：
多維情況
許多機器人都是有幾個軸的運動或多個自由度，我們通常都用一個向量來表示這些自由度。輪式機器人用它的位置（x,y）或位姿（x,y,theta）來表徵。機器人的工具有位置(x,y,z)，姿態（r,p,y）或者位姿（x,y,z,r,p,y）。我們因此需要一個從起始位姿到終止位姿的多維空間的運動。
在機器人工具箱中，使用函數mtraj()來實現。例如分50步從(0,2)到（1,-1）：
多維分割軌跡
在機器人應用中通常需要經過幾箇中間點沿着一條光滑軌跡運動。但有時候爲了得到連續的速度，我們通常都會一些中間點，不會精確地到達這些中間點，而是會和之前一樣，由一些多項式組合而成。
機器人工具箱提供了函數mstraj()來產生一條基於中間點的多段多軸的軌跡。例如2軸經過4箇中間點的軌跡就
可以這樣產生：
其中via就是4箇中間點的座標，mstraj的第一個參數就是要通過的中間點，第二個參數就是每個軸的最大的速度
所組成的向量，第三個參數是每個段的間隔所組成的向量，第四個參數是初始的軸座標，第六個參數是取樣週期，
最後一個參數是加速時間。mstraj函數會返回一個矩陣，行表示時間間隔，列表示座標值。
三維姿態的插補
在機器人學中，我們通常需要插補姿態，例如，要求機器人末端從一個姿態平滑地運動到另一個姿態。
一般使用歐拉角或RPY來做姿態插補，而不用正交矩陣，例如，我們定義兩個姿態：
可以找到對應的RPY：
然後可以在這兩個姿態之間以50個步長創建一條軌跡：
可以以動畫的形式來演示：
也可以用單位四元素法來做插補，例如上面的例子中，我們先找到其單位四元素：
然後進行插補：
用tranimate（q）就可以動態顯示了。
笛卡爾運動
上面只討論了從一個姿態變換到另一個姿態的運動，而沒有平移運動，然後在實際的機器人系統中，一般都是兩
種運動都有的，所以當設計到從一個位姿運動到另一個位姿的時候，我們叫做笛卡爾運動。
我們用齊次變換來表示起始和終止位姿：
機器人工具箱中提供函數trinterp()提供這種插補，例如：
在這裏平移部分用的是線性插補，姿態部分用的方式還是四元素插補所用的方法：interp。
所以一條在兩個位姿之間的步長50的軌跡就可以產生了：
其中Ts是一個三維矩陣，前二維表示齊次變換矩陣，後一維表示時間步長：
我們可以看到路徑上第一個位姿：
我們也可以tranimate(Ts)看到它的變換過程：
我們可以得到這個軌跡的平移部分：
>>p=transl(Ts)；
可以得到一個50x3的矩陣，每一行就是軌跡點的位置，我們可以繪製出來：
>>plot(P)
我們也可以得到其姿態矩陣：
>>rpy=tr2rpy(Ts);
>>plot(rpy)
從上圖中可以看到平移運動在開始和結束的地方有速度和加速度的不連續。我們可以用tpoly和lspb函數來使其
連續。讓上述兩個函數作爲trinterp函數的第三個參數。
>>Ts=trinterp(T0,T1,lspb(0,1,50));
從圖中可以看出其速度和加速度都是連續的，但軌跡是沒有改變的。機器人工具箱提供了另外一個比較方便的函數：
>>Ts=ctraj(T0,T1,50);
隨時間變化的座標系
前面部分討論的是如何產生有平移和旋轉速度分量的座標系運動。平移部分比較簡單，就是位置的變換速率，而姿
態的速度就有點複雜。
我們已經注意到旋轉的不可互換性，但是對於無窮小的角度改變姿態的乘法運算卻是可以互換的。我們可以用數值
來驗證一下：
從上面可以看到當旋轉的角度很小的時候，是可以相互交換順序的。我們也可以通過函數vex得到角度的變化：
那麼對於兩個相對變化很小的兩個姿態，我們可以用工具箱函數tr2delta來得到其之間的變化差值：
上面的d就是兩個姿態T0和T1之間的姿態差值，我們也可以用工具箱函數delta2tr來用這個差值得到T0經過這個差值變換之後的結果：
從上面可以看出這樣得到的結果和T1其實是很接近的，之所以會有差距是因爲這個差值並不是足夠小，所以會有一
些差距。