基于設備體感控制的雙輪自平衡搏擊機器人*

姜文濤, 張博強, 朱正安, 劉萬軍

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

0 引 言

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

本文設計了一種基于設備體感控制的雙輪自平衡搏擊機器人。采用MPU6050傳感器檢測控制者的手臂姿態和上身傾斜動作,極大地減少了設備成本。利用傳感器識別控制者的姿態,由機器人完成動作,從而實現了實體對實體的搏擊體驗感。

1 整體架構

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

1.1 體感架構

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

1.2 搏擊機器人架構

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

圖1 系統整體架構與搏擊機器人整體結構

2 實現原理

2.1 機器人原理

機器人在其所受外力不大于臨界值時可以調回平衡狀態。在機器人中有一只MPU6050傳感器,用于實時檢測機器人姿態信息,通過控制雙輪的電機使得機器人達到一直平衡的穩態。根據上述原理得出,通過測量機器人的傾角和角速度,控制機器人向對應方向加速,消除傾角,實現機器人直立行走。因此,測量傾角和計算加速度是控制機器人持續直立的關鍵。

2.1.1 平衡算法

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

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

(1)

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

F=mgθ-mk1θ-mk2θ′

(2)

可得出控制機器人加速度的算法

a=k1θ+k2θ′

(3)

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

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

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

機器人模型運動方程

在角度θ很小時,運動方程簡化為

機器人靜止時

式中 系數k1,k2分別為比例和微分控制參數。k2等同于阻尼力,可以有效遏制機器人振動。

2.1.2 測 速

通過安裝在電機輸出軸上的光碼盤測量得到機器人的雙輪速度。利用計數器,檢測一段時間內脈沖信號數量,可計算出機器人速度。

2.1.3 速度差控制傾角

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

為了確保系統的穩態,機器人傾角控制時間常數Tz很大。使得系統產生的振動現象,解決方法是：在控制反饋中加入對角度的微分和積分,這樣就出現了一個比例控制環節,便可在穩態中傳遞函數不變,也避免了微分。因為使用了比例控制,若MPU6050安裝有誤差機器人會一直往一個方向運行,所以，在角度控制輸出中加入了直接積分補償,便可消除誤差。機器人在啟動時必須直立放置,才可正常運行。

2.1.4 整體控制

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

圖3 整體控制

采集MPU6050傳感器的X,Y,Z軸信號,角速度信號和電機脈沖信號,便可實現機器人的直立行走。

2.2 體感原理

2.2.1 方向檢測

當控制者身體前傾時,傳感器的X軸角度會減少約19°；控制者后傾時傳感器的X軸會增加約15°；控制者左傾時,傳感器的Y軸減少約18°；控制者右傾時,傳感器的Y軸增加約17°。控制者做出前后左右動作時陀螺儀采集到的數據如圖4所示。

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

2.2.2 出拳檢測

首先需要檢測出手臂的姿態,使用MPU6050傳感器,因為可以測出X,Y,Z三個方向的角度。只要將角度轉化為空間中的坐標,對這些采樣點進行曲線擬合,得到出拳軌跡。

1)角度坐標化

圖5 手臂出拳的立體示意

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

設大臂的MPU6050傳感器放置的位置距離肩部為L則L′的表達式為

L′=Lsin∠Z1

(7)

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

設小臂的傳感器放置的位置距離肘部為l,則l′的表達式為

l′=lsin(Z2-90°)

(8)

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

在圖6(b)中建立平面直角坐標系,以大臂肩部為原點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)的坐標系中的X,Y坐標值。

小臂上的2個傳感器在圖6(a)的坐標系中的計算方法要在大臂的基礎上進行計算。其中Xd到Xx的距離為ΔX,Yd到Yx的距離為ΔY。

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

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

(11)

ΔX和ΔY的計算為

Δ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)

此時小臂、大臂、肘部傳感器放置點的坐標可以求出。

2)采樣點線性擬合

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

圖7 手臂點示意與理想出拳線

使用拉格朗日插值法對這5個點進行曲線擬合,擬合公式為

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

(15)

式中li(x)為

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

根據這些點得到曲線后,分別求出B點和D點的斜率,在這兩點的斜率設定一定范圍用來確定控制者動作,進而實現了對控制者手臂姿態檢測。其中，斜率的界限如圖7(b)所示。

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

3 測 試

3.1 出拳測試

為驗證三種出拳姿態的準確性和實時性。控制者手臂處于豎直狀態時,進行陀螺儀角度初始化。控制者做出直拳時的姿態如圖8(a)所示。當直拳動作時,陀螺儀采集到的數據如圖8(b)所示。

圖8 控制者出直拳姿態及采集到的數據 (從第一行左側開始依次為A、B、C、D、E五個點)

控制者做直拳動作時,Z軸和Y軸曲線逐漸上升,分別在80°和45°附近保持穩定。

上勾拳動作時陀螺儀采集到的數據如圖9所示。

圖9 上勾拳時陀螺儀數據 (從第一行左側開始依次為A、B、C、D、E五個點)

控制者做上勾拳動作時,A，B兩點Y軸和Z軸曲線逐漸上升,分別為45°和75°附近保持穩定。C,D,E三點Y軸和Z軸曲線逐漸上升,前者在5°附近保持穩定,后者在120°附近保持穩定。

左勾拳動作時陀螺儀采集到的數據如圖10所示。

圖10 左勾拳時陀螺儀數據 (從第一行左側開始依次為A、B、C、D、E五個點)

控制者做出左勾拳動作,A、B兩點Y軸Z軸曲線逐漸上升,都在75°附近保持穩定。C,D,E三點Y軸Z軸曲線逐漸上升,前者在110°附近保持穩定,后者在20°附近保持穩定。出拳測試數據見表1。

表1 出拳測試數據

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

3.2 機器人性能測試

為了測試小車在穩定狀態下的最大運動速度、轉向角速度、起步加速度,與雙足機器人做對比,進行如下測試。

3.2.1 勻速運動

在測試開始和結束位置各放置一對光電門,然后記錄經過2個光電門間隔時間。

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

3.2.2 角速度測試

在機器人旋轉數周進入勻速轉動狀態后,開始計時,并采集數據，通過測試機器人旋轉一周用時集中在[3.64,4.82],計算得到機器人的角速度為1.505。

3.2.3 加速度測試

將紙帶穿過打點計時器固定于機器人后方測量數據，測試數據見表2。

表2 機器人加速度測試數據

紙帶上每4個點為一個計數點,表中的s1,s2,s3,s4為相鄰計數點間距。計算得出平均加速度為2.015 m/s2。

本文設計的雙輪自平衡搏擊機器人和現有雙足機器人相比,雙輪機器人的運行速度可達到0.728 m/s,加速度可達到2.015 m/s2,加速到0.728 m/s時僅需要0.36 s,轉向的角速度可以達到1.505,1.04 s可完成直角轉彎。

4 結 論

本文所設計的基于設備體感控制的雙輪自平衡搏擊機器人,其中體感部分是由陀螺儀來實現的,相比于微軟的Kinect,具有低成本和適用環境普適的優點。搏擊機器人采用雙輪代替雙腿,使機器人移動敏捷性較強,實現了體感游戲中實體對實體的特殊體驗,同時增強了用戶的沉浸感與代入感。在實際的體驗中具有穩定性高、娛樂性強的特點。