【V-REP自學筆記（八）】控制youBot抓取和移動物體

---

【導讀】

在這一系列的V-REP自學筆記中，我們定了一個小目標，完成一個Demo。使用官方提供的KUKA公司的YouBot機器人模型來實驗機器人的感知和控制過程，控制機器人從A點抓取物品，然後移動到B點將物品放置在B點的工作臺上，這其中涉及到V-REP環境中的機器人感知和控制過程。沒有看過前期學習筆記的讀者，可以在文末找到往期文章地址。

在前面的幾期自學筆記（在文末可以找到地址）中，我們分別從V-REP軟體使用、Lua語言程式設計、Matlab/Python的遠端介面控制等角度來闡述瞭如何使用V-REP進行機器人模擬環境的搭建和程式碼編寫過程。在本期的筆記中，我們將實現本系列自學筆記的小目標，通過編寫程式碼來控制youBot機器人從A點的工作臺上抓取物體，並移動到B點將物體放置在B點的工作臺上。此外，我們還測試了在多個位置之間移動的路徑規劃，而非單一A、B點的路徑規劃。話不多說，先放一個最終的效果演示視訊：

本期筆記約6000字，如果要全部看懂需要一定的時間，建議先關注+收藏。本文配套的程式碼採用Python語言編寫，由於前面幾期筆記已經介紹了有關V-REP平臺和Python語言構建控制演算法的基本過程，這裡不再對其中的基礎進行介紹，如果有問題，可以參考我之前寫的自學筆記。本文所涉及到的所有工程檔案和演算法程式碼已經開源在Github上

另外，本文的參考書籍是《Modern Robotics: Mechainism, Planning and Control》，中文版本為《現代機器人學：機構、規劃與控制》，英文版的書籍可在本公眾號後臺回覆【MR】獲取下載連結，但是對於初次接觸機器人學的讀者（尤其是沒有接觸過旋量、李群李代數等的讀者）來說，我強烈建議大家購買中文版的書籍。

1. 問題的整體分析

在開始之前，我們可以先構思一下，如果想要控制youBot機器人進行物體的抓取和移動任務，我們需要做一些什麼工作？流程是什麼？看下圖，先在腦袋裡構思1分鐘 …

那麼，你想到了什麼呢？我們試著來看看以下問題：

1. A點和B點的位置怎麼確定？是實現規定好的呢還是實際通過感測器檢測得到？
2. A點的工作臺形狀是什麼樣的？高度和寬度有多少？youBot的機械臂能夠得著嗎？
3. 實際待抓取的物體放置在工作臺的什麼位置呢？如何去感知這個物體的位置？
4. A點工作臺上待抓取的物體形狀呢？放置的姿態呢？youBot的機械手尺寸夠大嗎？該用什麼姿勢抓取物體才能保證不掉落？
5. A點工作臺上待抓取的物體重量有多少？youBot的機械臂是否有足夠的力量來移動它？
6. 待抓取的物體是什麼材料的呢？表面紋理是什麼？如果機械手用力過大是否會破壞待抓取的物體？
7. 如果規劃youBot的路徑讓它從當前位置移動到A點，以及從A點移動到B點？
8. 從A點移動到B點只有一條路徑嗎？如果不止一條，如何選擇呢？
9. 根據規劃的路徑移動的時候如果撞到了工作臺怎麼辦？如何避免碰撞？
10. $\dots$

由於實際控制機器人完成這個任務時所面臨的問題太多，這裡不再贅述，但是希望大家明白，在控制一個實際的機器人去完成類似的任務時我們面臨的問題往往要比我們事先知道的多得多。但是，在V-REP等機器人的模擬環境中，有許多現實世界要面臨的問題都可以被忽略或者採用比較簡單的方式進行約束和實現，因此，在本期筆記中，我們不會解決上述所有的問題，只專注在機器人的運動規劃上，而對其中的環境感知問題、機械動力學問題與控制問題進行簡化。例如，在模擬環境中，我們可以通過介面函式直接得到A點位置、B點位置以及待抓取物體的位置和姿態。當然，如果讀者想要嘗試使用感測器去感知也是可以的，這裡我們不做探討。

2. 演算法的構思過程

假設我們要抓取的物體是一個正方體，我們叫它Cube，再假設在整個移動過程中，我們的機械手有一個相對於youBot機器人的“待命”位置，稱為standoff位置，所有的操作過程都是基於這個standoff位置來開展的。則我們可以將任務分段簡化為以下8個過程：

1. 移動youBot，從當前位置到目標位置A點，並將機械手移動到standoff位置；
2. 開啟機械手，控制機械臂將機械手移動到Cube處；
3. 閉合機械手，抓取Cube；
4. 控制機械臂將機械手抬起回到standoff位置；
5. 移動youBot到B點的Cube目標位置上方，並將機械手至於standoff位置；
6. 控制機械臂下降到Cube的目標位置；
7. 開啟機械手，放置Cube；
8. 控制機械臂抬起機械手，回到standoff位置；

我們考慮上述過程，規劃好每一步當中機器人各個關節的運動過程，然後將所有的步驟串聯起來，就完成了整個Pick and Place的過程。在這八個步驟中，我們需要用到以下幾個部分的內容：

• youBot的運動學解析；
• 一個運動規劃模擬器（Simulator）
• 參考軌跡的生成
• 前饋/反饋控制器

2.1 youBot的運動學分析

這部分內容其實在之前的筆記中我們已經完成了，這裡不再重複，如果不記得了，可以參考：（放置VREP學習筆記四的連結）。這裡我們對V-REP中youBot機器人模型的運動學引數做一個說明：

V-REP中youBot機器人的模型示意圖如上圖所示，底盤輪子編號和機械臂尺寸引數如圖所示，其夾持機構（機械手）的尺寸引數為：

其中：$d_{1,min}=2cm,d_{1,max}=7cm,d_2=3.5cm,d_3=4.3cm$。

對於youBot的底盤來說，前後輪之間的距離是$0.47m$，左右輪之間的距離是$0.3m$，輪子的半徑為$0.0475m$。參考這些引數我們可以合理的設定A、B兩點工作臺的高度和寬度，Cube的尺寸以及規劃的路徑引數等，保證我們的想法能夠實現。比如，在本工程中，所用的正方體Cube的尺寸為：

其他的引數詳見原始碼。

2.2 運動規劃模擬器（Simulator）

回憶一下，如果要控制youBot進行移動和機械臂抓取操作，我們一共需要控制多少機構？4個輪子+5個機械臂關節+1個機械手。在V-REP中，這個機械手只有一個關節，有2種狀態（open和close），由於機械手的開啟和關閉需要時間，因此為了確保機械手能夠正確開啟和關閉，設定好對應的機械手狀態以後要在該位置保持0.625秒以上，一般直接設定為1秒。

這裡我們引入一個來自《Modern Robotics》書中的概念：Configuration。Configuration來自於機器人Motion Planning裡面的一個空間概念，即所謂的構型空間（Configuration Space, C-Space，中文譯名也有配置空間，構位空間，C空間等），是一個與機器人構型相關的空間，其本身的理論非常豐富，本文不做深入介紹，可以參考相關書籍。簡單理解，Conciguration就是指的一些特殊的歐式群（例如向量），用來表徵機器人的空間狀態。例如本文中，youBot底盤在二維平面內運動，因此其空間狀態我們可以用$(x,y,\theta {)}^T$來表示，這個在我們之前的運動學分析筆記部分已經做了介紹，只是沒有突出Configuration這個概念。又如，一個六軸的機械臂，我們可以用一個六維向量來表示其Configuration：$({\theta }_1,{\theta }_2,{\theta }_3,{\theta }_4,{\theta }_5,{\theta }_6{)}^T$ ，即所謂的關節空間。而所謂空間就是一組向量的集合+特定運算，對於機器人來說，這個特定的運算過程就是指的在各個Configuration之間的變換。

那麼所謂的Simulator就是設計一個函式，使得其可以在輸入機器人的各個Configuration和所需的運動時間以後能夠隨著時間輸出對應的機器人運動狀態作為模擬模型的參考輸入。這個函式我們取名NextState()，其輸入輸出分別為：

輸入：

• 一個表徵youBot機器人當前Configuration的具有12個元素的向量（底盤空間位姿的3個變數，機械臂關節的5個變數，輪子關節的4個變數）；
• 一個表徵運動狀態的具有9個元素的向量（機械臂關節運動速度$\dot{\theta }$的5個變數，4個輪子的運動速度）；
• 最小時間間隔${\Delta }_t$；
• 一個表徵機械臂和輪子最大運動速度的正實數，用來限制機械臂和輪子的最大運動速度（為什麼要做這個，因為實際情況下為了防止出現意外事故，運動關節的最高速必須要被限制）。

輸出：

• 一個表徵下一時刻youBot機器人Configuration的具有12個元素的向量。

函式NextState()採用的計算方式是最簡單的一階線性方法：${\theta }_{t+1}={\theta }_t+{\dot{\theta }}_t\cdot {\Delta }_t$，詳細的實現過程見原始碼。

有了NextState()這個函式以後，我們只需要給定初始的機器人的Configuration，以及在各個時刻點的Configuration，將所有的Configuration輸入到函式，其輸出就是我們要的隨時間變化的機器人空間狀態。

2.3 參考軌跡生成器（TrajectoryGenerator）

首先，為了描述方便，我們在這裡做一些符號定義。

如上圖所示：

1. 定義世界座標系為 { s } \{s\} {s};
2. 定義youBot模型底盤中心位置所在的區域性座標系為 { b } \{b\} {b};
3. 定義機械臂在youBot底盤上的基座的區域性座標系為 { 0 } \{0\} {0};
4. 定義youBot機械臂末端的機械手所在的區域性座標系為 { e } \{e\} {e}；
5. 定義Cube所在的區域性座標系為 { c } \{c\} {c}；（圖中未畫出）

參考軌跡生成的核心內容是通過獲取每一個區域性座標系在全域性世界座標系中的位置和姿態，並使用四階方陣 $T$ 來表示，然後通過設計插值演算法來實現各個區域性座標系之間的轉移過程，最後使用運動規劃器和控制器來完成機器人的動作規劃和控制。這裡需要注意的是，所謂的“參考軌跡”並不是通俗意義上的那種在三維空間中使用$(x,y,z)$等來表示的空間位置點的集合，這裡的參考軌跡是$T$的集合，除了空間位置，可能還包括關節角度，速度等資訊。

為了生成參考軌跡，我們定義一個軌跡生成器函式TrajectoryGenerator()，用於生成末端執行機構（機械手）的空間運動軌跡，即區域性座標系 { e } \{e\} {e}的空間變換軌跡。前面我們已經將整個過程分為了8個階段，因此，對於每個階段來說，我們的處理過程一樣，從每一段初始的Configuration到結束的Configuration（下一段初始的Configuration）。因此，這裡函式TrajectoryGenerator()的輸入輸出分別為：

輸入：

• 機械手的初始Configuration： $T_{se\_initial}$

• Cube的初始位置的Configuration：$T_{sc\_initial}$

• Cube的目標位置的Configuration：$T_{sc\_final}$

• 在機械手抓取Cube時表徵兩者之間的相對位置的Configuration：$T_{ce\_grasp}$

• 在抓取前和抓取後，機械手相對於Cube的Standoff位置上的Configuration：$T_{ce\_standoff}$。

舉個例子，假設在初始時刻機械手在世界座標系 { s } \{s\} {s}中位置為$p=(p_x,p_y,p_z{)}^T$, 其姿態為：
$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$
則 { e } \{e\} {e}在世界座標系 { s } \{s\} {s}中的Configuration可以表示為 $T_{se}$：
$$T_{se}=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right]$$
這部分可能不好理解，可以理解為youBot的機械手預設不是直接到達Cube所在的位置，而是到達Cube上方的一個待命位置（Standoff），所有的抓取都是基於這個位置出發，然後回到這個位置。在抓取Cube完成，且機械手回到standoff位置以後，youBot再移動底盤來完成整體移動。

輸出：

• 一個具有$N$個Configuration的，能覆蓋所有8個階段的參考軌跡。其中，參考軌跡上的每一個時刻用一個$T_{se}$來表示機械手在那一刻的Configuration，外加一個0/1 state用來表示機械手的open/close狀態。

在我們實際實現的過程中，TrajectoryGenerator()函式的輸出（參考軌跡）可以表示為一個具有13個元素的向量，我們定義其順序和對應的內容如下：

編號	1	2	3	4	5	6	7	8	9	10	11	12	13
數值	phi	x	y	J1	J2	J3	J4	J5	w1	w2	w3	w4	gripper state

其中，

• x、 y、phi 分別表示World_X_Joint、World_Y_Joint、World_Th_Joint的座標和偏轉角度；
• J1， $\dots$， J5 分別表示youBot的機械臂上的5個關節角度；
• w1，$\dots$， w4分別表示front left, front right, rail right, rail left四個輪子的位置（旋轉角度），如下圖所示；
• gripper state即表示gripper（機械手）的狀態，0表示開啟，1表示閉合。

注意：x、y 的單位是米，phi、J1~J5， w1~w4等角度的單位是弧度。控制Gripper的開啟和閉合過程至少需要0.625秒，所以為了保證Gripper能正常完成Open/Close的動作，在編寫程式控制Gripper從開啟到閉合（或從閉合到開啟），確保要在該位置保持0.625秒以上，這裡可以直接設定為1秒。youBot底盤上四個輪子的編號順序如圖所示：

[外鏈圖片轉存失敗,源站可能有防盜鏈機制,建議將圖片儲存下來直接上傳(img-gcPhjaec-1603381200793)(V-REP自學筆記(八)].assets/YoubotTopView.png)

分別對應w1、w2、w3、w4四個變數作為輪子旋轉的角度。

只是生成了參考軌跡還不行，如何合理的提取隨時間變化的參考軌跡，並轉換為youBot各個關節軌跡的控制指令，使其能夠“平滑地”跟隨參考軌跡運動是一個難點。所以我們需要設計一個前饋/反饋控制器。

2.4 前饋/反饋控制器（FeedbackControl）

有了隨時間變化的參考軌跡以後，我們就可以設計機器人的控制器，控制機器人的各個關節來跟隨預定的關節角度參考曲線實現控制。這部分的內容在《Modern Robotics》的第11.6章節和13.5章節部分有更為詳細的介紹，如果不是很理解這部分的內容，可以參考書籍的內容，會更詳細。

還是從這個圖出發，機械手的區域性座標系 { e } \{e\} {e}在全域性座標系 { s } \{s\} {s}中可以表示為$X(q,\theta )$：
$$X(q,\theta )=T_{se}(q,\theta )=T_{sb}(q)T_{b0}{T}_{0e}(\theta )\in SE(3)$$
其中，$\theta \in {\mathbb{R}}^n$表示具有$n$個關節的機械臂的關節角度，也稱之為機械臂的Configuration，這裡$n=5$。$T_{0e}(\theta )$表示機械臂的正運動學計算，$T_{b0}$表示座標系 { 0 } \{0\} {0}相對於座標系 { b } \{b\} {b}的位置偏移，$q=(\varphi ,x,y)$表示底盤的Configuration，於是我們有：
$$T_{sb}(q)=\left[\begin{array}{cccc}\cos \varphi & -\sin \varphi & 0& x\\ \sin \varphi & \cos \varphi & 0& y\\ 0& 0& 1& z\\ 0& 0& 0& 1\end{array}\right]$$
其中，$z$表示座標系 { b } \{b\} {b}的原點離地面的高度，其實就是底盤的高度，是一個固定值。

假設$X(t)$是一個隨時間變化的函式，表示機械手的空間軌跡，則$[\mathcal{V}(t)]=X^{-1}(t)\dot{X}(t)$表示機械手在座標系 { e } \{e\} {e}中的旋量（twist）。旋量是數學幾何與物理中的一個方法，在機器人學理論中可以用來構建機器人運動學模型（在某些情況下優於DH方法）和對應的控制理論，這部分內容本身比較多，夠寫好多篇筆記了，這裡按下不表，除了《Modern Robotics》一書，感興趣的讀者還可以參考Peter Corke老爺子的經典書籍。

假設輪子的速度為$u\in {\mathbb{R}}^m$，其中$m$表示輪子的數量，這裡為4。在構建我們的控制器之前，我們需要使用到運動學模型的雅克比矩陣$J_e(\theta )\in {\mathbb{R}}^{6\times (m+n)}$，並使其滿足：
$${\mathcal{V}}_e=J_e(\theta )\left[\begin{array}{c}u\\ \dot{\theta }\end{array}\right]=\left[J_{\mathrm{b}\mathrm{a}\mathrm{s}\mathrm{e}}(\theta )J_{\mathrm{arm}}(\theta )\right]\left[\begin{array}{c}u\\ \dot{\theta }\end{array}\right]$$
其中，$J_{base}(\theta )\in {\mathbb{R}}^{6\times m}$和$J_{arm}(\theta )\in {\mathbb{R}}^{6\times n}$分別表示底盤和機械臂的雅克比矩陣。這兩個雅克比矩陣的構建方法在《Modern Robtics》的第6.2章節有詳細的介紹，請參考原書內容。在我們的原始碼中，我們直接使用了這個雅克比矩陣進行計算。

這裡我們要構建一個函式FeedbackControl()用於疊加前饋和反饋控制演算法，其計算公式如下：
$$\mathcal{V}(t)=\left[{\mathrm{Ad}}_{X^{-1}{X}_d}\right]{\mathcal{V}}_d(t)+K_p{X}_{\mathrm{e}\mathrm{r}\mathrm{r}}(t)+K_i{\int }_0^t{X}_{\mathrm{e}\mathrm{r}\mathrm{r}}(\mathrm{t})d\mathrm{t}$$
其中，$K_p$和$K_i$是控制器的引數。函式的輸入和輸出分別為：

輸入：

• 當前機時刻械手的實際Configuration $X$（或者寫作$T_{se}$）
• 當前機時刻械手的參考Configuration $X_d$（或者寫作$T_{se}\_d$），$d$ 是desired的意思
• 從參考軌跡中取得的下一個時刻(一個${\Delta }_t$以後)機械手的參考Configuration $X_{d\_next}$（或者寫作$T_{se}\_d\_next$）
• PI控制的引數 $K_p$和$K_i$
• 時間${\Delta }_t$

輸出：

• 在座標系 { e } \{e\} {e}中表示的機械手的旋量$\mathcal{V}$

當前時刻機械手的Configuration可以表示為 $X(q,\theta )$，$q$和$\theta$分別表示底盤和機械臂的Configuration，這兩個Configuration可以從我們前面設計的Simulator獲取，其中具體元素的值在V-REP中可以直接通過函式介面獲取得到，在現實情況下也可以通過高精度的感測器獲取。

$X_{err}$表示$X_d$與$X$的偏差，計算方式為 $[X_{err}]=log(X_d^{-1})$，此外，$[{\mathcal{V}}_d]=(\frac{1}{{\Delta }_t})log(X_d^{-1}{X}_{d\_next})$，$[A{d}_{X^{-1}{X}_d}]$表示一個變換，用來將旋量${\mathcal{V}}_d$從參考座標系$X_d$中轉換到當前的實際座標系$X$中。最後，我們需要將FeedbackControl()函式的輸出 $\mathcal{V}$轉換為輪子的角速度和機械臂各個關節的運動角速度：
$$\left[\begin{array}{c}u\\ \dot{\theta }\end{array}\right]=J_e^{†}(\theta )\mathcal{V}$$
其中，$J_e^{†}(\theta )$表示雅克比矩陣$J_e(\theta )$的逆。

這部分內容應該是本期筆記中最難的部分，由於篇幅有限，很大一部分內容我都直接略過去了，沒有講細節，如果想要搞明白其中的細節，請參考《Modern Robotics》原書內容，所以還是強烈建議讀者買中文版的書籍，結合英文版的看，更容易理解。

3. 演算法實現

演算法實現我們使用Python來完成，並通過遠端通訊介面將計算結果傳送給V-REP中的youBot模型，完成模擬過程，因此我們會用到前期系列筆記中的部分程式碼，例如youBot的運動學模型程式碼，Python遠端控制V-REP中模型的程式碼等，所有的程式碼都已經開源在github上，我們分別實現了前面所說的3個函式：

在實際編碼的過程中，我們還會用到一些前期筆記中未使用到的介面函式，這裡做一些簡要說明。例如：

3.1 獲取Cube的位置資訊

首先，我們通過手動放置和調整工作臺讓它處於一個合適的位置，可以自己隨意設定，然後在上面放置我們的物體Cube。本文附帶的原始碼中有相關的V-REP工程檔案，其中內建的工作臺和Cube可以自己移動和編輯。在V-REP中模擬時，我們可以通過simxGetObjectPosition( )這個Python的函式介面來獲得Cube在V-REP中的空間位置，因此我們可以直接獲取得到Cube的初始位置以及需要放置的目標位置，Python程式碼如下：

_, cube_1_position = vrep_sim.simxGetObjectPosition(clientID, cube_1_handle, -1, vrep_sim.simx_opmode_blocking)
_, cube_2_position = vrep_sim.simxGetObjectPosition(clientID, cube_2_handle, -1, vrep_sim.simx_opmode_blocking)
_, cube_goal_position = vrep_sim.simxGetObjectPosition(clientID, cube_goal_handle, -1, vrep_sim.simx_opmode_blocking)

同樣地，youBot機器人本身的位置和姿態也可以通過該函式介面直接獲取，詳見原始碼。
另外，實際使用的時候，注意需要先啟動V-REP的模擬環境，再執行main.py檔案。

3.2 機器人運動學模型計算和雅克比矩陣

演算法的實現過程使用了《Modern Robotics》一書中的部分基礎性的原始碼，主要是一些矩陣運算、機器人運動學計算的一些函式，這些程式碼都是開源的，地址在這裡。當然，這個開源的程式碼除了Python版本，還有Matlab和C++版本，能夠覆蓋大部分讀者的使用需求了。獲取《Modern Robotics》書籍的英文版，可在公眾號“博士的沙漏”後臺回覆【MR】，往期筆記也可以在這裡找到。

參考

1.Modern Robotics

2.Mobile Manipulation Capstone