基于测程法的四轮全向移动机器人自定位方法研究

竞赛机器人共性技术 17 基于测程法的四轮全向移动机器人自定位方法研究 刘玉鹏,海丹,郑志强 （国防科技大学机电工程与自动化学院 湖南长沙 410073） 摘要：通过对四轮全向移动机器人的运动学分解，建立了利用测程法通过解算得到机器人自定位信息的 一般模型，对造成定位误差的原因进行了分析。跟据该全向移动机器人四个驱动轮应满足的约束条件，提出 了轮子打滑的判断依据。最后针对自主开发的一种四轮全向移动机器人，通过实验验证了模型的正确性。 关键词：测程法； 全向移动机器人； 自定位； 打滑判断 Odometry Based Self-localization for 4-Wheel Omnidirectional Mobile Robots LIU Yu-peng,HAI Dan,ZHENG Zhi-qiang (College of Mechatro-electronic and Automation,National Univ. of Defense Tech.Changsha 410073) Abstract: This paper proposes a general model for self-localization based on odometry by the kinematic characteristics analyzing of 4-wheel omnidirectional robots.Some causes resulted to error are discussed and a method is presented to identify wheel slippage according to over-determined restriction of 4-wheel omnidirectional robots. Experimental results show validity of the model. Keywords: odometry; omnidirectional robots; self-localization; wheel slippage 1 引言(Introduction) 在 RoboCup 中型组机器人比赛中，机器人的自定位是一个关键问 题 快递公司问题件快递公司问题件货款处理关于圆的周长面积重点题型关于解方程组的题及答案关于南海问题 ，它同时也是所有自 主移动机器人的一个基础性难题。据 RoboCup2004 官方统计结果，中型组机器人用于自定位 的传感器有里程计、全向视觉系统、局部视觉系统、红外传感器、声纳、激光测距仪、加速 度计、陀螺仪和电子罗盘等多种类型。根据利用的传感器类型，机器人自定位可分为相对位 置法和绝对位置法。两种方法各有优劣，关于机器人定位的大量文献 表 关于同志近三年现实表现材料材料类招标技术评分表图表与交易pdf视力表打印pdf用图表说话 pdf 明，在实际应用中不 存在一种真正普适的方法可以完美的解决移动机器人的自定位问题 [1]。测程法(Odometry)进 行自定位属于相对位置测量法，统计表明，这种方法在移动机器人特别是 RoboCup 中型组机 器人中广泛应用。 测程法实现机器人定位的方法就是根据编码器数据和编码器与机器人本体的捷联结构， 推算出机器人的相对位姿变化。Chong K.和 Kleeman L.对双轮差动机器人里程计数据解算模型 进行了深入的讨论[2]，Alireza Fadaei Tehrani 等人对三轮全向机器人的里程计数据解算模型进 行了分析[3]。本文针对自主开发的四轮全向移动机器人，提出了该类型机器人测程法进行位 姿解算的一般模型，通过实验验证了模型的正确性，并给出了轮子打滑的判断模型。 竞赛机器人共性技术 18 2 基于四轮全向移动机器人的测程法定位模型(Model for odometry based 4-Wheel omnidirectional mobile robots) 2.1 四轮全向移动机器人的运动学分解 在比赛中，激烈的对抗给机器人的灵活性提出了更高的要求。一般的双轮差动结构是一 种非完整性约束，限制了机器人的侧向运动[4]，成为了机器人灵活性提高的一个瓶颈，因此， 全向移动机器人得到了广泛应用。这类机器人能够在平面内向任何方向运动，在不考虑障碍 的情况下，能够实现任意点对点之间的直线路径规划，并且能够在整体直线运动的同时改变 机器人的朝向，所以全向移动机器人的测程法比较双轮差动有其特殊性。图 1 描述的是四轮 全向移动机器人在全局坐标系下位姿变化。 针对四轮全向移动机器人，其一般运动如图 1 所示。定义其 k 时刻在全局坐标系下的位 姿描述为： T( )k k k kx , y ,θ=s (1) 机器人运动一段时间后，在全局坐标系下产生了一个位姿偏移 kΔs ： T=( )k x, y, θΔ Δ Δ Δs (2) 设编码器数据的采样周期为 T，每个周期从四个与电机相连的编码器读出计数差值，计算 得测程增量结果分别为 D1、D2、D3、D4，以向量的形式记为： T( 1 2 3 4)k D ,D ,D ,D=d (3) 因为机器人是由四个轮子驱动的，则必然存在一个 3×4 矩阵 kC ，满足下面关系关系： k k kΔ =s C d (4) x y 2 1 3 4 O R kθ ( , )k kx y 图1 四轮全向移动机器人在全局坐标系下的运动 RoboCup 中型组机器人在场地上的移动可以抽象为刚体在平面内的运动，所以可以把其 分解成刚体的平动和绕一基点的转动，如图 2 所示。 竞赛机器人共性技术 19 x y 1( , , )k k kx y θ− ( , , )k k kx y θ 2 1 3 4 ϕ xˆ yˆ 1 1 1( , , )k k kx y θ− − − θΔ O 图2 机器人平面运动分解 2.2 测程耦合矩阵 选取四个驱动轮的机械中心为基点，以右手系规定每个轮旋转的正方向和机器人整体的 旋转正方向，设驱动轮到机械中心的距离为 R。 在不考虑打滑或死锁的情况下，四个驱动轮的测程增量在机器人只作平动时，满足： 1 2 3 4 0D D D D+ + + = (5) 在只作转动时，满足： 1 2 3 4D D D D= = = (6) 因此，以机器人前向为 yˆ 轴，顺时针旋转90°为 xˆ 轴，四个轮系的机械中心为原点建立机 器人体坐标系，由(5)(6)在体坐标系下运动分解可得： 4 2 3 1ˆ sin sin 2 2 4 2 3 1ˆ cos cos 2 2 1 2 3 4 4 D D D Dx D D D Dy D D D D R ϕ ϕ ϕ ϕ θ − −⎧ Δ = −⎪⎪ − −⎪Δ = +⎨⎪ + + +⎪ Δ =⎪⎩ (7) 将其换算到全局坐标系下为： 1 1 ˆ cos ˆ sin k k x y y y θ θ θ θ − − Δ = Δ⎧⎪Δ = Δ⎨⎪ Δ = Δ⎩ ˆ sin ˆ cos x x θ θ +Δ −Δ (8) 可以得到： cos( ) cos( ) cos( ) cos( ) 12 2 2 2 2sin( ) sin( ) sin( ) sin( ) 32 2 2 2 1 1 1 1 4 4 4 4 4 D x D y D D R R R R θ ϕ θ ϕ θ ϕ θ ϕ θ ϕ θ ϕ θ ϕ θ ϕ θ + − + −⎡ ⎤− −⎢ ⎥ ⎛ ⎞Δ⎛ ⎞ ⎢ ⎥ ⎜ ⎟+ − + −⎜ ⎟ ⎢ ⎥ ⎜ ⎟Δ = − −⎜ ⎟ ⎢ ⎥ ⎜ ⎟⎜ ⎟Δ ⎢ ⎥ ⎜ ⎟⎝ ⎠ ⎜ ⎟⎝ ⎠⎢ ⎥⎢ ⎥⎣ ⎦ (9) 竞赛机器人共性技术 20 记(9)式中的 3×4 矩阵为 kC ，则得到测程法解算四轮全向移动机器人自定位信息的一般 模型为： 1k k k k−= +s s C d (10) 其中 kC 称之为测程耦合矩阵。 3 误差源分析(Causes resulted to error) 测程法在短期内能提供很高的定位精度，但其无限的误差累积是最大的缺点[5,6]，对其误 差源进行分析，最大可能的控制测程法误差累积的程度，在实际应用中是非常有价值的。从 前面的自定位模型中可以看出，测程法的主要误差源有三种： 1）计算误差。在测程法中，首先是作了一些近似，以 k-1 时刻的机器人朝向角计算 k 时 刻的机器人位置，势必带来误差。文献[6]以 ( )2k θθ Δ+ 进行计算，在一定程度上减小了这种 近似带来的误差。再则当采样频率很高时，测程增量非常小，计算过程中的舍入误差也会影 响到结果的输出，因此在这些计算中最好使用双精度型数据。 2）模型误差。这部分误差主要是指那些会影响到测程耦合矩阵 kC 的因素。包括机械中 心的精度、轮径、轮间距等。 3）输入误差。这部分误差是指会造成 kd 数据偏差的那部分误差，主要由齿轮间隙，轮子 打滑和死锁等导致的编码器计数值不能如实反映驱动轮的前进而造成。这是测程法当中最主 要的误差源，其中齿轮间隙相对造成的误差较小，且容易判断，可以通过补偿的方法解决， 而打滑和死锁是机器人最主要的方向误差因素[6]，有必要对其进行判断。 4 打滑判断模型(Model of slippage judgement) 虽然我们设计了巧妙的机构保证四个驱动轮都能充分接触地面，但由于四轮机构的特殊 性，打滑和死锁依然可能出现。因为三个点确定一个面，四轮全向移动机器人在平面内的运 动，事实上是有冗余且相互约束的。文献[7]通过对四轮全向移动机器人详细的动力学分析给 出了这种约束关系： k k k+= Δd C s (11) 其中 k +C 是一个 4×3 矩阵，它和 kC 互为伪逆矩阵[7]。可以求得： 1 1 2 cos( ) 2sin( ) 1 1 2 cos( ) 2 sin( ) 1 1 2 cos( ) 2sin( ) 1 1 2 cos( ) 2sin( ) k R R R R θ ϕ θ ϕ θ ϕ θ ϕ θ ϕ θ ϕ θ ϕ θ ϕ + ⎡ ⎤− −⎢ ⎥+ +⎢ ⎥⎢ ⎥− −⎢ ⎥− −⎢ ⎥= ⎢ ⎥⎢ ⎥+ +⎢ ⎥⎢ ⎥⎢ ⎥− −⎢ ⎥⎣ ⎦ C (12) 根据前面得到的测程增量和机器人位姿变化的关系,由(4)(11)得到： k k k k +=d C C d (13) 竞赛机器人共性技术 21 将(12)代入，得到四个编码器测程增量应满足的约束条件，也既是轮子打滑的判断模型： 3 1 1 1 4 4 4 4 1 11 3 1 1 2 24 4 4 4 3 1 1 3 1 3 4 4 4 44 4 1 1 1 3 4 4 4 4 D D D D D D D D ⎡ ⎤−⎢ ⎥⎢ ⎥⎛ ⎞ ⎛ ⎞⎢ ⎥−⎜ ⎟ ⎜ ⎟⎢ ⎥⎜ ⎟ ⎜ ⎟= ⎢ ⎥⎜ ⎟ ⎜ ⎟⎢ ⎥−⎜ ⎟ ⎜ ⎟⎜ ⎟ ⎜ ⎟⎢ ⎥⎝ ⎠ ⎝ ⎠⎢ ⎥−⎢ ⎥⎢ ⎥⎣ ⎦ (14) 通过(14)式可以看到，在没有轮子打滑的情况下，四个驱动轮的测程增量应满足一定的方 程，互相存在约束关系。 另外注意到该打滑判断模型中没有角度ϕ 的因素，这说明轮子打滑的判断模型和四轮全 向移动机器人的驱动轮安装结构无关。 要强调的一点是，利用该模型进行打滑判断，是基于假设地面能提供足够大的摩擦力或 者对电机进行了输出力矩控制，而不会出现四个驱动轮同时打滑的情况。事实上，这一点在 实际应用中是可以满足的。 5 实验结果(Experimental results) 图 4 为我们开发的四轮全向移动机器人，其中ϕ 为 45°，根据(9)式很容易得到其测算耦 合矩阵。 图4 四轮全向移动机器人 分别对机器人直线运动和圆周运动轨迹进行了分析。实验中，将机器人初始位置置于场 地中心，机器人运动过程中通过测程法在线计算定位结果，并保存入文件，在场外离线进行 轨迹重现，结果如图所示： 图5 直线运动定位结果 竞赛机器人共性技术 22 图6 圆周运动定位结果 图 5 显示了机器人从起始位置以 0.5m/s 速度直线运动 6 米的轨迹重现结果，多次操作定 位结果误差在 100mm 以内。 图 6 是机器人从起始位置作直径 3m 的匀速圆周运动三圈后回到起始位置的轨迹重现结 果，最终定位误差在 20mm 以内。 6 结论(Conclusion) 测程法是一种自包含的定位方法，不需要再借助其它传感器就能够实现对机器人位置和 方向的估计，方法简单成本低且容易实时完成[5]。本文结合用于 RoboCup 中型组比赛的四轮 全向移动机器人，提出了其测程法解算自定位信息的一般模型，实验表明，该模型能够有效 的解算出机器人的相对位姿变化。本文对测程法误差源进行了简单分析，根据四轮全向移动 机器人特有的约束关系，提出了打滑判断模型，为减小测程法定位偏差提供了参考。 由于测程法本身无法消除累积误差，在实际机器人的定位应用中，需要定期对定位结果 校准修正，进行信息融合，测程法模型的分析将为下一步的信息融合奠定良好的基础。 参考文献 [1] J. Borenstein, H. R. Everett, and L.Feng. Where am I? Sensors and Methods for Mobile RobotPositioning. Oak Ridge National Lab (ORNL) D&D Program，1996. [2] Chong K，Kleeman L．Accurate odometry and elTor modelling for a mobile robot [C]．Proceedings of the IEEE International Conference on Robotics and Automation. Albuquerque，USA：1997，Vo1．4．2783—2788． [3] Alireza Fadaei Tehrani, Ali Mohammad Doosthosseini, Hamid Reza Moballegh, Peiman Amini, Mohammad Mehdi DaneshPanah: A New Odometry System to Reduce Asymmetric Errors for Omnidirectional Mobile Robots. RoboCup 2003: 600-610 [4 邹小兵，蔡自兴，孙国荣.基于变结构的移动机器人侧向控制器设计[J].中南大学学报(自然科学版)，2004， 35(2): 262-267 [5] Borenstein J，Feng L．UMBmark—a method for measuring,comparing,and correcting dead-reckoning errors in mobile robots[R]．USA：UmvemiW of Michigan，1994. [6] 王卫华，熊有伦，孙容磊.一种移动机器人轮子打滑的实验校核方法[J].机器人，2005，27(3):197-202. [7] Raul Rojas.Omnidirectional Control.[R] , Freie University Berlin.2005. [8] Johann Borenstein.Experimental Results from Internal Odometry Error Correction with the OmniMate Mobile Robot[J]. IEEE Transactions on Robotics and Automation, 1998,14(6):963 竞赛机器人共性技术 23 作者简介： 刘玉鹏（1981-）男，河南确山人，硕士研究生，研究领域：多传感器信息融合、智能机器人及机器人足 球。 海丹（1980-）男，山西怀仁人，硕士研究生，研究领域：机器人控制、智能机器人及机器人足球。 郑志强（1965-）男，湖南常德人，博士，教授，博士生导师，研究领域：多机器人系统、智能机器人、 机器人足球系统及精确制导与控制等。