家用智能看護(hù)機(jī)器人高精度里程計估計算法

余雷　劉怡明　張靈霖　徐揚

摘要：為了解決家用智能看護(hù)機(jī)器人車輪輪胎和地面作用力與電機(jī)輸出力矩不平衡，易導(dǎo)致看護(hù)機(jī)器人產(chǎn)生滑動、里程計估計精度低的問題，引入牽引系數(shù)描述機(jī)器人滑動情況，推導(dǎo)含牽引系數(shù)的看護(hù)機(jī)器人運動學(xué)模型，以增量式光電編碼器和慣性測量單元2種傳感器為輸入，將含牽引系數(shù)的運動學(xué)模型應(yīng)用于基于擴(kuò)展卡爾曼濾波算法的看護(hù)機(jī)器人里程計估計算法，搭建看護(hù)機(jī)器人實驗系統(tǒng)完成算法驗證。結(jié)果表明：在家用瓷磚地面，看護(hù)機(jī)器人分別以0.1，0.2，0.4 m/s的速度移動時，與傳統(tǒng)里程計估計算法相比，所提出的機(jī)器人里程計估計算法的誤差降低了40%左右。將含牽引系數(shù)的運動學(xué)模型應(yīng)用于機(jī)器人里程計估計算法，可有效降低看護(hù)機(jī)器人的里程計估計誤差，為提高看護(hù)機(jī)器人在室內(nèi)地面的自主導(dǎo)航精度提供了一定的參考。

關(guān)鍵詞：機(jī)器人控制;智能看護(hù)機(jī)器人;里程計估計;多傳感器融合;牽引系數(shù)

中圖分類號：TP242.6文獻(xiàn)標(biāo)識碼：A

DOI：10.7535/hbkd.2022yx03006

High precision odometer estimation algorithm for household intelligent nursing robot

YU Lei，LIU Yiming，ZHANG Linglin，XU Yang

（School of Mechanical and Electrical Engineering，Soochow University，Suzhou，Jiangsu 215031，China）

Abstract：In order to solve the problem that the imbalance between wheel tire of home nursing robot and the ground force and the motor output torque is easy to the sliding of the robot and the low accuracy of odometer estimation，the traction coefficient was introduced to describe the sliding condition of the robot，and the kinematic model of robot containing traction coefficient was deduced.Two sensors of incremental photoelectric encoder and inertial measurement unit were adopted as inputs to apply the kinematic model containing traction coefficient in the odometer estimation algorithm based on extended Kalman Filter algorithm.Finally，a robot experimental system was formulated to complete the algorithm verification.The results indicate that when the robot moves at the speed of 0.1，0.2，0.4 m/s on home ceramic ground，the estimation error of the proposed robot odometer is reduced by about 40% compared with the traditional odometer estimation algorithm.Applying the kinematic model with traction coefficient to the robot range meter estimation algorithm can effectively reduce the odometer estimation error of the nursing robot，thus providing a certain reference for the improvement of the independent navigational accuracy of the home nursing robot on indoor grounds.

Keywords：

robot control;intelligent nursing robot;odometer estimation;multisensory fusion;traction coefficient

隨著人口老齡化問題的日益緊迫，獨居老人在家中發(fā)生意外時無法自救的問題愈發(fā)引起社會的廣泛關(guān)注[1]。在這種背景下，基于物聯(lián)網(wǎng)技術(shù)和機(jī)器人技術(shù)的家庭智能看護(hù)機(jī)器人逐漸得到人們的認(rèn)可[2-3]。然而，目前家庭智能看護(hù)機(jī)器人的研究仍處于初期階段，實現(xiàn)看護(hù)機(jī)器人在家庭復(fù)雜室內(nèi)環(huán)境下的精準(zhǔn)導(dǎo)航依然是該領(lǐng)域極具挑戰(zhàn)性的研究方向之一?？醋o(hù)機(jī)器人能夠?qū)崿F(xiàn)精準(zhǔn)導(dǎo)航主要依賴于機(jī)器人的狀態(tài)估計、控制算法、路徑規(guī)劃算法和環(huán)境感知能力，其中，機(jī)器人的狀態(tài)估計精度是決定看護(hù)機(jī)器人導(dǎo)航精準(zhǔn)程度的基礎(chǔ)，里程計是獲知機(jī)器人位姿和速度、實現(xiàn)機(jī)器人定位與導(dǎo)航的重要信息來源。家庭地面材質(zhì)比較光滑，極易引起輪胎與地面間的作用力同電機(jī)輸出力矩之間的不平衡，導(dǎo)致機(jī)器人產(chǎn)生滑動。傳統(tǒng)的機(jī)器人運動學(xué)模型并未考慮滑動問題，對機(jī)器人在空間移動時的狀態(tài)（如位置和方向）估計往往有偏差，導(dǎo)致機(jī)器人里程計估計發(fā)生偏差，降低了看護(hù)機(jī)器人的自主導(dǎo)航精度。此外，由于人們往往會低估真實世界中機(jī)器人狀態(tài)估計的難度，相對于其他方面而言，對家用智能看護(hù)機(jī)器人高精度里程計估計算法的研究相對較少[4-5]。因此，研究看護(hù)機(jī)器人受滑動影響時的高精度里程計估計算法十分必要。

看護(hù)機(jī)器人的里程計估計誤差無法徹底消除，且會隨著時間的增加而發(fā)散。為了抑制里程計發(fā)散，目前最常用的方法是通過編碼器、慣性測量單元[6-9]、GPS[10-12]和視覺傳感器[13-17]等多傳感器的融合，實現(xiàn)傳感器間的特性互補(bǔ)，進(jìn)而降低估計誤差。但是，當(dāng)輪胎與地面的滑動效應(yīng)較為嚴(yán)重時，用于多傳感器融合的傳統(tǒng)機(jī)器人運動學(xué)模型同樣會產(chǎn)生偏差，導(dǎo)致里程計估計精度降低。目前，處理機(jī)器人運動學(xué)模型偏差的主要方法是使用高斯白噪聲進(jìn)行誤差補(bǔ)償，但這種假設(shè)與真實情況偏差較大，估計精度提升較小。REICHARD等[18-19]提出了引入瞬時轉(zhuǎn)動中心點實時估計機(jī)器人的動力學(xué)模型，使用擴(kuò)展卡爾曼濾波器對中心點進(jìn)行實時估計，但計算量較大。

本文在看護(hù)機(jī)器人運動學(xué)模型推導(dǎo)過程中，引入一個牽引系數(shù)，應(yīng)對機(jī)器人滑動產(chǎn)生的影響。該牽引系數(shù)與地面和輪胎之間的作用力有關(guān)，將隨著地面材質(zhì)的不同發(fā)生變化，但對同一種地面，該牽引系數(shù)近似為常數(shù)。為了驗證牽引系數(shù)對機(jī)器人里程計估計精度的影響，將該模型應(yīng)用于基于多傳感器融合的里程計估計算法進(jìn)行了對比。

1家用看護(hù)機(jī)器人運動學(xué)模型

家用看護(hù)機(jī)器人可近似為一種四輪差速移動機(jī)器人，本文只考慮看護(hù)機(jī)器人在二維平面的運動，其運動模型示意圖如圖1所示。

2.1基于編碼器的里程計估計

2.2基于慣性測量單元的機(jī)器人姿態(tài)估計

2.3基于擴(kuò)展卡爾曼濾波的里程計估計

為實現(xiàn)編碼器和慣性測量單元融合，本設(shè)計使用擴(kuò)展卡爾曼濾波算法[20]，該算法便于編程的實現(xiàn)，且能夠?qū)ΜF(xiàn)場采集的數(shù)據(jù)進(jìn)行實時更新處理，在里程計估計中運用比較成熟，已成為當(dāng)下應(yīng)用十分廣泛的濾波方法。因此，本文選用此方法進(jìn)行實驗對比分析。擴(kuò)展卡爾曼濾波一共分為5個步驟。

3實驗平臺設(shè)計與結(jié)果分析

3.1實驗平臺設(shè)計

為了驗證本文算法的有效性，選用Autolabor Pro通用底盤、Jetson TX2主控制器、ZKP3808-001G-2500BZ1-5L增量式光電編碼器和Xsens公司的MTi-300慣性測量單元搭建看護(hù)機(jī)器人，用于室內(nèi)環(huán)境下的應(yīng)用測試。

實驗場地如圖4所示，室內(nèi)地面為瓷磚地面?？醋o(hù)機(jī)器人使用式（7）—式（9）所示的傳統(tǒng)運動學(xué)模型和式（16）—式（18）所示的考慮機(jī)器人滑動的動力學(xué)模型，分別以0.1，0.2，0.4 m/s的速度，環(huán)繞場地內(nèi)4個標(biāo)定盒，行走5圈后回到出發(fā)原點，行駛距離約65 m。為了確保實驗的一般性，每種設(shè)定速度下，均使看護(hù)機(jī)器人重復(fù)運行5次，并取數(shù)據(jù)平均值作為測量結(jié)果。盡管每次實驗過程中機(jī)器人的行進(jìn)路線存在著一定的不一致，但最終機(jī)器人都回到了與初始位置相同的原點。

實驗過程中，針對不同的地面材質(zhì)使機(jī)器人本體繞重心旋轉(zhuǎn)360°以及直行1 m，對牽引系數(shù)α進(jìn)行標(biāo)定。結(jié)果表明，不同材質(zhì)牽引系數(shù)不同，本次實驗室內(nèi)瓷磚地面標(biāo)定結(jié)果為α=1.538。

3.2結(jié)果分析

圖5 a）和圖5 b）對比可知，當(dāng)看護(hù)機(jī)器人移動速度較慢時，傳統(tǒng)里程計估計和本文提出的含牽引系數(shù)的里程計估計在開始階段誤差均較小，但是傳統(tǒng)里程計估計在看護(hù)機(jī)器人運行到第4圈后，里程計估計值與機(jī)器人實際運動軌跡產(chǎn)生了較大偏差。

為了不失一般性，本文采用5次運行結(jié)果的平均誤差進(jìn)行對比，由于看護(hù)機(jī)器人運行5圈后，回到了與出發(fā)時完全一致的位置，因此對位置誤差和角誤差分別使用如下公式：

本文以0.1 m/s的速度運行5次，對2種情況的里程計估計值與機(jī)器人實際值的平均誤差進(jìn)行計算，結(jié)果如表1所示。由表1可以看出，2種情況的里程計估計均出現(xiàn)了偏差，但是本文提出的含牽引系數(shù)估計里程計方法，與機(jī)器人實際終點坐標(biāo)直線距離平均誤差僅為0.10 m;而未考慮滑動的傳統(tǒng)里程計平均誤差達(dá)到0.18 m。由圖5 a）可以看出，看護(hù)機(jī)器人在進(jìn)行繞標(biāo)定盒旋轉(zhuǎn)后，軌跡偏離較大，此種情況跟機(jī)器人在旋轉(zhuǎn)過程中航向角估計值發(fā)生偏差有關(guān)。由表1還可以看出，2種情況下，傳統(tǒng)里程計估計航向角偏差為0.026°。大于含牽引系數(shù)里程計估計的航向角偏差。

圖5和圖6對比可知，隨著看護(hù)機(jī)器人運行速度的提高，機(jī)器人在直線運行和旋轉(zhuǎn)時的滑動進(jìn)一步加大，導(dǎo)致2種情況下的誤差均有所增加。表2是速度為0.2 m/s時里程計估計平均誤差對比情況，

可以看出，未考慮滑動的傳統(tǒng)里程計估計5次平均誤差為0.291 m，而含牽引系數(shù)的里程計誤差為0.177 m。

進(jìn)一步加大看護(hù)機(jī)器人的運行速度，當(dāng)移動速度達(dá)到0.4 m/s時，由于地面較光滑，車速較快，因而車輪與地面之間的滑動進(jìn)一步加劇，2種情況下的里程計誤差進(jìn)一步擴(kuò)大。表3是速度為0.4 m/s時里程計估計平均誤差對比，可以看出，未考慮滑動的傳統(tǒng)里程計估計5次平均誤差為0.490 m，而含牽引系數(shù)的里程計誤差為0.260 m。

通過以上分析可知，當(dāng)看護(hù)機(jī)器人分別以0.1，0.2，0.4 m/s的速度運行時，含牽引系數(shù)的機(jī)器人里程計的估計誤差降低約40%，滿足實際使用，可以為提高看護(hù)機(jī)器人的自動導(dǎo)航精度提供參考。

4結(jié)語

本文研究了家用智能看護(hù)機(jī)器人輪胎與地面作用力與電機(jī)力矩不平衡導(dǎo)致的機(jī)器人里程計精度低的問題。首先，引入牽引系數(shù)描述機(jī)器人滑動情況，推導(dǎo)了含牽引系數(shù)的看護(hù)機(jī)器人運動學(xué)模型;然后，以增量式光電編碼器和慣性測量單元2種傳感器為輸入，將含牽引系數(shù)的運動學(xué)模型應(yīng)用于基于擴(kuò)展卡爾曼濾波算法的看護(hù)機(jī)器人里程計估計算法，通過搭建實驗系統(tǒng)進(jìn)行了驗證。結(jié)果表明，在家庭常用的瓷磚地面上，智能看護(hù)機(jī)器人分別以0.1，0.2，0.4 m/s的速度移動時，可以有效降低看護(hù)機(jī)器人的里程計估計誤差，為提升家用智能看護(hù)機(jī)器人在室內(nèi)的自主導(dǎo)航精度提供了參考。

本文僅針對一種地面材質(zhì)進(jìn)行實驗，未來還需研究木質(zhì)地板等更多類型的地面材質(zhì)，提升算法在多場景中的適應(yīng)性。

參考文獻(xiàn)/References：

[1]利莉，胡治宇.家庭智能看護(hù)機(jī)器人的運用研究——基于空巢老人環(huán)境視角[J].現(xiàn)代信息技術(shù)，2019，3（9）：153-154.LI li，HU Zhiyu.Research on the application of family intelligence care robot：Based on the environmental perspective of empty nest elderly [J].Modern Information Technology，2019，3（9）：153-154.

[2]姜立志，楊慕升，鞏曉丹.結(jié)合深度學(xué)習(xí)的智能看護(hù)機(jī)器人目標(biāo)檢測研究[J].機(jī)械設(shè)計與制造，2021（5）：24-28.

JIANG Lizhi，YANG Musheng，Gong Xiaodan.Research on target detection of intelligent care robot based on deep learning [J].Machinery Design & Manufacture，2021（5）：24-28.

[3]候艷芳.面向兒童看護(hù)機(jī)器人的情感化設(shè)計研究[D].哈爾濱：哈爾濱理工大學(xué)，2021.HOU Yanfang.Design of Robot for Child Care Research on Emotional Design[D].Harbin：Harbin University of Science and Technology，2021.

[4]高翔，謝曉佳.機(jī)器人學(xué)中的狀態(tài)估計[M].西安：西安交通大學(xué)出版社，2018.GAO Xiang，XIE Xiaojia.State Estimation for Robotics[M].Xi′an：Xi′an Jiaotong University Press，2018.

[5]霍鳳財，遲金，黃梓健，等.移動機(jī)器人路徑規(guī)劃算法綜述[J].吉林大學(xué)學(xué)報（信息科學(xué)版），2018，36（6）：639-647.HUO Fengcai，CHI Jin，HUANG Zijian，et al.Review of path planning for mobile robots[J].Journal of Jilin University （Information Science Edition），2018，36（6）：639-647.

[6]BROSSARD M，BONNABEL S.Learning wheel odometry and IMU errors for localization[C]// 2019 International Conference on Robo-tics and Automation （ICRA）.Montreal Canada：IEEE，2019：1-11.

[7]SELMANAJ D，CORNO M，PANZANI G，et al.Vehicle sideslip estimation：A kinematic based approach[J].Control Engineering Practice，2017，67（10）：1-12.

[8]ZHU Jie，TANG Yujie，SHAO Xin，et al.Multisensor fusion using fuzzy inference system for a Visual-IMU-Wheel odometry[J].IEEE Transactions on Instrumentation and Measurement，2021，70：DOI：10.1109/TIM.2021.3051999.

[9]賈曉輝，徐文楓，劉今越，等.基于慣性測量單元輔助的激光里程計求解方法[J].儀器儀表學(xué)，2021，42（1）：39-48.JIA Xiaohui，XU Wenfeng，LIU Jinyue，et al.A solving method of lidar odometry based on IMU[J].Chinese Journal of Scientific Instrument，2021，42（1）：39-48.

[10]任利簡，趙修斌，龐春雷，等.基于抗差估計改進(jìn)的GNSS/INS組合導(dǎo)航完好性監(jiān)測方法[J].航天控制，2021，39（5）：21-26.REN Lijian，ZHAO Xiubin，PANG Chunlei，et al.Improved integrity monitoring method based on robust estimation of GNSS/INS integrated navigation[J].Aerospace Control，2021，39（5）：21-26.

[11]YANG Zhehua，LI Zengke，LIU Zan，et al.Improved robust and adaptive filter based on non-holonomic constraints for RTK/INS integ-rated navigation[J].Measurement Science and Technology，2021，32（10）.DOI：10.1088/1361-6501/ac0370.

[12]TEUNISSEN P，KHODABANDEH A.Review and principles of PPP-RTK methods[J].Journal of Geodesy，2015，89（3）：217-240.

[13]齊乃新，楊小岡，李小峰，等.基于ORB特征和LK光流的視覺里程計算法[J].儀器儀表學(xué)報，2018，39（12）：216-227.QI Naixin，YANG Xiaogang，LI Xiaofeng，et al.Visual odometry algorithm based on ORB features and LK optical flow[J].Chinese Journal of Scientific Instrument.2018，39（12）：216-227.

[14]ENGEL J，KOLTUN V，CREMERS D.Direct sparse odometry[J].IEEE Transactions on Pattern Analysis & Machine Intelligence，2018，40（3）：611-625.

[15]ZHOU Lipu，WANG Shengze，KAESS M.DPLVO：Direct point-line monocular visual odometry[J].IEEE Robotics and Automation Letters，2021，6（4）：7113-7120.

[16]范涵奇，吳錦河.基于拉普拉斯分布的雙目視覺里程計[J].自動化學(xué)報，2022，48（3）：865-876.FAN Hanqi，WU Jinhe.Stereo visual odometry based on laplace distribution[J].Acat Automatica Sinica，2022，48（3）：865-876.

[17]MURARTAL R，TARDOS J.ORB-SLAM2：An open-source SLAM system for monocular，stereo，and RGB-D cameras[J].IEEE Transactions on Robotics，2017，33（5）：1255-1262.

[18]REICHARD K ，BRENNAN S ，PENTZER J .Model-based prediction of skid-steer robot kinematics using online estimation of track instantaneous centers of rotation[J].Journal of Field Robotics，2014，31（3）：455-476.

[19]PENTZER J，BRENNAN S，REICHARD K.On-line estimation of vehicle motion and power model parameters for skid-steer robot energy use prediction[C]// American Control Conference （ACC）.[S.l.]：IEEE，2014：2786-2791.

[20]MYUNG H，LEE H.Mobile robot localization with gyroscope and constrained Kalman filter[J].International Journal of Control Automation & Systems，2010，8（3）：667-676.[ST][JP]