CN108036786A - 基于辅助线的位姿检测方法、装置和计算机可读存储介质 - Google Patents
基于辅助线的位姿检测方法、装置和计算机可读存储介质 Download PDFInfo
- Publication number
- CN108036786A CN108036786A CN201711249452.3A CN201711249452A CN108036786A CN 108036786 A CN108036786 A CN 108036786A CN 201711249452 A CN201711249452 A CN 201711249452A CN 108036786 A CN108036786 A CN 108036786A
- Authority
- CN
- China
- Prior art keywords
- sampled point
- ceiling
- axis coordinate
- posterior
- course angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Image Analysis (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明公开了一种基于辅助线的位姿检测方法、装置和计算机可读存储介质，该位姿检测方法包括以下步骤：检测流程初始化、采集传感器数据、位姿先验估计、提取航向角检测值集合、航向角后验估计、提取位置检测值集合与位置后验估计，最终输出位姿信息。本发明利用惯性测量单元、摄像头等传感器的组合与估计算法，可以实时的检测机器人的位姿。相比于已有技术，本发明具有低成本、高精度与高可靠性的特点，为多种机器人的位姿信息的获取，提供了一种高性价比的检测手段。
Description
技术领域
本发明涉及机器人感知技术领域，特别是涉及一种基于辅助线的位姿检测方法、装置和计算机可读存储介质。
背景技术
机器人是自动执行工作的机器装置。它既可以接受人类指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动。它的任务是协助或取代人类工作的工作，不仅在工业、农业、国防、医疗、服务等行业中得到广泛的应用，而且在排雷、搜捕、救援、辐射和空间领域等有害与危险场合得到很好的应用。因此，机器人技术的研究已经得到世界各国的普遍关注。
机器人的一个基本功能是移动，或者称之为位姿控制，这也是机器人实现其他功能的基础。而精准的位姿控制是依赖于精确的位姿检测的，一个普遍的观点认为“控制的精度是无法超过反馈(检测)的精度的”，所以精确的位姿检测***就显得尤为重要。
对于工作在二维平面上的机器人来说，位姿检测包括对航向角与二维位置坐标的检测。传统的方法存在如下问题：
1)利用磁力计实现的航向角检测大多只能用在室外空旷的环境，对于室内环境，如果存在大量铁磁、电磁干扰，该检测手段可能失效；
2)利用陀螺仪实现的航向角检测会随着时间的推移变得越来越不可靠，而漂移小的陀螺仪成本过高，在很多低成本应用场景中无法使用；
3)多卫星定位手段大多只能用在室外空旷环境下，不适用于在树林、峡谷、室内、地下、水下、隧道等卫星信号无法到达的环境中；
4)基于WiFi、UWB的无线电定位手段存在着多路径干扰等问题，而且定位范围有限，如果要扩大定位范围将势必增加设备的成本；
5)基于视觉的定位手段受到光照的影响，对于光线过强、过暗基于剧烈变换的场景，该手段的精度较低，除此之外还有特征错误匹配、视距遮挡等问题；
6)基于视觉、WiFi或者磁场指纹的定位手段，由于事先需要做大量的实验以获取机器人位置的指纹特征，所以该手段不够实用，而且对指纹会发生变化的场景(特别是磁场指纹)，该定位手段是不可靠的。
发明内容
本发明的目的在于提供一种基于辅助线的位姿检测方法，用于室内环境。
本发明的目的在于提供一种基于辅助线的位姿检测装置，用于室内环境。
为此，本发明一方面提供了一种基于辅助线的位姿检测方法，在定位区域的天花板或地面配置有辅助线，包括以下步骤：
S101：对采样点序号t、采样间隔T、第一颜色辅助线间隔距离EX、第二颜色辅助线间隔距离EY、第t个采样点的前向移动速度VF,t和侧向移动速度VL,t,第t个采样点的位姿后验估计向量进行初始化赋值，其中与分别表示第t个采样点的X轴坐标、Y轴坐标与航向角的后验估计值，航向角定义为机器人前进方向相对于X轴的逆时针旋转角度；
S102：将采样点序号自增t←t+1，采集惯性测量单元的数据，得到第t个采样点的偏航速率rt、前进加速度aF,t与侧向加速度aL,t；采集图像传感器得到第t个采样点的天花板或地面图像
S103：利用步骤S102得到的第t个采样点的偏航速率rt、前进加速度aF,t与侧向加速度aL,t，并基于第t-1个采样点的位姿后验估计向量进行位姿先验估计，以获取第t个采样点的位姿先验估计向量
S104：根据步骤S102获取的第t个采样点的天花板或地面图像提取第t个采样点的天花板或地面视觉航向角检测值集合Θt和Hough距离集合ρX,t、ρY,t；
S105：根据步骤S104获取的第t个采样点的天花板或地面视觉航向角检测值集合Θt，与步骤3获取的第t个采样点的航向角先验估计进行航向角后验估计，以获取第t个采样点的航向角后验估计值
S1106：根据步骤S104获取第t个采样点的Hough距离集合ρX,t、ρY,t，与步骤S105获取的航向角后验估计值提取第t个采样点的天花板或地面视觉X轴坐标检测值集合与第t个采样点的天花板或地面视觉Y轴坐标检测值集合以及
S107：根据步骤S104获取的第t个采样点的天花板或地面视觉X轴坐标检测值集合与天花板或地面视觉Y轴坐标检测值集合与步骤S103获取的第t个采样点的X轴坐标先验估计与Y轴坐标先验估计进行位置坐标后验估计，以获取第t个采样点的X轴坐标后验估计与Y轴坐标后验估计以及
S108：重复步骤S102至S107，输出每个采样点的位姿后验估计向量，即位姿检测值。
根据本发明的另一方面，提供了一种基于辅助线的位姿检测装置，包括：惯性测量单元，用于检测偏航速率、前进加速度与侧向加速度；图像传感器，用于采集天花板或地面图像，当采集天花板图像时所述图像传感器的镜头垂直朝向天花板，并保证机器人中心在天花板的垂直投影点正好位于天花板图像的最左下角，当采集地面图像时所述图像传感器的镜头垂直朝向地面，并保证机器人中心在地面的垂直投影点正好位于地面图像的最左下角；数据处理单元，用于执行位姿检测程序，以获得位姿信息，所述位姿检测程序在执行时实现上述步骤S101至S108。
本发明还提供了一种计算机可读存储介质，存储有上面所描述的位姿检测程序。
本发明还提供了一种具有上述位姿检测方法的机器人。
与传统技术相比，本发明具有如下优点：1)由于没有磁力传感器的参与，所以本发明可以应用于铁磁干扰、电磁干扰较为丰富的环境，具有较强的检测精度与环境适应性；2)由于天花板的特征物是事先设定好的，所以很少出现路标错误匹配的问题，提高了***稳定性；3)由于涉及的图像处理算法比较简单，所以本发明的计算复杂度较低，从而可以在低成本的数据处理器上运行，这样一方面降低了硬件成本，另一方面提高了检测频率与***可靠性；4)由于在天花板布设辅助线较为容易，所以本发明具有较大的定位覆盖面积。
除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。
附图说明
构成本申请的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
图1是根据本发明的基于天花板视觉的位姿检测方法的流程图；
图2是根据本发明的基于天花板视觉的位姿检测方法中的天花板辅助线的示意图；以及
图3是根据本发明的基于天花板视觉的位姿检测装置的结构框图。
具体实施方式
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
本发明利用惯性测量单元、摄像头等传感器的组合与估计算法，可以实时的检测机器人的位姿。相比于已有技术，本发明具有低成本、高精度与高可靠性的特点，为多种机器人的位姿信息的获取，提供了一种高性价比的检测手段。
本发明的一种基于天花板辅助线的位姿检测方法包括以下步骤：检测流程初始化、采集传感器数据、位姿先验估计、提取航向角检测值集合、航向角后验估计、提取位置坐标检测值集合与位置坐标后验估计，最终输出位姿信息。如图1所示，具体如下：
步骤1、检测流程初始化
对采样点序号t初始化赋值：t←0；根据实际情况对采样间隔T、红色辅助线间隔距离EX、蓝色辅助线间隔距离EY、第t个采样点的前向移动速度VF,t和侧向移动速度VL,t,第t个采样点的位姿后验估计向量进行初始化赋值；其中 与分别表示第t个采样点的X轴坐标、Y轴坐标与航向角的后验估计值；航向角定义为机器人前进方向相对于X轴的逆时针旋转角度；矩阵A′表示矩阵A的转置；
在本方案的实施前，在需要定位服务的区域的地面绘制辅助线，具体如下：
定位区域的天花板配置有一系列颜色为红色的平行于X轴的平行线，相邻的平行线具有相等的间隔EX；同时，定位区域的天花板还配置有一系列颜色为蓝色的平行于Y轴的平行线，相邻的平行线具有相等的间隔EY。效果如图2所示，这里为了区分将红色辅助线标为虚线，实际中应当尽可能采用空隙较小的虚线或者实线。
步骤2、采集传感器数据
将采样点序号自增t←t+1，采集惯性测量单元的数据，得到第t个采样点的偏航速率rt、前进加速度aF,t与侧向加速度aL,t；采集图像传感器得到第t个采样点的天花板图像
步骤3、位姿先验估计
利用步骤2得到的第t个采样点的偏航速率rt、前进加速度aF,t与侧向加速度aL,t，并基于第t-1个采样点的位姿后验估计向量进行位姿先验估计，以获取第t个采样点的位姿先验估计向量具体如下：
VF,t＝VF,t-1+TaF,t
VL,t＝VL,t-1+TaL,t
步骤4、提取航向角检测值集合
根据步骤3获取的第t个采样点的天花板图像提取第t个采样点的天花板视觉航向角检测值集合Θt。具体如下：
步骤401：对步骤102采集到的第t个采样点的天花板图像分别进行基于红色与蓝色的图像阈值分割，以分别获取只包含对应颜色辅助线的二值图像与其中，中白色区域为红色辅助线，中白色区域为蓝色辅助线，其余部分为黑色；
步骤402：对步骤401获取的二值图像与依次进行形态学闭运算、骨架提取操作、剪枝操作，其目的分别为填充图像中内细小空洞、提取辅助线的中心线、消除游离于或依附于辅助线骨架上的小枝，得到第t个采样点的只包含辅助线中心线的二值图像集合与
步骤403：对步骤402中获取的二值图像与进行Hough变换，以分别获取第t个采样点的Hough距离集合ρX,t、ρY,t与Hough角度集合∈X,t、∈Y,t。
步骤404：基于步骤403获取的Hough角度集合∈X,t，计算天花板视觉航向角检测值集合Θt，具体如下：Θt＝∪e∈∈X，t{-e,-e+180}。
步骤5、航向角后验估计
根据步骤4获取的第t个采样点的天花板视觉航向角检测值集合Θt，与步骤3获取的第t个采样点的航向角先验估计进行航向角后验估计，以获取第t个采样点的航向角后验估计值具体如下：
如果Θt不是空集，则计算第t个采样点的航向角后验估计集合如下：
其中abs(·)为求绝对值符号，为Θt的第i个元素，δθ＞0为航向角筛选阈值，NΘ为Θt的元素个数；
如果不是空集，计算航向角后验估计值具体如下：
其中，为中第i个元素，Nθ为的元素个数；
如果Θt是空集或者是空集，计算第t个采样点的航向角后验估计值具体如下：
步骤6、提取位置坐标检测值集合
根据步骤404获取第t个采样点的Hough距离集合ρX,t、ρY,t，与步骤5获取的航向角后验估计值提取第t个采样点的天花板视觉X轴坐标检测值集合与第t个采样点的天花板视觉Y轴坐标检测值集合具体如下：
如果则
如果则
如果则
如果则
其中，MX与MY分别为红色辅助线与蓝色辅助线的数量。
步骤7、位置坐标后验估计
根据步骤4获取的第t个采样点的天花板视觉X轴坐标检测值集合与天花板视觉Y轴坐标检测值集合与步骤3获取的第t个采样点的X轴坐标先验估计与Y轴坐标先验估计进行位置坐标后验估计，以获取第t个采样点的X轴坐标后验估计与Y轴坐标后验估计具体如下：
步骤701：计算X轴坐标后验估计值
如果不是空集，则计算第t个采样点的X轴坐标后验估计集合如下：
其中abs(·)为求绝对值符号，为的第i个元素，δx＞0为X轴坐标筛选阈值，NX为的元素个数；
如果不是空集，计算X轴坐标后验估计值如下：
其中，为中第i个元素，Nx为的元素个数；
如果是空集或者是空集，计算第t个采样点的X轴坐标后验估计值
步骤702：计算Y轴坐标后验估计值
如果不是空集，则计算第t个采样点的Y轴坐标后验估计集合如下：
其中abs(·)为求绝对值符号，为的第i个元素，δy＞0为Y轴坐标筛选阈值，NY为的元素个数；
如果不是空集，计算Y轴坐标后验估计值如下：
其中，为中第i个元素，Ny为的元素个数；
如果是空集或者是空集，计算第t个采样点的Y轴坐标后验估计值
即为位姿后验估计向量。
重复步骤2至步骤7，输出每个采样点的位姿后验估计向量，即位姿检测值。
在另一实施例中，提供了一种基于地面视觉的位姿检测方法，与上述实施例不同之处在于，辅助线配置在地面上，地面上的辅助线的配置方式与天花板上的辅助线的配置方法相同，图像传感器由采集天花板图像替代为采集地面图像，其他步骤中也针对该地面图像进行处理。
本发明的上述检测方法以机器人的数据处理单元上运行位姿检测程序的方式实施，为此本发明还给出了一种存储有该位姿检测程序的计算机可读存储介质。
本发明还给出了用于实现上述位姿检测方法的装置，如图3所示，一种基于天花板视觉的位姿检测装置，包括：
惯性测量单元，用于检测偏航速率、前进加速度与侧向加速度；
图像传感器，用于采集天花板图像，所述图像传感器的镜头垂直朝向天花板，并保证机器人中心在天花板的垂直投影点正好位于天花板图像的最左下角；
数据处理单元，用于执行位姿检测程序，以获得位姿信息，所述位姿检测程序在执行时实现以下步骤：检测流程初始化、采集传感器数据、位姿先验估计、提取航向角检测值集合、航向角后验估计、提取位置坐标检测值集合与位置坐标后验估计。
保证机器人中心在天花板的垂直投影点正好位于天花板图像的最左下角的好处是：由于机器人中心点与图像最左下角点重合，这样方便后面位置坐标的计算。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (10)
1.一种基于辅助线的位姿检测方法，其特征在于，在定位区域的天花板或地面配置有辅助线，所述检测方法包括以下步骤：
S101：对采样点序号t、采样间隔T、第一颜色辅助线间隔距离EX、第二颜色辅助线间隔距离EY、第t个采样点的前向移动速度VF，t和侧向移动速度VL，t，第t个采样点的位姿后验估计向量进行初始化赋值，其中与分别表示第t个采样点的X轴坐标、Y轴坐标与航向角的后验估计值，航向角定义为机器人前进方向相对于X轴的逆时针旋转角度；
S102：将采样点序号自增t←t+1，采集惯性测量单元的数据，得到第t个采样点的偏航速率rt、前进加速度aF，t与侧向加速度aL，t；采集图像传感器得到第t个采样点的天花板或地面图像
S103：利用步骤S102得到的第t个采样点的偏航速率rt、前进加速度aF，t与侧向加速度aL，t，并基于第t-1个采样点的位姿后验估计向量进行位姿先验估计，以获取第t个采样点的位姿先验估计向量
S104：根据步骤S102获取的第t个采样点的天花板或地面图像提取第t个采样点的天花板或地面视觉航向角检测值集合Θt和Hough距离集合ρX，t、ρY，t；
S105：根据步骤S104获取的第t个采样点的天花板或地面视觉航向角检测值集合Θt，与步骤3获取的第t个采样点的航向角先验估计进行航向角后验估计，以获取第t个采样点的航向角后验估计值
S1106：根据步骤S104获取第t个采样点的Hough距离集合ρX，t、ρY，t，与步骤S105获取的航向角后验估计值提取第t个采样点的天花板或地面视觉X轴坐标检测值集合与第t个采样点的天花板或地面视觉Y轴坐标检测值集合以及
S107：根据步骤S104获取的第t个采样点的天花板或地面视觉X轴坐标检测值集合与天花板或地面视觉Y轴坐标检测值集合与步骤S103获取的第t个采样点的X轴坐标先验估计与Y轴坐标先验估计进行位置坐标后验估计，以获取第t个采样点的X轴坐标后验估计与Y轴坐标后验估计以及
S108：重复步骤S102至S107，输出每个采样点的位姿后验估计向量，即位姿检测值。
2.根据权利要求1所述的基于辅助线的位姿检测方法，其特征在于，所述辅助线的配置方法如下：
定位区域的天花板或地面配置有一系列具有第一颜色CX的平行于X轴的平行线，相邻的平行线具有相等的间隔EX；同时，定位区域的天花板或地面还配置有一系列具有第二颜色CY的平行于Y轴的平行线，相邻的平行线具有相等的间隔EY。
3.根据权利要求2所述的基于辅助线的位姿检测方法，其特征在于，所述步骤S103中的第t个采样点的位姿先验估计向量的计算过程如下：
<mrow>
<msub>
<mover>
<mi>&theta;</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>t</mi>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>&theta;</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>Tr</mi>
<mi>t</mi>
</msub>
<mo>;</mo>
</mrow>
VF，t＝VF，t-1+TaF，t；
VL，t＝VL，t-1+TaL，t；
以及
4.根据权利要求3所述的基于辅助线的位姿检测方法，其特征在于，所述步骤S104包括以下步骤：
S401：对步骤S102采集到的第t个采样点的天花板或地面图像分别进行基于第一颜色与第二颜色的图像阈值分割，以分别获取只包含对应颜色辅助线的二值图像与其中，中白色区域为第一颜色辅助线，中白色区域为第二颜色辅助线，其余部分为黑色；
S402：对步骤S401获取的二值图像与依次进行形态学闭运算、骨架提取操作、剪枝操作，以填充图像中内细小空洞、提取辅助线的中心线、消除游离于或依附于辅助线骨架上的小枝，得到第t个采样点的只包含辅助线中心线的二值图像集台与
S403：对步骤S402中获取的二值图像与进行Hough变换，以分别获取第t个采样点的Hough距离集合ρX，t、ρY，t与Hough角度集合∈X，t、∈Y，t；以及
S404：基于步骤S403获取的Hough角度集合∈X，t，计算天花板或地面视觉航向角检测值集合Θt，具体如下：Θt＝Ue∈∈X，t{-e，-e+180}。
5.根据权利要求4所述的基于辅助线的位姿检测方法，其特征在于，所述步骤S105中第t个采样点的航向角后验估计值的计算过程如下：
如果Θt不是空集，则计算第t个采样点的航向角后验估计集合如下：
<mrow>
<msub>
<mover>
<mi>&Theta;</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
</msub>
<mo>=</mo>
<mo>{</mo>
<msubsup>
<mi>&Theta;</mi>
<mi>t</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>|</mo>
<mi>a</mi>
<mi>b</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mover>
<mi>&theta;</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>t</mi>
<mo>,</mo>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msubsup>
<mi>&Theta;</mi>
<mi>t</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>&delta;</mi>
<mi>&theta;</mi>
</msub>
<mo>,</mo>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
<mo>,</mo>
<mn>2</mn>
<mo>,</mo>
<mn>...</mn>
<mo>,</mo>
<msub>
<mi>N</mi>
<mi>&Theta;</mi>
</msub>
<mo>}</mo>
<mo>,</mo>
</mrow>
其中abs(·)为求绝对值符号，为Θt的第i个元素，δθ＞0为航向角筛选阈值，NΘ为Θt的元素个数；
如果不是空集，计算航向角后验估计值具体如下
<mrow>
<msub>
<mover>
<mi>&theta;</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msubsup>
<mi>&Sigma;</mi>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<msub>
<mi>N</mi>
<mi>&theta;</mi>
</msub>
</msubsup>
<msubsup>
<mover>
<mi>&Theta;</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
<mrow>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</msubsup>
</mrow>
<msub>
<mi>N</mi>
<mi>&theta;</mi>
</msub>
</mfrac>
<mo>,</mo>
</mrow>
其中，为中第i个元素，Nθ为的元素个数；
如果Θt是空集或者是空集，计算第t个采样点的航向角后验估计值具体如下：
6.根据权利要求5所述的基于辅助线的位姿检测方法，其特征在于，所述步骤S107包括以下步骤：
S701：计算X轴坐标后验估计值包括以下步骤：
如果不是空集，则计算第t个采样点的X轴坐标后验估计集合如下：
其中abs(·)为求绝对值符号，为的第i个元素，δx＞0为X轴坐标筛选阈值，NX为的元素个数；
如果不是空集，计算X轴坐标后验估计值如下：
其中，为中第i个元素，Nx为的元素个数；
如果是空集或者是空集，计算第t个采样点的X轴坐标后验估计值
S702：计算Y轴坐标后验估计值包括以下步骤：
如果不是空集，则计算第t个采样点的Y轴坐标后验估计集合如下：
其中abs(·)为求绝对值符号，为的第i个元素，δy＞0为Y轴坐标筛选阈值，NY为的元素个数；
如果不是空集，计算Y轴坐标后验估计值如下：
其中，为中第i个元素，Ny为的元素个数；
如果是空集或者是空集，计算第t个采样点的Y轴坐标后验估计值 则即为位姿后验估计向量。
7.一种计算机可读存储介质，存储有位姿检测程序，其特征在于，所述位姿检测程序在处理器上执行时实现以下步骤：
S101：对采样点序号t、采样间隔T、第一颜色辅助线间隔距离EX、第二颜色辅助线间隔距离EY、第t个采样点的前向移动速度VF，t和侧向移动速度VL，t，第t个采样点的位姿后验估计向量进行初始化赋值，其中与分别表示第t个采样点的X轴坐标、Y轴坐标与航向角的后验估计值，航向角定义为机器人前进方向相对于X轴的逆时针旋转角度；
S102：将采样点序号自增t←t+1，采集惯性测量单元的数据，得到第t个采样点的偏航速率rt、前进加速度aF，t与侧向加速度aL，t；采集图像传感器得到第t个采样点的天花板或地面图像
S103：利用步骤S102得到的第t个采样点的偏航速率rt、前进加速度aF，t与侧向加速度aL，t，并基于第t-1个采样点的位姿后验估计向量进行位姿先验估计，以获取第t个采样点的位姿先验估计向量
S104：根据步骤S102获取的第t个采样点的天花板或地面图像提取第t个采样点的天花板或地面视觉航向角检测值集合Θt和Hough距离集合ρX，t、ρY，t；
S105：根据步骤S104获取的第t个采样点的天花板或地面视觉航向角检测值集合Θt，与步骤3获取的第t个采样点的航向角先验估计进行航向角后验估计，以获取第t个采样点的航向角后验估计值
S1106：根据步骤S104获取第t个采样点的Hough距离集合ρX，t、ρY，t，与步骤S105获取的航向角后验估计值提取第t个采样点的天花板或地面视觉X轴坐标检测值集合与第t个采样点的天花板或地面视觉Y轴坐标检测值集合以及
S107：根据步骤S104获取的第t个采样点的天花板或地面视觉X轴坐标检测值集合与天花板或地面视觉Y轴坐标检测值集合与步骤S103获取的第t个采样点的X轴坐标先验估计与Y轴坐标先验估计进行位置坐标后验估计，以获取第t个采样点的X轴坐标后验估计与Y轴坐标后验估计以及
S108：重复步骤S102至S107，输出每个采样点的位姿后验估计向量，即位姿检测值。
8.一种位姿检测装置，其特征在于，包括：
惯性测量单元，用于检测偏航速率、前进加速度与侧向加速度；
图像传感器，用于采集天花板或地面图像，当采集天花板图像时所述图像传感器的镜头垂直朝向天花板，并保证机器人中心在天花板的垂直投影点正好位于天花板图像的最左下角，当采集地面图像时所述图像传感器的镜头垂直朝向地面，并保证机器人中心在地面的垂直投影点正好位于地面图像的最左下角；
数据处理单元，用于执行位姿检测程序，以获得位姿信息，所述位姿检测程序在执行时实现以下步骤：
S101：对采样点序号t、采样间隔T、第一颜色辅助线间隔距离EX、第二颜色辅助线间隔距离EY、第t个采样点的前向移动速度VF，t和侧向移动速度VL，t，第t个采样点的位姿后验估计向量进行初始化赋值，其中与分别表示第t个采样点的X轴坐标、Y轴坐标与航向角的后验估计值，航向角定义为机器人前进方向相对于X轴的逆时针旋转角度；
S102：将采样点序号自增t←t+1，采集惯性测量单元的数据，得到第t个采样点的偏航速率rt、前进加速度aF，t与侧向加速度aL，t；采集图像传感器得到第t个采样点的天花板图像
S103：利用步骤S102得到的第t个采样点的偏航速率rt、前进加速度aF，t与侧向加速度aL，t，并基于第t-1个采样点的位姿后验估计向量进行位姿先验估计，以获取第t个采样点的位姿先验估计向量
S104：根据步骤S102获取的第t个采样点的天花板图像提取第t个采样点的天花板视觉航向角检测值集合Θt和Hough距离集合ρX，t、ρY，t；
S105：根据步骤S104获取的第t个采样点的天花板视觉航向角检测值集合Θt，与步骤3获取的第t个采样点的航向角先验估计进行航向角后验估计，以获取第t个采样点的航向角后验估计值
S1106：根据步骤S104获取第t个采样点的Hough距离集合ρX，t、ρY，t，与步骤S105获取的航向角后验估计值提取第t个采样点的天花板视觉X轴坐标检测值集合与第t个采样点的天花板视觉Y轴坐标检测值集合以及
S107：根据步骤S104获取的第t个采样点的天花板视觉X轴坐标检测值集合与天花板视觉Y轴坐标检测值集合与步骤S103获取的第t个采样点的X轴坐标先验估计与Y轴坐标先验估计进行位置坐标后验估计，以获取第t个采样点的X轴坐标后验估计与Y轴坐标后验估计以及
S108：重复步骤S102至S107，输出每个采样点的位姿后验估计向量，即位姿检测值。
9.一种机器人，包括位置检测装置，其特征在于，所述位置检测装置为根据权利要求8所述的位姿检测装置。
10.根据权利要求9所述的机器人，其特征在于，所述机器人为无人机或轮式机器人。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711249452.3A CN108036786B (zh) | 2017-12-01 | 2017-12-01 | 基于辅助线的位姿检测方法、装置和计算机可读存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711249452.3A CN108036786B (zh) | 2017-12-01 | 2017-12-01 | 基于辅助线的位姿检测方法、装置和计算机可读存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108036786A true CN108036786A (zh) | 2018-05-15 |
| CN108036786B CN108036786B (zh) | 2021-02-09 |
Family
ID=62095108
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711249452.3A Active CN108036786B (zh) | 2017-12-01 | 2017-12-01 | 基于辅助线的位姿检测方法、装置和计算机可读存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108036786B (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109458977A (zh) * | 2018-10-21 | 2019-03-12 | 合肥优控科技有限公司 | 机器人定向方法、***与计算机可读存储介质 |
| CN109709804A (zh) * | 2018-12-20 | 2019-05-03 | 安徽优思天成智能科技有限公司 | 一种随动***的姿态检测方法 |
| CN111650936A (zh) * | 2020-06-03 | 2020-09-11 | 杭州迦智科技有限公司 | 一种伺服控制方法、处理器、存储介质和可移动平台 |
| CN112773272A (zh) * | 2020-12-29 | 2021-05-11 | 深圳市杉川机器人有限公司 | 移动方向确定方法、装置、扫地机器人和存储介质 |
| CN114115212A (zh) * | 2020-08-26 | 2022-03-01 | 宁波方太厨具有限公司 | 一种清洁机器人的定位方法及采用该方法的清洁机器人 |
| CN114612563A (zh) * | 2022-03-22 | 2022-06-10 | 清华大学 | 航空线缆自动化插接方法、***及存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20090210092A1 (en) * | 2008-02-15 | 2009-08-20 | Korea Institute Of Science And Technology | Method for self-localization of robot based on object recognition and environment information around recognized object |
| CN103207634A (zh) * | 2013-03-20 | 2013-07-17 | 北京工业大学 | 一种智能车辆中差分gps与惯性导航数据融合的***和方法 |
| CN106123908A (zh) * | 2016-09-08 | 2016-11-16 | 北京京东尚科信息技术有限公司 | 车辆导航方法和*** |
| CN106323294A (zh) * | 2016-11-04 | 2017-01-11 | 新疆大学 | 变电站巡检机器人定位方法及定位装置 |
| CN106525053A (zh) * | 2016-12-28 | 2017-03-22 | 清研华宇智能机器人（天津）有限责任公司 | 一种基于多传感器融合的移动机器人室内定位方法 |
-
2017
- 2017-12-01 CN CN201711249452.3A patent/CN108036786B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20090210092A1 (en) * | 2008-02-15 | 2009-08-20 | Korea Institute Of Science And Technology | Method for self-localization of robot based on object recognition and environment information around recognized object |
| CN103207634A (zh) * | 2013-03-20 | 2013-07-17 | 北京工业大学 | 一种智能车辆中差分gps与惯性导航数据融合的***和方法 |
| CN106123908A (zh) * | 2016-09-08 | 2016-11-16 | 北京京东尚科信息技术有限公司 | 车辆导航方法和*** |
| CN106323294A (zh) * | 2016-11-04 | 2017-01-11 | 新疆大学 | 变电站巡检机器人定位方法及定位装置 |
| CN106525053A (zh) * | 2016-12-28 | 2017-03-22 | 清研华宇智能机器人（天津）有限责任公司 | 一种基于多传感器融合的移动机器人室内定位方法 |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109458977A (zh) * | 2018-10-21 | 2019-03-12 | 合肥优控科技有限公司 | 机器人定向方法、***与计算机可读存储介质 |
| CN109709804A (zh) * | 2018-12-20 | 2019-05-03 | 安徽优思天成智能科技有限公司 | 一种随动***的姿态检测方法 |
| CN111650936A (zh) * | 2020-06-03 | 2020-09-11 | 杭州迦智科技有限公司 | 一种伺服控制方法、处理器、存储介质和可移动平台 |
| CN111650936B (zh) * | 2020-06-03 | 2023-01-17 | 杭州迦智科技有限公司 | 一种伺服控制方法、处理器、存储介质和可移动平台 |
| CN114115212A (zh) * | 2020-08-26 | 2022-03-01 | 宁波方太厨具有限公司 | 一种清洁机器人的定位方法及采用该方法的清洁机器人 |
| CN112773272A (zh) * | 2020-12-29 | 2021-05-11 | 深圳市杉川机器人有限公司 | 移动方向确定方法、装置、扫地机器人和存储介质 |
| CN114612563A (zh) * | 2022-03-22 | 2022-06-10 | 清华大学 | 航空线缆自动化插接方法、***及存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108036786B (zh) | 2021-02-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108036786A (zh) | 基于辅助线的位姿检测方法、装置和计算机可读存储介质 | |
| CN109658461B (zh) | 一种基于虚拟仿真环境的合作二维码的无人机定位方法 | |
| CN102435188B (zh) | 一种用于室内环境的单目视觉/惯性全自主导航方法 | |
| CN102313536B (zh) | 基于机载双目视觉的障碍物感知方法 | |
| CN107451593B (zh) | 一种基于图像特征点的高精度gps定位方法 | |
| Bazin et al. | Motion estimation by decoupling rotation and translation in catadioptric vision | |
| CN107808407A (zh) | 基于双目相机的无人机视觉slam方法、无人机及存储介质 | |
| CN104596502A (zh) | 一种基于cad模型与单目视觉的物***姿测量方法 | |
| CN108665499B (zh) | 一种基于视差法的近距飞机位姿测量方法 | |
| CN112967339B (zh) | 一种车辆位姿的确定方法、车辆控制方法及装置、车辆 | |
| CN104281148A (zh) | 基于双目立体视觉的移动机器人自主导航方法 | |
| CN105222788A (zh) | 基于特征匹配的飞行器航偏移误差的自校正方法 | |
| CN109141396A (zh) | 辅助信息与随机抽样一致算法融合的无人机位姿估计方法 | |
| Kostavelis et al. | Visual odometry for autonomous robot navigation through efficient outlier rejection | |
| Deng et al. | A binocular vision-based measuring system for UAVs autonomous aerial refueling | |
| Lim et al. | Orora: Outlier-robust radar odometry | |
| Kim et al. | Target detection and position likelihood using an aerial image sensor | |
| Lv et al. | FVC: A novel nonmagnetic compass | |
| Butt et al. | Monocular SLAM initialization using epipolar and homography model | |
| Kupervasser et al. | Robust positioning of drones for land use monitoring in strong terrain relief using vision-based navigation | |
| Abdellali et al. | A direct least-squares solution to multi-view absolute and relative pose from 2D-3D perspective line pairs | |
| Brink et al. | Probabilistic outlier removal for robust landmark identification in stereo vision based SLAM | |
| CN105225232A (zh) | 一种基于视觉注意机制的彩色着舰合作目标检测方法 | |
| CN107945212A (zh) | 基于惯导信息辅助和背景差分法的红外弱小移动目标检测方法 | |
| CN108088446B (zh) | 移动机器人航向角检测方法、装置与计算机可读存储介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |