# 【SOLAMR】Multi-robot collaboration


# 討論結果
先測試 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車結合機構處==留裕度==，雖不需軸承且設計簡單但控制精度下降磨耗程度上升

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

## 衍生問題 3 - A 車的 Sensor 視野會被 B 車萬向輪擋住
* 將 B 車==萬向輪後移==至使得Blind spot範圍縮小或是不遮擋前進方向
* 原本 Lidar 就有死角，可將 Ｂ 車萬向輪移動到死角處

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

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

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

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

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

1. 先旋轉後移動
* 導航控制簡單但缺乏效率
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$)，以是否考慮任一種誤差分成四種情況討論。

* 

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

* 

### 第二種狀況：完美馬達控制但$\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$超過某個值時，輪子的側向靜摩擦力將小於拖動的力量，暨時即會發生輪子側向打滑

* 

### 第三種狀況：$\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$也被改變
* 

## 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相對於大車的夾角

### 三種 approach
| |量測與大車相對夾角|量測對方小車位置|無線通訊溝通定位|
| ----- | ----- | ------| --- |
|示<br>意<br>圖| |||
|定<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
兩台小車的距離固定，上述三種方法皆可以代入此限制條件消除一些定位的不確定性

* Consistency Problem - 以Leader為主體
* 假設其中有一台車定位失效(被綁架)，系統會如何應對
### 定位架構方塊圖
紅色部分為協同搬運定位新增部分

## 導航問題
Leader vs Follower
規劃出一條Global path

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

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

* Centralized and decentralized

## kinamatics Interactive GUI
github: https://github.com/spiderkiller13/multirobot_kinematics
### 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車線速度與角速度


暗紅色是大車的初始的姿態
亮紅色是給定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)


$\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]$



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


### Kinematics diagram
Forward Kinematic: 給速度求最終位置
Inverse Kinematics: 給最終位置求速度
Link Constraint： 給大車姿態求兩台小車位置
只有一組可行解

## 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)直接算出大車的位置


$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 %}