李秀智,楊 宇,賈 桐,張祥銀

(1.北京工業(yè)大學(xué) 信息學(xué)部,北京 100124； 2.數(shù)字社區(qū)教育部工程研究中心,北京 100124)

0 引言

智能床椅一體化機器人(后文簡稱床椅機器人)是一種新興的智能化康復(fù)護理系統(tǒng)。其特點是通過一系列動作自動實現(xiàn)輪椅-床姿態(tài)的雙向變形，且具有導(dǎo)航、避障、自主對接與運動控制等智能行為。其中，點鎮(zhèn)定控制作為運動控制的首要問題，通常指控制機器人到達期望位置，并在該位置保持穩(wěn)定狀態(tài)，為后續(xù)機器人在某一固定位姿下實現(xiàn)相應(yīng)的功能提供可能。

由于室內(nèi)狹小空間限制以及橫向床-椅對接的需要，客觀要求床椅機器人具有全向運動能力。麥克納姆輪移動平臺可通過調(diào)節(jié)四輪速度就可以完成全方位移動，從而實現(xiàn)點鎮(zhèn)定控制[1]。雖便于控制，但由于車輪輥子接地點不連續(xù)導(dǎo)致車體振動，顯著降低了乘坐的舒適性。

四輪驅(qū)動及四輪轉(zhuǎn)向(four-wheel driving, four-wheel steering, 4WD-4WS)型驅(qū)動設(shè)計是一種可行的替代方案，在動力學(xué)參數(shù)未知的情況下，首先考慮其運動學(xué)點鎮(zhèn)定控制。通過阿克曼轉(zhuǎn)向[2]、合成軸轉(zhuǎn)向[3]、滑移轉(zhuǎn)向[4]等模型計算各電機的控制量。

由于此類非完整約束移動機器人平臺不滿足Brokett所提出的光滑反饋鎮(zhèn)定的必要條件，無法選取合適的連續(xù)光滑反饋控制律將移動機器人穩(wěn)定到期望位姿[5]。有學(xué)者提出了魯棒狀態(tài)反饋法[6]、基于齊次理論、平均系統(tǒng)理論的魯棒指數(shù)法[7]等不連續(xù)控制方法，但是在實際應(yīng)用中無法得到很多理想的不連續(xù)特性，因此導(dǎo)致在實際中難以應(yīng)用[8]。

由于不連續(xù)控制中的坐標變換能避免Brockett必要條件的限制，同時變換后的系統(tǒng)能夠通過Laypunov函數(shù)法構(gòu)造出直觀的連續(xù)光滑反饋控制律且其連續(xù)性有利于在實際中應(yīng)用[9-10]。因此，本文自主研發(fā)設(shè)計了一種全向床椅機器人樣機，并設(shè)計了基于CAN總線與ROS分布式運動控制系統(tǒng)，在此基礎(chǔ)上針對床椅機器人樣機的點鎮(zhèn)定控制問題，采用不連續(xù)坐標變換和Laypunov函數(shù)的方法來實現(xiàn)連續(xù)光滑反饋控制律的設(shè)計；為了提供控制器中的位置反饋，設(shè)計了超寬帶(ultra wide band, UWB)與慣性測量單元(inertial measurement unit, IMU)多源信息融合的定位方案；通過實驗驗證了整體控制方案的有效性。

1 床椅機器人樣機開發(fā)與運動學(xué)建模

1.1 機械與電氣系統(tǒng)設(shè)計

設(shè)計研制了4WD-4WS驅(qū)動模式的全向床椅機器人樣機，機器人樣機包括機械結(jié)構(gòu)的設(shè)計與運動控制系統(tǒng)的設(shè)計，兩部分均采用模塊化設(shè)計，提了床椅機器人的可擴展性。

各部分執(zhí)行機構(gòu)在系統(tǒng)指令的控制下完成相應(yīng)動作，使機器人調(diào)整至工作中所需要的不同位姿狀態(tài)，因此機器人底盤結(jié)構(gòu)設(shè)計將影響其運動性能，同時也將決定運動學(xué)模型的分析與建立和運動控制系統(tǒng)的設(shè)計。通過Solidworks完成對床椅機器人的結(jié)構(gòu)設(shè)計，其中底盤結(jié)構(gòu)是重點設(shè)計部分。床椅機器人底盤主體包含四個輪組，每個輪組包括一個輪轂電機、一個直流無刷轉(zhuǎn)向電機以及懸架減振機構(gòu)，如圖1所示。輪轂電機為主動輪提供行進的驅(qū)動力，直流無刷轉(zhuǎn)向電機用于機器人的轉(zhuǎn)向控制。該方式避免了四驅(qū)麥輪由于接地點不連續(xù)導(dǎo)致的車體振動，在保留全向運動的同時，顯著提高了乘坐的舒適性。

圖1 機器人底盤結(jié)構(gòu)圖

為提高復(fù)雜機器人系統(tǒng)的響應(yīng)速度及可靠性，采用CAN總線與串口通信收發(fā)傳感器數(shù)據(jù)和控制指令，具有較好的擴展性。軟件上，機器人操作系統(tǒng)(robot operating system, ROS)是一種基于分布式和模塊化的操作系統(tǒng)，有助于提高控制器數(shù)據(jù)計算與程序執(zhí)行的效率[11]。鑒于上述系統(tǒng)和通訊方式的優(yōu)點，設(shè)計了基于CAN總線通信與ROS的分布式運動控制系統(tǒng)，設(shè)計方案如圖2所示。

圖2 機器人控制系統(tǒng)方案圖

該控制系統(tǒng)采用模塊化設(shè)計，主要包括一下幾部分：

1)主控制模塊。該模塊包括工控機與USB-CAN設(shè)備。床椅機器人的控制算法均在工控機的ROS操作系統(tǒng)下完成。ROS系統(tǒng)中執(zhí)行節(jié)點包括算法解算節(jié)點、UWB節(jié)點、IMU節(jié)點、串口節(jié)點、驅(qū)動電機控制節(jié)點。各節(jié)點單獨控制設(shè)備數(shù)據(jù)的收發(fā)，使得各控制節(jié)點具備低耦合的特點。

2)驅(qū)動電機模塊。該四組伺服控制器與輪轂電機組成，輪轂電機與伺服控制器連接，并配有增量式編碼器，伺服控制器通過USB-CAN設(shè)備與工控機連接。為得到穩(wěn)定控制速度，通過速度閉環(huán)PID控制算法實現(xiàn)控制。

3)推桿電機與轉(zhuǎn)向電機模塊。該模塊硬件設(shè)備主要包括一個STM32單片機、四組轉(zhuǎn)向電機和四組推桿電機。單片機通過串口與工控機和推桿電機通信，通過CAN總線與轉(zhuǎn)向電機通信。為了實現(xiàn)對轉(zhuǎn)向電機精確的位置控制，采用雙閉環(huán)PID控制，即速度閉環(huán)與位置閉環(huán)。

4)電源模塊。電源模塊主要為床椅機器人各個傳感器以及設(shè)備提供電能，包括充電鋰電池、電壓顯示模塊、降壓模塊(24 V、12 V以及19 V)以及隔離開關(guān)。供電電源采用48 V鋰電池，經(jīng)各降壓模塊為各類控制器及電機提供可靠穩(wěn)定的電源。

5)導(dǎo)航定位模塊。該控制模塊主要是組合導(dǎo)航定位系統(tǒng)，為床椅機器人的自主移動功能提供實時定位坐標。硬件設(shè)備包括IMU與UWB，均通過串口通信與工控機連接，并對采集的數(shù)據(jù)完成優(yōu)化處理，同時將兩種設(shè)備固定于床椅機器人中心處。

1.2 運動學(xué)模型建立

阿克曼轉(zhuǎn)向原理[12]為4WD-4WS型運動學(xué)控制提供了設(shè)計依據(jù)。其優(yōu)勢在于，每個車輪均可沿自然路線運動，車輪處于純滾動的狀態(tài)且無滑移現(xiàn)象或者滑移的趨勢[13]。根據(jù)該原理，機器人可采用前輪轉(zhuǎn)向、后輪轉(zhuǎn)向以及全轉(zhuǎn)向等多種形式。由于單一的前輪或后輪轉(zhuǎn)向形式易產(chǎn)生床椅機器人的重心偏移，導(dǎo)致側(cè)翻、側(cè)滑等事故，而前后輪反向偏轉(zhuǎn)全輪驅(qū)動形式具備運動靈活、轉(zhuǎn)彎半徑小等特點，因此采用前后輪反向偏轉(zhuǎn)全輪驅(qū)動形式并對其進行分析[14]。

在轉(zhuǎn)向瞬間，床椅機器人的運動可看作相對于瞬時旋轉(zhuǎn)中心(instantaneous center of rotation, ICR)的瞬時旋轉(zhuǎn)運動，并且這個瞬時旋轉(zhuǎn)中心是時變的，也就是床椅機器人的速度方向垂直于機器人中心點與ICR的連線，且每個車輪中心點同ICR的連線與每個車輪的線速度方向垂直。當ICR位于床椅機器人橫向?qū)ΨQ軸的延長線上時，旋轉(zhuǎn)半徑為最小，因此內(nèi)側(cè)兩輪轉(zhuǎn)向角大小相等，外側(cè)兩輪轉(zhuǎn)向角大小相等[15]。因此通過幾何關(guān)系轉(zhuǎn)換，4WD-4WS模型中驅(qū)動電機與轉(zhuǎn)向電機的控制輸入量為式(1)～(4)中所示。

(1)

(2)

(3)

(4)

式中，r為床椅機器人的旋轉(zhuǎn)半徑，rl為床椅機器人的車輪半徑，2L為前后兩車輪的輪距，2W為左右兩車輪的輪距。vin、vout為內(nèi)、外側(cè)車輪的線速度，如圖3中所示線速度v1,3、v2,4；ωin、ωout為內(nèi)、外側(cè)車輪的角速度，如圖2中所示角速度ω1,3、ω2,4。

圖3 前后輪反向偏轉(zhuǎn)示意圖

2 控制器設(shè)計及分析

2.1 極坐標系下控制模型

本文設(shè)計床椅機器人系統(tǒng)是非完整約束系統(tǒng)，為了實現(xiàn)室內(nèi)環(huán)境下，在無障礙物時能夠快速鎮(zhèn)定至期望位姿，并保持穩(wěn)定，需要設(shè)計合適的控制器。若將目標姿態(tài)表示為Pg=[xgygθg]T(xg、yg、θg均為指定參數(shù))，則在全局坐標系下，床椅機器人當前位姿與期望位姿之間的誤差可以表示為:

(5)

式中，P=[xyθ]T為機器人的當前位姿，x、y和θ分別為床椅機器人當前坐標及偏航角。

如前文所述，床椅機器人作為非完整約束系統(tǒng)，不滿足Brockett定理中提出的存在反饋控制律的必要條件，但是極坐標變換作為不連續(xù)變換方法之一，變換后的模型中非完整約束條件不再顯示出現(xiàn)，可以通過設(shè)計連續(xù)光滑反饋控制律完成鎮(zhèn)定控制[16]。因此在本文點鎮(zhèn)定控制方法中，為設(shè)計出相應(yīng)的控制模型與全局速度控制律，采用極坐標形式表示出在全局坐標系下機器人當前位姿與期望位姿的位姿差[7,17]?，F(xiàn)以床椅機器人中心為坐標原點建立機器人坐標系Or-xryr，如圖3所示，xr軸的正向為床椅機器人的正前方。由于點鎮(zhèn)定過程中期望位姿點可能位于當前位置的任意方向，為便于對誤差模型總體描述，定義符號函數(shù):

(6)

式中，d為當前位置與目標位置之間的距離；α為當前時刻機器人坐標系x軸與當前位置和目標位置連線的夾角；Ψ為機器人目標偏航角與當前偏航角之間的差值，即θe。由此可見d、α符號僅代表位姿調(diào)整方向。

2.2 控制器設(shè)計及穩(wěn)定性分析

為得到控制律一般形式，忽略期望位姿點所在位置的影響，選取合適的誤差變量描述點鎮(zhèn)定控制模型，使β=α-Ψ，代入式(6)可得到如下公式。

(7)

若要實現(xiàn)床椅機器人的點鎮(zhèn)定控制目標，應(yīng)滿足式(7)中三個狀態(tài)量分別收斂至0。

對式(7)兩邊求導(dǎo)可得到床椅機器人的位姿誤差微分方程如式(8)所示。

(8)

為保證式(7)中三個變量均收斂到0，采用Lyapunov函數(shù)法設(shè)計控制律[18-19]，選擇的Lyapunov函數(shù)如下所示。

V=exp(d2+2(1-cosα)+2(1-cosβ)+ε)

對其微分求導(dǎo)，并將方程式(8)代入可得：

V′=

(9)

根據(jù)以上等式設(shè)計如下控制律：

式中，ε=0.1,k1,k2,k3> 0。

將控制律代入等式(9)可以得到:

V′=-k1d2cos2α-k2k3sin2α≤0

由李雅普諾夫穩(wěn)定性第二定律可得，對k1、k2、k3取合理的正值，則V’非正恒成立，即V正定，V’為負定[20]。在控制律的作用下，機器人的位姿誤差將會穩(wěn)定地收斂至0，從而實現(xiàn)床椅機器人鎮(zhèn)定控制。由式(1)～(4)可得到驅(qū)動電機與轉(zhuǎn)向電機的輸入控制律，如下所示。

ωin=

ωout=

式中，ξ1、ξ2為轉(zhuǎn)角系數(shù)，b1、b2、b3、b4為驅(qū)動輪轉(zhuǎn)動系數(shù)，K是車體物理參數(shù)且K=L2+W2，驅(qū)動輪的轉(zhuǎn)動方向與期望位姿點的所在位置相關(guān)。

3 基于卡爾曼濾波的組合定位

通常情況下，UWB定位精度約為0.1 m，且無定位累計誤差，但是在室內(nèi)環(huán)境下無法保證在UWB定位時，設(shè)備間無任何信號遮擋，而造成非視距(not line of sight, NLOS)情況的出現(xiàn)，導(dǎo)致定位精度嚴重下降，甚至出現(xiàn)UWB數(shù)據(jù)丟失現(xiàn)象[21]。因此在多形態(tài)特征的室內(nèi)環(huán)境下，采用單一的定位方案，難以獲取精確地實時定位坐標。由于IMU能夠在不借助外界其他設(shè)備的情況下，在短時間內(nèi)完成定位導(dǎo)航。鑒于上述UWB與IMU的定位優(yōu)點，為了解決床椅機器人運動控制過程中實時準確定位的問題，在卡爾曼濾波框架下完成多源信息的融合，以實現(xiàn)床椅機器人位置的最優(yōu)估計[21]，技術(shù)方案如圖4所示。

圖4 組合導(dǎo)航技術(shù)方案

在本方案組合導(dǎo)航算法中，由IMU數(shù)據(jù)的航跡推演提供一步狀態(tài)預(yù)測，作為組合導(dǎo)航的狀態(tài)方程。通過TOA定位思想，將優(yōu)化后的UWB數(shù)據(jù)采用兩次加權(quán)三邊定位法完成坐標解算，并作為觀測量，同時對預(yù)測量進行不斷修正，得到最小方差意義下的最優(yōu)位置估計。若出現(xiàn)UWB觀測信息缺失的情況，為保證定位的連續(xù)性，以IMU航位推算的先驗估計作為估值?，F(xiàn)定義系統(tǒng)狀態(tài)量為[xwywvxwvyw]T(各數(shù)值均為UWB全局坐標系w系下的數(shù)值，xw、yw為當前時刻坐標，vxwvyw分別為x軸方向、y軸方向的速度)，控制輸入量為[axrayr]T(各量為機器人在機器人坐標系r系下的加速度分量)，基于卡爾曼濾波組合定位系統(tǒng)的狀態(tài)方程與觀測方程具體分別為:

Xk=AXk-1+BRk,k-1Uk-1+W

(10)

Zk=HXk+V

(11)

式中，k時刻的系統(tǒng)狀態(tài)矢量Xk為:

狀態(tài)轉(zhuǎn)移矩陣A及控制輸入矩陣B分別為:

其中:T為采樣周期。k-1時刻到k時刻的坐標轉(zhuǎn)換矩陣Rk,k-1為:

k-1時刻輸入矩陣Uk-1為:

k時刻觀測矢量Zk及測量矩陣H為:

V為觀測噪聲，V～N(0,R)；W為系統(tǒng)噪聲，W～N(0,Q)。

4 實驗結(jié)果與分析

4.1 仿真及分析

定義仿真實驗中的全局坐標系為Os-xsys，且與初始狀態(tài)的機器人坐標系重合，即機器人初始位姿為(0, 0, 0)。在該坐標系下，分別選取不同象限內(nèi)姿態(tài)相同且位置不同的4個位姿，即(2, 2, π/4)、(-2, 2, π/4)、(2, -2, π/4)和(-2, -2, π/4)為目標位姿?？紤]實際的物理約束，設(shè)置機器人最大線速度與角速度分別為vmax=1 m/s，ωmax=1 rad/s。

1)第一象限內(nèi)點鎮(zhèn)定仿真實驗如圖5所示。

圖5 第一現(xiàn)象實驗結(jié)果圖

2)第四象限內(nèi)點鎮(zhèn)定仿真實驗如圖6所示。

圖6 第二現(xiàn)象實驗結(jié)果圖

經(jīng)仿真實驗發(fā)現(xiàn)，在仿真實驗的全局坐標系Os-xsys下，一、三象限與二、四象限的點鎮(zhèn)定軌跡圖像大致呈中心對稱，現(xiàn)以一、四象限結(jié)果為例進行分析。如實驗結(jié)果所示，點鎮(zhèn)定軌跡圖中箭頭表示機器人在期望位姿與初始位姿時的姿態(tài)方向，在鎮(zhèn)定過程中機器人的運動軌跡平滑且無明顯震蕩現(xiàn)象，且各速度及誤差變量能夠快速收斂至0，驗證了鎮(zhèn)定控制算法的有效性。

4.2 實驗及分析

床椅一體化機器人(如圖7所示)的機械部分除了上文所述的運動底盤，還包括用于變形的背部連桿機構(gòu)、腿部連桿機構(gòu)、扶手升降機構(gòu)等。運動控制系統(tǒng)中主控制器為一臺研華工控機，Intel i7內(nèi)核；驅(qū)動輪選用5寸輪轂電機，轉(zhuǎn)向電機選用大疆M3508直流無刷電機；IMU包含三軸加速度計和三軸角加速度計，UWB由四個基站和一個標簽組成。經(jīng)測試計算，各種傳感器及電機均滿足尺寸、功率和扭矩等實驗需要。

圖7 床椅一體化機器人整體實物圖

將UWB四個基站配置為矩形分布，并建立UWB全局坐標參考系Ow-xwyw。基站A0為坐標原點Ow，基站A0指向A1的水平方向為xw軸正向，基站A0指向A2的水平方向為yw軸正向。因此基站的坐標分別為A0(0, 0)、A1(4.7, 0)、A2(0, 5.8)和A3(4.7, 5.8)。將UWB標簽與IMU同時固定于床椅一體化機器人的中心處，實時獲取組合導(dǎo)航定位坐標，具體實驗場景如圖8所示。設(shè)定全局坐標系下期望位姿的真值為(2.55, 3.49, π/3)，本實驗設(shè)定初始狀態(tài)下全局坐標系與機器人坐標系x與y軸對應(yīng)平行，因此初始狀態(tài)下，期望位姿在床椅機器人坐標系中為(2, 2, π/3)。

圖8 點鎮(zhèn)定實驗場景圖

為驗證設(shè)計方案有效性，對同一期望位姿做出多組實驗。圖9、10、11中的(a)圖為床椅機器人點鎮(zhèn)定仿真運動軌跡與實況下運動軌跡采樣點的對比圖。為方便對比真實軌跡點與理想軌跡，將真實環(huán)境下點鎮(zhèn)定控制的定位采樣坐標由全局坐標系轉(zhuǎn)換至機器人坐標系下。通過對比看出，理想運動軌跡與真實運動軌跡采樣點基本重合，控制精度較為理想，最終使機器人鎮(zhèn)定到期望位姿范圍內(nèi)。如圖9、10、11中的(b)圖所示，在現(xiàn)場鎮(zhèn)定實驗過程中，機器人當前位姿與期望位姿的x軸誤差、y軸誤差以及偏航角的誤差均在最后收斂至同一水平。

圖9 A組點鎮(zhèn)定實驗結(jié)果

圖10 B組點鎮(zhèn)定實驗結(jié)果

圖11 C組點鎮(zhèn)定實驗結(jié)果

圖12為C組實驗中床椅機器人點鎮(zhèn)定控制達到的穩(wěn)定狀態(tài)，并對其做出分析。圖中叉子標志為期望位姿真值位置，十字標志為床椅機器人到達穩(wěn)定狀態(tài)質(zhì)心的真實坐標。經(jīng)過測量機器人到達穩(wěn)定狀態(tài)質(zhì)心的真實坐標為(2.49, 3.44, 0.96)。與期望位姿相比較x軸誤差為0.06 m，y軸誤差為0.05 m，偏航角誤差為0.08 rad，該誤差均為可接受的范圍內(nèi)，相比較UWB定位方案0.1 m的定位精度而言，本文設(shè)計組合定位系統(tǒng)具有更高的定位精度與實時性，同時多組實驗也驗證了本文所設(shè)計的點鎮(zhèn)定控制器在真實環(huán)境下的有效性。

圖12 床椅機器人穩(wěn)定狀態(tài)示意圖

5 結(jié)束語

針對全驅(qū)麥輪床椅機器人所存在的問題，設(shè)計實現(xiàn)了一種具備一定減震功能的4WD-4WS全向移動床椅機器人，并設(shè)計搭建了基于CAN總線通信與ROS的分布式床椅機器人控制系統(tǒng)。通過分析其轉(zhuǎn)向形式與驅(qū)動形式，建立了基于阿克曼轉(zhuǎn)向原理的床椅機器人運動學(xué)模型。經(jīng)后續(xù)運動控制實驗證明所設(shè)計床椅機器人樣機具有良好的穩(wěn)定性。針對床椅機器人室內(nèi)定位問題，設(shè)計實現(xiàn)了一種基于卡爾曼濾波的UWB/IMU多源信息融合的導(dǎo)航定位系統(tǒng)，同時解決了單一傳感器在復(fù)雜環(huán)境下定位受限的問題。通過實驗表明組合導(dǎo)航定位系統(tǒng)具有良好的實時性與較高的精確度。針對床椅機器人點鎮(zhèn)定控制問題，通過不連續(xù)坐標變換法，采用極坐標表示出誤差模型，并選用合適的變量建立出控制模型。繼而，設(shè)計出基于位置閉環(huán)的反饋控制器，實現(xiàn)了床椅機器人的點鎮(zhèn)定控制。多組MATLAB仿真與現(xiàn)場實驗表明點鎮(zhèn)定控制器具有可行性與良好的有效性。

在組合導(dǎo)航系統(tǒng)的配合下，本文設(shè)計的點鎮(zhèn)定控制方案，能夠?qū)崿F(xiàn)床椅機器人無障礙物時的快速鎮(zhèn)定。但是運動路徑上存在障礙物時，如何在本文所設(shè)計點鎮(zhèn)控制方案中加入避障策略，實現(xiàn)快速本文點鎮(zhèn)定是下一個重點工作。