SLAMWARE解决方案适配三轮全向型底盘说明

在機(jī)器人界中，三輪全向型底盤(pán)也具有非常廣泛的應(yīng)用，基于SLAMWARE的集成過(guò)程中，三輪全向型底盤(pán)也存在一定難度，為協(xié)助用戶(hù)集成，本文將做詳細(xì)說(shuō)明。

目前，常見(jiàn)的三輪全向型底盤(pán)可分為兩種，一種是1個(gè)全向輪在前，2個(gè)在后（即下圖中的a），另一種是2個(gè)全向輪在前，1個(gè)在后（即下圖中的b）

在下面的內(nèi)容中，將針對(duì)這兩種常用底盤(pán)做一個(gè)說(shuō)明。

一、定義

1.機(jī)器人坐標(biāo)系

所有跟機(jī)器人運(yùn)動(dòng)有關(guān)的計(jì)算均在機(jī)器人坐標(biāo)系中進(jìn)行：

如圖所示，機(jī)器人前進(jìn)的正前方為x軸正方向，機(jī)器人左側(cè)為y軸正方向，機(jī)器人的中心點(diǎn)的地面為坐標(biāo)系的原點(diǎn)（O）

2.機(jī)器人運(yùn)動(dòng)控制指令

SLAMWARE Core模塊采用統(tǒng)一的全向速度指令來(lái)對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)控制，其坐標(biāo)系如下：

機(jī)器人往前方移動(dòng)的速度為Vx，往左方平移的速度為Vy，逆時(shí)針旋轉(zhuǎn)的角速度為ω。

同理，如果需要機(jī)器人向后移動(dòng)，那么Vx為負(fù)值；向右移動(dòng)，Vy為負(fù)值；順時(shí)針旋轉(zhuǎn)，則ω為負(fù)值。

這些速度和角速度的計(jì)量單位均為公制單位，即分別為米每秒，和弧度每秒。

注意：角速度的單位是弧度每秒，而非角度每秒，在實(shí)際使用中，需要根據(jù)實(shí)際單位進(jìn)行換算。

3.輪組速度定義

根據(jù)輪組布局不同，輪組的速度定義如下圖所示

Va，Vb和Vc分別表示各個(gè)輪子的線速度，以讓機(jī)器人發(fā)生逆時(shí)針旋轉(zhuǎn)的方向?yàn)檎?，單位依然是米每?/p>

4.機(jī)器人位移定義

SLAMWARE在向底盤(pán)下達(dá)運(yùn)動(dòng)指令的同時(shí)，需要底盤(pán)上報(bào)機(jī)器人輪組的位移信息。

該位移信息由三部分組成，分別是Dx，Dy，Dθ，定義如下：

同樣的，按照機(jī)器人的坐標(biāo)系，如果機(jī)器人向前移動(dòng)了，則Dx為正，向左移動(dòng)了，則Dy為正，逆時(shí)針旋轉(zhuǎn)了，則Dθ為正。

對(duì)應(yīng)的每個(gè)輪子上的線位移分別是La，Lb，Lc

二、涉及指令

針對(duì)傳統(tǒng)的兩輪差動(dòng)式的底盤(pán)，可以直接使用

SLAMWARECORECB_CMD_GET_BASE_MOTOR_DATA

和SLAMWARECORECB_CMD_SET_BASE_MOTOR兩條指令來(lái)操作。

針對(duì)其他類(lèi)型的機(jī)器人底盤(pán)，涉及到的指令主要是

SLAMWARECORECB_CMD_SET_V_AND_GET_DEADRECKON指令，即0x41指令。

它的請(qǐng)求報(bào)文的內(nèi)容是：

 所以對(duì)應(yīng)的：

而這個(gè)指令的響應(yīng)報(bào)文如下：(由于歷史原因，響應(yīng)報(bào)文中dx ,dy的單位是mm, dtheta的單位是度)

對(duì)應(yīng)的代碼如下：

三、計(jì)算公式

備注：所有公式中的R表示的是機(jī)器人的旋轉(zhuǎn)半徑，單位是米（也就是輪子距離機(jī)器人中心的距離）

1.速度分解

將SLAMWARE Core下發(fā)的速度指令分解到各個(gè)輪子上。

布局a：1個(gè)全向輪在前，2個(gè)全向輪在后

Va = Vy + R * ω

Vb = - Vx * cos(30°) - Vy * sin(30°) + R * ω

Vc = Vx * cos(30°) - Vy * sin(30°) + R * ω

布局b：2個(gè)全向輪在前，1個(gè)全向輪在后

Va = - Vy + R * ω

Vb = Vx * cos(30°) + Vy * sin(30°) + R * ω

Vc = -Vx * cos(30°) + Vy * sin(30°) + R * ω

2.里程累積

當(dāng)?shù)妆P(pán)收到上述指令以后，需要首先獲得三個(gè)輪子在上一次收到這條指令和這一次收到這條指令之間的線位移，分別記作Da，Db，Dc

布局a：1個(gè)全向輪在前，2個(gè)全向輪在后

Dθ = (Da + Db + Dc) / R / 3

Dx = (- Db * cos(30°) + Dc /  cos(30°)) / 2

Dy = (- Db * sin(30°) - Dc / sin(30°) + Da) / 3

布局b：2個(gè)全向輪在前，1個(gè)全向輪在后

Dθ = (Da + Db + Dc) / R / 3

Dx = (Db /  cos(30°) - Dc / cos(30°)) / 2

Dy = (-Da + Db / sin(30°) + Dc / sin(30°)) / 3

四、示例代碼