CN111539994B - Particle filter repositioning method based on semantic likelihood estimation - Google Patents

Abstract

The invention provides a particle filter repositioning method based on semantic likelihood estimation, which comprises the following steps: step S1: constructing a grid map; step S2: identifying an object by a target detection method of a convolutional neural network, and obtaining semantic information of the object; and step S3: simulating a laser radar by using a visual sensor to obtain semantic laser data; and step S4: constructing a semantic map; step S5: obtaining an obstacle likelihood domain through a laser grid map, and obtaining an object semantic likelihood domain through a semantic map; step S6: the exact position of the robot is determined. The method fully combines the grid information of the obstacles and the semantic information of the objects to perform repositioning, and avoids the problem that positioning fails under similar environments by using laser radar information alone. The method overcomes the defect that the original particle filtering method only utilizes environment structure information for matching, can effectively solve the problem of global relocation error of the robot, and simultaneously enhances the convergence rate of relocation.

Description

A particle filter relocalization method based on semantic likelihood estimation

Technical Field

The invention belongs to the field of robot navigation relocation, and in particular relates to a particle filtering relocation method based on semantic likelihood estimation.

Background Art

In today's robotics field, people's demands are getting higher and higher, so robot navigation is required to be more accurate, and the problem of robot repositioning is particularly important.

Relocalization is the most important part of SLAM (simultaneous localization and mapping). SLAM relocalization is defined as: given a known environment map, how to find out the machine's own location based on the machine's current sensor observation data. Only after successful positioning can map construction and navigation be carried out.

Researchers at home and abroad have combined a variety of information to help robots complete repositioning. Some have done this by placing QR codes in the environment and adding additional information to constrain particle sampling. Some have done this by placing multiple camera monitors in the environment and extracting ORB features from images captured by multiple cameras to detect the robot and determine where the robot is located in the sub-map, thereby improving the time efficiency of repositioning after kidnapping. Others have corrected the robot's positioning through landmark information in the environment and semantic lasers obtained in the robot's actual position, thereby enhancing positioning accuracy and precision.

Whether it is through the layout of artificial markers such as QR codes, or the use of video surveillance to help the robot locate, the method is not direct and autonomous enough. The robot posture correction and relocation based on a single object landmark is highly correlated with the accuracy of a single landmark detection, and requires the object landmark to be as flat as possible to facilitate the extraction of the laser point coordinates at the center of the laser cluster as the object position coordinates. Therefore, when relocating, there is a large error in target recognition and the accuracy of relocation is low.

Based on the above problems existing in the prior art, the present invention proposes a particle filtering relocalization method based on semantic likelihood estimation, which increases the semantic information matching of objects, overcomes the shortcomings of matching only with environmental structure information, can effectively solve the problem of global relocalization errors of robots, and at the same time enhances the convergence speed of relocalization.

Summary of the invention

The technical solution adopted by the present invention to solve the problems existing in the prior art is as follows:

A particle filtering relocation method based on semantic likelihood estimation, characterized in that it comprises the following steps:

Step S1: construct a grid map, and the environment must have obvious roadblock features for obstacle information recognition;

Step S2: Identify the object through the object detection method of the convolutional neural network and obtain the semantic information of the object;

Step S3: using the visual sensor to simulate the laser radar to obtain semantic laser data;

Step S4: constructing a semantic map according to the positional relationship between the robot and the visual sensor, the positional relationship between the global coordinate system and the robot, and the semantic laser data of the object;

Step S5: When the positioning is deviated, the robot uses the laser radar and visual sensor to identify the environment, obtains the obstacle likelihood domain through the laser grid map, and obtains the object semantic likelihood domain through the semantic map;

Step S6: Predict the initial posture of the robot, and change the particle weights through simultaneous likelihood matching of laser and object semantics, and resample, gradually iterate, complete the estimation of the robot state after the filter converges, and determine the exact position of the robot.

The specific steps of step S1 are as follows:

Step S101: selecting obstacles, selecting objects with high recognition and that will not move as obstacles to be identified;

Step S102: A grid map is created based on the laser radar data. The robot moves in a circle in the environment, obtains laser information of the environment and robot odometer information based on the laser radar, and creates a grid map using the gmapping algorithm.

The target detection method of step S2 is to use the SSD method for object detection to obtain the semantic information of the object, and the sensor used for visual recognition is kinectV2.

The specific steps of step S3 to obtain semantic laser data of the object are as follows:

Step S301: aligning the KinectV2 color camera and the depth camera, and calibrating the relative position relationship between the laser radar and the color camera;

Step S302: After parameter calibration and registration, the point cloud data in the semantic information of the object is simulated as a laser radar hit point by removing the ground and retaining only the nearest point of each column in the detection frame to obtain the semantic laser data of the object;

Step S303: The coordinates of the semantic laser data obtained are as follows:

z _k = d/s (1)

x _k =(uc _x )*z _k /f _x (2)

y _k =(vc _y )*z _k /f _y (3)

Where d is the depth value of the pixel coordinate (u, v) in a frame of the color camera in the corresponding depth map, s is the scaling factor of the depth map, c _x , _cy , f _x , f _y are the focal length and aperture center of the RGB camera on the two axes respectively, and x _k , y _k , z _k are the three-dimensional coordinates in the kinectV2 coordinate system.

The specific steps of constructing the semantic map in step S4 are as follows:

Step S401: according to the transformation matrix T _rk between the camera coordinate system and the robot coordinate system and the transformation matrix T _wr between the robot coordinate system and the global coordinate system, the semantic laser data coordinates are transformed into the robot coordinate system and then transformed into the global map;

Wherein the coordinates (x _ki , y _ki , z _ki ) are the coordinates of a point in the semantic laser data, and the coordinates (x _wi , y _wi , z _wi ) are the coordinates of the corresponding point in the global coordinate system;

Step S402: transform the point cloud coordinates in the global coordinate system to the grid map coordinate system to obtain the coordinates of the object in the grid map, and connect all the coordinate points to obtain a semantic map:

Where r is the ratio of the length of the real map to the length of the grid map, int is the integer symbol, (x _w , y _w ) is the coordinate value in the global plane coordinate system, and (x _cell , y _cell ) is the coordinate value of the semantic grid map.

The formula for obtaining the likelihood domain in step S5 is as follows:

为传感器t时刻测量的物体的概率，z_t为t时刻传感器信息，

为t时刻第k个粒子的位姿信息，m为环境地图；Where dist is the Euclidean distance between a point in the map and the nearest obstacle, σ is the maximum distance between the sensor hit point and the obstacle point in the map,

is the probability of the object measured by the sensor at time t, z _t is the sensor information at time t,

is the position information of the kth particle at time t, and m is the environment map;

The probability of an obstacle is represented by a likelihood domain model. The higher the probability, the greater the possibility of an obstacle. In the semantic map, with the semantic category information of the obstacle, the sensor hit point will look for the same type and closest obstacle point in the map to obtain dist:

The specific steps of step S6 to determine the exact position of the robot are as follows:

进行机器人的位姿预测；Step S601 Pose prediction: At time t, the robot uses the state estimate xt _-1 at time t-1 and the control amount ut from time t-1 to time _t to obtain the state transfer distribution p( _xt | _xt-1 , _ut ) through the motion model, and randomly samples from the state transfer distribution to obtain the assumed state at time t.

Predict the robot’s posture;

公式如下：Step S602 Matching update: Through the observation model function at time t, the likelihood matching degree between the laser information and camera detection information contained in each particle and the environment map is returned, that is, the particle weight

The formula is as follows:

为t时刻第k个粒子的权重，表示测量概率随着时间的积分，且满足

与

分别为激光和相机测量的似然概率；l_t、k_t分别为t时刻的激光雷达数据与语义点云数据；m_l、m_k为激光栅格地图与语义地图；Where: Weight

is the weight of the kth particle at time t, which represents the integral of the measurement probability over time and satisfies

and

are the likelihood probabilities of laser and camera measurements, respectively; l _t , k _t are the laser radar data and semantic point cloud data at time t, respectively; m _l , m _k are the laser grid map and semantic map;

按照轮盘转法复制新的粒子

添加进粒子集合中，并归一化粒子权重，保证了重采样产生的粒子向高权值粒子位姿移动，用粒子群近似后验概率分布，以趋近机器人真实位姿；Step S603: When the robot moves beyond a certain threshold, the weight of each particle is

Copy new particles according to the roulette wheel method

Add them to the particle set and normalize the particle weights to ensure that the particles generated by resampling move to the position of particles with high weights, and use the particle swarm to approximate the posterior probability distribution to approach the actual position of the robot;

After the robot has moved enough and all particles have converged, the center of gravity of all particles, i.e. the true position of the robot, is obtained by normalizing the weights of the particles. The center of gravity formula is as follows:

The present invention has the following advantages:

The particle filter relocalization method based on semantic likelihood estimation of the present invention fully combines obstacle grid information and object semantic information for relocalization, avoiding the problem of positioning failure in similar environments using only laser radar information. This method overcomes the deficiency of the original particle filter method that only uses environmental structure information for matching, can effectively solve the problem of global relocalization errors of robots, and at the same time enhances the convergence speed of relocalization.

BRIEF DESCRIPTION OF THE DRAWINGS

Fig. 1 is a flow chart of the method of the present invention;

Figure 2 is a grid map;

Figure 3 is a semantic map;

Figure 4 is a map of semantic obstacles and map likelihood matching;

FIG5 is a flow chart of particle filtering positioning according to the present invention.

DETAILED DESCRIPTION

The technical solution of the present invention is further specifically described below through embodiments and in conjunction with the accompanying drawings. A particle filtering relocation method based on semantic likelihood estimation is characterized in that it comprises the following steps:

Step S1: construct a grid map, and the environment must have obvious roadblock features for obstacle information recognition;

Step S2: Identify the object through the object detection method of the convolutional neural network and obtain the semantic information of the object;

Step S3: using the visual sensor to simulate the laser radar to obtain semantic laser data;

Step S4: constructing a semantic map according to the positional relationship between the robot and the visual sensor, the positional relationship between the global coordinate system and the robot, and the semantic laser data of the object;

Step S5: When the positioning is deviated, the robot uses the laser radar and visual sensor to identify the environment, obtains the obstacle likelihood domain through the laser grid map, and obtains the object semantic likelihood domain through the semantic map;

Step S6: Predict the initial posture of the robot, and change the particle weights through simultaneous likelihood matching of laser and object semantics, and resample, gradually iterate, complete the estimation of the robot state after the filter converges, and determine the exact position of the robot.

The specific steps of step S1 are as follows:

Step S101: selecting obstacles, selecting objects with high recognition and that will not move as obstacles to be identified;

Step S102: A grid map is created based on the laser radar data. The robot moves in a circle in the environment, obtains laser information of the environment and robot odometer information based on the laser radar, and creates a grid map using the gmapping algorithm, as shown in FIG2 .

The target detection method of step S2 is to use the SSD method for object detection to obtain the semantic information of the object, and the sensor used for visual recognition is kinectV2.

The specific steps of step S3 to obtain semantic laser data of the object are as follows:

Step S301: aligning the KinectV2 color camera and the depth camera, and calibrating the relative position relationship between the laser radar and the color camera;

Step S302: After parameter calibration and registration, the point cloud data in the semantic information of the object is simulated as a laser radar hit point by removing the ground and retaining only the nearest point of each column in the detection frame to obtain the semantic laser data of the object;

Step S303: The coordinates of the semantic laser data obtained are as follows:

z _k = d/s (1)

x _k =(uc _x )*z _k /f _x (2)

y _k =(vc _y )*z _k /f _y (3)

Where d is the depth value of the pixel coordinate (u, v) in a frame of the color camera in the corresponding depth map, s is the scaling factor of the depth map, c _x , _cy , f _x , f _y are the focal length and aperture center of the RGB camera on the two axes respectively, and x _k , y _k , z _k are the three-dimensional coordinates in the kinectV2 coordinate system.

The specific steps of constructing the semantic map in step S4 are as follows:

Step S401: according to the transformation matrix T _rk between the camera coordinate system and the robot coordinate system and the transformation matrix T _wr between the robot coordinate system and the global coordinate system, the semantic laser data coordinates are transformed into the robot coordinate system and then transformed into the global map;

Wherein the coordinates (x _ki , y _ki , z _ki ) are the coordinates of a point in the semantic laser data, and the coordinates (x _wi , y _wi , z _wi ) are the coordinates of the corresponding point in the global coordinate system;

Step S402: transform the point cloud coordinates in the global coordinate system to the grid map coordinate system to obtain the coordinates of the object in the grid map, connect all the coordinate points, and obtain a semantic map, as shown in FIG3 , where there are many different types of obstacles, such as A-chair, B-door, C-cabinet, D-trash can, etc.:

Where r is the ratio of the length of the real map to the length of the grid map, int is the integer symbol, (x _w , y _w ) is the coordinate value in the global plane coordinate system, and (x _cell , y _cell ) is the coordinate value of the semantic grid map.

The formula for obtaining the likelihood domain in step S5 is as follows:

为传感器t时刻测量的物体的概率，z_t为t时刻传感器信息，

为t时刻第k个粒子的位姿信息，m为环境地图；Where dist is the Euclidean distance between a point in the map and the nearest obstacle, σ is the maximum distance between the sensor hit point and the obstacle point in the map,

is the probability of the object measured by the sensor at time t, z _t is the sensor information at time t,

is the position information of the kth particle at time t, and m is the environment map;

The probability of an obstacle is represented by the likelihood domain model. The higher the probability, the greater the possibility of the obstacle. Figure 4 shows the likelihood matching diagram of the visual data and the original map data. Line 1 in the figure is the semantic data measured by the sensor, and line 2 is the map data. The figure is divided into four different semantic categories: a, b, c, and d. The points on line 1 are connected to the points of the same category and the nearest points in line 2. That is, in the semantic map, with the semantic category information of the obstacle, the sensor hit point will look for the same type and the nearest obstacle point in the map, so that dist can be obtained:

The specific steps of step S6 to determine the exact position of the robot are as follows:

进行机器人的位姿预测；Step S601 Pose prediction: At time t, the robot uses the state estimate xt _-1 at time t-1 and the control amount ut from time t-1 to time _t to obtain the state transfer distribution p( _xt | _xt-1 , _ut ) through the motion model, and randomly samples from the state transfer distribution to obtain the assumed state at time t.

Predict the robot’s posture;

公式如下：Step S602 Matching update: Through the observation model function at time t, the likelihood matching degree between the laser information and camera detection information contained in each particle and the environment map is returned, that is, the particle weight

The formula is as follows:

为t时刻第k个粒子的权重，表示测量概率随着时间的积分，且满足

与

分别为激光和相机测量的似然概率；l_t、t_k分别为t时刻的激光雷达数据与语义点云数据；m_l、m_k为激光栅格地图与语义地图。Where: Weight

is the weight of the kth particle at time t, which represents the integral of the measurement probability over time and satisfies

and

are the likelihood probabilities of laser and camera measurements respectively; l _t , t _k are the lidar data and semantic point cloud data at time t respectively; m _l , m _k are the laser grid map and semantic map.

按照轮盘转法复制新的粒子

添加进粒子集合中，并归一化粒子权重，保证了重采样产生的粒子向高权值粒子位姿移动，用粒子群近似后验概率分布，以趋近机器人真实位姿；Step S603: When the robot moves beyond a certain threshold, the weight of each particle is

Copy new particles according to the roulette wheel method

Add them to the particle set and normalize the particle weights to ensure that the particles generated by resampling move to the position of particles with high weights, and use the particle swarm to approximate the posterior probability distribution to approach the actual position of the robot;

After the robot has moved enough and all particles have converged, the center of gravity of all particles, i.e. the true position of the robot, is obtained by normalizing the weights of the particles. The center of gravity formula is as follows:

The protection scope of the present invention is not limited to the above-mentioned embodiments. Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the scope and spirit of the present invention. If these changes and modifications fall within the scope of the claims of the present invention and their equivalents, the intention of the present invention also includes these changes and modifications.

Claims (5)

1. A particle filter repositioning method based on semantic likelihood estimation is characterized by comprising the following steps:

step S1: constructing a grid map, wherein obvious roadblock characteristics in the environment are used for identifying obstacle information;

step S2: identifying an object by a target detection method of a convolutional neural network, and obtaining semantic information of the object;

and step S3: simulating a laser radar by using a visual sensor to obtain semantic laser data;

and step S4: constructing a semantic map according to the position relationship between the robot and the visual sensor, the position relationship between the global coordinate system and the robot, and semantic laser data of the object;

step S5: when deviation occurs in positioning, the robot recognizes the environment by using a laser radar and a visual sensor, obtains an obstacle likelihood domain through a laser grid map, and obtains an object semantic likelihood domain through a semantic map;

step S6: predicting the initial pose of the robot, changing the weight of particles through simultaneous likelihood matching of the semantics of the laser and the object, resampling, gradually iterating, finishing estimation of the state of the robot after the convergence of a filter, and determining the accurate position of the robot;

the step S3 of acquiring object semantic laser data specifically includes the following steps:

step S301: registering the KinectV2 color camera and the depth camera, and calibrating the relative position relation between the laser radar and the color camera;

step S302: after parameter calibration and registration, point cloud data in object semantic information is simulated into laser radar hit points by removing the ground and only keeping the closest point of each row in a detection frame, so as to obtain semantic laser data of the object;

step S303: the obtained semantic laser data coordinates are as follows:

z _k ＝d/s (l)

x _k ＝(u-c _x )*z _k /f _x (2)

y _k ＝(v-c _y )*z _k /f _y (3)

where d is the depth value of the coordinate (u, v) of the pixel in a frame of picture of the color camera in the corresponding depth map, s is the scaling factor of the depth map, c _x 、c _y 、f _x 、f _y Focal length and aperture center, x, of RGB camera on two axes, respectively _k 、y _k 、z _k Is a three-dimensional coordinate under a kinect V2 coordinate system;

the formula for obtaining the likelihood domain in step S5 is as follows:

dist is the Euclidean distance between a point in the map and the nearest obstacle, and sigma is the maximum point of the sensor hitting point and the obstacle point in the mapThe distance between the two adjacent light-emitting diodes is large,

probability of an object measured for sensor time t, z _t For the sensor information at the time of the t,

the pose information of the kth particle at the time t, and m is an environment map;

the probability of the obstacle is represented by a likelihood domain model, and the higher the probability is, the higher the possibility of the obstacle is; in the semantic map, semantic category information of the obstacles exists, the sensor hit point can search for the same type and nearest obstacle point in the map, and dist is obtained:

2. the particle filter relocation method based on semantic likelihood estimation as claimed in claim 1, wherein the specific steps of the step S1 are as follows:

step S101: selecting an obstacle, namely selecting an object which has higher identification degree and cannot move as the identified obstacle;

step S102: and establishing a grid map according to the laser radar data, moving the robot for one circle in the environment, acquiring laser information of the environment and odometer information of the robot according to the laser radar, and establishing the grid map by using a mapping algorithm.

3. The particle filter repositioning method based on semantic likelihood estimation of claim 1, wherein the target detection method in step S2 is to use SSD method for object detection to obtain semantic information of the object, and the sensor used for visual recognition is kinectV2.

4. The particle filter repositioning method based on semantic likelihood estimation as claimed in claim 1, wherein the step S4 of constructing the semantic map comprises the following specific steps:

step S401: according to the transformation matrix T between the camera coordinate system and the robot coordinate system _rk Transformation matrix T between robot coordinate system and global coordinate system _wr Converting the semantic laser data coordinate into a robot coordinate system and then converting into a global map;

formula of middle coordinate (x) _ki ，y _ki ，z _ki ) Coordinate of a point, coordinate (x) for semantic laser data _wi ，y _wi ，z _wi ) The coordinates of the corresponding points in the global coordinate system are obtained;

step S402: and transforming the finally obtained point cloud coordinate under the global coordinate system to a grid map coordinate system to obtain the coordinate of the object in the grid map, and connecting all coordinate points to obtain the semantic map:

where r is the ratio of the length of the real map to the length of the grid map, int is the rounding symbol, (x) _w ，y _w ) Is a coordinate value under the global plane coordinate system, (x) _cell ，y _cell ) Is the coordinate value of the semantic grid map.

5. The particle filter repositioning method based on semantic likelihood estimation as claimed in claim 1, wherein the step S6 of determining the accurate position of the robot comprises the following specific steps:

step S601 pose prediction: the robot estimates x at time t by using the state at time t-1 _t-1 And a control amount u from time t-1 to time t _t The state transition distribution p (x) is obtained by a motion model _t |x _t-1 ，u _t ) Randomly sampling from the state transition distribution to obtain the pose information of the kth particle at the t moment

Predicting the pose of the robot;

step S602 matching update: returning the likelihood matching degree of the laser information and the camera detection information contained in each particle and the environment map, namely the particle weight, through an observation model function at the time t

The formula is as follows:

in the formula: weight of

The weight of the kth particle at the moment t represents the integral of the measurement probability with time, and satisfies

And/or>

Likelihood probabilities measured for the laser and camera, respectively; l _t 、l _t Respectively laser radar data and semantic point cloud data at the time t; m is a unit of _l 、m _k A laser grid map and a semantic map are used;

step S603 resampling: when the robot motion exceeds a certain threshold, according to the weight of each particle

Copying new particles according to the roulette method>

Adding the particles into a particle set, normalizing the weight of the particles, ensuring that the particles generated by resampling move to the pose of high-weight particles, and approximating posterior probability distribution by a particle swarm to approach the real pose of the robot;

after the robot moves enough and all the particles converge, the gravity centers of all the particles, namely the real positions of the robot, are obtained by summing and normalizing the weights of the particles, and the gravity center formula is as follows:

Images