家用智能看護機器人高精度里程計估計算法

余雷　劉怡明　張靈霖　徐揚

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

關鍵詞：機器人控制;智能看護機器人;里程計估計;多傳感器融合;牽引系數

中圖分類號：TP242.6文獻標識碼：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

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

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

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

1家用看護機器人運動學模型

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

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

2.2基于慣性測量單元的機器人姿態估計

2.3基于擴展卡爾曼濾波的里程計估計

為實現編碼器和慣性測量單元融合，本設計使用擴展卡爾曼濾波算法[20]，該算法便于編程的實現，且能夠對現場采集的數據進行實時更新處理，在里程計估計中運用比較成熟，已成為當下應用十分廣泛的濾波方法。因此，本文選用此方法進行實驗對比分析。擴展卡爾曼濾波一共分為5個步驟。

3實驗平臺設計與結果分析

3.1實驗平臺設計

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

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

實驗過程中，針對不同的地面材質使機器人本體繞重心旋轉360°以及直行1 m，對牽引系數α進行標定。結果表明，不同材質牽引系數不同，本次實驗室內瓷磚地面標定結果為α=1.538。

3.2結果分析

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

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

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

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

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

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

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

4結語

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

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

參考文獻/References：

[1]利莉，胡治宇.家庭智能看護機器人的運用研究——基于空巢老人環境視角[J].現代信息技術，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]姜立志，楊慕升，鞏曉丹.結合深度學習的智能看護機器人目標檢測研究[J].機械設計與制造，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]候艷芳.面向兒童看護機器人的情感化設計研究[D].哈爾濱：哈爾濱理工大學，2021.HOU Yanfang.Design of Robot for Child Care Research on Emotional Design[D].Harbin：Harbin University of Science and Technology，2021.

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

[5]霍鳳財，遲金，黃梓健，等.移動機器人路徑規劃算法綜述[J].吉林大學學報（信息科學版），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].儀器儀表學，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]任利簡，趙修斌，龐春雷，等.基于抗差估計改進的GNSS/INS組合導航完好性監測方法[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].儀器儀表學報，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].自動化學報，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]