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

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

Info

Publication number
WO2021077769A1
WO2021077769A1 PCT/CN2020/096031 CN2020096031W WO2021077769A1 WO 2021077769 A1 WO2021077769 A1 WO 2021077769A1 CN 2020096031 W CN2020096031 W CN 2020096031W WO 2021077769 A1 WO2021077769 A1 WO 2021077769A1
Authority
WO
WIPO (PCT)
Prior art keywords
map
mobile robot
data
observation
pose
Prior art date
Application number
PCT/CN2020/096031
Other languages
English (en)
French (fr)
Inventor
吕太之
Original Assignee
江苏海事职业技术学院
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 江苏海事职业技术学院 filed Critical 江苏海事职业技术学院
Publication of WO2021077769A1 publication Critical patent/WO2021077769A1/zh

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

Definitions

  • the invention relates to an application in the field of autonomous navigation of a mobile robot, in particular to a method and system for creating a map based on stream computing.
  • Mobile robots are widely used in military, aerospace, deep-sea, medical, disaster relief, service and other fields, and the accuracy and reliability of their autonomous navigation are becoming higher and higher. Because of its important theoretical and application value, map creation and positioning are the key to realizing autonomous navigation of mobile robots.
  • map creation The on-board capacity of mobile robots is difficult to meet the computational load of map creation.
  • the traditional map creation method completely relies on the local computing resources of the mobile robot to solve in real time, and the execution speed is slow.
  • cloud computing technology Benefiting from the rapid growth of network data transmission rates, cloud computing technology has begun to be applied in the field of robotics.
  • constructing a map creation streaming computing service framework and realizing distributed parallel computing for map creation can improve the running speed and accuracy, and better support the rapid development of autonomous navigation technology for mobile robots.
  • performing map creation on the cluster side reduces the performance requirements of mobile robots, and can unload part of the airborne equipment to improve the mobile portability of the robot.
  • the purpose of the present invention is to utilize the characteristics of real-time data processing of streaming computing to provide a streaming computing service for map creation, to make up for the lack of on-board capacity and overburden when the mobile robot map is created, and to improve the creation of mobile robot maps. Precision and efficiency.
  • a map creation method based on streaming computing, applied to a mobile robot is characterized in that: the mobile robot transmits collected data to a streaming computing cluster to realize map creation and update; specifically including the following steps:
  • Step 1 Data collection; the mobile robot is moving, the lidar on the mobile robot side collects the observation data of the mobile robot in real time; the inertial sensor on the mobile robot side collects the motion data in real time; the motion data and the observation data are preprocessed and stored in Mobile robot end.
  • Step 2 Data transmission; encapsulate the preprocessed data into a message, and submit it to the streaming computing cluster through the message gateway.
  • Step 3 Positioning and map creation; map creation method based on streaming computing, through a number of computing nodes to execute distributed parallel square root Kalman filter particle filter algorithm to obtain mobile robot pose and map feature information; take out the message from the queue, The data is distributed to the computing nodes, and each computing node updates the particles to realize the sampling and resampling of the particles; the final positioning and map feature data are sent to the mobile robot.
  • Step 4 Positioning and map update; the mobile robot updates the global map according to the returned positioning and map feature data, and at the same time pushes it to the local path planning algorithm to assist the mobile robot in avoiding obstacles.
  • step three specifically includes the following steps:
  • Step S31 fetch data: fetch motion data and observation data from the message queue, and distribute them to multiple computing nodes; the motion data includes linear velocity and rudder angle; the observation data is the distance and direction angle to a set of features; parallel Step S32 and step S34.
  • S32 Parallel pose prediction: a specific computing node among multiple computing nodes performs this step in parallel; each computing node uses a square root-based Kalman filter algorithm to realize the pose prediction of the corresponding particle; this step is first based on the current particle at the previous moment Initialize the sigma point of the state and covariance matrix, then update the sigma point through the motion model, and finally predict the new pose and the square root of the covariance matrix; the calculated result is transmitted to step 33.
  • S33 Data association: Associate the current particle information with the observed environmental landmarks and the environmental landmarks that have been marked on the map; transmit the associated result to step S35.
  • Priori knowledge adjustment another part of the specific computing nodes among the multiple computing nodes adjusts the prior knowledge Q and R according to the fitness function of the mobile robot, where Q and R represent the covariance matrix of the motion error and the covariance of the observation error. Variance matrix; this step is the process of incorporating prior knowledge correction into the particle sampling process, so that it can reflect the true prior knowledge; this step sets the prior knowledge Q and R to be dynamic, and marks them as Q t and R t ; adjust the fitness function of Q and R defined by the inconsistency of nearby observations. Based on this fitness function, an estimated value of Q and R is searched out by the particle swarm optimization (PSO) algorithm. According to the estimated value , The correction of Q t and R t is completed; the corrected result is transmitted to step S35.
  • PSO particle swarm optimization
  • each computing node uses the square root Kalman filter algorithm to update the corresponding particle pose; first update the observation information according to the predicted Sigma point and the observation equation; then calculate the covariance matrix based on the predicted observation value Square root, and then update the square root of the covariance matrix by QR decomposition and cholupdate function; finally calculate the Kalman gain, update the square root of the state mean and the covariance matrix according to the Kalman gain.
  • M is the number of particles
  • I the normalized weight of the k-th particle; if N eff is less than the specified threshold, the particle set will be resampled. After resampling, all particle weights are reset.
  • Step S37 and step S38 can be executed concurrently;
  • Step S39 store all particle information in a distributed database; the steps S39 and S38 can be executed concurrently.
  • the parallel pose prediction specifically includes a specific process including steps S321 to S322:
  • X t-1 [x t-1 x t-1 + ⁇ S t-1 x t-1 - ⁇ S t-1 ] (1)
  • x t-1 is the estimation of the particle's pose of the mobile robot at t-1
  • St-1 is the square root of the covariance matrix at t-1
  • scale constant ⁇ is calculated as formula (2):
  • L is the dimension of the system state
  • ⁇ determines the distribution of the Sigma point near the mean value, usually ⁇ is set to 1e-4 ⁇ 1
  • ⁇ is the secondary scale adjustment factor, generally set to 0
  • ⁇ is the prior distribution used to combine the state of the system, and for the Gaussian distribution, the value of ⁇ is 2.
  • the g function describes the motion model of the robot, and u t is the motion data collected from the inertial navigation at time t.
  • the mean value and the square root of the covariance matrix of pose prediction are calculated by Sigma points; firstly, it is calculated by QR decomposition To ensure the square root matrix of covariance Positive semi-definite, using Cholesky first-order update As in formula (4)
  • the f function is the motion model of the robot in polar coordinates
  • S343 Data Association: Associate the two nearby road signs observed; the association process uses the dynamic joint nearest neighbor algorithm to achieve data association.
  • Is the observation value of the j-th road sign Is the observed value of the j-th road sign prediction
  • M o is the number of road signs observed twice in the vicinity
  • use Represents the motion error, Dx t ⁇ N(0,1) obey the normal distribution; in order to avoid frequent adjustment, only adjust Q t when certain probability events occur; if
  • ⁇ q is a parameter that is dynamically adjusted according to the number of road signs observed; set Represents the observation error of the j-th road sign; Mn represents The quantity whose absolute value is greater than 1.4, Ln means The quantity whose absolute value is less than 0.2;
  • R t is adjusted as follows:
  • ⁇ r is an adjustment coefficient; when M 0 decreases, ⁇ q and ⁇ r decrease; otherwise, ⁇ q and ⁇ r increase.
  • the parallel pose update specifically includes steps S351 to S354.
  • S351 predicts the observation of the mobile robot at time t according to the predicted Sigma point and the observation equation
  • the h function is the observation model, and z t is the observation data at time t;
  • the parallel map update specifically includes steps S361 to S362.
  • each particle is associated with the EKF estimation of N landmarks:; the update of the landmark is determined by whether the landmark is observed at time t; if the landmark is newly observed, its mean and covariance
  • the initialization is as follows:
  • h -1 is the inverse function of the observation function
  • H rz is the Jacobian matrix of the function h -1 ; if the j-th road sign is not observed at time t, its mean and variance remain unchanged ; If the j-th road sign is observed at time t, the mean and variance are updated as follows:
  • K t is the Kalman gain
  • H z is the Jacobian matrix of the function h.
  • the weight is defined as the ratio of the target distribution to the recommended distribution
  • a map creation system based on streaming computing includes a mobile robot terminal and a streaming computing cluster terminal; the mobile robot terminal runs on an embedded operating system; the mobile robot terminal includes four parts: a data collection module and a map management module , Message gateway and message client;
  • the streaming computing cluster includes a distributed parallel computing module, a message service module, a distributed storage module, and an application coordination module; the streaming computing cluster realizes a message service cluster module with high availability and load balance through a cluster; through distribution A distributed storage module that realizes the storage of mobile robot sensor data and map data in a way; a distributed map creation module realized by distributed parallel computing nodes; realization of coordination and monitoring between the main control node and N computing nodes in the computing cluster Coordinating the service module with the managed application, N is an integer greater than or equal to 1.
  • a stream computing service for map creation can make up for the shortcomings of insufficient airborne capacity and overburden when the mobile robot map is created, and improve the accuracy and efficiency of the mobile robot map creation.
  • FIG. 1 is a diagram of the map creation method based on streaming computing and its equipment architecture diagram of the present invention
  • Figure 2 is a flow chart of the method for creating a map based on streaming computing according to the present invention
  • Figure 3 is a mobile robot pose coordinate system based on stream computing of the present invention
  • Figure 4 is a mobile robot motion model based on stream computing of the present invention
  • Fig. 5 is a mobile robot lidar observation model based on flow calculation of the present invention.
  • Fig. 6 is a flow chart of real-time collection of movement and observation data of a mobile robot according to the present invention.
  • Fig. 7 is a flow chart of real-time uploading of movement and observation data information of the mobile robot according to the present invention.
  • Fig. 8 is a flow chart of the distributed parallel square root Kalman filter particle filter algorithm of the present invention.
  • Fig. 9 is a flow chart of prior knowledge adjustment of the present invention.
  • the step three specifically includes the following steps:
  • Step S31 fetch data: fetch motion data and observation data from the message queue, and distribute them to multiple computing nodes; the motion data includes linear velocity and rudder angle; the observation data is the distance and direction angle to a set of features; parallel Step S32 and step S34.
  • S32 Parallel pose prediction: a specific computing node among multiple computing nodes performs this step in parallel; as shown in the flowchart of the distributed parallel square root Kalman filter particle filter algorithm in FIG. 8, each computing node adopts a square root-based algorithm.
  • the Kalman filter algorithm realizes the pose prediction of the corresponding particles; this step first initializes the sigma point according to the current particle state and covariance matrix at the previous moment, then updates the sigma point through the motion model, and finally predicts the new pose and covariance matrix square root ; Transmit the calculated result to step 33.
  • S33 Data association: Associate the current particle information with the observed environmental landmarks and the environmental landmarks that have been marked on the map; transmit the associated result to step S35.
  • each computing node uses the square root Kalman filter algorithm to update the corresponding particle pose; first update the observation information according to the predicted Sigma point and the observation equation; then calculate the covariance matrix based on the predicted observation value Square root, and then update the square root of the covariance matrix by QR decomposition and cholupdate function; finally calculate the Kalman gain, update the square root of the state mean and the covariance matrix according to the Kalman gain.
  • M is the number of particles
  • I the normalized weight of the k-th particle; if N eff is less than the specified threshold, the particle set will be resampled. After resampling, all particle weights are reset.
  • Step S37 and step S38 can be executed concurrently;
  • Step S39 store all particle information in a distributed database; the steps S39 and S38 can be executed concurrently.
  • the parallel pose prediction specifically includes a specific process including steps S321 to S322:
  • X t-1 [x t-1 x t-1 + ⁇ S t-1 x t-1 - ⁇ S t-1 ] (1)
  • x t-1 is the estimation of the particle's pose of the mobile robot at t-1
  • St-1 is the square root of the covariance matrix at t-1
  • scale constant ⁇ is calculated as formula (2):
  • L is the dimension of the system state
  • ⁇ determines the distribution of the Sigma point near the mean value, usually ⁇ is set to 1e-4 ⁇ 1
  • ⁇ is the secondary scale adjustment factor, generally set to 0
  • ⁇ is the prior distribution used to combine the state of the system, and for the Gaussian distribution, the value of ⁇ is 2.
  • the g function describes the motion model of the robot, and u t is the motion data collected from the inertial navigation at time t.
  • the mean value and the square root of the covariance matrix of pose prediction are calculated by Sigma points; firstly, it is calculated by QR decomposition To ensure the square root matrix of covariance Positive semi-definite, using Cholesky first-order update As in formula (4)
  • the f function is the motion model of the robot in polar coordinates.
  • the s function is the observation model in polar coordinates.
  • Is the observation value of the j-th road sign Is the observed value predicted by the j-th road sign
  • M o is the number of road signs observed twice in the vicinity
  • use Represents the motion error, Dx t ⁇ N(0,1) obey the normal distribution; in order to avoid frequent adjustment, only adjust Q t when a certain probability event occurs; if
  • ⁇ q is a parameter that is dynamically adjusted according to the number of road signs observed; set Represents the observation error of the j-th road sign; Mn represents The quantity whose absolute value is greater than 1.4, Ln means The quantity whose absolute value is less than 0.2;
  • R t is adjusted as follows:
  • ⁇ r is an adjustment coefficient; when M 0 decreases, ⁇ q and ⁇ r decrease; otherwise, ⁇ q and ⁇ r increase.
  • the parallel pose update specifically includes steps S351 to S354;
  • S351 predicts the observation of the mobile robot at time t according to the predicted Sigma point and the observation equation
  • the h function is the observation model, and z t is the observation data at time t;
  • the parallel map update specifically includes steps S361 to S362;
  • each particle is associated with the EKF estimation of N landmarks:; the update of the landmark is determined by whether the landmark is observed at time t; if the landmark is newly observed, its mean and covariance
  • the initialization is as follows:
  • h -1 is the inverse function of the observation function
  • H rz is the Jacobian matrix of the function h -1 ; if the j-th road sign is not observed at time t, its mean and variance remain unchanged ; If the j-th road sign is observed at time t, the mean and variance are updated as follows:
  • K t is the Kalman gain
  • H z is the Jacobian matrix of the function h.
  • the weight is defined as the ratio of the target distribution to the recommended distribution
  • a map creation method based on streaming computing, applied to a mobile robot characterized in that: the mobile robot transmits the collected data to the streaming computing cluster to realize map creation and update, such as the map creation method and device architecture of Figure 1 As shown in the figure; specifically including the following steps: its flow chart is shown in the flow chart of the map creation method in FIG.
  • Step 1 Data collection, as shown in the flowchart of real-time acquisition of mobile robot motion and observation data in Figure 6; the mobile robot is moving, and the lidar on the mobile robot side collects the observation data of the mobile robot in real time, as shown in Figure 5, the mobile robot laser The radar observation model is shown; the inertial sensor on the mobile robot side collects motion data in real time, such as the mobile robot motion model in Figure 4; the motion data and the observation data are preprocessed and stored on the mobile robot side.
  • Figure 3 is the pose coordinate system of the mobile robot in the present invention.
  • Step 2 Data transmission; encapsulate the preprocessed data into a message, and submit it to the streaming computing cluster through the message gateway. As shown in FIG. 7, the flow chart of real-time uploading of the movement and observation data of the mobile robot is shown.
  • Step 3 Positioning and map creation; map creation method based on streaming computing, through a number of computing nodes to execute distributed parallel square root Kalman filter particle filter algorithm to obtain mobile robot pose and map feature information; take out the message from the queue, The data is distributed to the computing nodes, and each computing node updates the particles to realize the sampling and resampling of the particles; the final positioning and map feature data are sent to the mobile robot.
  • Step 4 Positioning and map update; the mobile robot updates the global map according to the returned positioning and map feature data, and at the same time pushes it to the local path planning algorithm to assist the mobile robot in avoiding obstacles.
  • a map creation system based on streaming computing includes a mobile robot terminal and a streaming computing cluster terminal; the mobile robot terminal runs on an embedded operating system; the mobile robot terminal includes four parts: a data collection module and a map management module , Message gateway and message client;
  • the streaming computing cluster includes a distributed parallel computing module, a message service module, a distributed storage module, and an application coordination module; the streaming computing cluster realizes a message service cluster module with high availability and load balance through a cluster; through distribution A distributed storage module that realizes the storage of mobile robot sensor data and map data in a way; a distributed map creation module realized by distributed parallel computing nodes; realization of coordination and monitoring between the main control node and N computing nodes in the computing cluster Coordinating the service module with the managed application, N is an integer greater than or equal to 1.

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

一种基于流式计算的地图创建方法及系统，方法包括：步骤一，移动机器人端的激光雷达和惯性传感器实时采集移动机器人的观测数据和运动数据，运动数据与观测数据进行预处理后，存于移动机器人端；步骤二，将预处理后的数据封装为消息，通过消息网关提交到流式计算集群上；步骤三，基于流式计算的地图创建方法，通过若干计算节点执行分布式并行的平方根卡尔曼滤波粒子滤波算法获得移动机器人位姿和地图特征信息，最终的定位和地图特征数据发送给移动机器人；步骤四，移动机器人根据返回的定位和地图特征数据，更新全局地图，同时推送给局部路径规划算法辅助移动机器人避障。该方法将流式计算集群端强大的计算能力和平方根卡尔曼滤波粒子滤波算法结合起来实现基于流式计算的分布式并行SLAM算法，将特征地图生成过程在流式计算框架中进行实时并行处理，弥补移动机器人地图生成时候机载能力不足，负担过重的短板，提高处理效率。

Description

一种基于流式计算的地图创建方法及其系统 技术领域
本发明涉及移动机器人自主导航领域的应用，具体为一种基于流式计算的地图创建方法及其系统。
背景技术
移动机器人广泛应用于军事、航天、深海、医疗、救灾、服务等领域，对其自主导航的精度和可靠性要求越来越高。地图创建和定位由于其重要的理论和应用价值，是实现移动机器人自主导航的关键。
移动机器人机载能力较难满足地图创建的计算量。传统地图创建方法完全依靠移动机器人本地的计算资源实时求解，执行速度慢。受益于网络数据传输速率的快速增长，云计算技术在机器人领域开始得到应用。通过流式计算实时处理数据的特点，构建地图创建流式计算服务框架，实现地图创建分布式并行计算能够提高运行速度和精度，更好的支持移动机器人自主导航技术的快速发展。同时在集群端执行地图创建降低了移动机器人性能要求，可以对部分机载设备进行卸载，提高机器人的移动便携性能。
发明内容
1.所要解决的技术问题：
本发明的目的在于利用流式计算实时处理数据的特点，提供一种的地图创建的流式计算服务，弥补移动机器人地图创建时候机载能力不足和负担过重的短板，提高移动机器人地图创建的精度和效率。
2.技术方案：
一种基于流式计算的地图创建方法，应用于移动机器人，其特征在于：移动机器人将采集的数据传输至流式计算集群实现地图创建与更新；具体包括以下步骤：
步骤一：数据采集；所述移动机器人在移动，移动机器人端的激光雷达实时采集移动机器人的观测数据；移动机器人端的惯性传感器实时采集运动数据；所述运动数据与观测数据进行预处理后，存于移动机器人端。
步骤二：数据传输；将预处理后的数据封装为消息，通过消息网关提交到流式计算集群上。
步骤三：定位与地图创建；基于流式计算的地图创建方法，通过若干计算节点执行分布式并行的平方根卡尔曼滤波粒子滤波算法获得移动机器人位姿和地图特征信息；从队列中取出消息，将数据分发给计算节点，每个计算节点更新粒子，实现粒子的采样和重采样；最终 的定位和地图特征数据发送给移动机器人。
步骤四：定位和地图更新；移动机器人根据返回的定位和地图特征数据，更新全局地图，同时推送给局部路径规划算法辅助移动机器人避障。
进一步地，所述步骤三具体包括以下步骤：
S31:取出数据：从消息队列中取出运动数据和观测数据，分发给多个计算节点；所述运动数据包括线速度和舵角；所述观测数据为与一组特征的距离和方向角；并行步骤S32与步骤S34。
S32:并行位姿预测：多个计算节点中的特定计算节点并行进行本步骤；其中每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子的位姿预测；该步骤首先根据上一时刻当前粒子的状态和协方差矩阵初始化Sigma点，然后通过运动模型更新Sigma点，最后预测新的位姿和协方差矩阵平方根；将计算的结果传输至步骤33。
S33：数据关联：将当前粒子的信息与观测到的环境路标和地图上已标注过的环境路标进行关联；将关联的结果传输至步骤S35。
S34:先验知识调整：多个计算节点中的另一部分特定计算节点根据移动机器人适应度函数调整先验知识Q和R，其中Q和R分别代表的是运动误差协方差矩阵和观测误差的协方差矩阵；该步骤是在粒子采样过程中融入先验知识修正的过程，使其能反映真实的先验知识；该步骤将先验知识Q和R设定为动态的，并标记为Q t和R t；调整通过临近观测的不一致性定义关于Q和R的适应度函数，在此适应度函数基础上，通过粒子群优化(PSO)算法搜索出Q和R的一个预估值据预估值，完成Q t和R t的修正；将修正的结果传输至步骤S35。
S35:并行位姿更新，每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子位姿的更新；首先根据预测的Sigma点和观测方程更新观测信息；然后计算出基于预测观测值的协方差矩阵平方根，再通过QR分解和cholupdate函数更新该协方差矩阵平方根；最后计算卡尔曼增益，根据卡尔曼增益更新状态均值和协方差矩阵的平方根。
S36:并行地图更新；地图的估计被分解为M个相互独立特征的估计；通过卡尔曼滤波算法EKF来估计当前粒子的每个特征的均值和方差；最后按照目标分布和建议分布的比例计算该粒子的权值。
S37：将权值最大的粒子作为位姿和环境特征地图返回给移动机器人。
S38：计算有效粒子数N eff，当小于指定的阀值执行粒子集重采样；
Figure PCTCN2020096031-appb-000001
其中，M是粒子数量，
Figure PCTCN2020096031-appb-000002
是第k个粒子归一化的权重；如果N eff小于指定的阀值粒子集会被重采样。重采样过后，所有粒子权重被重置。
步骤S37与步骤S38能够并发执行；
步骤S39：将所有粒子信息存储到分布式数据库中；所述步骤S39与步骤S38能够并发执行。
进一步地，所述并行位姿预测具体包括具体过程包括步骤S321至S322：
S321：计算t-1时刻的Sigma点X t-1，如式(1)：
X t-1＝[x t-1 x t-1+ηS t-1 x t-1-ηS t-1]    (1)
式(1)中：x t-1是t-1时刻该粒子对移动机器人位姿的估计，S t-1是t-1时刻的协方差矩阵平方根，尺度常量η计算如式(2)：
Figure PCTCN2020096031-appb-000003
式(2)中，L为系统状态的维数，λ＝α 2(L+κ)-L为尺度调节因子，常量α决定了Sigma点在均值附近的分布，通常将α设置为1e-4≤α≤1，κ为次级尺度调节因子，一般设置为0；β是用来结合系统状态的先验分布，对于高斯分布，β取值为2。
S322:预测：根据t-1时刻的Sigma点和运动模型预测t时刻的Sigma点
Figure PCTCN2020096031-appb-000004
如式(3)；
Figure PCTCN2020096031-appb-000005
(3)式中g函数描述了机器人的运动模型，u t是t时刻采自惯性导航的将运动数据。
位姿预测的均值和协方差矩阵平方根通过Sigma点来计算；首先通过QR分解来求
Figure PCTCN2020096031-appb-000006
为保证协方差的平方根矩阵
Figure PCTCN2020096031-appb-000007
的半正定性，使用Cholesky一阶更新
Figure PCTCN2020096031-appb-000008
如式(4)
Figure PCTCN2020096031-appb-000009
Figure PCTCN2020096031-appb-000010
Figure PCTCN2020096031-appb-000011
其中：
Figure PCTCN2020096031-appb-000012
为第i个Sigma点均值的权值，
Figure PCTCN2020096031-appb-000013
为第i个Sigma点方差的权值；且
Figure PCTCN2020096031-appb-000014
Figure PCTCN2020096031-appb-000015
根据预测的机器人位姿和协方差矩阵更新Sigma点
Figure PCTCN2020096031-appb-000016
进一步地，所述先验知识调整具体的过程包括步骤S341至S346；
S341：初始化：如果本次或者上次观测的路标数量小于3，不执行Q t和R t的调整；否则将移动机器人上一时刻的位姿
Figure PCTCN2020096031-appb-000017
设为原点，转向角设为0，位姿采用极坐标，地图信息由上一次的观测z t-1表示；即：
Figure PCTCN2020096031-appb-000018
Figure PCTCN2020096031-appb-000019
S342：位姿预测：根据极坐标下的运动模型预测t时刻机器人的位姿
Figure PCTCN2020096031-appb-000020
Figure PCTCN2020096031-appb-000021
(6)式中，f函数是极坐标下的机器人的运动模型；
S343：数据关联：将临近两次观测到的路标进行关联；关联过程使用动态联合最近邻算法实现数据关联。
S344：通过DFC&ASD-PSO算法搜索最优的参数；
将预测的位姿
Figure PCTCN2020096031-appb-000022
上一时刻Q t-1和R t-1以及观测z t和z t-1作为输入参数，通过DFC&ASD-PSO搜索最优的
Figure PCTCN2020096031-appb-000023
和
Figure PCTCN2020096031-appb-000024
使得适应度函数值尽可能大；
Figure PCTCN2020096031-appb-000025
S345：观测预测：根据机器人位姿和地图信息预测t时刻的观测信息；
Figure PCTCN2020096031-appb-000026
式中s函数是极坐标下的观测模型；
S346：Q t和R t的调整；
根据预测的机器人位姿和通过DFC&ASD-PSO搜索的
Figure PCTCN2020096031-appb-000027
进行对比，当两者之间差值较大的时候，调整Q t和R t；即带入(7)式中：
Figure PCTCN2020096031-appb-000028
Figure PCTCN2020096031-appb-000029
式(7)中，
Figure PCTCN2020096031-appb-000030
是第j个路标的观测值，
Figure PCTCN2020096031-appb-000031
是第j个路标预测的观测值；M o是临近两次重复观测到的路标数量；使用
Figure PCTCN2020096031-appb-000032
表示运动误差，Dx t～N(0,1)服从正态分布；为了避免频繁地调整，只有当一定概率事件发生的情况下才调整Q t；如果|Dx t|大于1.4或者小于0.2，Q t调整如下：
Figure PCTCN2020096031-appb-000033
否则Q t不调整；
Q t＝Q t-1     (9)
式(8)中，ω q是一个根据观测到的路标数量动态调整的参数；设定
Figure PCTCN2020096031-appb-000034
代表第j个路标的观测误差；Mn表示
Figure PCTCN2020096031-appb-000035
绝对值大于1.4的数量，Ln表示
Figure PCTCN2020096031-appb-000036
绝对值小于0.2的数量；
Figure PCTCN2020096031-appb-000037
如果Mn p或者Ln p大于设定的阀值N max，R t调整如下：
Figure PCTCN2020096031-appb-000038
式(10)中，ω r是一个调整系数；当M 0减少的时候，ω q和ω r减少；反之，ω q和ω r增加。
进一步地，所述并行位姿更新具体包括步骤S351至S354。
S351根据预测的Sigma点和观测方程预测t时刻移动机器人的观测；
Figure PCTCN2020096031-appb-000039
其中，h函数是观察模型，z t是t时刻的观测数据；
Figure PCTCN2020096031-appb-000040
S352：根据式(12)计算出基于预测观测值的协方差矩阵平方根
Figure PCTCN2020096031-appb-000041
QR分解和cholupdate
更新同样应用于
Figure PCTCN2020096031-appb-000042
的更新；
Figure PCTCN2020096031-appb-000043
S353；为了通过观测来更新对机器人位姿的预测值和协方差矩阵的平方根，计算卡尔曼增益K t；如式(13)；
Figure PCTCN2020096031-appb-000044
Figure PCTCN2020096031-appb-000045
S354；最后通过卡尔曼增益更新状态均值和协方差矩阵的平方根；如下式(14)：
Figure PCTCN2020096031-appb-000046
进一步地，所述并行地图更新具体包括步骤S361至S362。
S361：地图特征的均值和方差的估计：
由于路标估计是基于粒子的，每个粒子关联了N个路标的EKF估计:；路标的更新确定于该路标在t时刻有没有被观测到；如果该路标是新近观测的，其均值和协方差初始化如下：
Figure PCTCN2020096031-appb-000047
Figure PCTCN2020096031-appb-000048
式(15)中，h -1是观测函数的逆函数，H rz是函数h -1的雅可比矩阵；如果第j个路标在t时刻没有被观测到，则它的均值和方差保持不变；如果第j个路标在t时刻被观测到，均值和方差更新如下：
Figure PCTCN2020096031-appb-000049
Figure PCTCN2020096031-appb-000050
式(16)中，K t是卡尔曼增益，H z是函数h的雅可比矩阵。
S362：计算权值
权值被定义为目标分布和建议分布的比例；
Figure PCTCN2020096031-appb-000051
Figure PCTCN2020096031-appb-000052
代表t时刻第i个粒子的权值。
一种基于流式计算的地图创建系统，包括移动机器人端和流式计算集群端；所述移动机器人端运行在嵌入式操作系统上；移动机器人端包括四个部分：数据采集模块、地图管理模块、消息网关和消息客户端；
流式计算集群端包括分布式并行计算模块、消息服务模块、分布式存储模块、应用协调模块；流式计算集群端通过集群方式实现高可用和负载均衡的消息服务的消息服务集群模块；通过分布式方式实现移动机器人传感器数据和地图数据的存储的分布式存储模块；通过分布式并行计算节点实现的分布式地图创建模块；实现计算集群中主控节点、N个计算节点之间的协调、监控和管理的应用协调服务模块，N为大于等于1的整数。
3.有益效果：
本发明中采用一种的地图创建的流式计算服务，能够弥补移动机器人地图创建时候机载能力不足和负担过重的短板，提高移动机器人地图创建的精度和效率。
附图说明
图1为本发明的基于流式计算的地图创建方法及其设备架构图；
图2为本发明的基于流式计算的地图创建方法流程图；
图3为本发明的基于流式计算的移动机器人位姿坐标系；
图4为本发明的基于流式计算的移动机器人运动模型；
图5为本发明的基于流式计算的移动机器人激光雷达观测模型；
图6为本发明的移动机器人运动和观测数据实时采集流程图；
图7为本发明的移动机器人运动和观测数据信息实时上传流程图；
图8为本发明的分布式并行平方根卡尔曼滤波粒子滤波算法的流程图；
图9为本发明的先验知识调整流程图。
本发明的最佳实施例
所述步骤三具体包括以下步骤：
S31:取出数据：从消息队列中取出运动数据和观测数据，分发给多个计算节点；所述运动数据包括线速度和舵角；所述观测数据为与一组特征的距离和方向角；并行步骤S32与步骤S34。
S32:并行位姿预测：多个计算节点中的特定计算节点并行进行本步骤；如附图8的分布式并行平方根卡尔曼滤波粒子滤波算法的流程图所示，其中每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子的位姿预测；该步骤首先根据上一时刻当前粒子的状态和协方差矩阵初始化Sigma点，然后通过运动模型更新Sigma点，最后预测新的位姿和协方差矩阵平方根；将计算的结果传输至步骤33。
S33：数据关联：将当前粒子的信息与观测到的环境路标和地图上已标注过的环境路标进行关联；将关联的结果传输至步骤S35。
S34:先验知识调整，如附图9的先验知识调整流程图所示：多个计算节点中的另一部分特定计算节点根据移动机器人适应度函数调整先验知识Q和R，其中Q和R分别代表的是运动误差协方差矩阵和观测误差的协方差矩阵；该步骤是在粒子采样过程中融入先验知识修正的过程，使其能反映真实的先验知识；该步骤将先验知识Q和R设定为动态的，并标记为Q t和R t；调整通过临近观测的不一致性定义关于Q和R的适应度函数，在此适应度函数基础上，通过粒子群优化(PSO)算法搜索出Q和R的一个预估值据预估值，完成Q t和R t的修正；将修正的结果传输至步骤S35。
S35:并行位姿更新，每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子位姿的更新；首先根据预测的Sigma点和观测方程更新观测信息；然后计算出基于预测观测值的协方差矩阵平方根，再通过QR分解和cholupdate函数更新该协方差矩阵平方根；最后计算卡尔曼增益，根据卡尔曼增益更新状态均值和协方差矩阵的平方根。
S36:并行地图更新；地图的估计被分解为M个相互独立特征的估计；通过卡尔曼滤波算法EKF来估计当前粒子的每个特征的均值和方差；最后按照目标分布和建议分布的比例计算该粒子的权值。
S37：将权值最大的粒子作为位姿和环境特征地图返回给移动机器人。
S38：计算有效粒子数N eff，当小于指定的阀值执行粒子集重采样；
Figure PCTCN2020096031-appb-000053
其中，M是粒子数量，
Figure PCTCN2020096031-appb-000054
是第k个粒子归一化的权重；如果N eff小于指定的阀值粒子集会被重采样。重采样过后，所有粒子权重被重置。
步骤S37与步骤S38能够并发执行；
步骤S39：将所有粒子信息存储到分布式数据库中；所述步骤S39与步骤S38能够并发执行。
进一步地，所述并行位姿预测具体包括具体过程包括步骤S321至S322：
S321：计算t-1时刻的Sigma点X t-1，如式(1)：
X t-1＝[x t-1 x t-1+ηS t-1 x t-1-ηS t-1]   (1)
式(1)中：x t-1是t-1时刻该粒子对移动机器人位姿的估计，S t-1是t-1时刻的协方差矩阵平方根，尺度常量η计算如式(2)：
Figure PCTCN2020096031-appb-000055
式(2)中，L为系统状态的维数，λ＝α 2(L+κ)-L为尺度调节因子，常量α决定了Sigma点在均值附近的分布，通常将α设置为1e-4≤α≤1，κ为次级尺度调节因子，一般设置为0；β是用来结合系统状态的先验分布，对于高斯分布，β取值为2。
S322:预测：根据t-1时刻的Sigma点和运动模型预测t时刻的Sigma点
Figure PCTCN2020096031-appb-000056
如式(3)；
Figure PCTCN2020096031-appb-000057
(3)式中g函数描述了机器人的运动模型，u t是t时刻采自惯性导航的将运动数据。
位姿预测的均值和协方差矩阵平方根通过Sigma点来计算；首先通过QR分解来求
Figure PCTCN2020096031-appb-000058
为保证协方差的平方根矩阵
Figure PCTCN2020096031-appb-000059
的半正定性，使用Cholesky一阶更新
Figure PCTCN2020096031-appb-000060
如式(4)
Figure PCTCN2020096031-appb-000061
Figure PCTCN2020096031-appb-000062
Figure PCTCN2020096031-appb-000063
其中：
Figure PCTCN2020096031-appb-000064
为第i个Sigma点均值的权值，
Figure PCTCN2020096031-appb-000065
为第i个Sigma点方差的权值；且
Figure PCTCN2020096031-appb-000066
Figure PCTCN2020096031-appb-000067
根据预测的机器人位姿和协方差矩阵更新Sigma点
Figure PCTCN2020096031-appb-000068
进一步地，所述先验知识调整具体的过程包括步骤S341至S346；
S341：初始化：如果本次或者上次观测的路标数量小于3，不执行Q t和R t的调整；否则将移动机器人上一时刻的位姿
Figure PCTCN2020096031-appb-000069
设为原点，转向角设为0，位姿采用极坐标，地图信息由上一次的观测z t-1表示；即：
Figure PCTCN2020096031-appb-000070
Figure PCTCN2020096031-appb-000071
S342：位姿预测：根据极坐标下的运动模型预测t时刻机器人的位姿
Figure PCTCN2020096031-appb-000072
Figure PCTCN2020096031-appb-000073
(6)式中，f函数是极坐标下的机器人的运动模型。
S343：数据关联：将临近两次观测到的路标进行关联；关联过程使用动态联合最近邻算
法实现数据关联。
S344：通过DFC&ASD-PSO算法搜索最优的参数；
将预测的位姿
Figure PCTCN2020096031-appb-000074
上一时刻Q t-1和R t-1以及观测z t和z t-1作为输入参数，通过DFC&ASD-PSO搜索最优的
Figure PCTCN2020096031-appb-000075
和
Figure PCTCN2020096031-appb-000076
使得适应度函数值尽可能大；
Figure PCTCN2020096031-appb-000077
S345：观测预测：根据机器人位姿和地图信息预测t时刻的观测信息；
Figure PCTCN2020096031-appb-000078
式中s函数是极坐标下的观测模型。
S346：Q t和R t的调整；
根据预测的机器人位姿和通过DFC&ASD-PSO搜索的
Figure PCTCN2020096031-appb-000079
进行对比，当两者之间差值较大的时候，调整Q t和R t；即带入(7)式中：
Figure PCTCN2020096031-appb-000080
Figure PCTCN2020096031-appb-000081
式(7)中，
Figure PCTCN2020096031-appb-000082
是第j个路标的观测值，
Figure PCTCN2020096031-appb-000083
是第j个路标预测的观测值；M o是临近 两次重复观测到的路标数量；使用
Figure PCTCN2020096031-appb-000084
表示运动误差，Dx t～N(0,1)服从正态分布；为了避免频繁地调整，只有当一定概率事件发生的情况下才调整Q t；如果|Dx t|大于1.4或者小于0.2，Q t调整如下：
Figure PCTCN2020096031-appb-000085
否则Q t不调整；
Q t＝Q t-1    (9)
式(8)中，ω q是一个根据观测到的路标数量动态调整的参数；设定
Figure PCTCN2020096031-appb-000086
代表第j个路标的观测误差；Mn表示
Figure PCTCN2020096031-appb-000087
绝对值大于1.4的数量，Ln表示
Figure PCTCN2020096031-appb-000088
绝对值小于0.2的数量；
Figure PCTCN2020096031-appb-000089
如果Mn p或者Ln p大于设定的阀值N max，R t调整如下：
Figure PCTCN2020096031-appb-000090
式(10)中，ω r是一个调整系数；当M 0减少的时候，ω q和ω r减少；反之，ω q和ω r增加。
进一步地，所述并行位姿更新具体包括步骤S351至S354；
S351根据预测的Sigma点和观测方程预测t时刻移动机器人的观测；
Figure PCTCN2020096031-appb-000091
其中，h函数是观察模型，z t是t时刻的观测数据；
Figure PCTCN2020096031-appb-000092
S352：根据式(12)计算出基于预测观测值的协方差矩阵平方根
Figure PCTCN2020096031-appb-000093
QR分解和cholupdate
更新同样应用于
Figure PCTCN2020096031-appb-000094
的更新；
Figure PCTCN2020096031-appb-000095
S353；为了通过观测来更新对机器人位姿的预测值和协方差矩阵的平方根，计算卡尔曼增益K t；如式(13)；
Figure PCTCN2020096031-appb-000096
Figure PCTCN2020096031-appb-000097
S354；最后通过卡尔曼增益更新状态均值和协方差矩阵的平方根；如下式(14)：
Figure PCTCN2020096031-appb-000098
进一步地，所述并行地图更新具体包括步骤S361至S362；
S361：地图特征的均值和方差的估计：
由于路标估计是基于粒子的，每个粒子关联了N个路标的EKF估计:；路标的更新确定于该路标在t时刻有没有被观测到；如果该路标是新近观测的，其均值和协方差初始化如下：
Figure PCTCN2020096031-appb-000099
Figure PCTCN2020096031-appb-000100
式(15)中，h -1是观测函数的逆函数，H rz是函数h -1的雅可比矩阵；如果第j个路标在t时刻没有被观测到，则它的均值和方差保持不变；如果第j个路标在t时刻被观测到，均值和方差更新如下：
Figure PCTCN2020096031-appb-000101
Figure PCTCN2020096031-appb-000102
式(16)中，K t是卡尔曼增益，H z是函数h的雅可比矩阵。
S362：计算权值
权值被定义为目标分布和建议分布的比例；
Figure PCTCN2020096031-appb-000103
Figure PCTCN2020096031-appb-000104
代表t时刻第i个粒子的权值。
发明的具体实施方式
下面结合附图对本发明进行具体的说明。
一种基于流式计算的地图创建方法，应用于移动机器人，其特征在于：移动机器人将采集的数据传输至流式计算集群实现地图创建与更新，如附图1的地图创建方法及其设备架构图所示；具体包括以下步骤：其流程图如附图2的地图创建方法流程图所示；
步骤一：数据采集，如附图6的移动机器人运动和观测数据实时采集流程图所示；所述移动机器人在移动，移动机器人端的激光雷达实时采集移动机器人的观测数据如附图5移动机器人激光雷达观测模型所示；移动机器人端的惯性传感器实时采集运动数据，如附图4的移动机器人运动模型；所述运动数据与观测数据进行预处理后，存于移动机器人端。图3为本发明中的移动机器人位姿坐标系。
步骤二：数据传输；将预处理后的数据封装为消息，通过消息网关提交到流式计算集群 上。如附图7的移动机器人运动和观测数据信息实时上传流程图所示。
步骤三：定位与地图创建；基于流式计算的地图创建方法，通过若干计算节点执行分布式并行的平方根卡尔曼滤波粒子滤波算法获得移动机器人位姿和地图特征信息；从队列中取出消息，将数据分发给计算节点，每个计算节点更新粒子，实现粒子的采样和重采样；最终的定位和地图特征数据发送给移动机器人。
步骤四：定位和地图更新；移动机器人根据返回的定位和地图特征数据，更新全局地图，同时推送给局部路径规划算法辅助移动机器人避障。
一种基于流式计算的地图创建系统，包括移动机器人端和流式计算集群端；所述移动机器人端运行在嵌入式操作系统上；移动机器人端包括四个部分：数据采集模块、地图管理模块、消息网关和消息客户端；
流式计算集群端包括分布式并行计算模块、消息服务模块、分布式存储模块、应用协调模块；流式计算集群端通过集群方式实现高可用和负载均衡的消息服务的消息服务集群模块；通过分布式方式实现移动机器人传感器数据和地图数据的存储的分布式存储模块；通过分布式并行计算节点实现的分布式地图创建模块；实现计算集群中主控节点、N个计算节点之间的协调、监控和管理的应用协调服务模块，N为大于等于1的整数。
虽然本发明已以较佳实施例公开如上，但它们并不是用来限定本发明的，任何熟习此技艺者，在不脱离本发明之精神和范围内，自当可作各种变化或润饰，因此本发明的保护范围应当以本申请的权利要求保护范围所界定的为准。

Claims (7)

  1. 一种基于流式计算的地图创建方法，应用于移动机器人，其特征在于：移动机器人将采集的数据传输至流式计算集群实现地图创建与更新；具体包括以下步骤：
    步骤一：数据采集；所述移动机器人在移动，移动机器人端的激光雷达实时采集移动机器人的观测数据；移动机器人端的惯性传感器实时采集运动数据；所述运动数据与观测数据进行预处理后，存于移动机器人端；
    步骤二：数据传输；将预处理后的数据封装为消息，通过消息网关提交到流式计算集群上；
    步骤三：定位与地图创建；基于流式计算的地图创建方法，通过若干计算节点执行分布式并行的平方根卡尔曼滤波粒子滤波算法获得移动机器人位姿和地图特征信息；从队列中取出消息，将数据分发给计算节点，每个计算节点更新粒子，实现粒子的采样和重采样；最终的定位和地图特征数据发送给移动机器人；
    步骤四：定位和地图更新；移动机器人根据返回的定位和地图特征数据，更新全局地图，同时推送给局部路径规划算法辅助移动机器人避障。
  2. 根据权利要求1所述的一种基于流式计算的地图创建方法，其特征在于：所述步骤三具体包括以下步骤：
    S31:取出数据：从消息队列中取出运动数据和观测数据，分发给多个计算节点；所述运动数据包括线速度和舵角；所述观测数据为与一组特征的距离和方向角；并行步骤S32与步骤S34；
    S32:并行位姿预测：多个计算节点中的特定计算节点并行进行本步骤；其中每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子的位姿预测；该步骤首先根据上一时刻当前粒子的状态和协方差矩阵初始化Sigma点，然后通过运动模型更新Sigma点，最后预测新的位姿和协方差矩阵平方根；将计算的结果传输至步骤33；
    S33：数据关联：将当前粒子的信息与观测到的环境路标和地图上已标注过的环境路标进行关联；将关联的结果传输至步骤S35；
    S34:先验知识调整：多个计算节点中的另一部分特定计算节点根据移动机器人适应度函数调整先验知识Q和R，其中Q和R分别代表的是运动误差协方差矩阵和观测误差的协方差矩阵；该步骤是在粒子采样过程中融入先验知识修正的过程，使其能反映真实的先验知识；该步骤将先验知识Q和R设定为动态的，并标记为Q t和R t；调整通过临近观测的不一致性定义关于Q和R的适应度函数，在此适应度函数基础上，通过粒子群优化(PSO)算法搜索出Q和R的一个预估值据预估值，完成Q t和R t的修正；将修正的结果传输至步骤S35；
    S35:并行位姿更新，每个计算节点采用基于平方根卡尔曼滤波算法实现相应粒子位姿的更新；首先根据预测的Sigma点和观测方程更新观测信息；然后计算出基于预测观测值的协方差矩阵平方根，再通过QR分解和cholupdate函数更新该协方差矩阵平方根；最后计算卡尔曼增 益，根据卡尔曼增益更新状态均值和协方差矩阵的平方根；
    S36:并行地图更新；地图的估计被分解为M个相互独立特征的估计；通过卡尔曼滤波算法EKF来估计当前粒子的每个特征的均值和方差；最后按照目标分布和建议分布的比例计算该粒子的权值；
    S37：将权值最大的粒子作为位姿和环境特征地图返回给移动机器人；
    S38：计算有效粒子数N eff，当小于指定的阀值执行粒子集重采样；
    Figure PCTCN2020096031-appb-100001
    M是粒子数量，
    Figure PCTCN2020096031-appb-100002
    是第k个粒子归一化的权重；如果N eff小于指定的阀值粒子集会被重采样。重采样过后，所有粒子权重被重置；
    步骤S37与步骤S38能够并发执行；
    步骤S39：将所有粒子信息存储到分布式数据库中；所述步骤S39与步骤S38能够并发执行。
  3. 根据权利要求2所述的一种基于流式计算的地图创建方法，其特征在于：所述并行位姿预测具体包括具体过程包括步骤S321至S322：
    S321：计算t-1时刻的Sigma点X t-1，如式(1)：
    X t-1＝[x t-1 x t-1+ηS t-1 x t-1-ηS t-1]  (1)
    式(1)中：x t-1是t-1时刻该粒子对移动机器人位姿的估计，S t-1是t-1时刻的协方差矩阵平方根，尺度常量η计算如式(2)：
    Figure PCTCN2020096031-appb-100003
    式(2)中，L为系统状态的维数，λ＝α 2(L+κ)-L为尺度调节因子，常量α决定了Sigma点在均值附近的分布，通常将α设置为1e-4≤α≤1，κ为次级尺度调节因子，一般设置为0；β是用来结合系统状态的先验分布，对于高斯分布，β取值为2；
    S322:预测：根据t-1时刻的Sigma点和运动模型预测t时刻的Sigma点
    Figure PCTCN2020096031-appb-100004
    如式(3)；
    Figure PCTCN2020096031-appb-100005
    (3)式中g函数描述了机器人的运动模型，u t是t时刻采自惯性导航的将运动数据；
    位姿预测的均值和协方差矩阵平方根通过Sigma点来计算；首先通过QR分解来求
    Figure PCTCN2020096031-appb-100006
    为保证协方差的平方根矩阵
    Figure PCTCN2020096031-appb-100007
    的半正定性，使用Cholesky一阶更新
    Figure PCTCN2020096031-appb-100008
    如式(4)
    Figure PCTCN2020096031-appb-100009
    Figure PCTCN2020096031-appb-100010
    Figure PCTCN2020096031-appb-100011
    其中：
    Figure PCTCN2020096031-appb-100012
    为第i个Sigma点均值的权值，
    Figure PCTCN2020096031-appb-100013
    为第i个Sigma点方差的权值；且
    Figure PCTCN2020096031-appb-100014
    Figure PCTCN2020096031-appb-100015
    根据预测的机器人位姿和协方差矩阵更新Sigma点
    Figure PCTCN2020096031-appb-100016
  4. 根据权利要求3所述的一种基于流式计算的地图创建方法，其特征在于：所述先验知识调整具体的过程包括步骤S341至S346；
    S341：初始化：如果本次或者上次观测的路标数量小于3，不执行Q t和R t的调整；否则将移动机器人上一时刻的位姿
    Figure PCTCN2020096031-appb-100017
    设为原点，转向角设为0，位姿采用极坐标，地图信息由上一次的观测z t-1表示；即：
    Figure PCTCN2020096031-appb-100018
    Figure PCTCN2020096031-appb-100019
    S342：位姿预测：根据极坐标下的运动模型预测t时刻机器人的位姿
    Figure PCTCN2020096031-appb-100020
    Figure PCTCN2020096031-appb-100021
    (6)式中，f函数是极坐标下的机器人的运动模型；
    S343：数据关联：将临近两次观测到的路标进行关联；关联过程使用动态联合最近邻算法实现数据关联；
    S344：通过DFC&ASD-PSO算法搜索最优的参数；
    将预测的位姿
    Figure PCTCN2020096031-appb-100022
    上一时刻Q t-1和R t-1以及观测z t和z t-1作为输入参数，通过DFC&ASD-PSO搜索最优的
    Figure PCTCN2020096031-appb-100023
    和
    Figure PCTCN2020096031-appb-100024
    使得适应度函数值尽可能大；
    Figure PCTCN2020096031-appb-100025
    S345：观测预测：根据机器人位姿和地图信息预测t时刻的观测信息；
    Figure PCTCN2020096031-appb-100026
    式中s函数是极坐标下的观测模型；
    S346：Q t和R t的调整；
    根据预测的机器人位姿和通过DFC&ASD-PSO搜索的
    Figure PCTCN2020096031-appb-100027
    进行对比，当两者之间差值较大的时候，调整Q t和R t；即带入(7)式中：
    Figure PCTCN2020096031-appb-100028
    Figure PCTCN2020096031-appb-100029
    式(7)中，
    Figure PCTCN2020096031-appb-100030
    是第j个路标的观测值，
    Figure PCTCN2020096031-appb-100031
    是第j个路标预测的观测值；M o是临近两次重复观测到的路标数量；使用
    Figure PCTCN2020096031-appb-100032
    表示运动误差，Dx t～N(0,1)服从正态分布；为了避免频繁地调整，只有当一定概率事件发生的情况下才调整Q t；如果|Dx t|大于1.4或者小于0.2，Q t调整如下：
    Figure PCTCN2020096031-appb-100033
    否则Q t不调整；
    Q t＝Q t-1    (9)
    式(8)中，ω q是一个根据观测到的路标数量动态调整的参数；设定
    Figure PCTCN2020096031-appb-100034
    代表第j个路标的观测误差；Mn表示
    Figure PCTCN2020096031-appb-100035
    绝对值大于1.4的数量，Ln表示
    Figure PCTCN2020096031-appb-100036
    绝对值小于0.2的数量；
    Figure PCTCN2020096031-appb-100037
    如果Mn p或者Ln p大于设定的阀值N max，R t调整如下：
    Figure PCTCN2020096031-appb-100038
    式(10)中，ω r是一个调整系数；当M 0减少的时候，ω q和ω r减少；反之，ω q和ω r增加。
  5. 根据权利要求4所述的一种基于流式计算的地图创建方法，其特征在于：所述并行位姿更新具体包括步骤S351至S354；
    S351根据预测的Sigma点和观测方程预测t时刻移动机器人的观测；
    Figure PCTCN2020096031-appb-100039
    其中，h函数是观察模型，z t是t时刻的观测数据；
    Figure PCTCN2020096031-appb-100040
    S352：根据式(12)计算出基于预测观测值的协方差矩阵平方根
    Figure PCTCN2020096031-appb-100041
    QR分解和cholupdate更新同样应用于
    Figure PCTCN2020096031-appb-100042
    的更新；
    Figure PCTCN2020096031-appb-100043
    S353；为了通过观测来更新对机器人位姿的预测值和协方差矩阵的平方根，计算卡尔曼增益 K t；如式(13)；
    Figure PCTCN2020096031-appb-100044
    Figure PCTCN2020096031-appb-100045
    S354；最后通过卡尔曼增益更新状态均值和协方差矩阵的平方根；如下式(14)：
    Figure PCTCN2020096031-appb-100046
  6. 根据权利要求5所述的一种基于流式计算的地图创建方法，其特征在于：所述并行地图更新具体包括步骤S361至S362；
    S361：地图特征的均值和方差的估计：
    由于路标估计是基于粒子的，每个粒子关联了N个路标的EKF估计:；路标的更新确定于该路标在t时刻有没有被观测到；如果该路标是新近观测的，其均值和协方差初始化如下：
    Figure PCTCN2020096031-appb-100047
    式(15)中，h -1是观测函数的逆函数，H rz是函数h -1的雅可比矩阵；如果第j个路标在t时刻没有被观测到，则它的均值和方差保持不变；如果第j个路标在t时刻被观测到，均值和方差更新如下：
    Figure PCTCN2020096031-appb-100048
    式(16)中，K t是卡尔曼增益，H z是函数h的雅可比矩阵；
    S362：计算权值
    权值被定义为目标分布和建议分布的比例；
    Figure PCTCN2020096031-appb-100049
    Figure PCTCN2020096031-appb-100050
    代表t时刻第i个粒子的权值。
  7. 一种基于流式计算的地图创建系统，使用如权利要求1至6任一权利要求所述的一种基于流式计算的地图创建方法，其特征在于：包括移动机器人端和流式计算集群端；所述移动机器人端运行在嵌入式操作系统上；移动机器人端包括四个部分：数据采集模块、地图管理模块、消息网关和消息客户端；
    流式计算集群端包括分布式并行计算模块、消息服务模块、分布式存储模块、应用协调 模块；流式计算集群端通过集群方式实现高可用和负载均衡的消息服务的消息服务集群模块；通过分布式方式实现移动机器人传感器数据和地图数据的存储的分布式存储模块；通过分布式并行计算节点实现的分布式地图创建模块；实现计算集群中主控节点、N个计算节点之间的协调、监控和管理的应用协调服务模块，N为大于等于1的整数。
PCT/CN2020/096031 2019-10-25 2020-06-15 一种基于流式计算的地图创建方法及其系统 WO2021077769A1 (zh)

Applications Claiming Priority (2)

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

Publications (1)

Publication Number Publication Date
WO2021077769A1 true WO2021077769A1 (zh) 2021-04-29

Family

ID=69333693

Family Applications (1)

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

Country Status (2)

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

Families Citing this family (5)

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

Citations (4)

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

Patent Citations (4)

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

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
LIANG YI, HOU YING; CHEN CHENG; JIN YI: "A survey of task management techniques for big data stream computing", COMPUTER ENGINEERING AND SCIENCE, GUOFANG KEJI DAXUE JISUANJI XUEYUAN, CN, vol. 39, no. 2, 1 February 2017 (2017-02-01), CN, pages 215 - 226, XP055803757, ISSN: 1007-130X *
LÜ TAI-ZHI, ZHOU WU, ZHAO CHUN-XIA: "An Improved UKF-SLAM Algorithm", JOURNAL OF NORTH UNIVERSITY OF CHINA(NATURAL SCIENCE EDITION), vol. 39, no. 6, 1 January 2018 (2018-01-01), pages 717 - 751, XP055803754, ISSN: 1673-3193, DOI: 10.3969/j.issn.1673-3193.2018.06.014 *

Also Published As

Publication number Publication date
CN110763245A (zh) 2020-02-07

Similar Documents

Publication Publication Date Title
WO2021077769A1 (zh) 一种基于流式计算的地图创建方法及其系统
US11727812B2 (en) Airplane flight path planning method and device based on the pigeon-inspired optimization
Moon et al. Deep reinforcement learning multi-UAV trajectory control for target tracking
CN112511250B (zh) 一种基于drl的多无人机空中基站动态部署方法及系统
CN112068154A (zh) 一种激光建图定位方法、装置、存储介质及电子设备
Ma et al. Adaptive vision-based guidance law with guaranteed performance bounds
CN109491241B (zh) 一种针对机动目标的无人机鲁棒化跟踪方法
CN110580740A (zh) 多智能体协同三维建模方法及装置
Tian et al. Resilient and distributed multi-robot visual slam: Datasets, experiments, and lessons learned
CN113701742B (zh) 一种基于云端与边端融合计算的移动机器人slam方法
Zhao et al. Review of slam techniques for autonomous underwater vehicles
Zhang et al. Distributed control of nonholonomic robots without global position measurements subject to unknown slippage constraints
Wang et al. H-SwarmLoc: Efficient Scheduling for Localization of Heterogeneous MAV Swarm with Deep Reinforcement Learning
Chen et al. Real-time unmanned aerial vehicle flight path prediction using a bi-directional long short-term memory network with error compensation
Li et al. A warm-started trajectory planner for fixed-wing unmanned aerial vehicle formation
CN109900272B (zh) 视觉定位与建图方法、装置及电子设备
Lv et al. A SLAM algorithm based on edge-cloud collaborative computing
Wang et al. Tracking moving target for 6 degree-of-freedom robot manipulator with adaptive visual servoing based on deep reinforcement learning PID controller
Saito et al. Performance evaluation of a DQN-based autonomous aerial vehicle mobility control method in an indoor single-path environment with a staircase
Zheng et al. A method for UAV tracking target in obstacle environment
Li et al. Three‐dimensional dead reckoning of wall‐climbing robot based on information fusion of compound extended Kalman filter
CN117608199B (zh) 一种预瞄机制下网络化多机器人数据驱动编队控制方法
Li et al. Image-based visual servoing for quadrotor helicopters using genetic algorithm
Liu et al. Research on simultaneous localization and mapping of indoor mobile robot
Luo et al. Research on Path Planning Based on the Integration of Binocular Camera and IMU Mileage Calculation Method

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 20879147

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 20879147

Country of ref document: EP

Kind code of ref document: A1