A laser radar positioning system and method with track constraints

Abstract

The application claims a laser radar positioning system and method with track constraint. The system is composed of a data acquisition module, a preprocessing module, a track data interface module and a positioning module. The data acquisition module receives the point cloud data obtained by the laser radar and fuses the point clouds from multiple laser radars. The preprocessing module receives the fused point cloud data, filters out the clutter through point cloud filtering according to the scene features and selects the region of interest. The track data interface module reads the pre-stored track geographic information point set, and fits the track constraint equation according to the point set. The positioning module receives the preprocessed point cloud and the track constraint equation, calculates the position based on the particle filtering algorithm, limits the particles to be distributed only on the track constraint equation, reduces the particle distribution area, and obtains more accurate positioning results. The system can effectively improve the positioning accuracy of track transportation vehicles in environments such as mines, tunnels and workshops, and realize accurate cooperation between track transportation vehicles and other equipment.

Description

Technical Field

[0001] This invention belongs to the field of intelligent mobile platform positioning and navigation, specifically relating to a lidar positioning system with orbital constraints. Background Technology

[0002] Positioning technology is a key technology in many application fields. Indoor positioning technology mainly includes wireless communication solutions based on ultra-wideband (UWB) and radio frequency identification (RFID), as well as solutions based on lidar and vision. In complex scenarios such as industrial sites and mines, characterized by heavy smoke and dust, severe electromagnetic interference, and insufficient light, lidar, as a primary sensing sensor, plays a crucial role in the positioning function of autonomous unmanned systems due to its high accuracy and strong anti-interference capabilities.

[0003] Chinese patent application CN201810594133.4 discloses a positioning method for a track-mounted robot, characterized by: using photoelectric sensors and incremental encoders mounted on the robot, and setting code plates at fixed positions on the track to correct the robot's position information. This method requires a large number of code plates and does not utilize the spatial position information of the track.

[0004] Chinese patent application CN201610648150.2, entitled "Onboard Positioner and Mining Locomotive Including the Onboard Positioner," discloses an onboard positioner that uses ultra-wideband (UWB) technology for positioning. This method requires more UWB tags as the mine length increases and does not utilize track spatial location information.

[0005] Chinese patent application CN201910980197.2, entitled "A Train Positioning System and Method Based on Car Number System," discloses a train positioning system and method based on car number system, using RFID technology to determine the train's position on the track. This method requires a tag for each car and does not utilize track spatial location information.

[0006] Rail vehicles play an important role in the industrial field, but the above methods do not effectively utilize the prior information of the track, and the information utilization is insufficient, thus affecting the positioning effect. Summary of the Invention

[0007] This invention aims to solve the problems of the prior art mentioned above. It proposes a lidar positioning system and method with orbital constraints. The technical solution of this invention is as follows:

[0008] A laser radar positioning system with orbit constraints includes four modules: a data acquisition module, a preprocessing module, an orbit data interface module, and a positioning module.

[0009] The data acquisition module is used to acquire point cloud data detected by lidar, and to fuse point cloud data from multiple lidars and send it to the preprocessing module.

[0010] The preprocessing module receives the fused point cloud data, filters the region of interest based on scene features, and filters the point cloud to remove clutter before sending it to the positioning module.

[0011] The track data interface module reads the pre-stored track geographic information point set, fits the track constraint equation according to the coordinate order of the points, and sends it to the positioning module.

[0012] The positioning module receives the preprocessed point cloud and orbital constraint equations, calculates the position based on the particle filtering algorithm, narrows the particle distribution area by restricting the particle distribution to the orbital constraint equations, and outputs accurate positioning results.

[0013] A lidar positioning method with orbital constraints, comprising the following steps:

[0014] (1) Data acquisition: Acquire point cloud data (P1, ..., P2) from n lidars. n );

[0015] (2) Point cloud fusion: The point cloud comes from multiple different lidars. Based on the pre-calibrated rotation and translation parameters between the lidars, the point clouds of multiple lidars are transformed into the fixed coordinate system of the mobile platform and a fused point cloud P′ is formed.

[0016] (3) Point cloud preprocessing: Receive the fused point cloud P′, select the region of interest according to different scenarios, and filter p′ to remove clutter to obtain the preprocessed point cloud P, which is then sent to the positioning module;

[0017] (4) Trajectory constraint generation: Read the pre-stored set of orbital geographic information points Z c The coordinates are then transformed to the O-XYZ coordinate system, and the trajectory constraint equations L are generated by fitting the coordinates of the point set in order. c ;

[0018] (5) Positioning calculation: Receive the preprocessed point cloud P and the orbital constraint equation L c Calculate and obtain the positioning result X result = [x, y, θ] T .

[0019] Furthermore, the detailed process of step (5) of the positioning calculation method is as follows:

[0020] (1) Particle initialization: n particles representing poses are uniformly distributed in the constraint equation L c Above, all particles form a particle set. Each particle The state is Let x, y, heading angle, and score of the particle at time k, satisfying (x k y k θ k )∈L c ;

[0021] (2) Particle motion: According to the particle motion equation X k+1 =F m (X k L c ), to obtain the [x] of the particle at time k+1. k+1 y k+1 θ k+1 ] T ;

[0022] (3) Particle scoring: using particle state X k+1 Transform the point cloud P into the coordinate system O-XYZ, denoted as P c According to the particle scoring equation s k+1 =F s (P c P map The score s of the particle at time k+1 is obtained. k+1 And normalize it, where P map For a pre-read 3D environment point cloud map;

[0023] (4) Particle resampling: According to G o The probability corresponding to each particle is its score s. k+1 And construct the probability density function f r According to f r The particle set G is obtained by random sampling n And replace G o The number of particles remains the same before and after.

[0024] (5) Positioning output: Output particle set G n The geometric center position, X result = [x, y, θ] T As the localization result, where x, y, and θ are G n All particles x k+1 y k+1 θ k+1 The arithmetic mean;

[0025] (6) Repeat steps (2) to (5) above to achieve real-time positioning.

[0026] A LiDAR positioning method with orbit constraints

[0027] Furthermore, the fitting trajectory constraint equations in step (4) of the positioning method include:

[0028] Fit the orbit to a polygonal line, according to Z c The equation of the straight line L is obtained by taking the coordinates of every two adjacent points. a =y=kx+b, m piecewise equations constitute the orbital constraint equations

[0029] The orbit is fitted using a quadratic spline function, based on Z. c The equation of a quadratic spline curve L is obtained by taking the coordinates of every three adjacent points. b =y=ax 2 +bx+c, k piecewise equations constitute the orbital constraint equations

[0030] Furthermore, the particle motion equations in step (2) of the positioning calculation process include:

[0031] If wheel speed exists, obtain the wheel speed information v, and then determine the wheel speed based on X. k+1 =F m =f m1 (X k L c , v), to obtain the state X of the particle at time k+1. k+1 ;

[0032] If there is no wheel speed, according to X k+1 =F m =f m2 (X k L c P), to obtain the state X of the particle at time k+1. k+1 .

[0033] Furthermore, the particle scoring step in step (3) of the positioning calculation process includes:

[0034] (1) Using the rotation and translation formula P c =P*R+T, transforms the point cloud P in the fixed coordinate system of the mobile platform to P in the O-XYZ coordinate system. c , where R and T are the rotation and translation matrices, respectively;

[0035] (2) Using the distance formula between two points, D = f min (P c P map ), to obtain P c The middle point and P map The distance D to the nearest point in the middle;

[0036] (3) Using the normal distribution formula, q = f N (D, u, σ) 2 ), where u = 0, D is the independent variable, and q is the point cloud P. c The score should be given for this point;

[0037] (4) Repeat steps (1) to (3) to calculate the point cloud P. c All the point scores are (q1, ..., q). n ), and normalize it.

[0038] Furthermore, the probability density function f r for:

[0039]

[0040] The random discrete variable x(1, 2, ..., n) represents a particle. to The probabilities of being selected are respectively to in The score is the normalized score for the particles.

[0041] Furthermore, the particle motion equation X k+1 =F m =f m1 (X k L c ,v) specifically refers to:

[0042] (1) Calculate the displacement Δe = v * Δt based on the time difference Δt between the wheel speed v at time k+1 and the previous time k. Where (x) k y k (x) represents the particle position at time k, and (x) represents the position of the particle at time k. k+1 y k+1 () represents the particle position at time k+1;

[0043] (2) The particle is always within the orbital constraint equations Go to exercise, satisfy

[0044] (3) The orbital constraint equations Substitution X was calculated k+1 (x k+1 y k+1 ).

[0045] Furthermore, the particle motion equation X k+1 =F m =f m2 (X k L cSpecifically, P) is:

[0046] (1) Based on the point cloud P at time k+1 and the point cloud P at time k k The translation matrix calculated using the iterative nearest point algorithm is used as the displacement.

[0047] (2) The particle satisfies the orbital constraint equations

[0048] (3) The orbital constraint equations Substitution X was calculated k+1 (x k+1 y k+1 ).

[0049] The advantages and beneficial effects of this invention are as follows:

[0050] This invention proposes a track-constrained lidar positioning system. This system calculates position using a particle filter algorithm and generates a set of track constraint equations based on prior geographical information about the track. These equations restrict particle distribution, limiting it to the area represented by the equations and thus narrowing the particle distribution area, resulting in more accurate positioning. This invention fully utilizes known prior track information to improve positioning accuracy, providing more precise positioning for rail transport vehicles in complex environments with strong interference, such as tunnels, mines, and workshops, and enabling accurate coordination between rail transport vehicles and other equipment. Attached Figure Description

[0051] Figure 1 This is the overall framework of a preferred embodiment of the present invention, which provides a laser radar positioning system and method based on orbital constraints.

[0052] Figure 2 This is a flowchart of the orbit-constrained lidar positioning system described in this invention.

[0053] Figure 3 This is a flowchart of the positioning calculation method described in this invention;

[0054] Figure 4 This is a schematic diagram of a particle with orbital constraints as proposed in this invention. Detailed Implementation

[0055] The technical solutions of the embodiments of the present invention will be clearly and thoroughly described below with reference to the accompanying drawings. The described embodiments are merely some embodiments of the present invention.

[0056] The technical solution of the present invention to solve the above-mentioned technical problems is:

[0057] Figure 1The diagram shows the overall framework of a track-constrained lidar positioning system according to the present invention. The system comprises four parts: a data acquisition module, a preprocessing module, a track data interface module, and a positioning module.

[0058] The data acquisition module is used to acquire point cloud data detected by lidar, and to fuse point cloud data from multiple lidars and send it to the preprocessing module.

[0059] The preprocessing module receives the fused point cloud data, filters the region of interest based on scene features, and filters the point cloud to remove clutter before sending it to the positioning module.

[0060] The track data interface module reads the pre-stored track geographic information point set, fits the track constraint equation according to the coordinate order of the points, and sends it to the positioning module.

[0061] The positioning module receives the preprocessed point cloud and orbital constraint equations, and outputs accurate positioning results based on the particle filtering algorithm and constraining the particle pose using the orbital constraint equations.

[0062] Figure 2 The diagram illustrates the implementation flow of the orbit-constrained lidar positioning system described in the invention, which includes the following steps:

[0063] (1) Data Acquisition: The data acquisition module acquires raw point cloud data (P1, ..., P2) from n lidars. n );

[0064] (2) Point cloud preprocessing: The point cloud comes from multiple different lidars. Based on the pre-calibrated rotation and translation parameters between the lidars, the point clouds of multiple lidars are transformed into the fixed coordinate system of the mobile platform and a fused point cloud P′ is formed. Then, the clutter in the point cloud is filtered out.

[0065] 1) In formula (1), R and T are the rotation and translation matrices between radars, respectively, and P′ is the transformed point cloud;

[0066] P′=P*R+T#(1)

[0067] 2) A direct-pass filtering method is used to filter the region of interest. For each point in P′, filtering is performed based on the upper and lower limits of the set height z and distance r, denoted as Z and R, respectively. max Z min R max R min If a point exceeds the set upper or lower limit, then that point is removed.

[0068]

[0069] 3) A radius-based filtering method is used to remove clutter. For each point in P′, a radius of R is determined. f If the number of points in the neighborhood is less than N, then the point is considered a noise point and is removed.

[0070] (3) Track constraint generation: Read the pre-stored track geographic information point set Z c (d1, ..., d n ), where d(x, y) are points in the point set; Z c In a fixed coordinate system O-XYZ, taking a piecewise linear orbit as an example, based on Z... c The equation of the straight line L is obtained by taking the coordinates of every two adjacent points. a =y=kx+b, m piecewise equations constitute the orbital constraint equations

[0071]

[0072] (4) Positioning calculation: Receive the preprocessed point cloud P and the orbital constraint equation L c Calculate and obtain the positioning result X result = [x, y, θ] T .

[0073] Figure 3 This is a flowchart of the positioning calculation method described in this invention, which includes the following steps:

[0074] (1) Particle initialization: n particles representing poses are uniformly distributed in the orbital constraint equation L c Above, forming a particle set Each of the particles The state is Let x and y represent the x-coordinates and y-coordinates of the particle at time k, respectively, and let x and y represent the equation of the line it lies on. The slope is used as the heading angle, and the score is calculated accordingly; (e.g.) Figure 4 a)

[0075] (2) Particle motion: The particle motion is calculated based on wheel speed information or point cloud information to obtain the particle state at time k+1:

[0076] a) When wheel speed information exists:

[0077] 1) Obtain the wheel velocity v at time k+1, and the time increment Δt between time k+1 and time k, to obtain the particle displacement Δe:

[0078] Δe=v*Δt#(4)

[0079] 2) Using Δe as the particle in L c The distance moved upwards yields the particle's state X at time k+1.k+1 =[x k+1 y k+1 θ k+1 s k ] T ;

[0080]

[0081] b) When wheel speed information is unavailable, the translation matrix calculated using the iterative nearest point algorithm is used as Δe:

[0082] 1) Current frame point cloud P, let the previous frame point cloud P be... k Take the point set q i ∈P, take the point set o i ∈P k Two points whose corresponding points are in a set of equal parts are called the closest points.

[0083] min = ||q i -o i ‖#(6)

[0084] 2) Calculate the optimal matching parameters: rotation matrix R and translation matrix t, which minimizes the error function.

[0085]

[0086] 3) For o i Using R and t to perform rotation and translation transformations, we obtain a new point set o. i ′;

[0087] 4) Calculate q i and o i If the average distance d is less than the threshold or greater than the maximum number of iterations, stop the iteration calculation; otherwise, return to step (b.1) until the convergence condition is met.

[0088]

[0089] 5) Obtain Δe from the translation matrix t in step 2), and obtain the state X of the particle at time k+1 according to formula (5). k+1 =[x k+1 y k+1 θ k+1 s k ] T ;

[0090] (3) Particle scoring: Particle scoring is based on the likelihood domain model (e.g., Figure 4 b. (Particles receive different scores after passing through the likelihood domain scoring process) The likelihood domain scoring process is as follows:

[0091] 1) Based on particle state Xk+1 The point cloud P in the radar coordinate system is unified to the point cloud in the coordinate system O-XYZ using the rotation and translation transformation formula. Where d c (x c y c ) is P c The point in the middle;

[0092] P c =P*R+T#(9)

[0093] 2) Let<x′,y′> For environmental point cloud map P map The point cloud set, d c With P map The distance to the nearest point in the middle is D:

[0094]

[0095] 3) Given a normal distribution N(u,σ) 2 ), where u=0, σ=1, and d is obtained according to the normal distribution formula. c The score q is:

[0096]

[0097] 4) Repeat steps 2)-3) to calculate P. c All the point ratings, from q1 to q n And by summing them up sequentially, we can obtain the particle score w:

[0098] w = q1 + q2 ... q n #(12)

[0099] Repeat step (3) to obtain G o All particle scores are from w1 to w n And normalized to obtain

[0100]

[0101] (4) Particle resampling: Let the random discrete variable x(1,2,…,n) have a density function f of the following form. r (x) represents the particle to The probabilities of being selected are respectively to

[0102]

[0103] (5) Global location: based on the probability density function f r (x), from particle set G oRandomly select n particles to form a particle set G n And replace G o It outputs the geometric center position X of the particle set. result = [x, y, θ] T As the localization result, where x, y, and θ are G n All particles x k+1 y k+1 θ k+1 Arithmetic mean:

[0104]

[0105] (6) Repeat steps (2)-(5) above to achieve real-time positioning.

[0106] like Figure 4 a is a schematic diagram of particle initialization when the present invention has orbital constraints. Under the constraint of orbital information, the particles are only initialized on the orbit. The size of the circle represents the total score of all particles at this point.

[0107] like Figure 4 b is a schematic diagram of particle scoring in this invention with orbital constraints. Particles obtain different scores after passing through the likelihood domain scoring process.

[0108] like Figure 4 c is a schematic diagram of particle resampling with orbital constraints according to the present invention, and Figure 4 Compared to when particles were not resampled, the proportion of high-scoring particles in the new particle set increases, while the proportion of low-scoring particles decreases or disappears.

[0109] The systems, devices, modules, or units described in the above embodiments can be implemented by computer chips or entities, or by products with certain functions. A typical implementation device is a computer. Specifically, a computer can be, for example, a personal computer, laptop computer, cellular phone, camera phone, smartphone, personal digital assistant, media player, navigation device, email device, game console, tablet computer, wearable device, or any combination of these devices.

[0110] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0111] The above embodiments should be understood as illustrative only and not as limiting the scope of protection of the present invention. After reading the description of the present invention, those skilled in the art can make various alterations or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims

1. A lidar positioning system with orbital constraints, characterized in that, include: The system comprises a data acquisition module, a preprocessing module, a track data interface module, and a positioning module, among which: The data acquisition module is used to acquire point cloud data detected by lidar, and to fuse point cloud data from multiple lidars and send it to the preprocessing module. The preprocessing module is used to receive the fused point cloud data, filter the region of interest according to scene features, and filter the point cloud to remove clutter before sending it to the positioning module. The track data interface module is used to read the pre-stored track geographic information point set, fit the track constraint equation according to the coordinate order of the point set, and send it to the positioning module. The positioning module receives the preprocessed point cloud and orbital constraint equations, calculates the position based on the particle filtering algorithm, and narrows the particle distribution area by restricting particle distribution to the orbital constraint equations. The particles are initialized and moved according to the constraint equations, and an accurate positioning result is output, specifically including: 2.1 Particle Initialization: n particles representing poses are uniformly distributed within the orbital constraint equations. Above, forming a particle set Each particle The state is , representing the particle's x-coordinate, y-coordinate, heading angle, and score at time k; 2.2 Particle Motion: According to the equations of particle motion To obtain the state of the particle at time k+1. ,in Representing the orbital constraint equations A system of equations consisting of particle displacement; 2.3 Particle scoring: based on particle state , will dot clouds Transform to coordinate system Below is dot cloud According to the particle scoring equation Update the particle score and normalize it, where For the pre-read environmental point cloud, Represent the likelihood domain scoring equation system; 2.4 Particle resampling: based on The probability of each particle being selected is used to assign it a score. And construct the probability density function The particle set is obtained by randomly sampling according to the probability density function. replace ; 2.5 Positioning Output: Output Geometric center position As a location result; 2.6 Repeat steps 2.2 to 2.5 above to achieve real-time positioning; Step 2.2 specifically includes: The particle motion is deduced based on wheel speed information or point cloud information, and the particle state at time k+1 is obtained: a) When wheel speed information exists: 1) Obtain the wheel speed at time k+1 and the time increments at times k+1 and k. The particle displacement was obtained. : ； 2) Utilize As particles The distance moved upwards yields the particle's state at time k+1. ; ； b) When wheel speed information is unavailable, the translation matrix calculated using the iterative nearest point algorithm is used as... : 1) Current frame point cloud Let the cloud at the previous time point be... , take the point set , take the point set Two points whose corresponding points are in a set of equal parts are called the closest points. ; 2) Calculate the optimal matching parameter rotation matrix. Translation matrix This minimizes the error function, where n represents the point set. and Number of points contained: ； 3) To use and By performing rotation and translation transformations, a new set of points can be obtained. ; 4) Calculation and If the average distance d is less than the threshold or greater than the maximum number of iterations, the iteration calculation stops; otherwise, return to step (b.1) until the convergence condition is met. ; 5) Based on the translation matrix in step 2), get The state of the particle at time k+1 can be obtained according to formula (5). .

2. A laser radar positioning method with orbital constraints based on the system of claim 1, characterized in that, Includes the following steps: 4.1 Data Acquisition Steps: Acquire point cloud data from n lidar units. ; 4.2 Point Cloud Fusion Steps: Point cloud data comes from multiple different radars. Based on the pre-calibrated pose parameters between the multiple radars, the point clouds from the multiple radars are transformed into the fixed coordinate system of the mobile platform, and then combined to form a fused point cloud. ; 4.3 Point Cloud Preprocessing Steps: Receive and merge point clouds Then, regions of interest are selected based on scene features, and the point cloud is filtered to remove clutter, resulting in a preprocessed point cloud. It is sent to the positioning module; 4.

4. Track constraint generation steps: Read the pre-stored set of track geographic information points. And transform to a stationary coordinate system fixed to the ground. Below, the coordinate system selects a point on the ground as the origin, with due north as the Y-axis, due east as the X-axis, and the Z-axis perpendicular to the ground. The points are connected in coordinate order to fit and generate the orbital constraint equations. ; 4.

5. Positioning Calculation Steps: Receive the preprocessed point cloud. and orbital constraint equations Calculate the positioning results .

Images