A sensor fusion indoor positioning method for unmanned ground vehicles

Abstract

This invention relates to a sensor fusion indoor positioning method for an unmanned ground vehicle, comprising the following steps: 1. Establishing a discretized kinematic model of a four-wheel drive vehicle; 2. Establishing a visual measurement model based on camera and inertial measurement unit (IMU) measurement data; 3. Designing a model based on the visual measurement model and an extended H... ∞ 4. Design a visual estimator for the filter; 5. Design a lidar estimator based on the adaptive Monte Carlo localization method; 6. Design a fusion estimator based on the visual estimator and lidar estimator; The method of this invention is based on the establishment of a visual estimator, a lidar estimator and a fusion estimator, which realizes the robustness of indoor localization for unmanned ground vehicles and solves the problems of camera information loss, noise model uncertainty and poor localization reliability of lidar in similar map scenes in the prior art.

Description

Technical fields:

[0001] This invention belongs to the field of vehicle sensors, specifically relating to an indoor positioning method for sensor fusion of unmanned ground vehicles. Background technology:

[0002] The high level of intelligence and ease of operation of unmanned ground vehicles (UGVs) provide a fast, efficient, and convenient transportation method for indoor scenarios such as warehouses. The ability to perceive its surroundings and perform collaborative actions through sensor-vehicle communication is considered a revolutionary force in indoor transportation. Positioning accuracy has always been a key focus and challenge in UGV research, with LiDAR and camera sensors finding wide application in indoor positioning. Simultaneously, various sensor combination positioning technologies, such as inertial navigation and ultra-wideband technology, also play a crucial role in indoor positioning. Extensive research and applications have demonstrated that accurate sensor information can significantly improve indoor transportation safety, traffic efficiency, and time efficiency.

[0003] Considering the cost of individual sensors and the complexity and diversity of indoor scenes, each sensor has its own advantages and disadvantages in different indoor environments. Multi-sensor fusion can complement the strengths of each sensor. For robots equipped with LiDAR, adaptive Monte Carlo localization is a commonly used method that can adapt to most indoor scenes. However, its localization reliability is poor when the map scene is similar and environmental features are insufficient. Given the limited computing power of autonomous vehicles, using cameras to detect QR codes for indoor localization is an effective method. By assigning the absolute position and orientation of each QR code in the world coordinate system, the vehicle's position and orientation in the world coordinate system can be calculated. Although camera localization can obtain rich image information for more accurate positioning, the working principle of cameras also brings two key problems: firstly, cameras are easily affected by changes in lighting, leading to information loss; secondly, due to the uncertainty of noise in the camera-detected images, it is generally difficult to provide a specific model of the noise, thus limiting the accuracy of camera localization. All of these factors can affect the localization performance of autonomous ground vehicles in indoor scenes, and may even lead to traffic accidents and unnecessary losses. Summary of the Invention:

[0004] The purpose of this invention is to propose a sensor fusion indoor positioning method for unmanned ground vehicles, which addresses the advantages and disadvantages of lidar and camera sensors in different indoor scenarios. This method allows the advantages of the two sensors to complement each other, solves the problem of noise uncertainty, and improves the reliability of indoor positioning for unmanned ground vehicles.

[0005] The technical solution of this invention is: a sensor fusion indoor positioning method for unmanned ground vehicles, the method comprising the following steps:

[0006] Step 1: Establish a discretized kinematic model of the four-wheel drive vehicle;

[0007] Step 2: Establish a visual measurement model based on camera and inertial measurement unit measurement data;

[0008] Step 3: Design a vision-based measurement model and extended H ∞ A visual estimator for filters;

[0009] Step 4: Design a lidar estimator based on the adaptive Monte Carlo localization method;

[0010] Step 5: Design a fusion estimator based on the visual estimator and the lidar estimator.

[0011] Furthermore, the process of establishing the discretized kinematic model of the four-wheel drive vehicle is as follows:

[0012] Considering that the vehicle speed of a four-wheel drive vehicle is related to the speed of each wheel during movement, its kinematic model can be equivalent to that of a two-wheel differential vehicle. The kinematic model is established based on the instantaneous center of rotation:

[0013]

[0014] Among them, v u (t) represents the overall linear velocity of the vehicle, ω u (t) represents the overall angular velocity of the vehicle, v l (t) and v r (t) represents the linear velocities of the left and right wheels of the vehicle after the model is equivalent, respectively, ω l (t) and ω r (t) represents the angular velocities of the left and right wheels of the vehicle after the model is equivalent, respectively, and d represents the angular velocities of the left and right wheels. LR Let ξ(t) represent the distance between the left and right wheels after the model is equivalent, and r be the wheel radius. This is achieved by introducing the state variable ξ(t) = [x(t), y(t), θ(t)]. T Where x(t) and y(t) are the X-axis and Y-axis components of the vehicle's rear axle center point in the world coordinate system, and θ(t) is the vehicle's heading angle, a new vehicle kinematic model is obtained:

[0015]

[0016] in,

[0017] μ and σ represent the uncertainties in the overall linear velocity and angular velocity of the vehicle, respectively. ICR Ω(t) is the distance between the instantaneous center of rotation of the vehicle and the center of gravity of the vehicle. r (t),ω l (t)] T ; using the forward Euler method with a sampling period Ts Discretize the kinematic model as follows:

[0018]

[0019] Where ξ k Indicates kT s The vehicle's state variables at any given time. ΔΨ k =Ω k T s =[ΔΨ r,k ,ΔΨ l,k ] T It's kT s and (k+1)T s angular displacement between, δ Ψ,k It is a random noise vector.

[0020] Furthermore, a visual measurement model based on camera and inertial measurement unit measurement data is established:

[0021] First, the position at time k is measured:

[0022] Assigning fixed position information (x, y) to the QR code in the world coordinate system a ,y a and offset angle information θ a Using a camera to detect a fixed QR code yields:

[0023]

[0024] in, and These represent the horizontal and vertical distances of the camera relative to the QR code, respectively. For camera and θ a angular difference, and This represents the uncertainty factor of the corresponding variable; let The estimated position output of the camera is as follows:

[0025]

[0026] in, Represents the state vector of the vision system. For the positional information of the vision system, For the heading angle information of the vision system, when the camera detects the QR code When the camera does not detect the QR code Obtained through iteration of the visual estimator;

[0027] Secondly, the heading angle at time k is measured:

[0028] In a vision system, the vehicle heading angle information obtained by detecting a QR code using a camera is: The vehicle heading angle information obtained through the inertial measurement unit is: Where, ω k Let k be the angular velocity measurement value of the inertial measurement unit at time k. This represents the vehicle's heading estimation error; combining the above, the measurement of the heading angle at time k can be expressed as:

[0029]

[0030] Finally, the visual measurement model is derived as follows:

[0031]

[0032] Where, ε k The measurement noise of the vision system is assumed to be random noise.

[0033] Furthermore, the design is based on a visual measurement model and extended H ∞ Visual estimator for filters:

[0034] First, the extended H is given. ∞ The filter's indicator function is:

[0035]

[0036] The operational rules for the operator ||·|| are as follows: a and M are a vector and a matrix of appropriate dimensions, respectively. and They depend on δ Ψ,j and ε j A symmetric positive definite weighted matrix; based on the indicator function J k Set a threshold Make The expression for the visual estimator can be obtained as follows:

[0037]

[0038] Among them, F k and G k They represent in place ξ v and Δψ for The Jacobian matrix, H k+1 Indicates in place ξ v for Jacobian matrix,

[0039]

[0040] The non-negative coefficients are obtained from the 99th percentile of the position error. They represent variance

[0041]

[0042] Furthermore, the design of the lidar estimator based on the adaptive Monte Carlo localization method includes the following steps:

[0043] The radar estimator is designed based on an improved adaptive Monte Carlo localization method. This improved adaptive Monte Carlo localization method feeds back the information from the fusion estimator to the adaptive Monte Carlo localization method, thereby solving problems such as scene matching errors and cumulative odometry errors. The adaptive Monte Carlo localization method mainly consists of four steps: (1) initializing particle scattering; (2) simulating the motion of each particle; (3) calculating the score and weight of each particle; and (4) particle resampling.

[0044] First, define the state variable of the radar estimator at time k as follows: And the state variable of the odometer information at time k is Define M as the number of particles. A collection of particles. The value represents the radar observation, and m represents the raster map data built based on the known scene. Let be the weight of the i-th particle. Based on and The probability of an increase in particle number is then assessed by monitoring sensor measurements, as shown below:

[0045]

[0046] Where, subscript 1:k represents the sequence from time 1 to time k; the average weight ω of all particles avg As shown below:

[0047]

[0048] Then we get:

[0049]

[0050] Where, ω slow and ω fast Let β represent the long-run likelihood estimate and the short-run likelihood estimate, respectively. slow and β fast Two indicator parameters and 0 ≤ β slow<β fast The resulting improved adaptive Monte Carlo localization method is shown below:

[0051]

[0052] in, The state variables are the output of the fusion estimator; the results of the fusion estimator are used to correct the state information of the radar estimator and odometry, and then the average weight of all particles is obtained in the first iteration.

[0053]

[0054] Where i iterates from 1 to M, Let i be the state of the i-th particle; based on the average weight ω of all particles avg The long-term likelihood estimate ω is obtained. slow and short-term likelihood estimation ω fast As shown below:

[0055] ω slow =ω slow +β slow (ω avg -ω slow )

[0056] ω fast =ω fast +β fast (ω avg -ω fast ),

[0057] Then, the second iteration process is initiated to obtain the final state information:

[0058] Before resampling, with probability max{0.0,1.0-ω} fast / ω slow To add random particles, in China-Israel probability To get The final state of the car was then obtained as follows:

[0059]

[0060] Finally, the covariance matrix of the radar estimator is obtained as follows:

[0061]

[0062] Where Ε[·] represents the expected value.

[0063] Furthermore, a fusion estimator is designed based on the visual estimator and the lidar estimator;

[0064] To obtain the linear minimum covariance, the expression for the fusion estimator is:

[0065]

[0066] in, n = v, l are the weight parameter matrices. Let be the error covariance matrix of the fusion estimator.

[0067] Beneficial effects

[0068] Compared with the prior art, the present invention has the following advantages:

[0069] 1. Considering the problems of unmodelable errors in the vision system caused by the cumulative error of the inertial measurement unit and the random noise of the camera, as well as the problem of information loss from the camera, a system based on extended H was designed. ∞ The visual estimator for the filter achieves negligible noise modeling while compensating for the robustness of camera information loss through iteration.

[0070] 2. Considering the cumulative error of the odometer and the poor reliability of adaptive Monte Carlo positioning in similar map environments, a radar estimator based on an improved adaptive Monte Carlo positioning method was designed, which improves the accuracy of indoor positioning while correcting the cumulative error of the odometer.

[0071] 3. By fusing the output information of two visual estimators through a weighted matrix, a fusion estimator is obtained, realizing indoor positioning with multi-sensor fusion. The proposed estimator design method is robust to indoor positioning information processing. Attached image description:

[0072] Figure 1 This is a flowchart of a sensor fusion indoor positioning method for an unmanned ground vehicle according to the present invention.

[0073] Figure 2 This is a schematic diagram of the parameters of the four-wheel drive vehicle of the present invention in the world coordinate system;

[0074] Figure 3 This is a schematic diagram of the parameters of the visual measurement model of the present invention in the world coordinate system;

[0075] Figure 4 This is a flowchart of the improved adaptive Monte Carlo localization method of the present invention. Detailed implementation method:

[0076] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments.

[0077] This invention provides a sensor fusion indoor positioning method for unmanned ground vehicles. Figure 1This invention provides a flowchart of a sensor fusion indoor positioning method for an unmanned ground vehicle. Specifically, it includes the following steps:

[0078] Step 1, Figure 2 This is a schematic diagram of the parameters of the four-wheel drive vehicle of the present invention in the world coordinate system. Considering that the vehicle speed is related to the speed of each wheel when the four-wheel drive vehicle is in motion, its kinematic model can be equivalent to the kinematic model of a two-wheel differential vehicle. Based on the instantaneous center of rotation and the relationship between the vehicle's linear velocity and angular velocity, the kinematic model is established as follows:

[0079]

[0080] Among them, v u (t) represents the overall linear velocity of the vehicle, ω u (t) represents the overall angular velocity of the vehicle, v l (t) and v r (t) represents the linear velocities of the left and right wheels of the vehicle after the model is equivalent, respectively, ω l (t) and ω r (t) represents the angular velocities of the left and right wheels of the vehicle after the model is equivalent, respectively, and d represents the angular velocities of the left and right wheels. LR Let ξ(t) represent the distance between the left and right wheels after the model is equivalent, and r be the wheel radius. This is achieved by introducing the state variable ξ(t) = [x(t), y(t), θ(t)]. T Where x(t) and y(t) are the X-axis and Y-axis components of the vehicle's rear axle center point in the world coordinate system, and θ(t) is the vehicle's heading angle, a new vehicle kinematic model is obtained:

[0081]

[0082] in,

[0083] μ and σ represent the uncertainties in the overall linear velocity and angular velocity of the vehicle, respectively. ICR Ω(t) is the distance between the instantaneous center of rotation of the vehicle and the center of gravity of the vehicle. r (t),ω l (t)] T Using the forward Euler method with a sampling period T s Discretize the kinematic model as follows:

[0084]

[0085] Where ξ k Indicates kT s The vehicle's state variables at any given time. ΔΨ k =Ω k T s =[ΔΨ r,k,ΔΨ l,k ] T It's kT s and (k+1)T s angular displacement between, δ Ψ,k It is a random noise vector.

[0086] Step 2, Figure 3 This is a schematic diagram of the parameters of the visual measurement model of the present invention in the world coordinate system. The establishment of the visual measurement model based on measurement data from the camera and inertial measurement unit is as follows:

[0087] First, the position at time k is measured:

[0088] Assigning fixed position information (x, y) to the QR code in the world coordinate system a ,y a and offset angle information θ a Using a camera to detect a fixed QR code yields:

[0089]

[0090] in, and These represent the horizontal and vertical distances of the camera relative to the QR code, respectively. For camera and θ a angular difference, and This represents the uncertainty factor of the corresponding variable. Let... The estimated position output of the camera is as follows:

[0091]

[0092] in, Represents the state vector of the vision system. For the positional information of the vision system, For the heading angle information of the vision system, when the camera detects the QR code When the camera does not detect the QR code It is obtained through iteration of the visual estimator.

[0093] Secondly, the heading angle at time k is measured:

[0094] In a vision system, the vehicle heading angle information obtained by detecting a QR code using a camera is: The vehicle heading angle information obtained through the inertial measurement unit is: Where, ω k Let k be the angular velocity measurement value of the inertial measurement unit at time k. This represents the vehicle's heading estimation error. Combining the above, the heading angle measurement at time k can be expressed as:

[0095]

[0096] Finally, the visual measurement model is derived as follows:

[0097]

[0098] Where, ε k The measurement noise of the vision system is assumed to be random noise.

[0099] Step 3, Design: Design based on visual measurement model and extended H ∞ Visual estimator for filters:

[0100] First, the extended H is given. ∞ The filter's indicator function is:

[0101]

[0102] The operational rules for the operator ||·|| are as follows: a and M are a vector and a matrix of appropriate dimensions, respectively. and They depend on δ Ψ,j and ε j A symmetric positive definite weighted matrix. Based on the indicator function J... k Set a threshold Make The expression for the visual estimator can be obtained as follows:

[0103]

[0104] Among them, F k and G k They represent in place ξ v and Δψ for The Jacobian matrix, H k+1 Indicates in place ξ v for Jacobian matrix,

[0105]

[0106] The non-negative coefficients are obtained from the 99th percentile of the position error. They represent variance

[0107]

[0108] Step 4, Figure 4 The flowchart of the improved adaptive Monte Carlo localization method of this invention is shown below. A lidar estimator based on the adaptive Monte Carlo localization method is designed as follows:

[0109] The radar estimator is designed based on an improved adaptive Monte Carlo localization method. The adaptive Monte Carlo localization method mainly consists of four steps: (1) initializing the particle dispersion; (2) simulating the motion of each particle; (3) calculating the score, i.e., the weight, of each particle; and (4) particle resampling. First, the state variable of the radar estimator at time k is defined as... And the state variable of the odometer information at time k is Define M as the number of particles. A collection of particles. The value represents the radar observation, and m represents the raster map data built based on the known scene. Let be the weight of the i-th particle. Based on and The probability of an increase in particle number is then assessed by monitoring sensor measurements as follows:

[0110]

[0111] Where the subscript 1:k represents the sequence from time 1 to time k. The average weight ω of all particles avg As shown below:

[0112]

[0113] Then we get:

[0114]

[0115] Where, ω slow and ω fast Let β represent the long-run likelihood estimate and the short-run likelihood estimate, respectively. slow and β fast Two indicator parameters and 0 ≤ β slow <β fast The resulting improved adaptive Monte Carlo localization method is shown below:

[0116]

[0117] in, The state variables are the output of the fusion estimator; the results of the fusion estimator are used to correct the state information of the radar estimator and odometry, and then the average weight of all particles is obtained in the first iteration.

[0118]

[0119] Where i iterates from 1 to M, Let i be the state of the i-th particle; based on the average weight ω of all particles avg The long-term likelihood estimate ω is obtained. slow and short-term likelihood estimation ω fast As shown below:

[0120] ω slow =ω slow +β slow (ω avg -ω slow )

[0121] ω fast =ω fast +β fast (ω avg -ω fast ),

[0122] Then, the second iteration process is initiated to obtain the final state information:

[0123] Before resampling, with probability max{0.0,1.0-ω} fast / ω slow To add random particles, in China-Israel probability To get The final state of the car was then obtained as follows:

[0124]

[0125] To more intuitively illustrate the localization method of the lidar estimator, the pseudocode for the improved adaptive Monte Carlo localization method is as follows:

[0126]

[0127] Finally, the covariance matrix of the radar estimator is obtained as follows:

[0128]

[0129] Where Ε[·] represents the expected value.

[0130] Step 5: Design a fusion estimator based on the visual estimator and the LiDAR estimator:

[0131] To obtain the linear minimum covariance, the inverse matrix of the sum of the inverses of the covariances of the visual estimator and the lidar estimator is multiplied by the inverse matrix of the covariance of each estimator itself to obtain the weights of each estimator. Then, the weights are weighted and summed with the estimated states to obtain the final fused state. The expression for the fused estimator is then:

[0132]

[0133] in, n = v, l are the weight parameter matrices. Let be the error covariance matrix of the fusion estimator.

Claims

1. A sensor fusion indoor positioning method for an unmanned ground vehicle, characterized in that, Includes the following steps: Step 1: Establish a discretized kinematic model of the four-wheel drive vehicle; Step 2: Establish a visual measurement model based on camera and inertial measurement unit measurement data; Step 3: Design a vision-based measurement model and its extension A visual estimator for filters; Step 4: Design a lidar estimator based on the adaptive Monte Carlo localization method; Step 5: Design a fusion estimator based on the visual estimator and the LiDAR estimator; In step 3, the design is based on a visual measurement model and an extension. The visual estimator process for the filter: First, let's give the extension. The filter's indicator function is: ， Among them, the operator The operation rules are as follows , and These are vectors and matrices of appropriate dimensions, respectively. , , and They depend on , and A symmetric positive definite weighted matrix; based on the indicator function Set a threshold Make The visual estimator expression can be obtained as follows: ； in, and They represent in Place and for Jacobian matrix, Indicates in Place for Jacobian matrix, ， ， The non-negative coefficients are obtained from the 99th percentile of the position error. They represent variance 。 2. The sensor fusion indoor positioning method for an unmanned ground vehicle according to claim 1, characterized in that, Step 1, the process of establishing the discretized kinematic model of the four-wheel drive vehicle: A kinematic model is established based on the instantaneous center of rotation: ， in, The overall linear velocity of the vehicle. The overall angular velocity of the vehicle. and These represent the linear velocities of the left and right wheels of the vehicle after the model is equivalent. and These represent the angular velocities of the left and right wheels of the vehicle after the model is equivalent. This is the distance between the left and right wheels after the model is equivalent. To determine the wheel radius, a state variable is introduced. ,in and The rear axle center point of the vehicle in the world coordinate system Axial components and Axial components, Given the vehicle's heading angle, a new vehicle kinematic model is obtained: ， in, , and These are the uncertainties in the overall linear velocity and angular velocity of the vehicle, respectively. The distance between the vehicle's instantaneous center of rotation and its center of gravity. ; using the forward Euler method with sampling period Discretize the kinematic model as follows: ， in express The vehicle's state variables at any given time. , yes and angular displacement between It is a random noise vector.

3. The sensor fusion indoor positioning method for an unmanned ground vehicle according to claim 1, characterized in that, In step 2, the establishment of the visual measurement model based on camera and inertial measurement unit measurement data is as follows: First, the position at time k is measured: Assigning fixed position information to QR codes in the world coordinate system and offset angle information Using a camera to detect a fixed QR code yields: ， in, and These represent the horizontal and vertical distances of the camera relative to the QR code, respectively. For cameras and angular difference, , and This represents the uncertainty factor of the corresponding variable; let The estimated position output of the camera is: ， in, , Represents the state vector of the vision system. For the positional information of the vision system, For the heading angle information of the vision system, when the camera detects the QR code When the camera does not detect the QR code, Obtained through iteration of the visual estimator; Secondly, the heading angle at time k is measured: In a vision system, the vehicle heading angle information obtained by detecting a QR code using a camera is: The vehicle heading angle information obtained through the inertial measurement unit is ,in, Let k be the angular velocity measurement value of the inertial measurement unit at time k. This represents the vehicle's heading estimation error; combining the above, the measurement of the heading angle at time k can be expressed as: ； Finally, the visual measurement model is derived as follows: ， in, The measurement noise of the vision system is assumed to be random noise.

4. The sensor fusion indoor positioning method for an unmanned ground vehicle according to claim 1, characterized in that, In step 4, the design is based on a lidar estimator using an adaptive Monte Carlo localization method: First, define the state variable of the radar estimator at time k as follows: And the state variable of the odometer information at time k is ,definition For the number of particles, A collection of particles. This represents the radar's observations. This represents raster map data created based on a known scene. For the first The weight of each particle, weight Based on and The probability of an increase in particle number is then assessed by monitoring sensor measurements, as shown below: ， Among them, subscript Represents the sequence from time 1 to time k; the average weight of all particles. As shown below: ， Then we get: ， in, and Let these represent the long-run likelihood estimate and the short-run likelihood estimate, respectively. and Two indicator parameters and The resulting improved adaptive Monte Carlo localization method is shown below: ， in, The state variables are the output of the fusion estimator; the results of the fusion estimator are used to correct the state information of the radar estimator and odometry, and then the average weight of all particles is obtained in the first iteration. ， Where i iterates from 1 to M, Let i be the state of the i-th particle; based on the average weight of all particles The long-term likelihood estimate is obtained. and short-term likelihood estimation As shown below: ， Then, the second iteration process is initiated to obtain the final state information: Before resampling, with probability To increase random particles, in China-Israel probability To get The final state of the car is then obtained as follows: ； Finally, the covariance matrix of the radar estimator is obtained as follows: ， in, This indicates the expectation value.

5. The sensor fusion indoor positioning method for an unmanned ground vehicle according to claim 1, characterized in that, In step 5, the fusion estimator is designed based on the visual estimator and the lidar estimator: To obtain the linear minimum covariance, the expression for the fusion estimator is: ， in, The weight parameter matrix, Let be the error covariance matrix of the fusion estimator.

Images