UR機器人之二：直線插補詳解

1.引言

看過UR機器人腳本手冊的都應該知道有這樣一個直線插補函數：

interpolate_pose(p_from, p_to, alpha);

參數：p_from表示初始pose，p_to表示目標pose，alpha通常爲0-1之間的浮點數，如果alpha=0，則函數插值返回的pose是p_from；如果alpha=1，則函數插值返回的pose是p_to；如果alpha<0，則函數插值返回的pose是這條直線在p_from之前的部分；如果alpha>1，則函數插值返回的pose是這條直線在p_to之後的部分；
返回值：返回一個插值之後的pose；
這個函數在實際應用的過程中表現非常好，很少出現奇異點和不可達的點的問題。那麼這個函數具體是如何實現的呢？
也許有人要說了，直線插補太簡單了，不屑一顧。但是我看過許多國內機器人教材，很少能把機器人軌跡是如何生成的這一過程講清楚，或者連一個簡單的點到點直線插補全過程都寫得含糊不清。
以前我也覺得這樣簡單的算法沒什麼好研究的，隨便找本書或論文看看就知道了，但當我真正要在機器人上實現這一算法時，遇到了各種各樣的問題，於是我就懷疑是否真的瞭解了機器人生成軌跡的方法，直到我看完這本書爲止。

2.《Modern Robotics》

這本書的全稱是《Modern Robotics - Mechanics, Planning, and Control》2016年出的機器人學方面的書。
這本書的第九章《Trajectory Generation》是我看過的十幾本機器人學書籍裏面寫的最透徹的。
機器人軌跡的定義：機器人位置隨時間變化的描述稱爲軌跡；
首先要明白兩個概念：path和time scaling。
path表示一組路徑的集合，也可以理解爲一組機器人位置的集合；
time scaling表示一組時間序列，即什麼時間機器人要到什麼位置。
本章考慮了三種軌跡規劃問題：點到點的直線軌跡；通過一系列時間和點的集合的問題；最短時間軌跡問題。
而本文只考慮點到點的直線軌跡問題。這個問題分爲關節空間和工作空間兩種，這兩種在下文中都有詳解。

3.Point-to-Point Trajectories

首先要區分路徑（path）和時間尺度（time scaling）這兩個概念。
路徑用θ(s)表示，其中s就是時間尺度，有時s就是表示時間t，但將s與時間參數t分離是一種更有效的方式，本書就是這麼做的。
時間尺度s(t)∈[0, 1]，t∈[0, T]，如果s=0，表示the start of the path，如果s=1時表示the end of the path。
將路徑（path）和時間尺度（time scaling）合起來就是軌跡（trajectory）表示成θ(s(t))。軌跡的速度和加速度就可以表示爲：

所以要想軌跡有較好的動態性能（即平滑的速度和加速度曲線），那麼要保證θ(s)和s(t)都可以二次微分。

根據上面的分析，將軌跡分爲θ(s)和s(t)，這樣一來，路徑就與時間進行了分離，分離的好處在於：如果想讓軌跡是一條直線或圓弧，那麼我們只需要考慮θ(s)函數的設計，而不用考慮時間問題；同理，如果我們想要得到梯形速度曲線或S形加速度曲線，那麼也只用考慮s(t)函數，而不需要知道路徑到底是直線還是圓弧；

所以，接下來的直線軌跡算法就分別以路徑（path）和時間尺度（time scaling）進行。

4.Straight-Line Paths

如果是關節空間的直線插補，那麼path就是：

微分：

關節空間的直線插補，通常在工作空間不是直線運動。

如果是工作空間的直線插補，那麼將上式的θ改成X表示工作空間的位置和姿態。
如果X是使用最小座標集來表示工作空間的位置和姿態，這裏的最小座標集的意思是用最少的參數來表示位姿，如UR中使用的p[x, y, z, ax, ay, az]，那麼工作空間的直線插補與上式一樣：

在上式的直線插補中，需要注意兩個問題：

1. 如果路徑會經過奇異點附近，那麼幾乎整個時間尺度上的所有路徑的關節角速度都會變得非常大；
2. 因爲機器人的工作空間可能不是凸的，所以有可能會出現Xstart和Xend都在工作空間內，但是中間點卻不在工作空間內的情況；
   
   上圖表示的是，在關節空間每個關節都進行直線插補，在工作空間的軌跡不是直線的情況；
   下圖表示的是，直線插補的起點和終點都在工作空間內，而插補出來的直線中間點不在工作空間內的情況；

上面的分析，是當X使用最小座標集來表示工作空間的位置和姿態的情況。
如果X∈SE(3)的方式來表示機器人在工作空間的位姿，那麼直線插補也要滿足SE(3)的表示方法。這裏的SE(3)的定義如下：
**定義1：**特殊歐幾里得羣（Special Euclidean Group）SE(3)，或者可以稱爲齊次變換矩陣，表示成：

這裏R∈SO(3)，p是列向量。（SO(3)見定義2）

通過螺旋運動的理論得到直線插補公式（具體怎麼得到的這個公式請看《Modern Robotics - Mechanics, Planning, and Control》這本書）：

利用這個公式插補出來的直線，並不是一條直線，而是一條螺旋運動的軌跡。
通過將位置和姿態分離可以得到另外一個直線插補公式：

在這個公式裏，R表示旋轉變換矩陣，表示機器人在工作空間的姿態，p表示位置。通過這樣位置和姿態分離之後，可以看出位置保持一條直線軌跡，而姿態以螺旋運動的方式變化。
以上兩種公式，插補得到的直線軌跡區別如下圖：

如圖所示，上面一條軌跡沒有分離位置和姿態（平移和旋轉），實際得到的軌跡不是一條直線，而是一種螺旋運動曲線。而分離之後的軌跡在位置上是一條直線，姿態上以螺旋方式運動。

看了上面的公式，可能有人會覺得奇怪，怎麼對矩陣進行對數、指數運算？
討論這個問題之前，先插入兩個定義：
**定義2：**特殊正交羣（Special Orthogonal Group）SO(3)，也就是通常說的旋轉變換矩陣，是一個33的實數矩陣R，滿足：

定義3：對於給定的向量：

定義：

這裏[x]是相對於x的33斜對稱矩陣，對於所有這樣的斜對稱矩陣集合成爲so(3)。

關於指數、對數的運算，這裏只給出一個結論，至於爲什麼是這樣的，就需要自己去看書了。
1.首先看矩陣的對數運算，如果R∈SO(3)，那麼對R求對數得到：

其中

也就是說對R求對數就是將R轉換成ω和θ的表示方式，如何得到ω和θ？方法如下：


2.再來看矩陣的指數運算：

指數運算和對數運算是相反的過程，這裏就表示將ω和θ轉換成R，轉換方法如下：

這裏cθ和sθ分別表示cosθ和sinθ。

到這裏，Straight-Line Paths的部分就結束了，接下來是Time Scaling。

5.Time Scaling a Straight-Line Path

這裏只寫三次多項式和梯形速度曲線兩種，其他的都大同小異。
由上述軌跡的速度和加速度公式如下：

對於直線而言dθ/ds是常數，d²θ/ds²=0，所以直線軌跡的速度和加速度主要取決於ds/st和d²s/dt²。

1.三次多項式
對於時間尺度函數s(t)，三次多項式的形式爲：

滿足下列兩個約束條件：


那麼函數s(t)及其速度、加速度曲線如下：

注意這裏的s(t)∈[0,1]，將其代入上面的Path函數中，就能得到速度爲二次曲線的直線插補軌跡；

2.梯形速度曲線
符合梯形速度曲線的公式如下：

s(t)函數及其速度曲線如下圖所示：

將s(t)代入到Path函數中，就可以得到速度滿足梯形曲線要求的直線插補軌跡；

上面寫了這麼多，其實算法很簡單，只是想說明一個思想，就是：軌跡(Trajectories)=路徑(Path)+時間(Time Scaling)，將時間和路徑分離開是這個思想的核心。明白了這個思想，看到其他更復雜的軌跡問題時，就會覺得簡單明瞭。包括在代碼實現的過程中，用這個思想封裝API不論是自己還是讓其他人調用起來也更加方便。