---
# System prepended metadata

title: 【SOLAMR】Multi-robot collaboration

---

# 【SOLAMR】Multi-robot collaboration

![](https://i.imgur.com/fMIIs38.jpg)

# 討論結果
先測試 Omni-differential driving (名稱暫定)，而非 skid differential driving

> Two linkded differential drive
> Two linkded two-wheel drive
> 我參考員成給我的 paper:
> Actuator Failure Compensation for Two Linked 2WD Mobile Robots Based on Multiple-Model Control
> [name=yc.huang]

## 衍生問題 1 - 結合機構
* **D 直徑多大才足以承受載B車的加速度**
    代定，經驗法則 or 簡單計算動力學
* **A , B車結合件磨耗**
    1. 使用==滾針軸承==作為結合件，但軸承可能在結合過程中因震動損壞
    2. A, B車結合機構處==留裕度==，雖不需軸承且設計簡單但控制精度下降磨耗程度上升
![](https://i.imgur.com/Snz1owt.png)


## 衍生問題 2 - 如何得知 $\theta_1$ 與 $\theta_2$
* ==相機==配合==Aruco== detection 算出角度，Aruco貼於Ｂ車中央下方
![](https://i.imgur.com/4HHXdnp.png)
* ==IMU==角速度積分，但可能會積分誤差累積
* ==Lidar==辨識Ｂ車萬向輪位置
* homemade 合適尺寸的==encoder==置於A, B車結合處

## 衍生問題 3 - A 車的 Sensor 視野會被 B 車萬向輪擋住
* 將 B 車==萬向輪後移==至使得Blind spot範圍縮小或是不遮擋前進方向
![](https://i.imgur.com/pdua7S7.png)
* 原本 Lidar 就有死角，可將 Ｂ 車萬向輪移動到死角處
![](https://i.imgur.com/6iDwXkM.png)

* 用相機或其他 Sensor 來彌補死角區

## 衍生問題 4 - 更改 A, B 的幾何尺寸以利Ａ車自轉
* A車
    * ==A車長度縮短==以利旋轉 (長方形->接近正方形)
    * Ａ車==改為四輪==(兩個驅動輪、兩個萬向輪)而非六輪
        * 原本需要載重物設計六輪，如今Ｂ車不重故可縮減輪數
    ![](https://i.imgur.com/03iSWfe.png)

* B車
    * 尺寸可隨Ａ車設計後更動

## 衍生問題 5 - 結合後運動學驗證
* ==Gazebo 模擬==系統建立，直接畫出結合後的AB車以測試控制方法
    * 找出 $\theta_1$ 與 $\theta_2$ 的關係
    * 建立一套==控制方法==使 B 車能順利導航

> 模擬與硬體分別進行，模擬內容以簡單為主，主要是為了探討如何進行 motion planing。
> 進一步還要整合 lidar 資訊，並且怎麼做 localization 與 odometry。
> [name=yc.huang]
> 

1. 先旋轉後移動
    ![](https://i.imgur.com/W2bj0tg.jpg)
    * 導航控制簡單但缺乏效率
2. DWA-like navigation 
3. 以大車為中心考慮兩個A車的command velocity



## 衍生問題 6 - 兩A車間通訊架構
* 無線通訊的Latency 
* Multimaster fkie 



## 協同搬運誤差分析
目的：找出輪子可能會側向打滑的情形，並分析誤差存在時會對系統產生什麼影響
執行動作：直行(前後車皆直行)，四顆馬達都給單位10的速度
假設: 馬達編碼器感測完美，不存在誤差

### 定義變數與誤差來源

$v_1$,$v_2$,$v_3$,$v_4$: 分別為四個輪子的轉速
$v_c$,$\omega_c$: 為大車質心線速度與角速度
$v_f$,$\omega_f$: 為前小車(front)的線速度與角速度
$v_r$,$\omega_r$: 為後小車(rear)的線速度與角速度
$\theta_1$,$\theta_2$: 分別為前、後小車heading相對於大車的夾角

主要考慮兩種誤差，其一為$\theta_1$,$\theta_2$感測誤差，其二為小車內馬達控制誤差($v_1 \neq v_2$, $v_3 \neq v_4$)，以是否考慮任一種誤差分成四種情況討論。


* ![](https://i.imgur.com/wZhtafn.jpg)

### 第一種狀況：完美馬達控制且完美$\theta_1$,$\theta_2$感測
前後小車馬達仍然會不同步，原因是兩台小車無線通訊的延遲與兩台車組裝製造的個體差異。
加入可拖動假設：**走較慢的馬達能被走較快的馬達拖動**，且拖動過程不影響$\theta_1$,$\theta_2$


* ![](https://i.imgur.com/NYTBhho.jpg)

### 第二種狀況：完美馬達控制但$\theta_1$,$\theta_2$感測有雜訊
系統認為起始位置為$\theta_1=0$,$\theta_2=0$，但實際上並非如此

$v_f$,$v_r$可被拆解成垂直與水平分量，垂直分量可能會造成前後馬達不同步，變成第一種情況。水平分量會造成大車旋轉，並讓$\theta_1$,$\theta_2$在前進過程中不斷改變

只要前後小車在heading axis上找到可行的configuration，就不會出現輪子側向滑動，但是隨著$\theta_1$,$\theta_2$越來越大，前後小車拖動對方的分力會隨夾角越來越小

猜測實務上當$\theta_1$,$\theta_2$超過某個值時，輪子的側向靜摩擦力將小於拖動的力量，暨時即會發生輪子側向打滑

* ![](https://i.imgur.com/r7lf6Vl.jpg)

### 第三種狀況：$\theta_1$,$\theta_2$感測完美，但馬達控制有雜訊
起始位置是正確的$(\theta_1=0$,$\theta_2=0)$，但隨著有雜訊的馬達控制加入$(v_1 \neq v_2$,$v_3 \neq v_4)$，前後小車的heading會逐漸歪斜，$\theta_1$,$\theta_2$發生變化

### 第四種狀況：$\theta_1$,$\theta_2$感測與馬達控制皆有雜訊
最接近現實也是最複雜的狀況
起始位置是有偏差$(\theta_1\neq0$,$\theta_2\neq0)$，且經過一段時間的雜訊馬達控制，$\theta_1$,$\theta_2$也被改變
* ![](https://i.imgur.com/3MkFuUR.jpg)



## Differential Drive Kinemetic

$x' = x-\frac{v}{w} sin\theta+\frac{v}{w}sin(\theta+\omega\Delta t)$
$y' = y+\frac{v}{w} cos\theta-\frac{v}{w}cos(\theta+\omega\Delta t)$

$\theta' = \theta+\omega\Delta t+\gamma\Delta t$

Where $\gamma=\frac{\theta'-\theta}{\Delta t}  -\omega$
$x,y,\theta$ are $x_{t-1}$ state, $x',y



| Column 1 | Column 2 | Column 3 |
| -------- | -------- | -------- |
| Text     | Text     | Text     |
',\theta'$ are $x_{t}$ state
 $v,w$ are $u_{t}$ control signal, $v$ is linear velocity and $w$ is angular velocity.
m

## Collaborative Localization

### If we do nothing
跟原本的單台AMR定位方式一樣
*  無法知道另一台車的定位
*  無法知道大車定位
*  無法協同導航

### Goal
讓某一台小車能獲得大車的完整姿態，而大車姿態由五個變數決定
$x_c$,$y_c$: 大車中心位置
$\theta_c$: 大車heading
$\theta_f$: 前車heading相對於大車的夾角
$\theta_r$: 後車heading相對於大車的夾角
![](https://i.imgur.com/Maf8vOk.jpg =300x)


### 三種 approach
|  |量測與大車相對夾角|量測對方小車位置|無線通訊溝通定位|
| ----- | ----- | ------| --- |
|示<br>意<br>圖|![](https://i.imgur.com/lTRBkaB.jpg) |![](https://i.imgur.com/O00EZ0n.jpg)|![](https://i.imgur.com/stm3lAK.jpg)|
|定<br>位<br>結<br>果|$x_c$,$y_c$,$\theta_c$,$\theta_f$|$x_c$,$y_c$,$\theta_c$,$\theta_f$,$\theta_r$|$x_c$,$y_c$,$\theta_c$,$\theta_f$,$\theta_r$|
|優<br>點| 近距離量測**誤差較小**<br>軟體開發**成本低** |未結合時也能量測到對方|簡單且定位結果較可靠|
|缺<br>點| 無法知道$\theta_r$<br> |遠距量測**誤差較大**<br>軟體開發**成本高**|佔據通訊頻寬<br>可能的延遲通訊|
|實<br>作| 相機量測Aruco<br>Lidar cluster |相機量測Aruco<br>Lidar cluster|WiFi/ Bluetooth|



### Physical Constraint
兩台小車的距離固定，上述三種方法皆可以代入此限制條件消除一些定位的不確定性
![](https://i.imgur.com/9lMFOqB.jpg =300x)

* Consistency Problem - 以Leader為主體
* 假設其中有一台車定位失效(被綁架)，系統會如何應對
### 定位架構方塊圖
紅色部分為協同搬運定位新增部分
![](https://i.imgur.com/tikvaUQ.jpg =400x)

## 導航問題
Leader vs Follower
規劃出一條Global path 
![](https://i.imgur.com/zWyYyIp.jpg =180x)

Leader需要傳輸怎樣的指令給Follower達到有效率的合作

|Leader傳輸|馬達控制指令($v$,$w$)|Local goal|不傳輸資訊|
| ----- | ----- | ------| --- |
|優點|較精準、馬達拖動量少|馬達拖動量普通|不佔據任何通訊頻寬<br>導航獨立於通訊狀態|
|缺點|溝通頻率高|溝通頻率中|最不精準、拖動量高、響應速度慢|
|障礙物感測|直接Abort|繞不過即Abort|繞不過即Abort|



* Centralized and decentralized

## kinamatics Interactive GUI
github: https://github.com/spiderkiller13/multirobot_kinematics
![](https://i.imgur.com/lcDOM8T.png)
### Input
$v_c$($v_h$) : 大車線速度
$w_c$($w$) : 大車角速度
$(x_a, y_a, \theta_a), (x_b, y_b, \theta_b)$ : 兩個小車的位置與heading
### Output: 
$v_a$,$w_a$: a車線速度與角速度
$v_b$,$w_b$: b車線速度與角速度

![](https://i.imgur.com/5VX71Mh.png)

暗紅色是大車的初始的姿態
亮紅色是給定Input並經過$\Delta t$時間的大車姿態
黃色線段是兩台小車的運動軌跡
綠色箭頭是兩台小車最後的姿態


### Forward kinematics
$x' = x-\frac{v}{w} sin\theta+\frac{v}{w}sin(\theta+\omega\Delta t)$
$y' = y+\frac{v}{w} cos\theta-\frac{v}{w}cos(\theta+\omega\Delta t)$

$\theta' = \theta+\omega\Delta t$

Where 
$x,y,\theta$ are initial position
$x',y',\theta'$ are final position
$v,w$ are linear velocity and angular velocity
$\Delta t$ is a small time interval



### Inverse kinematics 

Given $x',y',\theta'$ want to know  $x_c,y_c,r$

$\frac{x' - x}{y' - y}$ = $\frac{-sin(\theta) + sin(\theta')}{cos(\theta) - cos(\theta')}$

$w = \frac{\theta' - \theta}{\Delta t}$
[No way](http://www.cs.columbia.edu/~allen/F17/NOTES/icckinematics.pdf)
[Maybe](https://ieeexplore.ieee.org/document/7003726)

![](https://i.imgur.com/B0SjRPG.jpg =300x)

$\begin{bmatrix} 2(x'-x) & 2(y'-y) \\ cos(\theta) & sin(\theta) \end{bmatrix}  \left[ \begin{array}{c} x_c \\ y_c \end{array} \right]=\left[ \begin{array}{c} x'^2 - x^2 +y'^2 -y^2 \\ cos(\theta)x + sin(\theta)y \end{array} \right]$

![](https://i.imgur.com/9B5LvVG.png =300x)

![](https://i.imgur.com/4wR2kfP.gif =400x)

## Kinematic GUI interface 
假設$\Delta t$內的速度皆不變，畫出大車的姿態
暗紅色為大車起始姿態，亮紅色為大車最終姿態
左側兩個滑塊為input: $\dot{x}_c$, $\dot{\theta}_c$（缺$\dot{y}_c$）
下方四個數值為output: $v_a$, $\omega_a$, $v_b$, $\omega_b$.
![](https://i.imgur.com/VNeDzS1.gif =400x)

![](https://i.imgur.com/jps0WHB.gif =400x)

### Kinematics diagram 
Forward Kinematic: 給速度求最終位置
Inverse Kinematics: 給最終位置求速度
Link Constraint： 給大車姿態求兩台小車位置
只有一組可行解
![](https://i.imgur.com/UjQDWDO.jpg =300x)



## ref
 https://www.frontiersin.org/articles/10.3389/frobt.2018.00090/full
 
 
 Wang, Z. and Schwager, M. (2016c). “Multi-robot manipulation without communication,” in Tracts in Advanced Robotics, Vol. 112, eds Y.-J. Cho and N.-Y. Chong (Daejeon: Springer), 135–149.
 
 [demo video](https://msl.stanford.edu/multi-robot-manipulation-without-communication)

### Odom Fuser
* 目的：先前大車的定位都是用，前車的Laser detector反算的。所以大車定位會繼承了小車的定位誤差與小車的感測誤差，使得大車定位在很多時候會跑掉。
* 解法：用前後兩台小車的定位(Odom)直接算出大車的位置

![](https://i.imgur.com/Xz0fpFv.png =300x)

$dx_B = (dx_1cos(\theta_1) - dx_2*cos(\theta_2))/2$
$dy_B = (dx_1sin(\theta_1) - dx_2*sin(\theta_2))/2$
$d\theta_B = atan2(a_1-a_2)$
$vector = dx_2cos(\theta_2) + dx_1cos(\theta_1) + L$

 
## Experiment
### Multirobot collaboration without fusing localization
 {%youtube TOt8WEfUsJg  %}
 
### Multirobot collaboration without teleop
 {%youtube SWnmVpcFpDE  %}
 
