耀禮士多德

 重點：現(xiàn)實中更加在意的是手臂末端與物體（G）之間的相對關系，例如，要做一個噴漆的動作，就可以規(guī)劃一條末端的路線。同時也要控制過程速度，時間。

理想軌跡：Smooth path，就是位置，速度的曲線，都是連續(xù)“絲滑”的。

Initial：手臂末端最初的位置，姿態(tài)。

via Point：中間必須經(jīng)過的位置，姿態(tài)。

final：最后要停留的位置，姿態(tài)。

I.K：反算出機器臂各個關節(jié)，再各個點下的角度。（上圖，假設機械臂沒有伸縮）

（看看有沒有角度，是機器做不了的，也看看兩點之間的角度斜率，有沒有很大，很大有可能手臂的硬件加速度也很大）

Trajectory planning：相當于擬合一條線，得到連續(xù)時間下各個關節(jié)的角度。

F.K：正算，檢查一下和最開始規(guī)劃的圖，是不是一樣的。

---

 另一種規(guī)劃方式

 先將位置連起來。（應該是按照曲線，加密一下via point，多I.K幾組姿態(tài)）

---

 使用兩個via points來擬合3次曲線

θ(t) =  a₁ * t  + a₂ * t²  +  a₃ * t³ + a₀  

因為只有2個點，但是要有4個未知數(shù)，因此加入2個速度，才剛好夠4個方程

將θ(t) =  a₁ * t  + a₂ * t²  +  a₃ * t³ + a₀  變?yōu)椋?/p>

θ(Δt) =  a₁ * Δt + a₂ * Δt²  +  a₃ * Δt³ + a

 使t歸一化，只看[t_i, t_i+1] 那一段，有：

 

---

 如何設置速度條件 θ_i'

 8 個未知數(shù)

當t^~ = 0 時，可以得到  a₁₀ = θ₀

注意到，兩段使用不同的兩個模型的，因此，單獨看[t1,tf]段，

 

---

 

目標：獲得位置，速度，加速度的，平滑的表達式，用于仿真。

（藍色軌跡，改為綠色軌跡）

 假設：

開始速度：θ'₀ = 0

最終速度：θ'_final = 0

那么，要做的事情，就是找出：

θ_b , θ_f-b ,  Δt = t_b - t₀

θ_b , θ_f-b  就是中間恒定速度的起始、結束的位置。

綠色線中間，速度是恒定的，加速度為0

 [t₀, t_b]段，軌跡為二次曲線。

 [t_b, t_f - t_b]段，軌跡為直線。

 [ t_f - t_b，t_f ]段，軌跡也為二次曲線

 問：t_b如何確定？ （總不能按照直覺，隨便點一個θ_b，t_b出來）

思考：

1. θ_h, t_h，直接是已知的，直接一半。

2. 為保證平滑， θ_b 必須大于θ₀ ，θ_f-b 必須小于θ _f 。

3. 是不是應該根據(jù)硬件的參數(shù)，比如說最大扭力的50%之類的來確定？

4. 最大扭力決定的是加速度，是不是應該將時間t ， 位置，加速度的關系列出來，再根據(jù)設定的速度或者加速度，得到t_b的值？？

 

 由2式可得：

 t_b 處的速度 =  初速 （0） +  加速度 * t_b 

 根據(jù)： h處的速度 = b處的速度

θ_h = (θ_f + θ₀) / 2

θ_b =   θ₀ + （θ₀)' * t_b + 1 / 2 *（θ₀)'' * ( t_b)²  ( θ₀處的速度為0（θ₀)’ = 0)

t_h = t_f / 2

 最終得到：t_b，是加速度θ''的函數(shù)

 結論：

1. t_b，是加速度θ''的函數(shù)

2. 加速度θ''， 有最小值

（如果機器無法達到最小加速度，那么初速度（θ₀)' 可能不能為0，才能使得曲線平滑）

---

 

加速度θ'' 對軌跡的幾何影響

如果使用最小的

 代入，得到：

這表明：如果機器出最小的力氣，那么中間的軌跡，就是2條開口相反的二次曲線。

加速θ'' 達不到θ''_min，造成軌跡的影響：

 結論：加速θ''不夠時，不能在某個時間點，到達某個位置，除非改變初速度

 

 補充：

如果出力最小，那么中間速度，就是勻速2倍了。

也就是，可以選擇大于θ''_min 的加速度，控制速度。

---

 

首先可以得到直線部分的速度：

 已知速度與加速度有如下關系：

(θ_kl)' = (θ_jk)' + t_k * (θ_k)'' 

速度對時間微分，就是加速度。

也就是：

 可以：

直接根據(jù)硬件的承受能力，得到(θ_k)'' , 從而得到t_k, 注意正負號

或者根據(jù)t_k，得到(θ_k)'' ， 再判斷有無超出硬件承受能力。

 目標：

要得到一段段方程，形如：

θ = f₁(t)  t∈ [t_j, t_j + Δt₁ ]

θ = f₂(t)  t∈ [  t_j + Δt₁ ,  t_k - Δt₁]

θ = f₃(t)  t∈ [  t_k - Δt₁ ,  t_k + Δt₁]

θ = f₄(t)  t∈ [  t_k + Δt₁ ,  t_l - Δt₂]

 .....

---

 

起始位置

對于整個過程，在t₀ 和 t_f 要特別處理，因為那里的速度為0

在實際的操作中，位置相同：θ₀ = θ₁ ，但速度不相同， 因為機器需要一個啟動的過程，而t = 0時的速度為0， t = t_θ1時的速度就不是為0了

1. 直線段的速度  =  θ₁  處的加速度（θ‘’₁ ） *  t₁時間段 ， 因為t = 0時的初始速度為0

（相當于在t₁時間段，加速到平均速度，然后保持勻速）

2. 直線段的速度  = t₁₂   時間內(nèi)的平均速度

根據(jù)機器能提供的加速度，去反推時間t₁

 或者規(guī)劃時間， 去求加速度，但是要考慮機子的承受能力：

 最終算出的（θ‘’₁ ）≤ 硬件最大能提供的加速度

結束位置

 位置相同：θ_n = θ_n+1 ，但速度不相同t = t_{n  時，}還有一個微小的速度，直到 t = t _{n +1} 時完全靜止

 那么，同理可得：

值得注意的是，這只是機械要提供運動所分配的加速度，具體但離電機提供的加速度還有一段距離。

電機提供的力  = 加速度需要的力  + 克服重力提供的力 （和姿態(tài)有關）+  克服慣性有關的力（和運動速度有關）

所以，加速度θ‘’ 的最大值，  最起碼要從機器能承受的最大值，扣掉克服重力的值才可以。

---

 

遺留的問題：

 如果via point是必須要通過，沒得商量的，那就在via point前后插入一些額外的 via Point，做一段直線段

插入后，中間的必須要通過的via point，是不是可以省掉了？？不納入編程中？

---

 

編程部分

前面已經(jīng)計算好：

1. 加速區(qū)；（t_j）

2. 減速區(qū)；(t_k)

3. 加速度；（θ‘’_k ）

接下來就是定義好位置方程，就可以在matlab上畫線了。

直線段：

 

---

 

初始姿態(tài)對軌跡的影響

可以看出，如果A點是初始位置，那么手臂的姿態(tài)有兩個解。

如果以第1張圖的解，那么第1個關節(jié)，在3、4之間的旋轉角度，就發(fā)生劇烈的變化，不符合平滑的要求

---

 

例子：

已知手臂DH表。

設定via Point

矩陣最后一列，就是via Point的位置。

矩陣的旋轉部分，就是C坐標系的軸，在以World 坐標系為參考下的向量。

0到1處，只是讓杯子提高一點

1到2處，就是讓到差不多的位置，并且旋轉杯子令杯耳對準。

注意到：2處，C坐標系可以想象先與word對齊，然后繞Y軸旋轉，使得杯子橫放。

 ^W₀T 、 ⁶_CT是固定的， C與6是固連的（定義上C與6必須是固連）

 ⁶₀T = ^W₀T^-1 W_CT  ⁶_CT^-1

4、5、6 的原點，在0系下的位置：

 ⁰P_4org = ⁰₁T ¹₂T ²₃T³P_4org 

而³P_4org  ， 又是 ³₄T 的第4列。

令：

[f₁(θ₃), f₂(θ₃) ,f₃(θ₃), 1 ]^T = ²₃T ³P_4org 得到第1層

⁰P_4org  = ⁰₁T ¹₂T *  [f₁(θ₃), f₂(θ₃) ,f₃(θ₃), 1 ]^T

[g₁(θ₂,θ₃), g₂(θ₂,θ₃) ,g₃(θ₂,θ₃), 1 ]^T = ¹₂T *  [f₁(θ₃), f₂(θ₃) ,f₃(θ₃), 1 ]^T 得到第2層

然後，發(fā)現(xiàn)：

⁰P_4org = [ x , y , z , 1] 中，與g有如下關系：

 r =  x²  + y² +  z² = g₁² + g₂² + g₃²

并且：z  =  g₃

令：

k₁ = f₁

k₂ = -f₂

k₃ = f₁² + f₂² + f₃² + a₁² + d₂² + 2d₂f₃

k4 = f₃cos(α₁) + d₂cos(α₁)

然后，代入 r =  x²  + y² +  z² = g₁² + g₂² + g₃²

得到：

r = (k₁c2 + k₂s2) 2a₁ + k₃

z = g3 = (k₁s2 - k₂c2) sin(α₁) + k₄

里面k都只是θ₃的函數(shù)，將：

u = tan(θ₃ / 2)

cosθ₃ = (1 - u² ) / (1 + u²)

sinθ₃ =  2u / (1 + u²) 

代入,  解u，再解θ₃ 

然后解θ₁ θ₂   

然后解³₆R

 最后，根據(jù)Z-Y-Z構成的歐拉角矩陣，解出θ₄ θ₅  θ₆ 

 因為初始時，幾個坐標系原點重合在一起。

在此處建立一個統(tǒng)一的坐標系，根據(jù)歐拉角，先繞Z軸（Z₄）旋轉，再繞旋轉后的Y軸（Z₅）旋轉，再繞（Z₆）旋轉時，Z₄其實和Z₆是重合的，也就是依然繞Z軸旋轉，因此是Z-Y-Z順序的歐拉角。

注意：

因為DH定義的關系，使用ZYZ求出的角度，要經(jīng)過加工才能變成θ₄，θ₅，θ₆

從正算的角度來看：

1. 繞Z₄旋轉θ₄

2. 如果要使用歐拉角，那么相當于還要繞Z₄ 多轉180

那么，用ZYZ求得的歐拉角α，與θ₄ 有此關系（同理θ₆）：

 而α，β，γ是有多解的，最終選θ變化最小的解

 

---

 

軌跡規(guī)劃，其實就是對⁰P_6org 進行的規(guī)劃

因此，先得到⁶₀T

 

方法1：先規(guī)劃XYZ，再繪制θ₁θ₂θ₃θ₄θ₅θ₆ 圖

首先，需要將其轉換成坐標，與6系相對于0系的姿態(tài)，用來做軌跡規(guī)劃。

根據(jù)坐標規(guī)劃， 繪制圖θ₁θ₂θ₃θ₄θ₅θ₆ ， 

 方法2：先規(guī)劃θ₁θ₂θ₃θ₄θ₅θ₆ ， 再繪制XYZ圖