CN110793513A - 一种正交式平面移动机器人位姿检测装置与检测方法 - Google Patents

Abstract

本发明提供了一种正交式平面移动机器人位姿检测装置与检测方法，其中，正交式平面移动机器人位姿检测装置包括：整体固定块、支撑框架、以及四个从动轮组；所述整体固定块设置在支撑框架上，每个从动轮组包括：从动轮支架、全方位从动轮、连接机构、滑动机构和线性速度采集模块，从动轮组既能绕从动轮组轮轴转动又能在垂直于全方位从动轮轮轴的竖直方向上滑动，且时刻保持与地面接触，无需参照物位置或其他己知的参照信息，就可以计算出移动机器人的位置和姿态。

Description

一种正交式平面移动机器人位姿检测装置与检测方法

技术领域

本发明涉及移动机器人位姿检测技术领域，具体涉及一种正交式平面移动机器人位姿检测装置与检测方法。

背景技术

移动机器人在人类生产、生活中得到越来越广泛的应用，如各类竞赛机器人、车间搬运机器人、清洁机器人及各种服务机器人。移动机器人的底盘类型主要有：履带式、差动轮式、全向轮式等。对于移动机器人，位姿检测技术非常重要，所谓位姿检测是要确定机器人在环境中的实时位姿。如果移动机器人没有位姿检测功能，机器人的任何自主运动都是盲目的。移动机器人在运动过程中的三个关键问题是：现在何处？到何处去？如何去？

在工业机器人领域，主要有两种位姿检测方式：一部分机器人是通过在各类移动机器人的底盘主动轮上安装编码器，通过编码器得到主动轮转动的角度，进而计算出机器人相对于上一采样时刻位置和姿态的改变量，通过位移量的累积就可估算机器人的位置。但这类定位方式具有较大的局限性：由于编码器安装在主动轮上以测量主动轮转动的角度，但当机器人遇到阻力过大、地面崎岖或倾斜的情况，会导致某一轮悬空或打滑，引起较大的位姿检测误差，严重影响了机器人位姿检测的精度。

另一部分机器人是采用巡线或巡磁的方式进行位姿检测，通过使机器人单轴移动的方式，提高机器人的定位精度。但这种方法大大的限制了机器人的移动范围和实用性，这在实际工业生产中远远不够，人们更希望实现多方向的高精度位姿检测。

发明内容

本发明旨在至少解决现有技术或相关技术中存在的技术问题之一。

为此，本发明的第一方面提出一种正交式平面移动机器人位姿检测装置。

本发明的第二方面提出一种正交式平面移动机器人位姿检测方法。

有鉴于此，本发明第一方面提供了一种正交式平面移动机器人位姿检测装置，包括：整体固定块、支撑框架、以及四个从动轮组；所述整体固定块设置在支撑框架上，整体固定块上设置有安装孔，用于连接移动机器人，每个从动轮组包括：从动轮支架、全方位从动轮、连接机构、滑动机构和线性速度采集模块，全方位从动轮的轮轴与从动轮支架相连接，全方位从动轮套设于轮轴上，使全方位从动轮绕轮轴在地面滚动；线性速度采集模块设置在从动轮支架上；连接机构分别与从动轮支架和支撑框架连接，使从动轮组沿支撑框架周向均匀设置，且相邻的全方位从动轮的轮轴相互垂直；滑动机构分别与从动轮支架和整体固定块连接，使从动轮组在垂直于全方位从动轮轮轴的竖直方向上滑动时仍保持与地面接触滚动。

优选地，所述从动轮支架包括：上盖板、内侧面盖板、外侧面盖板和两个支撑柱，内侧面盖板和外侧面盖板平行设置，内侧面盖板和外侧面盖板的上端均与上盖板连接，两支撑柱平行设置在内侧面盖板和外侧面盖板之间，支撑柱的两端分别与内侧面盖板和外侧面盖板相连接，所述线性速度采集模块设置在靠近全方位从动轮轮轴处的外侧面盖板上。

优选地，所述从动轮支架还包括滑动限位板，滑动限位板数量为两个，分别设置在上盖板的两侧面上。

优选地，所述从动轮支架还包括固定柱，固定柱的数量为两个，且平行设置，固定柱的两端分别与两滑动限位板相连接。

优选地，所述从动轮支架还包括两个弹簧限位部，且分别套设在两固定柱上。

优选地，所述支撑框架的顶点处周向设置有四个连接部，每个连接部上设置有对称的连接孔。

优选地，所述连接机构为拉簧结构，包括两个拉簧，拉簧的一端与固定柱连接，且位于弹簧限位部与滑动限位板之间，另一端与支撑框架的连接孔连接。

优选地，所述滑动机构包括滑块和滑轨，所述滑块连接在整体固定块上，滑轨位于内侧面盖板上，且沿垂直于全方位从动轮轮轴的竖直方向设置，滑块与滑轨相配合。

优选地，所述从动轮支架还包括限位螺钉，限位螺钉的数量为两个，分别设置在滑轨底端两侧，滑轨的上端面距滑动限位板的距离小于滑块的高度。

本发明的第二方面提供了一种采用上述任一技术方案所述的正交式平面移动机器人位姿检测装置进行移动机器人位姿检测的检测方法，包括如下步骤：

步骤1：定义在水平面车体坐标系XOY中，其中以车体中心为中心点O、OX方向位于移动机器人进行方向、OY方向为移动机器人行进的垂直方向，移动机器人位于世界坐标系XOY的初始位置坐标值x、y、θ₀为原点；

步骤2：通过线性速度采集模块获得每个全方位从动轮的所转过的角度θ₁、θ₂、θ₃、θ₄，已知各全方位从动轮半径为R，可得各全方位从动轮移动的距离l₁、l₂、l₃、l₄如下:

l_n＝θ_n*R(n＝1,2,3,4)

步骤3、通过步骤2得到各全方位从动轮移动的距离l₁、l₂、l₃、l₄，且已知相邻的全方位从动轮的轮轴相互垂直，可计算出移动机器人相对于自身旋转的角度θ：

步骤4：通过所得移动机器人相对于自身旋转的角度θ以及每一瞬时全方位从动轮转过的距离d_l1、d_l2、d_l3、d_l4,得到移动机器人的位于每一瞬时相对于以自身为原点的XOY坐标系的OX和OY方向的位移如下：

步骤5、将每一瞬时OX和OY方向的位移dX，dY进行积分，得到移动机器人相对于原点位置的OX和OY方向的位移△X和△Y。通过如下公式：

得到移动机器人相对于原点位置OX、OY方向的位移△X和△Y，相对于自身旋转的角度θ；

其中上述公式中的各个参数表示的含义如下：

x、y、θ₀：为移动机器人初始位置的坐标值；

θ₁、θ₂、θ₃、θ₄：分别为根据线性速度采集模块采集的信息获得的四个全方位从动轮的瞬时角速度；

θ：为携带定位装置的移动机器人运动结束时的位置相对于原点位置自身所旋转的角度；

l₁、l₂、l₃、l₄：分别为四个全方位从动轮分别移动的距离；

d_l1、d_l2、d_l3、d_l4：分别为四个全方位从动轮每一瞬时各全方位从动轮转过的距离；

dX，dY：为移动机器人的位于每一瞬时相对于以自身为原点的XOY坐标系的OX和OY方向的位移；

△X和△Y：分别为移动机器人相对于原点位置的OX和OY方向的位移。

与现有技术相比，本发明的有益效果是：

1、本装置体积小，重量轻，有配置的安装孔，安装方便，测量精准；

2、本装置的四个全方位从动轮可沿任意方向移动，避免了四个全方位从动轮打滑现象对位姿检测精度的影响，具有很高的位姿检测精度；

3、进行位姿检测时，无需参照物位置或其他己知的参照信息，就可以计算出移动机器人的位置和姿态；

4、适用性强，可适用于各种机器人底盘，也可安装在移动机器人底盘的任意位置，对底盘的要求较小；

5、无需陀螺仪机构，因此对温度、湿度等外界环境的变化不敏感，避免了无温漂、零漂等现象，也避免了在机器人静止时，因外部环境的变化而出现机器人原地旋转、晃动的情况。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

图1示出了根据本发明的一个实施例的正交式平面移动机器人位姿检测装置的结构示意图；

图2示出了根据本发明的又一个实施例的正交式平面移动机器人位姿检测装置的结构示意图；

图3示出了根据本发明的再一个实施例的正交式平面移动机器人位姿检测装置的结构示意图；

图4示出了根据本发明的一个实施例的正交式平面移动机器人位姿检测装置的从动轮组的结构示意图；

图5示出了根据本发明的又一个实施例的交式平面移动机器人位姿检测装置的从动轮组的结构示意图；

图6示出了根据本发明的一个实施例的交式平面移动机器人位姿检测装置的全方位从动轮的结构示意图；

图7示出了根据本发明的一个实施例的交式平面移动机器人位姿检测装置的全方位从动轮结构的侧视图；

其中：1整体固定块；11安装孔；2支撑框架；21连接部；211连接孔；3从动轮组；31从动轮支架；310上盖板；311内侧面盖板；312外侧面盖板；313支撑柱；314滑动限位板；315固定柱；316弹簧限位部；317限位螺钉；32全方位从动轮；321轮轴；331拉簧；341滑块；342滑轨；35线性速度采集模块。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

下面参照图1至图7描述根据本发明一些实施例所述正交式平面移动机器人位姿检测装置与检测方法。

在本发明第一方面的实施例中，如图1至图5所示，本发明提供了一种正交式平面移动机器人位姿检测装置，包括：整体固定块1、支撑框架2、以及四个从动轮组3；整体固定块1设置在支撑框架2上，整体固定块1上设置有安装孔11，用于连接移动机器人，每个从动轮组3包括：从动轮支架31、全方位从动轮32、连接机构、滑动机构和线性速度采集模块35，全方位从动轮32的轮轴321与从动轮支架31相连接，全方位从动轮32装配于轮轴321上，使全方位从动轮32绕轮轴321在地面滚动；线性速度采集模块35设置在从动轮支架31上；连接机构分别与从动轮支架31和支撑框架2连接，使从动轮组3沿支撑框架2周向均匀设置，且相邻的全方位从动轮32的轮轴321相互垂直；滑动机构分别与从动轮支架31和整体固定块1连接，使从动轮组3在垂直于全方位从动轮32轮轴321的竖直方向上滑动时仍保持与地面接触滚动。

在该实施例中，支撑框架2呈近似正方形状，各边均相同，整体固定块1呈近似正四棱柱状，且中间中空的框架结构，整体固定块1的上表面上设置有安装孔11，用于安装移动机器人，整体固定块1的下表面通过螺钉和螺孔的配合安装在支撑框架2上，从动轮组3包括：从动轮支架31、全方位从动轮32、连接机构、滑动机构和线性速度采集模块35，全方位从动轮32的轮轴321与从动轮支架31连接，全方位从动轮32装配于轮轴321上，使全方位从动轮32绕轮轴321转动，连接机构分别与从动轮组3和支撑框架2连接，使四个从动轮组3沿支撑框架2四边周向均匀设置，且相邻的全方位从动轮32的轮轴321相互垂直，从动轮组3通过滑动机构与整体固定块1连接，使从动轮组3既能绕从动轮组3轮轴321转动又能在垂直于从动轮组3轮轴321的竖直方向上滑动，进而保持时刻与地面接触，线性速度采集模块35设置在从动轮支架31上，用于实时采集每个全方位从动轮32的线性速度以及转动角度，检测全方位从动轮32的运动状态信息，线性速度采集模块35可搭载其他处理系统或分析模块，对线性速度采集模块35所采集的数据进行分析和处理，在具体实施例中，线性速度采集模块35采用速度传感器，处理系统采用PCB的小型计算机处理系统。

在本发明的一个实施例中，优选地，如图1至图5所示，从动轮支架31包括：上盖板310、内侧面盖板311、外侧面盖板312和两个支撑柱313，内侧面盖板311和外侧面盖板312平行设置，内侧面盖板311和外侧面盖板312的上端均与上盖板310连接，两支撑柱313平行设置在内侧面盖板311和外侧面盖板312之间，支撑柱313的两端分别与内侧面盖板311和外侧面盖板312相连接，线性速度采集模块35设置在靠近全方位从动轮32轮轴321处的外侧面盖板312上。

在该实施例中，内侧面盖板311和外侧面盖板312平行设置，且通过上盖板310和两支撑柱313连接，上盖板310位于内侧面盖板311和外侧面盖板312的上端，且内侧面盖板311和外侧面盖板312的顶端均与上盖板310连接，两支撑柱313平行设置在内侧面盖板311和外侧面盖板312之间，且支撑柱313的两端分别通过螺钉与外侧面盖板312和内侧面盖板311连接，线性速度采集块通过螺钉设置在外侧面盖板312上，且正对全方位从动轮32的轮轴321，使线性速度采集模块35采集数据更加精确，在实际操作中上盖板310、内侧面盖板311和外侧面盖板312以及两个支撑柱313可为一体成型。

在本发明的一个实施例中，优选地，如图1至图5所示，从动轮支架31还包括滑动限位板314，滑动限位板314数量为两个，分别设置在上盖板310的两侧面上。

在该实施例中，每个从动轮支架31还包括两个滑动限位板314，两个滑动限位板314分别通过螺钉固定在上盖板310的两侧面上。

在本发明的一个实施例中，优选地，如图1至图5所示，从动轮支架31还包括固定柱315，固定柱315的数量为两个，且平行设置，固定柱315的两端分别与两滑动限位板314相连接。

在该实施例中，每个从动轮支架31还包括两个固定柱315，两固定柱315平行设置在两滑动限位板314之间，固定柱315的两端分别通过螺钉与设置在上盖板310两侧面上的滑动限位板314连接。

在本发明的一个实施例中，优选地，如图1、图3和图4所示，从动轮支架31还包括弹簧限位部316，数量为两个，且分别套设在两固定柱315上。

在该实施例中，每个从动轮支架31还包括两个弹簧限位部316，弹簧限位部316为环形结构，例如卡环，分别套设在两固定柱315上，在具体操作中，弹簧限位部316也可与上盖板310、内侧面盖板311、外侧面盖板312以及两个支撑柱313一体成型。

在本发明的一个实施例中，优选地，如图1和图3所示，所述支撑框架2的顶点处周向设置有四个连接部21，每个连接部21上设置有对称的连接孔211。

在该实施例中，沿支撑架周向设置的四个角的顶点处设置有四个连接部21，每个连接部21上均设置有左右对称的连接孔211。

在本发明的一个实施例中，优选地，如图1至图5所示，连接机构为拉簧331结构，包括两个拉簧331，拉簧331的一端与固定柱315连接，且位于弹簧限位部316与滑动限位板314之间，另一端与支撑框架2的连接孔211连接。

在该实施例中，连接机构为两个拉簧331，每个从动轮组3有两个拉簧331，拉簧331的一端套设在固定柱315上，另一端套设在支撑框架2的连接孔211上，连接机构用于向从动轮组3施加向下的力，使从动轮组3在垂直于全方位从动轮32轮轴321的竖直方向上滑动时，不会因滑动过量而造成全方位从动轮32悬空，进而离开地面的状况出现，同时，拉簧331套设于固定柱315的一端位于弹簧限位部316和滑动限位板314之间，使其仅在弹簧限位部316和滑动限位板314之间移动，从而防止拉簧331在固定柱315移动距离过大，导致施力效果不佳。

在本发明的一个实施例中，优选地，如图1至图5所示，滑动机构包括滑块341和滑轨342，滑块341设置在整体固定块1上，滑轨342位于内侧面盖板311上，且沿垂直于全方位从动轮32轮轴321的竖直方向设置，滑块341与滑轨342相配合。

在该实施例中，滑动机构包括滑块341和滑轨342，每个从动轮组3有一个滑轨342和一个滑块341相互配合滑动，滑块341通过螺钉固定在整体固定块1上，滑轨342通过螺钉设置在内侧面盖板311上，且沿垂直于全方位从动轮32轮轴321的竖直方向设置，滑块341与滑轨342相配合，当从动轮组3在凸凹的地面移动时，位于内侧面盖板311上的滑轨342相对于固定在整体固定块1的滑块341在竖直方向上滑动，进而使从动轮组3在竖直方向上下移动，同时连接机构对从动轮组3施加向下的力，保证全方位从动轮32时刻紧贴底面，既不会使压力过量，也不会使全方位从动轮32悬空，全方位从动轮32与机器人能保持相同的运动状态，测量精确度高。

在本发明的一个实施例中，优选地，如图4所示，从动轮支架31还包括限位螺钉317，限位螺钉317的数量为两个，分别设置在滑轨342下端面的两侧，滑轨342的上端面距滑动限位板314的距离小于滑块341的高度。

在该实施例中，滑轨342的上端设置有滑动限位板314，且滑轨342的上端面距滑动限位板314的距离小于滑块341的高度，滑轨342下端面的两侧设置有限位螺钉317，保证滑块341在滑轨342上移动且不会从滑轨342滑出，避免了滑块341脱落破坏装置的危险。

在本发明第二方面实施例中，本发明提供了采用正交式平面移动机器人位姿检测装置进行移动机器人位姿检测的检测方法，包括如下步骤：

步骤1：定义在水平面车体坐标系XOY中，其中以车体中心为中心点O、OX方向位于移动机器人进行方向、OY方向为移动机器人行进的垂直方向，移动机器人位于世界坐标系XOY的初始位置坐标值x、y、θ₀为原点；

步骤2：通过线性速度采集模块35获得每个全方位从动轮32的所转过的角度θ₁、θ₂、θ₃、θ₄，已知各全方位从动轮32半径为R，可得各全方位从动轮32移动的距离l₁、l₂、l₃、l₄如下:

l_n＝θ_n*R(n＝1,2,3,4)

步骤3、通过步骤2得到各全方位从动轮32移动的距离l₁、l₂、l₃、l₄，且已知相邻的全方位从动轮32的轮轴321相互垂直，可计算出移动机器人相对于自身旋转的角度θ：

步骤4：通过所得移动机器人相对于自身旋转的角度θ以及每一瞬时全方位从动轮32转过的距离d_l1、d_l2、d_l3、d_l4,得到移动机器人的位于每一瞬时相对于以自身为原点的XOY坐标系的OX和OY方向的位移如下：

步骤5、将每一瞬时OX和OY方向的位移dX，dY进行积分，得到移动机器人相对于原点位置的OX和OY方向的位移△X和△Y。通过如下公式：

得到移动机器人相对于原点位置OX、OY方向的位移△X和△Y，相对于自身旋转的角度θ；

其中上述公式中的各个参数表示的含义如下：

x、y、θ₀：为移动机器人初始位置的坐标值；

θ₁、θ₂、θ₃、θ₄：分别为根据线性速度采集模块35采集的信息获得的四个全方位从动轮32的瞬时角速度；

θ：为携带定位装置的移动机器人运动结束时的位置相对于原点位置自身所旋转的角度；

l₁、l₂、l₃、l₄：分别为四个全方位从动轮32分别移动的距离；

d_l1、d_l2、d_l3、d_l4：分别为四个全方位从动轮32每一瞬时各全方位从动轮转过的距离；

dX，dY：为移动机器人的位于每一瞬时相对于以自身为原点的XOY坐标系的OX和OY方向的位移；

△X和△Y：分别为移动机器人相对于原点位置的OX和OY方向的位移。

在该实施例中，通过位于从动轮组3上的线速度采集模块采集到每个全方位从动轮32的瞬时角速度，计算出移动机器人终止位置相对于原点位置自身所旋转的角度，以及移动机器人相对于原点位置的OX和OY方向的位移。

在本发明的描述中，术语“多个”则指两个或两个以上，除非另有明确的限定，术语“上”、“下”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制；术语“连接”、“安装”、“固定”等均应做广义理解，例如，“连接”可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是直接相连，也可以通过中间媒介间接相连。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明的描述中，术语“一个实施例”、“一些实施例”、“具体实施例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或特点包含于本发明的至少一个实施例或示例中。在本发明中，对上述术语的示意性表述不一定指的是相同的实施例或实例。而且，描述的具体特征、结构、材料或特点可以在任何的一个或多个实施例或示例中。

Claims (10)

1.一种正交式平面移动机器人位姿检测装置，其特征在于，包括：整体固定块、支撑框架、以及四个从动轮组；所述整体固定块设置在支撑框架上，整体固定块上设置有安装孔，用于连接移动机器人，每个从动轮组包括：从动轮支架、全方位从动轮、连接机构、滑动机构和线性速度采集模块，全方位从动轮的轮轴与从动轮支架相连接，全方位从动轮装配于轮轴上，使全方位从动轮绕轮轴在地面滚动；线性速度采集模块设置在从动轮支架上；连接机构分别与从动轮支架和支撑框架连接，使从动轮组沿支撑框架周向均匀设置，且相邻的全方位从动轮的轮轴相互垂直；滑动机构分别与从动轮支架和整体固定块连接，使从动轮组在垂直于全方位从动轮轮轴的竖直方向上滑动时仍保持与地面接触滚动。

2.根据权利要求1所述的正交式平面移动机器人位姿检测装置，其特征在于，所述从动轮支架包括：上盖板、内侧面盖板、外侧面盖板和两个支撑柱，内侧面盖板和外侧面盖板平行设置，内侧面盖板和外侧面盖板的上端均与上盖板连接，两支撑柱平行设置在内侧面盖板和外侧面盖板之间，支撑柱的两端分别与内侧面盖板和外侧面盖板相连接，所述线性速度采集模块设置在靠近全方位从动轮轮轴处的外侧面盖板上。

3.根据权利要求2所述的正交式平面移动机器人位姿检测装置，其特征在于，所述从动轮支架还包括：滑动限位板，滑动限位板数量为两个，分别设置在上盖板的两侧面上。

4.根据权利要求3所述的正交式平面移动机器人位姿检测装置，其特征在于，所述从动轮支架还包括：固定柱，固定柱的数量为两个，且平行设置，固定柱的两端分别与两滑动限位板相连接。

5.根据权利要求4所述的正交式平面移动机器人位姿检测装置，其特征在于，所述从动轮支架还包括:两个弹簧限位部，且分别套设在两固定柱上。

6.根据权利要求5所述的，其特征在于，所述支撑框架的顶点处周向设置有四个连接部，每个连接部上设置有对称的连接孔。

7.根据权利要求6所述的正交式平面移动机器人位姿检测装置，其特征在于，所述连接机构为拉簧结构，包括：两个拉簧，拉簧的一端与固定柱连接，且位于弹簧限位部与滑动限位板之间，另一端与支撑框架的连接孔连接。

8.根据权利要求1所述的正交式平面移动机器人位姿检测装置，其特征在于，所述滑动机构包括：滑块和滑轨，所述滑块设置在整体固定块上，滑轨位于内侧面盖板上，且沿垂直于从动轮组轮轴的竖直方向设置，滑块与滑轨相配合。

9.根据权利要求8所述的正交式平面移动机器人位姿检测装置，其特征在于，所述从动轮支架还包括：限位螺钉，限位螺钉的数量为两个，分别设置在滑轨下端面的两侧，滑轨的上端面距滑动限位板的距离小于滑块的高度。

10.一种采用如权利要求1至9中任一项所述的正交式平面移动机器人位姿检测装置进行移动机器人位姿检测的检测方法，其特征在于，包括：

步骤1：定义在水平面车体坐标系XOY中，其中以车体中心为中心点O、OX方向位于移动机器人进行方向、OY方向为移动机器人行进的垂直方向，移动机器人位于世界坐标系XOY的初始位置坐标值x、y、θ₀为原点；

步骤2：通过线性速度采集模块获得每个全方位从动轮的所转过的角度θ₁、θ₂、θ₃、θ₄，已知各全方位从动轮半径为R，可得各全方位从动轮移动的距离l₁、l₂、l₃、l₄如下:

l_n＝θ_n*R(n＝1,2,3,4)

步骤3、通过步骤2得到各全方位从动轮移动的距离l₁、l₂、l₃、l₄，且已知相邻的全方位从动轮的轮轴相互垂直，可计算出移动机器人相对于自身旋转的角度θ：

步骤4：通过所得移动机器人相对于自身旋转的角度θ，每一瞬时全方位从动轮转过的距离d_l1、d_l2、d_l3、d_l4,得到移动机器人的位于每一瞬时相对于以自身为原点的XOY坐标系的OX和OY方向的位移如下：

步骤5、将每一瞬时OX和OY方向的位移dX，dY进行积分，得到移动机器人相对于原点位置的OX和OY方向的位移△X和△Y，通过如下公式：

得到移动机器人相对于原点位置OX、OY方向的位移△X和△Y，相对于自身旋转的角度θ；

其中上述公式中的各个参数表示的含义如下：

x、y、θ₀：为移动机器人初始位置的坐标值；

θ₁、θ₂、θ₃、θ₄：分别为根据线性速度采集模块采集的信息获得的四个全方位从动轮的瞬时角速度；

θ：为移动机器人运动结束时的位置相对于原点位置自身所旋转的角度；

l₁、l₂、l₃、l₄：分别为四个全方位从动轮分别移动的距离；

d_l1、d_l2、d_l3、d_l4：分别为四个全方位从动轮每一瞬时各轮子转过的距离；

dX，dY：为移动机器人的位于每一瞬时相对于以自身为原点的XOY坐标系的OX和OY方向的位移；

△X和△Y：分别为移动机器人相对于原点位置的OX和OY方向的位移。

Images