本文的內容是對另一篇文章（連結）的補充，對Trajectory_example.cpp涉及到的原理作一些簡單的講解，主要內容是：
  （1）機器人路徑規劃圓弧過渡的原理；
  （2）機器人路徑規劃梯形波的原理；
  （3）機器人末端姿態插值的方法（角-軸）；
  （4）KDL使用了pathlength的概念並簡述插值的方法。

1 圓弧過渡

  機械臂末端從起點A到終點F，中間需要經過若干個中間點（B-E）。首先將所有的點，按順序用線段連線起來，若兩段相鄰的線段出現較大的轉折，則對兩段線段作圓弧過渡。

圖1 初步用線段連線



圖2 圓弧過渡

  程式見Path_RoundedComposite::Add(const Frame& F_base_point)

2梯形波的原理

  以一維空間裡的位移規劃為例，若要使質點從起點到終點滿足梯形的速度規律，需要知道的引數為（1）兩點之間的位移的距離 ；（2）最大的速度的值 ；（3）加速度的值 ；由於引數選擇的差異，可能會出現以下兩種波形如圖3，一種是完整的梯形波，一種是不完整的梯形波：

圖3 兩種梯形波

$$L\left( t \right)=\begin{matrix} \left\{ \begin{matrix} {{a}_{1}}+{{a}_{2}}t+{{a}_{3}}{{t}^{2}} \\ {{b}_{1}}+{{b}_{2}}t+{{b}_{3}}{{t}^{2}} \\ {{c}_{1}}+{{c}_{2}}t+{{c}_{3}}{{t}^{2}} \\ \end{matrix} \right. & \begin{matrix} t<{{t}_{1}} \\ {{t}_{1}}<t<{{t}_{2}} \\ {{t}_{2}}<t<duration \\ \end{matrix} \\ \end{matrix}$$ 式（1）
其中：
$$\begin{align} & {{a}_{3}}=\frac{a}{2};{{a}_{2}}=0;{{a}_{1}}={{s}_{0}}; \\ & {{b}_{3}}=0;{{b}_{2}}={{a}_{2}}+2{{a}_{3}}{{t}_{1}}-2{{b}_{3}}{{t}_{1}};{{b}_{1}}={{a}_{1}}+\left( {{a}_{2}}+{{a}_{3}}{{t}_{1}} \right){{t}_{1}}-\left( {{b}_{2}}+{{b}_{3}}{{t}_{1}} \right){{t}_{1}}; \\ & {{c}_{3}}=-\frac{a}{2};{{c}_{2}}={{b}_{2}}+2{{b}_{3}}{{t}_{2}}-2{{c}_{3}}{{t}_{2}};{{c}_{1}}={{b}_{1}}+\left( {{b}_{2}}+{{b}_{3}}{{t}_{2}} \right){{t}_{2}}-\left( {{c}_{2}}+{{c}_{3}}{{t}_{2}} \right){{t}_{2}}; \\ \end{align}$$

  兩種情況都可以使用式（1）來對t時刻的位移值進行計算，這也是插值的基礎。
  程式見：

void VelocityProfile_Trap::SetProfile(double pos1,double pos2)；

和

 double VelocityProfile_Trap::Pos(double time) const；

3姿態插值

  對位移進行插值比較清晰，對剛體的姿態插值則是難點。KDL使用“角-軸”的方法對剛體的姿態進行插值。
角-軸表示姿態：我們知道，給定兩個在笛卡爾空間中具有相同原點不同姿態的起始座標系和終點座標系，總可以確定一個單位向量，使得可以通過起始座標系繞單位向量的軸，旋轉一個合適的角度得到終點座標系。
  令${{R}_{i}}$ 和${{R}_{f}}$分別表示起始座標系和終點座標系的旋轉矩陣，那麼這兩個座標系之間的旋轉矩陣可以通過（2）式來計算得出：

$${{R}^{'}}=R_{i}^{T}{{R}_{f}}=\left[ \begin{matrix} {{r}_{11}} & {{r}_{12}} & {{r}_{13}} \\ {{r}_{21}} & {{r}_{22}} & {{r}_{23}} \\ {{r}_{31}} & {{r}_{32}} & {{r}_{33}} \\ \end{matrix} \right](2)$$

  如果使用矩陣 ${{R}_{t}}\left( t \right)$來表示${{R}_{i}}$到

相關推薦