CN114415648A - 确定地面平面的方法、装置及设备 - Google Patents
确定地面平面的方法、装置及设备 Download PDFInfo
- Publication number
- CN114415648A CN114415648A CN202011080514.4A CN202011080514A CN114415648A CN 114415648 A CN114415648 A CN 114415648A CN 202011080514 A CN202011080514 A CN 202011080514A CN 114415648 A CN114415648 A CN 114415648A
- Authority
- CN
- China
- Prior art keywords
- ground plane
- point
- points
- position point
- determining
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 91
- 230000008569 process Effects 0.000 claims abstract description 35
- 238000001514 detection method Methods 0.000 claims description 82
- 238000012545 processing Methods 0.000 claims description 20
- 238000006243 chemical reaction Methods 0.000 claims description 19
- 239000011159 matrix material Substances 0.000 description 17
- 238000004364 calculation method Methods 0.000 description 12
- 238000010586 diagram Methods 0.000 description 11
- 230000009466 transformation Effects 0.000 description 9
- 230000006870 function Effects 0.000 description 7
- 230000015654 memory Effects 0.000 description 7
- 238000005457 optimization Methods 0.000 description 6
- 238000013507 mapping Methods 0.000 description 4
- 238000005259 measurement Methods 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000001174 ascending effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
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
-
- 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
-
- 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Traffic Control Systems (AREA)
Abstract
本申请公开了一种确定地面平面的方法、装置及设备,属于智能控制技术领域。所述方法包括:获取移动设备在移动过程中多个位置点对应的第一局部地图;确定所述移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面;基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。采用本申请可以根据移动设备所处的地面平面确定移动设备所处的平层,能够避免因位姿数据的错误将移动设备确定在错误的平层中。
Description
技术领域
本申请涉及智能控制技术领域,特别涉及一种确定地面平面的方法、装置及设备。
背景技术
随着智能控制技术的发展,移动设备(如机器人)可以在一些场景中进行移动,然后根据位姿数据完成对应的一些任务。例如机器人可以根据位姿数据确定机器人在场景中的位置和姿态,从而在移动的过程中可以完成避障任务。另外,机器人上还可以设置有激光雷达,可以根据机器人在场景进行移动时的位姿数据以及激光雷达扫描的点云数据,建立场景对应的全局地图(3D地图)。
在相关技术中,机器人上可以设置有激光雷达、IMU(Inertial MeasurementUnit,惯性测量单元)、轮速传感器等装置。可以通激光里程计对IMU、轮速等高频传感器获取的数据以及激光雷达扫描的点云数据进行计算得到机器人对应的位姿数据。
在实现本申请的过程中,发明人发现相关技术至少存在以下问题:
由于现有激光雷达扫描的点云数据以及激光里程计算法存在一定的误差,会使计算得到的机器人移动过程中的位姿数据在Z轴上存在向上飘移的问题,从而导致在一些复杂的多平层场景中,根据计算的位姿数据很可能将机器人当前的位置确定在错误的平层中,例如机器人实际在负二层,而由于机器人的位姿数据存在误差,很有可能将机器人的位置确定在负一层。
发明内容
本申请实施例提供了一种确定地面平面的方法、装置及设备,能够避免因位姿数据的错误将移动设备确定在错误的平层中。所述技术方案如下:
一方面,提供了一种确定地面平面的方法,所述方法包括:
获取移动设备在移动过程中多个位置点对应的第一局部地图;
确定所述移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面;
基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。
可选的,所述方法还包括:
获取所述移动设备在移动过程中多个位置点对应的第一位姿数据;
所述基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面之后,所述方法还包括:
基于所述多个位置点对应的第二地面平面对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据;
基于多个位置点对应的第一局部地图和所述多个位置点的第二位姿数据,确定全局地图。
可选的,所述基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面,包括:
将检测时序上第一个位置点设置为起始位置点,由所述起始位置点开始按检测时序逐个获取位置点,每获取一个位置点,确定当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度,如果确定出的接近度大于接近度阈值时,将当前获取的位置点确定为结束位置点,如果当前获取的位置点是检测时序上最后一个位置点,则将所述当前获取的位置点确定为结束位置点;
在确定出结束位置点之后,将由所述起始位置点至所述结束位置点的全部位置点确定为对应同一物理平面的位置点,将所述结束位置在检测时序上的下一个位置点确定为起始位置点,转至执行所述由所述起始位置点开始按检测时序逐个获取位置点;
将所述对应同一物理平面的多个位置点对应的第一地面平面,调整为相同的地面平面,得到多个位置点的第二地面平面。
可选的,所述确定当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度,包括:
将所述下一个位置点的第一地面平面转换至当前获取的位置点对应的最坐标系中,得到下一个位置点转换之后的第一地面平面;
将所述下一个位置点转换之后的第一地面平面的法向量与当前获取的位置点对应的第一地面平面的法向量的夹角,以及所述下一个位置点转换之后的第一地面平面距与当前获取的位置点的距离与当前获取的位置点对应的第一地面平面距当前获取的位置点的距离的差值确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度;或者,
将所述下一个位置点转换之后的第一地面平面中距所述当前获取的位置点最近的坐标点,与当前获取的位置点对应的第一地面平面距当前获取的位置点最近的坐标点的距离确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度。
可选的,所述基于所述多个位置点对应的第二地面平面对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据,包括:
以所述多个位置点对应的第二地面平面、第一位姿数据为约束条件,基于最小二乘法对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据。
可选的,所述基于多个位置点对应的第一局部地图和所述多个位置点的第二位姿数据,确定全局地图,包括:
基于所述多个位置点对应的第二位姿数据,对多个位置点对应的第一局部地图进行调整处理,得到多个位置点对应的第二局部地图;
对所述多个位置点对应的第二局部地图进行回环检测,得到回环检测结果,其中,所述回环检测结果中包括满足回环检测条件的第二局部地图对所对应的位置点对;
以所述多个位置点对应的第二地面平面、第二位姿数据以及所述回环检测结果中每个位置点对对应的第二位姿数据对为约束条件,基于最小二乘法对所述多个位置点对应的第二位姿数据进行调整处理,得到多个位置点对应的第三位姿数据;
基于所述多个位置点对应的第三位姿数据,对所述多个位置点对应的第二局部地图进行调整处理,得到多个位置点对应的第三局部地图;
基于所述多个位置点对应的第三局部地图,确定全局地图。
另一方面,提供了一种确定地面平面的装置,所述装置包括:
获取模块,用于获取移动设备在移动过程中多个位置点对应的第一局部地图;
确定模块,用于确定所述移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面;
调整模块,用于基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。
可选的,所述获取模块,还用于获取所述移动设备在移动过程中多个位置点对应的第一位姿数据;
所述调整模块,还用于基于所述多个位置点对应的第二地面平面对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据;
所述确定模块,还用于基于多个位置点对应的第一局部地图和所述多个位置点的第二位姿数据,确定全局地图。
可选的,所述调整模块,用于:
将检测时序上第一个位置点设置为起始位置点,由所述起始位置点开始按检测时序逐个获取位置点,每获取一个位置点,确定当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度,如果确定出的接近度大于接近度阈值时,将当前获取的位置点确定为结束位置点,如果当前获取的位置点是检测时序上最后一个位置点,则将所述当前获取的位置点确定为结束位置点;
在确定出结束位置点之后,将由所述起始位置点至所述结束位置点的全部位置点确定为对应同一物理平面的位置点,将所述结束位置在检测时序上的下一个位置点确定为起始位置点,转至执行所述由所述起始位置点开始按检测时序逐个获取位置点;
将所述对应同一物理平面的多个位置点对应的第一地面平面,调整为相同的地面平面,得到多个位置点的第二地面平面。
可选的,所述调整模块,用于:
将所述下一个位置点的第一地面平面转换至当前获取的位置点对应的最坐标系中,得到下一个位置点转换之后的第一地面平面;
将所述下一个位置点转换之后的第一地面平面的法向量与当前获取的位置点对应的第一地面平面的法向量的夹角,以及所述下一个位置点转换之后的第一地面平面距当前获取的位置点的距离与当前获取的位置点对应的第一地面平面距当前获取的位置点的距离的差值确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度;或者,
将所述下一个位置点转换之后的第一地面平面中距所述当前获取的位置点最近的坐标点,与当前获取的位置点对应的第一地面平面距当前获取的位置点最近的坐标点的距离确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度。
以所述多个位置点对应的第二地面平面、第一位姿数据为约束条件,基于最小二乘法对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据。
可选的,所述确定模块,用于:
基于所述多个位置点对应的第二位姿数据,对多个位置点对应的第一局部地图进行调整处理,得到多个位置点对应的第二局部地图;
对所述多个位置点对应的第二局部地图进行回环检测,得到回环检测结果,其中,所述回环检测结果中包括满足回环检测条件的第二局部地图对所对应的位置点对;
以所述多个位置点对应的第二地面平面、第二位姿数据以及所述回环检测结果中每个位置点对对应的第二位姿数据对为约束条件,基于最小二乘法对所述多个位置点对应的第二位姿数据进行调整处理,得到多个位置点对应的第三位姿数据;
基于所述多个位置点对应的第三位姿数据,对所述多个位置点对应的第二局部地图进行调整处理,得到多个位置点对应的第三局部地图;
基于所述多个位置点对应的第三局部地图,确定全局地图。
再一方面,提供了一种移动设备,所述移动设备包括行进部件、激光雷达、处理器,其中:
所述行进部件,用于在所述处理器的控制下带动所述移动设备进行移动;
所述激光雷达,用于在所述移动设备的移动过程中扫描周围环境的点云数据;
所述处理器,用于根据所述周围环境的点云数据,确定所述移动设备在移动过程中多个位置点对应的第一局部地图;确定所述移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面;基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。
再一方面,提供了一种计算机可读存储介质,所述存储介质中存储有至少一条指令,所述至少一条指令由处理器加载并执行以实现如上所述的确定地面平面的所执行的操作。
本申请实施例提供的技术方案带来的有益效果是:
通过获取移动设备在多个位置点对应的局部地图,然后确定局部地图中第一地面平面,根据多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整,得到每个位置点的第二地面平面。这样经过调整得到的第二地面平面为移动设备在每个位置点对应的较为准确的地面平面,然后可以根据每个移动设备在各位置点对应的第二地面平面确定移动设备所在的平层,可见采用本申请,能够避免因位姿数据的错误将移动设备确定在错误的平层中。
附图说明
为了更清楚地说明本申请实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的一种实施环境的示意图;
图2是本申请实施例提供的一种确定地面平面的方法流程图;
图3是本申请实施例提供的一种确定地面平面的方法示意图;
图4是本申请实施例提供的一种确定地面平面的方法示意图;
图5是本申请实施例提供的一种确定地面平面的方法示意图;
图6是本申请实施例提供的一种确定地面平面的方法示意图;
图7是本申请实施例提供的一种确定地面平面的方法流程图;
图8是本申请实施例提供的一种确定地面平面的方法示意图;
图9是本申请实施例提供的一种确定地面平面的装置结构示意图;
图10是本申请实施例提供的一种移动设备结构示意图;
图11是本申请实施例提供的一种服务器结构示意图。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚,下面将结合附图对本申请实施方式作进一步地详细描述。
本申请实施例提供的确定地面平面的方法可以终端单独实现,或由终端和服务器共同实现。如果本申请实施例由终端单独实现,则终端可以具备处理器和存储器,存储器能够存储本申请实施例提供的确定地面平面的方法对应的执行程序,处理器可以对对应的执行程序进行执行,以实现本申请实施例提供的确定地面平面的方法。另外,终端还可以具备激光雷达、IMU(Inertial Measurement Unit,惯性测量单元)、摄像头、轮速传感器等部件,以获取实现确定地面平面的方法的处理数据。如果本申请实施例由终端和服务器共同实现,则终端和服务器之间可以建立连接,终端可以通过各个传感器获取实现确定地面平面的方法的处理数据,并将数据传送至服务器,由服务器完成本申请实施例提供的确定地面平面的方法对应的处理,其中,该服务器可以是单独的服务器,也可以是服务器组,服务器组中的不同服务器分别可以确定地面平面的方法对应的不同处理,具体的处理分配情况可以由技术人员根据实际需求任意设置,此处不再赘述。
图1是本申请实施例提供的一种实施环境的示意图。参见图1,装载有激光雷达的终端(即移动设备)如机器人,可以在任一多平层的环境中进行移动,如地下停车场,激光雷达可以按照固定的扫描周期扫描周围环境的点云数据。然后可以根据移动设备在移动过程中扫描周围环境的点云数据确定对应的局部地图,其中,局部地图为部分周围环境对应的3D地图,例如可以将多帧点云数据进行点云配准,得到对应的局部地图。然后根据每个局部地图确定终端在每个位置点时对应的地面平面。这样就可以根据每个位置点对应的地面平面确定每个位置点对应的平层,即终端在各个位置点时所在的层数。
图2为本申请实施例提供的一种确定地面平面的方法流程图。参见图2,本申请实施例包括:
步骤201、获取移动设备在移动过程中多个位置点对应的第一局部地图。
在实施中,装载有激光雷达的机器人(移动终端)可以在任一场景中进行移动,并按照固定的扫描周期扫描周围环境的点云数据。在移动设备还可以具备有IMU(InertialMeasurement Unit,惯性测量单元)、摄像头、轮速传感器等部件。在移动过程中IMU可以得到的机器人在移动过程中不同时刻的角速度和加速度,轮速传感器可以得到各个时刻的轮速。其中,可以将机器人在移动过程中的角速度、加速度以及轮速输入到预设的IMU轮速融合模块中得到机器人在移动过程中各个时刻的初始位姿数据。然后再根据各个时刻的初始位姿数据以及移动过程中的扫描的多帧点云数据,对点云数据进行去畸变处理,然后再根据去畸变之后点云数据和初始位姿数据以及激光里程计算法得到机器人的移动轨迹,即机器人在各个时刻的位姿数据。其中,该位姿数据的精确度高于初始位姿数据。然后再根据各个时刻的初始位姿数据以及移动过程中的扫描的多帧点云数据,对点云数据进行去畸变处理,然后再根据去畸变之后点云数据和初始位姿数据以及激光里程计算法得到机器人的移动轨迹,即机器人在各个时刻的位姿数据。其中,该位姿数据的精确度高于初始位姿数据。
在得到机器人对应的移动轨迹之后,可以根据机器人的移动轨迹确定移动过程中的多个位置点。例如可以按照时间,或移动距离对移动轨迹进行划分,得到多段子移动轨迹。其中多个划分节点对应的位置点即为移动过程中的多个位置点。在本申请中以移动距离对移动轨迹进行划分为例,将对应的划分节点对应的位置点确定为移动过程中的多个位置点,对方案进行详细说明。例如,可以以移动距离为5米对移动轨迹进行划分,则在激光雷达移动轨迹中的第0米的位置、第5米开始位置、第10米的开始位置作为移动轨迹中的多个位置点。然后可以将激光雷达在每个位置点对应的移动轨迹中扫描的多帧点云数据进行点云配准,得到每个子移动轨迹对应的局部地图,即得到每个位置点对应的第一局部地图。其中,第一局部地图为多帧点云数据进行点云配准之后的点云数据。例如激光雷达在第5米开始位置的位置点对应的第一局部地图可以为激光雷达在第5米的开始位置到第9米的结束位置之间扫描的多帧点云数据进行点云配准之后,得到的第一局部地图,该第一局部地图中各点的坐标值可以为以对应位置点为原点的局部坐标系下的坐标值。
步骤202、确定移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面。
在实施中,在获取位置点对应的第一局部地图之后,可以通过现有的地面提取算法,提取第一局部地图中的地面点云数据。然后利用区域增长算法,对提取出的地面点云数据进行聚类,并利用RANSAC算法对提取出的点云进行平面拟合,去除提取出的地面点云数据中不属于地面的点云数据,例如,停车场中的路障、与地面接触的轮胎等。
其中,可以将每个位置点对应的地面点云数据确定为每个位置点对应的第一地面平面,也可以根据地面点云数据中各点的在对应局部地图中的坐标值,确定用于表达局部地图的对应第一地面平面的表达式等。
在另一种可行的方式中,可以在每个地面点云数据中确定与对应位置点距离最近的地面点;对于每个位置点,将包括最近的地面点且与位置点和地面点的连线垂直的平面,确定为对应的地面平面。
在实施中,可以通过第一局部地图对应的地面点云数据中距离对应位置点最近的地面点来表示局部地图的地面所处地面平面,该地面平面即为包括距离对应位置点最近的地面点且与距离对应位置点最近的地面点和位置点连线垂直的平面。如图3所示,由于激光雷达位于地面的上方,所以该距离对应位置点最近的地面点与对应位置点不会重合,所以通过距离对应位置点最近的地面点可以唯一的表示局部地图的地面所处平面,即第一地面平面。
步骤203、基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。
在实施中,在确定每个位置点对应的第一地面平面之后,可以根据每个位置点对应第一地面平面,确定各个位置点对应的第一地面平面是否属于同一个物理平面,即每个位置点对应局部地图是否在同一个地面上。然后将可以将属于同一个物理平面的多个第一地面平面调整为同一个地面平面,得到每个位置点的第二地面平面。例如将多个属于同一个物理平面的位置点A中,是检测时序上第一个点对应的第一地面平面确定为多个位置点A对应的第二地面平面。
可选的,确定各个位置点对应的第一地面平面是否属于同一个物理平面的处理可以如下:
步骤2031、将检测时序上第一个位置点设置为起始位置点,由起始位置点开始按检测时序逐个获取位置点,每获取一个位置点,确定当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度,如果确定出的接近度大于接近度阈值时,将当前获取的位置点确定为结束位置点,如果当前获取的位置点是检测时序上最后一个位置点,则将当前获取的位置点确定为结束位置点。
在实施中,可以按照位置点对应的检测时序,确定各个相邻的位置点对应的第一地面平面是否属于同一物理平面。可以将检测时序上第一个位置点A设置为起始位置点,然后确定起始位置点A对应第一地面平面A与下一个位置点B对应的第一地面平面B是否属于同一物理平面,即确定第一地面平面A第一地面平面B的接近度是否大于接近度阈值,如果大于接近度阈值则确定第一地面平面A和第一地面平面B属于同一物理平面。在确定第一地面平面A和第一地面平面B属于同一物理平面之后,可以确定位置点B对应的第一地面平面B与位置点C对应的第一地面平面C是否属于同一物理平面,其中位置点C为位置点B对应的下一个位置点,然后依次类推,直到确定的位置点N对应的第一地面平面N与位置点M对应的第一地面平面M不属于同一物理平面,则将位置点N确定为结束位置点。另外,如果当前获取的位置点是检测时序上最后一个位置点,则将当前获取的位置点确定为结束位置点。
步骤2032、在确定出结束位置点之后,将由起始位置点至结束位置点的全部位置点确定为对应同一物理平面的位置点,将结束位置点在检测时序上的下一个位置点确定为起始位置点,转至执行由起始位置点开始按检测时序逐个获取位置点。
在实施中,在确定结束位置点之后,可以将结束位置点对应的起始位置点至结束位置点的全部位置点确定为对应同一物理平面的位置点。例如上述步骤2031中位置点A为起始位置点A,位置点N确定为结束位置点,则位置点A至位置点N中所有的位置点(包括位置点A和位置点N)对应的第一地面平面对应同一物理平面。且在多个位置点中确定结束位置点之后,可以将结束位置点在检测时序上的下一个位置点确定为起始位置点,并转至执行步骤2031中设置为起始位置点之后的处理。
步骤2033、将对应同一物理平面的多个位置点对应的第一地面平面,调整为相同的地面平面,得到多个位置点的第二地面平面。
在实施中,对于属于同一物理平面的多个位置点,可以将多个位置点对应的不同的第一地面平面确定为同一地面平面,即第二地面平面。例如,可以将对应同一物理平面的多个位置点中,在检测时序上最靠前的位置点对应的第一地面平面确定为对应同一物理平面的多个位置点第二地面平面。
可选的,在步骤2031中,确定当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度的处理可以有如下两种处理方式:
方式一:将下一个位置点的第一地面平面转换至当前获取的位置点对应的最坐标系中,得到下一个位置点转换之后的第一地面平面。将下一个位置点转换之后的第一地面平面的法向量与当前获取的位置点对应的第一地面平面的法向量的夹角,以及下一个位置点转换之后的第一地面平面的法向量距当前获取的位置点的距离与当前获取的位置点对应的第一地面平面与当前获取的位置点的距离的差值确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度。
在实施中,可以获取第一位置点(当前获取的位置点)对应的第一位姿数据、第一地面平面以及第二位置点(当前获取的位置点的下一个位置点)对应的第一位姿数据、第一地面平面。然后可以根据第一位置点对应的第一位姿数据和第二位置点对应的第一位姿数据,确定对应的坐标系转换矩阵,根据坐标系转换矩阵将第二位置点对应的第一地面平面转换到第一位置点对应的坐标系中,得到转换之后的第一地面平面,如图4所示。然后可以计算第一位置点对应的第一地面平面的第一法向量以及距第一位置点的第一距离,并计算第二位置点对应的转换之后的第一地面平面的第二法向量和距第一位置点的第二距离,可以将第一法向量和第二法向量的夹角以及第一距离和第二距离的差值确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度。其中,如果第一法向量n1和第二法向量n2的夹角角度在技术人员预设角度范围内,且第一距离值d1和第二距离值d2的差值在第二地面平面预设差值范围内,则可以确定第一位置点与第二位置点对应的地面平面为同一个物理平面。
方式二:将下一个位置点的第一地面平面转换至当前获取的位置点对应的最坐标系中,得到下一个位置点转换之后的第一地面平面。将下一个位置点转换之后的第一地面平面中距当前获取的位置点最近的坐标点,与当前获取的位置点对应的第一地面平面距当前获取的位置点最近的坐标点的距离确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度。
在实施中,可以获取第一位置点(当前获取的位置点)对应的第一位姿数据、第一地面平面以及第二位置点(当前获取的位置点的下一个位置点)对应的第一位姿数据、第一地面平面。其中,第一位置点对应的第一地面平面可以是距对应的第一位置点最近的地面点云数据中第一地面点的坐标值,第二位置点对应的第一地面平面可以是距对应的第二位置点最近的地面点云数据中第二地面点的坐标值。然后可以根据第一位置点对应的第一位姿数据和第二位置点对应的第一位姿数据,确定对应的坐标系转换矩阵,根据坐标系转换矩阵将第二地面点的坐标值转换到第一位置点对应的坐标系中,得到转换之后的第二地面点的坐标值,如图5所示。然后得到转换之后的第二地面点的坐标值。然后可以将转换之后的第二地面点和第一地面点的距离确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度。其中,如果该距离在技术人员预设距离范围内,则可以认为第一位置点与第二位置点对应的地面平面为同一个物理平面。
可选的,根据每个位置点对应的第二地面平面,确定移动设备所处的平层位置。
在实施中,在得到每个位置点对应的第二地面平面之后,可以按照对应的每个位置点确定每个位置点所处的平层位置。可以通过各个位置点的检测时序确定每个位置点对应的平层位置。例如将第一个位置点对应的平层确定对第一平层,然后按照检测时序确定第一个位置点之后的位置点与前一个位置点对应的第二地面平面是否相同,若相同则相邻的两个位置点位于同一个平层中。当存在相邻的A位置点与B位置点为不同的物理平面时,则可以确定A位置点与B位置点位于不同平层。例如图6所示,A位置点在第N层,B位置点在第N层与其他平层连接的坡道上。之后可以以B位置点为起始位置点确定在B位置点之后的位置点与前一个位置点对应的第二地面平面是否相同,若相同则相邻的两个位置点都位于坡道上,若存在C位置点与前一个位置点对应的第二地面平面不相同,则可以确定C位置点位于第二平层中。进一步的,当相邻的两个位置点对应的第二地面平面不同时,可以根据两个第二地面平面的夹角确定对应的坡道为上坡道还是下坡道,进而可以确定第一平层与第二平层的位置关系。
本申请实施例通过获取移动设备在多个位置点对应的局部地图,然后确定局部地图中第一地面平面,根据多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整,得到每个位置点的第二地面平面。这样经过调整得到的第二地面平面为移动设备在每个位置点对应的较为准确的地面平面,然后可以根据每个移动设备在各位置点对应的第二地面平面确定移动设备所在的平层,可见采用本申请,能够避免因位姿数据的错误将移动设备确定在错误的平层中。
图7是本申请实施例提供的一种确定地面平面的方法流程图。在上述实施例中,确定位置点对应的第二地面平面,可以根据第二地面平面对各个位置点的位姿数据进行调整,从而根据调整后的位姿数据以及局部地图建立移动设备当前所在环境的全局地图,如图7所示,该实施例包括:
步骤701、获取移动设备在移动过程中多个位置点对应的第一位姿数据和第一局部地图。
在实施中,装载有激光雷达的机器人(终端)可以在需要建图的场景中进行移动,并按照固定的扫描周期扫描建图场景中环境的点云数据。其中,需要建图的场景可以是平层场景,即需要建图的场景只对应有一个物理地面,例如广场、某栋建筑的其中一个楼层等;需要建图的场景可以是多平层场景,例如多层停车场、某栋建筑的多个楼层等。如图1所示,建图场景为多层停车场。载有激光雷达的机器人(终端)可以在需要建图的场景中进行移动,并按照固定的扫描周期扫描建图场景中环境的点云数据。其中,第一位姿数据为机器人在移动过程中在各个时刻的位姿数据。获取第一位姿数据和第一局部地图的处理与上述步骤201中的处理相同,此处不再赘述。
其中需要说明的是,多个位置点对应的第一位姿数据和第一局部地图可以在机器人移动的过程生成并获取,也可以在机器人在建图场景中完成所有环境点云数据的扫描之后,再进行计算生成。
步骤702、确定移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面。
其中,步骤702的处理与上述步骤202中的处理相同,此处不再赘述。
另外,由于第一局部地图对应的地面点云数据中可能存在噪点、误差等,导致确定的距离对应位置点最近的地面点也可能存在一定的误差,所以在本申请实施例中还提供了一种鲁棒地面参数优化算法,以提高确定距离对应位置点最近的地面点的准确度。即在确定距离对应位置点最近的地面点之后,可以对地面点的坐标值进行调整。相应的,可以将确定的地面点的坐标输入至下面的算法中:
其中,Π为确定的距离对应位置点最近的地面点的在局部地图中坐标值,{pi}i=1,..,N为局部地面的地面点云,Π*为优化后的坐标值,即调整之后的地面点的坐标值,上述公式可以通过高斯牛顿算法求解对应的Π*。
步骤703、基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。
其中,步骤703的处理与上述步骤203中的处理相同,此处不再赘述。
步骤704、基于多个位置点对应的第二地面平面对多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据。
在实施中,可以对对应同一个物理平面的位置点的第一位姿数据进行调整,以消除第一位姿数据在Z轴上的误差,例如将对应的同一个物理平面的位置点的第一位姿数据的Z轴值调整为一致,得到多个位置点对应的第二位姿数据。如图8所示,调整之前的第一位姿数据在空间中的位置呈上升的趋势,在调整之后的第二位姿数据可以位于同一水平位置。
可选的,可以以多个位置点对应的第二地面平面、第一位姿数据为约束条件,基于最小二乘法对多个位置点的位姿数据进行调整处理,得到多个位置点对应的第二位姿数据。
在实施中,可以以地面约束和激光里程计约束,构建最小二乘问题对多个位置点的第一位姿数据进行联合优化处理。即将激光里程计算法得到的多个位置点的第一位姿数据以及多个位置点对应的第二地面平面同时作为约束条件,最多个位置点的第一位姿数据进行调整。具体可以将第一位姿数据输入到以下公式(1)-(4)得到调整之后的第二位姿数据。
e(Πj,Ti)=Πj-dmRnm-nm TRTtRnm∈R3 公式(3)
x*=arg min F(x) 公式(4)
其中,F(x)为目标函数,Co对应激光里程计约束,包括由激光里程计算法得到的多个位置点的第一位姿数据;i和j分别表示任意两个相邻的位置点的标识;Ti和Tj分别为位置点i和位置点j对应的第一位姿数据;Tij表示由激光里程计计算出的第j个局部地图对应坐标系到第i个局部地图对应坐标系的相对变换矩阵;eij表示第i个位置点与第j个位置点对应的第一位姿数据的差值与第i个位置点与第j个位置点对应的调整之后的第二位姿数据的差值。
Cp对应地面检测约束,包括由激光里程计算法得到的多个位置点的第一位姿数据以及对应的第二地面平面;Πj为第j个位置点对应的第二地面平面;dm和nm分别为在第i个位置点对应坐标系下观测到位置点j对应的距离坐标系原点距离和法向量;R为第i个位置点对应的位姿数据中的旋转矩阵;t为第i个位置点对应的位姿数据中的平移向量;e(Πj,Ti)表示第i个位置点的第一地面平面与第二地面平面的差值。
Ωα、Ωγ为预设的转换矩阵,可以由技术人员预先设置,x*为目标函数F(x)为最小值时对应的矩阵,即各个位置点对应的调整之后的第二位姿数据组成的矩阵。其中目标函数F(x)可以通过高斯牛顿算法解决对应的优化问题,得到x*。
步骤705、基于多个位置点对应的第一局部地图和多个位置点的第二位姿数据,确定全局地图。
在实施中,在得到多个位置点对应的第二位姿数据之后,可以根据第二位姿数据对对应的第一局部地图进行调整,得到每个位置点对应的调整之后的局部地图,然后可以根据每个调整之后的局部地图对应的位置点的位姿数据,将各个调整之后的局部地图投影到同一个坐标系下,从而得到全局地图。
另外在得到全局地图之前,还可以对调整之后的局部地图进行回环检测,然后将回环检测结果作为约束条件,以及多个位置点对应的第二地面平面、第一位姿数据为约束条件对第二位姿数据进行再次联合优化处理,相应的处理如下:
步骤7051、基于多个位置点对应的第二位姿数据,对多个位置点对应的第一局部地图进行调整处理,得到多个位置点对应的第二局部地图。
在实施中,在得到多个位置点对应的第二位姿数据之后,可以根据第二位姿数据与对应的第一位姿数据,确定局部地图的调整矩阵,然后根据调整矩阵对对应的第一局部地图进行调整,得到调整之后的局部地图,即得到多个位置点对应的第二局部地图。
步骤7052、对多个位置点对应的第二局部地图进行回环检测,得到回环检测结果。
其中,回环检测结果中包括满足回环检测条件的第二局部地图对所对应的位置点对。
在实施中,可以根据位置点在移动轨迹中的顺序对对应的第二局部地图进行回环检测,例如对应的检测顺序为第二局部地图A、第二局部地图B、第二局部地图C...,则可以先在多个局部地图中确定与局部地图A的欧式距离小于预设欧式距离阈值的局部地图,然后以为初值,采用ICP(一种点云配准算法)或NDT(点云配准算法)等对两个欧式距离小于预设欧式距离阈值的局部地图进行点云配准。其中Ti为第二局部地图A对应位置点的位姿,Tj为与第二局部地图A的欧式距离小于预设欧式距离阈值的任一局部地图的位姿。如果配准残差小于预设阈值,则认为成功检测到回环,可以将检测到回环的位置点对进行记录,得到回环检测结果。这样通过对调整之后的第二局部地图进行回环检测,能够减少激光雷达扫描的数据以及激光里程计算法存在的误差对回环检测结果的影响,能够提高回环检测的精度。
步骤7053、以多个位置点对应的第二地面平面、第二位姿数据以及回环检测结果中每个位置点对对应的第二位姿数据对为约束条件,基于最小二乘法对多个位置点对应的第二位姿数据进行调整处理,得到多个位置点对应的第三位姿数据。
在实施中,可以以地面约束和激光里程计约束,构建最小二乘问题对多个位置点的第一位姿数据进行联合优化处理。即将激光里程计算法得到的多个位置点的第一位姿数据以及多个位置点对应的第二地面平面同时作为约束条件,最多个位置点的第一位姿数据进行调整。具体可以通过以下公式(5)-(9)进行调整。
x*=arg min F(x) 公式(9)
其中,F(x)为目标函数,Co对应激光里程计约束,包括由激光里程计算法得到的多个位置点的第一位姿数据;i和j分别表示任意两个相邻的位置点的标识;Ti和Tj分别为位置点i和位置点j对应的第一位姿数据;Tij表示由激光里程计计算出的第j个局部地图对应坐标系到第i个局部地图对应坐标系的相对变换矩阵;eij表示第i个位置点与第j个位置点对应的第一位姿数据的差值与第i个位置点与第j个位置点对应的调整之后的第二位姿数据的差值。
Cl对应的回环检测约束,包括各个回环检测条件的位置点对应的第一位姿数据,k和l分别表示任意两个满足回环检测条件的位置点的标识;Tk和Tl分别为位置点k和位置点l对应的第一位姿数据;Tkl表示由激光里程计算法计算出的第k个局部地图对应坐标系到第l个局部地图对应坐标系的相对变换矩阵;其中ekl表示满足回环检测条件的位置点对(第k个位置点与第l个位置点)对应的第二位姿数据的差值与位置点对对应的调整之后的第二位姿数据的差值。
Cp对应地面检测约束,包括由激光里程计算法得到的多个位置点的第一位姿数据以及对应的第二地面平面;Πj为第j个位置点对应的第二地面平面;dm和nm分别为在第i个位置点对应坐标系下观测到位置点j对应的距离坐标系原点距离和法向量;R为第i个位置点对应的位姿数据中的旋转矩阵;t为第i个位置点对应的位姿数据中的平移向量;e(Πj,Ti)表示第i个位置点的第一地面平面与第二地面平面的差值。
Ωα、Ωβ、Ωγ为预设的转换矩阵,可以由技术人员预先设置,x*为目标函数F(x)为最小值时对应的矩阵,即各个位置点对应的调整之后的第二位姿数据组成的矩阵。其中目标函数F(x)可以通过高斯牛顿算法解决对应的优化问题,得到x*。
步骤7054、基于多个位置点对应的第三位姿数据,对多个位置点对应的第二局部地图进行调整处理,得到多个位置点对应的第三局部地图,基于多个位置点对应的第三局部地图,确定全局地图。
在实施中,在得到多个位置点对应的第三位姿数据之后,可以根据第三位姿数据对对应的第二局部地图进行调整,得到第三局部地图,然后可以根据每个第三局部地图对应的位置点的第三位姿数据,将各个第三局部地图投影到同一个坐标系下,从而得到全局地图。
本申请实施例,通过获取每个位置点对应的局部地图的地面平面,然后可以根据每个位置点对应的地面平面的接近度,确定对每个位置点对应的地面平面进行调整,这样接近度较高的地面平面可以调整为一致,然后可以根据调整之后的地面平面在对对应位置点的位姿数据进行调整,得到调整之后的位姿数据,然后再根据调整之后的位姿数据和各个局部地图确定对应的全局地图。基于上述对地面平面的调整方式,可以使调整之后的地面平面在空间垂直方向上相对稳定,所以通过调整之后的地面平面对位姿数据进行调整,可以使对应同一物理平面的多个位置点的位姿数据在Z轴上的值相对稳定,可以减小位姿数据在Z轴上向上飘移的问题。所以采用本申请还可以提高生成全局地图的精度。
上述所有可选技术方案,可以采用任意结合形成本公开的可选实施例,在此不再一一赘述。
图9是本申请实施例提供的一种确定地面平面的装置,该装置可以是上述实施例中的终端或服务器,该装置包括:
获取模块910,用于获取移动设备在移动过程中多个位置点对应的第一局部地图;
确定模块920,用于确定所述移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面;
调整模块930,用于基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。
可选的,所述获取模块910,用于获取所述移动设备在移动过程中多个位置点对应的第一位姿数据;
所述调整模块930,还用于基于所述多个位置点对应的第二地面平面对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据;
所述确定模块920,还用于基于多个位置点对应的第一局部地图和所述多个位置点的第二位姿数据,确定全局地图。
可选的,所述调整模块930,用于:
将检测时序上第一个位置点设置为起始位置点,由所述起始位置点开始按检测时序逐个获取位置点,每获取一个位置点,确定当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度,如果确定出的接近度大于接近度阈值时,将当前获取的位置点确定为结束位置点,如果当前获取的位置点是检测时序上最后一个位置点,则将所述当前获取的位置点确定为结束位置点;
在确定出结束位置点之后,将由所述起始位置点至所述结束位置点的全部位置点确定为对应同一物理平面的位置点,将所述结束位置在检测时序上的下一个位置点确定为起始位置点,转至执行所述由所述起始位置点开始按检测时序逐个获取位置点;
将所述对应同一物理平面的多个位置点对应的第一地面平面,调整为相同的地面平面,得到多个位置点的第二地面平面。
可选的,所述调整模块930,用于:
将所述下一个位置点的第一地面平面转换至当前获取的位置点对应的最坐标系中,得到下一个位置点转换之后的第一地面平面;
将所述下一个位置点转换之后的第一地面平面的法向量与当前获取的位置点对应的第一地面平面的法向量的夹角,以及所述下一个位置点转换之后的第一地面平面距当前获取的位置点的距离与当前获取的位置点对应的第一地面平面距当前获取的位置点的距离的差值确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度;或者,
将所述下一个位置点转换之后的第一地面平面中距所述当前获取的位置点最近的坐标点,与当前获取的位置点对应的第一地面平面距当前获取的位置点最近的坐标点的距离确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度。
以所述多个位置点对应的第二地面平面、第一位姿数据为约束条件,基于最小二乘法对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据。
可选的,所述确定模块920,用于:
基于所述多个位置点对应的第二位姿数据,对多个位置点对应的第一局部地图进行调整处理,得到多个位置点对应的第二局部地图;
对所述多个位置点对应的第二局部地图进行回环检测,得到回环检测结果,其中,所述回环检测结果中包括满足回环检测条件的第二局部地图对所对应的位置点对;
以所述多个位置点对应的第二地面平面、第二位姿数据以及所述回环检测结果中每个位置点对对应的第二位姿数据对为约束条件,基于最小二乘法对所述多个位置点对应的第二位姿数据进行调整处理,得到多个位置点对应的第三位姿数据;
基于所述多个位置点对应的第三位姿数据,对所述多个位置点对应的第二局部地图进行调整处理,得到多个位置点对应的第三局部地图;
基于所述多个位置点对应的第三局部地图,确定全局地图。
图10示出了本申请一个示例性实施例提供的移动终端1000的结构框图。其中,所述移动设备包括行进部件1010、激光雷达1020、处理器1030,其中:所述行进部件,用于在所述处理器的控制下带动所述移动设备进行移动;所述激光雷达,用于在所述移动设备的移动过程中扫描周围环境的点云数据;所述处理器,用于根据所述周围环境的点云数据,确定所述移动设备在移动过程中多个位置点对应的第一局部地图;确定所述移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面;基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。
图11是本申请实施例提供的一种服务器的结构示意图,该服务器1100可因配置或性能不同而产生比较大的差异,可以包括一个或一个以上处理器(central processingunits,CPU)1110和一个或一个以上的存储器1120,其中,所述存储器1120中存储有至少一条指令,所述至少一条指令由所述处理器1110加载并执行以实现上述各个方法实施例提供的方法。当然,该服务器还可以具有有线或无线网络接口、键盘以及输入输出接口等部件,以便进行输入输出,该服务器还可以包括其他用于实现设备功能的部件,在此不做赘述。
在示例性实施例中,还提供了一种计算机可读存储介质,例如包括指令的存储器,上述指令可由终端中的处理器执行以完成上述实施例中确定地面平面的方法。该计算机可读存储介质可以是非暂态的。例如,所述计算机可读存储介质可以是ROM(Read-OnlyMemory,只读存储器)、RAM(Random Access Memory,随机存取存储器)、CD-ROM、磁带、软盘和光数据存储设备等。
本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成,也可以通过程序来指令相关的硬件完成,所述的程序可以存储于一种计算机可读存储介质中,上述提到的存储介质可以是只读存储器,磁盘或光盘等。
以上所述仅为本申请的较佳实施例,并不用以限制本申请,凡在本申请的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本申请的保护范围之内。
Claims (12)
1.一种确定地面平面的方法,其特征在于,所述方法包括:
获取移动设备在移动过程中多个位置点对应的第一局部地图;
确定所述移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面;
基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。
2.根据权利要求1所述的方法,其特征在于,所述方法还包括:
获取所述移动设备在移动过程中多个位置点对应的第一位姿数据;
所述基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面之后,所述方法还包括:
基于所述多个位置点对应的第二地面平面对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据;
基于多个位置点对应的第一局部地图和所述多个位置点的第二位姿数据,确定全局地图。
3.根据权利要求1所述的方法,其特征在于,所述基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面,包括:
将检测时序上第一个位置点设置为起始位置点,由所述起始位置点开始按检测时序逐个获取位置点,每获取一个位置点,确定当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度,如果确定出的接近度大于接近度阈值时,将当前获取的位置点确定为结束位置点,如果当前获取的位置点是检测时序上最后一个位置点,则将所述当前获取的位置点确定为结束位置点;
在确定出结束位置点之后,将由所述起始位置点至所述结束位置点的全部位置点确定为对应同一物理平面的位置点,将所述结束位置在检测时序上的下一个位置点确定为起始位置点,转至执行所述由所述起始位置点开始按检测时序逐个获取位置点;
将所述对应同一物理平面的多个位置点对应的第一地面平面,调整为相同的地面平面,得到多个位置点的第二地面平面。
4.根据权利要求1所述的方法,其特征在于,所述确定当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度,包括:
将所述下一个位置点的第一地面平面转换至当前获取的位置点对应的最坐标系中,得到下一个位置点转换之后的第一地面平面;
将所述下一个位置点转换之后的第一地面平面的法向量与当前获取的位置点对应的第一地面平面的法向量的夹角,以及所述下一个位置点转换之后的第一地面平面距当前获取的位置点的距离与当前获取的位置点对应的第一地面平面距当前获取的位置点的距离的差值确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度;或者,
将所述下一个位置点转换之后的第一地面平面中距所述当前获取的位置点最近的坐标点,与当前获取的位置点对应的第一地面平面距当前获取的位置点最近的坐标点的距离确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度。
5.根据权利要求2所述的方法,其特征在于,所述基于所述多个位置点对应的第二地面平面对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据,包括:
以所述多个位置点对应的第二地面平面、第一位姿数据为约束条件,基于最小二乘法对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据。
6.根据权利要求2所述的方法,其特征在于,所述基于多个位置点对应的第一局部地图和所述多个位置点的第二位姿数据,确定全局地图,包括:
基于所述多个位置点对应的第二位姿数据,对多个位置点对应的第一局部地图进行调整处理,得到多个位置点对应的第二局部地图;
对所述多个位置点对应的第二局部地图进行回环检测,得到回环检测结果,其中,所述回环检测结果中包括满足回环检测条件的第二局部地图对所对应的位置点对;
以所述多个位置点对应的第二地面平面、第二位姿数据以及所述回环检测结果中每个位置点对对应的第二位姿数据对为约束条件,基于最小二乘法对所述多个位置点对应的第二位姿数据进行调整处理,得到多个位置点对应的第三位姿数据;
基于所述多个位置点对应的第三位姿数据,对所述多个位置点对应的第二局部地图进行调整处理,得到多个位置点对应的第三局部地图;
基于所述多个位置点对应的第三局部地图,确定全局地图。
7.一种确定地面平面的装置,其特征在于,所述装置包括:
获取模块,用于获取移动设备在移动过程中多个位置点对应的第一局部地图;
确定模块,用于确定所述移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面;
调整模块,用于基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。
8.根据权利要求7所述的装置,其特征在于,所述获取模块,还用于获取所述移动设备在移动过程中多个位置点对应的第一位姿数据;
所述调整模块,还用于基于所述多个位置点对应的第二地面平面对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据;
所述确定模块,还用于基于多个位置点对应的第一局部地图和所述多个位置点的第二位姿数据,确定全局地图。
9.根据权利要求7所述的装置,其特征在于,所述调整模块,用于:
将检测时序上第一个位置点设置为起始位置点,由所述起始位置点开始按检测时序逐个获取位置点,每获取一个位置点,确定当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度,如果确定出的接近度大于接近度阈值时,将当前获取的位置点确定为结束位置点,如果当前获取的位置点是检测时序上最后一个位置点,则将所述当前获取的位置点确定为结束位置点;
在确定出结束位置点之后,将由所述起始位置点至所述结束位置点的全部位置点确定为对应同一物理平面的位置点,将所述结束位置在检测时序上的下一个位置点确定为起始位置点,转至执行所述由所述起始位置点开始按检测时序逐个获取位置点;
将所述对应同一物理平面的多个位置点对应的第一地面平面,调整为相同的地面平面,得到多个位置点的第二地面平面。
10.根据权利要求7所述的装置,其特征在于,所述调整模块,用于:
将所述下一个位置点的第一地面平面转换至当前获取的位置点对应的最坐标系中,得到下一个位置点转换之后的第一地面平面;
将所述下一个位置点转换之后的第一地面平面的法向量与当前获取的位置点对应的第一地面平面的法向量的夹角,以及所述下一个位置点转换之后的第一地面平面距当前获取的位置点的距离与当前获取的位置点对应的第一地面平面距当前获取的位置点的距离的差值确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度;或者,
将所述下一个位置点转换之后的第一地面平面中距所述当前获取的位置点最近的坐标点,与当前获取的位置点对应的第一地面平面距当前获取的位置点最近的坐标点的距离确定为当前获取的位置点对应的第一地面平面与下一个位置点的第一地面平面的接近度;
以所述多个位置点对应的第二地面平面、第一位姿数据为约束条件,基于最小二乘法对所述多个位置点对应的第一位姿数据进行调整处理,得到多个位置点对应的第二位姿数据。
11.根据权利要求8所述的装置,其特征在于,所述确定模块,用于:
基于所述多个位置点对应的第二位姿数据,对多个位置点对应的第一局部地图进行调整处理,得到多个位置点对应的第二局部地图;
对所述多个位置点对应的第二局部地图进行回环检测,得到回环检测结果,其中,所述回环检测结果中包括满足回环检测条件的第二局部地图对所对应的位置点对;
以所述多个位置点对应的第二地面平面、第二位姿数据以及所述回环检测结果中每个位置点对对应的第二位姿数据对为约束条件,基于最小二乘法对所述多个位置点对应的第二位姿数据进行调整处理,得到多个位置点对应的第三位姿数据;
基于所述多个位置点对应的第三位姿数据,对所述多个位置点对应的第二局部地图进行调整处理,得到多个位置点对应的第三局部地图;
基于所述多个位置点对应的第三局部地图,确定全局地图。
12.一种移动设备,其特征在于,所述移动设备包括行进部件、激光雷达、处理器,其中:
所述行进部件,用于在所述处理器的控制下带动所述移动设备进行移动;
所述激光雷达,用于在所述移动设备的移动过程中扫描周围环境的点云数据;
所述处理器,用于根据所述周围环境的点云数据,确定所述移动设备在移动过程中多个位置点对应的第一局部地图;确定所述移动设备在每个第一局部地图中所处位置的第一地面平面,得到每个位置点对应的第一地面平面;基于多个位置点的第一地面平面之间的接近度,对多个位置点的第一地面平面进行调整处理,得到多个位置点的第二地面平面。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011080514.4A CN114415648A (zh) | 2020-10-10 | 2020-10-10 | 确定地面平面的方法、装置及设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011080514.4A CN114415648A (zh) | 2020-10-10 | 2020-10-10 | 确定地面平面的方法、装置及设备 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114415648A true CN114415648A (zh) | 2022-04-29 |
Family
ID=81260176
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011080514.4A Pending CN114415648A (zh) | 2020-10-10 | 2020-10-10 | 确定地面平面的方法、装置及设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114415648A (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
CN109238264A (zh) * | 2018-07-06 | 2019-01-18 | 中国农业大学 | 一种家畜位置姿势归一化方法及装置 |
WO2019140745A1 (zh) * | 2018-01-16 | 2019-07-25 | 广东省智能制造研究所 | 机器人定位方法及装置 |
CN111508021A (zh) * | 2020-03-24 | 2020-08-07 | 广州视源电子科技股份有限公司 | 一种位姿确定方法、装置、存储介质及电子设备 |
CN111709997A (zh) * | 2020-06-30 | 2020-09-25 | 华东理工大学 | 一种基于点与平面特征的slam实现方法及*** |
-
2020
- 2020-10-10 CN CN202011080514.4A patent/CN114415648A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2019140745A1 (zh) * | 2018-01-16 | 2019-07-25 | 广东省智能制造研究所 | 机器人定位方法及装置 |
CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
CN109238264A (zh) * | 2018-07-06 | 2019-01-18 | 中国农业大学 | 一种家畜位置姿势归一化方法及装置 |
CN111508021A (zh) * | 2020-03-24 | 2020-08-07 | 广州视源电子科技股份有限公司 | 一种位姿确定方法、装置、存储介质及电子设备 |
CN111709997A (zh) * | 2020-06-30 | 2020-09-25 | 华东理工大学 | 一种基于点与平面特征的slam实现方法及*** |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108550318B (zh) | 一种构建地图的方法及装置 | |
US10437252B1 (en) | High-precision multi-layer visual and semantic map for autonomous driving | |
US10794710B1 (en) | High-precision multi-layer visual and semantic map by autonomous units | |
US10549430B2 (en) | Mapping method, localization method, robot system, and robot | |
US11802769B2 (en) | Lane line positioning method and apparatus, and storage medium thereof | |
JP6760114B2 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
CN112734852B (zh) | 一种机器人建图方法、装置及计算设备 | |
JP5930346B2 (ja) | 自律移動システムおよび管制装置 | |
EP4080248A1 (en) | Method and apparatus for vehicle positioning, controller, smart car and system | |
CN109710724B (zh) | 一种构建点云地图的方法和设备 | |
US20230175862A1 (en) | Distributed Device Mapping | |
CN112346463B (zh) | 一种基于速度采样的无人车路径规划方法 | |
CN111862214B (zh) | 计算机设备定位方法、装置、计算机设备和存储介质 | |
EP3690400B1 (en) | Method and device for ego-vehicle localization to update hd map by using v2x information fusion | |
CN112835064B (zh) | 一种建图定位方法、***、终端及介质 | |
CN112700486B (zh) | 对图像中路面车道线的深度进行估计的方法及装置 | |
US20230071794A1 (en) | Method and system for building lane-level map by using 3D point cloud map | |
CN114549738A (zh) | 无人车室内实时稠密点云重建方法、***、设备及介质 | |
JP2017181476A (ja) | 車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラム | |
CN111510704A (zh) | 校正摄像头错排的方法及利用其的装置 | |
KR20210057402A (ko) | 비주얼 로컬리제이션과 오도메트리를 기반으로 한 경로 추적 방법 및 시스템 | |
CN112304322B (zh) | 一种视觉定位失效后的重启方法及车载终端 | |
CN114415648A (zh) | 确定地面平面的方法、装置及设备 | |
JP7466144B2 (ja) | プログラム、自律移動装置の管理装置、管理方法及び管理システム | |
KR102675138B1 (ko) | 복수개의 라이다들의 캘리브레이션 방법 및 이를 실행하기 위하여 기록매체에 기록된 컴퓨터 프로그램 |
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 |