一种室内移动机器人导航定位方法
【技术领域】
[0001] 本发明涉及的是一种导航方法，具体地说是机器人的导航方法。
【背景技术】
[0002] 目前关于移动机器人导航定位方法的研究，均是基于惯性导航、视觉导航、卫星导 航等方法进行导航定位，其导航定位系统均借助于导航定位电子封装模块。对于移动机器 人，定位技术非常重要，如果移动机器人没有定位功能，机器人的任何自主运动都是盲目 的。移动机器人在运动过程中的二个关键问题是：现在何处？到何处去？如何去？
[0003] 中国专利（CN1945351A)公开的一种基于无线传感反应网络对机器人进行导航定 位的新系统和方法。此方法的无线传感反应网络系统相对较复杂，且对系统的稳定性能要 求较高。
【发明内容】
[0004] 本发明的目的在于提供适用于竞技机器人、餐厅服务机器人及工业移动机器人等 领域的一种室内移动机器人导航定位方法。
[0005] 本发明的目的是这样实现的：
[0006] 本发明一种室内移动机器人导航定位方法，其特征是：
[0007] 采用如下移动机器人机构：
[0008] 包括总体框架、定位码盘、第一被动全向轮、第二被动全向轮、第一编码器、第二编 码器，总体框架为正三角形，三角形的三个顶点分别安装一个驱动全向轮，总体框架里安装 三个支撑方管，相邻支撑方管之间夹角为60度，L型的定位码盘安装在其中两个支撑方管 上，第一被动全向轮和第二被动全向轮分别安装在L型定位码盘的两条边上，第一被动全 向轮上安装第一编码器，第二被动全向轮上安装第二编码器，总体框架上安装有陀螺仪，在 总体框架所在平面上，第二被动全向轮的直径与其旁边的驱动全向轮的轴线相重合，第一 被动全向轮旋转方向所在平面与第二被动全向轮旋转方向所在平面以及总体框架所在平 面相交的点为移动机器人的几何中心，
[0009] 以初始时刻移动机器人的几何中心所在点为世界坐标系xoy的原点，此原点的位 置始终固定不变，以移动机器人的几何中心为原点建立机器人坐标系Χ0Υ，此原点随移动 机器人位置的改变而改变，第二被动全向轮旋转方向所在平面与总体框架所在平面的交线 为坐标系XOY的X轴，第一被动全向轮旋转方向所在平面与总体框架所在平面的交线为坐 标系XOY的Y轴，X轴与Y轴的交点即为移动机器人的几何中心，第一被动全向轮转动方向 与第二被动全向轮转动方向的夹角为Θ，第一被动全向轮转动方向与世界坐标系y方向的 夹角为α，即定位码盘的安装误差角，α =90° -Θ ;
[0010] 机器人先沿X方向运动到机器人坐标系XOY下坐标（S，0)处，此时机器人在世界 坐标系Xoy的位置为A，位置A的坐标为（S, -S1)，其中S1= Stan α ;
[0011] 机器人再沿Y方向运动到机器人坐标系XOY下坐标（S，S)处，此位置为终点位置， 此时机器人在世界坐标系xoy的B位置，B位置的坐标为（S，S2)，机器人的几何中心终点位 置与起点位置连线与世界坐标系X方向的夹角为S1，其中
[0012] 此时机器人几何中心起始位置的距离为S3，
[0014] 则机器人起始位置连线与X方向的夹角θρ
[0016] 可得：
[0017] 2 Θ 1= 90-α
[0018] 上式即为第一被动全向轮6的转动方向与机器人坐标系Y方向的夹角α与Θ 间的定式关系，又α = 90 ° - Θ，则
[0019] Θ = 2 Θ 1；
[0020] 通过Θ与Θ i的关系可得到定位码盘的安装角；
[0021] 移动机器人只转动时，即定位码盘只转动，定位码盘两个被动全向轮的速度V/ = V2' = 0,陀螺仪反馈的角速度ω'与机器人转动角速度ω之间的转化关系：
[0022] ω，= ω ;
[0023] 移动机器人只移动，即定位码盘只有移动，定位码盘两个被动全向轮的速度 V/'，V2"与机器人坐标系速度分量Vx，Vy的转化关系
[0024] Vx= V2^
[0026] 移动机器人在移动过程中也会伴随着转动，因此全方位移动机器人定位码盘的两 个被动全向轮的速度V1= V / +V/'，V2= V 2' +V2"陀螺仪反馈的角速度ω '与机器人坐标系 速度分量^，Vy及机器人转动角速度ω之间的转化关系为
[0028] 机器人的姿态角改变时，机器人坐标系与世界坐标系不完全重合，需要进行坐标 系转化，初始时刻机器人坐标系X方向与世界坐标系X方向的夹角为β，经过t时刻，机器 人姿态角变化为γ，其转化关系为：
[0030] 由上述两个式子进行融合得世界坐标系速度分量Vx，Vy，机器人转动角速度ω与 机器人定位码盘的两个被动全向轮的速度V1, V2，陀螺仪反馈的角速度ω'之间的关系为：
[0032] 在机器人运动过程中，时刻对世界坐标系速度分量Vx、Vy、机器人转动角速度即陀 螺仪反馈的角速度ω进行积分，V x= V i，Vy= V 2，即可得到机器人在世界坐标系下的坐标 (X，y)及机器人姿态角Φ。
[0033] 本发明的优势在于：
[0034] 本发明是基于定位码盘和单轴光纤陀螺仪，即两个安装有全向轮的光电编码器垂 直分布属于机械装置，与基于电子元器件的无线传感反应网络系统、GPS导航定位系统等相 比，本发明测量装置简单，实施方便，且机械装置的稳定性高；
[0035] 本发明适用于编码器轴线不平行且互成任意角度分布，对编码器轴线的安装角度 要求较低，适用范围广；
[0036] 本发明通过5次左右的实验调试可以将机器人的定位精度控制在0. 5%以内，具 有较高的定位精度精度。
【附图说明】
[0037] 图1为本发明的移动机器人结构示意图；
[0038] 图2为机器人运动学模型示意图；
[0039] 图3为定位码盘上编码器的速度与机器人坐标系速度关系转化图；
[0040] 图4为定位码盘二维示意图；
[0041] 图5为机器人坐标系与世界坐标系转化关系示意图；
[0042] 图6为机器人定位控制系统示意图；
[0043] 图7为机器人定位过程流程示意图。
【具体实施方式】
[0044] 下面结合附图举例对本发明做更详细地描述：
[0045] 结合图1~7,本发明的技术方案是：一种室内移动机器人基于定位码盘及单轴光 纤陀螺仪的定位方法，包括全方位移动机器人、两个编码器轴线不平行且互成任意角度分 布的定位码盘和单轴光纤陀螺仪。
[0046] 本发明提供的是室内移动机器人基于定位码盘及陀螺仪的定位方法，具体实施步 骤如下：
[0047] 编码器1上安装有被动全向轮6,编码器2上安装有被动全向轮7,其都安装在码 盘安装架上，具体装配关系如图4所示，而定位码盘在移动机器人上的安装位置如图1所 示。机器人总体框架为正三角形，三个驱动全向轮8分别安装在三个顶点所在位置，机器 人框架最内侧的三个支撑方管互成60°，定位码盘通过码盘安装架固定在相邻的两个方管 上，其中被动全向轮7的转动方向与任意驱动全向轮8的轴线重合，且被动全向轮6的转动 方向与被动全向轮7的转动方向的交点与机器人的结合中心重合。
[0048] 建立世界坐标系xoy，始终以机器人初始时刻的几何中心为世界坐标系xoy的原 点，此原点的位置始终固定不变，以机器人的几何中心为原点建立机器人坐标系Χ0Υ，以定 位码盘的编码器2上被动全向轮7的转动方向为机器人坐标系的X方向，以定位码盘的编 码器1上被动全向轮6的转动方向为机器人坐标系的Y方向，机器人坐标系XOY的原点随 着机器人几何中心的改变而改变，默认定位码盘的编码器2上被动全向轮7的转动方向与 世界坐标系X方向重合来建立机器人运动学模型，定位码盘上被动全向轮6转动方向与被 动全向轮7的转动方向的夹角为Θ，定位码盘的编码器1上被动全向轮6的转动方向与世 界坐标系y方向的夹角为α，即定位码盘的安装误差角，α =90° -Θ ;
[0049] 机器人先沿X方向运动到机器人坐标系XOY下坐标（S，0)处，此时机器人在世界 坐标系xoy的位置为A，位置A的坐标为（S, -S1)，其中S1= Stan α ;
[0050] 机器人再沿Y方向运动到机器人坐标系XOY下的终点位置，其坐标为（S，S)，此时 机器人在世界坐标系xoy的B位置，而机器人的几何中心终点位置与起点位置连线与世界 坐标系X方向的夹角为Q1, B位置的坐标为（S, S2)，其中
[0051] 此时机器人几何中心起始位置的距离为S3，
[0053] 则机器人起始位置连线与X方向的夹角θρ
[0055] 可得：
[0056] 2 Θ 1= 90-α (1-1)
[0057] 上式即为定位码盘的编码器1上被动全向轮6的转动方向与机器人坐标系Y方向 的夹角α与Θ 间的定式关系。又α = 90° - Θ，结合式（1-1)可得
[0058] 0=20! (1-2)
[0059] 通过实验可测得机器人几何中心的起点位置与终点位置（S，S)的距离，结合机器 人的运动坐标可计算出S 1，通过Θ与Q1的关系可得到定位码盘的安装角。
[0060] 以机器人几何中心即定位码盘中心为原点建立机器人坐标系Χ0Υ，并且将定位码 盘的2编码器上被动全向轮7的转动方向作为机器人坐标系X方向，两个编码器轴线不平 行且互成任意角度，此时1编码器上被动全向轮6的转动方向与机器人坐标系Y方向不重 合；
[0061] 移动机器人只转动时，即定位码盘只转动，定位码盘两个被动全向轮的速度V/ = V2' = 0,陀螺仪反馈的角速度ω'与机器人转动角速度ω之间的转化关系：
[0062] ω' = ω (1-3)
[0063] 移动机器人只移动，即定位码盘只有移动，定位码盘两个被动全向轮的速度 V/'，V2"与机器人坐标系速度分量Vx，Vy的转化关系
[0066] 移动机器人在移动过程中也会伴随着转动，因此全方位移动机器人定位码盘的两 个被动全向轮的速度V1= V / +V/'，V2= V 2' +V2"陀螺仪反馈的角速度ω '与机器人坐标系 速度分量^，Vy及机器人转动角速度ω之间的转化关系为
[0068] 如图5所示，机器人的姿态角改变时，机器人坐标系与世界坐标系不完全重合，需 要进行坐标系转化，初始时刻机器人坐标系X方向与世界坐标系X方向的夹角为β，经过t 时刻，机器人姿态角变化为γ，其转化关系为：
[0070] 由式（1-5)、（1-6)进行融合可得世界坐标系速度分量Vx，Vy，机器人转动角速度 ω与机器人定位码盘的两个被动全向轮的速度1，V2，陀螺仪反馈的角速度ω'