CN115077467B - 清洁机器人的姿态估计方法、装置及清洁机器人 - Google Patents
清洁机器人的姿态估计方法、装置及清洁机器人 Download PDFInfo
- Publication number
- CN115077467B CN115077467B CN202210655620.3A CN202210655620A CN115077467B CN 115077467 B CN115077467 B CN 115077467B CN 202210655620 A CN202210655620 A CN 202210655620A CN 115077467 B CN115077467 B CN 115077467B
- Authority
- CN
- China
- Prior art keywords
- constraint
- vertical
- point cloud
- determining
- cleaning robot
- 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.)
- Active
Links
- 238000004140 cleaning Methods 0.000 title claims abstract description 70
- 238000000034 method Methods 0.000 title claims abstract description 54
- 239000013598 vector Substances 0.000 claims description 24
- 239000011159 matrix material Substances 0.000 claims description 23
- 238000001514 detection method Methods 0.000 claims description 20
- 238000004590 computer program Methods 0.000 claims description 16
- 238000006073 displacement reaction Methods 0.000 claims description 16
- 230000000007 visual effect Effects 0.000 claims description 16
- 238000005457 optimization Methods 0.000 claims description 12
- 238000012795 verification Methods 0.000 claims description 9
- 238000010276 construction Methods 0.000 claims description 8
- 230000006870 function Effects 0.000 claims description 5
- 238000012216 screening Methods 0.000 claims description 5
- 230000011218 segmentation Effects 0.000 claims description 5
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 239000000523 sample Substances 0.000 claims 1
- 238000005516 engineering process Methods 0.000 abstract description 3
- 238000005096 rolling process Methods 0.000 abstract 1
- 230000000875 corresponding effect Effects 0.000 description 16
- 241001417527 Pempheridae Species 0.000 description 8
- 230000005540 biological transmission Effects 0.000 description 5
- 238000013519 translation Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- GXFZCDMWGMFGFL-KKXMJGKMSA-N (+)-Tubocurarine chloride hydrochloride Chemical compound [Cl-].[Cl-].C([C@H]1[N+](C)(C)CCC=2C=C(C(=C(OC3=CC=C(C=C3)C[C@H]3C=4C=C(C(=CC=4CC[NH+]3C)OC)O3)C=21)O)OC)C1=CC=C(O)C3=C1 GXFZCDMWGMFGFL-KKXMJGKMSA-N 0.000 description 2
- 239000008709 Curare Substances 0.000 description 2
- 238000013135 deep learning Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000010408 sweeping Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000032683 aging Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 238000013499 data model Methods 0.000 description 1
- 238000002996 descriptor matching method Methods 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
- G01P3/00—Measuring linear or angular speed; Measuring differences of linear or angular speeds
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02E—REDUCTION OF GREENHOUSE GAS [GHG] EMISSIONS, RELATED TO ENERGY GENERATION, TRANSMISSION OR DISTRIBUTION
- Y02E10/00—Energy generation through renewable energy sources
- Y02E10/50—Photovoltaic [PV] energy
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Image Analysis (AREA)
Abstract
本发明实施例提供了清洁机器人的姿态估计方法、装置及清洁机器人，该方法包括：对清洁机器人采集的图像数据进行垂线检测；根据检测到的垂线构建垂线约束；根据该垂线约束与该清洁机器人的角速度数据对该清洁机器人进行姿态估计，可以解决相关技术中全局的俯仰角和翻滚角主要依赖于加速度计进行估计，估计误差较大的问题，通过对清洁机器人自带的相机采集的图像数据进行垂线检测，构建垂线约束，再融合陀螺仪的角速度数据进行扫地机的姿态估计，不依赖于加速度计，便可实现高精度的、鲁棒性高、不随时间漂移的清洁机器人的位姿估计。
Description
技术领域
本发明实施例涉及通信领域，具体而言，涉及一种清洁机器人的姿态估计方法、装置及清洁机器人。
背景技术
高精度的机体姿态估计是目前自主导航扫地机进行建图、规划、避障的必要前提。机体姿态主要为基于基于世界或全局坐标系的偏航角、俯仰角、翻滚角。现有自主导航扫地机中姿态估计的偏航角主要利用单线激光雷达、相机、陀螺仪信息进行估计。然而全局的俯仰角和翻滚角主要依赖于加速度计进行估计。但是加速度计在使用时，其加速度读数会受到加速度计的出厂标定、偏置、噪声、不稳定性、老化等影响，导致全局的的俯仰角和翻滚角估计会造成较大误差且随时间出现漂移。尤其是扫地机出于成本考虑，会采用超低成本加速度计，其精度和稳定性较差，会产生更大的全局的俯仰角和翻滚角的位姿估计误差。
针对相关技术中全局的俯仰角和翻滚角主要依赖于加速度计进行估计，估计误差较大的问题，尚未提出解决方案。
发明内容
本发明实施例提供了一种清洁机器人的姿态估计方法、装置及清洁机器人，以至少解决相关技术中全局的俯仰角和翻滚角主要依赖于加速度计进行估计，估计误差较大的问题。
根据本发明的一个实施例，提供了一种清洁机器人的姿态估计方法，所述方法包括：
对清洁机器人采集的图像数据进行垂线检测；
根据检测到的垂线构建垂线约束；
根据所述垂线约束与所述清洁机器人的角速度数据对所述清洁机器人进行姿态估计。
根据本发明的另一个实施例，还提供了一种清洁机器人的姿态估计装置，所述装置包括：
垂线检测模块，用于对清洁机器人采集的图像数据进行垂线检测；
构建模块，用于根据检测到的垂线构建垂线约束；
姿态估计模块，用于根据所述垂线约束与所述清洁机器人的角速度数据对所述清洁机器人进行姿态估计。
根据本发明的另一个实施例，还提供了一种清洁机器人，所述清洁机器人至少包括上述的装置。
根据本发明的又一个实施例，还提供了一种计算机可读的存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。
根据本发明的又一个实施例，还提供了一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行上述任一项方法实施例中的步骤。
本发明实施例，对清洁机器人采集的图像数据进行垂线检测；根据检测到的垂线构建垂线约束；根据所述垂线约束与所述清洁机器人的角速度数据对所述清洁机器人进行姿态估计，可以解决相关技术中全局的俯仰角和翻滚角主要依赖于加速度计进行估计，估计误差较大的问题，通过对清洁机器人自带的相机采集的图像数据进行垂线检测，构建垂线约束，再融合陀螺仪的角速度数据进行扫地机的姿态估计，不依赖于加速度计，便可实现高精度的、鲁棒性高、不随时间漂移的清洁机器人的位姿估计。
附图说明
图1是本发明实施例的清洁机器人的姿态估计方法的移动终端的硬件结构框图；
图2是根据本发明实施例的清洁机器人的姿态估计方法的流程图；
图3是根据本发明可选实施例的清洁机器人的姿态估计方法的流程图一；
图4是根据本发明可选实施例的清洁机器人的姿态估计方法的流程图二；
图5是根据本发明实施例的机体姿态估计的流程图；
图6是根据本发明实施例的垂线检测的流程图；
图7是根据本实施例的清洁机器人的姿态估计装置的框图。
具体实施方式
下文中将参考附图并结合实施例来详细说明本发明的实施例。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
本发明实施例中所提供的方法实施例可以在移动终端、计算机终端或者类似的运算装置中执行。以运行在移动终端上为例，图1是本发明实施例的清洁机器人的姿态估计方法的移动终端的硬件结构框图，如图1所示，移动终端可以包括一个或多个(图1中仅示出一个)处理器102(处理器102可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置)和用于存储数据的存储器104，其中，上述移动终端还可以包括用于通信功能的传输设备106以及输入输出设备108。本领域普通技术人员可以理解，图1所示的结构仅为示意，其并不对上述移动终端的结构造成限定。例如，移动终端还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。
存储器104可用于存储计算机程序，例如，应用软件的软件程序以及模块，如本发明实施例中的清洁机器人的姿态估计方法对应的计算机程序，处理器102通过运行存储在存储器104内的计算机程序，从而执行各种功能应用以及业务链地址池切片处理，即实现上述的方法。存储器104可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器104可进一步包括相对于处理器102远程设置的存储器，这些远程存储器可以通过网络连接至移动终端。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
传输装置106用于经由一个网络接收或者发送数据。上述的网络具体实例可包括移动终端的通信供应商提供的无线网络。在一个实例中，传输装置106包括一个网络适配器(Network Interface Controller，简称为NIC)，其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输装置106可以为射频(Radio Frequency，简称为RF)模块，其用于通过无线方式与互联网进行通讯。
在本实施例中提供了一种运行于上述移动终端或网络架构的清洁机器人的姿态估计方法，图2是根据本发明实施例的清洁机器人的姿态估计方法的流程图，如图2所示，该流程包括如下步骤：
步骤S202，对清洁机器人采集的图像数据进行垂线检测；
步骤S204，根据检测到的垂线构建垂线约束；
步骤S206，根据垂线约束与清洁机器人的角速度数据对清洁机器人进行姿态估计。
通过上述步骤S202至S206，对清洁机器人采集的图像数据进行垂线检测；根据检测到的垂线构建垂线约束；根据所述垂线约束与所述清洁机器人的角速度数据对所述清洁机器人进行姿态估计，可以解决相关技术中全局的俯仰角和翻滚角主要依赖于加速度计进行估计，估计误差较大的问题，通过对清洁机器人自带的相机采集的图像数据进行垂线检测，构建垂线约束，再融合陀螺仪的角速度数据进行扫地机的姿态估计，不依赖于加速度计，便可实现高精度的、鲁棒性高、不随时间漂移的清洁机器人的位姿估计。
图3是根据本发明可选实施例的清洁机器人的姿态估计方法的流程图一，如图3所示，上述步骤S202具体可以包括：
S302，基于垂线检测与分割算法从图像数据中确定多组ROI区域；
S304，确定多组ROI区域的3D点云坐标；
一方面，若相机未设置3D点云传感器，分别在上一帧图像与当前帧图像的多组ROI区域中提取2D特征点；针对每组ROI区域，对上一帧图像和当前帧图像的2D特征点进行匹配，得到匹配点的2D坐标，并确定上一帧图像与当前帧图像的位姿增量，具体的，基于匹配点的2D坐标，利用相机的极线约束确定单应矩阵；根据单应矩阵分解得到上一帧图像与当前帧图像之间的旋转矩阵和不带尺度的位移向量，通过传感器获取上一帧图像与当前帧图像之间的绝对位置增量；根据不带尺度的位移向量与绝对位置增量确定带尺度的位移向量；根据旋转矩阵与带尺度的位移向量确定位姿增量。之后针对每组ROI区域，对该位姿增量与该匹配点的2D坐标进行三角化，得到匹配点在相机坐标系下的3D点云坐标。
另一方面，若相机设置有3D点云传感器，获取基于3D点云传感器采集的3D点云，对相机与3D点云传感器进行配准，获得图像数据中每个像素与3D点云中每个点云坐标的对应关系；根据图像数据中ROI区域的像素范围，获取对应的3D点云坐标。
S306，确定多组ROI区域的3D点云坐标的垂线方程。
对于3D点云坐标中的每组点云，进行外点剔除与内点筛选，得到多组最大内点数对应的垂线方程参数，之后分别对多组垂线方程参数对应的点云进行长度校验，具体的，对于多组垂线方程参数对应的点云中的每组点云，利用筛选出的内点选择相距最远的两个点，之后根据两个点计算垂线的最大距离；如果最大距离大于或等于预设长度阈值，确定垂线通过长度校验；如果最大距离小于预设长度阈值，确定垂线未通过长度校验。最后，对于通过长度校验的每组点云，利用筛选出的内点，以垂线方程参数作为初值，加上Huber鲁棒核函数，通过最小二乘优化出最优的垂线方程，得到多组垂线方程。
本实施例中，上述步骤S204具体可以包括：对于上述多组垂线方程中的每组垂线方程，与当前旋转姿态构建一个垂线约束，得到多组垂线约束。进一步的，对多组垂线方程中的每组垂线方程执行以下步骤，得到多组垂线约束，其中，正在执行的垂线方程称为当前垂线方程：基于当前旋转姿态计算世界坐标系中的垂线向量，根据垂线向量与所述当前垂线方程确定在当前相机坐标系的垂线方程估计值，根据垂线方程估计值与当前垂线方程的差值确定垂线约束。
图4是根据本发明可选实施例的清洁机器人的姿态估计方法的流程图二，如图4所示，上述步骤S206具体可以包括：
S402，根据角速度数据构建陀螺仪约束，具体的，基于上一时刻旋转姿态与陀螺仪的当前角速度数据确定当前旋转姿态预测值，确定当前旋转姿态预测值与当前旋转姿态的差值，得到陀螺仪约束。
S404，根据垂线约束与陀螺仪约束进行非线性优化，得到清洁机器人的姿态估计值。
进一步的，构建视觉里程计约束，具体的，首先根据上一帧图像与当前帧图像的特征点匹配结果，利用极线约束，确定相邻两帧图像之间的第一姿态增量；之后基于上一时刻旋转姿态与所述第一姿态增量，确定基于视觉里程计的第一姿态估计值；最后将第一姿态估计值与当前旋转姿态的差值确定为视觉里程计约束，和/或构建点云配准约束，具体的，首先基于3D点云传感器，根据上一时刻3D点云和当前时刻3D点云进行点云配准，得到相邻两帧之间的第二姿态增量；之后基于上一时刻旋转姿态与所述第二姿态增量确定基于点云配准的第二姿态估计值；最后将第二姿态估计值与当前旋转姿态的差值确定为点云配准约束，和/或构建垂线一致性约束，具体的，根据语义识别确定不同图像帧中的同一垂线，基于同一垂线构建所述垂线一致性约束；根据上述的视觉里程计约束、点云配准约束以及垂线一致性约束三者之一，与垂线约束、陀螺仪约束进行非线性优化，便可得到清洁机器人的姿态估计值。
图5是根据本发明实施例的机体姿态估计的流程图，如图5所示，包括：
步骤S501，内参和外参离线标定，对用到的扫地机各传感器进行外参和内参标定，主要标定如下参数：
(1)相机内参矩阵和畸变模型。根据选取的相机数学模型不同，其相应的相机内参矩阵和畸变模型也不同。以针孔模型为例，相机内参矩阵K包含：焦距fx，fy，主点偏移x0，y0，轴倾斜s等。
(2)陀螺仪内参，包含：3轴或单轴的偏置、尺度系数等。
(3)相机坐标系和陀螺仪坐标系的外参，包含：旋转矩阵和平移。
(4)陀螺仪坐标系和机体坐标系的外参，包含：旋转矩阵和平移。
(5)加速度计内参，如扫地机自带加速度计。包含：3轴或单轴的偏置、尺度系数等。
(6)加速度计坐标系和陀螺仪坐标系的外参，包含：旋转矩阵和平移。
(7)TOF传感器的点云内参模型，如扫地机自带TOF传感器。
(8)TOF传感器坐标系和陀螺仪坐标系的外参，包含：旋转矩阵和平移。
步骤S502，垂线检测，本发明实施例中的垂线的检测范围包含且不限于：门框的两侧垂直线、桌腿、玻璃的两侧垂直线、墙体两侧的垂直线，等物体的垂线。图6是根据本发明实施例的垂线检测的流程图，如图6所示，包括：
S601，通过相机2D图像的检测得到感兴趣区域(Region Of Interest，简称为ROI区域)；
具体可以基于相机图像的检测和分割算法采用深度学***面上的多组ROI区域或范围。
S602，计算多组ROI区域的3D点云坐标；
根据是否带有点云传感器，共分为两种情况。
若相机不带有3D点云传感器，在上一帧图像的ROI区域中提取2D特征点；在当前帧图像的ROI区域中提取2D特征点；对上一帧和当前帧的特征点进行匹配，得到匹配点2D坐标，方法不限于光流法、描述子匹配法等；基于2D特征点匹配结果，利用相机的极线约束，求取单应矩阵，采用RANSAC的方法抑制外点并筛选内点；根据单应矩阵分解到两帧之间的旋转矩阵和不带尺度的位移向量；根据扫地机的其他传感器或其他算法获得平移的尺度，例如：采用扫地机左右轮的码盘或轮速计可以得到两帧之间的绝对位置增量，根据不带尺度的位移向量与绝对位置增量确定待尺寸的位移向量，根据旋转矩阵和不带尺度的位移向量确定相邻两帧图像的位姿增量；根据相邻两帧的位姿增量和匹配点2D坐标，进行三角化，得到所有内点在相机坐标系下的3D坐标。
若相机带有3D点云传感器。例如常见的RGBD相机或TOF传感器。则第一步，通过该类点云传感器获得基于点云传感器坐标系的3D点云。第二步，再利用相机和点云传感器的外参，对相机和点云传感器的配准，获得相机图像中每个像素和点云传感器每个点云的对应关系。第三步，根据相机图像中ROI区域的像素范围，取出对应区域的3D点云坐标。
其中，RANSAC是random sample consensus的缩写，指的是随机抽样一致。它表示可以从一组包含“局外点”的数据集中，通过迭代的方式估计某个数学模型的参数。RANSAC算法中，数据是由“内点”和“外点”组成的。“内点”是组成模型参数的数据，“外点”就是不适合模型的数据。同时RANSAC假设：在给定一组含有少部分“内点”的数据，存在一个程序可以估计出符合“内点”的模型。
通过反复选择数据集去估计出模型，一直迭代到估计出认为比较好的模型。具体实现步骤分为以下几步：
1，选择出可以估计出模型的最小数据集；
2，使用数据集来计算出数据模型；
3，将所有数据带入这个模型，计算出“内点”数目；(累加在一定误差范围内的适合当前迭代推出模型的数据)
4，比较当前模型和之前推出的最好的模型的“内点”的数量，记录最大“内点”数的模型参数和“内点”数；
5，重复上述步骤，直到迭代结束或者当前模型已经足够好了(内点数目大于一定数量)。
S603，求取多组点云的垂线方程；
对于每组点云，通过RANSAC过程对上一步提取的点云进行外点剔除、内点筛选，并得到最大内点数对应垂线方程参数。
对于每组点云，进行长度校验。利用上一步筛选出的内点，选择相距最远的两个点，计算该垂线的最大距离，如果距离小于长度阈值，则认为该垂线太短，不用后续操作。
对于每组通过校验的点云，利用第一步筛选出的内点，以最大内点数对应的垂线方程参数作为初值，加上Huber鲁棒核函数，通过最小二乘优化出最优的垂线方程L_opt_i。平面方程以下式表示：
A_i*x+B_i*y+C_i*z+D_i＝0，其中，A_i、B_i、C_i为第i组对应的垂线方程参数。
步骤S503，基于多组垂线方程的构建垂线约束；
第一步，获取传感器采集的当前时刻旋转姿态，当前时刻旋转姿态采用R_cur表示，R_cur的具体表示形式不限制为旋转矩阵、四元数或欧拉角等。
第二步，对于每组垂线方程，可以和当前旋转姿态R_cur构建一个垂线约束e_i＝f_i(R_cur,L_opt_i)。垂线约束构建包括：
(1)求取垂线估计值。基于当前旋转姿态R_cur可以计算出世界坐标系中垂线向量L_global＝[0,0,1]在当前相机坐标系的垂线方程估计值L_cur＝R_cur*L_global。
(2)构建第i个垂线约束。基于每组点云，计算一个垂线约束e_i＝(L_curΘL_opt_i)/Σ_i，其中Θ代表两个向量之差，不限制于欧拉距离、模长、叉乘等，其中Σ_i代表该组垂线点云的不确定性，跟线长、点数、离相机距离、观测次数等正相关。
(3)构建N个垂线约束。遍历N组点云，共构建N组垂线约束。
需要说明的是，上述垂线构建的方式仅仅是示例性说明，其他类似的实施方式也可以，在此不再一一说明。
步骤S504，构建其他姿态约束；
(1)构建陀螺仪约束。基于上一时刻旋转姿态R_prev与陀螺仪读数w_cur陀螺仪可以递推出当前旋转姿态预测值R_predict，该预测值与当前旋转姿态R_cur求差，即可得到陀螺仪约束。
(2)构建视觉里程计约束。根据上一时刻图像和当前时刻图像的特征点匹配，利用极线约束，可以得到两帧之间的姿态增量△R_visual。基于上一时刻旋转姿态R_prev和姿态增量△R_visual，可以得到基于视觉里程计的姿态估计值R_visual。该估计值R_visual与当前旋转姿态R_cur求差，即可得到视觉里程计约束。
(3)构建点云配准约束。基于点云传感器，根据上一时刻3D点云和当前时刻3D点云，利用ICP方法(Iterative Closest Point,即迭代最近点)等点云配准方法，可以得到相邻两帧之间的姿态增量△R_pcl。基于上一时刻旋转姿态R_prev和姿态增量△R_pcl，可以得到基于点云配准的姿态估计值R_pcl。该估计值R_pcl与当前旋转姿态R_cur求差，即可得到点云配准约束。
(4)构建垂线一致性约束。如果根据识别语义信息，可以得到不同图像帧中某段垂线是同一垂线，可构建垂线一致性约束。例如：根据语义信息，可知图像帧P_1观测到的门框和图像帧P_2观测的门框属于同一门框。具体包括：
基于图像帧P_1的位姿，将图像帧P_1中门框垂线上点云从相机坐标系转换到世界坐标系，再将点云坐标投影到地面，即[x_1,y_1]；
基于图像帧P_2的位姿，将图像帧P_2中门框垂线上点云从相机坐标系转换到世界坐标系，再将点云坐标投影到地面，即[x_2,y_2]；
由于两帧观测到的门框是同一门框，故两帧门框垂线上点云在世界坐标系的投影应该是同一点，即理论上[x_1,y_1]应该等于[x_2,y_2]。故可以构建垂线一致性约束＝[x_1,y_1]Θ[x_2,y_2]；
步骤S505，通过非线性优化求解姿态。
基于以上各类姿态约束，进行非线性优化，求解姿态的最优值。本实施例不限制具体的非线性优化方法，例如高斯牛顿法、dogleg、LM等非线性优化方法。
根据本发明的另一个实施例，还提供了一种清洁机器人的姿态估计装置，图7是根据本发明实施例的清洁机器人的姿态估计装置的框图，如图7所示，所述装置包括：
垂线检测模块72，用于对清洁机器人采集的图像数据进行垂线检测；
构建模块74，用于根据检测到的垂线构建垂线约束；
姿态估计模块76，用于根据所述垂线约束与所述清洁机器人的角速度数据对所述清洁机器人进行姿态估计。
可选的，垂线检测模块72包括：
第一确定子模块，用于基于垂线检测与分割算法从所述图像数据中确定多组感兴趣区域ROI区域；
第二确定子模块，用于确定所述多组ROI区域的3D点云坐标；
第三确定子模块，用于确定所述多组ROI区域的3D点云坐标的垂线方程。
可选的，所述第二确定子模块，还用于若所述相机未设置3D点云传感器，分别在上一帧图像与当前帧图像的所述多组ROI区域中提取2D特征点；针对每组ROI区域，对所述上一帧图像和所述当前帧图像的2D特征点进行匹配，得到匹配点的2D坐标，并确定所述上一帧图像与所述当前帧图像的位姿增量；针对所述每组ROI区域，对所述位姿增量与所述匹配点的2D坐标进行三角化，得到所述匹配点在相机坐标系下的3D点云坐标。
可选的，所述第二确定子模块，还用于基于所述匹配点的2D坐标，利用相机的极线约束确定单应矩阵；根据所述单应矩阵分解得到所述上一帧图像与所述当前帧图像之间的旋转矩阵和不带尺度的位移向量；通过传感器获取所述上一帧图像与所述当前帧图像之间的绝对位置增量；根据所述不带尺度的位移向量与所述绝对位置增量确定带尺度的位移向量；根据所述旋转矩阵与所述带尺度的位移向量确定所述位姿增量。
可选的，所述第二确定子模块，还用于若所述相机设置有3D点云传感器，获取基于所述3D点云传感器采集的3D点云；对所述相机与所述3D点云传感器进行配准，获得所述图像数据中每个像素与所述3D点云中每个点云坐标的对应关系；根据所述图像数据中ROI区域的像素范围，获取对应的3D点云坐标。
可选的，所述第三确定子模块，还用于对于所述3D点云坐标中的每组点云，进行外点剔除与内点筛选，得到多组最大内点数对应的垂线方程参数；分别对多组垂线方程参数对应的点云进行长度校验；对于通过长度校验的每组点云，利用筛选出的内点，以所述垂线方程参数作为初值，加上Huber鲁棒核函数，通过最小二乘优化出最优的垂线方程，得到多组垂线方程。
可选的，所述第三确定子模块，还用于对于多组垂线方程参数对应的点云中的每组点云，利用所述筛选出的内点选择相距最远的两个点；根据所述两个点计算垂线的最大距离；如果所述最大距离大于或等于预设长度阈值，确定所述垂线通过长度校验；如果所述最大距离小于所述预设长度阈值，确定所述垂线未通过长度校验。
可选的，构建模块74包括：
第一构建子模块，用于对于所述多组垂线方程中的每组垂线方程，与当前旋转姿态构建一个垂线约束，得到多组垂线约束。
可选的，所述第一构建子模块，还用于对所述多组垂线方程中的每组垂线方程执行以下步骤，得到所述多组垂线约束，其中，正在执行的垂线方程称为当前垂线方程：基于当前旋转姿态计算世界坐标系中的垂线向量；根据所述垂线向量与所述当前垂线方程确定在当前相机坐标系的垂线方程估计值；根据所述垂线方程估计值与所述当前垂线方程的差值确定所述垂线约束。
可选的，姿态估计模块76包括：
第二构建子模块，用于根据所述角速度数据构建陀螺仪约束；
非线性优化子模块，用于根据所述垂线约束与所述陀螺仪约束进行非线性优化，得到所述清洁机器人的姿态估计值。
可选的，所述第二构建子模块，还用于基于上一时刻旋转姿态与陀螺仪的当前角速度数据确定当前旋转姿态预测值；确定所述当前旋转姿态预测值与当前旋转姿态的差值，得到所述陀螺仪约束。
可选的，所述非线性优化子模块，还用于构建视觉里程计约束，和/或构建点云配准约束，和/或构建垂线一致性约束；根据所述视觉里程计约束、所述点云配准约束以及所述垂线一致性约束三者之一，与所述垂线约束、所述陀螺仪约束进行非线性优化，得到所述清洁机器人的姿态估计值。
可选的，所述非线性优化子模块，还用于根据上一帧图像与当前帧图像的特征点匹配结果，利用极线约束，确定相邻两帧图像之间的第一姿态增量；基于上一时刻旋转姿态与所述第一姿态增量，确定基于视觉里程计的第一姿态估计值；将所述第一姿态估计值与当前旋转姿态的差值确定为所述视觉里程计约束；
基于3D点云传感器，根据上一时刻3D点云和当前时刻3D点云进行点云配准，得到相邻两帧之间的第二姿态增量；基于上一时刻旋转姿态与所述第二姿态增量确定基于点云配准的第二姿态估计值；将所述第二姿态估计值与当前旋转姿态的差值确定为所述点云配准约束；
根据语义识别确定不同图像帧中的同一垂线，基于所述同一垂线构建所述垂线一致性约束。
本发明的实施例还提供了一种计算机可读存储介质，该计算机可读存储介质中存储有计算机程序，其中，该计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。
在一个示例性实施例中，上述计算机可读存储介质可以包括但不限于：U盘、只读存储器(Read-Only Memory，简称为ROM)、随机存取存储器(Random Access Memory，简称为RAM)、移动硬盘、磁碟或者光盘等各种可以存储计算机程序的介质。
本发明的实施例还提供了一种电子装置，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。
在一个示例性实施例中，上述电子装置还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。
本实施例中的具体示例可以参考上述实施例及示例性实施方式中所描述的示例，本实施例在此不再赘述。
显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (17)
1.一种清洁机器人的姿态估计方法，其特征在于，所述方法包括：
对清洁机器人采集的图像数据进行垂线检测；
根据检测到的垂线构建垂线约束；
根据所述垂线约束与所述清洁机器人的角速度数据对所述清洁机器人进行姿态估计。
2.根据权利要求1所述的方法，其特征在于，对清洁机器人采集的图像数据进行垂线检测包括：
基于垂线检测与分割算法从所述图像数据中确定多组感兴趣区域ROI区域；
确定所述多组ROI区域的3D点云坐标；
确定所述多组ROI区域的3D点云坐标的垂线方程。
3.根据权利要求2所述的方法，其特征在于，确定所述多组ROI区域的3D点云坐标包括：
若相机未设置3D点云传感器，分别在上一帧图像与当前帧图像的所述多组ROI区域中提取2D特征点；
针对每组ROI区域，对所述上一帧图像和所述当前帧图像的2D特征点进行匹配，得到匹配点的2D坐标，并确定所述上一帧图像与所述当前帧图像的位姿增量；
针对所述每组ROI区域，对所述位姿增量与所述匹配点的2D坐标进行三角化，得到所述匹配点在相机坐标系下的3D点云坐标。
4.根据权利要求3所述的方法，其特征在于，确定所述上一帧图像与所述当前帧图像的位姿增量包括：
基于所述匹配点的2D坐标，利用相机的极线约束确定单应矩阵；
根据所述单应矩阵分解得到所述上一帧图像与所述当前帧图像之间的旋转矩阵和不带尺度的位移向量；
通过传感器获取所述上一帧图像与所述当前帧图像之间的绝对位置增量；
根据所述不带尺度的位移向量与所述绝对位置增量确定带尺度的位移向量；
根据所述旋转矩阵与所述带尺度的位移向量确定所述位姿增量。
5.根据权利要求2所述的方法，其特征在于，确定所述多组ROI区域的3D点云坐标包括：
若相机设置有3D点云传感器，获取基于所述3D点云传感器采集的3D点云；
对所述相机与所述3D点云传感器进行配准，获得所述图像数据中每个像素与所述3D点云中每个点云坐标的对应关系；
根据所述图像数据中ROI区域的像素范围，获取对应的3D点云坐标。
6.根据权利要求2所述的方法，其特征在于，确定所述多组ROI区域的3D点云坐标的垂线方程包括：
对于所述3D点云坐标中的每组点云，进行外点剔除与内点筛选，得到多组最大内点数对应的垂线方程参数；
分别对多组垂线方程参数对应的点云进行长度校验；
对于通过长度校验的每组点云，利用筛选出的内点，以所述垂线方程参数作为初值，加上Huber鲁棒核函数，通过最小二乘优化出最优的垂线方程，得到多组垂线方程。
7.根据权利要求6所述的方法，其特征在于，分别对多组垂线方程参数对应的点云进行长度校验包括：
对于多组垂线方程参数对应的点云中的每组点云，利用所述筛选出的内点选择相距最远的两个点；
根据所述两个点计算垂线的最大距离；
如果所述最大距离大于或等于预设长度阈值，确定所述垂线通过长度校验；
如果所述最大距离小于所述预设长度阈值，确定所述垂线未通过长度校验。
8.根据权利要求6所述的方法，其特征在于，根据检测到的垂线构建垂线约束包括：
对于所述多组垂线方程中的每组垂线方程，与当前旋转姿态构建一个垂线约束，得到多组垂线约束。
9.根据权利要求8所述的方法，其特征在于，对于所述多组垂线方程中的每组垂线方程，与当前旋转姿态构建一个垂线约束，得到多组垂线约束包括：
对所述多组垂线方程中的每组垂线方程执行以下步骤，得到所述多组垂线约束，其中，正在执行的垂线方程称为当前垂线方程：
基于当前旋转姿态计算世界坐标系中的垂线向量；
根据所述垂线向量与所述当前垂线方程确定在当前相机坐标系的垂线方程估计值；
根据所述垂线方程估计值与所述当前垂线方程的差值确定所述垂线约束。
10.根据权利要求1所述的方法，其特征在于，根据所述垂线约束与所述清洁机器人的角速度数据对所述清洁机器人进行姿态估计包括：
根据所述角速度数据构建陀螺仪约束；
根据所述垂线约束与所述陀螺仪约束进行非线性优化，得到所述清洁机器人的姿态估计值。
11.根据权利要求10所述的方法，其特征在于，根据所述角速度数据构建陀螺仪约束包括：
基于上一时刻旋转姿态与陀螺仪的当前角速度数据确定当前旋转姿态预测值；
确定所述当前旋转姿态预测值与当前旋转姿态的差值，得到所述陀螺仪约束。
12.根据权利要求10所述的方法，其特征在于，根据所述垂线约束与所述陀螺仪约束进行非线性优化，得到所述清洁机器人的姿态估计值包括：
构建视觉里程计约束，和/或构建点云配准约束，和/或构建垂线一致性约束；
根据所述视觉里程计约束、所述点云配准约束以及所述垂线一致性约束三者之一，与所述垂线约束、所述陀螺仪约束进行非线性优化，得到所述清洁机器人的姿态估计值。
13.根据权利要求12所述的方法，其特征在于，
构建视觉里程计约束包括：根据上一帧图像与当前帧图像的特征点匹配结果，利用极线约束，确定相邻两帧图像之间的第一姿态增量；基于上一时刻旋转姿态与所述第一姿态增量，确定基于视觉里程计的第一姿态估计值；将所述第一姿态估计值与当前旋转姿态的差值确定为所述视觉里程计约束；
构建点云配准约束包括：基于3D点云传感器，根据上一时刻3D点云和当前时刻3D点云进行点云配准，得到相邻两帧之间的第二姿态增量；基于上一时刻旋转姿态与所述第二姿态增量确定基于点云配准的第二姿态估计值；将所述第二姿态估计值与当前旋转姿态的差值确定为所述点云配准约束；
构建垂线一致性约束包括：根据语义识别确定不同图像帧中的同一垂线，基于所述同一垂线构建所述垂线一致性约束。
14.一种清洁机器人的姿态估计装置，其特征在于，所述装置包括：
垂线检测模块，用于对清洁机器人采集的图像数据进行垂线检测；
构建模块，用于根据检测到的垂线构建垂线约束；
姿态估计模块，用于根据所述垂线约束与所述清洁机器人的角速度数据对所述清洁机器人进行姿态估计。
15.一种清洁机器人，其特征在于，所述清洁机器人至少包括权利要求14中所述的装置。
16.一种计算机可读的存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行所述权利要求1至13任一项中所述的方法。
17.一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行所述权利要求1至13任一项中所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210655620.3A CN115077467B (zh) | 2022-06-10 | 2022-06-10 | 清洁机器人的姿态估计方法、装置及清洁机器人 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210655620.3A CN115077467B (zh) | 2022-06-10 | 2022-06-10 | 清洁机器人的姿态估计方法、装置及清洁机器人 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115077467A CN115077467A (zh) | 2022-09-20 |
| CN115077467B true CN115077467B (zh) | 2023-08-08 |
Family
ID=83251904
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210655620.3A Active CN115077467B (zh) | 2022-06-10 | 2022-06-10 | 清洁机器人的姿态估计方法、装置及清洁机器人 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115077467B (zh) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111928861A (zh) * | 2020-08-07 | 2020-11-13 | 杭州海康威视数字技术股份有限公司 | 地图构建方法及装置 |
| CN112729294A (zh) * | 2021-04-02 | 2021-04-30 | 北京科技大学 | 适用于机器人的视觉和惯性融合的位姿估计方法及*** |
| CN113192140A (zh) * | 2021-05-25 | 2021-07-30 | 华中科技大学 | 一种基于点线特征的双目视觉惯性定位方法和*** |
| CN113341989A (zh) * | 2021-06-18 | 2021-09-03 | 广州蓝胖子移动科技有限公司 | 轮式移动机器人及控制点模型建立方法、装置、存储介质 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10260862B2 (en) * | 2015-11-02 | 2019-04-16 | Mitsubishi Electric Research Laboratories, Inc. | Pose estimation using sensors |
-
2022
- 2022-06-10 CN CN202210655620.3A patent/CN115077467B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111928861A (zh) * | 2020-08-07 | 2020-11-13 | 杭州海康威视数字技术股份有限公司 | 地图构建方法及装置 |
| CN112729294A (zh) * | 2021-04-02 | 2021-04-30 | 北京科技大学 | 适用于机器人的视觉和惯性融合的位姿估计方法及*** |
| CN113192140A (zh) * | 2021-05-25 | 2021-07-30 | 华中科技大学 | 一种基于点线特征的双目视觉惯性定位方法和*** |
| CN113341989A (zh) * | 2021-06-18 | 2021-09-03 | 广州蓝胖子移动科技有限公司 | 轮式移动机器人及控制点模型建立方法、装置、存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115077467A (zh) | 2022-09-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9243916B2 (en) | Observability-constrained vision-aided inertial navigation | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| US8437501B1 (en) | Using image and laser constraints to obtain consistent and improved pose estimates in vehicle pose databases | |
| US8447116B2 (en) | Identifying true feature matches for vision based navigation | |
| CN112183171B (zh) | 一种基于视觉信标建立信标地图方法、装置 | |
| Zuo et al. | Visual-inertial localization with prior LiDAR map constraints | |
| CN112734852A (zh) | 一种机器人建图方法、装置及计算设备 | |
| CN110470333B (zh) | 传感器参数的标定方法及装置、存储介质和电子装置 | |
| CN109903330B (zh) | 一种处理数据的方法和装置 | |
| CN112197764B (zh) | 实时位姿确定方法、装置及电子设备 | |
| CN114013449B (zh) | 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆 | |
| CN104848861A (zh) | 一种基于图像消失点识别技术的移动设备姿态测量方法 | |
| CN113763549A (zh) | 融合激光雷达和imu的同时定位建图方法、装置和存储介质 | |
| CN116958452A (zh) | 三维重建方法和*** | |
| CN115077467B (zh) | 清洁机器人的姿态估计方法、装置及清洁机器人 | |
| CN115060268A (zh) | 一种机房融合定位方法、***、设备及存储介质 | |
| CN113126117B (zh) | 一种确定sfm地图绝对尺度的方法及电子设备 | |
| CN113034538B (zh) | 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 | |
| CN114842224A (zh) | 一种基于地理底图的单目无人机绝对视觉匹配定位方案 | |
| CN109919998B (zh) | 卫星姿态确定方法、装置和终端设备 | |
| CN114983302B (zh) | 姿态的确定方法、装置、清洁设备、存储介质及电子装置 | |
| Wang et al. | Slam-based cooperative calibration for optical sensors array with gps/imu aided | |
| Steffen | Visual SLAM from image sequences acquired by unmanned aerial vehicles | |
| CN117433511B (zh) | 一种多传感器融合定位方法 | |
| Arslan | Accuracy assessment of single viewing techniques for metric measurements on single images |
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 |