A heterogeneous slam system trajectory fusion method based on fuzzy reasoning

Abstract

The application discloses a heterogeneous SLAM system trajectory fusion method based on fuzzy reasoning, which comprises the following steps: trajectory maps are established by using an RGB-D SLAM algorithm and a RatSLAM algorithm in a heterogeneous SLAM system; corresponding points of two trajectory maps are found according to time stamps, and trajectory alignment is carried out by using an ICP algorithm; two fuzzy inputs and one fuzzy output are set by analyzing pose deviation between the corresponding points of the two trajectory maps and front and back point cloud projection errors; a fuzzy reasoning system is established by setting fuzzy rules; the fuzzy system is solved by using a maximum membership degree method, so that the weights of the RGB-D SLAM and the RatSLAM are obtained; and trajectory fusion is carried out by using the fuzzy reasoning result. The application has the beneficial effect that, compared with a single SLAM method, the method provided in the application can establish a more accurate trajectory map; and the trajectory optimization results of the two methods are judged by using a fuzzy set theory, so that a trajectory map with higher accuracy is obtained.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of mobile robot environment mapping and exploration, in particular to a heterogeneous SLAM system trajectory fusion method based on fuzzy reasoning. BACKGROUND

[0002] SLAM (simultaneous localization and mapping) is a key technology for mobile robots to achieve autonomous positioning and navigation, which can be divided into three parts: front-end, back-end and loop detection. The front-end visual odometry can give an incremental map, but due to the inevitable error accumulation, the map is not accurate over a long period of time. When the map grows to a certain extent, error accumulation will make the later data more and more inaccurate, at which time all map data needs to be optimized together, i.e. back-end optimization, to reduce the error of each part.

[0003] The back-end of the RGB-D SLAM algorithm is based on graph optimization. Graph optimization represents the back-end optimization problem of SLAM in the form of a graph, where the pose of the robot is a vertex in the graph, and the connection between vertices, i.e. the relationship between poses, constitutes an edge. When the graph is established, back-end optimization is needed to adjust the pose of the robot to satisfy the constraints formed by these edges as much as possible. RatSLAM algorithm is a bionic navigation algorithm inspired by the navigation mechanism of animal brain, and its back-end uses an iterative optimization algorithm. After the link relationship between experience points (i.e. robot positions) is established, a correction constant is used to continuously make the experience points in the map satisfy the link relationship in the iteration, thereby reducing the error of the global map.

[0004] Under complex environment, both RGB-D SLAM and RatSLAM have great problems in representing the environment, so RGB-D SLAM and RatSLAM are used for joint mapping. Feature point visual odometry is used to replace the front-end of RatSLAM to improve its mapping accuracy. Since the back-end optimization methods of RGB-D SLAM and RatSLAM are different, there may be a large deviation between the two trajectories, so the two trajectories need to be fused. Trajectory fusion is the fusion of a segment of trajectory, which contains multiple robot pose information and cannot be expressed by an exact quantity. We introduce the concept of fuzziness, set two fuzzy input quantities according to the difference between the two trajectory segments and the corresponding point cloud information, add subjective judgment, and establish a fuzzy reasoning system to complete the trajectory fusion, which may result in a more accurate trajectory.

[0005] Therefore, the study of fuzzy reasoning trajectory fusion method is of great significance for improving the joint mapping accuracy of RGB-D SLAM and RatSLAM. SUMMARY

[0006] The present invention aims to provide a trajectory fusion method for heterogeneous SLAM systems based on fuzzy reasoning to solve the problems raised in the above background technology.

[0007] The technical solution of the present invention is: a heterogeneous SLAM system trajectory fusion method based on fuzzy reasoning, comprising the following steps:

[0008] Step 1: Use RGB-D SLAM algorithm and RatSLAM algorithm to build trajectory map in heterogeneous SLAM system;

[0009] Step 2: Use timestamps and ICP algorithm to align the two trajectory maps of the SLAM system in step 1;

[0010] Step 3: Set two fuzzy inputs by analyzing the pose and point cloud information of RatSLAM and RGB-D SLAM, set fuzzy rules, and establish a fuzzy inference system;

[0011] Step 4: Use the maximum membership method to solve the fuzzy system and obtain the weights of RGB-D SLAM and RatSLAM;

[0012] Step 5: Fusion the two trajectories based on the weights of RGB-D SLAM and RatSLAM.

[0013] Further preferably, in step 1, the RGB-D camera is used to collect data and input it into RGB-D SLAM and RatSLAM to establish two trajectory maps M Rat and M RGB-D The RGB-D SLAM algorithm is a traditional visual navigation algorithm that uses an RGB-D camera as a visual sensor for simultaneous localization and mapping (SLAM) technology. The RatSLAM algorithm is a bionic navigation algorithm that simulates biological simultaneous localization and mapping technology.

[0014] Further preferably, in step 2, the trajectory map established by RGB-D SLAM is used as a reference to find the corresponding point set of RGB-D SLAM trajectory and RatSLAM trajectory by timestamp

[0015] P OS ={(p Rat1 , p RGB-D1 ), (p Rat2， p RGB-D2 ),...,(p Ratn , p RGB-Dn )}, and then use the ICP algorithm to align the two trajectory maps. The calculation formula is:

[0016]

[0017] Among them, p RGB-D Represents the pose of the RGB-D SLAM matching point, p Rat Represents the pose of the RatSLAM matching point.

[0018] Further preferably, the two fuzzy inputs set in step 3 are the pose deviation Δ of RGB-D SLAM and RatSLAM and the accuracy η of RGB-D SLAM relative to RatSLAM, respectively. The pose deviation of RGB-D SLAM and RatSLAM is the average pose deviation of the two SLAMs in a small segment of the trajectory. The relative accuracy of RGB-D SLAM is calculated by the point cloud projection error e of the previous and next frames of the RGB-D SLAM in the current trajectory segment. RGB-D Relative to RatSLAM front and back frame point cloud head projection error e Rat The change is obtained if e RGB-D Less than e Rat This indicates that RGB-D SLAM is more accurate at the current moment; if e RGB-D Equal to e Rat , indicating that the accuracy of the two SLAMs is equally high; if e RGB-D Greater than e Rat , indicating that the accuracy of RGB-DSLAM is lower at the current moment; the calculation formula of the pose deviation Δ between RGB-DSLAM and RatSLAM is:

[0019]

[0020] Among them, n represents the number of poses in this trajectory, p RGB-D，t represents the t-th pose of the RGB-D SLAM of this trajectory, p Rat，t Represents the t-th pose of RatSLAM in this trajectory;

[0021] The point cloud projection error e of the RatSLAM front and back frames Rat The calculation formula is:

[0022]

[0023] The point cloud projection error e of the RGB-D SLAM front and back frames RGB-D The calculation formula is:

[0024]

[0025] in, and They represent the transformation matrix from the t-th point cloud of RatSLAM to the world coordinate system and the transformation matrix from the t-th point cloud of RGB-D SLAM to the world coordinate system in this trajectory, respectively. It is the i-th point of the t-th point cloud among the K pairs of matching points found by RatSLAM's t-th point cloud and t+1 point cloud through K nearest neighbors. It is the i-th point of the t-th point cloud among the K pairs of matching points found by K nearest neighbors between the t-th point cloud and the t+1 point cloud of RGB-D SLAM;

[0026] The calculation formula for the accuracy η of the RGB-D SLAM relative to RatSLAM is:

[0027]

[0028] Further preferably, the step 3 utilizes triangular fuzzy numbers to fuzzify the two input variables and the output variable, and sets a fuzzy rule table to establish a fuzzy inference system.

[0029] Further preferably, the step 4 uses the maximum membership method to solve the fuzzy system to obtain the confidence of RGB-D SLAM, and the calculation formula is:

[0030]

[0031] Among them, V represents the domain of the output membership function, μ d is the inverse function of the output membership function, and N is the number of the same maximum membership.

[0032] Further preferably, in step 5, trajectory fusion is performed according to the weights of the two SLAMs, that is, the confidence, and the fusion calculation formula is:

[0033] P Fus =D*P RGB-D +(1-D)*P Rat ,

[0034] Among them, P Fus Represents the pose after fusion, P RGB-D represents the pose of RGB-D SLAM, P Rat Represents the pose of RatSLAM.

[0035] The advantages of the present invention are: the heterogeneous SLAM system trajectory fusion method based on fuzzy reasoning disclosed in the present invention utilizes RGB-D SLAM and RatSLAM to jointly build a map, analyzes the deviation of the two SLAM trajectories and the corresponding point cloud information, and fuses a new trajectory through fuzzy reasoning. Compared with the mapping accuracy of RGB-D SLAM or RatSLAM when running alone, it has higher accuracy and can establish a more accurate trajectory map. BRIEF DESCRIPTION OF DRAWINGS

[0036] The application will be further described below in conjunction with the accompanying drawings and embodiments:

[0037] Figure 1 A flow chart of the RGB-D SLAM and RatSLAM trajectory fusion method disclosed in the embodiments of the application;

[0038] Figure 2 A pose deviation corresponding membership function diagram of the RGB-D SLAM and RatSLAM disclosed in the embodiments of the application;

[0039] Figure 3 A membership function diagram corresponding to the accuracy of the RGB-D SLAM relative to the RatSLAM disclosed in the embodiments of the application;

[0040] Figure 4 A membership function diagram corresponding to the confidence of the RGB-D SLAM disclosed in the embodiments of the application;

[0041] Figure 5 A fuzzy rule table disclosed in the embodiments of the application;

[0042] Figure 6 A self-captured data environment picture disclosed in the embodiments of the application;

[0043] Figure 7 A trajectory fusion result comparison diagram disclosed in the embodiments of the application;

[0044] Figure 8 A specified position point and real environment error analysis diagram disclosed in the embodiments of the application;

[0045] Figure 9 A mapping precision evaluation table disclosed in the embodiments of the application. DETAILED DESCRIPTION

[0046] Embodiment:

[0047] The following is a further description of the technical solutions of the application in conjunction with the embodiments of the application and the accompanying drawings, but the application is not limited to these embodiments.

[0048] As shown in the accompanying drawings, Figure 1 a heterogeneous SLAM system trajectory fusion method based on fuzzy reasoning includes the following steps:

[0049] Step 1, using RGB-D SLAM algorithm and RatSLAM algorithm to establish a trajectory map in a heterogeneous SLAM system;

[0050] Step 2: Use the timestamp and ICP algorithm to align the two trajectory maps of the SLAM system in step 1. Using the trajectory map established by RGB-DSLAM as a reference, find the corresponding point set P of the RGB-D SLAM trajectory and the RatSLAM trajectory through the timestamp. OS ={(p Rat1 , p RGB-D1 ), (p Rat2 , p RGB-D2 ),...,(p Ratn , p RGB-Dn )}, and then use the ICP algorithm to align the two trajectory maps. The calculation formula is:

[0051]

[0052] Among them, p RGB-D Represents the pose of the RGB-D SLAM matching point, p Rat Represents the pose of the RatSLAM matching point;

[0053] Step 3: Set two fuzzy inputs by analyzing the pose and point cloud information of RatSLAM and RGB-D SLAM, use triangular fuzzy numbers to fuzzify the two input variables and output variables, set fuzzy rules, and establish a fuzzy inference system. Among them, the membership function of the pose deviation Δ of RGB-D SLAM and RatSLAM is as follows: Figure 2 As shown, the membership function of RGB-D SLAM’s accuracy η relative to RatSLAM Figure 3 As shown in the figure, the weight of RGB-D SLAM is set as the fuzzy output variable, and the membership function corresponding to its confidence is as follows: Figure 4 As shown, the fuzzy rules are set as Figure 5 As shown in the figure, the two fuzzy inputs are the pose deviation Δ between RGB-D SLAM and RatSLAM and the accuracy η of RGB-D SLAM relative to RatSLAM. The pose deviation between RGB-D SLAM and RatSLAM is the average pose deviation of the two SLAMs in a small segment of the trajectory. The relative accuracy of RGB-D SLAM is calculated by the point cloud projection error e of the previous and next frames of the RGB-D SLAM in the current trajectory segment. RGB-D Relative to RatSLAM front and back frame point cloud head projection error e Rat The change is obtained if e RGB-D Less than e Rat This indicates that RGB-D SLAM is more accurate at the current moment; if e RGB-D Equal to e Rat , indicating that the accuracy of the two SLAMs is equally high; if e RGB-D Greater than e Rat, which indicates that the accuracy of RGB-D SLAM at the current time is lower; the calculation formula of the pose deviation Δ of RGB-D SLAM and RatSLAM is as follows:

[0054]

[0055] wherein n represents the number of poses of the trajectory, p RGB-D，t represents the tth pose of the trajectory of RGB-D SLAM, p Rat，t represents the tth pose of the trajectory of RatSLAM;

[0056] The calculation formula of the point cloud projection error e Rat of the front and rear frames of RatSLAM is as follows:

[0057]

[0058] The calculation formula of the point cloud projection error e RGB-D of the front and rear frames of RGB-D SLAM is as follows:

[0059]

[0060] wherein, and respectively represent the transformation matrix of the tth point cloud of RatSLAM to the world coordinate system and the transformation matrix of the tth point cloud of RGB-D SLAM to the world coordinate system in the trajectory, is the ith point of the tth point cloud of RatSLAM in a pair of K matching points found by K nearest neighbors between the tth point cloud and the t+1th point cloud, is the ith point of the tth point cloud of RGB-D SLAM in a pair of K matching points found by K nearest neighbors between the tth point cloud and the t+1th point cloud;

[0061] The calculation formula of the accuracy η of RGB-D SLAM relative to RatSLAM is as follows:

[0062]

[0063] Step 4, the maximum membership degree method is used to solve the fuzzy system, the weights of RGB-D SLAM and RatSLAM are obtained, and the confidence of RGB-D SLAM is obtained, and the calculation formula is as follows:

[0064]

[0065] wherein V represents the domain of the output membership function, μ d is the inverse function of the output membership function, and N is the number of maximum membership degrees;

[0066] Step 5: Fusion of the two trajectories is performed based on the weights of the two SLAMs, RGB-D SLAM and RatSLAM. Trajectory fusion is performed based on the weights of the two SLAMs, i.e., the confidence. The fusion calculation formula is:

[0067] P Fus =D*P RGB-D +(1-D)*P Rat ,

[0068] Among them, P Fus Represents the pose after fusion, P RGB-D represents the pose of RGB-D SLAM, P Rat Represents the pose of RatSLAM.

[0069] In this application, in step 1, the RGB-D camera is used to collect data and input it into RGB-D SLAM and RatSLAM to build two trajectory maps M Rat and M RGB-D The RGB-D SLAM algorithm is a traditional visual navigation algorithm that uses an RGB-D camera as a visual sensor for simultaneous localization and mapping (SLAM). The RatSLAM algorithm is a biomimetic navigation algorithm. This invention replaces the front end of RatSLAM with a point odometry to improve RatSLAM's mapping accuracy.

[0070] like Figure 6 As shown in Figure 1, the comparison of the results of fuzzy reasoning trajectory fusion is shown. When RGB-D SLAM and RatSLAM have different representations of the environment, the fuzzy reasoning system analyzes the deviation of the two SLAM trajectories and the corresponding point cloud information, and finally fuses a new trajectory. Figure 7 、 Figure 8 and Figure 9 , the accuracy of the fused trajectory is higher.

[0071] The above embodiments are intended only to illustrate the technical concepts and features of the present invention. Their purpose is to enable those skilled in the art to understand the present invention and implement it accordingly. They are not intended to limit the scope of protection of the present invention. Any modifications made within the spirit of the main technical solution of the present invention are intended to be included within the scope of protection of the present invention.

Claims

1. A trajectory fusion method for heterogeneous SLAM systems based on fuzzy reasoning, characterized by: The steps include: Step 1: Use RGB-D SLAM algorithm and RatSLAM algorithm to build a trajectory map in the heterogeneous SLAM system; Step 2: Use timestamps and ICP algorithm to align the two trajectory maps of the SLAM system in step 1; Step 3: Set two fuzzy inputs by analyzing the pose and point cloud information of RatSLAM and RGB-D SLAM, set fuzzy rules, and establish a fuzzy inference system; the two fuzzy inputs are the pose deviations of RGB-D SLAM and RatSLAM respectively. and the accuracy of RGB-D SLAM relative to RatSLAM The pose deviation of the RGB-D SLAM and RatSLAM is the average pose deviation of the two SLAMs in a small segment of the trajectory. The relative accuracy of the RGB-D SLAM is calculated by the point cloud projection error of the previous and next frames of the RGB-D SLAM in the current trajectory segment. Relative to RatSLAM front and back frame point cloud head projection error The change in the pose deviation between the RGB-D SLAM and RatSLAM is obtained. The calculation formula is: ， Among them, n represents the number of poses of this trajectory, Represents the t-th pose of this trajectory RGB-D SLAM, Represents the t-th pose of RatSLAM in this trajectory; The point cloud projection error of the RatSLAM front and back frames The calculation formula is: ， The point cloud projection error of the RGB-D SLAM front and back frames The calculation formula is: ， in, and They represent the transformation matrix from the t-th point cloud of RatSLAM to the world coordinate system and the transformation matrix from the t-th point cloud of RGB-D SLAM to the world coordinate system in this trajectory, respectively. It is the i-th point of the t-th point cloud among the K pairs of matching points found by RatSLAM's t-th point cloud and t+1 point cloud through K nearest neighbors. It is the i-th point of the t-th point cloud among the K pairs of matching points found by K nearest neighbors between the t-th point cloud and the t+1 point cloud of RGB-D SLAM; Accuracy of RGB-D SLAM relative to RatSLAM The calculation formula is: ； Step 4: Use the maximum membership method to solve the fuzzy system and obtain the weights of RGB-D SLAM and RatSLAM; Step 5: Fusion the two trajectories based on the weights of RGB-D SLAM and RatSLAM. The fusion calculation formula is: ， in, represents the pose after fusion, represents the pose of RGB-D SLAM, Represents the pose of RatSLAM.

2. A heterogeneous SLAM system trajectory fusion method based on fuzzy reasoning according to claim 1, characterized in that: In step 1, the RGB-D camera is used to collect data and input it into RGB-D SLAM and RatSLAM to build two trajectory maps. and .

3. A heterogeneous SLAM system trajectory fusion method based on fuzzy reasoning according to claim 1, characterized in that: In step 2, the trajectory map established by RGB-D SLAM is used as a reference to find the corresponding point set of RGB-D SLAM trajectory and RatSLAM trajectory through timestamps. , and then use the ICP algorithm to align the two trajectory maps. The calculation formula is: ， in, Represents the pose of the RGB-D SLAM matching point, Represents the pose of the RatSLAM matching point.

4. A heterogeneous SLAM system trajectory fusion method based on fuzzy reasoning according to claim 1, characterized in that: The step 3 utilizes triangular fuzzy numbers to fuzzify the two input variables and the output variable, and sets a fuzzy rule table to establish a fuzzy inference system.

5. A heterogeneous SLAM system trajectory fusion method based on fuzzy reasoning according to claim 1, characterized in that: In step 4, the maximum membership method is used to solve the fuzzy system to obtain the confidence of RGB-D SLAM, and the calculation formula is: ， Among them, V represents the domain of the output membership function, is the inverse function of the output membership function, and N is the number of the same maximum membership.

Images