Active feature-assisted adaptive fusion positioning method in point cloud sparse scenes

Abstract

The present invention discloses an adaptive fusion positioning method assisted by active features in a point cloud sparse scene, and solves the problem of reduced positioning accuracy or even inability to position laser SLAM due to sparse point clouds through a multi-sensor fusion solution. The observation amount of laser radar is increased by laying active features on the roadside, and UWB distance observation information is introduced to identify the active feature number scanned by the laser radar; at the same time, the observation accuracy of laser radar and UWB is classified by a fuzzy logic classification algorithm, so as to adaptively adjust the noise variance matrix of laser radar and UWB; finally, the fusion positioning of the vehicle is realized by an extended Kalman filter algorithm. It has the advantages of low cost, high accuracy and good real-time performance.

Description

Technical Field

[0001] The present invention belongs to the field of vehicle navigation and positioning, and in particular relates to an active feature-assisted adaptive fusion positioning method in a point cloud sparse scene. Background Art

[0002] With the rapid development of the economy and society, the number of motor vehicles in my country has increased rapidly, and road traffic is facing huge challenges. In order to solve the increasingly severe and complex traffic problems, Intelligent Transport Systems (ITS) came into being and gradually became the latest research and development direction. The realization of intelligent transportation systems is inseparable from high-precision vehicle navigation and positioning technology. Only under the premise of obtaining accurate and reliable vehicle location information in real time, and implementing the interaction of dynamic real-time information between vehicles and roads in all directions, and fully realizing the effective coordination of people, vehicles and roads, can the safe driving of vehicles be guaranteed and the traffic environment be improved. Therefore, vehicle positioning technology is one of the foundations and core contents of research on intelligent transportation.

[0003] At present, the most widely used technology in the field of vehicle navigation and positioning is the Global Navigation Satellite System (GNSS). In an unobstructed environment, GNSS can provide vehicles with real-time and all-weather information such as three-dimensional position, speed and attitude. However, in confined traffic environments such as tunnels and underground parking lots, due to the obstruction of satellite signals, GNSS will have periodic failures and cannot achieve accurate and reliable continuous positioning.

[0004] In recent years, simultaneous localization and mapping (SLAM) technology has made great progress in scenarios where GNSS signals are blocked. The key technology of SLAM is to construct the pose and environmental map of the moving carrier in an unknown environment, estimate the position of its own movement based on the pose and map information, and build an incremental map along with the movement process to realize the navigation and positioning function of the vehicle. In the implementation of SLAM technology, one of the commonly used sensors is Lidar, which has a wide viewing angle, robustness in low-light environments, and the ability to capture environmental details at a long distance. Compared with cameras, Lidar is not affected by light and has excellent performance during the day and night. However, when the vehicle drives to an area with fewer feature points, Lidar SLAM will reduce the positioning accuracy or even fail to locate due to the sparse point cloud. Therefore, how to achieve high-precision positioning based on Lidar in scenarios with sparse point clouds is a key issue that needs to be solved urgently. Summary of the invention

[0005] The purpose of the present invention is to overcome the shortcomings of the prior art and propose a vehicle positioning method based on Lidar / UWB fusion to achieve accurate and reliable estimation of the vehicle position when laser SLAM cannot locate due to sparse point clouds. The present invention facilitates laser radar detection by arranging special-shaped objects at fixed positions on sections of road where the laser radar point clouds are sparse. The position of the object has been accurately measured in advance. After detecting the feature points of the object, the accurate position information of the object can be known, and the relative position relationship with the object can be solved, which is equivalent to actively providing feature information for the laser radar. Therefore, the special-shaped object is called an active feature in this patent. At the same time, the active feature is equipped with UWB on the top, which can provide numbering information to distinguish different active features and add additional UWB distance auxiliary observation information; finally, according to the distance observation values ​​of the laser radar and UWB signals, the observation accuracy of the laser radar and UWB is classified based on the fuzzy logic classification algorithm, and then the observation noise array of the extended Kalman filter is adaptively adjusted according to the classification results, and finally the precise position of the vehicle after fusion positioning is output in real time. The specific steps are as follows:

[0006] Step 1: Deployment of roadside active features and UWB base stations

[0007] The present invention arranges active features and UWB base stations at equal intervals of 10-20m in areas where the laser radar point cloud is sparse. The active features are low-cost white cylindrical foams, which are installed on corresponding brackets, and corresponding UWB base stations are installed on the top of each active feature. A coordinate system is established with the east direction as the OX axis, the north direction as the OY axis, and a fixed point on the earth's surface as the origin. The positions of each active feature and UWB base station in the coordinate system can be obtained by pre-calibration. The UWB signal receiving tag and laser radar are installed on the top of the moving vehicle.

[0008] Step 2: LiDAR Observation Identification

[0009] According to the layout in step 1, the observation information that the UWB tag can receive at the same time is r 1 ,r 2 ,…,r N , where r N represents the distance between the Nth active feature and the vehicle; the observation information that can be solved by the laser radar after detecting the active feature is d,θ, where d is the distance between an unknown active feature on the roadside and the laser radar, and θ is the absolute value of the angle between the active feature and the laser radar relative to the north direction. Since the laser radar cannot determine the number of the active feature when detecting the active feature, the UWB base station on the top of the active feature just makes up for this shortcoming. By comparing the distance information d solved by the laser radar with the distance information received by the UWB tag, if |dr i|<ε, (i=1,2,…,N, ε is a preset threshold), then the active feature number i (i=1,2,…,N) to which the distance d and angle θ detected by the current laser radar belong can be determined.

[0010] Step 3: Classification of UWB and LiDAR observation accuracy based on fuzzy logic algorithm

[0011] For LiDAR and UWB, the accuracy of object detection and distance measurement will decrease as the distance between the sensor and the object increases. Based on this feature, the present invention uses fuzzy logic algorithm to classify the observation accuracy of UWB base station and LiDAR respectively, which are divided into three levels: A, B, and C. The accuracy of UWB and LiDAR observation values ​​is from low to high. The specific steps of the fuzzy logic classification algorithm are as follows:

[0012] 1) Determine the three membership functions of the distance feature and classify the distances into N, M, and R, representing the distances of "near", "medium", and "far". The three membership functions of distance are as follows (d, unit: meter):

[0013]

[0014]

[0015]

[0016] 2) According to the fuzzy classification rules, determine the three accuracy levels A, B, and C of the fuzzy logic classification output. The fuzzy classification rules are as follows:

[0017] 1. When the distance is N, the accuracy level is A;

[0018] 2. When the distance is M, the accuracy level is B;

[0019] 3. When the distance is R, the accuracy level is C;

[0020] Step 4: Vehicle localization based on adaptive extended Kalman filter

[0021] From step 2, we can see that the observation values ​​that can be received at discrete time k include the distance information r provided by the UWB base station 1 ,r 2 ,…,r N As well as the distance and angle information d and θ of the active features scanned by the laser radar, the real-time vehicle position can be obtained by establishing an extended Kalman filter. The discrete state equation and observation equation are established as follows:

[0022]

[0023] Where k represents discrete time, X(k) = [pe (k)p n (k)]′ is the state vector, the superscript ′ indicates the matrix transposition, p e (k) and p n (k) represents the east-west and north-west position coordinates of the vehicle, and the state transition vector is the observation vector of the system, where is the distance observation value of N UWB base stations, and are the distance and angle observations of the corresponding active features detected by the lidar; h is the corresponding observation function:

[0024]

[0025] (X i ,Y i ) is the coordinate of the ith active feature that has been calibrated in advance, represents the corresponding observation noise vector; w and v are the state noise and observation noise vectors of the system respectively, and their covariance matrices are Q and R respectively, where R = diag[R UWB R Lidar ], R UWB With R Lidar are the observation noise variance matrices of UWB and LiDAR respectively. The observation accuracy of UWB base station and LiDAR are classified according to the fuzzy classification algorithm described in step 3, and then the noise variance matrices R of UWB and LiDAR are adaptively adjusted according to the classification results. UWB , R Lidar , and obtain the final observation noise variance matrix R(k). The specific method is as follows:

[0026] 1. Calculate the observation noise of the jth active feature detected by the i-th UWB base station and the lidar in and are the noise variance base values ​​of UWB distance observation value and lidar active feature observation value respectively; μ and ρ are adjustment coefficients, which are determined by the results of fuzzy classification. The adjustment coefficients corresponding to the three different accuracy levels A, B, and C are μ respectively. a ,μ b ,μ c and ρ a ,ρ b ,ρ c The model adjustment coefficient and accuracy level are shown in the following table:

[0027] Table 1 Distance accuracy level and model adjustment coefficient

[0028] Distance accuracy level Model adjustment coefficient A 1 B 2.5 C 6

[0029] 2. Calculate the observation noise variance matrix of UWB and LiDAR:

[0030] 3. Update the noise variance matrix R(k): R(k) = diag[R UWB R Lidar ];

[0031] After the adaptive adjustment of the system state model, observation model and noise variance matrix described in the above steps, the iterative update of the extended Kalman filter can be performed. The standard extended Kalman filter process includes time update and measurement update:

[0032] Time update:

[0033]

[0034] P(k,k-1)=F(k,k-1)P(k-1)A′(k,k-1)+Q(k-1) (7)

[0035] Measurement Update:

[0036] K(k)=P(k,k-1)H′(k)[H(k)P(k,k-1)H′(k)+R(k)] -1 (8)

[0037]

[0038] P(k)=[IK(k)H(k)]P(k,k-1) (10)

[0039] Where I represents the identity matrix and H is the Jacobian matrix of the derivative of h(·) with respect to X.

[0040] Through filtering calculation, the position information of the vehicle at each moment can be calculated, and reliable positioning of the vehicle can be achieved in sparse point cloud scenarios.

[0041] The remarkable effects of the present invention are:

[0042] (1) The present invention proposes a reliable vehicle positioning method in a scene with sparse point clouds. In view of the fact that the positioning accuracy of the laser SLAM algorithm decreases or even fails to locate in a scene with sparse features, a vehicle fusion positioning method based on Lidar / UWB is proposed. By deploying active features and UWB base stations on the roadside to increase the observation amount of the Lidar, the vehicle positioning accuracy is significantly improved.

[0043] (2) The present invention uses a fuzzy logic algorithm to classify the observation accuracy of UWB and LiDAR, and then adaptively adjusts the observation noise variance matrix of UWB and LiDAR, which can effectively solve the problem that the ranging accuracy decreases as the distance between the sensor and the object increases. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is the technical roadmap of the present invention;

[0045] Figure 2 It is the layout diagram of active features and UWB;

[0046] Figure 3 It is the membership function graph of the fuzzy logic classification algorithm;

[0047] Figure 4 This is a comparison of vehicle positioning trajectories before and after the adaptive extended Kalman filter algorithm was used;

[0048] Figure 5 This is a comparison chart of vehicle positioning errors before and after the adaptive extended Kalman filter algorithm was used. DETAILED DESCRIPTION

[0049] In recent years, intelligent transportation systems have developed rapidly around the world. They can integrate various technologies such as new sensor technology, data communication transmission technology, and information technology into transportation systems, thereby effectively realizing the comprehensive management of transportation systems and maximizing the harmony between people, vehicles, and roads. For most intelligent transportation systems, obtaining accurate and reliable location information is a prerequisite for their function.

[0050] At present, the most widely used technology in the field of vehicle navigation and positioning is the Global Navigation Satellite System (GNSS). In an unobstructed environment, GNSS can provide vehicles with real-time and all-weather information such as three-dimensional position, speed and attitude. However, in confined traffic environments such as tunnels and underground parking lots, due to the obstruction of satellite signals, GNSS will have periodic failures and cannot achieve accurate and reliable continuous positioning.

[0051] In recent years, simultaneous localization and mapping (SLAM) technology has made great progress in scenarios where GNSS signals are blocked. The key technology of SLAM is to construct the pose and environmental map of the moving carrier in an unknown environment, estimate the position of its own movement based on the pose and map information, and build an incremental map along with the movement process to realize the navigation and positioning function of the vehicle. Many implementations of SLAM technology utilize Lidar sensors because they have a wide viewing angle, robustness in low-light environments, and the ability to capture environmental details at a long distance. Compared with cameras, Lidar is not affected by light and has excellent performance during the day and night. However, when the vehicle drives to an area with fewer feature points, Lidar SLAM will reduce the positioning accuracy or even fail to locate due to the sparse point cloud. Therefore, in scenarios with sparse point clouds, combining road traffic facilities, selecting a suitable method to achieve high-precision positioning based on Lidar is a key issue that needs to be solved urgently.

[0052] In order to make up for the problem of reduced positioning accuracy or even inability to position due to sparse point clouds in laser SLAM, the present invention proposes a vehicle fusion positioning method based on Lidar / UWB, which increases the Lidar observation volume by laying out white cylindrical active features on the road sections with sparse Lidar point clouds; at the same time, a UWB base station is installed on the top of the active features to cooperate with the Lidar to identify the corresponding active feature number, and add additional UWB distance auxiliary observation information; finally, according to the distance characteristics of the Lidar and UWB signals, the observation accuracy of the Lidar and UWB is classified based on the fuzzy logic classification algorithm, and then the observation noise array of the extended Kalman filter is adaptively adjusted according to the classification results, and finally the precise position of the vehicle after fusion positioning is output in real time. The specific steps are as follows:

[0053] Step 1: Deployment of roadside active features and UWB base stations

[0054] The present invention arranges active features and UWB base stations at equal intervals of 10-20m in areas where the laser radar point cloud is sparse. The active features are low-cost white cylindrical foams, which are installed on corresponding brackets, and corresponding UWB base stations are installed on the top of each active feature. A coordinate system is established with the east direction as the OX axis, the north direction as the OY axis, and a fixed point on the earth's surface as the origin. The positions of each active feature and UWB base station in the coordinate system can be obtained by pre-calibration. The UWB signal receiving tag and laser radar are installed on the top of the moving vehicle.

[0055] Step 2: LiDAR Observation Identification

[0056] According to the layout in step 1, the observation information that the UWB tag can receive at the same time is r1 ,r 2 ,…,r N , where r N represents the distance between the Nth active feature and the vehicle; the observation information that can be solved by the laser radar after detecting the active feature is d,θ, where d is the distance between an unknown active feature on the roadside and the laser radar, and θ is the absolute value of the angle between the active feature and the laser radar relative to the north direction. Since the laser radar cannot determine the number of the active feature when detecting the active feature, the UWB base station on the top of the active feature just makes up for this shortcoming. By comparing the distance information d solved by the laser radar with the distance information received by the UWB tag, if |dr i |<ε, (i=1,2,…,N, ε is a preset threshold), then the active feature number i (i=1,2,…,N) to which the distance d and angle θ detected by the current laser radar belong can be determined.

[0057] Step 3: Classification of UWB and LiDAR observation accuracy based on fuzzy logic algorithm

[0058] For LiDAR and UWB, the accuracy of object detection and distance measurement will decrease as the distance between the sensor and the object increases. Based on this feature, the present invention uses fuzzy logic algorithm to classify the observation accuracy of UWB base station and LiDAR respectively, which are divided into three levels: A, B, and C. The accuracy of UWB and LiDAR observation values ​​is from low to high. The specific steps of the fuzzy logic classification algorithm are as follows:

[0059] 1) Determine the three membership functions of the distance feature and classify the distances into N, M, and R, representing the distances of "near", "medium", and "far". The three membership functions of distance are as follows (d, unit: meter):

[0060]

[0061]

[0062]

[0063] 2) According to the fuzzy classification rules, determine the three accuracy levels A, B, and C of the fuzzy logic classification output. The fuzzy classification rules are as follows:

[0064] 1. When the distance is N, the accuracy level is A;

[0065] 2. When the distance is M, the accuracy level is B;

[0066] 3. When the distance is R, the accuracy level is C;

[0067] Step 4: Vehicle localization based on adaptive extended Kalman filter

[0068] From step 2, we can see that the observation values ​​that can be received at discrete time k include the distance information r provided by the UWB base station 1 ,r 2 ,…,r N As well as the distance and angle information d and θ of the active features scanned by the laser radar, the real-time vehicle position can be obtained by establishing an extended Kalman filter. The discrete state equation and observation equation are established as follows:

[0069]

[0070] Where k represents discrete time, X(k) = [p e (k)p n (k)]′ is the state vector, the superscript ′ indicates the matrix transposition, p e (k) and p n (k) represents the east-west and north-west position coordinates of the vehicle, and the state transition vector is the observation vector of the system, where is the distance observation value of N UWB base stations, and are the distance and angle observations of the corresponding active features detected by the lidar; h is the corresponding observation function:

[0071]

[0072] (X i ,Y i ) is the coordinate of the ith active feature that has been calibrated in advance, represents the corresponding observation noise vector; w and v are the state noise and observation noise vectors of the system respectively, and their covariance matrices are Q and R respectively, where R = diag[R UWB R Lidar ], R UWB With R Lidar are the observation noise variance matrices of UWB and LiDAR respectively. The observation accuracy of UWB base station and LiDAR are classified according to the fuzzy classification algorithm described in step 3, and then the noise variance matrices R of UWB and LiDAR are adaptively adjusted according to the classification results. UWB , R Lidar , and obtain the final observation noise variance matrix R(k). The specific method is as follows:

[0073] 1. Calculate the observation noise of the jth active feature detected by the i-th UWB base station and the lidar in and are the noise variance base values ​​of UWB distance observation value and lidar active feature observation value respectively; μ and ρ are adjustment coefficients, which are determined by the results of fuzzy classification. The adjustment coefficients corresponding to the three different accuracy levels A, B, and C are μ respectively. a ,μ b ,μ c and ρ a ,ρ b ,ρ c The model adjustment coefficient and accuracy level are shown in the following table:

[0074] Table 1 Distance accuracy level and model adjustment coefficient

[0075] Distance accuracy level Model adjustment coefficient A 1 B 2.5 C 6

[0076] 2. Calculate the observation noise variance matrix of UWB and LiDAR:

[0077] 3. Update the noise variance matrix R(k): R(k) = diag[R UWB R Lidar ];

[0078] After the adaptive adjustment of the system state model, observation model and noise variance matrix described in the above steps, the iterative update of the extended Kalman filter can be performed. The standard extended Kalman filter process includes time update and measurement update:

[0079] Time update:

[0080]

[0081] P(k,k-1)=F(k,k-1)P(k-1)F′(k,k-1)+Q(k-1) (7)

[0082] Measurement Update:

[0083] K(k)=P(k,k-1)H′(k)[H(k)P(k,k-1)H′(k)+R(k)] -1 (8)

[0084]

[0085] P(k)=[IK(k)H(k)]P(k,k-1) (10)

[0086] Where I represents the identity matrix and H is the Jacobian matrix of the derivative of h(·) with respect to X.

[0087] Through filtering calculation, the position information of the vehicle at each moment can be calculated, and reliable positioning of the vehicle can be achieved in sparse point cloud scenarios.

[0088] In this implementation example, in order to verify the beneficial effect of the proposed method on improving the vehicle positioning accuracy, a real vehicle experiment was carried out, and a comparative test was conducted on the vehicle positioning results before and after the fuzzy adaptive algorithm was adopted. In addition, it should be pointed out that the method "before the fuzzy algorithm is used" in the experiment here refers to the Lidar / UWB fusion method based on the traditional extended Kalman filter, and the method "after the fuzzy algorithm is used" refers to the Lidar / UWB fusion method based on the adaptive extended Kalman filter.

[0089] Figure 4 This is a set of test result diagrams. It can be clearly seen from the diagram that after using the adaptive extended Kalman filter algorithm, the vehicle positioning trajectory is closer to the true value. Figure 5 This is a comparison chart of the Euclidean error of vehicle positioning before and after the adaptive extended Kalman filter algorithm is used. It can be clearly seen from the positioning error in the figure that after using the observation noise matrix of the adaptively adjusted extended Kalman filter, the positioning error is reduced and the vehicle positioning accuracy is significantly improved.

Claims

1. An adaptive fusion positioning method assisted by active features in point cloud sparse scenes, characterized by: When the laser SLAM positioning accuracy is reduced or even impossible due to the sparse point cloud, the observation amount of the laser radar is increased by deploying active features on the roadside, and the UWB distance observation information is introduced to identify the active feature number scanned by the laser radar; at the same time, the observation accuracy of the laser radar and UWB is classified by the fuzzy logic classification algorithm, so as to adaptively adjust the noise variance matrix of the laser radar and UWB; finally, the vehicle fusion positioning is realized by the extended Kalman filter algorithm; the specific steps are as follows: Step 1: Deployment of roadside active features and UWB base stations In areas where the laser radar point cloud is sparse, active features and UWB base stations are arranged at equal intervals of 10-20m; the active features are low-cost white cylindrical foams, which are installed on the corresponding brackets, and the corresponding UWB base stations are installed on the top of each active feature; a coordinate system is established with the east direction as the OX axis, the north direction as the OY axis, and a fixed point on the earth's surface as the origin; the positions of each active feature and UWB base station in the coordinate system can be obtained through pre-calibration; the UWB signal receiving tag and laser radar are installed on the top of the moving vehicle; Step 2: LiDAR Observation Identification According to the layout in step 1, the observation information that the UWB tag can receive at the same time is r1, r2, …, r N , where r N represents the distance between the Nth active feature and the vehicle; the observation information that can be solved after the laser radar detects the active feature is d,θ, where d is the distance between an unknown active feature on the roadside and the laser radar, and θ is the absolute value of the angle between the active feature and the laser radar relative to the north direction; since the laser radar cannot determine the number of the active feature when detecting the active feature, the UWB base station on the top of the active feature just makes up for this shortcoming; by comparing the distance information d solved by the laser radar with the distance information received by the UWB tag, if |dr i |<ε, where i=1, 2, …, N, and ε is a preset threshold, then the active feature number i to which the distance d and angle θ detected by the current laser radar belong can be determined; Step 3: Classification of UWB and LiDAR observation accuracy based on fuzzy logic algorithm For LiDAR and UWB, the accuracy of object detection and distance measurement will decrease as the distance between the sensor and the object increases. Based on this feature, the fuzzy logic algorithm is used to classify the observation accuracy of UWB base station and LiDAR respectively, which are divided into three levels: A, B, and C. The accuracy of UWB and LiDAR observation values ​​is from low to high. The specific steps of the fuzzy logic classification algorithm are as follows: 1) Determine the three membership functions of the distance feature and classify the distances into N, M, and R, representing the distances of "near", "medium", and "far". The three membership functions of the distances are shown below, where: d, in meters: 2) According to the fuzzy classification rules, determine the three accuracy levels A, B, C of the fuzzy logic classification output. The fuzzy classification rules are as follows: (1) When the distance is N, the accuracy level is A; (2) When the distance is M, the accuracy level is B; (3) When the distance is R, the accuracy level is C; Step 4: Vehicle localization based on adaptive extended Kalman filter From step 2, we can see that the observation values ​​that can be received at discrete time k include the distance information r1, r2, …, r provided by the UWB base station. N As well as the distance and angle information d and θ of the active features scanned by the laser radar, the real-time vehicle position can be obtained by establishing an extended Kalman filter; the discrete state equation and observation equation are established as follows: Where k represents discrete time, X(k) = [p e (k)p n (k)]′ is the state vector, the superscript ′ indicates the matrix transposition, p e (k) and p n (k) represents the east-west and north-west position coordinates of the vehicle, and the state transition vector is the observation vector of the system, where is the observed distance value of N UWB base stations, and are the distance and angle observations of the corresponding active features detected by the lidar; h is the corresponding observation function: (X i ,Y i ) is the coordinate of the ith active feature that has been calibrated in advance, n ri represents the corresponding observation noise vector; w and v are the state noise and observation noise vectors of the system respectively, and their covariance matrices are Q and R respectively, where R = diag[R UWB R Lidar ], R UWB With R Lidar are the observation noise variance matrices of UWB and LiDAR respectively. The observation accuracy of UWB base station and LiDAR is classified according to the fuzzy logic classification algorithm described in step 3, and then the noise variance matrix R of UWB and LiDAR is adaptively adjusted according to the classification results. UWB , R Lidar , and obtain the final observation noise variance matrix R(k). The specific method is as follows: (1) Calculate the observation noise of the jth active feature detected by the i-th UWB base station and the lidar in and are the noise variance base values ​​of UWB distance observation value and lidar active feature observation value respectively; μ and ρ are adjustment coefficients, which are determined by the results of fuzzy classification. The adjustment coefficients corresponding to the three different accuracy levels A, B, and C are μ respectively. a ,μ b ,μ c and ρ a ,ρ b ,ρ c ; The model adjustment coefficient and accuracy level are as follows: the model adjustment coefficients of the distance accuracy levels A, B, and C are 1, 2.5, and 6 respectively; (2) Calculate the observation noise variance matrix of UWB and lidar: (3) Update the noise variance matrix R(k): R(k) = diag[R UWB R Lidar ]; After the adaptive adjustment of the system state model, observation model and noise variance matrix described in the above steps, the iterative update of the extended Kalman filter can be performed. The standard extended Kalman filter process includes time update and measurement update: Time update: P(k,k-1)=F(k,k-1)P(k-1)F′(k,k-1)+Q(k-1)(7) Measurement Update: K(k)=P(k,k-1)H′(k)[H(k)P(k,k-1)H′(k)+R(k)] -1 (8) P(k)=[IK(k)H(k)]P(k,k-1)(10) Where I represents the identity matrix, H is the Jacobian matrix of the derivative of h(·) with respect to X; Through filtering calculation, the position information of the vehicle at each moment can be calculated, and reliable positioning of the vehicle can be achieved in sparse point cloud scenarios.

Images