一种地图构建方法、装置、计算机设备及存储介质
文献发布时间:2023-06-19 13:26:15
技术领域
本申请涉及数据处理领域,尤其涉及一种地图构建方法、装置、计算机设备及存储介质。
背景技术
目前,SLAM框架分为两类,一类是视觉SLAM,一类是激光SLAM,但大多包含有复杂的回环检测,并且很多针对三维数据的算法并不适用于二维激光雷达数据,简言之,就是现有的SLAM框架复杂,对计算资源要求高,因此,怎样降低计算要求,提高定位和地图构建效率是目前需要解决的技术问题。
发明内容
本申请实施例提供了一种地图构建方法、装置、计算机设备及存储介质,提高了定位和地图构建的效率。
本申请实施例一方面公开了一种地图构建方法,该方法包括:
获取移动对象在k时刻的第一位姿信息,所述第一位姿信息是根据所述移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集进行数据配准后得到的;
获取所述移动对象在k时刻的第二位姿信息,所述第二位姿信息是根据所述移动对象的运动传感器在k时刻采集的运动数据确定出的;
根据所述第一位姿信息和所述第二位姿信息确定所述移动对象在k时刻的目标位姿信息;
根据所述移动对象在k时刻的目标位姿信息构建目标地图。
本申请实施例一方面公开了一种地图构建装置,该装置包括:
获取单元,用于获取移动对象在k时刻的第一位姿信息,所述第一位姿信息是根据所述移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集进行数据配准后得到的;
所述获取单元,还用于获取所述移动对象在k时刻的第二位姿信息,所述第二位姿信息是根据所述移动对象的运动传感器在k时刻采集的运动数据确定出的;
确定单元,用于根据所述第一位姿信息和所述第二位姿信息确定所述移动对象在k时刻的目标位姿信息;
处理单元,用于根据所述移动对象在k时刻的目标位姿信息构建目标地图。
本申请实施例一方面公开了一种计算机设备,该计算机设备包括处理器,适于实现一条或多条计算机程序;以及,计算机存储介质,所述计算机存储介质存储有一条或多条计算机程序,所述一条或多条计算机程序适于由所述处理器加载并执行上述的地图构建方法。
本申请实施一方面公开了一种计算机可读存储介质,所述计算机可读存储介质存储有一条或多条计算机程序,所述计算机程序适于由处理器加载并执行上述的地图构建方法。
本申请实施例一方面公开了一种计算机程序产品或计算机程序,该计算机程序产品包括计算机程序,该计算机程序存储在计算机可读存储介质中。服务器的处理器从计算机可读存储介质读取该计算机程序,处理器执行该计算机程序,使得该计算机设备执行上述的地图构建方法。
本申请实施例中,计算机设备获取移动对象在k时刻的第一位姿信息和第二位姿信息,这里的第一位姿信息是根据移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集进行数据配准后得到的,第二位姿信息是根据移动对象的运动传感器在k时刻采集的运动数据确定出的;然后根据第一位姿信息和第二位姿信息确定移动对象在k时刻的目标位姿信息;最后,基于移动对象在k时刻的目标位姿信息构建目标地图。该方法通过数据配准以及简单的融合,提高了定位和地图构建的效率。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本申请实施例公开的一种地图构建系统的架构示意图;
图2是本申请实施例公开的一种SLAM框架示意图;
图3是本申请实施例公开的一种地图构建方法的流程示意图;
图4是本申请实施例公开的另一种地图构建方法的流程示意图;
图5a是本申请实施例公开的一种采样和占栅格地图的示意图;
图5b是本申请实施例公开的另一种采样和占栅格地图的示意图;
图5c是本申请实施例公开的一种占栅格地图的示意图;
图6是本申请实施例公开的又一种地图构建方法的流程框图;
图7是本申请实施例公开的一种地图构建装置的结构示意图;
图8是本申请实施例公开的一种计算机设备的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
实现本申请实施例的地图构建系统可以如图1所示,地图构建系统100可以包括客户端101和计算机设备102,其中,客户端101主要用于接收计算机设备102发送的目标地图,计算机设备主要用于目标地图的构建与移动物体的定位。进一步的,客户端101和计算机设备102可以实现通信连接,对应的连接方式包括有线连接和无线连接,在此不对其进行限定。
根据上述阐述,上述所提及的客户端101可以是智能手机、平板电脑、笔记本电脑、台式计算机、智能音箱、智能手表、智能车载等,但并不局限于此。可选的,本申请实施例中,用于实现客户端101的功能的装置可以是智能手机等设备;也可以是能够支持终端设备实现该功能的装置,例如芯片系统,该装置可以被安装在客户端101中;计算机设备102可以服务器,服务器可以是独立的物理服务器,也可以是多个物理服务器构成的服务器集群或者分布式系统,还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、CDN、以及大数据和人工智能平台等基础云计算服务的云服务器。图1只是示例性地表征地图构建系统的架构,并不对此进行限定。例如,图1中计算机设备102可以部署成区块链网络中的节点,或者计算机设备102接入区块链网络,使得计算机设备102可将内部数据上传至区块链网络进行存储,以防止内部数据被篡改,从而保证数据安全性。
在一种可能的实现方式中,结合客户端101以及计算机设备102对本申请进行阐述,大致可以包括:计算机设备102根据移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集确定移动对象在k时刻的第一位姿信息,同时,根据移动对象的运动传感器在k时刻采集的运动数据确定出第二位姿信息;然后根据第一位姿信息和第二位姿信息确定移动对象在k时刻的目标位姿信息;最后,基于移动对象在k时刻的目标位姿信息构建目标地图。该方法通过数据配准以及简单的融合,提高了定位和地图构建的效率。
在具体的应用场景中,假设地图构建系统以移动机器人为移动对象,可以将其定位与地图构建问题进行简单的阐述,我们先对基于激光雷达的SLAM(simultaneouslocalization and mapping,即时定位与地图构建)问题进行描述。具体参见以下描述:
在未知环境中,即时定位与地图构建问题可以表述为,移动机器人仅依靠自身携带的传感器即可完成自身的定位,同时获得环境信息完成地图的构建。机器人的自定位(位姿)和未知环境的地图构建是相互依赖的,不是单独完成的。移动机器人装载着传感器在未知环境中移动,因为传感器的采样是有时间间隔的,所以为了方便数学描述本方法将连续时间的运动变成了离散时间的运动。移动机器人自身装载的传感器和其运动的关系可以用运动方程来表示,如公式(1):
M
其中,M
移动机器人构建的地图是由不同的路标构成。移动机器人在M
Z
其中,Q
同步定位与地图构建过程可以抽象为两个基本方程,使用不同的传感器两个方程会有不同的参数化形式,其中一种如公式(3)所示:
这两个方程描述了最基本的SLAM问题:已知上一时刻移动机器人的位姿M
在二维空间中,定位与地图构建可以使用一个2×1的平移矩阵和一个旋转角度表示移动机器人的位姿。常用来直接测量机器人的位姿的运动传感器有陀螺仪、加速度计、IMU和码盘等,这些传感器的测量精度虽然没有使用摄像头搭建的新兴里程计高,但是因为成本低而被广泛的使用。陀螺仪、加速度计和码盘混合使用,再加上控制移动机器人的电机运动,一般通过简单的换算就可以得到机器人当前时刻的旋转角度
可以简化为公式(5):
M
其中,
对应的观测方程具体化为公式(6):
可以简化为公式(7):
Z
其中,
综上所述,基于二维激光雷达的SLAM问题用数学可以表述为公式(8):
上述描述了二维激光雷达的SLAM问题,对于很多结构简单的室内环境来说,不需要很复杂的后端优化方法和回环检测算法,而且很多嵌入式平台无法像服务器一样高效运行一些复杂的运算,因此本申请对经典的SLAM进行改进,对应的,新的SLAM框架如图2所示,是本申请实施例公开的一种SLAM框架的示意图,为了增加传感器的可靠性和稳定性,本申请在整个框架中加入运动传感器,将后端优化问题转化为传感器融合问题。根据室内环境的特点,去掉回环检测,降低框架的复杂性。此外本方法将后端优化包括在建图当中,作为建图过程的优化。建图包括后端优化、创建占栅格地图和创建二值地图。前端里程计对于激光雷达来说,就是根据激光雷达采集到的相邻帧数据求解两帧数据之间的变换关系(机器人位姿)。其中,位姿中的旋转角度指的是移动对象当前时刻的朝向和前一时刻朝向之间的角度差;位姿中的平移矩阵指的是移动对象当前时刻和前一时刻之间的移动的距离,以矩阵的形式表示的。
基于上述地图构建系统,本申请实施例公开了一种地图构建方法,请参见图3,为本申请实施例公开的一种地图构建方法的流程示意图,该地图构建方法可以由计算机设备执行,具体可以是服务器执行的。该地图构建方法具体可以包括以下步骤:
S301、获取移动对象在k时刻的第一位姿信息,第一位姿信息是根据移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集进行数据配准后得到的。
其中,移动对象在具体的实现场景中可以是移动机器人,还可以是其他移动设备,在此不对其进行限定。
在一种可能的实现方式中,先获取移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集,然后将第一数据集和第二数据集进行数据配准,得到移动对象在k时刻的第一位姿信息。其中,数据配准是采用对应的数据配准算法实现的,在本申请实施例中,数据配准算法可以采用经典ICP(Iterative ClosestPoint,最近点迭代算法),是最为经典的数据配准算法,当然,还可以采用其他的方式,在此,不对其进行限定。
其中,采用第一数据集和第二数据集进行数据配准得到第一位姿信息,可以将其总结为不断的迭代操作,当迭代满足迭代停止条件,停止迭代,得到第一位姿信息。其中,执行第N-1次迭代操作,N为大于或等于2的整数,第N-1次迭代操作,包括:根据第一数据集和所述第N数据集构建第2N-3点对集合,第2N-3点对集合包括多个点对,各个点对包括第N数据集中的一个数据点以及第一数据集中与该数据点距离最近的数据点,当N为2时,所述第N数据集为所述第二数据集;根据第2N-3点对集合确定移动对象在k时刻的平移矩阵和在k时刻的旋转角度;通过平移矩阵和旋转角度对第一数据集进行转换处理,得到第N+1数据集;根据第N数据集和第N+1数据集构建第2N-2点对集合,第2N-2点对集合包括多个点对,各个点对包括所述第N+1数据集中的一个数据点以及第N数据集中与该数据点距离最近的数据点;根据第2N-2点对确定第N数据集和第N+1数据集之间的误差。
进一步,在得到误差之后,还需要根据误差判断迭代是否停止,具体的,当误差小于预设数值或在执行第N-1次迭代操作后统计的迭代次数达到预设次数时,将平移矩阵和所述旋转角度确定为移动对象在k时刻的第一位姿信息;当误差大于或等于预设数值或迭代次数未达到预设次数时,执行第N次迭代操作,以此类推,直到执行第N+M次迭代操作为止,将执行第N+M次迭代操作获得的平移矩阵和旋转角度,确定为移动对象在k时刻的第一位姿信息,其中,执行第N+M次迭代操作获得的误差小于预设数值或执行N+M次操作后统计出的迭代次数达到预设次数,M为大于或等于0的整数。
例如,执行第一次(即N=2)迭代操作具体可以包括:1、根据第一数据集和第二数据集构建第一点对集合,第一点对集合包括多个点对,每个点对包括第二数据集中的一个数据点以及第一数据集中与该数据点距离最近的数据点;2、根据第一点对集合确定移动对象在k时刻的平移矩阵和在k时刻的旋转角度;3、通过平移矩阵和旋转角度对第一数据集进行转换处理,得到第三数据集;4、根据第二数据集和第三数据集构建第二点对集合,第二点对集合包括多个点对,各个点对包括第三数据集中的一个数据点以及第二数据集中与该数据点距离最近的数据点;5、根据第二点对确定所述第二数据集和所述第三数据集之间的误差。依次类推,就不再一一列举了。
S302、获取移动对象在k时刻的第二位姿信息,第二位姿信息是根据移动对象的运动传感器在k时刻采集的运动数据确定出的。
其中,运动传感器可以是集成在移动对象上的。运动传感器的种类包括多类,具体的,运动传感器可以是独立的传感器,也可以是陀螺仪、加速度计和码盘混合使用的传感器。对于不同的传感器类型,确定第二位姿信息的过程也是不一样的。
在一种可能的实现方式中,若运动传感器为可以直接读取第二位姿信息的第一运动传感器时,则直接读取第一运动传感器的运动数据,该运动数据可以直接作为第二位姿信息。
在一种可能的实现方式中,若运动传感器为不可以直接读取第二位姿信息的第二运动传感器时,则读取第二运动传感器的运动数据,然后对该运动数据进行相应的运算,从而得到移动对象的第二位姿信息。这里的运算可以是简单的数学运算,在此不做具体的限定。
S303、根据第一位姿信息和第二位姿信息确定移动对象在k时刻的目标位姿信息。
在具体的实现方式中,可以采用卡尔曼滤波算法对第一位姿信息和第二位姿信息进行融合处理,得到移动对象在k时刻的目标位姿信息。除了卡尔曼滤波算法,还可以采用其他算法进行融合,如加权平均法、多贝叶斯估计法、D-S证据推理法、产生式规则等等。
在一种可能的实现方式中,若是采用卡尔曼滤波算法对第一位姿信息和第二位姿信息进行融合处理,则具体的融合方式可以包括:获取移动对象在k-1时刻的目标位姿信息;根据第二位姿信息以及移动对象在k-1时刻的目标位姿信息,确定移动对象在k时刻的位姿信息估计值;同时根据卡尔曼滤波器的系统误差确定移动对象在k时刻的协方差矩阵估计值;根据移动对象在k时刻的协方差矩阵估计值确定卡尔曼增益;进一步的,根据第一位姿信息、移动对象在k时刻的位姿信息估计值以及卡尔曼增益确定移动对象在k时刻的目标位姿信息。才用卡尔曼滤波进行融合,可以简单的解释为,用k-1时刻的数据对k时刻的数据进行预测,然后基于卡尔曼增益,对预测值进行更新,从而得到目标值。
S304、根据所述移动对象在k时刻的目标位姿信息构建目标地图。
在一种可能的实现方式中,在确定了移动对象在k时刻的目标位姿信息后,其中,目标位姿信息包括目标平移矩阵和目标旋转角度,根据目标位姿信息进行地图构建的过程包括:根据目标平移矩阵和目标旋转角度生成k时刻的局部占栅格地图,并同时获取k-1时刻的全局占栅格地图;然后根据k时刻的局部占栅格地图和k-1时刻的全局占栅格地图确定k时刻的全局占栅格地图;对k时刻的全局占栅格地图进行二值化处理,得到k时刻所述移动对象的目标地图。
进一步的,对所述k时刻的全局占栅格地图进行二值化处理,具体可以包括:确定各个栅格的状态值,若是各个栅格中存在第一栅格的状态值大于设定阈值,则将第一栅格对应状态值设置成第一二值化数值,若各个栅格中存在第二栅格的状态值小于或者等于设定阈值,则将第二栅格对应的状态值设置第二二值化数值,其中,第一栅格和第二栅格为各个栅格中的任意一个栅格。进一步的,各个栅格的状态值可以分为第一状态值共和第二状态值,在本申请实施例中,第一状态可以指的是自由空间状态(即无障碍状态),第二状态可以指的是有障碍物的状态。在具体实现中,第一二值化数值可以设置成1,第二二值化数值可以设置成0。
本申请实施例中,主要阐述的利用数据配准方法和融合方法定目标位姿信息的过程以及利用目标位姿信息确定目标地图的过程,主要包括根据移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集确定移动对象在k时刻的第一位姿信息,同时,根据移动对象的运动传感器在k时刻采集的运动数据确定出第二位姿信息;然后根据第一位姿信息和第二位姿信息确定移动对象在k时刻的目标位姿信息;最后,基于移动对象在k时刻的目标位姿信息构建目标地图。该方法通过数据配准以及简单的融合,提高了定位和地图构建的效率。
以移动对象为移动机器人为例,对本申请实施例的地图构建方法进行阐述,请参见图4,是本申请实施例公开的另一种地图构建方法的流程示意图,可以包括以下步骤:
S401、获取移动机器人的二维激光雷达k-1时刻的第一数据集和k时刻的第二数据集。
在具体的实现中,移动机器人先获取二维激光雷达k-1时刻的第一数据集和k时刻的第二数据集,其中,第一数据集和第二数据集是根据二维激光雷达读取的点云数据确定的,第一数据集可以用D
其中,N代表一帧激光雷达数据点的个数,
S402、对第一数据集和第二数据集进行数据配准,得到移动机器人的第一位姿。
第一位姿包括第一平移矩阵和第一旋转角度,在本申请中,可以将第一平移矩阵设为T
在获取到第一数据集和第二数据集后,可以对其进行数据配准,得到k时刻二维激光雷达对应第一位姿。通俗的理解,输入为两帧相邻时刻(位置)的二维激光雷达数据第一数据集D
1、根据欧式距离最近标准,从第二数据集D
2、通过第一点对集合中所有点对计算当前时刻的平移矩阵T
假设第二数据集D
并根据第一点对集合D
其中,
对应的,平移矩阵的计算如公式(14):
其中,C
3、使用步骤2得到的平移矩阵T
4、根据第三数据集D
5、根据第二点对集合D
其中,
若是误差值小于设定的阈值,或者迭代次数达到设定次数则终止循环,将得到的平移矩阵和旋转角度作为第一位姿信息包括的第一平移矩阵和第一旋转角度。若是前面两者均未满足,则用第三数据集D
为了确认得到的平移矩阵和旋转矩阵的精准性,将得到的平移矩阵和旋转角度转换成新的点集,并计算新的点击与之前点集的误差,当误差函数值小于设定的阈值,终止循环,确定出第一平移矩阵和第一旋转角度;又或者,在迭代次数达到设定次数时终止循环,确定出第一平移矩阵和第一旋转角度。
S403、获取移动机器人的运动传感器的第二位姿信息。
其中,第二位姿信息中包括第二平移矩阵和第二旋转角度。此处的第二位姿的获取方式在图3示出的步骤S302中进行了详细的阐述,这里就不进行一一赘述了。其中,第二位姿信息包括的第二平移矩阵可以用
S404、将第一位姿信息和第二位姿信息进行融合处理,得到移动机器人k时刻的目标位姿信息。
在一种可能的实现方式中,确定移动机器人k时刻的目标位姿信息,可以包括以下步骤:首先确定出k-1时刻的目标位姿信息M
然后卡尔曼滤波器的系统误差P
根据移动机器人在k时刻的协方差矩阵估计值确定卡尔曼增益,计算公式如公式(20):
根据第一位姿信息、移动机器人在k时刻的位姿信息估计值以及卡尔曼增益确定移动机器人在k时刻的目标位姿信息,如公式(21),其中,Z
可以基于公式(21)和公式(22)不断的进行迭代,使得得到的目标位姿信息更为精准。当迭代停止时,可以根据
S405、根据目标位姿信息构建目标地图。
其中,目标位姿信息包括目标平移矩阵和目标旋转角度。
在具体的地图构建过程中,可以包括以下步骤:
S41、构建k时刻的局部占栅格地图L
利用融合后得到的目标平移矩阵
其中,
根据新的数据集
在占栅格地图中,对于每个网格的状态使用概率来表示,当网格是自由空间时其概率表示为P(o=0),当网格为障碍物时其概率表示为P(o=1),两者的比值表示当前网格的状态,如公式(24):
当二维激光雷达新的数据点(也就是二维激光雷达新的采样点)出现时,二维激光雷达中心点到数据点之间的网格状态需要进行更新,用new来表示这个新得到的测量值,为了和上文保持一致,其取值只有0或者1,对应的网格状态如公式(25),表示在new发生的条件下,o的状态:
根据贝叶斯公式,可得公式(26)和公式(27):
将公式(26)和公式(27)代入公式(25),可得公式(28):
为了方便计算,将公式(28)两边取log对数,可得公式(29):
由公式(29)可知,logPro(o|new)由两部分组成。logpro(o)为新测量值到来之前的网格状态值。
Profree和Proccu表示当前网格被占据的状态,具有确定的值,具体由实际情况设定。上文用数学证明了当一个新的测量值得到来时,对应网格的状态更新实际上就是一个简单的加减法公式,公式(29)可以写成公式(32):
S
其中,S
其中,新的测量值的确定方式可以用下例进行阐述:
已知二维激光雷达返回的数值为距离,以图5a为移动机器人位于前一帧的采样和占栅格地图的示例图,带箭头的直线为采样激光,假设二维激光雷达只有一束光线,二维激光雷达在图5a这个角度时,返回的距离为1米,这个距离刚好对应图5a的虚线方块,则这块的值概率值根据设定置为0.9。在激光雷达到这个虚线方块的路径外的其他点,说明都没有障碍物,对应块的概率值设为0。
再参见图5b,是当前帧的采样和占栅格地图的示意图,带箭头的直线为采样激光,虚线框表示障碍物(Profree=0.9),带箭头直线下的实线框表示移动机器人和障碍物之间的自由空间(Proccu=-0.9)。Profree和Proccu的值是可以自定义设置的,主要用于区分是否有障碍物,能明显区分就可以,可以根据具体情况而设定,在此进行限定。
S42、获取前一时刻的全局占栅格地图G
对应的融合方法如公式(33):
G
其中,L
S43、根据占栅格地图G
在进行二值地图转换时,判断G
上述的整体流程框图可以参见图6,大致可以概括成:分别获取移动机器人k时刻和k-1时刻的激光雷达的数据(D
基于上述的方法实施例,本申请实施例还提供了一种地图构建装置的结构示意图。参见图7,为本申请实施例提供的一种地图构建装置的结构示意图。图7所示的地图构建装置700可运行如下单元:
获取单元701,用于获取移动对象在k时刻的第一位姿信息,所述第一位姿信息是根据所述移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集进行数据配准后得到的;
所述获取单元701,还用于获取所述移动对象在k时刻的第二位姿信息,所述第二位姿信息是根据所述移动对象的运动传感器在k时刻采集的运动数据确定出的;
确定单元702,用于根据所述第一位姿信息和所述第二位姿信息确定所述移动对象在k时刻的目标位姿信息;
处理单元702,用于根据所述移动对象在k时刻的目标位姿信息构建目标地图。
在一种可能的实现方式中,所述获取单元701获取移动对象在k时刻的第一位姿信息,具体用于:
执行第N-1次迭代操作,N为大于或等于2的整数,所述第N-1次迭代操作,包括:根据所述第一数据集和所述第N数据集构建第2N-3点对集合,所述第2N-3点对集合包括多个点对,所述点对包括所述第N数据集中的一个数据点以及所述第一数据集中与所述数据点距离最近的数据点;当N为2时,所述第N数据集为所述第二数据集;根据所述第2N-3点对集合确定所述移动对象在k时刻的平移矩阵和在k时刻的旋转角度;通过所述平移矩阵和所述旋转角度对所述第一数据集进行转换处理,得到第N+1数据集;根据所述第N数据集和所述第N+1数据集构建第2N-2点对集合,所述第2N-2点对集合包括多个点对,所述点对包括所述第N+1数据集中的一个数据点以及所述第N数据集中与所述数据点距离最近的数据点;根据所述第2N-2点对确定所述第N数据集和所述第N+1数据集之间的误差;
当所述误差小于预设数值或在执行第N-1次迭代操作后统计的迭代次数达到预设次数时,将所述平移矩阵和所述旋转角度确定为所述移动对象在k时刻的第一位姿信息;
当所述误差大于或等于预设数值或所述迭代次数未达到预设次数时,执行第N次迭代操作,以此类推,直到执行第N+M次迭代操作为止,将执行第N+M次迭代操作获得的平移矩阵和旋转角度,确定为所述移动对象在k时刻的第一位姿信息;其中,执行所述第N+M次迭代操作获得的误差小于预设数值或执行N+M次操作后统计出的迭代次数达到预设次数,M为大于或等于0的整数。
在一种可能的实现方式中,所述确定单元702根据所述第一位姿信息和所述第二位姿信息确定所述移动对象在k时刻的目标位姿信息,具体用于:
采用卡尔曼滤波算法对所述第一位姿信息和所述第二位姿信息进行融合处理,得到所述移动对象在k时刻的目标位姿信息。
在一种可能的实现方式中,所述处理单元703采用卡尔曼滤波算法对所述第一位姿信息和所述第二位姿信息进行融合处理,得到所述移动对象在k时刻的目标位姿信息,具体用于:
获取所述移动对象在k-1时刻的目标位姿信息;
根据所述第二位姿信息以及所述移动对象在k-1时刻的目标位姿信息,确定所述移动对象在k时刻的位姿信息估计值;
获取所述移动对象在k时刻的协方差矩阵估计值;
根据所述移动对象在k时刻的协方差矩阵估计值确定卡尔曼增益;
根据所述第一位姿信息、所述移动对象在k时刻的位姿信息估计值以及所述卡尔曼增益确定所述移动对象在k时刻的目标位姿信息。
在一种可能的实现方式中,所述目标位姿信息包括目标平移矩阵和目标旋转角度,所述处理单元703根据所述移动对象在k时刻的目标位姿信息构建目标地图,具体用于:
根据所述目标平移矩阵和所述目标旋转角度生成k时刻的局部占栅格地图;
获取k-1时刻的全局占栅格地图;
根据所述k时刻的局部占栅格地图和所述k-1时刻的全局占栅格地图确定k时刻的全局占栅格地图;
对所述k时刻的全局占栅格地图进行二值化处理,得到k时刻所述移动对象的目标地图。
在一种可能的实现方式中,所述处理单元703对所述k时刻的全局占栅格地图进行二值化处理,得到k时刻所述移动对象的目标地图,具体用于:
获取所述k时刻的全局占栅格地图包括的各个栅格的状态值;
对所述各个栅格的状态值进行二值化转换,得到转换后的全局占栅格地图;
将所述转换后的全局占栅格地图确定为目标地图。
在一种可能的实现方式中,所述处理单元703对所述各个栅格的状态值进行二值化转换,具体用于:
若所述各个栅格中存在第一栅格的状态值大于所述设定阈值,则将所述第一栅格对应状态值设置成第一二值化数值;
若所述各个栅格中存在第二栅格的状态值小于或者等于所述设定阈值,则将所述第二栅格对应的状态值设置第二二值化数值。
根据本申请的一个实施例,图3以及图4所示的地图构建方法所涉及各个步骤可以是由图7所示的地图构建装置中的各个单元来执行的。例如,图3所示的地图构建方法中步骤S301和步骤S302可由图7所示的地图构建装置中的获取单元701来执行,步骤S303由图7所示的地图构建装置中的确定单元702来执行,步骤S304可由图7所示的地图构建装置中的处理单元703来执行;再如,图4所示的地图构建方法中的步骤S401和步骤403可由图7所示的地图构建装置中的获取单元701来执行,步骤S402、步骤S404-S405可由图7所示的地图构建装置中的处理单元703来执行。
根据本申请的另一个实施例,图7所示的地图构建装置中的各个单元可以分别或全部合并为一个或若干个另外的单元来构成,或者其中的某个(些)单元还可以再拆分为功能上更小的多个单元来构成,这可以实现同样的操作,而不影响本申请的实施例的技术效果的实现。上述单元是基于逻辑功能划分的,在实际应用中,一个单元的功能也可以由多个单元来实现,或者多个单元的功能由一个单元实现。在本申请的其它实施例中,基于地图构建装置也可以包括其它单元,在实际应用中,这些功能也可以由其它单元协助实现,并且可以由多个单元协作实现。
根据本申请的另一个实施例,可以通过在包括中央处理单元(CPU)、随机存取存储介质(RAM)、只读存储介质(ROM)等处理元件和存储元件的例如计算机的通用计算设备上运行能够执行如图3以及图4所示的相应方法所涉及的各步骤的计算机程序(包括程序代码),来构造如图7中所示的地图构建装置,以及来实现本申请实施例地图构建方法。所述计算机程序可以记载于例如计算机可读存储介质上,并通过计算机可读存储介质装载于上述计算机设备中,并在其中运行。
本申请实施例中,获取单元701获取移动对象在k时刻的第一位姿信息和第二位姿信息,这里的第一位姿信息是根据移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集进行数据配准后得到的,第二位姿信息是根据移动对象的运动传感器在k时刻采集的运动数据确定出的;确定单元702根据第一位姿信息和第二位姿信息确定移动对象在k时刻的目标位姿信息;最后,处理单元703基于移动对象在k时刻的目标位姿信息构建目标地图。该方法通过数据配准以及简单的融合,提高了定位和地图构建的效率。
基于上述方法以及装置实施例,本申请实施例提供了一种计算机设备。参见图8,为本申请实施例提供的一种计算机设备的结构示意图。图8所示的计算机设备800至少包括处理器801、输入接口802、输出接口803、计算机存储介质804以及存储器805。其中,处理器801、输入接口802、输出接口803、计算机存储介质804以及存储器805可通过总线或其他方式连接。
计算机存储介质804可以存储在计算机设备800的存储器805中,所述计算机存储介质804用于存储计算机程序,所述计算机程序包括程序指令,所述处理器801用于执行所述计算机存储介质804存储的程序指令。处理器801(或称CPU(Central Processing Unit,中央处理器))是计算机设备800的计算核心以及控制核心,其适于实现一条或多条指令,具体适于加载并执行一条或多条计算机指令从而实现相应方法流程或相应功能。
本申请实施例还提供了一种计算机可读存储介质(Memory),所述计算机可读存储介质是计算机设备800中的记忆设备,用于存放程序和数据。可以理解的是,此处的计算机可读存储介质既可以包括计算机设备800中的内置存储介质,当然也可以包括计算机设备800支持的扩展存储介质。计算机可读存储介质提供存储空间,该存储空间存储了计算机设备800的操作系统。并且,在该存储空间中还存放了适于被处理器801加载并执行的一条或多条的指令,这些指令可以是一个或一个以上的计算机程序(包括程序代码)。需要说明的是,此处的计算机存储介质可以是高速RAM存储器,也可以是非不稳定的存储器(non-volatile memory),例如至少一个磁盘存储器;可选的还可以是至少一个位于远离前述处理器的计算机存储介质。
在一个实施例中,所述计算机可读存储介质可由处理器801加载并执行计算机存储介质中存放的一条或多条指令,以实现上述有关图3以及图4所示的地图构建下载方法的相应步骤。具体实现中,计算机可读存储介质中的一条或多条指令由处理器801加载并执行如下步骤:
获取移动对象在k时刻的第一位姿信息,所述第一位姿信息是根据所述移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集进行数据配准后得到的;
获取所述移动对象在k时刻的第二位姿信息,所述第二位姿信息是根据所述移动对象的运动传感器在k时刻采集的运动数据确定出的;
根据所述第一位姿信息和所述第二位姿信息确定所述移动对象在k时刻的目标位姿信息;
根据所述移动对象在k时刻的目标位姿信息构建目标地图。
在一种可能的实现方式中,所述处理器801获取移动对象在k时刻的第一位姿信息,具体用于:
执行第N-1次迭代操作,N为大于或等于2的整数,所述第N-1次迭代操作,包括:根据所述第一数据集和所述第N数据集构建第2N-3点对集合,所述第2N-3点对集合包括多个点对,所述点对包括所述第N数据集中的一个数据点以及所述第一数据集中与所述数据点距离最近的数据点;当N为2时,所述第N数据集为所述第二数据集;根据所述第2N-3点对集合确定所述移动对象在k时刻的平移矩阵和在k时刻的旋转角度;通过所述平移矩阵和所述旋转角度对所述第一数据集进行转换处理,得到第N+1数据集;根据所述第N数据集和所述第N+1数据集构建第2N-2点对集合,所述第2N-2点对集合包括多个点对,所述点对包括所述第N+1数据集中的一个数据点以及所述第N数据集中与所述数据点距离最近的数据点;根据所述第2N-2点对确定所述第N数据集和所述第N+1数据集之间的误差;
当所述误差小于预设数值或在执行第N-1次迭代操作后统计的迭代次数达到预设次数时,将所述平移矩阵和所述旋转角度确定为所述移动对象在k时刻的第一位姿信息;
当所述误差大于或等于预设数值或所述迭代次数未达到预设次数时,执行第N次迭代操作,以此类推,直到执行第N+M次迭代操作为止,将执行第N+M次迭代操作获得的平移矩阵和旋转角度,确定为所述移动对象在k时刻的第一位姿信息;其中,执行所述第N+M次迭代操作获得的误差小于预设数值或执行N+M次操作后统计出的迭代次数达到预设次数,M为大于或等于0的整数。
在一种可能的实现方式中,所述处理器801根据所述第一位姿信息和所述第二位姿信息确定所述移动对象在k时刻的目标位姿信息,具体用于:
采用卡尔曼滤波算法对所述第一位姿信息和所述第二位姿信息进行融合处理,得到所述移动对象在k时刻的目标位姿信息。
在一种可能的实现方式中,所述处理器801采用卡尔曼滤波算法对所述第一位姿信息和所述第二位姿信息进行融合处理,得到所述移动对象在k时刻的目标位姿信息,具体用于:
获取所述移动对象在k-1时刻的目标位姿信息;
根据所述第二位姿信息以及所述移动对象在k-1时刻的目标位姿信息,确定所述移动对象在k时刻的位姿信息估计值;
获取所述移动对象在k时刻的协方差矩阵估计值;
根据所述移动对象在k时刻的协方差矩阵估计值确定卡尔曼增益;
根据所述第一位姿信息、所述移动对象在k时刻的位姿信息估计值以及所述卡尔曼增益确定所述移动对象在k时刻的目标位姿信息。
在一种可能的实现方式中,所述目标位姿信息包括目标平移矩阵和目标旋转角度,所述处理器801根据所述移动对象在k时刻的目标位姿信息构建目标地图,具体用于:
根据所述目标平移矩阵和所述目标旋转角度生成k时刻的局部占栅格地图;
获取k-1时刻的全局占栅格地图;
根据所述k时刻的局部占栅格地图和所述k-1时刻的全局占栅格地图确定k时刻的全局占栅格地图;
对所述k时刻的全局占栅格地图进行二值化处理,得到k时刻所述移动对象的目标地图。
在一种可能的实现方式中,所述处理器801对所述k时刻的全局占栅格地图进行二值化处理,得到k时刻所述移动对象的目标地图,具体用于:
获取所述k时刻的全局占栅格地图包括的各个栅格的状态值;
对所述各个栅格的状态值进行二值化转换,得到转换后的全局占栅格地图;
将所述转换后的全局占栅格地图确定为目标地图。
在一种可能的实现方式中,所述处理器801对所述各个栅格的状态值进行二值化转换,具体用于:
若所述各个栅格中存在第一栅格的状态值大于所述设定阈值,则将所述第一栅格对应状态值设置成第一二值化数值;
若所述各个栅格中存在第二栅格的状态值小于或者等于所述设定阈值,则将所述第二栅格对应的状态值设置第二二值化数值。
本申请实例中,计算机设备的处理器801获取移动对象在k时刻的第一位姿信息和第二位姿信息,这里的第一位姿信息是根据移动对象的二维激光雷达传感器在k-1时刻采集的第一数据集以及在k时刻采集的第二数据集进行数据配准后得到的,第二位姿信息是根据移动对象的运动传感器在k时刻采集的运动数据确定出的;然后根据第一位姿信息和第二位姿信息确定移动对象在k时刻的目标位姿信息;最后,基于移动对象在k时刻的目标位姿信息构建目标地图。该方法通过数据配准以及简单的融合,提高了定位和地图构建的效率。
根据本申请的一个方面,本申请实施例还提供了一种计算机产品或计算机程序,该计算机产品或计算机程序包括计算机指令,该计算机指令存储在计算机可读存储介质中。处理器801从计算机可读存储介质中读取该计算机指令,处理器801执行该计算机指令,使得计算机设800执行图3以及图4所示的地图构建方法。
需要强调的是,为进一步保证上述数据的私密和安全性,上述数据还可以存储于一区块链的节点中。其中,本申请所指区块链是分布式数据存储、点对点传输、共识机制、加密算法等计算机技术的新型应用模式。区块链(Block chain),本质上是一个去中心化的数据库,是一串使用密码学方法相关联产生的数据块,每一个数据块中包含了一批次网络交易的信息,用于验证其信息的有效性(防伪)和生成下一个区块。区块链可以包括区块链底层平台、平台产品服务层以及应用服务层等。
需要说明的是,对于前述的各方法实施例,为了简单描述,故将其都表述为一系列的动作组合,但是本领域技术人员应该知悉,本申请并不受所描述的动作顺序的限制,因为依据本申请,某些步骤可能可以采用其他顺序或者同时进行。其次,本领域技术人员也应该知悉,说明书中所描述的实施例均属于优选实施例,所涉及的动作和模块并不一定是本申请所必须的。
在本申请所提供的几个实施例中,应该理解到,所揭露的装置,可通过其它的方式实现。例如,以上所描述的装置实施例仅仅是示意性的,例如上述模块的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个模块或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。
以上所述,仅为本申请的具体实施方式,但本申请的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本申请揭露的技术范围内,可轻易想到变化或替换,都应涵盖在本申请的保护范围之内。因此,本申请的保护范围应以所述权利要求的保护范围为准。
- 地图构建方法、装置和机器人定位方法、装置、计算机设备和存储介质
- 一种地图构建方法、装置、计算机设备及存储介质