4WD-4WS型床椅一體化機(jī)器人室內(nèi)定位與點(diǎn)鎮(zhèn)定控制研究

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

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

0 引言

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

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

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

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

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

1 床椅機(jī)器人樣機(jī)開(kāi)發(fā)與運(yùn)動(dòng)學(xué)建模

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

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

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

圖1 機(jī)器人底盤(pán)結(jié)構(gòu)圖

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

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

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

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

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

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

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

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

1.2 運(yùn)動(dòng)學(xué)模型建立

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

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

(1)

(2)

(3)

(4)

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

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

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

2.1 極坐標(biāo)系下控制模型

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

(5)

式中，P=[xyθ]T為機(jī)器人的當(dāng)前位姿，x、y和θ分別為床椅機(jī)器人當(dāng)前坐標(biāo)及偏航角。

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

(6)

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

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

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

(7)

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

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

(8)

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

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

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

V′=

(9)

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

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

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

V′=-k1d2cos2α-k2k3sin2α≤0

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

ωin=

ωout=

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

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

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

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

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

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

(10)

Zk=HXk+V

(11)

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

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

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

k-1時(shí)刻輸入矩陣Uk-1為:

k時(shí)刻觀測(cè)矢量Zk及測(cè)量矩陣H為:

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

4 實(shí)驗(yàn)結(jié)果與分析

4.1 仿真及分析

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

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

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

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

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

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

4.2 實(shí)驗(yàn)及分析

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

圖7 床椅一體化機(jī)器人整體實(shí)物圖

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

圖8 點(diǎn)鎮(zhèn)定實(shí)驗(yàn)場(chǎng)景圖

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

圖9 A組點(diǎn)鎮(zhèn)定實(shí)驗(yàn)結(jié)果

圖10 B組點(diǎn)鎮(zhèn)定實(shí)驗(yàn)結(jié)果

圖11 C組點(diǎn)鎮(zhèn)定實(shí)驗(yàn)結(jié)果

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

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

5 結(jié)束語(yǔ)

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

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