CN110763245A - 一种基于流式计算的地图创建方法及其系统 - Google Patents

一种基于流式计算的地图创建方法及其系统 Download PDF

Info

Publication number
CN110763245A
CN110763245A CN201911022206.3A CN201911022206A CN110763245A CN 110763245 A CN110763245 A CN 110763245A CN 201911022206 A CN201911022206 A CN 201911022206A CN 110763245 A CN110763245 A CN 110763245A
Authority
CN
China
Prior art keywords
data
map
mobile robot
observation
pose
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.)
Withdrawn
Application number
CN201911022206.3A
Other languages
English (en)
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.)
Jiangsu Maritime Institute
Original Assignee
Jiangsu Maritime Institute
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 Jiangsu Maritime Institute filed Critical Jiangsu Maritime Institute
Priority to CN201911022206.3A priority Critical patent/CN110763245A/zh
Publication of CN110763245A publication Critical patent/CN110763245A/zh
Priority to PCT/CN2020/096031 priority patent/WO2021077769A1/zh
Withdrawn legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种基于流式计算的地图创建方法及其系统，包括：移动机器人通过数据采集模块定时从激光雷达和惯性传感器读取数据并封装为消息，调用消息客户端将消息提交到流式计算集群；集群执行分布式并行基于的地图创建方法，从队列中取出消息，将数据分发给计算节点更新粒子；分发给下面的计算节点，实现粒子的采样和重采样，最终的定位和地图特征数据发送给移动机器人。本发明将流式计算集群端强大的计算能力和SR‑UKF SLAM算法结合起来实现基于流式计算的分布式并行SLAM算法，将特征地图生成过程在流式计算框架进行实时并行处理，弥补移动机器人地图生成时候机载能力不足，负担过重的短板，提高处理效率。

Description

一种基于流式计算的地图创建方法及其系统
技术领域
本发明涉及移动机器人自主导航领域的应用，具体为一种基于流式计算的地图创建方法及其系统。
背景技术
移动机器人广泛应用于军事、航天、深海、医疗、救灾、服务等领域，对其自主导航的精度和可靠性要求越来越高。地图创建和定位由于其重要的理论和应用价值，是实现移动机器人自主导航的关键。
移动机器人机载能力较难满足地图创建的计算量。传统地图创建方法完全依靠移动机器人本地的计算资源实时求解，执行速度慢。受益于网络数据传输速率的快速增长，云计算技术在机器人领域开始得到应用。通过流式计算实时处理数据的特点，构建地图创建流式计算服务框架，实现地图创建分布式并行计算能够提高运行速度和精度，更好的支持移动机器人自主导航技术的快速发展。同时在集群端执行地图创建降低了移动机器人性能要求，可以对部分机载设备进行卸载，提高机器人的移动便携性能。
发明内容
1.所要解决的技术问题：
本发明的目的在于利用流式计算实时处理数据的特点，提供一种的地图创建的流式计算服务，弥补移动机器人地图创建时候机载能力不足和负担过重的短板，提高移动机器人地图创建的精度和效率。
2.技术方案：
一种基于流式计算的地图创建方法，应用于移动机器人，其特征在于：移动机器人将采集的数据传输至流式计算集群实现地图创建与更新；具体包括以下步骤：
步骤一：数据采集；所述移动机器人在移动，移动机器人端的激光雷达实时采集移动机器人的观测数据；移动机器人端的惯性传感器实时采集运动数据；所述运动数据与观测数据进行预处理后，存于移动机器人端。
步骤二：数据传输；将预处理后的数据封装为消息，通过消息网关提交到流式计算集群上。
步骤三：定位与地图创建；基于流式计算的地图创建方法，通过若干计算节点执行分布式并行的平方根卡尔曼滤波粒子滤波算法获得移动机器人位姿和地图特征信息；从队列中取出消息，将数据分发给计算节点，每个计算节点更新粒子，实现粒子的采样和重采样；最终的定位和地图特征数据发送给移动机器人。
步骤四：定位和地图更新；移动机器人根据返回的定位和地图特征数据，更新全局地图，同时推送给局部路径规划算法辅助移动机器人避障。
进一步地，所述步骤三具体包括以下步骤：
S31:取出数据：从消息队列中取出运动数据和观测数据，分发给多个计算节点；所述运动数据包括线速度和舵角；所述观测数据为与一组特征的距离和方向角；并行步骤S32与步骤S34。
S32:并行位姿预测：多个计算节点中的特定计算节点并行进行本步骤；其中每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子的位姿预测；该步骤首先根据上一时刻当前粒子的状态和协方差矩阵初始化Sigma点，然后通过运动模型更新Sigma点，最后预测新的位姿和协方差矩阵平方根；将计算的结果传输至步骤33。
S33：数据关联：将当前粒子的信息与观测到的环境路标和地图上已标注过的环境路标进行关联；将关联的结果传输至步骤S35。
S34:先验知识调整：多个计算节点中的另一部分特定计算节点根据移动机器人适应度函数调整先验知识Q和R，其中Q和R分别代表的是运动误差协方差矩阵和观测误差的协方差矩阵；该步骤是在粒子采样过程中融入先验知识修正的过程，使其能反映真实的先验知识；该步骤将先验知识Q和R设定为动态的，并标记为Qt和Rt；调整通过临近观测的不一致性定义关于Q和R的适应度函数，在此适应度函数基础上，通过粒子群优化(PSO)算法搜索出Q和R的一个预估值据预估值，完成Qt和Rt的修正；将修正的结果传输至步骤S35。
S35:并行位姿更新，每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子位姿的更新；首先根据预测的Sigma点和观测方程更新观测信息；然后计算出基于预测观测值的协方差矩阵平方根，再通过QR分解和cholupdate函数更新该协方差矩阵平方根；最后计算卡尔曼增益，根据卡尔曼增益更新状态均值和协方差矩阵的平方根。
S36:并行地图更新；地图的估计被分解为M个相互独立特征的估计；通过卡尔曼滤波算法EKF来估计当前粒子的每个特征的均值和方差；最后按照目标分布和建议分布的比例计算该粒子的权值。
S37：将权值最大的粒子作为位姿和环境特征地图返回给移动机器人。
S38：计算有效粒子数Neff，当小于指定的阀值执行粒子集重采样；
其中，M是粒子数量，
Figure BDA0002247575040000022
是第k个粒子归一化的权重；如果Neff小于指定的阀值粒子集会被重采样。重采样过后，所有粒子权重被重置。
步骤S37与步骤S38能够并发执行；
步骤S39：将所有粒子信息存储到分布式数据库中；所述步骤S39与步骤S38能够并发执行。
进一步地，所述并行位姿预测具体包括具体过程包括步骤S321至S322：
S321：计算t-1时刻的Sigma点Xt-1，如式(1)：
Xt-1＝[xt-1 xt-1+ηSt-1 xt-1-ηSt-1] (1)
式(1)中：xt-1是t-1时刻该粒子对移动机器人位姿的估计，St-1是t-1时刻的协方差矩阵平方根，尺度常量η计算如式(2)：
Figure BDA0002247575040000031
式(2)中，L为系统状态的维数，λ＝α2(L+κ)-L为尺度调节因子，常量α决定了Sigma点在均值附近的分布，通常将α设置为1e-4≤α≤1，κ为次级尺度调节因子，一般设置为0；β是用来结合系统状态的先验分布，对于高斯分布，β取值为2。
S322:预测：根据t-1时刻的Sigma点和运动模型预测t时刻的Sigma点如式(3)；
Figure BDA0002247575040000033
(3)式中g函数描述了机器人的运动模型，ut是t时刻采自惯性导航的将运动数据。
位姿预测的均值和协方差矩阵平方根通过Sigma点来计算；首先通过QR分解来求
Figure BDA0002247575040000034
为保证协方差的平方根矩阵
Figure BDA0002247575040000035
的半正定性，使用Cholesky一阶更新
Figure BDA0002247575040000036
如式(4)
Figure BDA0002247575040000037
Figure BDA0002247575040000038
Figure BDA0002247575040000039
其中：
Figure BDA00022475750400000310
为第i个Sigma点均值的权值，
Figure BDA00022475750400000311
为第i个Sigma点方差的权值；且
Figure BDA00022475750400000312
Figure BDA00022475750400000313
根据预测的机器人位姿和协方差矩阵更新Sigma点
Figure BDA00022475750400000314
进一步地，所述先验知识调整具体的过程包括步骤S341至S346；
S341：初始化：如果本次或者上次观测的路标数量小于3，不执行Qt和Rt的调整；否则将移动机器人上一时刻的位姿
Figure BDA0002247575040000041
设为原点，转向角设为0，位姿采用极坐标，地图信息由上一次的观测zt-1表示；即：
Figure BDA0002247575040000042
Figure BDA0002247575040000043
S342：位姿预测：根据极坐标下的运动模型预测t时刻机器人的位姿
Figure BDA0002247575040000045
(6)式中，f函数是极坐标下的机器人的运动模型；
S343：数据关联：将临近两次观测到的路标进行关联；关联过程使用动态联合最近邻算法实现数据关联。
S344：通过DFC&ASD-PSO算法搜索最优的参数；
将预测的位姿上一时刻Qt-1和Rt-1以及观测zt和zt-1作为输入参数，通过DFC&ASD-PSO搜索最优的
Figure BDA0002247575040000047
和使得适应度函数值尽可能大；
S345：观测预测：根据机器人位姿和地图信息预测t时刻的观测信息；
Figure BDA00022475750400000410
式中s函数是极坐标下的观测模型；
S346：Qt和Rt的调整；
根据预测的机器人位姿和通过DFC&ASD-PSO搜索的
Figure BDA00022475750400000411
进行对比，当两者之间差值较大的时候，调整Qt和Rt；即带入(7)式中：
Figure BDA00022475750400000412
Figure BDA00022475750400000413
式(7)中，
Figure BDA00022475750400000414
是第j个路标的观测值，
Figure BDA00022475750400000415
是第j个路标预测的观测值；Mo是临近两次重复观测到的路标数量；使用
Figure BDA00022475750400000416
表示运动误差，Dxt～N(0,1)服从正态分布；为了避免频繁地调整，只有当一定概率事件发生的情况下才调整Qt；如果|Dxt|大于1.4或者小于0.2，Qt调整如下：
否则Qt不调整；
Qt＝Qt-1 (9)
式(8)中，ωq是一个根据观测到的路标数量动态调整的参数；设定
Figure BDA0002247575040000051
代表第
j个路标的观测误差；Mn表示
Figure BDA0002247575040000052
绝对值大于1.4的数量，Ln表示
Figure BDA0002247575040000053
绝对值小于
0.2的数量；
Figure BDA0002247575040000054
如果Mnp或者Lnp大于设定的阀值Nmax，Rt调整如下：
Figure BDA0002247575040000055
式(10)中，ωr是一个调整系数；当M0减少的时候，ωq和ωr减少；反之，ωq和ωr增加。
进一步地，所述并行位姿更新具体包括步骤S351至S354。
S351根据预测的Sigma点和观测方程预测t时刻移动机器人的观测；
Figure BDA0002247575040000056
其中，h函数是观察模型，zt是t时刻的观测数据；
Figure BDA0002247575040000057
S352：根据式(12)计算出基于预测观测值的协方差矩阵平方根
Figure BDA0002247575040000058
QR分解和cholupdate更新同样应用于
Figure BDA0002247575040000059
的更新；
Figure BDA00022475750400000510
S353；为了通过观测来更新对机器人位姿的预测值和协方差矩阵的平方根，计算卡尔曼增益Kt；如式(13)；
Figure BDA00022475750400000511
Figure BDA00022475750400000512
S354；最后通过卡尔曼增益更新状态均值和协方差矩阵的平方根；如下式(14)：
Figure BDA00022475750400000513
进一步地，所述并行地图更新具体包括步骤S361至S362。
S361：地图特征的均值和方差的估计：
由于路标估计是基于粒子的，每个粒子关联了N个路标的EKF估计:；路标的更新确定于该路标在t时刻有没有被观测到；如果该路标是新近观测的，其均值和协方差初始化如下：
Figure BDA0002247575040000061
式(15)中，h-1是观测函数的逆函数，Hrz是函数h-1的雅可比矩阵；如果第j个路标在t时刻没有被观测到，则它的均值和方差保持不变；如果第j个路标在t时刻被观测到，均值和方差更新如下：
式(16)中，Kt是卡尔曼增益，Hz是函数h的雅可比矩阵。
S362：计算权值
权值被定义为目标分布和建议分布的比例；
Figure BDA0002247575040000063
Figure BDA0002247575040000064
代表t时刻第i个粒子的权值。
一种基于流式计算的地图创建系统，包括移动机器人端和流式计算集群端；所述移动机器人端运行在嵌入式操作系统上；移动机器人端包括四个部分：数据采集模块、地图管理模块、消息网关和消息客户端；
流式计算集群端包括分布式并行计算模块、消息服务模块、分布式存储模块、应用协调模块；流式计算集群端通过集群方式实现高可用和负载均衡的消息服务的消息服务集群模块；通过分布式方式实现移动机器人传感器数据和地图数据的存储的分布式存储模块；通过分布式并行计算节点实现的分布式地图创建模块；实现计算集群中主控节点、N个计算节点之间的协调、监控和管理的应用协调服务模块，N为大于等于1的整数。
3.有益效果：
本发明中采用一种的地图创建的流式计算服务，能够弥补移动机器人地图创建时候机载能力不足和负担过重的短板，提高移动机器人地图创建的精度和效率。
附图说明
图1为本发明的基于流式计算的地图创建方法及其设备架构图；
图2为本发明的基于流式计算的地图创建方法流程图；
图3为本发明的基于流式计算的移动机器人位姿坐标系；
图4为本发明的基于流式计算的移动机器人运动模型；
图5为本发明的基于流式计算的移动机器人激光雷达观测模型；
图6为本发明的移动机器人运动和观测数据实时采集流程图；
图7为本发明的移动机器人运动和观测数据信息实时上传流程图；
图8为本发明的分布式并行平方根卡尔曼滤波粒子滤波算法的流程图；
图9为本发明的先验知识调整流程图。
具体实施方式
下面结合附图对本发明进行具体的说明。
一种基于流式计算的地图创建方法，应用于移动机器人，其特征在于：移动机器人将采集的数据传输至流式计算集群实现地图创建与更新，如附图1所示；具体包括以下步骤：其流程图如附图2所示；
步骤一：数据采集，如附图6所示；所述移动机器人在移动，移动机器人端的激光雷达实时采集移动机器人的观测数据如附图5所示；移动机器人端的惯性传感器实时采集运动数据，如附图4；所述运动数据与观测数据进行预处理后，存于移动机器人端。图3为本发明中的移动机器人位姿坐标系。
步骤二：数据传输；将预处理后的数据封装为消息，通过消息网关提交到流式计算集群上。如附图7所示。
步骤三：定位与地图创建；基于流式计算的地图创建方法，通过若干计算节点执行分布式并行的平方根卡尔曼滤波粒子滤波算法获得移动机器人位姿和地图特征信息；从队列中取出消息，将数据分发给计算节点，每个计算节点更新粒子，实现粒子的采样和重采样；最终的定位和地图特征数据发送给移动机器人。
步骤四：定位和地图更新；移动机器人根据返回的定位和地图特征数据，更新全局地图，同时推送给局部路径规划算法辅助移动机器人避障。
进一步地，所述步骤三具体包括以下步骤：
S31:取出数据：从消息队列中取出运动数据和观测数据，分发给多个计算节点；所述运动数据包括线速度和舵角；所述观测数据为与一组特征的距离和方向角；并行步骤S32与步骤S34。
S32:并行位姿预测：多个计算节点中的特定计算节点并行进行本步骤；如附图8所示，其中每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子的位姿预测；该步骤首先根据上一时刻当前粒子的状态和协方差矩阵初始化Sigma点，然后通过运动模型更新Sigma点，最后预测新的位姿和协方差矩阵平方根；将计算的结果传输至步骤33。
S33：数据关联：将当前粒子的信息与观测到的环境路标和地图上已标注过的环境路标进行关联；将关联的结果传输至步骤S35。
S34:先验知识调整，如附图9所示：多个计算节点中的另一部分特定计算节点根据移动机器人适应度函数调整先验知识Q和R，其中Q和R分别代表的是运动误差协方差矩阵和观测误差的协方差矩阵；该步骤是在粒子采样过程中融入先验知识修正的过程，使其能反映真实的先验知识；该步骤将先验知识Q和R设定为动态的，并标记为Qt和Rt；调整通过临近观测的不一致性定义关于Q和R的适应度函数，在此适应度函数基础上，通过粒子群优化(PSO)算法搜索出Q和R的一个预估值据预估值，完成Qt和Rt的修正；将修正的结果传输至步骤S35。
S35:并行位姿更新，每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子位姿的更新；首先根据预测的Sigma点和观测方程更新观测信息；然后计算出基于预测观测值的协方差矩阵平方根，再通过QR分解和cholupdate函数更新该协方差矩阵平方根；最后计算卡尔曼增益，根据卡尔曼增益更新状态均值和协方差矩阵的平方根。
S36:并行地图更新；地图的估计被分解为M个相互独立特征的估计；通过卡尔曼滤波算法EKF来估计当前粒子的每个特征的均值和方差；最后按照目标分布和建议分布的比例计算该粒子的权值。
S37：将权值最大的粒子作为位姿和环境特征地图返回给移动机器人。
S38：计算有效粒子数Neff，当小于指定的阀值执行粒子集重采样；
Figure BDA0002247575040000081
其中，M是粒子数量，
Figure BDA0002247575040000082
是第k个粒子归一化的权重；如果Neff小于指定的阀值粒子集会被重采样。重采样过后，所有粒子权重被重置。
步骤S37与步骤S38能够并发执行；
步骤S39：将所有粒子信息存储到分布式数据库中；所述步骤S39与步骤S38能够并发执行。
进一步地，所述并行位姿预测具体包括具体过程包括步骤S321至S322：
S321：计算t-1时刻的Sigma点Xt-1，如式(1)：
Xt-1＝[xt-1 xt-1+ηSt-1 xt-1-ηSt-1] (1)
式(1)中：xt-1是t-1时刻该粒子对移动机器人位姿的估计，St-1是t-1时刻的协方差矩阵平方根，尺度常量η计算如式(2)：
Figure BDA0002247575040000091
式(2)中，L为系统状态的维数，λ＝α2(L+κ)-L为尺度调节因子，常量α决定了Sigma点在均值附近的分布，通常将α设置为1e-4≤α≤1，κ为次级尺度调节因子，一般设置为0；β是用来结合系统状态的先验分布，对于高斯分布，β取值为2。
S322:预测：根据t-1时刻的Sigma点和运动模型预测t时刻的Sigma点
Figure BDA0002247575040000092
如式(3)；
Figure BDA0002247575040000093
(3)式中g函数描述了机器人的运动模型，ut是t时刻采自惯性导航的将运动数据。
位姿预测的均值和协方差矩阵平方根通过Sigma点来计算；首先通过QR分解来求
Figure BDA0002247575040000094
为保证协方差的平方根矩阵
Figure BDA0002247575040000095
的半正定性，使用Cholesky一阶更新
Figure BDA0002247575040000096
如式(4)
Figure BDA0002247575040000098
Figure BDA0002247575040000099
其中：
Figure BDA00022475750400000910
为第i个Sigma点均值的权值，
Figure BDA00022475750400000911
为第i个Sigma点方差的权值；且
Figure BDA00022475750400000912
Figure BDA00022475750400000913
根据预测的机器人位姿和协方差矩阵更新Sigma点
进一步地，所述先验知识调整具体的过程包括步骤S341至S346；
S341：初始化：如果本次或者上次观测的路标数量小于3，不执行Qt和Rt的调整；否则将移动机器人上一时刻的位姿设为原点，转向角设为0，位姿采用极坐标，地图信息由上一次的观测zt-1表示；即：
Figure BDA00022475750400000916
Figure BDA00022475750400000917
S342：位姿预测：根据极坐标下的运动模型预测t时刻机器人的位姿
Figure BDA00022475750400000918
Figure BDA00022475750400000919
(6)式中，f函数是极坐标下的机器人的运动模型。
S343：数据关联：将临近两次观测到的路标进行关联；关联过程使用动态联合最近邻算法实现数据关联。
S344：通过DFC&ASD-PSO算法搜索最优的参数；
将预测的位姿
Figure BDA0002247575040000101
上一时刻Qt-1和Rt-1以及观测zt和zt-1作为输入参数，通过DFC&ASD-PSO搜索最优的
Figure BDA0002247575040000102
和
Figure BDA0002247575040000103
使得适应度函数值尽可能大；
Figure BDA0002247575040000104
S345：观测预测：根据机器人位姿和地图信息预测t时刻的观测信息；
Figure BDA0002247575040000105
式中s函数是极坐标下的观测模型。
S346：Qt和Rt的调整；
根据预测的机器人位姿和通过DFC&ASD-PSO搜索的
Figure BDA00022475750400001017
进行对比，当两者之间差值较大的时候，调整Qt和Rt；即带入(7)式中：
Figure BDA0002247575040000106
式(7)中，
Figure BDA0002247575040000108
是第j个路标的观测值，
Figure BDA0002247575040000109
是第j个路标预测的观测值；Mo是临近两次重复观测到的路标数量；使用
Figure BDA00022475750400001010
表示运动误差，Dxt～N(0,1)服从正态分布；为了避免频繁地调整，只有当一定概率事件发生的情况下才调整Qt；如果|Dxt|大于1.4或者小于0.2，Qt调整如下：
Figure BDA00022475750400001011
否则Qt不调整；
Qt＝Qt-1 (9)
式(8)中，ωq是一个根据观测到的路标数量动态调整的参数；设定
Figure BDA00022475750400001012
代表第j个路标的观测误差；Mn表示
Figure BDA00022475750400001013
绝对值大于1.4的数量，Ln表示
Figure BDA00022475750400001014
绝对值小于0.2的数量；
Figure BDA00022475750400001015
如果Mnp或者Lnp大于设定的阀值Nmax，Rt调整如下：
Figure BDA00022475750400001016
式(10)中，ωr是一个调整系数；当M0减少的时候，ωq和ωr减少；反之，ωq和ωr增加。
进一步地，所述并行位姿更新具体包括步骤S351至S354；
S351根据预测的Sigma点和观测方程预测t时刻移动机器人的观测；
Figure BDA0002247575040000111
其中，h函数是观察模型，zt是t时刻的观测数据；
Figure BDA0002247575040000112
S352：根据式(12)计算出基于预测观测值的协方差矩阵平方根
Figure BDA0002247575040000113
QR分解和cholupdate
更新同样应用于
Figure BDA0002247575040000114
的更新；
Figure BDA0002247575040000115
S353；为了通过观测来更新对机器人位姿的预测值和协方差矩阵的平方根，计算卡尔曼增益Kt；如式(13)；
Figure BDA0002247575040000116
Figure BDA0002247575040000117
S354；最后通过卡尔曼增益更新状态均值和协方差矩阵的平方根；如下式(14)：
Figure BDA0002247575040000118
进一步地，所述并行地图更新具体包括步骤S361至S362；
S361：地图特征的均值和方差的估计：
由于路标估计是基于粒子的，每个粒子关联了N个路标的EKF估计:；路标的更新确定于该路标在t时刻有没有被观测到；如果该路标是新近观测的，其均值和协方差初始化如下：
Figure BDA0002247575040000119
式(15)中，h-1是观测函数的逆函数，Hrz是函数h-1的雅可比矩阵；如果第j个路标在t时刻没有被观测到，则它的均值和方差保持不变；如果第j个路标在t时刻被观测到，均值和方差更新如下：
Figure BDA00022475750400001110
式(16)中，Kt是卡尔曼增益，Hz是函数h的雅可比矩阵。
S362：计算权值
权值被定义为目标分布和建议分布的比例；
Figure BDA0002247575040000121
Figure BDA0002247575040000122
代表t时刻第i个粒子的权值。
一种基于流式计算的地图创建系统，包括移动机器人端和流式计算集群端；所述移动机器人端运行在嵌入式操作系统上；移动机器人端包括四个部分：数据采集模块、地图管理模块、消息网关和消息客户端；
流式计算集群端包括分布式并行计算模块、消息服务模块、分布式存储模块、应用协调模块；流式计算集群端通过集群方式实现高可用和负载均衡的消息服务的消息服务集群模块；通过分布式方式实现移动机器人传感器数据和地图数据的存储的分布式存储模块；通过分布式并行计算节点实现的分布式地图创建模块；实现计算集群中主控节点、N个计算节点之间的协调、监控和管理的应用协调服务模块，N为大于等于1的整数。
虽然本发明已以较佳实施例公开如上，但它们并不是用来限定本发明的，任何熟习此技艺者，在不脱离本发明之精神和范围内，自当可作各种变化或润饰，因此本发明的保护范围应当以本申请的权利要求保护范围所界定的为准。

Claims (7)

1.一种基于流式计算的地图创建方法，应用于移动机器人，其特征在于：移动机器人将采集的数据传输至流式计算集群实现地图创建与更新；具体包括以下步骤：
步骤一：数据采集；所述移动机器人在移动，移动机器人端的激光雷达实时采集移动机器人的观测数据；移动机器人端的惯性传感器实时采集运动数据；所述运动数据与观测数据进行预处理后，存于移动机器人端；
步骤二：数据传输；将预处理后的数据封装为消息，通过消息网关提交到流式计算集群上；
步骤三：定位与地图创建；基于流式计算的地图创建方法，通过若干计算节点执行分布式并行的平方根卡尔曼滤波粒子滤波算法获得移动机器人位姿和地图特征信息；从队列中取出消息，将数据分发给计算节点，每个计算节点更新粒子，实现粒子的采样和重采样；最终的定位和地图特征数据发送给移动机器人；
步骤四：定位和地图更新；移动机器人根据返回的定位和地图特征数据，更新全局地图，同时推送给局部路径规划算法辅助移动机器人避障。
2.根据权利要求1所述的一种基于流式计算的地图创建方法，其特征在于：所述步骤三具体包括以下步骤：
S31:取出数据：从消息队列中取出运动数据和观测数据，分发给多个计算节点；所述运动数据包括线速度和舵角；所述观测数据为与一组特征的距离和方向角；并行步骤S32与步骤S34；
S32:并行位姿预测：多个计算节点中的特定计算节点并行进行本步骤；其中每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子的位姿预测；该步骤首先根据上一时刻当前粒子的状态和协方差矩阵初始化Sigma点，然后通过运动模型更新Sigma点，最后预测新的位姿和协方差矩阵平方根；将计算的结果传输至步骤33；
S33：数据关联：将当前粒子的信息与观测到的环境路标和地图上已标注过的环境路标进行关联；将关联的结果传输至步骤S35；
S34:先验知识调整：多个计算节点中的另一部分特定计算节点根据移动机器人适应度函数调整先验知识Q和R，其中Q和R分别代表的是运动误差协方差矩阵和观测误差的协方差矩阵；该步骤是在粒子采样过程中融入先验知识修正的过程，使其能反映真实的先验知识；该步骤将先验知识Q和R设定为动态的，并标记为Qt和Rt；调整通过临近观测的不一致性定义关于Q和R的适应度函数，在此适应度函数基础上，通过粒子群优化(PSO)算法搜索出Q和R的一个预估值据预估值，完成Qt和Rt的修正；将修正的结果传输至步骤S35；
S35:并行位姿更新，每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子位姿的更新；首先根据预测的Sigma点和观测方程更新观测信息；然后计算出基于预测观测值的协方差矩阵平方根，再通过QR分解和cholupdate函数更新该协方差矩阵平方根；最后计算卡尔曼增益，根据卡尔曼增益更新状态均值和协方差矩阵的平方根；
S36:并行地图更新；地图的估计被分解为M个相互独立特征的估计；通过卡尔曼滤波算法EKF来估计当前粒子的每个特征的均值和方差；最后按照目标分布和建议分布的比例计算该粒子的权值；
S37：将权值最大的粒子作为位姿和环境特征地图返回给移动机器人；
S38：计算有效粒子数Neff，当小于指定的阀值执行粒子集重采样；
Figure FDA0002247575030000021
M是粒子数量，
Figure FDA0002247575030000022
是第k个粒子归一化的权重；如果Neff小于指定的阀值粒子集会被重采样。重采样过后，所有粒子权重被重置；
步骤S37与步骤S38能够并发执行；
步骤S39：将所有粒子信息存储到分布式数据库中；所述步骤S39与步骤S38能够并发执行。
3.根据权利要求2所述的一种基于流式计算的地图创建方法，其特征在于：所述并行位姿预测具体包括具体过程包括步骤S321至S322：
S321：计算t-1时刻的Sigma点Xt-1，如式(1)：
Xt-1＝[xt-1 xt-1+ηSt-1 xt-1-ηSt-1] (1)
式(1)中：xt-1是t-1时刻该粒子对移动机器人位姿的估计，St-1是t-1时刻的协方差矩阵平方根，尺度常量η计算如式(2)：
Figure FDA0002247575030000023
式(2)中，L为系统状态的维数，λ＝α2(L+κ)-L为尺度调节因子，常量α决定了Sigma点在均值附近的分布，通常将α设置为1e-4≤α≤1，κ为次级尺度调节因子，一般设置为0；β是用来结合系统状态的先验分布，对于高斯分布，β取值为2；
S322:预测：根据t-1时刻的Sigma点和运动模型预测t时刻的Sigma点
Figure FDA0002247575030000024
如式(3)；
(3)式中g函数描述了机器人的运动模型，ut是t时刻采自惯性导航的将运动数据；
位姿预测的均值和协方差矩阵平方根通过Sigma点来计算；首先通过QR分解来求为保证协方差的平方根矩阵
Figure FDA0002247575030000027
的半正定性，使用Cholesky一阶更新
Figure FDA0002247575030000028
如式(4)
Figure FDA0002247575030000029
Figure FDA0002247575030000031
Figure FDA0002247575030000032
其中：
Figure FDA0002247575030000033
为第i个Sigma点均值的权值，
Figure FDA0002247575030000034
为第i个Sigma点方差的权值；且
Figure FDA0002247575030000035
Figure FDA0002247575030000036
根据预测的机器人位姿和协方差矩阵更新Sigma点
Figure FDA0002247575030000037
4.根据权利要求3所述的一种基于流式计算的地图创建方法，其特征在于：所述先验知识调整具体的过程包括步骤S341至S346；
S341：初始化：如果本次或者上次观测的路标数量小于3，不执行Qt和Rt的调整；否则将移动机器人上一时刻的位姿设为原点，转向角设为0，位姿采用极坐标，地图信息由上一次的观测zt-1表示；即：
Figure FDA00022475750300000310
S342：位姿预测：根据极坐标下的运动模型预测t时刻机器人的位姿
Figure FDA00022475750300000311
Figure FDA00022475750300000312
(6)式中，f函数是极坐标下的机器人的运动模型；
S343：数据关联：将临近两次观测到的路标进行关联；关联过程使用动态联合最近邻算法实现数据关联；
S344：通过DFC&ASD-PSO算法搜索最优的参数；
将预测的位姿
Figure FDA00022475750300000313
上一时刻Qt-1和Rt-1以及观测zt和zt-1作为输入参数，通过DFC&ASD-PSO搜索最优的
Figure FDA00022475750300000314
和
Figure FDA00022475750300000315
使得适应度函数值尽可能大；
Figure FDA00022475750300000316
S345：观测预测：根据机器人位姿和地图信息预测t时刻的观测信息；
Figure FDA00022475750300000317
式中s函数是极坐标下的观测模型；
S346：Qt和Rt的调整；
根据预测的机器人位姿和通过DFC&ASD-PSO搜索的
Figure FDA00022475750300000318
进行对比，当两者之间差值较大的时候，调整Qt和Rt；即带入(7)式中：
式(7)中，是第j个路标的观测值，
Figure FDA0002247575030000043
是第j个路标预测的观测值；Mo是临近两次重复观测到的路标数量；使用
Figure FDA0002247575030000044
表示运动误差，Dxt～N(0,1)服从正态分布；为了避免频繁地调整，只有当一定概率事件发生的情况下才调整Qt；如果|Dxt|大于1.4或者小于0.2，Qt调整如下：
Figure FDA0002247575030000045
否则Qt不调整；
Qt＝Qt-1 (9)
式(8)中，ωq是一个根据观测到的路标数量动态调整的参数；设定
Figure FDA0002247575030000046
代表第j个路标的观测误差；Mn表示
Figure FDA0002247575030000047
绝对值大于1.4的数量，Ln表示
Figure FDA0002247575030000048
绝对值小于0.2的数量；
Figure FDA0002247575030000049
如果Mnp或者Lnp大于设定的阀值Nmax，Rt调整如下：
Figure FDA00022475750300000410
式(10)中，ωr是一个调整系数；当M0减少的时候，ωq和ωr减少；反之，ωq和ωr增加。
5.根据权利要求4所述的一种基于流式计算的地图创建方法，其特征在于：所述并行位姿更新具体包括步骤S351至S354；
S351根据预测的Sigma点和观测方程预测t时刻移动机器人的观测；
其中，h函数是观察模型，zt是t时刻的观测数据；
Figure FDA00022475750300000412
S352：根据式(12)计算出基于预测观测值的协方差矩阵平方根
Figure FDA00022475750300000413
QR分解和cholupdate更新同样应用于
Figure FDA00022475750300000414
的更新；
Figure FDA00022475750300000415
S353；为了通过观测来更新对机器人位姿的预测值和协方差矩阵的平方根，计算卡尔曼增益Kt；如式(13)；
Figure FDA0002247575030000051
Figure FDA0002247575030000052
S354；最后通过卡尔曼增益更新状态均值和协方差矩阵的平方根；如下式(14)：
Figure FDA0002247575030000053
6.根据权利要求5所述的一种基于流式计算的地图创建方法，其特征在于：所述并行地图更新具体包括步骤S361至S362；
S361：地图特征的均值和方差的估计：
由于路标估计是基于粒子的，每个粒子关联了N个路标的EKF估计:；路标的更新确定于该路标在t时刻有没有被观测到；如果该路标是新近观测的，其均值和协方差初始化如下：
Figure FDA0002247575030000054
Figure FDA0002247575030000055
式(15)中，h-1是观测函数的逆函数，Hrz是函数h-1的雅可比矩阵；如果第j个路标在t时刻没有被观测到，则它的均值和方差保持不变；如果第j个路标在t时刻被观测到，均值和方差更新如下：
Figure FDA0002247575030000056
式(16)中，Kt是卡尔曼增益，Hz是函数h的雅可比矩阵；
S362：计算权值
权值被定义为目标分布和建议分布的比例；
Figure FDA0002247575030000059
代表t时刻第i个粒子的权值。
7.一种基于流式计算的地图创建系统，使用如权利要求1至6任一权利要求所述的一种基于流式计算的地图创建方法，其特征在于：包括移动机器人端和流式计算集群端；所述移动机器人端运行在嵌入式操作系统上；移动机器人端包括四个部分：数据采集模块、地图管理模块、消息网关和消息客户端；
流式计算集群端包括分布式并行计算模块、消息服务模块、分布式存储模块、应用协调模块；流式计算集群端通过集群方式实现高可用和负载均衡的消息服务的消息服务集群模块；通过分布式方式实现移动机器人传感器数据和地图数据的存储的分布式存储模块；通过分布式并行计算节点实现的分布式地图创建模块；实现计算集群中主控节点、N个计算节点之间的协调、监控和管理的应用协调服务模块，N为大于等于1的整数。
CN201911022206.3A 2019-10-25 2019-10-25 一种基于流式计算的地图创建方法及其系统 Withdrawn CN110763245A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201911022206.3A CN110763245A (zh) 2019-10-25 2019-10-25 一种基于流式计算的地图创建方法及其系统
PCT/CN2020/096031 WO2021077769A1 (zh) 2019-10-25 2020-06-15 一种基于流式计算的地图创建方法及其系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911022206.3A CN110763245A (zh) 2019-10-25 2019-10-25 一种基于流式计算的地图创建方法及其系统

Publications (1)

Publication Number Publication Date
CN110763245A true CN110763245A (zh) 2020-02-07

Family

ID=69333693

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911022206.3A Withdrawn CN110763245A (zh) 2019-10-25 2019-10-25 一种基于流式计算的地图创建方法及其系统

Country Status (2)

Country Link
CN (1) CN110763245A (zh)
WO (1) WO2021077769A1 (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111928866A (zh) * 2020-09-27 2020-11-13 上海思岚科技有限公司 一种机器人地图差异更新的方法及设备
CN112070201A (zh) * 2020-08-26 2020-12-11 成都睿芯行科技有限公司 一种基于Gmapping提升建图速度的方法
WO2021077769A1 (zh) * 2019-10-25 2021-04-29 江苏海事职业技术学院 一种基于流式计算的地图创建方法及其系统
CN113701742A (zh) * 2021-08-24 2021-11-26 吕太之 一种基于云端与边端融合计算的移动机器人slam方法
CN113701739A (zh) * 2020-05-22 2021-11-26 杭州海康机器人技术有限公司 一种地图更新方法以及装置

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106323269B (zh) * 2015-12-10 2019-06-07 上海思岚科技有限公司 自主定位导航设备、定位导航方法及自主定位导航系统
CN108645413A (zh) * 2018-06-06 2018-10-12 江苏海事职业技术学院 一种移动机器人的同时定位与地图创建的动态纠正方法
CN110057360A (zh) * 2019-03-08 2019-07-26 江苏海事职业技术学院 一种基于分布式并行计算的路径规划方法及其系统
CN110763245A (zh) * 2019-10-25 2020-02-07 江苏海事职业技术学院 一种基于流式计算的地图创建方法及其系统

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2021077769A1 (zh) * 2019-10-25 2021-04-29 江苏海事职业技术学院 一种基于流式计算的地图创建方法及其系统
CN113701739A (zh) * 2020-05-22 2021-11-26 杭州海康机器人技术有限公司 一种地图更新方法以及装置
CN113701739B (zh) * 2020-05-22 2023-08-18 杭州海康机器人股份有限公司 一种地图更新方法以及装置
CN112070201A (zh) * 2020-08-26 2020-12-11 成都睿芯行科技有限公司 一种基于Gmapping提升建图速度的方法
CN111928866A (zh) * 2020-09-27 2020-11-13 上海思岚科技有限公司 一种机器人地图差异更新的方法及设备
CN111928866B (zh) * 2020-09-27 2021-02-12 上海思岚科技有限公司 一种机器人地图差异更新的方法及设备
CN113701742A (zh) * 2021-08-24 2021-11-26 吕太之 一种基于云端与边端融合计算的移动机器人slam方法
CN113701742B (zh) * 2021-08-24 2024-04-26 江苏海事职业技术学院 一种基于云端与边端融合计算的移动机器人slam方法

Also Published As

Publication number Publication date
WO2021077769A1 (zh) 2021-04-29

Similar Documents

Publication Publication Date Title
CN110763245A (zh) 一种基于流式计算的地图创建方法及其系统
CN108759833B (zh) 一种基于先验地图的智能车辆定位方法
CN109798896B (zh) 一种室内机器人定位与建图方法及装置
Indelman et al. Factor graph based incremental smoothing in inertial navigation systems
CN109813319B (zh) 一种基于slam建图的开环优化方法及系统
CN109597864B (zh) 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统
CN110398969A (zh) 自动驾驶车辆自适应预测时域转向控制方法及装置
Akca et al. Multiple model Kalman and Particle filters and applications: A survey
CN109459019A (zh) 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法
CN113470089B (zh) 一种基于三维点云的跨域协同定位和建图方法及系统
CN109782269B (zh) 一种分布式多平台协同有源目标跟踪方法
Peng et al. UAV positioning based on multi-sensor fusion
CN109491241B (zh) 一种针对机动目标的无人机鲁棒化跟踪方法
CN114459470A (zh) 基于多传感器融合的巡检机器人定位方法
CN103901891A (zh) 一种基于层次结构的动态粒子树slam算法
Luo et al. A fast algorithm of slam based on combinatorial interval filters
CN113701742B (zh) 一种基于云端与边端融合计算的移动机器人slam方法
Hasan et al. Automatic estimation of inertial navigation system errors for global positioning system outage recovery
Tian et al. Resilient and distributed multi-robot visual slam: Datasets, experiments, and lessons learned
Wang et al. H-SwarmLoc: Efficient Scheduling for Localization of Heterogeneous MAV Swarm with Deep Reinforcement Learning
CN112985391A (zh) 一种基于惯性和双目视觉的多无人机协同导航方法和装置
CN114993341B (zh) 一种基于天基测量的运载火箭弹道估计方法及装置
Dah-Jing et al. Neural network aided adaptive Kalman filter for GPS/INS navigation system design
CN116399335A (zh) 基于高斯信念传播的飞行器集群分布式协同定位方法
CN113744308B (zh) 位姿优化方法、装置、电子设备、介质及程序产品

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
WW01 Invention patent application withdrawn after publication
WW01 Invention patent application withdrawn after publication

Application publication date: 20200207