基于激光雷達的嵌入式護理床自動對接系統*

李素姣，楊皓文，孟巧玲△，王海濤，喻洪流

(1.上海理工大學康復工程與技術研究所，上海 200093;2.上海康復器械工程技術研究中心，上海 200093;3.民政部神經功能信息與康復工程重點實驗室，上海 200093)

1 引 言

據研究，目前我國失能老人已經達到4 200萬左右，預計2030年將達到6 000萬[1]。在2015年，我國老年人平均帶殘存活時間約為2～3年，老年人產生失能問題的比例達到10.48%～13.31%，自主移動困難[2]。為解決失能患者自主移動問題，一些高校和研究機構研發了智能可分離式護理床，其中床椅的自動對接是保障失能患者安全歸位的關鍵技術之一。

為解決智能可分離式護理床的自動對接技術，國內外學者投入了大量的研究工作。Mascaro等[3]最早提出了可重構的輪椅床系統，該系統基于力傳感器的對接控制策略，通過力傳感器逐步調整床體的姿態從而實現床椅對接[4]；Zou等[5]、Ye等[6]先后提出的兩種對接方式均采用安裝在輪椅上方的視覺傳感器，并采用直線跟蹤算法實現床椅的對接；胡木華等[7]采用激光雷達對接與巡線對接的方式，并結合開門式，旋轉座椅式等輪椅動作，實現了床椅的側方分離對接；李秀智等[8-9]提出了一種基于視覺與超聲波數據融合的床椅側邊對接策略，利用標定點視覺傳感器確定水平位置中心，超聲波傳感器獲取當前深度信息關系，借此獲取床椅相對位姿信息。綜上，目前床椅對接仍以側方對接方式為主，單側對接的結構問題不利于失能患者翻身，且人工挪動易造成二次傷害[10-11]。在床椅對接策略方面，基于力傳感器的床椅對接，對于初始床椅位置、姿態和接觸點具有嚴格要求，對接準確率低；超聲波與紅外測距傳感器提供的深度信息較少，精度較高的對接任務往往難以完成；基于視覺的床椅對接，需要在使用環境中預設標準點，并在床椅上方安裝視覺傳感器，對光線使用環境要求較高。

針對以上問題，本研究設計了一種基于激光雷達的嵌入式床椅自動對接系統，床椅對接分為局部定位和精準對接兩步。前期利用智能輪椅自主導航移動至床體框架前部目標區域，到達目標點，后向激光雷達對預置的床體人工標志進行特征提取與局部定位，保證對接定位的精度和魯棒性，同時提出相應的PID路徑跟蹤算法，保證床椅對接過程中的穩定性，從而實現床椅的安全精準對接。

2 機構和策略

2.1 床椅對接結構

為避免患者移動造成二次傷害，并實現精準對接，設計的輪椅床為嵌入式結構，見圖1。整體分為床體模塊和輪椅模塊，并進行相關的有限元和運動學分析，驗證了機械結構的合理性和可行性。在床體中部安裝輔助激光雷達定位的人工標志反光板(見圖2)，激光雷達反光板為兩邊等長的且成90°夾角的雙邊形結構為激光雷達定位提供了充分的角點與直線特征，定位過程中只需通過確定雙邊的直線結構方程，求解出角點位置便可獲取到當前床椅相對位姿，從而控制輪椅進行對接動作。此外，利用雙邊呈90°這個先驗知識，可對前一步獲取到的雙邊結果進行驗證，此結構極大簡化了激光雷達數據處理難度，提高了定位算法的魯棒性。

圖1 床椅機械結構Fig.1 Mechanical structure of bed chair

圖2 反光板結構Fig.2 Reflector structure

2.2 床椅對接策略

2.2.1局部定位算法 激光的掃描匹配即通過求解坐標轉換關系,將連續掃描的兩幀或多幀激光點云統一到同一坐標系中(scan-to-scan)，或者將當前掃描點云與已建立的地圖進行配準(scan-to-map)，從而最終恢復出載體的位置和姿態的變化[12-13]。目前從測距傳感器中提取的特征大多是線段、角點或圓等幾何基元，直線與角點在特征提取的過程中擁有計算簡單、魯棒性高等特點，故采用基于直線與角點特征的激光匹配方法[14-15]。未知環境下的直線提取有三個主要問題：有多少條線；哪些點屬于哪條線；給定屬于直線的點，如何估計直線模型參數。針對直線提取中的三個主要問題，為保證床椅自動對接過程中的定位精度與速度，本研究在床體框架中預置了雙邊直角結構，以增加先驗的直線與角點信息，即直線的長度與直線夾角的角度，從而解決了直線提取過程中的前兩個問題。對于第三個問題，采用最小二乘法進行對已提取的直線進行線性擬合，估計直線的模型參數。常見的激光雷達直線數據提取算法在處理不同情況的問題過程中各有優劣[16]，本研究從復雜性、計算速度、正確性以及精確性對以下6種算法進行評估，研究結果見表1。

表1 二維激光雷達直線提取算法比較Table 1 Comparison of two-dimensional lidar straight line extraction algorithms

由表1可以看出，分割合并法在運算速度、錯誤率等方面都有較好表現，合并分割算法的實現見圖3，主要包含6個步驟：

圖3 合并分割算法Fig.3 Merge segmentation algorithm

步驟1：初始化集合S1由N個點組成。將S1放入列表L。

步驟2：將一條直線L擬合到中的下一個集合Si。

步驟3：檢測距離直線最遠距離的dP的點P。

步驟4：如果dP小于一個閾值，繼續(轉到步驟2)。

步驟5：否則將在P的Si分裂為Si1和Si2，并以Si1和Si2取代L中的Si，返回步驟2。

步驟6: 當L中的所有集合(直線段)都被檢測出，合并共線段。

(1)

(2)

輪椅床框中預設了V型人工標志，在利用完成數據濾波后，采用分割合并的方法能夠快速而簡潔的對激光雷達目標特征的提取。見圖4，在激光雷達坐標系中，激光雷達數據以及人工標志的位置，在經過合并分割算法后，采用最小二乘法對激光雷達數據進行數據擬合從而得到激光雷達坐標系中預置人工標志中的兩個邊的的直線方程L1、L2，并通過對直線方程的處理獲得L1L2交點O的坐標，即人工標志中的直角在激光雷達坐標系的坐標，以及L1L2兩直線夾角的角平分線LP，并可以獲得直線LP與輪椅目標位姿之間的夾角θ，這是進行輪椅對接運動控制的重要參數。

圖4 激光雷達數據處理Fig.4 Lidar data processing

利用合并分割算法對激光雷達的數據處理后，為進一步實現局部定位，即智能輪椅與床體框架的相對位置關系，需要進行激光雷達與坐標系和機器人坐標系之間的坐標轉換，通過位姿變換后，得到圖5，以反光板頂角為中心建立直角坐標系，則智能輪椅在該坐標系下的位姿坐標可以用 (x，y，θ)進行表示，坐標中的x、y、θ與圖4中x、y、θ的一致，經過坐標系變換之后保持了一致性。可以看出θ表示了智能輪椅朝向與理想位姿的角度偏差，x表示智能輪椅當前理想位姿的水平偏差，y表示了智能輪椅與理想位姿的垂直距離偏差。由此便獲得智能輪椅與床體的相對位置關系，通過運動控制算法可實現實床椅的自動對接。

圖5 局部定位示意圖Fig.5 Schematic diagram of local positioning

2.2.2精準對接控制 對接過程中采用后向行駛，以實現床椅的對接。本系統中護理床采用的中央嵌入式結構，在保證患者的床椅分離便捷性的同時，也對床椅對接的控制算法提出了更高的要求。傳統的控制算法中常采用規劃對接路徑后，采用路徑跟蹤算法實現控制[19]，但由于規劃路徑的復雜性，使得路徑跟蹤效果一般都很差。本系統中，床椅對接動作在執行前，采用智能導航方式使得智能輪椅運動到床體前端中部，因此床椅對接的距離較近，對接運動距離較短。故直接采用PID控制算法將智能輪椅位姿坐標(x,y,θ)的加權值作為控制輸入從而控制智能輪椅實現床椅對接。采用反饋控制算法避免了路徑跟蹤精度較差的問題，同時保證了對接的流暢性。而直接采用PID進行對接控制，勢必會出現在控制初期大幅震蕩后趨于平穩的現象，這對患者在使用體驗的過程中是極差的，因此我們將偏差函數定義為：

(3)

式中，誤差函數與智能輪椅位姿、理想位姿的偏差值θ、x的加權值成正比，與y成反比，有效地避免了控制前期大幅震蕩的問題。

3 實驗結果與分析

為驗證床椅對接有效性，本研究從激光雷達數據處理和床椅對接兩個方面進行定性的驗證。

在實現床椅自動對接的過程中，局部定位的精度是關鍵，而本系統采用的分割合并算法在預置人工標志的基礎上取得了較好的效果。圖6(a)為激光雷達數據在可視化界面Rviz中的顯示，激光雷達的掃描角度270°，掃描間隔為1°，得到270個掃描點；通過采用機器人操作系統(ROS)[20]自帶的濾波算法實現對激光雷達數據的濾波[21]，濾波結果見圖6(b)，有明顯間斷點的數據被有效濾除；并采用合并分割算法對濾波后的數據進行處理，見圖6(c)，完成分割后的直線采用不同顏色進行標志，可以看出，該方法有效地對直線與角點數據進行了提取；圖6(d)通過對預置人工標志上激光雷達數據進行紫色標注，與其他激光雷達點進行區分，將人工標志中心位置通過較大顆粒進行標注，可以看出本研究所采用的激光雷達算法在實際的運用過程中依然保證了較高的穩定性與精度。

圖6 激光雷達處理可視化(a).激光雷達數據;(b).數據濾波;(c).特征提取;(d).對接結果Fig.6 Visualization of lidar processing(a).lidar data;(b).data filtering;(c).feature extraction;(d).docking result

本研究采用基于反饋的PID控制算法直接對智能輪椅當前位置坐標與理想位置坐標進行反饋控制。在床椅對接過程中，床椅通過自主導航運動到床體前方，見圖7(a)，并開始自動對接過程。開啟對接動作后，對激光雷達數據進行處理并獲得智能輪椅局部定位信息，即床椅相對位姿，通過PID算法進行自身位姿調節，見圖7(b)，智能輪椅以較快的速度平穩地向床體中心位置靠攏，通過不斷地調整自身位姿順利進入床體，見圖7(c)，并最終完成床椅的對接動作，整個過程中，智能輪椅保證平穩運行，且完成對接后智能輪椅位于床體中部，并與床體保證較小的間隙，見圖7(d)。

圖7 床椅對接過程(a).完成自主導航動作;(b).開始床椅對接動作;(c).進入床體;(d).完成對接Fig.7 Docking process of bed and chair(a).complete autonomous navigation;(b).start the bed-chair docking action;(c).enter the bed body;(d).complete docking

4 結論

針對當前輪椅床結構和對接問題，本研究提出了一種基于激光雷達的嵌入式床椅自動對接系統，重點研究了床椅自動對接過程中的局部定位算法與運動控制算法，局部定位算法則重點研究了對激光雷達數據直線特征的提取與擬合，通過合并分割算法結合預置人工標志有效地提高了直線提取的速度與魯棒性。運動控制算法則直接采用了PID控制算法，但結合對誤差函數的處理依然起到了很好的控制效果。最后通過實驗驗證了本研究所提出的基于激光雷達的中央嵌入式床椅對接策略的有效性，保障了床椅的結構安全性、對接準確性，一定程度上解決了失能患者的自主移動問題。