基于激光雷達(dá)的嵌入式護(hù)理床自動對接系統(tǒng)*

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

(1.上海理工大學(xué)康復(fù)工程與技術(shù)研究所，上海 200093;2.上?？祻?fù)器械工程技術(shù)研究中心，上海 200093;3.民政部神經(jīng)功能信息與康復(fù)工程重點(diǎn)實(shí)驗(yàn)室，上海 200093)

1 引 言

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

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

針對以上問題，本研究設(shè)計(jì)了一種基于激光雷達(dá)的嵌入式床椅自動對接系統(tǒng)，床椅對接分為局部定位和精準(zhǔn)對接兩步。前期利用智能輪椅自主導(dǎo)航移動至床體框架前部目標(biāo)區(qū)域，到達(dá)目標(biāo)點(diǎn)，后向激光雷達(dá)對預(yù)置的床體人工標(biāo)志進(jìn)行特征提取與局部定位，保證對接定位的精度和魯棒性，同時(shí)提出相應(yīng)的PID路徑跟蹤算法，保證床椅對接過程中的穩(wěn)定性，從而實(shí)現(xiàn)床椅的安全精準(zhǔn)對接。

2 機(jī)構(gòu)和策略

2.1 床椅對接結(jié)構(gòu)

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

圖1 床椅機(jī)械結(jié)構(gòu)Fig.1 Mechanical structure of bed chair

圖2 反光板結(jié)構(gòu)Fig.2 Reflector structure

2.2 床椅對接策略

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

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

由表1可以看出，分割合并法在運(yùn)算速度、錯(cuò)誤率等方面都有較好表現(xiàn)，合并分割算法的實(shí)現(xiàn)見圖3，主要包含6個(gè)步驟：

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

步驟1：初始化集合S1由N個(gè)點(diǎn)組成。將S1放入列表L。

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

步驟3：檢測距離直線最遠(yuǎn)距離的dP的點(diǎn)P。

步驟4：如果dP小于一個(gè)閾值，繼續(xù)(轉(zhuǎn)到步驟2)。

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

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

(1)

(2)

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

圖4 激光雷達(dá)數(shù)據(jù)處理Fig.4 Lidar data processing

利用合并分割算法對激光雷達(dá)的數(shù)據(jù)處理后，為進(jìn)一步實(shí)現(xiàn)局部定位，即智能輪椅與床體框架的相對位置關(guān)系，需要進(jìn)行激光雷達(dá)與坐標(biāo)系和機(jī)器人坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換，通過位姿變換后，得到圖5，以反光板頂角為中心建立直角坐標(biāo)系，則智能輪椅在該坐標(biāo)系下的位姿坐標(biāo)可以用 (x，y，θ)進(jìn)行表示，坐標(biāo)中的x、y、θ與圖4中x、y、θ的一致，經(jīng)過坐標(biāo)系變換之后保持了一致性?？梢钥闯靓缺硎玖酥悄茌喴纬蚺c理想位姿的角度偏差，x表示智能輪椅當(dāng)前理想位姿的水平偏差，y表示了智能輪椅與理想位姿的垂直距離偏差。由此便獲得智能輪椅與床體的相對位置關(guān)系，通過運(yùn)動控制算法可實(shí)現(xiàn)實(shí)床椅的自動對接。

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

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

(3)

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

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

為驗(yàn)證床椅對接有效性，本研究從激光雷達(dá)數(shù)據(jù)處理和床椅對接兩個(gè)方面進(jìn)行定性的驗(yàn)證。

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

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

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

圖7 床椅對接過程(a).完成自主導(dǎo)航動作;(b).開始床椅對接動作;(c).進(jìn)入床體;(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 結(jié)論

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