基于設(shè)備體感控制的雙輪自平衡搏擊機(jī)器人*

姜文濤, 張博強(qiáng), 朱正安, 劉萬軍

(遼寧工程技術(shù)大學(xué) 軟件學(xué)院,遼寧 葫蘆島 125105)

0 引 言

隨著科學(xué)技術(shù)的發(fā)展,體感技術(shù)在醫(yī)療、影視、游戲等領(lǐng)域發(fā)揮著重大作用。目前微軟Kinect[1～3]在體感識(shí)別領(lǐng)域使用較為廣泛,且技術(shù)較為成熟,但成本較高,使用環(huán)境有局限性。目前體感游戲大多數(shù)為面對屏幕進(jìn)行操作,游戲本身的體驗(yàn)感會(huì)大打折扣,所以本文設(shè)計(jì)的重點(diǎn)是實(shí)體對實(shí)體的體感搏擊機(jī)器人。在機(jī)器人的設(shè)計(jì)中,現(xiàn)在大部分機(jī)器人是雙腿站立[4],適逢平衡車[5～8]技術(shù)飛速發(fā)展,平衡車原理使機(jī)器人站立起來,可有效提高搏擊機(jī)器人移動(dòng)速度,并且具有高度的靈活性。

本文設(shè)計(jì)了一種基于設(shè)備體感控制的雙輪自平衡搏擊機(jī)器人。采用MPU6050傳感器檢測控制者的手臂姿態(tài)和上身傾斜動(dòng)作,極大地減少了設(shè)備成本。利用傳感器識(shí)別控制者的姿態(tài),由機(jī)器人完成動(dòng)作,從而實(shí)現(xiàn)了實(shí)體對實(shí)體的搏擊體驗(yàn)感。

1 整體架構(gòu)

基于設(shè)備體感控制的雙輪自平衡搏擊機(jī)器人擁有非常復(fù)雜的智能控制系統(tǒng)。在設(shè)備體感部分,由各個(gè)部位的陀螺儀檢測控制者的姿態(tài),并將指令發(fā)送給機(jī)器人,機(jī)器人由MPU6050傳感器檢測姿態(tài)信息,經(jīng)過PD平衡算法處理再對電機(jī)施加控制使其保持平衡,整體架構(gòu)如圖1(a)所示。

1.1 體感架構(gòu)

體感部分由1個(gè)STM32的核心板和13只MPU6050傳感器構(gòu)成,傳感器在左右胳膊各分布6只,1只在腰部。腰部傳感器用于檢測控制者的前、后、左、右姿態(tài)。胳膊上的6只傳感器檢測控制者的出拳姿態(tài)。

1.2 搏擊機(jī)器人架構(gòu)

圖1(b)是機(jī)器人整體架構(gòu),由2個(gè)直流電機(jī)、2個(gè)足輪、2個(gè)L5D11光柵測速模塊、1個(gè)TBDFNG直流電機(jī)驅(qū)動(dòng)模塊、1個(gè)STM32開發(fā)板、1個(gè)HC-06、1個(gè)MPU6050、1個(gè)舵機(jī)驅(qū)動(dòng)板、2個(gè)電源、2個(gè)機(jī)械手臂等組成。MPU6050檢測機(jī)器人傾角,機(jī)械手臂由5個(gè)舵機(jī)組成,可以最大程度模擬人類手臂動(dòng)作。

圖1 系統(tǒng)整體架構(gòu)與搏擊機(jī)器人整體結(jié)構(gòu)

2 實(shí)現(xiàn)原理

2.1 機(jī)器人原理

機(jī)器人在其所受外力不大于臨界值時(shí)可以調(diào)回平衡狀態(tài)。在機(jī)器人中有一只MPU6050傳感器,用于實(shí)時(shí)檢測機(jī)器人姿態(tài)信息,通過控制雙輪的電機(jī)使得機(jī)器人達(dá)到一直平衡的穩(wěn)態(tài)。根據(jù)上述原理得出,通過測量機(jī)器人的傾角和角速度,控制機(jī)器人向?qū)?yīng)方向加速,消除傾角,實(shí)現(xiàn)機(jī)器人直立行走。因此,測量傾角和計(jì)算加速度是控制機(jī)器人持續(xù)直立的關(guān)鍵。

2.1.1 平衡算法

控制機(jī)器人作加速運(yùn)動(dòng),分析機(jī)器人倒立擺受力情況,受到額外的慣性作用,該方向與車輪的加速度方向相反,大小成正比。受力分析如圖2(a)所示,所受到的回復(fù)力為式(1)。由于θ很小,因此進(jìn)行了線性化處理。假設(shè)穩(wěn)態(tài)的負(fù)反饋控制是給輪子加速度a與θ偏角成正比,比例值k1。如果比例值k1>g,回復(fù)力方向與位移方向相反。

F=mgsinθ-macosθ≈mgθ-mk1θ

(1)

為了讓機(jī)器人的倒立擺模型快速地回歸平穩(wěn),還需要加入阻尼力。阻尼力與偏角的角速度成正比,方向相反,根據(jù)式(1)可得

F=mgθ-mk1θ-mk2θ′

(2)

可得出控制機(jī)器人加速度的算法

a=k1θ+k2θ′

(3)

式中θ為小車角度,θ′為角速度。k1,k2為比例系數(shù)。

以上內(nèi)容分析,通過建立速度的比例微分負(fù)反饋控制,形成閉環(huán)系統(tǒng)使得機(jī)器人保持平衡。假設(shè)這個(gè)機(jī)器人受到外力的作用,會(huì)產(chǎn)生一個(gè)角加速度x(t),沿著垂直于機(jī)器人底盤方向進(jìn)行受力分析,可得到如圖2(b)所示,機(jī)器人的傾角與足輪運(yùn)動(dòng)加速度a(t)、外力干擾加速度x(t)之間的運(yùn)動(dòng)方程。

圖2 機(jī)器人倒立擺模型及外力作用下受力分析

機(jī)器人模型運(yùn)動(dòng)方程

在角度θ很小時(shí),運(yùn)動(dòng)方程簡化為

機(jī)器人靜止時(shí)

式中 系數(shù)k1,k2分別為比例和微分控制參數(shù)。k2等同于阻尼力,可以有效遏制機(jī)器人振動(dòng)。

2.1.2 測 速

通過安裝在電機(jī)輸出軸上的光碼盤測量得到機(jī)器人的雙輪速度。利用計(jì)數(shù)器,檢測一段時(shí)間內(nèi)脈沖信號(hào)數(shù)量,可計(jì)算出機(jī)器人速度。

2.1.3 速度差控制傾角

假使機(jī)器人初始靜立,然后提升到給定速率,為此機(jī)器人需前傾一定角度獲得加速度。機(jī)器人直立情況下,為獲得向前的傾斜角度,輪子需要向后運(yùn)動(dòng),會(huì)引起輪子速度下降。由于負(fù)反饋,使得機(jī)器人往前傾角增大。如此循環(huán),機(jī)器人就會(huì)傾倒。

為了確保系統(tǒng)的穩(wěn)態(tài),機(jī)器人傾角控制時(shí)間常數(shù)Tz很大。使得系統(tǒng)產(chǎn)生的振動(dòng)現(xiàn)象,解決方法是：在控制反饋中加入對角度的微分和積分,這樣就出現(xiàn)了一個(gè)比例控制環(huán)節(jié),便可在穩(wěn)態(tài)中傳遞函數(shù)不變,也避免了微分。因?yàn)槭褂昧吮壤刂?若MPU6050安裝有誤差機(jī)器人會(huì)一直往一個(gè)方向運(yùn)行,所以，在角度控制輸出中加入了直接積分補(bǔ)償,便可消除誤差。機(jī)器人在啟動(dòng)時(shí)必須直立放置,才可正常運(yùn)行。

2.1.4 整體控制

將上面所有對機(jī)器人直立行走的算法集中如圖3所示。

圖3 整體控制

采集MPU6050傳感器的X,Y,Z軸信號(hào),角速度信號(hào)和電機(jī)脈沖信號(hào),便可實(shí)現(xiàn)機(jī)器人的直立行走。

2.2 體感原理

2.2.1 方向檢測

當(dāng)控制者身體前傾時(shí),傳感器的X軸角度會(huì)減少約19°；控制者后傾時(shí)傳感器的X軸會(huì)增加約15°；控制者左傾時(shí),傳感器的Y軸減少約18°；控制者右傾時(shí),傳感器的Y軸增加約17°?？刂普咦龀銮昂笞笥覄?dòng)作時(shí)陀螺儀采集到的數(shù)據(jù)如圖4所示。

圖4 前后左右時(shí)陀螺儀數(shù)據(jù) (從第一個(gè)開始依次為前、后、左、右)

2.2.2 出拳檢測

首先需要檢測出手臂的姿態(tài),使用MPU6050傳感器,因?yàn)榭梢詼y出X,Y,Z三個(gè)方向的角度。只要將角度轉(zhuǎn)化為空間中的坐標(biāo),對這些采樣點(diǎn)進(jìn)行曲線擬合,得到出拳軌跡。

1)角度坐標(biāo)化

圖5 手臂出拳的立體示意

在圖5中大臂為直線OA傳感器Z軸的角度為∠BAO,也是圖6(a)中的∠Z1,并且大臂向傳感器Y軸偏轉(zhuǎn)的角度為∠OBC也是圖6(a)中的∠Y1。

設(shè)大臂的MPU6050傳感器放置的位置距離肩部為L則L′的表達(dá)式為

L′=Lsin∠Z1

(7)

小臂則是在大臂旋轉(zhuǎn)的基礎(chǔ)上進(jìn)行更進(jìn)一步的旋轉(zhuǎn),會(huì)先旋轉(zhuǎn)到直線OE再到OF,然后傳感器Y軸傾斜,直到合適的出拳姿態(tài)OG,其中∠GOH為傳感器的Z軸為圖6(a)中的∠Z2,傳感器Y軸角度為∠HOD也是圖6(a)中的∠Y2。

設(shè)小臂的傳感器放置的位置距離肘部為l,則l′的表達(dá)式為

l′=lsin(Z2-90°)

(8)

圖6 手臂出拳立體圖的俯視圖與對角線上的投影

在圖6(b)中建立平面直角坐標(biāo)系,以大臂肩部為原點(diǎn)O,其中Y1的角度為傳感器Y軸角度。所以

Xd=L′sin∠Y1,Yd=L′cos∠Y1

(9)

將式(7)代入式(9)可得

Xd=Lsin∠Z1sin∠Y1,Yd=Lsin∠Z1cos∠Y1

(10)

由上式可算出大臂和肘部傳感器在圖6(a)的坐標(biāo)系中的X,Y坐標(biāo)值。

小臂上的2個(gè)傳感器在圖6(a)的坐標(biāo)系中的計(jì)算方法要在大臂的基礎(chǔ)上進(jìn)行計(jì)算。其中Xd到Xx的距離為ΔX,Yd到Y(jié)x的距離為ΔY。

Xx和Yx的計(jì)算公式為其中Xd和Yd取肘部的傳感器

Xx=ΔX+Xd,Yx=ΔY+Yd

(11)

ΔX和ΔY的計(jì)算為

ΔX=l′sin∠Y2,ΔY=l′cos∠Y2

(12)

將式(8)代入式(12)可得

ΔX=lsin(Z2-90°)sin∠Y2,

ΔY=lsin(Z2-90°)cos∠Y2

(13)

所以將式(13)代入式(11)得出

Xx=lsin(Z2-90°)sin∠Y2+Lsin∠Z1sin∠Y1

Yx=lsin(Z2-90°)cos∠Y2+Lsin∠Z1cos∠Y1

(14)

此時(shí)小臂、大臂、肘部傳感器放置點(diǎn)的坐標(biāo)可以求出。

2)采樣點(diǎn)線性擬合

設(shè)手臂上這5個(gè)點(diǎn)為A(x1,y1),B(x2,y2),C(x3,y3),D(x4,y4),E(x5,y5)位置如圖7(a)所示。

圖7 手臂點(diǎn)示意與理想出拳線

使用拉格朗日插值法對這5個(gè)點(diǎn)進(jìn)行曲線擬合,擬合公式為

pn(x)=f(x0)l0(x)+f(x1)l1(x)+…+f(xn)ln(x)

(15)

式中l(wèi)i(x)為

將A～E這5個(gè)點(diǎn)代入式(16)可得到l0(x)～l4(x)，再將其代入式(15)可以得到擬合成功的曲線。

根據(jù)這些點(diǎn)得到曲線后,分別求出B點(diǎn)和D點(diǎn)的斜率,在這兩點(diǎn)的斜率設(shè)定一定范圍用來確定控制者動(dòng)作,進(jìn)而實(shí)現(xiàn)了對控制者手臂姿態(tài)檢測。其中，斜率的界限如圖7(b)所示。

有曲線后再求出一階導(dǎo)數(shù)可以計(jì)算出采樣點(diǎn)處的斜率,圖7(b)中的j1是直拳的下界,j2是直拳上屆也是左勾拳、上勾拳大臂的下界,j3是左勾拳的上界,j4是左勾拳的下界和上勾拳的上界。當(dāng)斜率在所屬的范圍內(nèi)就可以判定拳種。

3 測 試

3.1 出拳測試

為驗(yàn)證三種出拳姿態(tài)的準(zhǔn)確性和實(shí)時(shí)性?？刂普呤直厶幱谪Q直狀態(tài)時(shí),進(jìn)行陀螺儀角度初始化?？刂普咦龀鲋比瓡r(shí)的姿態(tài)如圖8(a)所示。當(dāng)直拳動(dòng)作時(shí),陀螺儀采集到的數(shù)據(jù)如圖8(b)所示。

圖8 控制者出直拳姿態(tài)及采集到的數(shù)據(jù) (從第一行左側(cè)開始依次為A、B、C、D、E五個(gè)點(diǎn))

控制者做直拳動(dòng)作時(shí),Z軸和Y軸曲線逐漸上升,分別在80°和45°附近保持穩(wěn)定。

上勾拳動(dòng)作時(shí)陀螺儀采集到的數(shù)據(jù)如圖9所示。

圖9 上勾拳時(shí)陀螺儀數(shù)據(jù) (從第一行左側(cè)開始依次為A、B、C、D、E五個(gè)點(diǎn))

控制者做上勾拳動(dòng)作時(shí),A，B兩點(diǎn)Y軸和Z軸曲線逐漸上升,分別為45°和75°附近保持穩(wěn)定。C,D,E三點(diǎn)Y軸和Z軸曲線逐漸上升,前者在5°附近保持穩(wěn)定,后者在120°附近保持穩(wěn)定。

左勾拳動(dòng)作時(shí)陀螺儀采集到的數(shù)據(jù)如圖10所示。

圖10 左勾拳時(shí)陀螺儀數(shù)據(jù) (從第一行左側(cè)開始依次為A、B、C、D、E五個(gè)點(diǎn))

控制者做出左勾拳動(dòng)作,A、B兩點(diǎn)Y軸Z軸曲線逐漸上升,都在75°附近保持穩(wěn)定。C,D,E三點(diǎn)Y軸Z軸曲線逐漸上升,前者在110°附近保持穩(wěn)定,后者在20°附近保持穩(wěn)定。出拳測試數(shù)據(jù)見表1。

表1 出拳測試數(shù)據(jù)

將表1中X,Y點(diǎn)代入式(16)就可以擬合出出拳軌跡。

3.2 機(jī)器人性能測試

為了測試小車在穩(wěn)定狀態(tài)下的最大運(yùn)動(dòng)速度、轉(zhuǎn)向角速度、起步加速度,與雙足機(jī)器人做對比,進(jìn)行如下測試。

3.2.1 勻速運(yùn)動(dòng)

在測試開始和結(jié)束位置各放置一對光電門,然后記錄經(jīng)過2個(gè)光電門間隔時(shí)間。

間隔時(shí)間數(shù)據(jù)集中分布在[1.48,1.89],計(jì)算得到機(jī)器人的平均速度為0.728 m/s。

3.2.2 角速度測試

在機(jī)器人旋轉(zhuǎn)數(shù)周進(jìn)入勻速轉(zhuǎn)動(dòng)狀態(tài)后,開始計(jì)時(shí),并采集數(shù)據(jù)，通過測試機(jī)器人旋轉(zhuǎn)一周用時(shí)集中在[3.64,4.82],計(jì)算得到機(jī)器人的角速度為1.505。

3.2.3 加速度測試

將紙帶穿過打點(diǎn)計(jì)時(shí)器固定于機(jī)器人后方測量數(shù)據(jù)，測試數(shù)據(jù)見表2。

表2 機(jī)器人加速度測試數(shù)據(jù)

紙帶上每4個(gè)點(diǎn)為一個(gè)計(jì)數(shù)點(diǎn),表中的s1,s2,s3,s4為相鄰計(jì)數(shù)點(diǎn)間距。計(jì)算得出平均加速度為2.015 m/s2。

本文設(shè)計(jì)的雙輪自平衡搏擊機(jī)器人和現(xiàn)有雙足機(jī)器人相比,雙輪機(jī)器人的運(yùn)行速度可達(dá)到0.728 m/s,加速度可達(dá)到2.015 m/s2,加速到0.728 m/s時(shí)僅需要0.36 s,轉(zhuǎn)向的角速度可以達(dá)到1.505,1.04 s可完成直角轉(zhuǎn)彎。

4 結(jié) 論

本文所設(shè)計(jì)的基于設(shè)備體感控制的雙輪自平衡搏擊機(jī)器人,其中體感部分是由陀螺儀來實(shí)現(xiàn)的,相比于微軟的Kinect,具有低成本和適用環(huán)境普適的優(yōu)點(diǎn)。搏擊機(jī)器人采用雙輪代替雙腿,使機(jī)器人移動(dòng)敏捷性較強(qiáng),實(shí)現(xiàn)了體感游戲中實(shí)體對實(shí)體的特殊體驗(yàn),同時(shí)增強(qiáng)了用戶的沉浸感與代入感。在實(shí)際的體驗(yàn)中具有穩(wěn)定性高、娛樂性強(qiáng)的特點(diǎn)。