退化环境中全向移动机器人位姿鲁棒修正算法.pdf

第44卷第3期2023年6 月文章编号：16 7 3-9 59 0(2 0 2 3)0 3-0 0 6 3-0 7大连交通大学学报JOURNAL OF DALIAN JIAOTONG UNIVERSITYVol.44No.3Jun.2023退化环境中全向移动机器人位姿鲁棒修正算法李荣华,祁宇峰,郑宇锋,杨景山,李宝良（大连交通大学机械工程学院，辽宁大连116 0 2 8）摘要：全向移动机器人定位算法在退化环境中通常会产生较大的定位漂移或失效，为此提出一种鲁棒的全向移动机器人在退化环境中的位姿修正算法。首先，通过全向移动机器人的结构、动力学分析确定定位误差产生来源；其次，对机器人的各运动状态进行受力分析，结合机器人的运动环境和机器人的运动状态建立打滑误差模型；再次，通过预试验得到的位移与定位误差，运用非线性加权拟合方法计算得出加工装配误差模型；最后，在轮式里程计中插入误差补偿模块，提高机器人定位精度。试验表明：算法位置平均误差为1.36%，姿态平均误差为0.7 5%，较传统算法分别提高了3.14倍和5.15倍。算法在保证定位精度的前提下具有很强的鲁棒性。关键词：退化环境；移动机器人；定位漂移；位姿修正；非线性加权拟合文献标识码：AD0I:10.13291/ki.djdxac.2023.03.011全向移动机器人 因其在狭窄、拥挤和高度动态环境中的高机动性与灵活性而被广泛应用。全向轮式里程计的定位精度在同步定位与地图构建（SLAM)和导航过程中具有至关重要的地位。目前，虽然已经有多种优秀的定位算法，但在退化环境中容易漂移或失效。孙建强等 2 提出的超宽带定位方式具有较高定位精度和鲁棒性，但存在成本高、只适用于小范围系统搭建、没有姿态信息等不足。扩展卡尔曼滤波 3 是基于滤波融合的定位中应用最广的方法之一。李荣冰等 4 提出EKF方法，以数值方式计算雅可比矩阵，需要较高的计算成本,并且融合传感器数量越多,计算成本越高。张亮等 5 提出的长走廊环境测试分析表明观测传感器在退化环境中容易失效。与观测传感器相比，轮式里程计有效地克服了这些缺点，但存在无界的累积误差。19 9 8 年，Borenstein 等 6 提出了UMBmark 校核方法并设计了正方形路径以补偿轮子直径的加工误差以及轮距的装配误差，使差分轮式机器人的定位精度至少提升一个数量级。与差分轮不同的是全向移动机器人具有多变量输入、麦克纳姆轮不连贯、易打滑且强耦合特性，该系统定位误差模型复杂度倍增且很难建立。因此 UMBmark 校核方法并不适用于全向移动机器人。该方法只注重机器人本身参数引起的误差，而忽略了环境和机器人运动状态不同引起的误差。为此,本文针对其他定位方式的现有问题,结合机器人运动环境和运动状态建立打滑误差模型，利用非线性加权拟合方法构建加工装配误差模型,提出一种在退化环境中的机器人位姿鲁棒修正算法，有效地提高了全向移动机器人在退化环境中的定位精度及鲁棒性。1误差来源分析如图1所示，麦克纳姆轮式移动机器人具有全向运动的特点。通过控制4个麦克纳姆轮的转向和转速可以实现机器人的全向运动，无需转弯半径,保证了机器人的灵活性和机动性 7 。但麦克纳姆轮的小辊子交替与地面以高副接触，在不连续的滚动接触过程中，极易产生振动、滑动和噪声 8 。为降低垂直方向的抖动和提高小辊子与地面接合率，本文使用的移动机器人底盘安装了收稿日期:2 0 2 2-0 1-0 7基金项目：辽宁省教育厅科学研究计划资助项目（LJKZ0475）第一作者：李荣华（19 8 3一），男，教授。E-mail:通信作者：祁宇峰（19 9 8），男，硕士研究生。E-mail:64可保持恒定轮距的独立悬挂系统。麦克纳姆轮的结构较为复杂，全向移动机器人的加工装配精度具有极大挑战，并且无法抑制此类误差产生。因此，将全向移动机器人的定位误差分为打滑误差和加工装配误差。大连交通大学学报45.0FFW2第44卷2aFw.45.0FT,WFTFfY45.0FW.(c）逆时针旋转2aFwF45.0图1机器人模型21.1 受力分析机器人各运动模式受力分析见图2。机器人运动时，电机轴主动转动带动轮毂转动，轮毂带动辊子的轴向和周向运动 。往往为了根据被驱动的车轮速度来计算机器人位姿，需要假设所有的小辊子都是自由滚动的。但实际上由于小辊子H45.0FwVFf45.0(a)向前直行2a45.045.0F/FWF45.045.0FW/FW(b)向后直行45.045.0FTXVFW2图2 机器人受力分析2aFw.45.0XFr45.045.0(d）顺时针旋转的轴承与轴等机械部件的摩擦，无法实现旋转部F件完全自由滚动。因此，辊子同时受到地面轴向滑动摩擦力和轴向的滚动摩擦力。1.2打滑误差麦克纳姆轮的小辊子交替与地面以高副接触，当机器人启动、刹车或转弯的过程中加速度剧增时，无法避免机器人产生打滑。在车轮与地板表面之间的相对运动过程中，接触地板的小辊子与地板表面之间的摩擦力不足以将电机旋转运动全部转化为机器人运动，无法只根据轮子转速估计机器人位姿。因此，全向移动机器人打滑偏移量受到加速度、角加速度和摩擦力的影响。机器人在刹车过程中，车轮抱死后利用辊子与地面最大摩擦力制动，需根据最大摩擦力计算制动时的打滑误差。1.3加工装配误差全向移动机器人的结构设计不合理和制造及安装精度不够精确等因素包含麦克纳姆轮实际直径与名义直径不相等、实际轮距与名义轮距不相等 10 。此外，小辊子间存在间隙，轮廓实际上并不是标准的圆形,而是一个包含圆弧的多边形拟45.00F/Fw(3)第3期合成的圆,其拟合直径与实际直径的差异往往被忽略。这些因素以常量累积,随着机器人运动时间和路程的增加，会产生无界的位姿累积误差。具有加工装配误差的差分轮式移动机器人往往以UMBMark及其改进方法校正轮径与轮距误差。但麦克纳姆轮相较差分轮具有多变量和非线性允余的强耦合误差特性,如四驱动存在耦合、小辊子与地面接触点周期变换等，机器人的运动学和动力学误差模型极其复杂且很难准确建立。针对以上问题,本文将所有机械结构产生的误差拟合成一个总的加工装配误差，利用最小二乘法求出最大似然误差。虽然在精度上有一定程度舍车轮角加速度质量+摩擦系数运动学方程受力分析机器人加速度机器人最大加速度打滑误差判定启动打滑误差制动打滑误差打滑误差图3算法流程框架2.1里程计模型如图4所示，麦克纳姆轮式机器人可以全向移动的原理是：轮子上的小辊子可以将沿车轮旋转方向的一部分力转换为车轮轴方向的力，根据全向移动机器人的运动模型，通过控制每个车轮转速，产生沿着期望方向的力或力矩，进而使机器人可以全向移动。VV2a4VV李荣华，等：退化环境中全向移动机器人位姿鲁棒修正算法2位姿修正算法本文提出了一种结合机器人运动状态和工作环境的打滑误差修正方法，可以有效地解决不同工作环境与运动状态定位精度差异性，提高机器人的定位鲁棒性。构建全向轮式里程计模型,结合机器人环境与运动状态建立打滑误差补偿模型，利用非线性加权方法拟合机器人加工装配产生的定位误差，计算最大似然误差。算法流程框架见图3。打滑误差补偿模型里程计模型车轮转速运动学方程机器人运动状态轮式里程计机器人位姿每个车轮在机器人坐标系下的实际速度为：V+V,cos;=V,-b;a(1)V,sin;=V,+a;0(2)V,+a.V;=V.-b;atani车轮转速为：1-1-(a+b)VW2W3R11L1-1L4(a+b)同理，根据4个电机的角速度，可以推算出机器人的运动状态为：RVV.=-(0i+W2+W3+04)4Y3R(-W+W2-W3+4).X65弃，但极大地简化了补偿模块的计算。加工装配误差补偿模型11预试验残差构建权重分配最小二乘模型车轮角加速度加工装配误差11TV1-(a+b)(a+b)V(4)L(5)R图4机器人运动学模型4(a+b)(-W-W2+;+w4)66本文各主要参数和符号见表1。序号符号1V;、Vi,2345678910111213141516171819202122232425根据每一时刻机器人的运动状态即可算得机器人的位姿为：rx,=xi-I+V,At cos e,-V,At sin 9,y,=yi-I+VAt sin O,+V,At cos,(6)lo,=0,-1+wt2.2打滑误差补偿打滑往往发生在启动和制动时，车轮的加速度超过机器人加速度致使车轮转速超过机器人移动速度，车轮转速没有完全转化为机器人移动速度，因此发生打滑。打滑误差表示为：Ex,+E,bE,=E=E+EE.Eo.+Eo.机器人启动，当车轮加速度超过机器人加速大连交通大学学报表1主要参数和符号代表参数i轮线速度、垂直辊子轴方向的分速度i轮角速度VVyW机器人方向速度、方向速度、角速度a、6机器人左右轮距的一半、前后轮距的一半R车轮半径t时刻里程计记录机器人x方向位置、方向位置、姿态Att-1与t时刻时间间隔E,机器人打滑误差ESE、EEEVVytot2t4tit3vtsFxF、TFT;mJAEABA,B E(x,y,0)Pa第44卷机器人方向、方向、角度的打滑误差机器人启动时x方向、方向、角度的打滑误差机器人制动时x方向、方向、角度的打滑误差机器人方向最大加速度、方向最大加速度、最大角加速度10根据车轮角加速度计算得出机器人x方向最大加速度、y方向最大加速度、最大角加速度根据车轮转速计算得出机器人方向速度、方向速度、角速度xa%、0.分别刚刚超过a,，、a y，o,的时刻Va。、Vy。、Vo.分别与机器人方向真实速度、方向真实速度、真实角速度相等的时刻车轮抱死前一刻，根据车轮转速计算得出机器人x方向速度、y方向速度、角速度车轮抱死时，机器人x方向受到的最大摩擦力、方向受到的最大摩擦力、最大转矩第i次预试验，机器人产生方向、方向、角度的加工装配误差机器人单位A方向位移产生的B方向加工装配误差第i次预试验，根据车轮旋转角度计算得出的机器人位姿度时发生打滑，直至轮速与机器人速度相同打滑才停止。启动打滑误差表示为：E(axx,t-V)dt3Eat2e,t-.)dtH机器人制动时打滑发生在轮子抱死，利用辊子轴向滑动摩擦力与轴向滚动摩擦力减速至机器人完全停止。制动打滑误差表示为：Ex,=-V.,/2a.,(7)ybEo,=-w/2ao,式中：机器人x方向最大加速度、y方向最大加速度和最大角加速度分别表示为：i轮辊子受到的最大轴向摩擦力i轮辊子受到的最大周向摩擦力机器人的质量机器人的转动惯量(8)xt-Vy)dt(9)(10)(11)(12)(13)第3期李荣华，等：退化环境中全向移动机器人位姿鲁棒修正算法67aaao,=T/J机器人制动时所受方向、方向最大摩擦力以及最大扭矩表示为：F=(Fw101+Fw+Fu+F)sin45+W2W3F,=(Fu+F+F3+Fm)cos 45+11031W2(F+F+F+FT)sin 45T=a sin 45 (Fw101F1+F1W3+FT.)+cos 45 (-Fr,+Fu+FT+F）T3+1W4+Fbcos 45(F.+一F+W1W3b sin 45 (Fr,+FwFT+Fm)(19)一1122.3加工装配误差补偿为计算机器人最大似然加工装配误差，建立最小二乘模型为：(20)2根据预试验测得机器人运动一段距离里程计产生的加工装配误差，可以推算出真实误差与补偿误差之间的差值为：d,=Ex-E,Puid,=Ey,-E,.P,YiWd,=o.-E.,P.PoW将每次预试验中真实误差与补偿误差之间的差值作为最小二乘模型中的残差，然后利用高斯-控制速度V/(m/s)w/(rad/s)0.160.3180.530m/mPyx/mm1 040.8477.2807.3851.01 043.5959.2918.8985.5713.5932.6992.5853.6751.21 038.5896.8(14)(15)(16)(17)(18)14(21)(22)(23)表2 传统算法试验结果真实位姿y/mm-599.2-415.7-288.5-626.8-689.5-464.6-347.3-533.7-612.4-561.8-745.8-346.1-240.4-522.4-574.5牛顿法求解出单位位移产生的最大似然加工装配误差。残差为：f(AEp,AEp,AEp)=dd=d:+d,+d?由于预试验中存在较多离群点，这些离群点会极大地影响求解的精度。因此,利用Huber 函数作为损失函数降低离群点的权重，即：x,x1p(x)2/x-1,x 1最后,采用利用非线性加权拟合方法求解出AE,、E,、Ep。，建立加工装配误差补偿方程：E,=E,P(27)3试验结果与分析本文采用基于实验室自主搭建的配有4个麦克纳姆轮的全向移动机器人平台。该平台主要由可保持恒定轮距的独立悬挂系统、野火STM32F429开发板、工控机、4组大疆C620电调和大疆M3508直流无刷减速电机构成。相关软件都在安装机器人操作系统（ROS-melodic）的工控机中运行。机器人在5m5m的正方形路径上试验，为防止试验的偶然性,每组相同试验重复5次。表2和表3分别显示了传统算法与本文提出的算法以不同运动状态在相同地面所获得的试验结果与机器人的真实位姿。传统算法0/radx/mm14.3164.09.2171.712.5183.515.1143.617.6228.414.897.212.1192.513.4103.617.534.315.084.218.3-11.314.8-71.416.583.711.423.618.6-34.8(24)(25)(26)误差y/mm0/rad-196.74.2-175.63.6-231.55.8-188.32.6-226.04.2-284.52.1-341.51.6233.4-1.4-266.61.9-308.42.6-338.7-3.1-372.7-3.5-350.4-3.4334.4-2.7-372.0-4.1位置/mm角度/rad964.810.1388.65.6626.46.7832.312.5937.713.4880.612.7726.310.5931.614.8762.215.6885.412.41.083.221.4925.418.3842.119.91 078.614.1953.422.768V/(m/s)w/(rad/s)0.160.3180.530从表2 可以看到，机器人的运动速度为0.1、0.3、0.5m/s,角速度为6、18、30 rad/s时，传统算法位置平均误差为7 49.6、8 37.2 2、9 7 6.54mm，且姿态平均误差为9.6 6、13.2、19.2 8 rad。随着机器人运动速度增大，传统算法的定位误差明显上升。从表3可以看到，机器人速度分别为0.1、0.3、0.5m/s 时，本文算法位置平均误差为265.14、2 7 1.36、2 7 9.4m m，且姿态平均误差为2.4、2.7 6、2.9 r a d。本文算法产生的定位误差仍然随机器人运动速度增大而增大，但相较传统算法增幅较低。试验表明机器人速度分别为0.1、0.3、0.5m/s 时，本文算法较传统算法位置精度提高了2.8 3、3.0 9、3.50 倍，姿态精度提高了4.03、4.7 8,6.6 5 倍。4结论本文提出了一种在退化环境中机器人位姿鲁棒修正算法，相较于传统算法，考虑了机器人的打滑误差与加工装配误差，利用机器人的运动环境与运动状态等参数构建误差补偿方程，以修正位姿。利用实验室自主搭建的机器人来验证本文算法的有效性。试验结果显示，本文算法相对于传统算法,定位精度与鲁棒性都有明显提升。在机器人各运动状态、各环境下定位精度不会产生较大连交通大学学报表3本文算法试验结果控制速度真实位姿x/mmy/mm371.324.0106.2-71.5420.8-361.5271.1111.2471.2-449.1175.3-422.470.4-315.1232.5-444.8264.8-556.5-83.2-510.8-175.0-142.8-192.6-223.3214.453.9-94.5-496.6251.2-555.1第44卷传统算法误差0/radx/mm8.5186.55.4209.47.3156.35.4160.44.2193.23.1-51.9-3.6204.4-0.5122.92.8108.24.777.4-5.024.8-1.893.40.73.5-0.4203.4-0.2-27.6大波动，因此较传统算法具有更广的应用前景。在此次工作中，发现机器人重心发生偏移，地面对机器人4个轮子的作用力大小产生差异。在前进或旋转的过程中，机器人每侧会受到不同大小的力和一个额外的力矩。并且通过仿真试验验证此项误差是非线性的，不易通过预试验方法采集数据建立补偿方程。因此,在下一步工作中,将尝试建立机器人重心发生偏移时的动力学与运动学模型来建立补偿方程。为降低非系统性误差，将尝试融合IMU（惯性测量单元）信息，进一步提高定位精度与鲁棒性。参考文献：1 Ilon Bengt Erland.Wheels for a course stable selfpropel-ling vehicle movable in any desired direction on theground or some other base:US3876255 P.1975-04-08.2孙建强，尚俊娜，刘新华，等.气压计辅助的UWB室内定位方法 J.传感器与微系统，2 0 2 1,40（6)：30-33.3张书亮,谭向全,吴清文.基于多传感器融合技术的室内移动机器人定位研究 J.传感器与微系统，2 0 2 1，40(8):5356.4 李荣冰,王智奇，廖自威.激光雷达/MEMS微惯性组合室内导航算法研究 J.传感器与微系统，2 0 2 0,39(12):14-17.5张亮,刘智宇,曹晶瑛，等.扫地机器人增强位姿融合的Cartographer算法及系统实现 J.软件学报，2 0 2 0，y/mm-176.2-120.2-248.7-174.5-244.469.2386.5375.4-226.4-269.5-317.6403.9-310.8-336.9-350.50/rad4.13.43.16.64.01.91.62.1-0.22.9-3.1-4.6-3.6-2.3-3.8位置/mm角度/rad272.54.4114.12.0287.54.2306.4-1.2345.20.2420.01.2151.8-5.2129.7-2.6365.43.0289.91.8230.5-1.9206.12.8421.34.3193.31.9345.83.6第3期31(9):2678-2690.6BORENSTEIN J,FRNG L Q.Measurement and correc-tion of systematic odometry errors in mobile robots.J.IEEE Trans.Robotics and Automation,1996,12(6):869-880.7BYUN K S,SONG J B.Design and construction of con-tinuous alternate wheels for an omnidirectional mobile ro-bot J.Journal of Robotic Systems,2003,20(9):569-579.8BAE J J,KANG N.Design optimization of a mecanum李荣华，等：退化环境中全向移动机器人位姿鲁棒修正算法69wheel to reduce vertical vibrations by the considerationof equivalent stiffness J.Shock and Vibration,2016(4):1-8.9 WU B H,QIN D Z,CHEN Y H,et al.Structure design ofan omni-directional wheeled handling robot J.Journalof Physics:Conference Series,2021,1885(5):052013.10JEHSAN S,ALI R H.A new algorithm for calibration ofan omni-directional wheeled mobile robot based on ef-fective kinematic parameters estimation J.Journal ofIntelligent&Robotic Systems,2021,101(2):01296-9.Robust Pose Correction Algorithm for Omni-DirectionalMobile Robots in Degraded EnvironmentLI Ronghua,QI Yufeng,ZHENG Yufeng,YANG Jingshan,LI Baoliang(School of Mechanical Engineering,Dalian Jiaotong University,Dalian 116028,China)Abstract:Aiming at omni-directional mobile robot positioning algorithms usually producing large positioningdrift or failure in degraded environments,a robust pose correction algorithm for omni-directional mobile robotsin degraded environments is proposed.In order to correct the pose of the robot,the source of the positioningerror is determined by analyzing the structure and dynamics of the Mecanum wheeled omni-directional mobilerobot.The force of the robot in each motion state is analyzed,and a slip error model based on the robot motionenvironment and state is established.To obtain the machining and assembly error model,the displacement andpositioning error is obtained by the pre-experiment,which are calculated by nonlinear weighted ftting meth-od.Finally,an error compensation module is incorporated into the wheel odometer to improve the positioningaccuracy of the robot.Experiments show that the average position error is 1.36%,and average orientation er-ror of this algorithm is 0.75%,which are 3.14 times and 5.15 times higher than the traditional algorithm.The algorithm in this paper has strong robustness under the premise of ensuring positioning accuracy.Keywords:degraded environment;mobile robot;positioning drift;pose correction;nonlinear weighted fittingmethod