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

摘要

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

主权项

一种室内移动机器人导航定位方法，其特征是：采用如下移动机器人机构：包括总体框架、定位码盘、第一被动全向轮、第二被动全向轮、第一编码器、第二编码器，总体框架为正三角形，三角形的三个顶点分别安装一个驱动全向轮，总体框架里安装三个支撑方管，相邻支撑方管之间夹角为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_2=\frac{S}{cos\alpha }-S_1=\frac{S(1-sin\alpha )}{cos\alpha };$此时机器人几何中心起始位置的距离为S₃，$S_3=\sqrt{S^2+S_2^2}=S\sqrt{1+{\left(\frac{1-\sin \alpha }{cos\alpha }\right)}^2}=S\sqrt{\frac{2}{1+sin\alpha }};$则机器人起始位置连线与x方向的夹角θ₁，${\mathrm{cos\theta }}_1^2=\frac{1+sin\alpha }{2}\to cos\left(2{\theta }_1\right)=\sin \alpha =cos(90-\alpha );$可得：2θ₁＝90‑α上式即为第一被动全向轮6的转动方向与机器人坐标系Y方向的夹角α与θ₁之间的定式关系，又α＝90°‑θ，则θ＝2θ₁；通过θ与θ₁的关系可得到定位码盘的安装角；移动机器人只转动时，即定位码盘只转动，定位码盘两个被动全向轮的速度V₁'＝V₂'＝0,陀螺仪反馈的角速度ω'与机器人转动角速度ω之间的转化关系：ω′＝ω；移动机器人只移动，即定位码盘只有移动，定位码盘两个被动全向轮的速度V₁”,V₂”与机器人坐标系速度分量V_X,V_Y的转化关系$\begin{array}{c}{V}_X=V_2^{\prime \prime }\\ V_Y=\frac{V_1^{\prime \prime }-V_2^{\prime \prime }sin\alpha }{\cos \alpha }\end{array};$移动机器人在移动过程中也会伴随着转动，因此全方位移动机器人定位码盘的两个被动全向轮的速度V₁＝V₁'+V₁”,V₂＝V₂'+V₂”陀螺仪反馈的角速度ω'与机器人坐标系速度分量V_X,V_Y及机器人转动角速度ω之间的转化关系为$\left[\begin{array}{c}{V}_X\\ V_Y\\ \omega \end{array}\right]=\left[\begin{array}{ccc}0& 1& 0\\ \frac{1}{\cos \alpha }& -tan\alpha & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{V}_1\\ V_2\\ {\omega }^{\prime }\end{array}\right];$机器人的姿态角改变时，机器人坐标系与世界坐标系不完全重合，需要进行坐标系转化，初始时刻机器人坐标系X方向与世界坐标系x方向的夹角为β，经过t时刻，机器人姿态角变化为γ，其转化关系为：$\left[\begin{array}{c}{V}_x\\ V_y\\ \omega \end{array}\right]=\left[\begin{array}{ccc}cos(\beta +\gamma )& -sin(\beta +\gamma )& 0\\ sin(\beta +\gamma )& \cos (\beta +\gamma )& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{V}_X\\ V_Y\\ \omega \end{array}\right];$由上述两个式子进行融合得世界坐标系速度分量V_x,V_y，机器人转动角速度ω与机器人定位码盘的两个被动全向轮的速度V₁,V₂，陀螺仪反馈的角速度ω'之间的关系为：$\left[\begin{array}{c}{V}_x\\ V_y\\ \omega \end{array}\right]=\left[\begin{array}{ccc}cos(\beta +\gamma )& -sin(\beta +\gamma )& 0\\ sin(\beta +\gamma )& cos(\beta +\gamma )& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}0& 1& 0\\ \frac{1}{\cos \alpha }& -tan\alpha & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{V}_1\\ V_2\\ {\omega }^{\prime }\end{array}\right];$在机器人运动过程中，时刻对世界坐标系速度分量V_x、V_y、机器人转动角速度即陀螺仪反馈的角速度ω进行积分，V_x＝V₁，V_y＝V₂，即可得到机器人在世界坐标系下的坐标(x,y)及机器人姿态角φ。