4WD-4WS型床椅一體化機器人室內定位與點鎮定控制研究

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

(1.北京工業大學 信息學部,北京 100124； 2.數字社區教育部工程研究中心,北京 100124)

0 引言

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

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

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

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

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

1 床椅機器人樣機開發與運動學建模

1.1 機械與電氣系統設計

設計研制了4WD-4WS驅動模式的全向床椅機器人樣機，機器人樣機包括機械結構的設計與運動控制系統的設計，兩部分均采用模塊化設計，提了床椅機器人的可擴展性。

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

圖1 機器人底盤結構圖

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

圖2 機器人控制系統方案圖

該控制系統采用模塊化設計，主要包括一下幾部分：

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

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

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

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

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

1.2 運動學模型建立

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

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

(1)

(2)

(3)

(4)

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

圖3 前后輪反向偏轉示意圖

2 控制器設計及分析

2.1 極坐標系下控制模型

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

(5)

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

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

(6)

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

2.2 控制器設計及穩定性分析

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

(7)

若要實現床椅機器人的點鎮定控制目標，應滿足式(7)中三個狀態量分別收斂至0。

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

(8)

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

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

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

V′=

(9)

根據以上等式設計如下控制律：

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

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

V′=-k1d2cos2α-k2k3sin2α≤0

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

ωin=

ωout=

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

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

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

圖4 組合導航技術方案

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

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

(10)

Zk=HXk+V

(11)

式中，k時刻的系統狀態矢量Xk為:

狀態轉移矩陣A及控制輸入矩陣B分別為:

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

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

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

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

4 實驗結果與分析

4.1 仿真及分析

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

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

圖5 第一現象實驗結果圖

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

圖6 第二現象實驗結果圖

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

4.2 實驗及分析

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

圖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同時固定于床椅一體化機器人的中心處，實時獲取組合導航定位坐標，具體實驗場景如圖8所示。設定全局坐標系下期望位姿的真值為(2.55, 3.49, π/3)，本實驗設定初始狀態下全局坐標系與機器人坐標系x與y軸對應平行，因此初始狀態下，期望位姿在床椅機器人坐標系中為(2, 2, π/3)。

圖8 點鎮定實驗場景圖

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

圖9 A組點鎮定實驗結果

圖10 B組點鎮定實驗結果

圖11 C組點鎮定實驗結果

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

圖12 床椅機器人穩定狀態示意圖

5 結束語

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

在組合導航系統的配合下，本文設計的點鎮定控制方案，能夠實現床椅機器人無障礙物時的快速鎮定。但是運動路徑上存在障礙物時，如何在本文所設計點鎮控制方案中加入避障策略，實現快速本文點鎮定是下一個重點工作。