姿态的确定方法、装置、清洁设备、存储介质及电子装置与流程

姿态的确定方法、装置、清洁设备、存储介质及电子装置
【技术领域】
1.本发明涉及通信领域，具体而言，涉及一种姿态的确定方法、装置、清洁设备、存储介质及电子装置。


背景技术：

2.随着人工智能技术的快速发展，越来越多的智能清洁设备(例如，扫地机、洗地机、吸尘器等等)进入人们的生活，使得人们的生活越来越便利。
3.相关技术中，机体姿态估计是自主导航扫地机(此处以扫地机为例说明，当然还可以是其他的清洁设备，例如，洗地机，等等)进行建图、规划、避障的必要前提。现有的自主导航扫地机中的机体姿态主要为基于全局坐标系的偏航角、俯仰角、翻滚角。其偏航角主要利用单线激光雷达、相机、陀螺仪信息等进行姿态估计，俯仰角和翻滚角主要依赖于加速度进行姿态估计，但加速度计在使用时，其加速度读数会受到加速度计的出厂标定、偏置、噪声、不稳定性、老化、自身运动加速度等影响，导致全局坐标系的俯仰角和翻滚角的姿态估计会存在较大误差，且长时间使用后会出现漂移，从而造成全局坐标系的俯仰角和翻滚角的姿态估计的误差更大。
4.针对相关技术中存在的需要依赖于加速度计进行姿态估计，从而导致估计误差较大的问题，目前尚未提出有效的解决方案。


技术实现要素：

5.本发明实施例提供了一种姿态的确定方法、装置、清洁设备、存储介质及电子装置，以至少解决相关技术中存在的需要依赖于加速度计进行姿态估计，从而导致估计误差较大的问题。
6.根据本发明的一个方面，提供了一种姿态的确定方法，包括：对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域；确定所述第一roi区域对应的第一3d点云信息；基于所述第一3d点云信息对所述清洁设备的角速度进行修正；基于修正后的所述清洁设备的角速度确定所述清洁设备的姿态。
7.在一个示例性实施例中，确定所述第一roi区域对应的第一3d点云信息包括：在确定所述清洁设备中包括目标传感器的情况下，确定所述目标传感器所采集到的目标区域的3d点云信息，其中，所述第一图像为对所述目标区域进行拍摄所得到的图像，且所述第一图像的拍摄时间和所述目标传感器采集所述目标区域的3d点云信息的时间相差小于预定阈值；将所述第一图像中包括的全部像素点与所述目标区域的3d点云信息进行配准；基于配准结果确定所述第一roi区域对应的所述第一3d点云信息。
8.在一个示例性实施例中，确定所述第一roi区域对应的第一3d点云信息包括：在确定所述清洁设备中不包括目标传感器的情况下，对所述第一摄像设备所拍摄的第二图像进行检测，确定所述第二图像中包括的第二平面的第二roi区域，其中，所述第二图像为所述
第一图像的前一帧图像；提取所述第一roi区域的第一2d特征点，以及，提取所述第二roi区域的第二2d特征点；基于特征点匹配方式对所述第一2d特征点与所述第二2d特征点进行匹配；基于匹配结果确定所述第一roi区域对应的所述第一3d点云信息。
9.在一个示例性实施例中，基于匹配结果确定所述第一roi区域对应的第一3d点云信息包括：确定所述第一摄像设备的平面约束，并基于所述匹配结果和所述平面约束确定目标单应矩阵；基于所述目标单应矩阵确定所述第一图像与所述第二图像之间的旋转矩阵和不带尺度的平移向量；基于所述清洁设备的第一传感器和/或第一算法确定所述第一图像与所述第二图像之间的绝对位置增量；基于所述不带尺度的平移向量和所述绝对位置增量确定带尺度的平移向量；对所述带尺度的平移向量和所述匹配结果进行三角化，以确定所述第一roi区域对应的第一3d点云信息。
10.在一个示例性实施例中，对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域包括：利用神经网络模型对所述第一图像进行检测，确定所述第一图像中包括的所述第一平面的所述第一roi区域，其中，所述神经网络模型为利用训练数据对初始神经网络模型进行训练后所得到的模型，所述训练数据包括多组数据，每组数据均包括训练图像以及所述训练图像中所包括的平面的roi区域。
11.在一个示例性实施例中，其特征在于，基于所述第一3d点云信息对所述清洁设备的角速度进行修正包括：按照目标筛选方式从所述第一3d点云中筛选出目标3d点云，并基于所述目标3d点云确定第一平面方程；从所述目标3d点云中确定出目标最大3d点云，并基于所述目标最大3d点云和huber鲁棒核函数通过最小二乘优化生成目标平面方程，其中，所述目标最大3d点云为距离所述第一平面方程小于预定距离阈值的3d点云；基于所述目标平面方程对所述角速度进行修正。
12.在一个示例性实施例中，基于所述目标平面方程对所述角速度进行修正包括：确定所述清洁设备的上一时刻姿态的四元数，并基于所述上一时刻姿态的四元数确定所述上一时刻姿态的第二平面方程；确定所述目标平面方程与所述第二平面方程之间的平面方程差值，以及，获取所述清洁设备的陀螺仪角速度测量值；基于所述平面方程差值确定角速度修正量，并基于所述角速度修正量对所述陀螺仪角速度测量值进行修正，得到修正后的角速度。
13.在一个示例性实施例中，在基于所述平面方程差值确定角速度修正量，并基于所述角速度修正量对所述陀螺仪角速度测量值进行修正，得到修正后的角速度之后，所述方法还包括：基于所述修正后的角速度对所述上一时刻姿态的四元数进行更新，以得到更新后的当前时刻姿态的四元数；对所述当前时刻姿态的四元数进行归一化处理，并将归一化处理后的所述四元数转化为欧拉角；基于所述欧拉角确定所述清洁设备的更新姿态。
14.在一个示例性实施例中，在对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域之前，所述方法还包括：对所述清洁设备中包括的传感器的参数进行标定，所述参数如下至少之一：所述第一摄像设备的内参矩阵和畸变模型；所述清洁设备的陀螺仪的内参；所述第一摄像设备坐标系和所述陀螺仪坐标系之间的外参；所述陀螺仪坐标系和所述清洁设备的机体坐标系之间的外参；所述清洁设备中包括的目标传感器的点云内参模型；所述目标传感器坐标系和所述陀
螺仪坐标系之间的外参。
15.根据本发明的另一个方面，还提供了一种姿态的确定装置，包括：第一确定模块，用于对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域；第二确定模块，用于确定所述第一roi区域对应的第一3d点云信息；修正模块，用于基于所述第一3d点云信息对所述清洁设备的角速度进行修正；第三确定模块，用于基于修正后的所述清洁设备的角速度确定所述清洁设备的姿态。
16.根据本发明的另一个方面，还提供了一种清洁设备，包括上述装置实施例中所述的姿态的确定装置。
17.根据本发明的另一个实施例，还提供了一种计算机可读的存储介质，所述计算机可读的存储介质包括存储的程序，其中，所述程序运行时执行上述任一项实施例中所述的方法。
18.根据本发明的另一个实施例，还提供了一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为通过所述计算机程序执行上述任一项实施例中所述的方法。
19.通过本发明，可以对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，以确定所述第一图像中包括的第一平面的第一roi区域，进而确定所述第一roi区域对应的第一3d点云信息，并基于所述第一3d点云信息对所述清洁设备的角速度进行修正，继而，可以基于修正后的所述清洁设备的角速度确定所述清洁设备的姿态，采用本发明，可以通过获取的第一3d点云信息对清洁设备的角速度进行修正，进而通过修正后的角速度确定清洁设备的姿态，从而实现提高姿态估计的精度和稳定性的目的，有效解决相关技术中存在的全局坐标系的俯仰角和翻滚角主要依赖于加速度计进行姿态估计，从而导致估计误差较大的问题。
【附图说明】
20.此处所说明的附图用来提供对本发明的进一步理解，构成本技术的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
21.图1是本发明实施例的一种姿态的确定方法的硬件结构框图；
22.图2是本发明实施例的地图的姿态的确定方法的流程图；
23.图3是根据本发明实施例的姿态的确定装置的结构框图。
【具体实施方式】
24.下文中将参考附图并结合实施例来详细说明本发明。需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。
25.需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
26.本技术实施例所提供的方法实施例可以在移动装置，或者类似的运算装置中执行。以运行在移动装置上为例，图1是本发明实施例的一种姿态的确定方法的硬件结构框图。如图1所示，移动装置可以包括一个或多个(图1中仅示出一个)处理器102(处理器102可以包括但不限于微处理器mcu或可编程逻辑器件fpga等的处理装置)和用于存储数据的存
储器104，在一个示例性实施例中，上述移动装置还可以包括用于通信功能的传输设备106以及输入输出设备108。本领域普通技术人员可以理解，图1所示的结构仅为示意，其并不对上述移动装置的结构造成限定。例如，移动装置还可包括比图1中所示更多或者更少的组件，或者具有与图1所示等同功能或比图1所示功能更多的不同的配置。
27.存储器104可用于存储计算机程序，例如，应用软件的软件程序以及模块，如本发明实施例中的姿态的确定方法对应的计算机程序，处理器102通过运行存储在存储器104内的计算机程序，从而执行各种功能应用以及数据处理，即实现上述的方法。存储器104可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器104可进一步包括相对于处理器102远程设置的存储器，这些远程存储器可以通过网络连接至移动装置。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
28.传输装置106用于经由一个网络接收或者发送数据。上述的网络具体实例可包括移动装置的通信供应商提供的无线网络。在一个实例中，传输装置106包括一个网络适配器(network interface controller，简称为nic)，其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输装置106可以为射频(radio frequency，简称为rf)模块，其用于通过无线方式与互联网进行通讯。
29.下面结合实施例对本发明进行说明：
30.在本实施例中提供了一种姿态的确定方法，如图2所示，该方法包括如下步骤：
31.s202，对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域；
32.s204，确定所述第一roi区域对应的第一3d点云信息；
33.s206，基于所述第一3d点云信息对所述清洁设备的角速度进行修正；
34.s208，基于修正后的所述清洁设备的角速度确定所述清洁设备的姿态。
35.其中，执行上述操作的可以是控制器，或者是决策模块，或者是具备拍摄、更新能力的设备(例如，智能扫地机、智能洗地机等)，或者还可以其他的具备类似处理能力的处理设备或处理单元等，其中，上述控制器或其他执行主体可以是单独存在的，也可以是集成于清洁设备内的。下面以控制器执行上述操作为例(仅是一种示例性说明，在实际操作中还可以是其他的设备或模块来执行上述操作)进行说明：
36.在上述实施例中，第一摄像设备包括但不限于rgb彩色摄像头、灰度摄像头等，第一平面和清洁设备运行平面可以是平行关系，第一平面可以有多种类型的平面，例如，地面、地毯、桌面、橱柜台面等等，因此，可以同时确定第一图像中包括的多种第一平面的第一roi区域，例如，当第一图像中的第一平面包括有三种平面，分别为地面、桌面、橱柜台面时，可以对第一图像中的地面、桌面、橱柜台面同时进行检测，并同时确定出地面、桌面、橱柜台面所分别对应的第一roi区域，还可以优先设置不同类型的第一平面的优先级，例如，当第一图像中的第一平面包括有三种平面，分别为地面、桌面、橱柜台面时，可以将将第一平面的优先级设为地面》桌面》橱柜台面，进而按地面》桌面》橱柜台面设定对第一图像中包括的该三种第一平面进行检测，并依次确定出对应的第一roi区域，另外，第一图像可以有多个，因此，可以同时对多个第一图像进行检测，并同时确定多个第一图像中包括的一种或多种第一平面的第一roi区域，还可以根据第一摄像设备拍摄多个第一图像的先后顺序，优先对
先拍摄的第一图像进行检测，以确定该第一图像中包括的一种或多种第一平面的第一roi区域，需要说明的是，上述第一平面的类型和上述第一图像的优先级的举例说明仅是一种示例性实施例，上述第一平面的类型和上述第一图像的优先级并不仅限于上述举例。
37.在上述实施例中，第一roi区域可以有多个，因此，可以同时确定多个第一roi区域对应的第一3d点云信息，例如，当第一图像中包括的第一平面为地面，且对第一图像进行检测，确定出该地面的多个第一roi区域时，可以同时确定该多个第一roi区域分别对应的第一3d点云信息，还可以根据确定第一roi区域的优先级确定对应的第一3d点云信息，例如，当第一图像中的第一平面包括有三种平面，分别为地面、桌面、橱柜台面时，可以将第一平面的优先级设为地面》桌面》橱柜台面，进而可以按地面》桌面》橱柜台面设定对第一图像中包括的该三种第一平面进行检测，并依次确定出对应的第一roi区域，继而，可以根据确定出的第一roi区域的先后顺序，优先确定先确定的第一roi区域对应的第一3d点云信息，需要说明的是，上述第一roi区域的优先级的举例说明仅是一种示例性实施例，上述第一roi区域的优先级并不仅限于上述举例。
38.在上述实施例中，可以对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，以确定所述第一图像中包括的第一平面的第一roi区域，进而确定所述第一roi区域对应的第一3d点云信息，并基于所述第一3d点云信息对所述清洁设备的角速度进行修正，继而，可以基于修正后的所述清洁设备的角速度确定所述清洁设备的姿态，采用本发明，可以通过获取的第一3d点云信息对清洁设备的角速度进行修正，进而通过修正后的角速度确定清洁设备的姿态，从而实现提高姿态估计的精度和稳定性的目的，有效解决相关技术中存在的需要依赖于加速度计进行姿态估计，从而导致估计误差较大的问题。
39.在一个示例性实施例中，确定所述第一roi区域对应的第一3d点云信息包括：在确定所述清洁设备中包括目标传感器的情况下，确定所述目标传感器所采集到的目标区域的3d点云信息，其中，所述第一图像为对所述目标区域进行拍摄所得到的图像，且所述第一图像的拍摄时间和所述目标传感器采集所述目标区域的3d点云信息的时间相差小于预定阈值；将所述第一图像中包括的全部像素点与所述目标区域的3d点云信息进行配准；基于配准结果确定所述第一roi区域对应的所述第一3d点云信息。在本实施例中，预定阈值是可以预先设定的值，可以设定为3毫秒、5毫秒、10毫秒、15毫秒，等等，例如，当预定阈值为10毫秒时，第一图像的拍摄时间和目标传感器采集目标区域的3d点云信息的时间差值需要小于10毫秒，另外，目标传感器包括但不限于3d点云传感器、tof传感器、rgbd传感器等，需要说明的是，上述预定阈值的设定仅是一种示例性实施例，在实际应用中，还可以采用其他的预定阈值，且预定阈值的设定还可以根据实际应用情况进行调整，此外，第一3d点云和第一图像的配准方法可以采用但不限于采用基于3d点云传感器和第一图像拍摄设备的坐标系外参进行转换对齐的方法。
40.在一个示例性实施例中，确定所述第一roi区域对应的第一3d点云信息包括：在确定所述清洁设备中不包括目标传感器的情况下，对所述第一摄像设备所拍摄的第二图像进行检测，确定所述第二图像中包括的第二平面的第二roi区域，其中，所述第二图像为所述第一图像的前一帧图像；提取所述第一roi区域的第一2d特征点，以及，提取所述第二roi区域的第二2d特征点；基于特征点匹配方式对所述第一2d特征点与所述第二2d特征点进行匹配；基于匹配结果确定所述第一roi区域对应的所述第一3d点云信息。在本实施例中，特征
点匹配方式包括光流法、描述子匹配法等，当然，还可以是其他的具备2d特征匹配功能的方式。
41.在一个示例性实施例中，基于匹配结果确定所述第一roi区域对应的第一3d点云信息包括：确定所述第一摄像设备的平面约束，并基于所述匹配结果和所述平面约束确定目标单应矩阵；基于所述目标单应矩阵确定所述第一图像与所述第二图像之间的旋转矩阵和不带尺度的平移向量；基于所述清洁设备的第一传感器和/或第一算法确定所述第一图像与所述第二图像之间的绝对位置增量；基于所述不带尺度的平移向量和所述绝对位置增量确定带尺度的平移向量；对所述带尺度的平移向量和所述匹配结果进行三角化，以确定所述第一roi区域对应的第一3d点云信息。在本实施例中，可以先基于匹配结果和ransac方法筛掉边缘的和/或错误的已匹配的2d特征点，再通过平面约束从未筛掉的已匹配的2d特征点中获取更多、更精准的已匹配的2d特征点，进而可以基于这些更精准的已匹配的2d特征点确定目标单应矩阵，从而提高匹配2d特征点的精度和鲁棒性，另外，第一传感器包括但不限于码盘传感器、轮速计传感器等，第一算法包括但不限于2d激光传感器的相邻两帧icp算法，例如，可以通过计算扫地机左右轮的码盘得到第一图像和第二图像之间的绝对位置增量，或者采用2d激光传感器的相邻两帧icp算法计算出两帧之间的绝对位置增量，若没有码盘、轮速计等传感器或没有其他算法(即上述第一算法)能计算出准确的平移尺度的情况下，可以采用近似尺度来代替绝对尺度估计值，例如，可以根据扫地机的运行状态获得扫地机的指令速度，基于匀速假设，再乘以两帧相隔时间，就可以近似得到计算出两帧之间的绝对位置增量，若既无法获得两帧之间准确的平移向量，也无法获得两帧之间近似的平移向量，还可以采用点云深度归一化等替代方法，例如，先随意指定一个平移向量的尺度，然后，基于随意尺度的平移向量，进行点云三角化，得到所有内点的3d坐标，基于扫地机(此处清洁设备以扫地机为例，当然也可以是其他的清洁设备，例如，洗地机等等)的运行场景大小，假设当前帧3d点云的平均深度为n米(例如，2.5米、3米、3.5米等等)，对上述得到的所有内点的3d点云进行归一化，该归一化操作使得所有内点的3d点云深度为n米。
42.需要说明的是上述匹配2d特征点和上述绝对位置增量的举例说明仅是一种示例性实施例，上述匹配2d特征点和上述绝对位置增量并不仅限于上述举例。
43.在一个示例性实施例中，对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域包括：利用神经网络模型对所述第一图像进行检测，确定所述第一图像中包括的所述第一平面的所述第一roi区域，其中，所述神经网络模型为利用训练数据对初始神经网络模型进行训练后所得到的模型，所述训练数据包括多组数据，每组数据均包括训练图像以及所述训练图像中所包括的平面的roi区域。在本实施例中，可以利用训练数据预先对初始神经网络模型进行训练，其中，进行训练时所使用的训练数据可以来自数据库，例如，预先收集已经被检测过的包括平面的roi区域的图像并形成数据库，进而基于训练完成的神经网络模型对第一图像进行识别，通过该神经网络训练方法，可以更精准的识别出第一图像中包括的平面的roi区域。
44.在一个示例性实施例中，其特征在于，基于所述第一3d点云信息对所述清洁设备的角速度进行修正包括：按照目标筛选方式从所述第一3d点云中筛选出目标3d点云，并基于所述目标3d点云确定第一平面方程；从所述目标3d点云中确定出目标最大3d点云，并基于所述目标最大3d点云和huber鲁棒核函数通过最小二乘优化生成目标平面方程，其中，所
述目标最大3d点云为距离所述第一平面方程小于预定距离阈值的3d点云；基于所述目标平面方程对所述角速度进行修正。在本实施例中，目标筛选方式包括ransac方法等，当然，还可以是其他的具备筛选功能的方式，另外，可以将包括目标最大3d点云的第一平面方程作为初值并加上huber鲁棒核函数通过最小二乘优化生成目标平面方程，其中，通过huber鲁棒核函数运用，可以进一步3d点云中外点的权重，从目标最大3d点云中确定出最优的目标平面方程，有效降低3d点云中外点数据的影响或降低3d点云的观测噪声的影响。
45.在上述实施例中，预定距离阈值是可以预先设定的值，可以设为2厘米，5厘米、10厘米等等，例如，当预定距离阈值为5厘米时，从所述目标3d点云中确定出距离所述第一平面方程小于5厘米的3d点云，进而可以将这些3d点云确定为目标最大3d点云。
46.在一个示例性实施例中，基于所述目标平面方程对所述角速度进行修正包括：确定所述清洁设备的上一时刻姿态的四元数，并基于所述上一时刻姿态的四元数确定所述上一时刻姿态的第二平面方程；确定所述目标平面方程与所述第二平面方程之间的平面方程差值，以及，获取所述清洁设备的陀螺仪角速度测量值；基于所述平面方程差值确定角速度修正量，并基于所述角速度修正量对所述陀螺仪角速度测量值进行修正，得到修正后的角速度。在本实施例中，可以基于清洁设备上一时刻姿态的四元数确定上一时刻姿态的第二平面方程，进而计算最优的目标平面方程与第二平面方程的平面方程差值，并基于该平面方程差值对清洁设备上一时刻姿态的陀螺仪角速度测量值进行修正，以得到修正后的角速度。
47.在一个示例性实施例中，在基于所述平面方程差值确定角速度修正量，并基于所述角速度修正量对所述陀螺仪角速度测量值进行修正，得到修正后的角速度之后，所述方法还包括：基于所述修正后的角速度对所述上一时刻的四元数进行更新，以得到更新后的当前时刻姿态的四元数；对所述更新后的当前时刻姿态的四元数进行归一化处理，并将归一化处理后的所述四元数转化为欧拉角；基于所述欧拉角确定所述清洁设备的更新姿态。在本实施例中，由于计算误差等因素，导致四元数会逐渐失去规范化特征，因此，需要对更新后的四元数进行规范化处理(即，所述归一化处理)，并将规范化处理后的四元数转化为偏航角、俯仰角、翻滚角(即，所述欧拉角)等。
48.在一个示例性实施例中，在对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域之前，所述方法还包括：对所述清洁设备中包括的传感器的参数进行标定，所述参数如下至少之一：所述第一摄像设备的内参矩阵和畸变模型；所述清洁设备的陀螺仪的内参；所述第一摄像设备坐标系和所述陀螺仪坐标系之间的外参；所述陀螺仪坐标系和所述清洁设备的机体坐标系之间的外参；所述清洁设备中包括的目标传感器的点云内参模型；所述目标传感器坐标系和所述陀螺仪坐标系之间的外参。在本实施例中，所述清洁设备的所述陀螺仪的内参包括但不限于3轴或单轴的偏置、尺度系数，所述第一摄像设备坐标系和所述陀螺仪坐标系之间的外参包括但不限于旋转矩阵和平移，所述陀螺仪坐标系和所述清洁设备的机体坐标系之间的外参包括但不限于旋转矩阵和平移，所述清洁设备中包括的目标传感器的点云内参模型，所述目标传感器坐标系和所述陀螺仪坐标系之间的外参包括但不限于旋转矩阵和平移。
49.在上述实施例中，所述清洁设备中包括的传感器的参数的标定可以是所述清洁设备在未出厂时进行标定的，还可以是所述清洁设备被应用之后的管理人员或维护人员进行
标定的，此外，还可以根据所述清洁设备的实际应用情况进行调整，等等。
50.显然，上述所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。
51.下面以上述清洁设备为扫地机器人为例，结合具体实施例对本发明进行具体说明：
52.步骤一：内参和外参离线标定。可以预先的对需要用到的扫地机中包括的各传感器进行外参和内参标定，主要标定如下参数：
53.1、相机(对应于上述第一摄像设备)内参矩阵和畸变模型。根据选取的相机数学模型的不同，其相应的相机内参矩阵和畸变模型也不同，以针孔模型为例，其内参矩阵k包含：焦距f
x
和fy，主点偏移系数x0和y0，轴倾斜系数s等；
[0054][0055]
畸变模型可为下式，其中，[xc；yc]
t
是未去畸变的点的坐标，[x
corrected
；y
corrected
]
t
是去畸变后的点的坐标，r是点的中心距离，参数[k1,k2,k3]是径向畸变的多项式参数，参数[p1,p2]是切向畸变系数。
[0056][0057]
2、陀螺仪内参包括：3轴或单轴的偏置、尺度系数等；
[0058]
3、相机坐标系和陀螺仪坐标系的外参包括：旋转矩阵和平移；
[0059]
4、陀螺仪坐标系和机体坐标系的外参包括：旋转矩阵和平移；
[0060]
5、tof传感器的点云内参模型(例如，扫地机自带的tof传感器)；
[0061]
6、tof传感器坐标系和陀螺仪坐标系的外参包括：旋转矩阵和平移。
[0062]
步骤二：平面检测
[0063]
1、检测目标
[0064]
检测目标为图像中的平面。该类平面应和清洁设备运行平面是平行关系，该平面可以有多种类型的平面，例如，地面、地毯、桌面、橱柜台面等等。
[0065]
2、检测算法
[0066]
(1)通过相机2d图像的检测以得到roi区域
[0067]
基于相机图像的检测和分割算法采用深度学习方法，相比传统方法，深度学习方法在识别率、准确率和语义等图像领域有明显优势，本发明不限制相机图像的平面的检测和分割算法的具体实现方法，该算法的检测结果为图像平面上的roi区域或范围。
[0068]
(2)计算roi区域的3d点云
[0069]
根据是否带有点云传感器(对应于上述目标传感器)，共分为两种情况:
[0070]
a.若相机不带有3d点云传感器。第一步，在上一帧图像(对应于上述第二图像)的roi区域(对应于上述第二roi区域)中提取2d特征点(对应于上述第二2d特征点)；第二步，在当前帧图像(对应于上述第一图像)的roi区域(对应于上述第一roi区域)中提取2d特征
点(对应于上述第一2d特征点)；第三步，将上一帧图像和当前帧图像的特征点进行匹配，方法不限于光流法、描述子匹配法等；第四步，基于2d特征点匹配结果，利用相机的极线约束(对应于上述平面约束)，求取单应矩阵(对应于上述目标单应矩阵)，其中，可以采用ransac的方法抑制外点并筛选内点；第五步，将单应矩阵分解到两帧图像之间的旋转矩阵和不带尺度的平移向量；第六步，根据扫地机的其他传感器或其他算法获得带尺度的平移向量，例如，可以采用扫地机左右轮的码盘得到两帧图像之间的绝对位置增量，或者采用2d激光传感器的相邻两帧icp算法计算出两帧之间的绝对位置增量，如果没有码盘、轮速计等传感器(对应于上述第一传感器)或者没有其他算法(对应于上述第一算法)能准确计算出准确的平移尺度，可以采用近似尺度来代替绝对尺度，例如，根据扫地机的运行状态得到扫地机的指令速度，基于匀速假设，再乘以两帧相隔时间，就可以近似计算出两帧之间的绝对位置增量；第七步，根据相邻两帧的相对位姿和匹配内点2d坐标，进行三角化，得到所有内点的3d坐标，其中，相邻两帧的相对位姿是指两帧之间的旋转矩阵和带尺度的平移向量。(ransac，random sample consensus，随机抽样一致，它表示可以从一组包含“局外点”的数据集中，通过迭代的方式估计某个数学模型的参数)。
[0071]
在上述第六步中，如果既无法获得两帧之间准确的平移向量，也无法获得两帧之间近似的平移向量，还可以采用点云深度归一化等替代方法，例如，先随意指定一个平移向量的尺度，然后，基于随意尺度的平移向量进行点云三角化，得到所有内点的3d坐标，基于扫地机的运行场景大小，假设当前帧3d点云的平均深度为n米(例如，2.5米、3米、3.5米等等)，对上述得到的所有内点的3d点云进行归一化，该归一化操作使得所有内点的3d点云深度为n米。
[0072]
b.若相机带有3d点云传感器，例如，rgbd相机或tof传感器。第一步，通过该类点云传感器获得基于点云传感器坐标系的3d点云；第二步，再根据相机和点云传感器的配准，获得相机图像中每个像素和点云传感器每个点云的对应关系；第三步，根据相机图像中roi区域的像素范围，取出对应区域的3d点云。
[0073]
如果存在当前帧图像中存在k个roi区域，会得到k(k≥1)组对应区域的3d点云。
[0074]
3、ransac算法详解
[0075]
数据是由“内点”和“外点”组成的，“内点”是组成模型参数的数据，“外点”就是不适合模型的数据。同时ransac假设：在给定一组含有少部分“内点”的数据，存在一个程序可以估计出符合“内点”的模型。基本思想和流程：通过反复选择数据集去估计出模型，一直迭代到估计出需求的模型。具体实现步骤分为以下几步：
[0076]
a.选择出可以估计出模型的最小数据集；
[0077]
b.使用数据集来计算出数据模型；
[0078]
c.将所有数据带入这个模型，计算出“内点”数目；(累加在一定误差范围内的适合当前迭代推出模型的数据)；
[0079]
d.比较当前模型和之前推出的最好的模型的“内点”的数量，记录最大“内点”数的模型参数和“内点”数；
[0080]
e.重复前四步，直到迭代结束或者当前模型为所需求的模型(内点数目大于一定数量)。
[0081]
4、求取点云的平面方程
[0082]
第一步，通过ransac过程对上一步提取的点云进行外点剔除、内点筛选，并得到最大内点数对应平面方程参数；
[0083]
第二步，利用筛选出的内点，以最大内点数对应的平面方程(对应于上述第一平面方程)作为初值，加上huber鲁棒核函数，通过最小二乘优化出最优的平面方程p
_opt
(对应于上述目标平面方程)。平面方程以下式表示，其中a、b、c为对应的平面方程参数：
[0084]
a*x b*y c*z-1＝0
[0085]
如果存在当前帧图像中存在k组对应区域的3d点云，则会得到k个平面方程：
[0086]
a1*x b1*y c1*z-1＝0
[0087]
a2*x b2*y c2*z-1＝0
[0088]
…
[0089]
ai*x bi*y ci*z-1＝0
[0090]
ak*x bk*y ck*z-1＝0
[0091]
其中i取值范围为[1，2，...，k]，ai、bi、ci为第i个平面方程对应的参数。
[0092]
步骤三：基于平面方程的位姿估计方法
[0093]
第一步，上一时刻姿态q
_last
采用四元数表示，即，q
_last
＝q
_0
q
_1
*i q
_2
*j q
_3
*k，其中，q
_0
是实部参数，[q
_1
，q
_2
，q
_3
]为三个虚部对应参数。
[0094]
其中，j，k和m为四元数的三个虚部向量。这三个虚部向量满足如下关系：
[0095][0096]
第二步，利用上一时刻姿态q
_last
得到其对应的上一时刻的平面方程(对应于上述第二平面方程)p
_last
：d*x e*y f*z-1＝0，其中，d、e和f是该平面方程的三个维度的系数，其和四元数各参数的对应关系为：
[0097]
d＝q1q
3-q0q2[0098]
e＝qoq1 q2q3[0099]
f＝q0q0 q3q
3-0.5
[0100]
第三步，计算最优平面方程p
_opt
和上一时刻的平面方程p
_last
的差e，该差的定义为e＝[e
_x
，e
_y
，e
_z
]，其中，[e
_x
，e
_y
，e
_z
]分别为平面差(对应于上述平面方程差值)在x、y和z三个维度的分量：
[0101]ex
＝b*f c*e
[0102]ey
＝c*d a*f
[0103]ez
＝a*e b*d
[0104]
如果当前帧图像中存在k组对应区域的3d点云的k个平面方程，该差的定义为e＝r
_1
*e
_1
r
_2
*e
_2
... r
_i
*e
_i
r
_k
*e
_k
，其中，i取值范围为[1，2，...，k]，e
_i
＝[e
_xi
，e
_yi
，e
_zi
]是第i组平面方程和上一时刻平面方程方程p
_last
的差：
[0105]exi
＝bi*f ci*e
[0106]eyi
＝ci*d ai*f
[0107]ezi
＝ai*e bi*d
[0108]r_k
是第i个平面方程的不确定性，代表第i个平面的可信度，例如，该可信度与第i平面的3d点云的点数成正比。
[0109]
第四步，修正陀螺仪的角速度。从陀螺仪传感器获得角速度信号w
_sensor，
将上一步得到的平面方程的e，通过比例和积分运算累加到角速度分量中，得到修正后的角速度w
correct
＝[w
x
，wy，wz]：
[0110]w_correct
＝w
_sensor
k
_prop
*e k
_integ
*e*δt
[0111]
其中，[w
x
，wy，wz]分别为修正后角速度的x分量、y分量和z分量，k
_prop
是比例系数，k
_integ
是积分系数，δt是陀螺仪采样周期。
[0112]
第五步，更新姿态。利用修正后的角速度w
_correct
对上一时刻姿态q
_last
利用修正后的角速度进行更新，更新后的四元数q
_correct
的计算公式为：
[0113]q_correct
＝q
_last
δq
[0114]
其中，δq＝[δq0，δq1，δq2，δq3]为更新增量，δq0是更新增量的实部参数，[δq1，δq2，δq3]分别是更新增量的三个虚部参数：
[0115]
δq0＝-(q1*w
x
q2*wy q3*wz)*δt/2
[0116]
δq1＝(q0*w
x
q2*w
z-q3*wy)*δt/2
[0117]
δq2＝(q0*w
y-q1*wz q3*w
x
)*δt/2
[0118]
δq3＝(q0*wz q1*w
y-q2*w
x
)*δt/2
[0119]
第六步，计算当前时刻四元数对应的归一化四元数q
_norm
＝[q
_n0
，q
_n1
，q
_n2
，q
_n3
]。其中q
_n0
是实部参数，[q
_n1
，q
_n2
，q
_n3
]为三个虚部对应参数。计算公式：
[0120][0121]
其中，j＝0，1，2，3。
[0122]
第七步，将当前时刻归一化四元数q
_norm
转换为当前时刻欧拉角，其转换公式如下：
[0123]
可以基于四元数通过以下关系式获得欧拉角，为偏航角，θ为俯仰角，为翻滚角：
[0124][0125]
其中，arctan和arcsin的取值范围是这并不能覆盖欧拉角所有朝向(对于俯仰角，其的取值范围已经满足)，因此，针对偏航角和翻滚角，需要用atan2来代替arctan：
[0126][0127]
其中，atan2的取值范围是
[0128]
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如rom/ram、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。
[0129]
在本实施例中还提供了一种姿态的确定装置，该装置用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。
[0130]
图3是根据本发明实施例的姿态的确定装置的结构框图，如图3所示，该装置包括：
[0131]
第一确定模块32，用于对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域；
[0132]
第二确定模块34，用于确定所述第一roi区域对应的第一3d点云信息；
[0133]
修正模块36，用于基于所述第一3d点云信息对所述清洁设备的角速度进行修正；
[0134]
第三确定模块38，用于基于修正后的所述清洁设备的角速度确定所述清洁设备的姿态。
[0135]
在一个示例性实施例中，上述第二确定模块34包括：第一确定单元，用于在确定所述清洁设备中包括目标传感器的情况下，确定所述目标传感器所采集到的目标区域的3d点云信息，其中，所述第一图像为对所述目标区域进行拍摄所得到的图像，且所述第一图像的拍摄时间和所述目标传感器采集所述目标区域的3d点云信息的时间相差小于预定阈值；第一匹配单元，用于将所述第一图像中包括的全部像素点与所述目标区域的3d点云信息进行配准；第二确定单元，用于基于配准结果确定所述第一roi区域对应的所述第一3d点云信息。
[0136]
在一个示例性实施例中，上述第二确定模块34包括：第三确定单元，用于在确定所述清洁设备中不包括目标传感器的情况下，对所述第一摄像设备所拍摄的第二图像进行检测，确定所述第二图像中包括的第二平面的第二roi区域，其中，所述第二图像为所述第一图像的前一帧图像；提取单元，用于提取所述第一roi区域的第一2d特征点，以及，提取所述第二roi区域的第二2d特征点；第二匹配单元，用于基于特征点匹配方式对所述第一2d特征点与所述第二2d特征点进行匹配；第四确定单元，用于基于匹配结果确定所述第一roi区域对应的所述第一3d点云信息。
[0137]
在一个示例性实施例中，上述第四确定单元包括：第一确定子单元，用于确定所述第一摄像设备的平面约束，并基于所述匹配结果和所述平面约束确定目标单应矩阵；第二确定子单元，用于基于所述目标单应矩阵确定所述第一图像与所述第二图像之间的旋转矩
阵和不带尺度的平移向量；第三确定子单元，用于基于所述清洁设备的第一传感器和/或第一算法确定所述第一图像与所述第二图像之间的绝对位置增量；第四确定子单元，用于基于所述不带尺度的平移向量和所述绝对位置增量确定带尺度的平移向量；第五确定子单元，用于对所述带尺度的平移向量和所述匹配结果进行三角化，以确定所述第一roi区域对应的第一3d点云信息。
[0138]
在一个示例性实施例中，上述第一确定模块32包括：第五确定单元，用于利用神经网络模型对所述第一图像进行检测，确定所述第一图像中包括的所述第一平面的所述第一roi区域，其中，所述神经网络模型为利用训练数据对初始神经网络模型进行训练后所得到的模型，所述训练数据包括多组数据，每组数据均包括训练图像以及所述训练图像中所包括的平面的roi区域。
[0139]
在一个示例性实施例中，上述修正模块36包括：第六确定单元，用于按照目标筛选方式从所述第一3d点云中筛选出目标3d点云，并基于所述目标3d点云确定第一平面方程；生成单元，用于从所述目标3d点云中确定出目标最大3d点云，并基于所述目标最大3d点云和huber鲁棒核函数通过最小二乘优化生成目标平面方程，其中，所述目标最大3d点云为距离所述第一平面方程小于预定距离阈值的3d点云；修正单元，用于基于所述目标平面方程对所述角速度进行修正。
[0140]
在一个示例性实施例中，上述修正单元包括：第六确定子单元，用于确定所述清洁设备的上一时刻姿态的四元数，并基于所述上一时刻姿态的四元数确定所述上一时刻姿态的第二平面方程；获取单元，用于确定所述目标平面方程与所述第二平面方程之间的平面方程差值，以及，获取所述清洁设备的陀螺仪角速度测量值；修正子单元，用于基于所述平面方程差值确定角速度修正量，并基于所述角速度修正量对所述陀螺仪角速度测量值进行修正，得到修正后的角速度。
[0141]
在一个示例性实施例中，上述装置还包括：更新模块，用于在基于所述平面方程差值确定角速度修正量，并基于所述角速度修正量对所述陀螺仪角速度测量值进行修正，得到修正后的角速度之后，基于所述修正后的角速度对所述上一时刻姿态的四元数进行更新，以得到更新后的当前时刻姿态的四元数；转化模块，用于对所述当前时刻姿态的四元数进行归一化处理，并将归一化处理后的所述四元数转化为欧拉角；第四确定模块，用于基于所述欧拉角确定所述清洁设备的更新姿态。
[0142]
在一个示例性实施例中，上述装置还包括：标定模块，用于在对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域之前，对所述清洁设备中包括的传感器的参数进行标定，所述参数如下至少之一：所述第一摄像设备的内参矩阵和畸变模型；所述清洁设备的陀螺仪的内参；所述第一摄像设备坐标系和所述陀螺仪坐标系之间的外参；所述陀螺仪坐标系和所述清洁设备的机体坐标系之间的外参；所述清洁设备中包括的目标传感器的点云内参模型；所述目标传感器坐标系和所述陀螺仪坐标系之间的外参。
[0143]
在本实施例中还提供了一种清洁设备，该清洁设备可以包括上述任一项所述的姿态的确定装置。
[0144]
需要说明的是，上述各个模块是可以通过软件或硬件来实现的，对于后者，可以通过以下方式实现，但不限于此：上述模块均位于同一处理器中；或者，上述各个模块以任意
组合的形式分别位于不同的处理器中。
[0145]
本发明的实施例还提供了一种计算机可读存储介质，该计算机可读存储介质中存储有计算机程序，其中，该计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。
[0146]
在本实施例中，上述计算机可读存储介质可以被设置为存储用于执行以下步骤的计算机程序：
[0147]
s1，对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域；
[0148]
s2，确定所述第一roi区域对应的第一3d点云信息；
[0149]
s3，基于所述第一3d点云信息对所述清洁设备的角速度进行修正；
[0150]
s4，基于修正后的所述清洁设备的角速度确定所述清洁设备的姿态。
[0151]
在一个示例性实施例中，上述计算机可读存储介质可以包括但不限于：u盘、只读存储器(read-only memory，简称为rom)、随机存取存储器(random access memory，简称为ram)、移动硬盘、磁碟或者光盘等各种可以存储计算机程序的介质。
[0152]
本发明的实施例还提供了一种电子装置，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。
[0153]
在一个示例性实施例中，上述电子装置还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。
[0154]
在一个示例性实施例中，上述处理器可以被设置为通过计算机程序执行以下步骤：
[0155]
s1，对清洁设备上设置的第一摄像设备所拍摄的第一图像进行检测，确定所述第一图像中包括的第一平面的第一roi区域；
[0156]
s2，确定所述第一roi区域对应的第一3d点云信息；
[0157]
s3，基于所述第一3d点云信息对所述清洁设备的角速度进行修正；
[0158]
s4，基于修正后的所述清洁设备的角速度确定所述清洁设备的姿态。
[0159]
通过本发明提供的一种高精度的、鲁棒性高、不随时间漂移的以及不依赖于加速度计姿态的确定方法，通过扫地机(以扫地机为例说明，还可以是其他的清洁设备，例如，洗地机等)自带的相机和/或tof传感器数据，进行水平面的平面检测，构建平面约束，再融合陀螺仪的3轴或单轴的角速度数据，进行扫地机的姿态估计。
[0160]
显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。
[0161]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。