本篇文章投稿者為Todd
簡介
Mobile manipulation是ModernRobotics線上課程的專題。這篇文章是筆者用於紀錄以及分享專題的實作，希望能達到拋磚引玉效果，如果內文有不正確的部份，再請隨時告知。
Mobile manipulation是協調基座與機器手臂之間的運動，使得末端執行器能實現期望的運動。在這個專題中，我們會使用KUKA youBot並在CoppeliaSim內模擬”抓取-放置”任務：
在這個專題中，會用到這些工具:
- python以及modern-robotics library
- CoppeliaSim, 並利用課程事先建立好的模擬環境，可以參考這個連結
而任務的流程圖如下所示:
- 軌跡規劃產生參考姿態
- 控制器依照參考姿態以及里程計的預估姿態產生控制輸出
- 姿態預估依照控制輸出計算youBot姿態 (假設可以完美地依照控制輸出去計算姿態)
- 過程中得到的youBot姿態會被儲存至CSV檔案
- 軟體讀取CSV檔案，模擬youBot的動作
接下來會分別介紹:
- 姿態預估: 簡介youBot底座運動學，里程計
- 軌跡規劃: 簡介軌跡規劃，如何設定路徑去夾取方塊
- 控制器: 簡介前饋與反饋控制器，如何依照期望的軌跡去產生youBot的驅動
並在最後的整合章節串連上述各個章節的結果，展示軟體模擬的結果，接下來就讓我們開始第一個章節吧。
姿態預估
youBot是由4個麥克納姆輪的底座加上5軸機械手臂所組成，姿態預估會依照控制器計算的關節速度去預測youBot的姿態(假設可以完美的進行預測)。姿態預估使用里程計以及first-order euler step進行計算。
里程計
里程計(odomery)是根據底座輪子運動去估算，基本上是對車輪速度進行積分。里程計便宜且方便，但是它會隨著車輪滑動、地面平坦度等因素而產生誤差，因此會和其他感測器(Ex: Lidar)一起使用去彌補里程計產生的誤差。(這裡假設模擬環境、youBot的運動是完美的，因此不會考慮里程計的誤差)在介紹里程計的計算之前，我們需要了解youBot底座的運動學，那就讓我們開始吧～
youBot底座運動學
youBot的底盤使用4個麥克納姆輪去產生任意方向的速度，麥克納姆輪的示意圖如下所示:
圖中的\hat{x} _w和\hat{y} _w是位於車輪中心的座標系。我們推導單一個輪子的驅動速度u與車輪中心的線速度v=(v_x, v_y)之間的關係：
v_{drive}是輪子的驅動速度，而v_{slide}是輪子因驅動速度產生的滑動速度。將上面的公式做移項處理，可以將v_{drive}, v_{slide}用v_x, v_y表示，並且得到:
知道輪子中心的速度v與輪子驅動速度u之間的關係後，我們可以利用座標轉換的方式推導u_{i}與\dot{q}之間的關係：u_{i}=h(\phi)\dot{q}
先將\dot{q}轉換成V_{b}:
接下來，我們找出在{b}座標系，底盤以V_{b}進行移動時，輪子中心速度v_{wheel}: (公式內使用到cross product進行計算，可以想像成輪子中心距離{b}座標系會受到w_{bz}的影響而產生線速度)
上述得到的輪子中心速度v_{wheel}是在{b}座標系，我們進一步把它轉換至輪子中心座標系\hat{x} _w–\hat{y} _w去求v:
再使用一開始u與v的變換，就可以得到驅動速度u_{i}了:
最後將上述的結果進行整理，可以求得h_{i}(\phi):
上述的h_{i}(\phi)表示u與\dot{q}之間的關係，我們可以進一步把\phi設定成0，表示u與V_b之間的關係。如此一來，利用h_{i}(0)，我們可以利用下圖右側的參數，建立youBout底盤的運動學:
由於youBot底盤有4個輪子，我們依序堆疊h_{i}(0)去建立V_b → u的公式。如果要計算u → V_b，則可以進行逆矩陣的計算:
這樣我們就完成了youBot底盤運動學的推導。
里程計推導
得到youBot底盤運動學之後，我們開始推導里程計的計算公式。我們考慮從時間k到k+1的車輪角度變化，目標是以先前的底盤姿態q_k來計算新的底盤姿態q_{k+1}。
我們假設輪子在時間間隔內是固定的，並且設\Delta \theta為時間間隔的角度變化，則車輪速度為
。利用底盤運動學的u → V_b， 可以得到:
這樣可以對V_b在時間間隔內進行計算，得到\Delta \theta產生的姿態變化\Delta q_{b} = (\Delta\phi_{b}, \Delta x_{b}, \Delta y_{b}):
將(w_{bz}, v_{bx}, v_{by})進行置換:
由此可見，\Delta t的單位選擇並不重要，我們可以直接利用F \Delta \theta與上述公式更新\Delta q_{b}。最後，可以利用底盤的角度\phi_{k}將 {b} 座標系中的\Delta q_{b}轉換至 {s} 座標系中的\Delta q並更新里程計的估計值:
youBot手臂關節角度更新
由於我們使用first-order euler step進行計算，將手臂的關節從時刻k更新至時刻k+1的方式如下:
實作
這邊會以python去實作里程計以及youBot手臂關節角度的更新。我們會將計算式包裝在一個Odometry class並使用next_state function去更新姿態。
首先先做出Odometry class，並定義底盤運動學的矩陣(機構參數可以參考)
class Odometry:
def __init__(self):
w = 0.15
l = 0.235
r = 0.0475
self.F = (r / 4) * np.array([
[-1 / (l + w), 1 / (l + w), 1 / (l + w), -1 / (l + w)],
[1, 1, 1, 1],
[-1, 1, -1, 1]
])
接著我們做出next_state(為了避免篇幅過長，以下簡述較為重要的部分):
def next_state(self, curr_config: list[float], speed: list[float], dt: float, max_speed: float = 1e9) -> list[float]:
"""
## Description
Calculate next robot state
## Input
1. 12-vector representing the current configuration of the robot:
* 3 variables for chassis configuration
* 5 variables for arm configuration
* 4 variables for wheel angels
2. 9-vector of controls indicating the wheel speeds and joint speeds
* 5 variables for arm speed
* 4 variables for base speed
3. Timestep, dt
4. A positive real value indicating the maximum angular speed of the arm joints and the wheels
* By default, it is set to a large value
## Output
1. A 12-vector representing the configuration of the robot after dt:
chassis phi, chassis x, chassis y, J1, J2, J3, J4, J5, W1, W2, W3, W4
"""
# 手臂角度更新
delta_joint_theta = dt * joint_theta_dot
next_joint_angles = curr_joint_angles + delta_joint_theta
# 底盤odometry計算
delta_wheel_theta = dt * wheel_theta_dot
next_wheel_angles = curr_wheel_angles + delta_wheel_theta
V_b = self.F @ delta_wheel_theta
wbz = V_b[0][0]
vbx = V_b[1][0]
vby = V_b[2][0]
if wbz == 0.0:
delta_qb = np.array([
[0],
[vbx],
[vby]
])
else:
delta_qb = np.array([
[wbz],
[(vbx * math.sin(wbz) + vby * (math.cos(wbz) - 1)) / wbz],
[(vby * math.sin(wbz) + vbx * (1 - math.cos(wbz))) / wbz]
])
transform = np.array([
[1, 0, 0],
[0, math.cos(curr_config[0]), -math.sin(curr_config[0])],
[0, math.sin(curr_config[0]), math.cos(curr_config[0])],
])
delta_q = transform @ delta_qb
q_k_1 = q_k + delta_q
return ...
上述便是程式實作的部分，細節可以參考這份github檔案。
參考資料
- MODERN ROBOTICS MECHANICS, PLANNING, AND CONTROL by Kevin M. Lynch and Frank C. Park.
- Mobile Manipulation Project Description