CN105300378B - 一种室内移动机器人导航定位方法 - Google Patents

Abstract

本发明的目的在于提供一种室内移动机器人导航定位方法，其基于两个编码器轴线不平行且互成任意角度分布的定位码盘及单轴光纤陀螺仪进行定位，定位码盘安装在室内移动机器人上，将定位数据时刻直接传输给控制系统，控制系统通过定位算法对编码器反馈回的数据进行处理，即可定位机器人坐标。本发明基于定位码盘和单轴光纤陀螺仪，即两个安装有全向轮的光电编码器垂直分布属于机械装置，与基于电子元器件的无线传感反应网络系统、GPS导航定位系统等相比，本发明测量装置简单，实施方便，且机械装置的稳定性高；通过5次左右的实验调试可以将机器人的定位精度控制在0.5％以内，具有较高的定位精度精度。

Description

一种室内移动机器人导航定位方法

技术领域

本发明涉及的是一种导航方法，具体地说是机器人的导航方法。

背景技术

目前关于移动机器人导航定位方法的研究，均是基于惯性导航、视觉导航、卫星导航等方法进行导航定位，其导航定位系统均借助于导航定位电子封装模块。对于移动机器人，定位技术非常重要，如果移动机器人没有定位功能，机器人的任何自主运动都是盲目的。移动机器人在运动过程中的三个关键问题是：现在何处？到何处去？如何去？

中国专利(CN1945351A)公开的一种基于无线传感反应网络对机器人进行导航定位的新系统和方法。此方法的无线传感反应网络系统相对较复杂，且对系统的稳定性能要求较高。

发明内容

本发明的目的在于提供适用于竞技机器人、餐厅服务机器人及工业移动机器人等领域的一种室内移动机器人导航定位方法。

本发明的目的是这样实现的：

本发明一种室内移动机器人导航定位方法，其特征是：

采用如下移动机器人机构：

包括总体框架、定位码盘、第一被动全向轮、第二被动全向轮、第一编码器、第二编码器，总体框架为正三角形，三角形的三个顶点分别安装一个驱动全向轮，总体框架里安装三个支撑方管，相邻支撑方管之间夹角为60度，L型的定位码盘安装在其中两个支撑方管上，第一被动全向轮和第二被动全向轮分别安装在L型定位码盘的两条边上，第一被动全向轮上安装第一编码器，第二被动全向轮上安装第二编码器，总体框架上安装有陀螺仪，在总体框架所在平面上，第二被动全向轮的直径与其旁边的驱动全向轮的轴线相重合，第一被动全向轮旋转方向所在平面与第二被动全向轮旋转方向所在平面以及总体框架所在平面相交的点为移动机器人的几何中心，

以初始时刻移动机器人的几何中心所在点为世界坐标系xoy的原点，此原点的位置始终固定不变，以移动机器人的几何中心为原点建立机器人坐标系XOY,此原点随移动机器人位置的改变而改变，第二被动全向轮旋转方向所在平面与总体框架所在平面的交线为坐标系XOY的X轴，第一被动全向轮旋转方向所在平面与总体框架所在平面的交线为坐标系XOY的Y轴，X轴与Y轴的交点即为移动机器人的几何中心，第一被动全向轮转动方向与第二被动全向轮转动方向的夹角为θ，第一被动全向轮转动方向与世界坐标系y方向的夹角为α，即定位码盘的安装误差角，α＝90°-θ；

机器人先沿X方向运动到机器人坐标系XOY下坐标(S,0)处，此时机器人在世界坐标系xoy的位置为A，位置A的坐标为(S,-S₁)，其中S₁＝Stanα；

此时机器人几何中心起始位置的距离为S₃，

则机器人起始位置连线与x方向的夹角θ₁，

可得：

2θ₁＝90-α

上式即为第一被动全向轮6的转动方向与机器人坐标系Y方向的夹角α与θ₁之间的定式关系，又α＝90°-θ，则

θ＝2θ₁；

通过θ与θ₁的关系可得到定位码盘的安装角；

移动机器人只转动时，即定位码盘只转动，定位码盘两个被动全向轮的速度V₁'＝V₂'＝0,陀螺仪反馈的角速度ω'与机器人转动角速度ω之间的转化关系：

ω′＝ω；

移动机器人只移动，即定位码盘只有移动，定位码盘两个被动全向轮的速度V₁”,V₂”与机器人坐标系速度分量V_X,V_Y的转化关系

V_X＝V₂”

移动机器人在移动过程中也会伴随着转动，因此全方位移动机器人定位码盘的两个被动全向轮的速度V₁＝V₁'+V₁”,V₂＝V₂'+V₂”陀螺仪反馈的角速度ω'与机器人坐标系速度分量V_X,V_Y及机器人转动角速度ω之间的转化关系为

机器人的姿态角改变时，机器人坐标系与世界坐标系不完全重合，需要进行坐标系转化，初始时刻机器人坐标系X方向与世界坐标系x方向的夹角为β，经过t时刻，机器人姿态角变化为γ，其转化关系为：

由上述两个式子进行融合得世界坐标系速度分量V_x,V_y，机器人转动角速度ω与机器人定位码盘的两个被动全向轮的速度V₁,V₂，陀螺仪反馈的角速度ω'之间的关系为：

在机器人运动过程中，时刻对世界坐标系速度分量V_x、V_y、机器人转动角速度即陀螺仪反馈的角速度ω进行积分，V_x＝V₁，V_y＝V₂，即可得到机器人在世界坐标系下的坐标(x,y)及机器人姿态角φ。

本发明的优势在于：

本发明是基于定位码盘和单轴光纤陀螺仪，即两个安装有全向轮的光电编码器垂直分布属于机械装置，与基于电子元器件的无线传感反应网络系统、GPS导航定位系统等相比，本发明测量装置简单，实施方便，且机械装置的稳定性高；

本发明适用于编码器轴线不平行且互成任意角度分布，对编码器轴线的安装角度要求较低，适用范围广；

本发明通过5次左右的实验调试可以将机器人的定位精度控制在0.5％以内，具有较高的定位精度精度。

附图说明

图1为本发明的移动机器人结构示意图；

图2为机器人运动学模型示意图；

图3为定位码盘上编码器的速度与机器人坐标系速度关系转化图；

图4为定位码盘二维示意图；

图5为机器人坐标系与世界坐标系转化关系示意图；

图6为机器人定位控制系统示意图；

图7为机器人定位过程流程示意图。

具体实施方式

下面结合附图举例对本发明做更详细地描述：

结合图1～7，本发明的技术方案是：一种室内移动机器人基于定位码盘及单轴光纤陀螺仪的定位方法，包括全方位移动机器人、两个编码器轴线不平行且互成任意角度分布的定位码盘和单轴光纤陀螺仪。

本发明提供的是室内移动机器人基于定位码盘及陀螺仪的定位方法，具体实施步骤如下：

编码器1上安装有被动全向轮6，编码器2上安装有被动全向轮7，其都安装在码盘安装架上，具体装配关系如图4所示，而定位码盘在移动机器人上的安装位置如图1所示。机器人总体框架为正三角形，三个驱动全向轮8分别安装在三个顶点所在位置，机器人框架最内侧的三个支撑方管互成60°，定位码盘通过码盘安装架固定在相邻的两个方管上，其中被动全向轮7的转动方向与任意驱动全向轮8的轴线重合，且被动全向轮6的转动方向与被动全向轮7的转动方向的交点与机器人的结合中心重合。

建立世界坐标系xoy，始终以机器人初始时刻的几何中心为世界坐标系xoy的原点，此原点的位置始终固定不变，以机器人的几何中心为原点建立机器人坐标系XOY,以定位码盘的编码器2上被动全向轮7的转动方向为机器人坐标系的X方向，以定位码盘的编码器1上被动全向轮6的转动方向为机器人坐标系的Y方向，机器人坐标系XOY的原点随着机器人几何中心的改变而改变，默认定位码盘的编码器2上被动全向轮7的转动方向与世界坐标系x方向重合来建立机器人运动学模型，定位码盘上被动全向轮6转动方向与被动全向轮7的转动方向的夹角为θ，定位码盘的编码器1上被动全向轮6的转动方向与世界坐标系y方向的夹角为α，即定位码盘的安装误差角，α＝90°-θ；

机器人先沿X方向运动到机器人坐标系XOY下坐标(S,0)处，此时机器人在世界坐标系xoy的位置为A，位置A的坐标为(S,-S₁)，其中S₁＝Stanα；

此时机器人几何中心起始位置的距离为S₃，

则机器人起始位置连线与x方向的夹角θ₁，

可得：

2θ₁＝90-α (1-1)

上式即为定位码盘的编码器1上被动全向轮6的转动方向与机器人坐标系Y方向的夹角α与θ₁之间的定式关系。又α＝90°-θ，结合式(1-1)可得

θ＝2θ₁ (1-2)

通过实验可测得机器人几何中心的起点位置与终点位置(S,S)的距离，结合机器人的运动坐标可计算出θ₁，通过θ与θ₁的关系可得到定位码盘的安装角。

以机器人几何中心即定位码盘中心为原点建立机器人坐标系XOY，并且将定位码盘的2编码器上被动全向轮7的转动方向作为机器人坐标系X方向，两个编码器轴线不平行且互成任意角度，此时1编码器上被动全向轮6的转动方向与机器人坐标系Y方向不重合；

移动机器人只转动时，即定位码盘只转动，定位码盘两个被动全向轮的速度V₁'＝V₂'＝0,陀螺仪反馈的角速度ω'与机器人转动角速度ω之间的转化关系：

ω'＝ω (1-3)

移动机器人只移动，即定位码盘只有移动，定位码盘两个被动全向轮的速度V₁”,V₂”与机器人坐标系速度分量V_X,V_Y的转化关系

V_X＝V₂”

移动机器人在移动过程中也会伴随着转动，因此全方位移动机器人定位码盘的两个被动全向轮的速度V₁＝V₁'+V₁”,V₂＝V₂'+V₂”陀螺仪反馈的角速度ω'与机器人坐标系速度分量V_X,V_Y及机器人转动角速度ω之间的转化关系为

如图5所示，机器人的姿态角改变时，机器人坐标系与世界坐标系不完全重合，需要进行坐标系转化，初始时刻机器人坐标系X方向与世界坐标系x方向的夹角为β，经过t时刻，机器人姿态角变化为γ，其转化关系为：

由式(1-5)、(1-6)进行融合可得世界坐标系速度分量V_x,V_y，机器人转动角速度ω与机器人定位码盘的两个被动全向轮的速度V₁,V₂，陀螺仪反馈的角速度ω'之间的关系为：

在机器人运动过程中，实际上机器人的控制系统是对定位码盘的两个被动全向轮的速度V₁,V₂及陀螺仪反馈的角速度ω；时刻对世界坐标系速度分量V_x,V_y，(V_x＝V₁，V_y＝V₂)机器人转动角速度即陀螺仪反馈的角速度ω进行积分并时刻按照关系进行转化，即可得到机器人在世界坐标系下的坐标(x,y)及机器人姿态角φ。

移动机器人驱动全向轮轮8、定位码盘、编码器1和编码器2、光纤陀螺仪3的位置关系如图1所示，移动机器人在控制系统的控制作用下进行运动时，编码器1和编码器2才会反馈给控制系统新的数据即机器人在机器人坐标系XOY下的坐标，其中以机器人的几何中心为原点建立机器人坐标系XOY,以定位码盘的编码器2上被动全向轮7的转动方向为机器人坐标系的X方向，以定位码盘的编码器1上被动全向轮6的转动方向为机器人坐标系的Y方向，机器人坐标系XOY的原点随着机器人几何中心的改变而改变。

编码器1上安装有被动全向轮6，编码器2上安装有被动全向轮7，其都安装在码盘安装架上，具体装配关系如图4所示，而定位码盘在移动机器人上的安装位置如图1所示。机器人总体框架为正三角形，三个驱动全向轮8分别安装在三个顶点所在位置，机器人框架最内侧的三个支撑方管互成60°，定位码盘通过码盘安装架固定在相邻的两个方管上，其中被动全向轮7的转动方向与任意驱动全向轮8的轴线重合，且被动全向轮6的转动方向与被动全向轮7的转动方向的交点与机器人的结合中心重合。

机器人控制系统会将编码器1和编码器2才会反馈的数据与目标点坐标进行比较，若未到达目标点，机器人继续运动；若到达目标点，机器人停止运动。

建立世界坐标系xoy，始终以机器人初始时刻的几何中心为世界坐标系xoy的原点，此原点的位置始终固定不变，被动全向轮安装在编码器的旋转轴上，其整体安装在码盘固定架上，其安装位置关系如图2所示，定位码盘的编码器1上被动全向轮6的转动方向与世界坐标系y方向的夹角为α，α＝90°-θ，θ为定位码盘上被动全向轮6转动方向与被动全向轮7的转动方向的夹角，α为矢量，当安装角小于90°时，α为正值；当安装角大于90°时，α为负值；根据机器人运动学模型，机器人定位码盘的安装误差测量方法可表示如下：

机器人先沿X方向运动到机器人坐标系XOY下坐标(S,0)处，此时机器人在世界坐标系xoy的A位置，A位置的坐标为(S,-S₁)，其中S₁＝Stanα；

此时机器人几何中心起始位置的距离为S₃，其中

则机器人起始位置连线与x方向的夹角θ₁，

可得：

2θ₁＝90-α (1-8)

上式即为定位码盘的编码器1上被动全向轮6的转动方向与机器人坐标系Y方向的夹角α与θ₁之间的定式关系。又α＝90°-θ，结合式(1-8)可得

θ＝2θ₁ (1-9)

在实验过程中可测得S₃，再结合S可求得θ₁，进一步可得定位码盘安装角θ。

若给定的S＝1000mm，测得的数据S₃＝1380mm，则θ₁＝43.6°，此时编码器的安装角θ＝2θ₁＝87.2°，α＝90°-2θ＝2.8°。

以机器人几何中心即定位码盘中心为原点建立机器人坐标系XOY，并且将定位码盘的编码器2上被动全向轮7的转动方向作为机器人坐标系X方向，两个编码器轴线不平行且互成任意角度，此时编码器1上被动全向轮6的转动方向与机器人坐标系Y方向不重合；

移动机器人只转动时，即定位码盘只转动，定位码盘两个被动全向轮的速度V₁'＝V₂'＝0,陀螺仪反馈的角速度ω'与机器人转动角速度ω之间的转化关系：

ω'＝ω (1-10)

移动机器人只移动，即定位码盘只有移动，定位码盘两个被动全向轮的速度V₁”,V₂”与机器人坐标系速度分量V_X,V_Y的转化关系

V_X＝V₂”

移动机器人在移动过程中也会伴随着转动，因此全方位移动机器人定位码盘的两个被动全向轮的速度V₁＝V₁'+V₁”,V₂＝V₂'+V₂”陀螺仪反馈的角速度ω'与机器人坐标系速度分量V_X,V_Y及机器人转动角速度ω之间的转化关系为

将计算得出的α＝2.8°带入上式可得V_X＝V₂,V_Y＝1.001V₁-0.05V₂。

如图5所示，机器人的姿态角改变时，机器人坐标系与世界坐标系不完全重合，需要进行坐标系转化，转化关系为：

假定机器人初始时刻机器人坐标系XOY的X方向与世界坐标系xoy的x方向的偏差角为β，若假定β＝2°，经过时间t机器人的姿态角变化为γ，若假定γ＝10°，则V_x＝0.978V_X-0.208V_Y,V_y＝0.208V_X+0.978V_Y。

由式(1-12)、(1-13)进行融合可得世界坐标系速度分量V_x,V_y，机器人转动角速度ω与机器人定位码盘的两个被动全向轮的速度V₁,V₂，陀螺仪反馈的角速度ω'之间的关系为：

根据已知条件α＝2.8°,β＝2°,γ＝10°，可得，V_x＝-0.208V₁+0.988V₂，V_y＝0.988V₁+0.159V₂，ω＝ω'。

在机器人运动过程中，实际上机器人的控制系统是对定位码盘的两个被动全向轮的速度V₁,V₂及陀螺仪反馈的角速度ω；时刻对世界坐标系速度分量V_x,V_y，(V_x＝V₁，V_y＝V₂)机器人转动角速度即陀螺仪反馈的角速度ω进行积分并时刻按照关系进行转化，即可得到机器人在世界坐标系下的坐标(x,y)及机器人姿态角φ。

Claims (1)

1.一种室内移动机器人导航定位方法，其特征是：

采用如下移动机器人机构：

包括总体框架、定位码盘、第一被动全向轮、第二被动全向轮、第一编码器、第二编码器，总体框架为正三角形，三角形的三个顶点分别安装一个驱动全向轮，总体框架里安装三个支撑方管，相邻支撑方管之间夹角为60度，L型的定位码盘安装在其中两个支撑方管上，第一被动全向轮和第二被动全向轮分别安装在L型定位码盘的两条边上，第一被动全向轮上安装第一编码器，第二被动全向轮上安装第二编码器，总体框架上安装有陀螺仪，在总体框架所在平面上，第二被动全向轮的直径与其旁边的驱动全向轮的轴线相重合，第一被动全向轮旋转方向所在平面与第二被动全向轮旋转方向所在平面以及总体框架所在平面相交的点为移动机器人的几何中心，

以初始时刻移动机器人的几何中心所在点为世界坐标系xoy的原点，此原点的位置始终固定不变，以移动机器人的几何中心为原点建立机器人坐标系XOY,此原点随移动机器人位置的改变而改变，第二被动全向轮旋转方向所在平面与总体框架所在平面的交线为坐标系XOY的X轴，第一被动全向轮旋转方向所在平面与总体框架所在平面的交线为坐标系XOY的Y轴，X轴与Y轴的交点即为移动机器人的几何中心，第一被动全向轮转动方向与第二被动全向轮转动方向的夹角为θ，第一被动全向轮转动方向与世界坐标系y方向的夹角为α，即定位码盘的安装误差角，α＝90°-θ；

机器人先沿X方向运动到机器人坐标系XOY下坐标(S,0)处，此时机器人在世界坐标系xoy的位置为A，位置A的坐标为(S,-S₁)，其中S₁＝Stanα；

机器人再沿Y方向运动到机器人坐标系XOY下坐标(S,S)处，此位置为终点位置，此时机器人在世界坐标系xoy的B位置，B位置的坐标为(S,S₂)，机器人的几何中心终点位置与起点位置连线与世界坐标系x方向的夹角为θ₁，其中

此时机器人几何中心起始位置的距离为S₃，

则机器人的几何中心终点位置与起点位置连线与世界坐标系x方向的夹角θ₁，

可得：

2θ₁＝90-α

上式即为第一被动全向轮6的转动方向与机器人坐标系Y方向的夹角α与θ₁之间的定式关系，又α＝90°-θ，则

θ＝2θ₁；

通过θ与θ₁的关系可得到定位码盘的安装角；

移动机器人只转动时，即定位码盘只转动，定位码盘两个被动全向轮的速度V₁'＝V₂'＝0,陀螺仪反馈的角速度ω'与机器人转动角速度ω之间的转化关系：

ω′＝ω；

移动机器人只移动，即定位码盘只有移动，定位码盘两个被动全向轮的速度V₁”,V₂”与机器人坐标系速度分量V_X,V_Y的转化关系

V_X＝V₂”

移动机器人在移动过程中也会伴随着转动，因此全方位移动机器人定位码盘的两个被动全向轮的速度V₁＝V₁'+V₁”,V₂＝V₂'+V₂”，陀螺仪反馈的角速度ω'与机器人坐标系速度分量V_X,V_Y及机器人转动角速度ω之间的转化关系为

机器人的姿态角改变时，机器人坐标系与世界坐标系不完全重合，需要进行坐标系转化，初始时刻机器人坐标系X方向与世界坐标系x方向的夹角为β，经过t时刻，机器人姿态角变化为γ，其转化关系为：

由上述两个式子进行融合得世界坐标系速度分量V_x,V_y，机器人转动角速度ω与机器人定位码盘的两个被动全向轮的速度V₁,V₂，陀螺仪反馈的角速度ω'之间的关系为：

在机器人运动过程中，时刻对世界坐标系速度分量V_x、V_y、机器人转动角速度即陀螺仪反馈的角速度ω进行积分，V_x＝V₁，V_y＝V₂，即可得到机器人在世界坐标系下的坐标(x,y)及机器人姿态角φ。