CN113674412A - Indoor map construction method, system and storage medium based on pose fusion optimization - Google Patents

Abstract

The invention provides an indoor map construction method, system and storage medium based on pose fusion optimization. The method includes: calculating the pose data of a robot according to real-time data obtained by KINECT and IMU; If the robot is in a static state, the extended Kalman filtering algorithm is used to fuse the pose data; if the robot is in motion, the pose data is processed by a dynamic weighting method. Fusion processing; build an indoor map according to the fusion processing results. This method has higher pose estimation accuracy, higher 2D map modeling accuracy and better modeling effect, and can be applied to scenes with poor features, high dynamics, and low light and shadow.

Description

Indoor map construction method, system and storage medium based on pose fusion optimization

technical field

The invention relates to an indoor map construction method, in particular to an indoor map construction based on KINECT/IMU pose fusion optimization in a scene with poor features, high dynamics and low light and shadow in the absence of satellite signals in the indoor environment. Method, system and storage medium.

Background technique

An accurate and efficient pose estimation and mapping system is the key to the robot's motion control and path planning. In the absence of satellite signals in the indoor environment, Simultaneous Localization and Mapping (SLAM) technology is the solution to the robot's environmental cognition. One of the effective means of navigation with positioning. SLAM refers to a process in which a subject equipped with a specific sensor (usually a robot, etc.) builds an environmental model in motion and estimates its own motion without prior information about the environment. According to the different sensors, SLAM can be divided into visual SLAM, laser SLAM and multi-sensor fusion SLAM. SLAM based on visual sensor (Visual SLAM) is mainly composed of sensor information reading, visual odometry, back-end optimization, loop detection and mapping. With the development of computer vision, image processing, artificial intelligence and other technologies, the usability of visual SLAM is constantly being explored, and attempts are made to integrate other sensor information to build a more robust visual SLAM system. With the emergence of RGB-D cameras, many scholars have proposed visual SLAM methods based on KINECT, and applied them to the fields of 3D modeling, human recognition, and system interaction.

However, when the visual SLAM method is used in scenes with poor features, high dynamics, and low light and shadow, the algorithm is subject to many limitations. Due to its own characteristics, IMU sensing (inertial sensor) has formed a good complementarity with the visual SLAM method. For example, when the vision sensor is moving at high speed, the success rate of pose estimation based on feature matching is low, while the IMU can still maintain a more accurate pose estimation by relying on its own acceleration and gyroscope; the vision sensor can effectively limit the IMU during the movement process. The existing cumulative drift error; the fusion of camera data and IMU data can improve the robustness of the pose estimation algorithm. This method of combining cameras and IMUs is called Visual-Inertial SLAM (VI-SLAM). Similar to visual SLAM, the VI-SLAM framework can be divided into two types: filtering-based and optimization-based. Filter-based VI-SLAM methods take states and parameters as part of online computation, input different filtering frameworks, update calibrations and output real-time poses. Optimization-based VI-SLAM methods are also called smoothing methods. They take IMU data as prior information, extract matching image features, define reprojection errors based on image features, construct cost functions, and solve robot poses. Many scholars have conducted research on this aspect: the SSF (Single-Sensor Fusion) algorithm designed by Weiss et al. takes the Extended Kalman Filter (EKF) as the basic framework, uses the pose calculated by the IMU as the prediction function, and uses the PTAM algorithm as the prediction function. The calculated pose is used as the measurement data, which effectively improves the performance of the algorithm, and when the visual solution of the pose fails for a short time, the motion prediction of the IMU in the algorithm can still provide accurate estimates. Liu Zhenbin et al. used an improved nonlinear method to optimize the monocular inertial navigation SLAM scheme. The scheme is based on the VINS-Mono system. The initialization of the acceleration Bayesian bias is added in the initialization process, and the ORB-SLAM is used for reference. (A fast feature point extraction and description algorithm) Feature points perform feature detection and matching on images at the front end, which not only improves the robustness of the scheme, but also achieves high-precision real-time positioning and sparse map construction. Campos et al. proposed a visual inertial navigation-based SLAM system VIORB-SLAM, which can support multiple camera types such as monocular, binocular, and RGBD. The VI ORB-SLAM system designs a hybrid map data association mechanism based on two sensors, which can correlate data collected by sensors in the short-term, mid-term and long-term. At the same time, based on the feature-based tightly coupled VIO system, the VI ORB-SLAM algorithm relies on maximum a posteriori estimation to perform geometric consistency detection on key frames, which improves the accuracy of map positioning.

However, although the pose estimation of the fusion of two sensor data can effectively improve the estimation accuracy, it also has some technical defects: the pose is fused in a loosely coupled way, and the subject still uses the vision algorithm to estimate the robot pose independently, and the IMU The measurement data is only added to some independent pose estimation processes, which directly causes the correlation between the camera and the internal state of the IMU to be ignored, lacks effective constraints and compensation for the pose estimation, and even causes problems between the camera and the IMU. Therefore, the loosely coupled method cannot achieve the optimal pose estimation accuracy; the tightly coupled method is to fuse the vision algorithm and IMU data together to jointly construct the motion equation and the state equation, which can improve the accuracy of the robot. The pose state is solved, but the tight coupling method of global optimization or local smoothing is overly dependent on the initialization pose estimation, and the process of calculating the camera and IMU data is more complicated and lacks real-time performance; at the same time, the monocular camera The scale information cannot be obtained directly, and it can only rely on the scale information calculated by the IMU. The tight coupling method cannot effectively enhance the estimation accuracy of the pose scale. In addition, the Markov property of the filtering-based VI-SLAM method also needs to be considered, that is, the relationship between the state at a certain moment and the state at all previous moments cannot be determined, the algorithm framework has defects, and the space for accuracy improvement is limited.

SUMMARY OF THE INVENTION

In order to solve the above technical problems, the purpose of the present invention is to provide a method for indoor map construction based on KINECT/IMU pose fusion optimization, the method has higher pose estimation accuracy, higher two-dimensional map modeling accuracy and Better modeling effects can be applied to scenes with poor features, high dynamics, and low light and shadow.

Based on the above purpose, one aspect of the present invention provides an indoor map construction method based on pose fusion optimization, the method comprising:

Calculate the pose data of the robot according to the real-time data obtained by KINECT and IMU;

Get the current motion state of the robot;

If the robot is in a static state, the extended Kalman filter algorithm is used to fuse the pose data, and if the robot is in a motion state, a dynamic weighting method is used to fuse the pose data;

Build an indoor map based on the fusion processing results.

Preferably, the specific method for calculating the robot pose data using the real-time data obtained by KINECT is: obtaining image data; matching the feature points of the same name of two partially overlapping images to obtain the corresponding relationship between the feature points of the same name, the The correspondence corresponds to the robot pose.

Preferably, the specific method for calculating the robot pose data using the real-time data obtained by the IMU is: adopting a direction cosine matrix algorithm to calculate the pose.

Preferably, the method for obtaining the current motion state of the robot is:

If the linear acceleration of the IMU is 0 and the odometer data does not increase, it is determined that the robot is currently in a stationary state;

If the linear acceleration of the IMU is not 0, and/or the odometer data increases, it is determined that the robot is currently in motion.

Preferably, the specific method of using the extended Kalman filter algorithm to fuse the pose data is as follows:

Obtain the system state quantity, calculate the error state prediction value and the system state noise covariance matrix;

The observation matrix and noise covariance matrix are obtained by calculation, and the gain matrix of the system is calculated;

From the gain matrix, update the system state quantity and the system covariance matrix;

Normalize the quaternion calculated by the system state quantity;

The quaternion is converted into Euler angles for convenient pose display to represent the pose state of the robot.

Preferably, the method of using the dynamic weighting method to fuse the pose data is as follows:

Obtain the corresponding image according to the key frame through KINECT;

The weight threshold is set according to the matching success rate μ of the feature points between the two key frames obtained by KINECT; if the matching success rate μ decreases, the weight of the IMU attitude is increased; if the matching success rate μ increases, the weight of the KINECT attitude is increased .

Preferably, the specific method for creating an indoor map according to the fusion processing result is:

Obtain the relative coordinates of the laser point and the radar itself after calculating the distance and azimuth angle;

Replace the pose estimation in the mapping algorithm with the pose estimation after fusion optimization in advance, and use the pose estimation as a new data source to calculate the relative relationship between the laser point and the radar, so as to update the distance and azimuth information of the laser point;

Using the Gauss-Newton-based scan matching method to match the coordinates of the laser points, a high-precision indoor two-dimensional map is established.

Another aspect of the present invention provides a kind of system that adopts the above-mentioned indoor map construction method based on pose fusion optimization to carry out indoor map construction, it is characterized in that, comprising:

The pose data calculation unit calculates the pose data of the robot according to the real-time data obtained by KINECT and IMU;

The motion state judgment unit judges the current motion state of the robot according to the linear acceleration data and odometer data of the IMU;

The pose data fusion processing unit is respectively connected with the pose data calculation unit and the motion state judgment unit. When the robot is in a static state, the extended Kalman filtering algorithm is used to fuse the pose data. When the robot is in a motion state When , adopt the dynamic weighting method to perform fusion processing on the pose data;

The two-dimensional map construction unit is connected with the pose data fusion processing unit, and creates an indoor map according to the fusion processing result.

Preferably, the pose data calculation unit includes:

The KINECT data solving subunit is used to obtain image data; it matches the feature points of the same name of two partially overlapping images to obtain the corresponding relationship between the feature points of the same name, and the corresponding relationship corresponds to the pose of the robot.

The IMU data solving sub-unit uses the direction cosine matrix algorithm to solve the pose and attitude of the real-time data obtained by the IMU.

In yet another aspect of the present invention, a storage medium is provided, and a computer program is stored in the storage medium, and when the computer program is processed and executed, the above-mentioned indoor map construction method based on pose fusion optimization is realized.

Compared with the prior art, the beneficial effects of the present invention are:

This method firstly determines the motion state of the robot, and uses the extended Kalman filter algorithm to fuse the pose data for the stationary state of the robot. The fused pose estimation accuracy makes the two-dimensional map modeling accuracy better and the modeling effect better. This method can effectively improve the pose estimation accuracy in scenes with poor features, high dynamics, and low light and shadow.

Description of drawings

The accompanying drawings forming a part of the present application are used to provide further understanding of the present application, and the schematic embodiments and descriptions of the present application are used to explain the present application and do not constitute a limitation to the present application.

1 is a flowchart of an indoor map construction method based on pose fusion optimization in an embodiment of the present invention;

2(a)-(c) are respectively a schematic diagram of selection and matching of feature points of aisles, corners, and tables and chairs in the embodiment of the present invention;

Fig. 3 is a part of natural scenes and artificial layout scenes in the embodiment of the present invention;

4 is the relationship between the matching success rate and the pose error in the embodiment of the present invention;

Fig. 5 is the robot platform and sensor built in the embodiment of the present invention;

Fig. 6 is the displacement deviation diagram of x, y, z direction in the embodiment of the present invention;

Fig. 7 is the displacement deviation diagram of x, y direction in the embodiment of the present invention;

Figure 8(a)-(d) is a comparison of local mapping effects in the embodiment of the present invention;

Fig. 9 (a) is the two-dimensional map constructed by Cartographer algorithm in the embodiment of the present invention;

Fig. 9(b) is a two-dimensional map constructed by the algorithm of the present invention in an embodiment of the present invention.

Detailed ways

The present invention will be further described below with reference to the accompanying drawings and embodiments.

It should be noted that the following detailed description is exemplary and intended to provide further explanation of the application. Unless otherwise defined, all technical and scientific terms used in the examples have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

It should be noted that the terminology used herein is for describing specific embodiments only, and is not intended to limit the exemplary embodiments according to the present application. As used herein, unless the context clearly dictates otherwise, the singular is intended to include the plural as well, and it should also be understood that when the terms "comprising" and/or "including" are used in this specification, it indicates that There are features, steps, operations, devices, components and/or combinations thereof.

This embodiment provides an indoor map construction method based on pose fusion optimization, as shown in FIG. 1 , the method includes:

Calculate the pose data of the robot according to the real-time data obtained by KINECT and IMU;

Get the current motion state of the robot;

If the robot is in a static state, the extended Kalman filter algorithm is used to fuse the pose data, and if the robot is in a motion state, a dynamic weighting method is used to fuse the pose data;

Build an indoor map based on the fusion processing results.

As a preferred embodiment, the specific method for calculating the robot pose data using the real-time data obtained by KINECT is as follows: obtaining image data; matching the feature points of the same name of two partially overlapping images to obtain one of the feature points of the same name. The corresponding relationship between the corresponding relationship and the robot pose. Specifically: Solving the pose of the robot based on the image data obtained by KINECT is usually achieved by matching the feature points of the same name in two partially overlapping images. First, the Oriented-Fast algorithm is used to extract image feature points; then, the Fast Library for Approximate Nearest Neighbors (FLANN) method is used to achieve matching between feature points. According to the corresponding relationship between the feature points with the same name in the two images, the camera motion parameters are calculated. For example: Assuming that the rigid body transformation of camera motion is (R, T), where R is the rotation vector and T is the translation vector, the relationship between the coordinates corresponding to the feature points and the motion process is shown in formula (1).

P _pm =RP _lm +T (1)

Among them, P _pm and RP _lm are the three-dimensional coordinates of the m-th feature point of the current frame image and the previous frame image, m=1, 2, 3...n. When using the nearest neighbor method to match feature points, it is necessary to eliminate the incorrectly matched feature point pairs, randomly select at least 4 pairs of feature points, calculate the rigid body transformation (R, T), and substitute its coordinates into ||P _pm -(RP _lm + T)|| ² , filter the points smaller than the threshold and substitute them into formula (1) to solve. Finally, the least square value of the relationship between the corresponding coordinates of the feature points and the motion process is obtained through iterative calculation, which is used to estimate the current pose of the robot.

As a preferred embodiment, the specific method for calculating the robot pose data using the real-time data obtained by the IMU is: adopting the direction cosine matrix algorithm to calculate the pose. specifically:

The IMU mainly includes a three-axis accelerometer and a three-axis gyroscope. By quadratic integration of the linear velocity measured by the accelerometer, the speed and position data of the IMU can be obtained; attitude data. In order to avoid the problem of singularity in the Euler angle differential equation when the pitch angle is 90°, resulting in the inability to obtain the full attitude solution of the IMU, this embodiment adopts the direction cosine matrix algorithm (Direction Cosine Matrix, DCM) to use the vector The direction cosine represents the attitude matrix, which effectively avoids the singularity problem encountered by using Euler angles to represent the attitude. Specifically, in this embodiment, the DCM algorithm is used to calculate the attitude of the IMU, and the DCM matrix is defined as C, as shown in formula (2) shown.

Among them, i′, j′, k′ are the unit vectors of the robot coordinates. Formula (3) can be obtained by derivation of i′, j′, k′.

如式(4)所示。Then take the derivative of the matrix C to get

As shown in formula (4).

Solve the equation (4) to obtain the relative pose transformation relationship between the IMU and the robot.

那么，IMU测量模型如式(5)所示。In addition, since the high measurement frequency of the IMU takes up a lot of computing resources in practice, in order to avoid adding a new state quantity during each IMU measurement, a re-parameterization process is usually added between two frames to achieve Motion constraints, avoiding repeated integration, this reparameterization process is called IMU pre-integration. Specifically: Assume that the acceleration and angular velocity measured by the IMU are expressed as

Then, the IMU measurement model is shown in equation (5).

为世界坐标系到IMU坐标系的旋转矩阵，w^α(t)为世界坐标系下的瞬时加速度，b_g(t)、 b_α(t)为陀螺仪与加速度的偏差，η_g(t)、η_α(t)为随机噪声。引入动力学积分模型，模型包含旋 转关系

速度

和平移

如式(6)所示。Among them, W is the world coordinate system, B is the IMU coordinate system, _B ω _WB (t) is the instantaneous angular velocity of B relative to W,

is the rotation matrix from the world coordinate system to the IMU coordinate system, w ^α (t) is the instantaneous acceleration in the world coordinate system, b _g (t), b _α (t) are the deviation between the gyroscope and the acceleration, η _g (t) , η _α (t) are random noises. Introduce the dynamic integral model, the model includes the rotation relationship

speed

and panning

As shown in formula (6).

得到IMU 在Δt时间内相对于世界坐标系的运动状态R_WB(t+Δt)、_Wv(t+Δt)和_Wp(t+Δt)，即两个时刻 IMU数据之间的相对关系。IMU与KINECT的更新率并不同步，积分两个关键帧之间的多 帧IMU数据可以有效约束关键帧。考虑到旋转矩阵

随着时间的不断变化，因此相对运动 增量ΔR_ij、Δv_ij、Ap_ij，如式(7)所示。Integrating Equation (6) can obtain the rotation amount R _WB (t+Δt), the velocity amount _w v(t+Δt) and the translation amount _W P(t+Δt) _W P(t+Δt) in the Δt time, Combine acceleration and angular velocity in the IMU measurement model

Obtain the motion state R _WB (t+Δt), _W v(t+Δt) and _W p(t+Δt) of the IMU relative to the world coordinate system within the time Δt, that is, the relative relationship between the IMU data at two moments. The update rates of IMU and KINECT are not synchronized, and integrating multi-frame IMU data between two key frames can effectively constrain key frames. Taking into account the rotation matrix

As time changes, the relative motion increments ΔR _ij , Δv _ij , and Ap _ij are shown in equation (7).

式(7)左侧为相对运动增量，右 侧为IMU数据，进一步将两个关键帧的相对运动增量ΔR_ij、Δv_ij、Δp_ij表示为式(8)。in,

The left side of equation (7) is the relative motion increment, and the right side is the IMU data, and the relative motion increments ΔR _ij , Δv _ij , and Δp _ij of the two key frames are further expressed as equation (8).

In this way, the relative motion increments ΔR _ij , Δv _ij , and Δp _ij between the two frames are obtained, that is, the result of re-parameterization between the two frames using the IMU pre-integration, which avoids repeated integration of the IMU observations and improves the performance of the algorithm. reliability.

As a preferred embodiment, the method for obtaining the current motion state of the robot is:

If the linear acceleration of the IMU is 0 and the odometer data does not increase, it is determined that the robot is currently in a stationary state;

If the linear acceleration of the IMU is not 0, or the odometer data increases, it is determined that the robot is currently in motion.

As a kind of preferred embodiment, the concrete method that adopts extended Kalman filter algorithm to carry out fusion processing to described pose data is:

Obtain the system state quantity, calculate the error state prediction value and the system state noise covariance matrix;

The observation matrix and noise covariance matrix are obtained by calculation, and the gain matrix of the system is calculated;

From the gain matrix, update the system state quantity and the system covariance matrix;

Normalize the quaternion calculated by the system state quantity;

The quaternion is converted into Euler angles for convenient pose display to represent the pose state of the robot.

specifically:

如式 (9)所示。In order to estimate the pose information of the robot more accurately, the extended Kalman filter can be used to fuse the IMU and KINECT poses. EKF is divided into prediction process and update process. For the prediction process, define the error state vector

As shown in formula (9).

为状态向量期望值。同时，使用

表示状态噪声n，根据误差状态向 量

的定义，线性化处理IMU测量得到的加速度与角速度，得到误差状态方程中平移、旋转、 速度、加速度计和陀螺零漂的表示量

参照系统的状态方程，通过 雅克比矩阵求解状态转移矩阵

如式(10)所示。in,

is the expected value of the state vector. use simultaneously

represents the state noise n, according to the error state vector

The definition of , linearizes the acceleration and angular velocity measured by the IMU, and obtains the representations of translation, rotation, velocity, accelerometer and gyro zero drift in the error state equation

Referring to the state equation of the system, solve the state transition matrix by the Jacobian matrix

As shown in formula (10).

可以求出系统噪声协方差矩阵Qd，进一步计算扩 展卡尔曼滤波的误差状态预测值X_k+1|k和系统状态协方差矩阵预测值P_k+1|k，如式(11)所示。According to the system noise matrix G and the state transition matrix

The system noise covariance matrix Qd can be obtained, and the error state prediction value X _k+1|k of the extended Kalman filter and the system state covariance matrix prediction value P _k+1|k can be further calculated, as shown in equation (11).

系统观测方程z_p、z_q，如式(12)所示。Among them, X _k|k and P _{k|k are} the optimal estimated values at the previous moment. Over time, the IMU error accumulates, and the KINECT pose of the low-frequency output needs to be used as the observed value z of the filter to correct the result of the IMU solution. Generally speaking, the pose transformation matrix of IMU and KINECT relative to the robot chassis has been determined, so the rotation and translation matrix between IMU and KINECT camera can be determined

The system observation equations z _p and z _q are shown in formula (12).

The update process of the EKF can be derived from the observation matrix and the observation noise covariance matrix, and the gain matrix K of the system is calculated, as shown in equation (13).

K=P _k+1|k H ^T (HP _k+1|k H ^T +R) ^-1 (13)

After the gain matrix K is obtained, the system state quantity X _k+1|k+1 is updated, as shown in equation (14).

Update the system covariance matrix P _k+1|k+1 , as shown in equation (15).

P _k+1|k+1 =(I ₁₅ -KH)P _k+1|k (I ₁₅ -KH) ^T +KRK ^T (15)

After updating the system state quantity X _k+1|k+1 and the system covariance matrix P _k+1|k+1 , normalize the quaternion calculated by the system state quantity, and then convert the quaternion into The Euler angle for convenient pose display is used to represent the pose state of the robot, and the pose output after the extended Kalman filter is fused with sensor data is realized.

As a preferred embodiment, the method of using the dynamic weighting method to fuse the pose data is as follows:

Obtain the corresponding image according to the key frame through KINECT;

The weight threshold is set according to the matching success rate μ of the feature points between the two key frames obtained by KINECT; if the matching success rate μ decreases, the weight of the IMU attitude is increased; if the matching success rate μ increases, the weight of the KINECT attitude is increased .

Specifically: the fusion mode of the pose depends on the pose state of the robot, and the pose state of the robot is divided into two types: motion and static, and the present embodiment uses IMU data and odometer data as the conditions for judging motion and static of the robot. If the linear acceleration of the IMU is 0 and the odometer data does not increase, it can be determined that the robot is in a static state, and the fusion of the pose and the pose directly uses the extended Kalman filter method to fuse the pose of the KINECT and the IMU; if the linear acceleration of the IMU is not 0 And/or the odometer data has increased, then it can be determined that the robot is in a motion state. During the motion of the robot, the texture features of the images acquired by KINECT have great uncertainty, and although the IMU errors accumulate, they are relatively stable. Therefore, in the process of pose fusion, the weight relationship between the two can be properly considered, and the matching success rate of key frame feature points can be analyzed to determine the degree of dependence of KINECT and IMU poses on feature point matching, and then achieve different pose orientations. The state adopts different weighting strategies to fuse the IMU and KINECT poses.

When the robot is in motion, the measured values of the IMU include linear velocity, acceleration and angular velocity, and KINECT obtains the corresponding image according to the key frame. The weight threshold is set according to the success rate of feature point matching between the key frames obtained by two KINECT; if the matching success rate is low, the correlation between the current pose and the KINECT pose is low, and the weight of the IMU pose should be increased; if the matching success rate is high , then the relationship between the current pose and the KINECT pose is high, and the weight of the KINECT pose should be increased.

Assuming that the ratio of the successfully matched feature points of two key frames to the total feature points is defined as the matching success rate μ, the key to weighted fusion pose is to determine the weight value of KINECT and IMU pose fusion according to the value range of μ, That is, the weight parameters of the pose weighted fusion.

As a preferred embodiment, the specific method for creating an indoor map according to the fusion processing result is as follows:

Obtain the relative coordinates of the laser point and the radar itself after calculating the distance and azimuth angle;

Replace the pose estimation in the mapping algorithm with the pose estimation after fusion optimization in advance, and use the pose estimation as a new data source to calculate the relative relationship between the laser point and the radar, so as to update the distance and azimuth information of the laser point;

Using the Gauss-Newton-based scan matching method to match the coordinates of the laser points, a high-precision indoor two-dimensional map is established.

During the construction of indoor 2D maps, unstructured scenes may cause the composition plane of the 2D lidar to tilt, reducing the quality of the mapping. In this embodiment, the laser triangulation technology is used to realize LIDAR mapping. The measured laser point and the LIDAR itself obtain relative coordinates after distance and azimuth angle calculation. Before composition, the optimized pose estimation is replaced in the mapping algorithm. Then use the Gauss-Newton-based scan matching method to match the coordinates of the laser points to establish a high-precision indoor 2D map.

Most of the indoor environments have a vertical structure, and the oblique radar data needs to be approximated projected in the experiment. The laser data (ρ, θ) obtained by scanning is expressed as (P _x , P _y , P _z ) in the form of map coordinates, as shown in equation (16).

Among them, (θ _x , θ _y , θ _z ) represent roll angle, pitch angle, and yaw angle. The coordinates (X, Y, Z) of the scanned laser spot are obtained according to the initial pose and angle transformation, as shown in formula (17).

After corresponding the optimized pose estimation and the relative coordinates of the laser points, the Gauss-Newton method is used to match the laser points to improve the mapping accuracy and effect. First, take the minimum value T ^* for the rigid body transformation T, as shown in equation (18).

Among them, S _i (T) is the plane coordinate of the laser spot, G(S _i (T)) is the occupancy value of S _i (T), when all laser spots are completely matched, the value of G(S _i (T)) is 1. Then optimize the radar data measurement error for a period of time, as shown in equation (19):

Through Taylor expansion and partial derivatives, the solution of ΔT is transformed into a Gauss-Newton minimization problem, as shown in equation (20).

输出基于牛顿高斯的最小二乘解后，根 据高斯牛顿法求得光束点匹配的结果，将与优化位姿对应点的距离与方位角作为数据源代入 建图过程，构建室内二维地图，有效去除了因二维激光雷达运动带来的畸变，提升了建图效 果。in,

After outputting the least-squares solution based on Newton-Gaussian, the result of beam spot matching is obtained according to the Gauss-Newton method, and the distance and azimuth angle of the corresponding point corresponding to the optimized pose are used as data sources into the mapping process to construct an indoor two-dimensional map, which is effective The distortion caused by the motion of the 2D lidar is removed, and the mapping effect is improved.

According to the above content, the indoor map construction method based on the pose fusion optimization of KINECT/IMU selects different pose fusion strategies according to the motion status of the robot. When the robot is stationary, the poses of KINECT and IMU are fused using the EKF method; when the robot is in motion, the poses of KINECT and IMU are fused using the weighting method. For the weighted fusion method, it is the key to determine the proportional relationship between the KINECT pose and the IMU pose to participate in the weighting. In this embodiment, the weight relationship between the KINECT pose and the IMU pose, that is, the weight parameter λ of the pose weighted fusion, is determined according to the ratio μ (matching success rate μ for short) of successfully matched feature points to the total feature points.

In order to obtain an accurate weight parameter λ of the pose weighted fusion, this embodiment also designs an experiment for determining the weight parameter of the pose weighted fusion. The experimental area selects the indoor environment of a laboratory. The experimental equipment is KINECT V2 camera and AH100B inertial measurement unit. Since the frequency of data collection by the inertial measurement unit is too high, in order to synchronize the frequency of data collection by the camera and the inertial measurement unit, the IMU pre-integration step was used in the early stage to unify the collection frequency of the camera and the inertial measurement unit to 30Hz of the KINECT V2 camera.

The experimental steps mainly include the following steps:

(1) Obtain the original pose data.

Select 26 indoor environment scenes respectively, use the KINECT V2 camera to capture images, match the feature points between the images, and solve the pose; use the IMU integral to record the acceleration and angular velocity data, and solve the pose; the commonly used pose solving methods are all The pose of the robot is obtained directly using the Extended Kalman Filtering method, so it can be used as the ground truth for experimental comparisons.

(2) Calculate the root mean square error.

Calculate the root mean square error between the pose solved by the KINECT V2 camera and the IMU and the pose solved by the extended Kalman filter, respectively.

(3) Analyze the relationship between the matching success rate μ and the root mean square error of the pose. When the matching success rate μ takes different values, analyze the relationship between the root mean square error calculated by the KINECT V2 camera and the IMU, and then determine the weight relationship between the KINECT pose and the IMU pose.

The matching success rate μ refers to the ratio between the number of feature points with the same name obtained from the matching of two overlapping images of a certain key frame and the total feature points obtained from the overlapping images. In this application, the matching success rate μ determines the environmental characteristics. The degree of significance, which in turn determines the accuracy of the pose calculation.

In this embodiment, 19 different scenarios are selected first, and the experimental scenarios include representative aisles, corners, tables and chairs, etc., and feature points are matched using the fast approximate nearest neighbor method based on ORB feature point detection.

As shown in Figure 2(a)-Figure 2(c), the images on the left represent the aisle, corner, table and chair scenes of the laboratory environment, and the three images on the right represent the images of the aisle, corner, table and chair obtained from different angles , where the horizontal line in the figure represents the ORB feature point pair that is successfully matched between the two images. During the experiment, 533, 149, and 653 feature points were detected in the aisle, corner, and table and chair scenes respectively. Among them, 50 pairs of feature points were successfully matched in the aisle scene, 4 pairs were successfully matched in the corner scene, and 76 pairs were successfully matched in the table and chair scene. pair of feature points. The matching success rates of aisle, corner, table and chair scenes are 9.38%, 2.68%, and 11.63% respectively, which means that scenes with rich texture, obvious structure and strong characteristics can detect more feature points and have a higher matching success rate; On the contrary, scenes with weak texture and single structure have few detected feature points and low matching success rate. More importantly, through the experiments of these 19 different scenarios, the maximum matching success rate is 43%, and it never exceeds 50%. In view of the above situation, this embodiment adds 7 experimental scenes with artificial features, so that the feature point matching success rate reaches 87.37%. Figure 3 shows some natural scenes and artificial layout scenes.

Secondly, as shown in Fig. 4, the root mean square error between the pose calculated by the KINECT V2 camera and the IMU and the pose calculated by the extended Kalman filter is calculated respectively. The experimental results show that: (1) When the feature points of the KINECT camera are The lower the matching success rate, the higher the root mean square error of the pose between the KINECT camera and the extended Kalman filter, indicating that the reliability of the KINECT camera to solve the pose is low at this time; (2) When the KINECT camera features The higher the success rate of point matching, the lower the root mean square error of the pose between the KINECT camera and the extended Kalman filter, indicating that the KINECT camera has a higher reliability in solving the pose; (3) However, considering The reason for the higher matching success rate is the addition of many artificial feature points, so this effect may need to be ignored when appropriate. If we simply observe that the matching success rate is lower than 50%, we can also find that the pose accuracy calculated by KINECT gradually decreases, and the pose accuracy calculated by IMU has not changed much. This phenomenon is in line with the actual situation. Therefore, when the robot is in a motion state, the present application fuses the posture calculated by KINECT and the posture calculated by IMU according to the weighted parameters, and is used as the optimized fusion posture.

1) When 0.6≤μ≤1, the weight parameter λ of the attitude calculated by KINECT is 0.5; the weight parameter μ of the attitude calculated by IMU is 0.5;

2) When 0.3≤μ<0.6, the weight parameter λ of the attitude calculated by KINECT is 0.33; the weight parameter μ of the attitude calculated by IMU is 0.67;

3) When 0≤μ<0.3, the weight parameter λ of the attitude calculated by KINECT is 0.2; the weight parameter μ of the attitude calculated by IMU is 0.8.

4KINECT/IMU pose fusion experiment:

The experimental platform of the KINECT/IMU pose fusion experiment uses Autolabor robot, which is equipped with KINECT V2 camera, AH100B inertial measurement unit and RPLIDAR A2 two-dimensional lidar. The three sensors and the Autolabor robot are connected by solid connection, as shown in the figure 5 shown.

Experimental data:

In order to verify the effectiveness of the pose fusion weighting strategy, the accuracy of the KINECT/IMU fusion pose, and the optimized LIDAR mapping effect, two groups of different experiments are designed in this embodiment. The first group of experiments uses the EuRoC public data set as the data source, another group of experiments collected a laboratory environmental data as a data source.

The EuRoC public data set is the data collected by ETH Zurich based on the unmanned aerial vehicle in the auditorium and the empty room. The sensors carried by the unmanned aerial vehicle mainly include the camera and the inertial measurement unit. The frequency of the camera is 20Hz, and the frequency of the inertial measurement unit is 200Hz. The experimental data is divided into 4 folders, each of which contains the transformation of the sensor relative to the coordinate system. The EuRoC public dataset is mainly used for comparative analysis of pose accuracy.

The second set of data sets uses the Autolabor robot to collect environmental data in a laboratory. The laboratory scene is an indoor flat ground with obstacles, with an area of 15m × 10m. The sensors carried by the robot mainly include the KINECT V2 camera and the AH100B inertial measurement unit. The frequency of the camera is 30Hz and the frequency of the inertial measurement unit is 37Hz. The data set collected in real time is recorded in the bag data package, and then the bag data package is exported to the corresponding pose data set according to different topics.

Experimental steps:

(1) Acquisition of experimental data

The first set of experiments selected the EuRoC public data set as the data source, and the second set of data sets used the Autolabor robot to collect laboratory environmental data.

(2) Pose calculation

Extract and match the ORB feature points of the two overlapping images, and calculate the motion parameters of the camera according to the matching relationship between the feature point pairs. Integrate the linear velocity recorded by the IMU twice, calculate the velocity and position information, and integrate the angular velocity to calculate the attitude information. After obtaining the velocity, position, and attitude information, use the direction cosine matrix to calculate the relative position and attitude relationship between the IMU and the experimental platform.

(3) Pose estimation optimization

Determine the motion state of the robot. If the robot is in a stationary state, the extended Kalman filter is used to fuse the pose calculated by the camera and the inertial navigation sensor; if the robot is in a motion state, the weighted strategy is used to fuse the pose and output the optimized Pose estimation.

(4) Two-dimensional map construction

The optimized and updated pose correction RPLIDAR A2 obtains the distance and azimuth information of the laser point, and uses Gauss-Newton-based scan matching to construct a two-dimensional environment map.

(5) Experimental analysis

First, based on the EuRoC public data set, compare the accuracy of the ROVIO algorithm and the algorithm in this embodiment in terms of pose fusion optimization. The ROVIO algorithm tightly couples visual information and IMU information, fuses data through iterative Kalman filtering, and updates the filtering state and pose. The algorithm framework and data fusion method are similar to this embodiment. Secondly, based on the collected data of the laboratory scene, the accuracy of the VIORB-SLAM algorithm in terms of pose fusion optimization is also compared with that of the algorithm in this embodiment. The VIORB-SLAM algorithm is composed of ORB-SLAM and IMU modules. Through the maximum a posteriori estimation, the geometric consistency detection of the key frames is performed to obtain the optimal estimation of the pose. Therefore, the VIORB-SLAM algorithm and the algorithm in this embodiment have higher performance. contrast. Finally, in the aspect of two-dimensional map construction, the modeling accuracy and effect of the Cartographer algorithm and the algorithm of this embodiment are compared. The Cartographer algorithm is a classic algorithm for building maps based on two-dimensional lidar. It predicts the pose and poses by filtering according to the IMU and odometer information, and converts the scanned points to the coordinates of the grid points by voxel filtering. Greatness.

The first set of experiments is based on the EuRoC public data set, comparing and analyzing the pose estimation accuracy of the ROVIO algorithm and the algorithm of this embodiment. The total length of the experiment is 149.96s, and a total of 29993 poses are selected. The experimental errors are shown in Table 5. The maximum error (Max), the minimum error (Min), the mean value (Mean), the root mean square error (RMSE) and the standard deviation (Std) are selected as the indicators for measuring the accuracy. At the same time, the offset graphs of the robot in the x-axis, y-axis and z-axis directions are analyzed (Fig. 6).

Table 5 This embodiment algorithm and ROVIO experiment error analysis (pose: 29993)

It can be seen from the experimental results that the absolute root mean square error is 2.5944 and the standard deviation absolute error is 1.5223 compared with the ROVIO algorithm, and the displacement deviations in the x, y, and z directions are compared at the same time, indicating that the algorithm of this embodiment is compared with The ROVIO algorithm can effectively filter excess offset and vibration, and effectively improve the robot's pose estimation accuracy with the help of the fusion optimization mechanism.

The second group of experiments compares and analyzes the pose estimation accuracy of the VIORB-SLAM algorithm and the algorithm of this embodiment based on the laboratory scene data collected by the robot in real time. The total length of the experiment is 367.30s, and a total of 3648 poses are selected. The experimental errors are shown in Table 6, and the maximum error (Max), the minimum error (Min), the mean value (Mean), the root mean square error (RMSE) and the standard deviation (Std) are also selected as the indicators for measuring the accuracy. At the same time, the offset graphs of the robot in the x-axis and y-axis directions are analyzed (Fig. 7).

Table 6 The algorithm of this application and the experimental error analysis of VIORB-SLAM (pose: 3648)

Among the 3648 pose estimations, compared with the VIORB-SLAM algorithm, the root mean square error is reduced by 0.26 and the standard deviation is reduced by 0.27. Compared with the pose calculation result of the VIORB-SLAM algorithm, the fusion pose optimization of the algorithm in this embodiment provides a higher-precision pose estimation for the robot, and the error analysis is shown in Table 6. As shown in Figure 7, during the movement process, the displacement deviation of the robot optimized by the fusion pose in the x and y directions is gradually fitted in the same motion trajectory, and the drift of the position over time decreases, indicating that the fusion pose optimization is in the local scene. It can also effectively improve the positioning accuracy of the robot.

Two sets of experiments show that the fusion optimized pose based on KINECT/IMU solves the difficult problem of robot pose estimation and positioning in the absence of satellite signals in the indoor environment, and at the same time improves the robot pose estimation accuracy; In order to solve the problem of low output frequency and poor reliability of KINECT camera, the high-frequency output of IMU used for fusion pose optimization also meets the requirements of high-precision pose estimation under the condition of high dynamic characteristics.

Map Construction Analysis:

The construction of a two-dimensional map of the indoor environment is realized through the robot operating system. The experiment compares and analyzes the modeling accuracy and effect of the indoor map construction method based on KINECT/IMU fusion pose optimization and the Cartographer algorithm.

First, compare and analyze the local mapping effect, as shown in Figure 8. Figures 8(a) and 8(b) show the distribution of laser points scanned by the Cartographer algorithm and the algorithm in this embodiment, respectively, and Figures 8(c) and 8(d) show the Cartographer algorithm and this embodiment, respectively. The algorithm builds a local map based on the scene. From the comparison results, it can be seen that under the condition of non-repetitive scanning of the same local scene, the laser points scanned by the Cartographer algorithm have local laser points discontinuity at objects with high reflectivity such as smooth iron surfaces, as shown in Figure 8 ( As shown in a), the edge of the actual two-dimensional map constructed by the Cartographer algorithm based on the two-dimensional laser points cannot be fitted, as shown in the box in Figure 8(c), which affects the map accuracy and construction effect; while using IMU/KINECT The algorithm of this embodiment of fusion pose compensation can correct the pose of the laser point scanned by the two-dimensional lidar, resulting in the optimization of the distance and azimuth angle of the laser point according to the robot's own pose, so that the deviation of the laser point from the true value is obtained. As shown in Figure 8(b), the gap generated by the edge of the two-dimensional image is closed and fitted, which improves the accuracy of scan matching. Finally, a two-dimensional map of the scene is constructed through the map construction algorithm, which improves the accuracy and visualization of local details of the map. The effect is shown in Fig. 8(d). The algorithm of this embodiment is used to construct a complete indoor environment map. In the experiment, the robot travels a total of 68.96m and generates 3648 marked points. The global mapping effect is shown in Figure 9(a) and Figure 9(b).

Secondly, from the analysis of the overall mapping effect, the edge details of the map constructed by the algorithm in this embodiment are more complete (Figure 9(b)). The mapping lines of the actual wall on the two-dimensional plane map are perpendicular to each other, and the relative sides The relationship is basically undistorted.

Therefore, from the perspective of mapping accuracy and visualization effect, the algorithm of this embodiment has a certain degree of improvement compared to the Cartographer algorithm.

In this example, taking the indoor environment as the experimental background, combined with the pose accuracy requirements of the robot indoor mapping and the characteristics of the indoor environment, a method of pose optimization based on KINECT/IMU fusion is proposed, which is applied to the modeling of two-dimensional maps. . First, the pose of the robot is calculated by KINECT and IMU; secondly, by determining the motion state of the robot, the IMU and KINECT data are fused using the extended Kalman filter or feature matching weighting strategy to form a new pose estimation; third, the fusion is optimized The resulting high-precision pose corresponds to the laser points measured at the same time, corrects the distance and azimuth information of the laser points in the coordinate system, and substitutes them into the matching process based on Newton-Gaussian sweep points to construct an indoor two-dimensional map. . Finally, the KINECT/IMU pose fusion experiment is designed, and the EuRoc public data set and the measured data set are used to verify the effectiveness of the algorithm. The experimental results show that the algorithm has higher pose estimation accuracy than the ROBVIO algorithm and the VI ORB-SLAM algorithm. Cartographer algorithm has higher 2D map modeling accuracy and modeling effect.

Another aspect of the present invention provides a kind of system that adopts the above-mentioned indoor map construction method based on pose fusion optimization to carry out indoor map construction, it is characterized in that, comprising:

The pose data calculation unit calculates the pose data of the robot according to the real-time data obtained by KINECT and IMU;

a motion state judging unit, for judging the current motion state of the robot according to the pose data;

The pose data fusion processing unit is respectively connected with the pose data calculation unit and the motion state judgment unit. When the robot is in a static state, the extended Kalman filtering algorithm is used to fuse the pose data. When the robot is in a motion state When , adopt the dynamic weighting method to perform fusion processing on the pose data;

The two-dimensional map construction unit is connected with the pose data fusion processing unit, and creates an indoor map according to the fusion processing result.

As a preferred embodiment, the pose data calculation unit includes:

The KINECT data solving subunit is used to obtain image data; it matches the feature points of the same name of two partially overlapping images to obtain the corresponding relationship between the feature points of the same name, and the corresponding relationship corresponds to the pose of the robot.

The IMU data solving sub-unit uses the direction cosine matrix algorithm to solve the pose and attitude of the real-time data obtained by the IMU.

In yet another aspect of the present invention, a storage medium is provided, and a computer program is stored in the storage medium, and when the computer program is processed and executed, the above-mentioned indoor map construction method based on pose fusion optimization is realized.

In order to improve the accuracy of indoor map construction, high-precision data obtained by various sensors such as KINECT and IMU can usually be fused. By judging the motion state of the robot, according to the degree of dependence of feature point matching, the present application uses EKF and weighted fusion to optimize the poses of the two sensors, KINECT and IMU, and realizes high-precision modeling of indoor maps. In this process, the motion state of the robot is first determined. For the stationary state of the robot, the extended Kalman filter algorithm is used to fuse the pose data. For the motion state of the robot, the KINECT and IMU pose data are fused by the feature matching weighting strategy. In this way, Effectively improve the accuracy of pose estimation after fusion, so that the two-dimensional map modeling accuracy is better and the modeling effect is better. This method can effectively improve the pose estimation accuracy in scenes with poor features, high dynamics, and low light and shadow.

Although the embodiments of the present invention have been shown and described above, it should be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Under the circumstance of the present invention, the above-described embodiments can be changed, modified, replaced and modified within the scope of the present invention, and any simple modifications, equivalent changes and modifications made to the above embodiments according to the technical essence of the present invention still belong to the present invention within the scope of the technical solution.

Claims (10)

1. An indoor map construction method based on pose fusion optimization is characterized by comprising the following steps:

resolving pose data of the robot according to real-time data acquired by KINECT and IMU;

acquiring the current motion state of the robot;

if the robot is in a static state, performing fusion processing on the pose data by adopting an extended Kalman filtering algorithm, and if the robot is in a motion state, performing fusion processing on the pose data by adopting a dynamic weighting method;

and constructing an indoor map according to the fusion processing result.

2. The pose fusion optimization-based indoor map construction method according to claim 1, wherein the specific method for resolving robot pose data by using the real-time data acquired by KINECT is as follows: acquiring image data; and matching the homonymous feature points of the two partially overlapped images to obtain a corresponding relation between the homonymous feature points, wherein the corresponding relation corresponds to the position and posture of the robot.

3. The pose fusion optimization-based indoor map construction method according to claim 1, wherein the specific method for resolving robot pose data by using real-time data acquired by the IMU comprises the following steps: and (4) adopting a direction cosine matrix algorithm to solve the pose.

4. The pose fusion optimization-based indoor map construction method according to claim 1, wherein the method for acquiring the current motion state of the robot is as follows:

if the linear acceleration of the IMU is 0 and the mileage count data is not increased, judging that the robot is in a static state currently;

and if the linear acceleration of the IMU is not 0 or the odometer data is increased, judging that the robot is in a motion state currently.

5. The pose fusion optimization-based indoor map construction method according to claim 1, characterized in that a specific method for performing fusion processing on the pose data by using an extended kalman filter algorithm is as follows:

obtaining system state quantity, and calculating an error state predicted value and a system state noise covariance matrix;

obtaining an observation matrix and a noise covariance matrix by calculation, and calculating a gain matrix of the system;

updating the system state quantity and the system covariance matrix by the gain matrix;

carrying out normalization processing on the quaternion obtained by calculating the system state quantity;

and converting the quaternion into an Euler angle convenient for displaying the pose, so as to represent the pose state of the robot.

6. The pose fusion optimization-based indoor map construction method according to claim 1, wherein the pose data fusion processing method by adopting a dynamic weighting method comprises the following steps:

acquiring a corresponding image according to the key frame through KINECT;

setting a weight value threshold according to the matching success rate mu of the feature points between the two key frames acquired by KINECT; if the matching success rate mu is reduced, the weight of the IMU posture is increased; and if the matching success rate mu is increased, the weight of the KINECT posture is increased.

7. The pose fusion optimization-based indoor map construction method according to claim 1, wherein a specific method for creating an indoor map according to the fusion processing result is as follows:

obtaining relative coordinates of the laser point and the radar after resolving the distance and the azimuth angle;

replacing pose estimation in a mapping algorithm with the pose estimation after fusion optimization in advance, and calculating the relative relation between the laser point and the radar by taking the pose estimation as a new data source so as to update the distance and azimuth angle information of the laser point;

and matching the laser point coordinates by using a scanning matching method based on Gauss Newton to establish a high-precision indoor two-dimensional map.

8. A system for indoor mapping using the pose fusion optimization-based indoor mapping method according to any one of claims 1 to 7, comprising:

the pose data resolving unit is used for resolving pose data of the robot according to real-time data acquired by the KINECT and the IMU;

the motion state judgment unit is used for judging the current motion state of the robot according to the linear acceleration data and the mileage counting data of the IMU;

the pose data fusion processing unit is respectively connected with the pose data resolving unit and the motion state judging unit, when the robot is in a static state, the pose data are fused by adopting an extended Kalman filtering algorithm, and when the robot is in a motion state, the pose data are fused by adopting a dynamic weighting method;

and the two-dimensional map construction unit is connected with the pose data fusion processing unit and used for creating an indoor map according to the fusion processing result.

9. The pose-fusion-optimization-based indoor map construction system according to claim 8, wherein the pose data solving unit includes:

a KINECT data decoding subunit for acquiring image data; and matching the homonymous feature points of the two partially overlapped images to obtain a corresponding relation between the homonymous feature points, wherein the corresponding relation corresponds to the position and posture of the robot.

And the IMU data resolving subunit is used for resolving the pose of the real-time data acquired by the IMU by adopting a direction cosine matrix algorithm.

10. A storage medium having a computer program stored therein, wherein the computer program is configured to implement the pose fusion optimization-based indoor map construction method according to any one of claims 1 to 7 when executed.

Images