多傳感器融合的自主導(dǎo)航護(hù)士助手機(jī)器人的設(shè)計(jì)與實(shí)現(xiàn)

陳再勝，王顏霞

（商丘工學(xué)院，河南 商丘 476000）

醫(yī)務(wù)人員供需不足一直是我國(guó)醫(yī)療系統(tǒng)的一項(xiàng)痛點(diǎn)，2019 年至今，新冠肺炎的成規(guī)模持續(xù)，使得這一痛點(diǎn)雪上加霜，醫(yī)務(wù)人員長(zhǎng)時(shí)間疲憊地工作，對(duì)醫(yī)護(hù)人員的身體和心理健康造成極為不良的影響[1-2]。護(hù)士助手機(jī)器人是一種可實(shí)現(xiàn)自主導(dǎo)航、避障、充電和物品運(yùn)送等的智能移動(dòng)機(jī)器人，可以幫助醫(yī)務(wù)人員承擔(dān)一部分重復(fù)、持續(xù)、規(guī)律性的工作，比如運(yùn)送藥品、手術(shù)耗材工具和其他物品等，緩解醫(yī)務(wù)人員的壓力。

目前，融合多傳感器的自主定位與導(dǎo)航技術(shù)是智能移動(dòng)機(jī)器人領(lǐng)域研究的熱點(diǎn)課題之一[3-5]。慣性導(dǎo)航可以通過(guò)編碼器和陀螺儀直接測(cè)得載體的運(yùn)動(dòng)速度數(shù)據(jù)信息，為機(jī)器人運(yùn)動(dòng)提供必要約束，但編碼器存在累計(jì)誤差，陀螺儀存在z 向漂移；激光雷達(dá)在室內(nèi)建圖和導(dǎo)航時(shí)能獲得較高的精度，但也存在一定的缺陷，比如只能獲得雷達(dá)本身高度平面內(nèi)的定位信息，當(dāng)障礙物超過(guò)掃描范圍時(shí)，激光雷達(dá)將無(wú)法檢測(cè)到[6-8]。

因此，本論文設(shè)計(jì)了一種護(hù)士助手機(jī)器人，該機(jī)器人以樹(shù)莓派為控制核心，融合姿態(tài)傳感器、里程計(jì)、激光雷達(dá)三種導(dǎo)航技術(shù)的同時(shí)采用遠(yuǎn)程控制和智能存取柜系統(tǒng)技術(shù)實(shí)現(xiàn)病人和醫(yī)務(wù)人員之間“無(wú)接觸、少接觸”溝通，并完成規(guī)定的運(yùn)送物品的任務(wù)。

1 機(jī)器人總體方案設(shè)計(jì)

醫(yī)生或者護(hù)士通過(guò)遠(yuǎn)程控制端監(jiān)控機(jī)器人移動(dòng)并發(fā)送指令，實(shí)現(xiàn)機(jī)器人按照要求實(shí)時(shí)定位和導(dǎo)航。同時(shí)，醫(yī)生或者護(hù)士通過(guò)顯示屏與病人遠(yuǎn)距離視頻通話，將與病人信息相對(duì)應(yīng)的二維碼作為存取物品或藥品的唯一識(shí)別碼，只有持有該二維碼的人才能打開(kāi)柜體門(mén)鎖。

護(hù)士助手機(jī)器人運(yùn)送環(huán)境是在半污染區(qū)，主要運(yùn)送藥品、餐品和被服等用品。運(yùn)送流程分為裝藥/物、選定目標(biāo)點(diǎn)、送藥/物、返回零位四個(gè)步驟。

第1 步裝藥/物：護(hù)士在護(hù)士站處置間將藥/物配好后，如果機(jī)器人存取柜清零后第一次使用，掃描二維碼啟動(dòng)超管或普管權(quán)限，機(jī)器人所有柜門(mén)打開(kāi)，將藥/物放進(jìn)柜子。如果是有未送完的藥物但需要再放入藥物時(shí)，直接掃描二維碼一次，機(jī)器人會(huì)隨機(jī)打開(kāi)一個(gè)空柜，護(hù)士可以放入藥物。

第2 步選定目標(biāo)點(diǎn)：有兩種方式，一種是目標(biāo)點(diǎn)全部選定，另一種是根據(jù)需要運(yùn)送的物品接收對(duì)象進(jìn)行選定。

第3 步運(yùn)送藥/物：機(jī)器人根據(jù)選定的目標(biāo)點(diǎn)進(jìn)行全局規(guī)劃，實(shí)現(xiàn)自主導(dǎo)航。

第4 步返回零位：藥/物運(yùn)送完成后，護(hù)士遠(yuǎn)程操控機(jī)器人選擇零位目標(biāo)點(diǎn)，機(jī)器人返回零位。

機(jī)器人總體設(shè)計(jì)方案，機(jī)器人總體采用決策層控制與執(zhí)行層控制相結(jié)合的方式。

決策層控制系統(tǒng)核心為一臺(tái)裝載ROS 系統(tǒng)的上位機(jī)，該上位機(jī)以有線的形式連接MPU、激光雷達(dá)和單目相機(jī)。決策層接收?qǐng)?zhí)行層發(fā)送的底盤(pán)數(shù)據(jù)與激光雷達(dá)數(shù)據(jù)進(jìn)行融合，經(jīng)過(guò)內(nèi)部程序運(yùn)算后將運(yùn)動(dòng)命令發(fā)送給執(zhí)行層。

執(zhí)行層的首要功能是驅(qū)動(dòng)功能，為實(shí)現(xiàn)該功能，執(zhí)行層驅(qū)控系統(tǒng)核心采用了Arduino2560，該芯片將決策層發(fā)送的速度控制命令轉(zhuǎn)換成信號(hào)脈沖傳輸給電機(jī)驅(qū)動(dòng)，以便控制驅(qū)動(dòng)電機(jī)轉(zhuǎn)動(dòng)使機(jī)器人運(yùn)動(dòng)。執(zhí)行層的次要功能是柜體控制，包括二維碼掃碼器、交互面板、門(mén)鎖控制系統(tǒng)和喇叭，其中門(mén)鎖控制系統(tǒng)核心采用STM32 的控制板，控制數(shù)個(gè)帶反饋電控鎖，電控鎖的開(kāi)關(guān)信息會(huì)及時(shí)反饋給門(mén)鎖控制系統(tǒng)，從而反饋至上位機(jī)，供操控人員及時(shí)了解病人取放物的情況。

2 機(jī)器人結(jié)構(gòu)設(shè)計(jì)

總體方案設(shè)計(jì)主要包括底盤(pán)及其載具平臺(tái)、上位機(jī)（樹(shù)莓派主控系統(tǒng)）、自主導(dǎo)航系統(tǒng)、視頻監(jiān)控系統(tǒng)、二維碼存取柜系統(tǒng)、顯示屏和遠(yuǎn)程控制終端等。機(jī)器人整體采用四層結(jié)構(gòu)，充分利用垂直空間，從低到高依次為底層、第二層、第三層和頂層。機(jī)器人底層放置了減速電機(jī)、編碼器、兩個(gè)驅(qū)動(dòng)輪和四個(gè)從動(dòng)輪，兩個(gè)驅(qū)動(dòng)輪差速驅(qū)動(dòng)且相互獨(dú)立，接收底盤(pán)控制板發(fā)出的指令就可以實(shí)現(xiàn)直行或轉(zhuǎn)彎；為了緩沖底層減速電機(jī)和不同路況對(duì)三層控制系統(tǒng)的震動(dòng)沖擊，二層加裝了減震板和5個(gè)彈簧；機(jī)器人第三層放置了激光雷達(dá)、底盤(pán)控制板、電機(jī)驅(qū)動(dòng)器、分線器、穩(wěn)壓器、一鍵制動(dòng)開(kāi)關(guān)、上位機(jī)和MPU 等。上位機(jī)與底盤(pán)控制板、激光雷達(dá)、MPU 通過(guò)USB 串口相連來(lái)進(jìn)行通信；機(jī)器人頂層放置存取柜、電池、單目相機(jī)、顯示屏、鎖系統(tǒng)主控板和操作界面等。

上位機(jī)選用樹(shù)莓派RaspberryPi4ModelB（簡(jiǎn)稱(chēng)4 代B 型）作為主控板，通過(guò)Pype-C 接口實(shí)現(xiàn)5V 供電，通過(guò)CSI 攝像頭接口連接單目相機(jī)，通過(guò)USB2.0/3.0接口接收激光雷達(dá)、IMU、Arduino2560 控制板傳輸?shù)臄?shù)據(jù)，通過(guò)MicroHDMI 接口連接10 寸顯示屏和USB接口連接無(wú)線鍵盤(pán)和鼠標(biāo)接收器實(shí)現(xiàn)人機(jī)交互和界面顯示。

該機(jī)器人設(shè)計(jì)的最大負(fù)載為30kg，選取的車(chē)輪直徑D 為130mm，最大移動(dòng)速度υmax為0.6m/s，經(jīng)計(jì)算所需單個(gè)驅(qū)動(dòng)輪電機(jī)功率為：

根據(jù)以上條件，為使機(jī)器人有足夠的動(dòng)力，選擇直流渦輪蝸桿減速電機(jī)DM08RC-150i11RV12 為機(jī)器人提供動(dòng)力，該電機(jī)具有強(qiáng)勁的驅(qū)動(dòng)能力，且減速比為50，非常適合醫(yī)院低速度運(yùn)行。

3 導(dǎo)航控制系統(tǒng)

3.1 導(dǎo)航控制系統(tǒng)總體設(shè)計(jì)

本護(hù)士助手機(jī)器人自主導(dǎo)航控制系統(tǒng)主要包括底盤(pán)控制系統(tǒng)、SLAM 系統(tǒng)、路徑規(guī)劃系統(tǒng)和可視化人機(jī)交互界面。

3.1.1 底盤(pán)控制系統(tǒng)

底盤(pán)控制系統(tǒng)主要用于控制機(jī)器人運(yùn)動(dòng)的速度和角度。選用ROS 系統(tǒng)中的bringup 功能包從上位機(jī)中人機(jī)交互系統(tǒng)接收操作者給定的速度命令，并通過(guò)PWM脈沖控制驅(qū)動(dòng)電動(dòng)機(jī)旋轉(zhuǎn)；同時(shí)在該功能包中可以獲取驅(qū)動(dòng)電機(jī)的運(yùn)行速度，再通過(guò)電機(jī)編碼器轉(zhuǎn)換為距離數(shù)據(jù)，傳輸?shù)缴衔粰C(jī)；同時(shí)通過(guò)MPU 收集機(jī)器人的角/線速度數(shù)據(jù)后反饋到上位機(jī)。

3.1.2 SLAM 系統(tǒng)

在ROS 系統(tǒng)中包含SLAM 系統(tǒng)包，可以依據(jù)IMU、激光雷達(dá)等位置和姿態(tài)傳感器傳遞的環(huán)境信息，使用相關(guān)算法構(gòu)建環(huán)境地圖，并實(shí)時(shí)測(cè)量機(jī)器人所在的位置和姿態(tài)。

對(duì)比目前室內(nèi)自主導(dǎo)航常用技術(shù)，本設(shè)計(jì)使用三種導(dǎo)航定位技術(shù)實(shí)現(xiàn)自主導(dǎo)航定位。第一種根據(jù)航跡推算法采用里程計(jì)（電機(jī)編碼器）得到機(jī)器人運(yùn)行的距離和角度，實(shí)現(xiàn)局部定位，存在較大的累積誤差；第二種根據(jù)擴(kuò)展卡爾曼濾波（EKF）算法利用 IMU 低噪聲的特性去矯正里程計(jì)的累計(jì)角度偏差，測(cè)量機(jī)器人相對(duì)于初始位置的距離和方向，從而推知機(jī)器人當(dāng)前的位置和下一步的目的地；第三種根據(jù)自適應(yīng)蒙特卡羅算法（Adaptive Monte Carlo Localization，AMCL）將里程計(jì)和IMU的融合數(shù)據(jù)與激光雷達(dá)數(shù)據(jù)再進(jìn)行融合，從而獲得機(jī)器人在已知的全局地圖下高精度的定位。本設(shè)計(jì)選擇粒子濾波的SLAM 方法（FastSLAM）實(shí)現(xiàn)全局地圖構(gòu)建，其中FastSLAM 算法利用EKF 進(jìn)行路標(biāo)點(diǎn)狀態(tài)估計(jì)，利用PF 進(jìn)行機(jī)器人的位姿估計(jì)。

3.1.3 路徑規(guī)劃系統(tǒng)

本文設(shè)計(jì)的護(hù)士助手機(jī)器人在實(shí)際運(yùn)行中，首先，根據(jù)SLAM 系統(tǒng)構(gòu)建的全局地圖，以及操作者設(shè)定的巡航目標(biāo)點(diǎn)，通過(guò)A*算法進(jìn)行總體路徑的規(guī)劃，即全局路徑規(guī)劃，并計(jì)算出機(jī)器人到目標(biāo)點(diǎn)的最優(yōu)路線；其次，在全局路徑規(guī)劃的最優(yōu)路線中，機(jī)器人通過(guò)機(jī)載的傳感器實(shí)時(shí)檢測(cè)機(jī)器人周?chē)欠裼姓系K物，當(dāng)存在障礙物時(shí)機(jī)器人通過(guò)模糊控制算法設(shè)計(jì)出幾條避障路線，并綜合評(píng)優(yōu)，選取最優(yōu)的避障路線，機(jī)器人按照此避障路線進(jìn)行運(yùn)動(dòng)；最后，當(dāng)避障結(jié)束后，還需要判斷機(jī)器人是否在第一步規(guī)劃的全局路徑上，如果在則繼續(xù)運(yùn)動(dòng)直至達(dá)到巡航目標(biāo)點(diǎn)；如果不在原全局路徑上，則重新進(jìn)行全局規(guī)劃，重復(fù)上述步驟直至機(jī)器人移動(dòng)到巡航目標(biāo)點(diǎn)。

3.1.4 可視化人機(jī)交互界面

ROS 系統(tǒng)中具有二維可視化界面工具Rviz，Rviz工具不僅可以實(shí)時(shí)顯示機(jī)器人在運(yùn)動(dòng)過(guò)程中的位置和姿態(tài)，并構(gòu)建實(shí)時(shí)環(huán)境地圖，地圖構(gòu)建成功后可以及時(shí)保存，而且可以設(shè)置機(jī)器人的巡航目標(biāo)點(diǎn)的位置和方向。

3.2 定點(diǎn)巡航系統(tǒng)設(shè)計(jì)

為了更好地適應(yīng)醫(yī)院復(fù)雜的環(huán)境，本護(hù)士助手機(jī)器人采用隨機(jī)坐標(biāo)點(diǎn)和順序坐標(biāo)點(diǎn)兩種無(wú)限巡航模式，同時(shí)為了彌補(bǔ)視頻監(jiān)控必須專(zhuān)人時(shí)時(shí)監(jiān)控的弊端，采用導(dǎo)航時(shí)間限定，當(dāng)超過(guò)設(shè)定時(shí)間后將前往下一個(gè)目標(biāo)點(diǎn)，并記入失敗次數(shù)。

第一步，超管或普管掃描二維碼進(jìn)入管理界面，系統(tǒng)自動(dòng)啟動(dòng)了move_base 節(jié)點(diǎn)后就可以使用Rviz 中的“2D Nav Goal”功能來(lái)獲取到地圖上坐標(biāo)點(diǎn)的坐標(biāo)和朝向，并獲取所有目標(biāo)點(diǎn)的位置信息。

第二步，設(shè)定patrol_type 巡邏模式，選擇是否開(kāi)啟隨機(jī)坐標(biāo)點(diǎn)無(wú)限巡航模式，如果是則選擇隨機(jī)坐標(biāo)點(diǎn)無(wú)限巡航模式，需要指定目標(biāo)點(diǎn)，柜鎖控制系統(tǒng)會(huì)打開(kāi)隨機(jī)柜子；如果選擇否，則選擇順序坐標(biāo)點(diǎn)無(wú)限巡航模式，則無(wú)需指定目標(biāo)點(diǎn)，機(jī)器人將按照順序巡航系統(tǒng)內(nèi)所有的目標(biāo)點(diǎn)，柜鎖控制系統(tǒng)將打開(kāi)所有柜子。同時(shí)需要設(shè)定巡航圈數(shù)或時(shí)間，當(dāng)選擇0 時(shí)，為巡航圈數(shù)模式，需要設(shè)定巡航圈數(shù)；當(dāng)選擇1 時(shí)為巡航時(shí)間，需要設(shè)定巡航時(shí)間。

第三步，放入藥物，關(guān)閉柜門(mén)，柜鎖控制系統(tǒng)將數(shù)據(jù)信息反饋給上位機(jī)，之后機(jī)器人啟動(dòng)巡航。

第四步，首次巡航，當(dāng)機(jī)器人移動(dòng)導(dǎo)航時(shí)間大于5min 時(shí)，將前往下一個(gè)目標(biāo)點(diǎn)，并記錄導(dǎo)航失敗次數(shù)和機(jī)器人位置信息，如果小于5min，將繼續(xù)導(dǎo)航直至到達(dá)目標(biāo)點(diǎn)。

第五步，機(jī)器人到達(dá)目標(biāo)點(diǎn)后等待病人取藥/物，如果等待時(shí)間未超過(guò)15min，則繼續(xù)等待病人；如果超過(guò)15min，則檢查總巡航時(shí)間是否超過(guò)1h，如果未超過(guò)則前往下一個(gè)目標(biāo)點(diǎn)，并記錄；如果超過(guò)則返回零位，并打印導(dǎo)航任務(wù)信息，結(jié)束。

這里的導(dǎo)航超時(shí)時(shí)間、等待時(shí)間和總巡航時(shí)間可以在開(kāi)機(jī)測(cè)試時(shí)根據(jù)醫(yī)院的具體情況測(cè)定。

4 系統(tǒng)測(cè)試

4.1 測(cè)試環(huán)境選擇

為更好地模擬醫(yī)院辦公室和長(zhǎng)走廊環(huán)境，特選擇本校行知苑1 樓作為測(cè)試環(huán)境，并選擇周六周日學(xué)生課少且人行隨機(jī)的時(shí)間段進(jìn)行測(cè)試。

4.2 運(yùn)行結(jié)果分析

該機(jī)器人運(yùn)動(dòng)為低勻速移動(dòng)，最大運(yùn)行速度為0.6m/s，負(fù)載能力不小于15kg，單次運(yùn)行時(shí)間不小于3 小時(shí)。本系統(tǒng)經(jīng)過(guò)測(cè)試，機(jī)器人自主導(dǎo)航定位能夠達(dá)到導(dǎo)航精度最大橫向偏差±0.25m，最大偏角±5°，剎車(chē)靈敏，運(yùn)行可靠，移動(dòng)過(guò)程中未發(fā)生碰撞。

5 結(jié)語(yǔ)

本文設(shè)計(jì)的護(hù)士助手機(jī)器人，采用AMCL 算法將MPU、里程計(jì)、激光雷達(dá)三種導(dǎo)航數(shù)據(jù)融合，實(shí)現(xiàn)高精度的導(dǎo)航定位，并采用A*算法和模糊控制理論實(shí)現(xiàn)全局和局部路徑規(guī)劃，進(jìn)而實(shí)現(xiàn)定點(diǎn)或多點(diǎn)巡航。同時(shí)采用遠(yuǎn)程控制和智能存取柜系統(tǒng)技術(shù)實(shí)現(xiàn)病人和醫(yī)務(wù)人員之間“無(wú)接觸、少接觸”溝通，并完成規(guī)定的運(yùn)送物品的任務(wù)，從而減輕醫(yī)院隔離病房醫(yī)務(wù)人員的工作壓力，緩解醫(yī)患關(guān)系。