基于多源信息融合的床椅機器人自主對接技術和方法研究

房志東，梅瓏，金來

(1.安徽三聯機器人科技有限公司，合肥 230081；2.安徽三聯學院，合肥 230601)

1 引言

隨著經濟發展和人民生活水平提高，失能臥床人群的照護需求劇增，尤其交通事故造成的殘疾失能人群以及失能老人照護問題越來越受到社會關注[1]。根據2020年民政部數據，我國僅有50余萬名養老機構護理員，遠不能滿足失能人群的照護需求，同時相關的照護技能和護理設備嚴重缺失[2]?；跈C器人和計算機技術的飛速發展，使代替護理員的智能康護機器人成為可能，一種床椅分離式機器人護理床由此創新地提出[3]，此床可以滿足失能人群的移位和部分護理功能，可由護工或受護者操作進行護理和康復訓練，床椅分離后可變成一張多功能移動輪椅，進行多功能體位變換康護，可以無障礙移位外出，輪椅返回可方便與床體對接合并成整床。然而床椅機器人的自護性需要解決床椅柔性、安全和精準的分離和對接的難題，目前市場可見分離式護理床基本全為人工輔助的非智能對接方式，人工對接方式一方面費時費力，自動化程度低，大大增加照護人員體力負擔，另一方面影響受護者尊嚴和出行便利性，每次輪椅歸位還要請求護理人員協助合并，自主獨立性弱。為了實現床椅機器人的自主對接，本文擬嘗試采用超聲等多源信息融合與定位導航技術，依托機器人伺服系統實現床椅機器人輪轂電機精準控制，旨在提升自主對接的精度和成熟度。

2 方法

床椅機器人自主對接首先需要基于通信和控制功能構建自主對接的床、椅硬件系統和單元；然后以床體為參照建立相對坐標系，通過超聲傳感器組等方式構建TOA(Time of Arrival，時間達到)定位系統，運用輪轂編碼器輸出脈沖信號對輪椅運動狀態推算，以IMU(Inertial Measurement Unit，慣性測量單元)加速度角速度信號進行航位解析，通過EKF(Extended Kalman Filter，擴展卡爾曼濾波器)方式進行多源定位信號融合，實現最優精確位姿估計。其間涉及移動輪椅閉環控制模型以及融合位姿與期望位置校對，通過路徑規劃和運動控制完成輪椅的精確驅動，最終完成自主對接。

自主對接的性能測試以對接時間、對接間隙、對接間隙角度，對接成功率、適應性等指標進行衡量。

(1)對接時間測量：床椅一體機進入可對接狀態，移動床椅和固定床體1米距離以上，完成對接前的到達預定區域位置和姿態準備，用秒表開始計時t1，直到移動床椅、固定床體完成對接，并及時調整到平躺姿態，計時結束t2i，則對接時間為(t2-t1)，時間取整到1 s。

(2)對接間隙距離：測量床椅一體機對接成功后，移動床椅和固定床體的中間間隙距離，用游標卡尺測量床頭、床中和床尾三個地方，取平均值。

(3)對接間隙角度測量：床椅一體機對接成功后，用角度儀測量移動床椅和固定床體偏斜角度。

(4)對接成功率：參考對接時間測量標準，n次對接實驗，首次成功m次，首次不成功二次成功s次，則一次對接成功率m/n，二次對接成功率(m+s)/n,n不小于20。

(5)適應性：以不同起始對接位置和航向角實驗測試對接成功率，反應系統對接適應的魯棒性。

3 系統設計

3.1 硬件系統設計

整個床椅電氣控制系統較為復雜，包括輪椅主控器、床側主控器、手控器、驅動器、電源轉換器和傳感器組、無線通信裝置等。這里主要探討的是床椅如何對接，故說明電控系統中對接功能部分的構成。

如圖1所示，為床椅機器人床體完成對接控制的系統框圖，包括一個床體處理器單元，和處理器單元連接的核心模塊包括無線通信收發模塊U，自研超聲發射傳感器模塊A、B和C，因為B、C和A的內部構造相同，圖中內部構造省略。超聲發射傳感模塊包括信號產生單元、放大單元和發射單元，分別由微處理器、放大器和發射探頭電路連接構成；床體處理器單元根據無線通信模塊U的超聲控制信息，進行邏輯運算，控制A、B和C傳感模塊的工作狀態。無線通信收發模塊U，一方面將床體處理器單元產生的數據信息發送給輪椅無線收發模塊，另一面給接收輪椅無線模塊發送的各類狀態和控制數據信息。

圖1 機器人床體對接系統組成圖

如圖2所示，為機器人輪椅部分完成合并的系統組成圖，包括一個輪椅處理器單元，和輪椅處理器連接的包括無線通信收發模塊V，自研超聲接收傳感器模塊L和R；以及紅外激光測距傳感器模塊G和H。超聲接收傳感器模塊L和R由可以接收床體超聲發射傳感模塊發射信號的接收單元，和對接收信號進行預處理單元，經過預處理的測距信號通過輪椅處理器進行分析解算，形成距離信息。紅外激光測距模塊G、H是一個發射和接收一體化模塊，內部的發射模塊完成信號的產生、放大和發射，發射信號經過外部物體產生反射信號；內部接收模塊接收發射產生的反射信號，然后經過放大、增益調整等預處理操作產生信號發送給輪椅處理器單元，經過邏輯運算，得到距離信息。

圖2 機器人輪椅對接系統組成圖

3.2 融合定位方法

3.2.1 室內坐標標定模型

以俯視角度，對位置空間進行簡化，將床椅機器人床體部分簡化為一個L型，輪椅部分簡化成矩形，另因床體和輪椅同處在一個地面，垂直方向上誤差基本可以忽略，因此簡化為二維水平面上幾何解算關系。如圖3所示，以L型床體部分內測邊緣交叉點為原點O，以長邊為y軸，短邊為x軸，建立以床體為參考的坐標系。床體部分，固定安裝了自研發的超聲發射模塊A、B和C，坐標分別為A(a1，b1)、B(a2，b2)、C(a3，b3)，各模塊的位置因設備組裝完成而確定。移動輪椅部分，以朝床體方向前方分別固定安裝兩個可以接收床體超聲發射信號的自研超聲接收模塊L和R，記LR的中點M，定義坐標分別為L(xl，yl)、R(xr、yr)、M(xm，ym)；同時在輪椅右側安裝兩個紅外激光測距模塊G和H，坐標為G(xg，yg)和H(xh，yh)。AB、AC之間距離在床椅機器人中為固定常數，記為dab和dac。

圖3 床椅機器人坐標系構建

3.2.2 超聲傳感器定位

超聲定位系統下輪椅的位姿以M點坐標和輪椅航向角確定。床體控制器通過無線收發模塊U和輪椅無線收發模塊V通信，當輪椅發出全局定位指令，在得到輪椅側傳過來的確認信息后，以特定周期規律Ts循環控制發射模塊A、B、C發射信號，A、B、C發射互斥，Ts較小通常為十毫秒級。例如收到指令后，啟動A，關閉B，此時A發射信號被輪椅側接收模塊L和R接收，經過輪椅處理單元解算完成傳輸時間tal、tar提取，得AR的距離dar和AL的距離dal。

Ts時間之后，床體控制器通過無線收發模塊U和輪椅無線收發模塊V通信，通知即將關閉A開啟B，在得到輪椅傳過來的確認信息后，啟動B關閉A。此時B發射的信號被輪椅側接收模塊L和R接收，經過硬件信號調理，進入輪椅處理器完成傳輸時間tbl、tbr提取，得到BR的距離dbr和BL的距離dbl表達式如(1)和(2)所示。

(1)

(2)

c=331.5+0.607K

(3)

上式(3)中c為聲速，等式表達了當前時刻環境溫度K的聲速c的補償關系。

R、L傳感器中點的坐標，以及輪椅的航向角，就可以準確測算相對床體輪椅的位姿。定義x軸負向逆時針與RL中垂線夾角定義為輪椅航向角θ。在RAB三點組成的三角形RAL中，由幾何關系得

(4)

(5)

令y軸正向順時針與AB邊與夾角為α，y軸正向順時針與AR與夾角為β，同時結合床椅機器人已確定傳感器的位置分布，可得

α=∠RAB+β

(6)

(7)

(8)

于是得到R(xr,yr)為

(9)

同理得到L(xl,yl)位置為

<1)，且各件產品是否為不合格品相互獨立．

(10)

于是得到位姿觀測量

(11)

3.2.3 IMU定位原理

基于IMU航位推算是一種以牛頓力學為基礎的相對定位方法，六軸IMU包括三軸加速度和三軸角速度信息，對加速度的一次積分得到速度，二次積分得到相對位移，角速度一次積分可以獲得旋轉角度。利用角度和速度信息計算得到載體坐標系下的位姿?；贗MU的位置推算不受外部環境影響，更新速率快短時間精度高，全天侯定位，在超聲定位非視距誤差較大情況下，可以對系統的位姿進行校正，當超聲定位置信度較高，可以校準航位推算的初始值。

(12)

其中C1、C2、C3分別為

(13)

3.2.4 編碼器信號定位實現

編碼器是一種精確的位移信號測量傳感器，在左右驅動輪各安裝一高精度編碼器，輪轂轉動會輸出密集編碼脈沖信號，通過對單位時間編碼信號測量，即可推算出移動輪椅的的位置和航向信息。設左右驅動輪編碼器參數為K線/圈，驅動輪半徑為已知量Rd，左右驅動輪橫向間距D。定義k時刻輪椅位姿為(xm(k)，ym(k)，θ(k))，Δt時間即k+1時刻后位姿為(xm(k+1)，ym(k+1)，θ(k+1))，Δt時間左右輪里程計輸出脈沖分別為Nl(k)，Nr(k)，依據運動學推算得Δt時間左右輪移動距離ΔS1(k)、ΔSr(k)和線速度v(k)、ω(k)旋轉角速度(k)、分別為

(14)

輪椅的移動距離ΔS1r,旋轉角度Δθ(k)和旋轉半徑R(k)為

(15)

由數學知識可得Δt時間輪椅位姿變化量Δxm(k)、Δym(k)

(16)

3.2.5 擴展卡爾曼多源信息融合定位

基于超聲波定位方式存在干擾和非視距誤差等因素，特別目前越來越多電子設備的應用，超聲工作頻率附近的干擾源將直接導致定位系統失靈，致使定位精度下降或直接不能定位。編碼器脈沖定位相對可靠，也存在重要缺陷，例如輪轂打滑將產生較大位移、角度推算誤差，同時也是一種相對定位方法。通常一種定位方法很難考慮周全，通過采用超聲、IMU、輪轂編碼器脈沖以及紅外多傳感器融合方式進行組合導航，為連續、可靠和精確的定位提供可能。常用多傳感融合方法有加權平均法、貝葉斯歸納推理、模糊邏輯、卡爾曼濾波(含擴展型)和神經網絡算法等，可以對測距信息、全局定位信息融合，實現被測對象的全面環境感知和描述。

如圖4所示采用EKF方案的基于多傳感器融合定位方法原理圖，超聲模組構建TOA定位系統，輸出基于測距信息獲得的位置、速度等位姿信息，激光紅外傳感器輸出測距信息，IMU傳感器輸出的加速度、角速度信息，經過捷聯解算模塊獲得輪椅位置、速度、航向角等參量，輪轂編碼器脈沖信號經過推算輸出位姿信息，同時將IMU和編碼器輸出的測距變量做比較后輸出偽距率信息，以上各類信息進入卡爾曼濾波器進行濾波運算融合，最終輸出最優定位結果。此外，EKF濾波器輸出位姿補償信息需要對編碼器位姿解算和IMU推算模塊進行反饋補償。

圖4 基于EKF的融合定位框圖

3.3 運動控制設計

3.3.1 控制模型

移動輪椅相對位姿可通過多傳感器測算融合獲取，輪椅已知位姿情況下導航到目標位置還需要精準對接控制方法，也應具有一定的魯棒性，整個系統核心部分是對接控制器和融合定位算法。如圖5所示為閉環對接控制模型框圖，采用雙閉環控制策略，系統期望的輪椅位姿與融合定位系統輸出的觀測位姿量進行比較，比較量輸入到對接控制器，對接控制器包括路徑規劃和運動控制算法，路徑規劃模塊獲得位姿比較量，輸出下一目標位姿參考量，運動控制算法模塊接收位姿參考量，運用基于PID的多閉環控制策略，應用inv-park、park進行電壓變換、clark進行電流變換等運算方法[5]，輸出旋轉空間PWM信號量驅動輪轂電機運轉，通過安裝在電機上的編碼器反饋的編碼脈沖信號，一方面與超聲紅外傳感器系統、IMU進行組合融合定位輸出觀測位姿量，一方面輸入到對接控制器，及時參與電機的閉環控制。最終完成超低速狀態下，簡潔可靠的高精準對接。

圖5 對接閉環控制模型圖

閉環控制模型的核心在于對接控制器，對接控制器主要包括路徑規劃和運動控制算法。路徑規劃算法有人工勢場、柵格法、蟻群法和神經網絡法等[6]，柵格法包括Dijkstra、A-star和D-star。床椅機器人對接的環境相對不復雜，規劃區域空間較小，路徑規劃可采用正向搜索方式的A-star算法比較合適，A-star是一種啟發式搜索算法，是目前最穩定和高效的算法之一。運動控制模塊采用伺服技術，應用基于PID算法的位置、速度、電流三環控制，PID參數的整定方法對控制精度和穩定性影響很大?；谏瞎澗幋a器脈沖信號反饋的推算公式，對公式進行微分和拉式變換，可得輪椅的運動學模型[7]，根據運動學模型進行反饋控制。

3.3.2 床椅對接設計

由于床椅對接的特點是對接環境可靠簡單無障礙物，對接精度要求非常高，一般不用考慮中間障礙物或不同房間非可視定位線路規劃問題。根據需要可采用不同對接控制策略，分別對效果進行評估。本研究依據對接環境的特點，提出一種多目標點區間分段對接的簡潔高效方案，以{x=d1，y=d2，d1和d2為設定正常數}將對接區域分為Ⅰ、Ⅱ、Ⅲ、Ⅳ，根據經驗選取已知的6個較小圓形目標區域Wi(Ni，ri)(i=1,2,…,6)，Ni為預設坐標點，ri為區域半徑。開始自動對接區域的起始條件M需滿足x>d1(Ⅰ和Ⅳ)。整個對接區域被分為W1W2、W2W3、W3W4、W4W5、W5W6五個區間段。對接基本過程描述如下。

(1)起始位姿檢測：輪椅接收用戶的指令，以ABRL和ACRL傳感器組循環組合定位，因傳感器角度無法定位時可旋轉左右驅動輪，直至有一組可靠位置結果輸出。判讀輪椅所處區域位置是否為Ⅰ和Ⅳ。

(2)航向調整：所處區域為Ⅳ時，對接控制系統開始工作，輪椅自主移動到W1區域并調整航向角滿足(0，π/2)，即M點在W1內(下同)進入下一步。當所處區域位于Ⅰ時直接進入下一步。

(3)自主循跡導航：對接控制系統持續工作，輪椅自主導航移動到W2區域(N2橫坐標xw2=d1)，然后自主導航移動到W3，再移動到W4區域(N4縱坐標yw2=d2)，然后繼續自主移動到W5。

(4)橫向末程對接：在W5區域，輪椅側邊已經很接近床體側邊，航向角接近90度，此時過大或滯后調整輪轂運動，將可能導致輪椅床體碰擦。實驗結果證明，此時應微調G、H與床體側面的距離值為主要目標，通過控制輪轂達到較為理想的效果。設d3、d4為合適距離常數，ε1、ε2為合適較小誤差正常數，控制輪椅滿足下式持續移動。

(17)

(5)縱向末程對接：設d5為合適距離常數，ε3為合適較小誤差正常數，當輪椅狀態測量參數滿足下式，輪轂電機停止運動，物理剎車裝置啟動，對接結束。

|dbr+dbl-2d5|≤ε3

(18)

(6)結束：通過輪椅向床體發出關閉尾門指令，床體關門，輪椅自動變形和床體合為整體，完成整個合體過程。

4 系統驗證

以多傳感器信息融合定位的方法提高了定位精度和定位可用性，對接精度可達厘米級；以雙閉環控制策略完成了移動輪椅和床體柔性、安全和精準的自主對接。為驗證導航定位精度和對接控制效果的可行性，研究人員準備了硬件和結構驗證平臺，實驗在研發部門開發的樣機上進行，如圖6實物圖，建立以床體為參考的坐標系，將移動輪椅置于表1所示的不同起始位置和航向角做了8組實驗，用秒表對時間進行計時，用角度儀測量起始航向角和對接間隙，利用卡尺對橫向和縱向誤差進行測量。表1為對接實驗數據統計，從實驗結果來看，對接完成時間能很好保證2 min以內，且橫向和縱向對接精度都達到了厘米級。本次8組實驗全部一次對接成功，此外大量測試結果表明，一次對接成功率能達到90%以上。

圖6 對接驗證平臺實物圖

表1 床椅機器人對接實驗結果

5 討論

床椅自主對接可研究的定位方式還包括基于磁導航循跡、基于圖像標記識別、激光雷達建圖定位等。(1)基于磁導航定位技術[8]在傳統AGV定位中應用較為成熟，一般定位精度良好，但對場地環境部署要求非常高，應用在床椅機器人中需在地面貼磁性條狀物，在輪椅的前向和后向均需安裝磁導航控制器識別引導床椅進行合并，很大程度上解決了人工輔助的合并問題，但問題也比較突出，一，貼在地面的磁性條狀物和床體發生相對位置偏移時，將無法保證輪椅順利合并，甚至出現碰撞風險；二，移動輪椅移動到導航磁條的起始端也是非常的難，依靠受護人在看不見地面磁條位置的情況下移動到起始端效率非低下；另外磁導航傳感器和磁導航控制器本身成本也較高。(2)基于圖像識別定位導航的方式在目前應用也比較廣泛，某高校研究所將其在床椅機器人中進行應用，此方案一般都需要在地面貼顯著性易識別的標記物，如純色的條帶、二維碼[9]等，采用攝像頭識別明顯標記物，然后采用基于圖像處理定位和導航控制算法，完成輪椅和床的自動合并，這種技術和磁導航技術一樣，一存在標記物和床體發生相對位移，無法完成床椅自動合并；另一方面，基于圖像識別的定位精準度可靠度可用性也滿足不了要求，如采用高分辨率的專用相機，算力要求較高，相機和處理器的成本也相對較高。(3)采用激光雷達傳感器以slam(Simultaneous localization and mapping，同步定位與建圖)定位技術方案[10]，需要采用激光雷達等硬件成本高，且需事先建圖使用受限，另外其精度約5cm，不能達到對接精度要求，還需要依賴其它傳感器輔助。

床椅機器人應用對象是失能或需康復人群，對接后需要是一張完整可靠的床，因此使用安全性和對接精度要求非常高。自主對接的精度和安全性關鍵在于導航定位持續可用性、運動控制策略適配以及電機的精準控制。本文對基于多源傳感器信息融合定位技術進行研究，應用超聲傳感器、紅外激光、脈沖編碼器、民用IMU等多類低成本傳感器進行多系統融合定位，提高了定位系統輸可用性和定位精度；研究了雙閉環對接控制方法，使床椅對接更加柔性、安全、可靠，具有跟隨床體移動的對接系統成功率更高。相對于上述磁導航、圖像識別、激光slam等技術方案，本研究技術方法在對接成本、性能和魯棒性都具有優勢：(1)采用的是普通超聲模組、紅外以及民用級IMU等低成本傳感器完成機器狀態的感測和定位，沒有激光雷達、專用攝像頭、高性能處理器等，方案成本低；(2)相對于slam定位精度不能滿足要求，采用多源融合定位方式，定位結果可靠性、可用性和精度高，能滿足使用要求；(3)相對于磁導航控制和基于圖像識別方式，不在床椅、地面之外的環境添加額外的標記物、設備等，對地面設施部署要求點，適用環境要求低，場景適用性強；(4)具有優秀的魯棒適應性，以床體為參考系，移動輪椅隨床體位置移動而自適應調整軌跡，對于床體位置經常有移動的場景，仍然可以完成高精度、高成功率的自動對接，大大提高了對接的可用性。

6 結語

本文采用超聲、紅外激光、IMU、編碼器脈沖等傳感器信息進行多傳感融合的定位解算，以床體為參考坐標系完成輪椅的位姿推算，通過輪轂控制器實現床椅自主對接。此種床椅機器人自主對接技術和方法，有效降低了算法復雜度和系統硬件成本，提高了系統響應實時性。床椅機器人的廣泛應用，將大大提升失能人群的康護水平，有效緩解社會康養護理員的人力資源缺口，對促進我國全人群健康質量和幸福指數具有重要的社會意義。