CN111684382A - 可移动平台状态估计方法、系统、可移动平台及存储介质 - Google Patents

可移动平台状态估计方法、系统、可移动平台及存储介质 Download PDF

Info

Publication number
CN111684382A
CN111684382A CN201980008419.2A CN201980008419A CN111684382A CN 111684382 A CN111684382 A CN 111684382A CN 201980008419 A CN201980008419 A CN 201980008419A CN 111684382 A CN111684382 A CN 111684382A
Authority
CN
China
Prior art keywords
point cloud
cloud data
ground point
accumulated frame
movable platform
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
Application number
CN201980008419.2A
Other languages
English (en)
Other versions
CN111684382B (zh
Inventor
王荣志
刘晓洋
张晓炜
王京
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Zhuoyu Technology Co ltd
Original Assignee
SZ DJI Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by SZ DJI Technology Co Ltd filed Critical SZ DJI Technology Co Ltd
Publication of CN111684382A publication Critical patent/CN111684382A/zh
Application granted granted Critical
Publication of CN111684382B publication Critical patent/CN111684382B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0255—Control of position or course in two dimensions specially adapted to land vehicles using acoustic signals, e.g. ultra-sonic singals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0285—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using signals transmitted via a public communication network, e.g. GSM network

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Acoustics & Sound (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明实施例提供了一种可移动平台状态估计方法、系统、可移动平台及存储介质，其中，该方法包括：获取可移动平台当前所处周围环境对应的三维点云数据；从所述三维点云数据中确定属于同一平面的地面点云数据；根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。通过这种方式，可以提高可移动平台状态估计的精准性和稳定性。

Description

可移动平台状态估计方法、系统、可移动平台及存储介质
技术领域
本发明实施例涉及控制技术领域，尤其涉及一种可移动平台状态估计方法、系统、可移动平台及存储介质。
背景技术
目前，无人车、运动机器人等可移动平台主要通过识别周围环境和可移动平台自身的状态控制可移动平台的移动，然而在某些特殊场景下，比如周围动态物体较多，夜晚光线昏暗等，导致地面行驶的无人车在横滚、俯仰、高度上的可观性较差，在这种情况下很难对可移动平台的状态进行准确的估计。因此，如何在可观性较差的环境下更有好地确定可移动平台的状态具有十分重要的意义。
发明内容
本发明实施例提供了一种可移动平台状态估计方法、系统、可移动平台及存储介质，可以实现在可观性较差的环境下更好地估计可移动平台的状态，并提高了对可移动平台状态估计的精度和稳定性。
第一方面，本发明实施例提供了一种可移动平台状态估计方法，包括：
获取所述可移动平台当前所处周围环境对应的三维点云数据；
从所述三维点云数据中确定属于同一平面的地面点云数据；
根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。
第二方面，本发明实施例提供了一种状态估计系统，包括：
点云传感器，用于获取三维点云数据；
存储器，用于存储程序指令；
处理器，用于调用所述程序指令，当所述程序指令被执行时，用于执行以下操作：
获取所述可移动平台当前所处周围环境对应的三维点云数据；
从所述三维点云数据中确定属于同一平面的地面点云数据；
根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。
第三方面，本发明实施例提供了一种可移动平台，所述可移动平台包括：
机身；
配置在机身上的动力系统，用于为所述可移动平台提供移动的动力；
如上述第二方面所述的状态估计系统。
第四方面，本发明实施例提供了一种计算机可读存储介质，该计算机可读存储介质存储有计算机程序，该计算机程序被处理器执行时实现如上述第一方面所述的方法。
本发明实施例，通过获取可移动平台当前所处周围环境对应的三维点云数据，从所述三维点云数据中确定属于同一平面的地面点云数据，并根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息，实现了通过对可移动平台的位置信息和/或姿态信息进行修正提高对可移动平台状态估计的稳定性和精准性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种状态估计系统的结构示意图；
图2是本发明实施例提供的一种可移动平台状态估计方法的流程示意图；
图3是本发明实施例提供的另一种可移动平台状态估计方法的流程示意图；
图4是本发明实施例提供的一种地面点云数据的匹配示意图；
图5a是本发明实施例提供的一种对可移动平台的状态修正前的高度变化结果示意图；
图5b是本发明实施例提供的一种对可移动平台的状态修正后的高度变化结果示意图；
图6是本发明实施例提供的一种状态估计系统的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
下面结合附图，对本发明的一些实施方式作详细说明。在不冲突的情况下，下述的实施例及实施例中的特征可以相互组合。
本发明实施例中提供的可移动平台状态估计方法可以由一种状态估计系统执行。其中，所述状态估计系统可应用于可移动平台。在某些实施例中，所述状态估计系统可以安装在可移动平台上；在某些实施例中，所述状态估计系统可以在空间上独立于可移动平台。在某些实施例中，所述可移动平台可以包括但不限于如能够自主移动的机器人、无人车、无人船等可移动系统。
通过状态估计系统可以获取所述可移动平台当前所处周围环境对应的三维点云数据，并从所述三维点云数据中确定属于同一平面的地面点云数据，从而根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。在某些实施例中，所述三维点云数据可以是通过激光雷达获取得到，也可以是通过毫米波雷达、超声波雷达、可移动平台上的摄像头等获取得到，本发明实施例不做具体限定。
本发明实施例考虑到在可观性较差的场景下，比如在隧道内跟大车等环境噪声较明显的场景，可移动平台通过视觉惯性导航系统对可移动平台的位置、速度、姿态等状态进行估计时存在较大的误差，容易估计错误，因此，利用获取到的所述可移动平台周围环境的三维点云数据提取出的地面点云数据来约束可移动平台的横滚角、俯仰角、高度等状态，从而能够有效提升在某些特殊场景下对可移动平台状态进行估计的稳定性和精度。通过判断前后相邻的地面点云数据的共面一致性来约束对可移动平台的前后变化状态进行估计，从而提升在诸如动态物体较多，纹理稀疏等特殊场景下的鲁棒性。
在一个实施例中，状态估计系统通过获取可移动平台周围环境的三维点云数据，根据所述三维点云数据确定出一个拟合平面，并对拟合平面的地面点云数据进行位姿修正，以及对位姿修正后的该拟合平面的地面点云数据进行二次修正，以确定出所述地面点云数据在高度上的误差信息，并将该误差信息作为一种约束因子传输给视觉惯性导航系统中进行非线性优化，从而提升状态估计的鲁棒性。
在一个实施例中，所述状态估计系统在确定地面点云数据时，可以在获取到可移动平台周围环境的三维点云数据后，以该可移动平台为中心将距离该可移动平台周围指定范围内的三维点云数据划分为多个栅格区域，并统计每个栅格区域内三维点云数据的高度，从而确定高度小于预设高度阈值的三维点云数据为初始地面点云数据。所述状态估计系统可以根据确定的所述初始地面点云拟合得到拟合平面和该拟合平面的法向量，并利用法向量确定与拟合平面的距离小于预设距离阈值的地面点云数据。
在一个实施例中，所述状态估计系统可以获取预设数量的帧累计得到的第一当前累计帧和第一历史累计帧，并将第一当前累计帧的地面点云数据投影至所述第一历史累计帧的地面点云数据中，以确定所述第一当前累计帧的地面点云数据与所述第一历史累计帧的地面点云数据的第一误差信息，并根据所述第一误差信息对可移动平台的横滚角、俯仰角、高度等进行修正，从而得到修正后的地面点云数据。
在一个实施例中，所述状态估计系统在得到所述修正后的地面点云数据之后，可以根据修正后的每帧地面点云数据，构建指定时间范围内累计获取到的第二当前累计帧和第二历史累计帧，并将所述第二当前累计帧的地面点云数据投影到所述第二历史累计帧的地面点云数据上，以确定所述第二当前累计帧的地面点云数据与所述第二历史累计帧的地面点云数据在高度上的第二误差信息，从而将该第二误差信息作为约束因子发送给视觉惯性导航系统进行优化，来提升状态估计的鲁棒性。
下面结合附图1对本发明实施例提供的状态估计系统进行示意性说明。
请参见图1，图1是本发明实施例提供的一种状态估计系统的结构示意图。所述状态估计系统包括：状态估计设备11、可移动平台12。其中，可移动平台12和状态估计设备11之间可以通过无线通信连接方式建立通信连接。其中，在某些场景下，所述可移动平台12和状态估计设备11之间也可以通过有线通信连接方式建立通信连接。在某些实施例中，所述状态估计设备11可以设置在可移动平台12上。在某些实施例中，所述可移动平台12可以包括但不限于无人车、无人船、可移动机器人等可移动设备。所述可移动平台12包括动力系统121，所述动力系统121用于为可移动平台12提供移动的动力。在其他实施例中，可移动平台12和状态估计设备11彼此独立，所述状态估计设备11可以包括遥控器、智能手机、平板电脑、膝上型电脑和穿戴式设备中的一种或者多种。在其他实施例中，状态估计设备11可以是远离于所述可移动平台12，例如，例如状态估计设备11设置在云端服务器中，通过无线通信连接方式与可移动平台12建立通信连接。在某些实施例中，所述状态估计设备11中包括点云传感器，所述点云传感器用于获取三维点云数据。
本发明实施例中，所述状态估计设备11可以获取所述可移动平台当前所处周围环境对应的三维点云数据，并从所述三维点云数据中确定属于同一平面的地面点云数据，以及根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息，从而提高可移动平台状态估计的精准性和稳定性。
下面结合附图对本发明实施例提供的可移动平台状态估计方法进行示意性说明。
具体请参见图2，图2是本发明实施例提供的一种可移动平台状态估计方法的流程示意图，所述方法可以由状态估计系统执行，其中，状态估计系统的具体解释如前所述。具体地，本发明实施例的所述方法包括如下步骤。
S201：获取可移动平台当前所处周围环境对应的三维点云数据。
本发明实施例中，状态估计系统可以获取可移动平台当前所处周围环境对应的三维点云数据。在某些实施例中，所述状态估计系统在获取所述可移动平台所处周围环境对应的三维点云数据时，可以通过点云传感器获取所述可移动平台所处周围环境对应的三维点云数据。
在一个实施例中，所述点云传感器可以包括激光雷达，所述状态估计系统在获取所述可移动平台所处周围环境对应的三维点云数据时，可以通过激光雷达获取所述可移动平台所处周围环境对应的三维点云数据。例如，所述状态估计系统在可以通过激光雷达获取所述可移动平台所处周围环境对应的100hz的三维点云数据。
在某些实施例中，所述激光雷达是一种感知传感器，可以获得场景的三维信息。其基本原理为主动对被探测对象发射激光脉冲信号，并获得其反射回来的脉冲信号，根据发射信号和接收信号之间的时间差计算被测对象的距离探测器的深度信息；基于激光雷达的已知发射方向，获得被测对象相对激光雷达的角度信息；结合前述深度信息和角度信息得到海量的探测点(称为点云)，基于点云即可以重建被测对象相对激光雷达的空间三维信息。
在一个实施例中，所述点云传感器可以包括摄像头，所述状态估计系统在获取所述可移动平台所处周围环境对应的三维点云数据时，可以通过摄像头获取所述可移动平台所处周围环境对应的三维点云数据。在某些实施例中，所述摄像头可以挂载在所述可移动平台上。在某些实施例中，所述摄像头还可以独立于可移动平台，安装于所述可移动平台所处环境当中。在某些实施例中，所述摄像头包括但不限于双目摄像头、单目摄像头，TOF摄像头等摄像装置。
在一些实施例中，所述状态估计系统在通过摄像头获取所述可移动平台所处周围环境对应的三维点云数据时，可以基于预设转换矩阵将所述摄像头获取的点云转换到世界坐标系中，得到所述可移动平台所处周围环境对应的三维点云数据；其中，所述预设转换矩阵包括内参矩阵和外参矩阵，所述外参矩阵包括旋转矩阵和/或平移向量。在某些实施例中，当所述世界坐标系的原点设定在所述可移动平台上时，所述外参矩阵只包括旋转矩阵。
在某些实施例中，所述内参矩阵是根据多个内参数确定得到，所述内参数是摄像头标定得到的参数，如焦距、像主点坐标等。在某些实施例中，所述外参矩阵可以包括旋转矩阵和/或平移向量，其中，所述旋转矩阵可以通过摄像头的姿态确定得到的，所述平移向量可以通过摄像头的可移动平台状态估计信息确定得到。
可见，本发明实施例通过将摄像头采集到的点云转换到世界坐标系，以得到所述三维点云数据，在将摄像头采集到的点云转换到世界坐标系的过程中，可以对摄像头采集到的点云进行去畸变等处理，从而提升所述三维点云数据的准确性。
S202：从所述三维点云数据中确定属于同一平面的地面点云数据。
本发明实施例中，状态估计系统可以从所述三维点云数据中确定属于同一平面的地面点云数据。
在一个实施例中，所述状态估计系统从所述三维点云数据中确定属于同一平面的地面点云数据时，可以根据预设的多个栅格区域对所述三维点云数据进行划分，并根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据。
在一种实施方式中，所述状态估计系统在根据预设的多个栅格区域对所述三维点云数据进行划分时，可以获取以所述可移动平台为中心，在所述可移动平台周围预设距离范围内的三维点云数据，并将在所述预设距离范围内的三维点云数据划分为多个栅格区域。例如，所述状态估计系统可以以所述可移动平台为中心，获取所述可移动平台前后左右距离该可移动平台4m范围内的三维点云数据，并根据预设的多个栅格区域将所述4m范围内的三维点云数据划分为多个栅格区域。
在一个实施例中，所述状态估计系统在根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据时，可以获取所述每个栅格区域内的三维点云数据的高度，并根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据。
在一个实施例中，所述状态估计系统在根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据时，可以确定所述每个栅格区域内的高度小于预设高度阈值的初始地面点云数据，并对所述初始地面点云数据进行拟合得到拟合平面以及所述拟合平面的法向量，从而根据所述法向量确定属于所述拟合平面的地面点云数据。
例如，假设预设高度阈值5cm，则所述状态估计系统可以获取划分得到的每个栅格区域内的三维点云数据的高度，并确定每个栅格区域内的高度小于5cm三维点云数据为初始地面点云数据。
在一个实施例中，所述状态估计系统在根据所述法向量确定属于所述拟合平面的地面点云数据时，可以根据所述法向量确定每个栅格区域内的三维点云数据与所述拟合平面的距离，并从所述每个栅格区域中确定与所述拟合平面的距离小于预设距离阈值的地面点云数据。
例如，假设所述预设距离阈值为2cm，则所述状态估计系统可以根据确定的属于拟合平面的法向量，确定每个栅格区域内的三维点云数据与所述拟合平面的距离，并从所述每个栅格区域中确定与所述拟合平面的距离小于2cm的三维点云数据为地面点云数据。
S203：根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。
本发明实施例中，状态估计系统可以根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。在某些实施例中，所述位置信息包括高度信息；所述姿态信息包括横滚角和/或俯仰角。
由于所述状态估计系统获取到的三维点云数据可能存在误差，因此需要根据确定出的地面点云数据对可移动平台的位置信息和/或姿态信息进行修正，以确保估计得到的可移动平台的状态的准确性和稳定性。
在一个实施例中，所述状态估计系统在根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息时，可以根据所述地面点云数据确定第一误差信息和第二误差信息，并根据第一误差信息和第二误差信息修正所述可移动平台的位置信息和/或姿态信息。
可见，通过这种根据地面点云数据对可移动平台进行两次修正的实施方式，可以提高估计可移动平台状态的精度和稳定性。
本发明实施例中，状态估计系统通过获取所述可移动平台当前所处周围环境对应的三维点云数据，从所述三维点云数据中确定属于同一平面的地面点云数据，并根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息，实现了对可移动平台的位置信息和/或姿态信息进行修正，提高了对可移动平台状态估计的稳定性和精准性。
具体请参见图3，图3是本发明实施例提供的另一种可移动平台状态估计方法的流程示意图，所述方法可以由状态估计系统执行，其中，状态估计系统的具体解释如前所述。本发明实施例与图2所述实施例的区别在于本发明实施例是对具体如何对可移动平台的位置信息和/或姿态信息进行修正进行示意性说明。具体地，本发明实施例的所述方法包括如下步骤。
S301：获取可移动平台当前所处周围环境对应的三维点云数据。
本发明实施例中，状态估计系统可以获取可移动平台当前所处周围环境对应的三维点云数据。
S302：从所述三维点云数据中确定属于同一平面的地面点云数据。
本发明实施例中，状态估计系统可以从所述三维点云数据中确定属于同一平面的地面点云数据。
S303：根据所述地面点云数据确定第一误差信息和第二误差信息，并根据第一误差信息和第二误差信息修正所述可移动平台的位置信息和/或姿态信息。
本发明实施例中，状态估计系统可以根据所述地面点云数据确定第一误差信息和第二误差信息，并根据第一误差信息和第二误差信息修正所述可移动平台的位置信息和/或姿态信息。
在一个实施例中，所述状态估计系统在根据第一误差信息和第二误差信息修正所述可移动平台的位置信息和/或姿态信息时，可以首先将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息，并根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息。
在一个实施例中，所述状态估计系统在将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累计帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息时，可以将所述第一当前累计帧中的栅格区域投影至所述第一历史累计帧的栅格区域，并确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值，以及将所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较，从而确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第一误差信息。在某些实施例中，所述第一当前累计帧是通过获取预设数量帧累计得到的，所述第一历史累计帧是根据历史获取到的预设数量帧累计得到的。
例如，假设预设数量帧为10帧，则所述状态估计系统可以获取当前累计的10帧以及当前累计的10帧之前历史累计的相邻10帧，并将所述当前累计的10帧中的栅格区域投影至所述历史累计的相邻10帧的栅格区域，并确定所述当前累计的10帧中每个栅格区域内的地面点云数据的高度均值，以及将所述当前累计的10帧中每个栅格区域内的地面点云数据的高度均值与所述历史累计的相邻10帧对应每个栅格区域内的地面点云数据的高度均值进行比较，从而确定所述当前累计的10帧中每个栅格区域内的地面点云数据的高度均值与所述历史累计的相邻10帧对应每个栅格区域内的地面点云数据的高度均值之间的第一误差信息。
可见，通过这种对可移动平台的状态进行修正的实施方式，可以提高对可移动平台状态估计的精度和稳定性。
在一个实施例中，所述状态估计系统在根据第一误差信息修正所述可移动平台的位置信息和/或姿态信息时，可以根据第一误差信息修正所述可移动平台的高度、横滚角、俯仰角等，以对所述可移动平台的状态进行优化。
在一种实施方式中，所述状态估计系统可以根据预设的优化规则和所述第一误差信息对所述可移动平台的状态进行优化。在某些实施例中，所述预设的优化规则可以为如下公式(1)所示：
Figure BDA0002583866120000101
其中，i代表第i个栅格区域，k代表第k帧，xik,yik,zik为第k帧地面点云数据在第i个栅格区域内的高度均值，
Figure BDA0002583866120000102
为第i个栅格区域内所有地面点云数据的高度均值，ak,bk,ck为待优化的变量，分别代表了横滚角、俯仰角、高度上的调整值。
在一个实施例中，所述状态估计系统可以根据公式(1)确定出第一误差信息，并根据所述第一误差信息对可移动平台的横滚角、俯仰角、高度进行优化调整，从而得到修正后的地面点云数据。
在一个实施例中，所述状态估计系统根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后，可以获取根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后得到的地面点云数据，并根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息。
在一个实施例中，所述状态估计系统在根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息时，可以将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息，并根据所述第二误差信息修正所述可移动平台的位置信息和/或姿态信息。
在一个实施例中，所述状态估计系统在将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息时，可以将所述第二当前累计帧中的栅格区域投影至所述第二历史累计帧的栅格区域，并确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值，以及将所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较，从而确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第二误差信息。在某些实施例中，所述第二当前累计帧是通过获取指定时间范围内的帧累计得到的，所述第二历史累计帧是根据历史获取到的指定时间范围内的帧累计得到的。在某些实施例中，第二当前累计帧中包括的帧数量与第二历史累计帧中包括的帧数量可以相同，也可以不相同。
例如，假设指定时间范围为10s，则所述状态估计系统可以获取在当前10s内累计的第二当前累计帧以及在当前10s之前历史累计的10s内的第二历史累计帧，并将所述第二当前累计帧中的栅格区域投影至所述第二历史累计帧的栅格区域，并确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值，以及将所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较，从而确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第二误差信息。
可见，通过这种第二次对可移动平台的状态进行修正的实施方式，可以进一步提高对可移动平台状态估计的精度和稳定性。
在一个实施例中，所述状态估计系统在将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配时，可以将所述第二当前累计帧中的栅格区域投影至所述第二历史累计帧的栅格区域进行匹配。
在一种实施方式中，所述状态估计系统将所述第二当前累计帧中的栅格区域投影至所述第二历史累计帧的栅格区域进行匹配的过程如图4所示，图4是本发明实施例提供的一种地面点云数据的匹配示意图。如图4所示包括在预设时间范围内获取到的第二当前累计帧41和第二历史累计帧42，所述状态估计系统可以将所述第二当前累计帧41中的栅格区域投影至所述第二历史累计帧42的栅格区域上，从而确定出所述第二当前累计帧41中的栅格区域411投影到第二历史累计帧42的栅格区域中的对应位置区域421。
在一个实施例中，所述状态估计系统在确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第二误差信息时，可以根据如下公式(2)确定所述第二误差信息。
Figure BDA0002583866120000121
Figure BDA0002583866120000122
Figure BDA0002583866120000123
其中，res为第二误差信息，对每个栅格区域，其包括高度均值p和法向量n；第二当前累计帧中某个栅格区域的特征为p1，n2，第二当前累计帧位姿为Rcur，Pcur；第二历史累计帧对应的栅格区域特征为p2，n2，第二历史累计帧位姿为Rpre，Ppre。
在一个实施例中，所述状态估计系统根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息之后，可以根据确定的第二误差信息修正所述可移动平台的高度信息、横滚角、俯仰角中的任意一种或多种，以使修正后得到的误差信息小于预设阈值。
在一个实施例中，所述状态估计系统根据所述第二误差信息修正所述可移动平台的高度信息、横滚角、俯仰角之后，可以确定出对所述可移动平台的状态估计的高度变化结果。具体可以图5a和图5b为例进行说明，图5a是本发明实施例提供的一种对可移动平台的状态修正前的高度变化结果示意图，图5b是本发明实施例提供的一种对可移动平台的状态修正后的高度变化结果示意图。通过图5a所示的高度变化结果示意图可以确定所述可移动平台的高度变化误差较大，从而可以确定所述第二误差信息较大。通过本发明实施例提供的方案对可移动平台的状态进行修正后，可以估计得到如图5b所示的高度变化结果示意图，图5b所示的高度变化误差明显比图5a所示的高度变化误差减小很多。可见，通过本发明实施例可以大大抑制在特殊场景下估计可移动平台状态的误差，提高了对可移动平台进行状态估计的鲁棒性和精度。
本发明实施例中，状态估计系统通过获取所述可移动平台当前所处周围环境对应的三维点云数据，从所述三维点云数据中确定属于同一平面的地面点云数据，并根据所述地面点云数据确定第一误差信息和第二误差信息，以及根据第一误差信息和第二误差信息修正所述可移动平台的位置信息和/或姿态信息。通过这种实施方式实现了对可移动平台的位置信息和/或姿态信息进行多次修正，进一步提高了对可移动平台状态估计的稳定性和精准性。
请参见图6，图6是本发明实施例提供的一种状态估计系统的结构示意图。具体的，所述状态估计系统包括：存储器601、处理器602。
在一种实施例中，所述状态估计系统还包括数据接口603，所述数据接口603，用于传递状态估计系统和其他系统之间的数据信息。
在一种实施例中，所述状态估计系统还包括点云传感器604，所述点云传感器604用于获取三维点云数据。
所述存储器601可以包括易失性存储器(volatile memory)；存储器601也可以包括非易失性存储器(non-volatile memory)；存储器601还可以包括上述种类的存储器的组合。所述处理器602可以是中央处理器(central processing unit，CPU)。所述处理器602还可以进一步包括硬件芯片。上述硬件芯片可以是专用集成电路(application-specificintegrated circuit，ASIC)，可编程逻辑器件(programmable logic device，PLD)或其组合。上述PLD可以是复杂可编程逻辑器件(complex programmable logic device，CPLD)，现场可编程逻辑门阵列(field-programmable gate array，FPGA)或其任意组合。
所述存储器601用于存储程序指令，所述处理器602可以调用存储器601中存储的程序指令，用于执行如下步骤：
获取可移动平台当前所处周围环境对应的三维点云数据；
从所述三维点云数据中确定属于同一平面的地面点云数据；
根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。
进一步地，所述处理器602从所述三维点云数据中确定属于同一平面的地面点云数据时，具体用于：
根据预设的多个栅格区域对所述三维点云数据进行划分；
根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据。
进一步地，所述处理器602根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据时，具体用于：
获取所述每个栅格区域内的三维点云数据的高度；
根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据。
进一步地，所述处理器602根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据时，具体用于：
确定所述每个栅格区域内的高度小于预设高度阈值的初始地面点云数据；
对所述初始地面点云数据进行拟合得到拟合平面以及所述拟合平面的法向量；
根据所述法向量确定属于所述拟合平面的地面点云数据。
进一步地，所述处理器602根据所述法向量确定属于所述拟合平面的地面点云数据时，具体用于：
根据所述法向量确定每个栅格区域内的三维点云数据与所述拟合平面的距离；
从所述每个栅格区域中确定与所述拟合平面的距离小于预设距离阈值的地面点云数据。
进一步地，所述处理器602根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息时，具体用于：
将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息；
根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息。
进一步地，所述处理器602将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累计帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息时，具体用于：
将所述第一当前累计帧中的栅格区域投影至所述第一历史累计帧的栅格区域；
确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值；
将所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较；
确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第一误差信息。
进一步地，所述第一当前累计帧是通过获取预设数量帧累计得到的，所述第一历史累计帧是根据历史获取到的预设数量帧累计得到的。
进一步地，所述处理器602根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后，还用于：
获取根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后得到的地面点云数据；
根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息。
进一步地，所述处理器602根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息时，具体用于：
将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息；
根据所述第二误差信息修正所述可移动平台的位置信息和/或姿态信息。
进一步地，所述处理器602将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息时，具体用于：
将所述第二当前累计帧中的栅格区域投影至所述第二历史累计帧的栅格区域；
确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值；
将所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较；
确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第二误差信息。
进一步地，所述第二当前累计帧是通过获取指定时间范围内的帧累计得到的，所述第二历史累计帧是根据历史获取到的指定时间范围内的帧累计得到的。
进一步地，所述位置信息包括高度信息；所述姿态信息包括横滚角和/或俯仰角。
进一步地，所述处理器602根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息之后，还用于：
根据确定的第二误差信息修正所述可移动平台的高度信息、横滚角、俯仰角中的任意一种或多种，以使修正后得到的误差信息小于预设阈值。
进一步地，所述处理器602获取可移动平台当前所处周围环境对应的三维点云数据时，具体用于：
通过激光雷达获取所述可移动平台当前所处周围环境对应的三维点云数据。
本发明实施例中，状态估计系统通过获取所述可移动平台当前所处周围环境对应的三维点云数据，从所述三维点云数据中确定属于同一平面的地面点云数据，并根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息，实现了通过对可移动平台的位置信息和/或姿态信息进行修正提高对可移动平台状态估计的稳定性和精准性。
本发明实施例还提供了一种可移动平台，所述可移动平台包括：机身；配置在机身上的动力系统，用于为可移动平台提供移动的动力；处理器，用于获取可移动平台当前所处周围环境对应的三维点云数据；从所述三维点云数据中确定属于同一平面的地面点云数据；根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。
进一步地，所述处理器从所述三维点云数据中确定属于同一平面的地面点云数据时，具体用于：
根据预设的多个栅格区域对所述三维点云数据进行划分；
根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据。
进一步地，所述处理器根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据时，具体用于：
获取所述每个栅格区域内的三维点云数据的高度；
根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据。
进一步地，所述处理器根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据时，具体用于：
确定所述每个栅格区域内的高度小于预设高度阈值的初始地面点云数据；
对所述初始地面点云数据进行拟合得到拟合平面以及所述拟合平面的法向量；
根据所述法向量确定属于所述拟合平面的地面点云数据。
进一步地，所述处理器根据所述法向量确定属于所述拟合平面的地面点云数据时，具体用于：
根据所述法向量确定每个栅格区域内的三维点云数据与所述拟合平面的距离；
从所述每个栅格区域中确定与所述拟合平面的距离小于预设距离阈值的地面点云数据。
进一步地，所述处理器根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息时，具体用于：
将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息；
根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息。
进一步地，所述处理器将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累计帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息时，具体用于：
将所述第一当前累计帧中的栅格区域投影至所述第一历史累计帧的栅格区域；
确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值；
将所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较；
确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第一误差信息。
进一步地，所述第一当前累计帧是通过获取预设数量帧累计得到的，所述第一历史累计帧是根据历史获取到的预设数量帧累计得到的。
进一步地，所述处理器根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后，还用于：
获取根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后得到的地面点云数据；
根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息。
进一步地，所述处理器根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息时，具体用于：
将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息；
根据所述第二误差信息修正所述可移动平台的位置信息和/或姿态信息。
进一步地，所述处理器将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息时，具体用于：
将所述第二当前累计帧中的栅格区域投影至所述第二历史累计帧的栅格区域；
确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值；
将所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较；
确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第二误差信息。
进一步地，所述第二当前累计帧是通过获取指定时间范围内的帧累计得到的，所述第二历史累计帧是根据历史获取到的指定时间范围内的帧累计得到的。
进一步地，所述位置信息包括高度信息；所述姿态信息包括横滚角和/或俯仰角。
进一步地，所述处理器根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息之后，还用于：
根据确定的第二误差信息修正所述可移动平台的高度信息、横滚角、俯仰角中的任意一种或多种，以使修正后得到的误差信息小于预设阈值。
进一步地，所述处理器获取可移动平台当前所处周围环境对应的三维点云数据时，具体用于：
通过激光雷达获取所述可移动平台当前所处周围环境对应的三维点云数据。
本发明实施例中，状态估计系统通过获取所述可移动平台当前所处周围环境对应的三维点云数据，从所述三维点云数据中确定属于同一平面的地面点云数据，并根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息，实现了通过对可移动平台的位置信息和/或姿态信息进行修正提高对可移动平台状态估计的稳定性和精准性。
本发明的实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现本发明图2或图3所对应实施例中描述的方法，也可实现图6所述本发明所对应实施例的系统，在此不再赘述。
所述计算机可读存储介质可以是前述任一实施例所述的系统的内部存储单元，例如系统的硬盘或内存。所述计算机可读存储介质也可以是所述系统的外部存储设备，例如所述设备上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(Flash Card)等。进一步地，所述计算机可读存储介质还可以既包括所述设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述终端所需的其他程序和数据。所述计算机可读存储介质还可以用于暂时地存储已经输出或者将要输出的数据。
以上所揭露的仅为本发明部分实施例而已，当然不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。

Claims (46)

1.一种可移动平台的状态估计方法，其特征在于，包括：
获取可移动平台当前所处周围环境对应的三维点云数据；
从所述三维点云数据中确定属于同一平面的地面点云数据；
根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。
2.根据权利要求1所述的方法，其特征在于，所述从所述三维点云数据中确定属于同一平面的地面点云数据，包括：
根据预设的多个栅格区域对所述三维点云数据进行划分；
根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据。
3.根据权利要求2所述的方法，其特征在于，所述根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据，包括：
获取所述每个栅格区域内的三维点云数据的高度；
根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据。
4.根据权利要求3所述的方法，其特征在于，所述根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据，包括：
确定所述每个栅格区域内的高度小于预设高度阈值的初始地面点云数据；
对所述初始地面点云数据进行拟合得到拟合平面以及所述拟合平面的法向量；
根据所述法向量确定属于所述拟合平面的地面点云数据。
5.根据权利要求4所述的方法，其特征在于，所述根据所述法向量确定属于所述拟合平面的地面点云数据，包括：
根据所述法向量确定每个栅格区域内的三维点云数据与所述拟合平面的距离；
从所述每个栅格区域中确定与所述拟合平面的距离小于预设距离阈值的地面点云数据。
6.根据权利要求1所述的方法，其特征在于，所述根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息，包括：
将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息；
根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息。
7.根据权利要求6所述的方法，其特征在于，所述将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累计帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息，包括：
将所述第一当前累计帧中的栅格区域投影至所述第一历史累计帧的栅格区域；
确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值；
将所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较；
确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第一误差信息。
8.根据权利要求7所述的方法，其特征在于，
所述第一当前累计帧是通过获取预设数量帧累计得到的，所述第一历史累计帧是根据历史获取到的预设数量帧累计得到的。
9.根据权利要求6所述的方法，其特征在于，所述根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后，还包括：
获取根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后得到的地面点云数据；
根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息。
10.根据权利要求9所述的方法，其特征在于，所述根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息，包括：
将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息；
根据所述第二误差信息修正所述可移动平台的位置信息和/或姿态信息。
11.根据权利要求10所述的方法，其特征在于，所述将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息，包括：
将所述第二当前累计帧中的栅格区域投影至所述第二历史累计帧的栅格区域；
确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值；
将所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较；
确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第二误差信息。
12.根据权利要求11所述的方法，其特征在于，
所述第二当前累计帧是通过获取指定时间范围内的帧累计得到的，所述第二历史累计帧是根据历史获取到的指定时间范围内的帧累计得到的。
13.根据权利要求1所述的方法，其特征在于，所述位置信息包括高度信息；所述姿态信息包括横滚角和/或俯仰角。
14.根据权利要求13所述的方法，其特征在于，所述根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息之后，还包括：
根据确定的第二误差信息修正所述可移动平台的高度信息、横滚角、俯仰角中的任意一种或多种，以使修正后得到的误差信息小于预设阈值。
15.根据权利要求1所述的方法，其特征在于，所述获取可移动平台当前所处周围环境对应的三维点云数据，包括：
通过激光雷达获取所述可移动平台当前所处周围环境对应的三维点云数据。
16.一种状态估计系统，用于可移动平台，其特征在于，包括：
点云传感器，用于获取三维点云数据；
存储器，用于存储程序指令；
处理器，用于调用所述程序指令，当所述程序指令被执行时，用于执行以下操作:
获取可移动平台当前所处周围环境对应的三维点云数据；
从所述三维点云数据中确定属于同一平面的地面点云数据；
根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。
17.根据权利要求16所述的系统，其特征在于，所述处理器从所述三维点云数据中确定属于同一平面的地面点云数据时，具体用于：
根据预设的多个栅格区域对所述三维点云数据进行划分；
根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据。
18.根据权利要求17所述的系统，其特征在于，所述处理器根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据时，具体用于：
获取所述每个栅格区域内的三维点云数据的高度；
根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据。
19.根据权利要求18所述的系统，其特征在于，所述处理器根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据时，具体用于：
确定所述每个栅格区域内的高度小于预设高度阈值的初始地面点云数据；
对所述初始地面点云数据进行拟合得到拟合平面以及所述拟合平面的法向量；
根据所述法向量确定属于所述拟合平面的地面点云数据。
20.根据权利要求19所述的系统，其特征在于，所述处理器根据所述法向量确定属于所述拟合平面的地面点云数据时，具体用于：
根据所述法向量确定每个栅格区域内的三维点云数据与所述拟合平面的距离；
从所述每个栅格区域中确定与所述拟合平面的距离小于预设距离阈值的地面点云数据。
21.根据权利要求16所述的系统，其特征在于，所述处理器根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息时，具体用于：
将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息；
根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息。
22.根据权利要求21所述的系统，其特征在于，所述处理器将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累计帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息时，具体用于：
将所述第一当前累计帧中的栅格区域投影至所述第一历史累计帧的栅格区域；
确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值；
将所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较；
确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第一误差信息。
23.根据权利要求22所述的系统，其特征在于，
所述第一当前累计帧是通过获取预设数量帧累计得到的，所述第一历史累计帧是根据历史获取到的预设数量帧累计得到的。
24.根据权利要求21所述的系统，其特征在于，所述处理器根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后，还用于：
获取根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后得到的地面点云数据；
根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息。
25.根据权利要求24所述的系统，其特征在于，所述处理器根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息时，具体用于：
将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息；
根据所述第二误差信息修正所述可移动平台的位置信息和/或姿态信息。
26.根据权利要求25所述的系统，其特征在于，所述处理器将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息时，具体用于：
将所述第二当前累计帧中的栅格区域投影至所述第二历史累计帧的栅格区域；
确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值；
将所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较；
确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第二误差信息。
27.根据权利要求26所述的系统，其特征在于，
所述第二当前累计帧是通过获取指定时间范围内的帧累计得到的，所述第二历史累计帧是根据历史获取到的指定时间范围内的帧累计得到的。
28.根据权利要求16所述的系统，其特征在于，所述位置信息包括高度信息；所述姿态信息包括横滚角和/或俯仰角。
29.根据权利要求28所述的系统，其特征在于，所述处理器根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息之后，还用于：
根据确定的第二误差信息修正所述可移动平台的高度信息、横滚角、俯仰角中的任意一种或多种，以使修正后得到的误差信息小于预设阈值。
30.根据权利要求16所述的系统，其特征在于，所述处理器获取可移动平台当前所处周围环境对应的三维点云数据时，具体用于：
通过激光雷达获取所述可移动平台当前所处周围环境对应的三维点云数据。
31.一种可移动平台，其特征在于，包括：
机身；
配置在机身上的动力系统，用于为可移动平台提供移动的动力；
处理器，用于执行：
获取可移动平台当前所处周围环境对应的三维点云数据；
从所述三维点云数据中确定属于同一平面的地面点云数据；
根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息。
32.根据权利要求31所述的可移动平台，其特征在于，所述处理器从所述三维点云数据中确定属于同一平面的地面点云数据时，具体用于：
根据预设的多个栅格区域对所述三维点云数据进行划分；
根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据。
33.根据权利要求32所述的可移动平台，其特征在于，所述处理器根据每个栅格区域内的三维点云数据，确定所述属于同一平面的地面点云数据时，具体用于：
获取所述每个栅格区域内的三维点云数据的高度；
根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据。
34.根据权利要求33所述的可移动平台，其特征在于，所述处理器根据所述每个栅格区域内的三维点云数据的高度，确定所述属于同一平面的地面点云数据时，具体用于：
确定所述每个栅格区域内的高度小于预设高度阈值的初始地面点云数据；
对所述初始地面点云数据进行拟合得到拟合平面以及所述拟合平面的法向量；
根据所述法向量确定属于所述拟合平面的地面点云数据。
35.根据权利要求34所述的可移动平台，其特征在于，所述处理器根据所述法向量确定属于所述拟合平面的地面点云数据时，具体用于：
根据所述法向量确定每个栅格区域内的三维点云数据与所述拟合平面的距离；
从所述每个栅格区域中确定与所述拟合平面的距离小于预设距离阈值的地面点云数据。
36.根据权利要求31所述的可移动平台，其特征在于，所述处理器根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息时，具体用于：
将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息；
根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息。
37.根据权利要求36所述的可移动平台，其特征在于，所述处理器将第一当前累计帧的每个栅格区域内的地面点云数据与第一历史累计帧对应的每个栅格区域内的地面点云数据进行匹配，确定第一当前累计帧的地面点云数据与第一历史累计帧的地面点云数据的第一误差信息时，具体用于：
将所述第一当前累计帧中的栅格区域投影至所述第一历史累计帧的栅格区域；
确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值；
将所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较；
确定所述第一当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第一历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第一误差信息。
38.根据权利要求37所述的可移动平台，其特征在于，
所述第一当前累计帧是通过获取预设数量帧累计得到的，所述第一历史累计帧是根据历史获取到的预设数量帧累计得到的。
39.根据权利要求36所述的可移动平台，其特征在于，所述处理器根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后，还用于：
获取根据所述第一误差信息修正所述可移动平台的位置信息和/或姿态信息之后得到的地面点云数据；
根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息。
40.根据权利要求39所述的可移动平台，其特征在于，所述处理器根据修正后得到的地面点云数据，修正所述可移动平台的位置信息和/或姿态信息时，具体用于：
将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息；
根据所述第二误差信息修正所述可移动平台的位置信息和/或姿态信息。
41.根据权利要求40所述的可移动平台，其特征在于，所述处理器将修正后获取到的第二当前累计帧的每个栅格区域内的地面点云数据与第二历史累积帧对应的每个栅格区域内的地面点云数据进行匹配，确定第二当前累计帧的地面点云数据与第二历史累计帧的地面点云数据的第二误差信息时，具体用于：
将所述第二当前累计帧中的栅格区域投影至所述第二历史累计帧的栅格区域；
确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值；
将所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值进行比较；
确定所述第二当前累计帧中每个栅格区域内的地面点云数据的高度均值与所述第二历史累计帧对应每个栅格区域内的地面点云数据的高度均值之间的第二误差信息。
42.根据权利要求41所述的可移动平台，其特征在于，
所述第二当前累计帧是通过获取指定时间范围内的帧累计得到的，所述第二历史累计帧是根据历史获取到的指定时间范围内的帧累计得到的。
43.根据权利要求31所述的可移动平台，其特征在于，所述位置信息包括高度信息；所述姿态信息包括横滚角和/或俯仰角。
44.根据权利要求43所述的可移动平台，其特征在于，所述处理器根据所述地面点云数据修正所述可移动平台的位置信息和/或姿态信息之后，还用于：
根据确定的第二误差信息修正所述可移动平台的高度信息、横滚角、俯仰角中的任意一种或多种，以使修正后得到的误差信息小于预设阈值。
45.根据权利要求31所述的可移动平台，其特征在于，所述处理器获取可移动平台当前所处周围环境对应的三维点云数据时，具体用于：
通过激光雷达获取所述可移动平台当前所处周围环境对应的三维点云数据。
46.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至15任一项所述方法。
CN201980008419.2A 2019-06-28 可移动平台状态估计方法、系统、可移动平台及存储介质 Active CN111684382B (zh)

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/CN2019/093580 WO2020258217A1 (zh) 2019-06-28 2019-06-28 可移动平台状态估计方法、系统、可移动平台及存储介质

Publications (2)

Publication Number Publication Date
CN111684382A true CN111684382A (zh) 2020-09-18
CN111684382B CN111684382B (zh) 2024-06-11

Family

ID=

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113253294A (zh) * 2021-06-16 2021-08-13 北京海天瑞声科技股份有限公司 关于3d雷达点云数据中地面点检测的方法、装置及介质
CN113848944A (zh) * 2021-10-18 2021-12-28 云鲸智能科技（东莞）有限公司 一种地图构建方法、装置、机器人及存储介质
WO2023087202A1 (zh) * 2021-11-18 2023-05-25 华为技术有限公司 运动状态估计方法及装置

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110052043A1 (en) * 2009-08-25 2011-03-03 Samsung Electronics Co., Ltd. Method of mobile platform detecting and tracking dynamic objects and computer-readable medium thereof
US20120209514A1 (en) * 2011-02-14 2012-08-16 Microsoft Corporation Change invariant scene recognition by an agent
CN105045263A (zh) * 2015-07-06 2015-11-11 杭州南江机器人股份有限公司 一种基于Kinect的机器人自定位方法
CN106650640A (zh) * 2016-12-05 2017-05-10 浙江大学 一种基于激光雷达点云局部结构特征的负障碍物检测方法
CN107179768A (zh) * 2017-05-15 2017-09-19 上海木爷机器人技术有限公司 一种障碍物识别方法及装置
CN107796397A (zh) * 2017-09-14 2018-03-13 杭州迦智科技有限公司 一种机器人双目视觉定位方法、装置和存储介质
CN108279670A (zh) * 2017-12-29 2018-07-13 百度在线网络技术（北京）有限公司 用于调整点云数据采集轨迹的方法、设备以及计算机可读介质
CN109214248A (zh) * 2017-07-04 2019-01-15 百度在线网络技术（北京）有限公司 用于识别无人驾驶车辆的激光点云数据的方法和装置

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110052043A1 (en) * 2009-08-25 2011-03-03 Samsung Electronics Co., Ltd. Method of mobile platform detecting and tracking dynamic objects and computer-readable medium thereof
US20120209514A1 (en) * 2011-02-14 2012-08-16 Microsoft Corporation Change invariant scene recognition by an agent
CN105045263A (zh) * 2015-07-06 2015-11-11 杭州南江机器人股份有限公司 一种基于Kinect的机器人自定位方法
CN106650640A (zh) * 2016-12-05 2017-05-10 浙江大学 一种基于激光雷达点云局部结构特征的负障碍物检测方法
CN107179768A (zh) * 2017-05-15 2017-09-19 上海木爷机器人技术有限公司 一种障碍物识别方法及装置
CN109214248A (zh) * 2017-07-04 2019-01-15 百度在线网络技术（北京）有限公司 用于识别无人驾驶车辆的激光点云数据的方法和装置
CN107796397A (zh) * 2017-09-14 2018-03-13 杭州迦智科技有限公司 一种机器人双目视觉定位方法、装置和存储介质
CN108279670A (zh) * 2017-12-29 2018-07-13 百度在线网络技术（北京）有限公司 用于调整点云数据采集轨迹的方法、设备以及计算机可读介质

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113253294A (zh) * 2021-06-16 2021-08-13 北京海天瑞声科技股份有限公司 关于3d雷达点云数据中地面点检测的方法、装置及介质
CN113848944A (zh) * 2021-10-18 2021-12-28 云鲸智能科技（东莞）有限公司 一种地图构建方法、装置、机器人及存储介质
WO2023087202A1 (zh) * 2021-11-18 2023-05-25 华为技术有限公司 运动状态估计方法及装置

Also Published As

Publication number Publication date
WO2020258217A1 (zh) 2020-12-30

Similar Documents

Publication Publication Date Title
US20210124029A1 (en) Calibration of laser and vision sensors
AU2018282302B2 (en) Integrated sensor calibration in natural scenes
CN112106111A (zh) 一种标定方法、设备、可移动平台及存储介质
US20160063717A1 (en) Point cloud position data processing device, point cloud position data processing system, point cloud position data processing method, and program therefor
CN112819896B (zh) 传感器的标定方法及装置、存储介质和标定系统
CN109446886B (zh) 基于无人车的障碍物检测方法、装置、设备以及存储介质
WO2020215172A1 (zh) 一种障碍物检测方法、设备、可移动平台及存储介质
US10104359B2 (en) Disparity value deriving device, movable apparatus, robot, disparity value producing method, and computer program
CN113748357A (zh) 激光雷达的姿态校正方法、装置和系统
US11233983B2 (en) Camera-parameter-set calculation apparatus, camera-parameter-set calculation method, and recording medium
CN112816949B (zh) 传感器的标定方法及装置、存储介质、标定系统
Nienaber et al. A comparison of low-cost monocular vision techniques for pothole distance estimation
CN111080784B (zh) 一种基于地面图像纹理的地面三维重建方法和装置
US20180266830A1 (en) Information processing device, information processing method, and program
EP2913999A1 (en) Disparity value deriving device, equipment control system, movable apparatus, robot, disparity value deriving method, and computer-readable storage medium
WO2020054408A1 (ja) 制御装置、情報処理方法、およびプログラム
WO2020258217A1 (zh) 可移动平台状态估计方法、系统、可移动平台及存储介质
CN111684382B (zh) 可移动平台状态估计方法、系统、可移动平台及存储介质
CN111656404A (zh) 图像处理方法、系统及可移动平台
US20210149412A1 (en) Position estimating apparatus, method for determining position of movable apparatus, and non-transitory computer readable medium
US20220018658A1 (en) Measuring system, measuring method, and measuring program
JP7300331B2 (ja) 機械学習用情報処理装置、機械学習用情報処理方法、および機械学習用情報処理プログラム
CN114791610A (zh) 一种转换参数的确定方法和装置
CN112272757A (zh) 一种探测装置的外参数标定方法、装置及可移动平台
EP4345750A1 (en) Position estimation system, position estimation method, and program

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
TA01 Transfer of patent application right

Effective date of registration: 20240515

Address after: Building 3, Xunmei Science and Technology Plaza, No. 8 Keyuan Road, Science and Technology Park Community, Yuehai Street, Nanshan District, Shenzhen City, Guangdong Province, 518057, 1634

Applicant after: Shenzhen Zhuoyu Technology Co.,Ltd.

Country or region after: China

Address before: 518057 Shenzhen Nanshan High-tech Zone, Shenzhen, Guangdong Province, 6/F, Shenzhen Industry, Education and Research Building, Hong Kong University of Science and Technology, No. 9 Yuexingdao, South District, Nanshan District, Shenzhen City, Guangdong Province

Applicant before: SZ DJI TECHNOLOGY Co.,Ltd.

Country or region before: China

GR01 Patent grant