Vehicle automatic positioning method, system and device in closed environment and medium

By combining laser data and inertial sensor data, using Kalman filtering and point cloud registration algorithm, the problem of unreliable speed readings in the automatic vehicle positioning method is solved, and the accurate estimation of real-time vehicle speed and posture in a closed environment is achieved. , suitable for highly dynamic scenarios.

CN118913279APending Publication Date: 2024-11-08GUANGDONG ELECTRIC POWER SCI RES INST ENERGY TECH CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202410964440.2
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-18
Publication Date
2024-11-08

AI Technical Summary

Technical Problem

The existing vehicle automatic positioning method obtains vehicle speed readings by measuring wheel speed or counting motor rotors, resulting in low accuracy of positioning data, especially in highly dynamic scenarios in closed environments such as in cabins.

Method used

Using laser data and inertial sensor measurement data, the vehicle target longitudinal running speed, relative posture and heading angle difference values ​​are calculated, combined with Kalman filtering algorithm and point cloud registration algorithm, the vehicle speed and posture are estimated in real time to ensure the positioning data accuracy.

Benefits of technology

In a closed environment, through laser inter-frame matching and inertial sensor recursiveness, the accurate estimation of the vehicle's real-time speed and robust calculation of position are achieved. It is suitable for highly dynamic scenarios and improves the accuracy and reliability of vehicle positioning data. .

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118913279A_ABST
    Figure CN118913279A_ABST
Patent Text Reader

Abstract

The invention discloses an automatic vehicle positioning method, system and device in a closed environment and a medium, and relates to the technical field of vehicle positioning. The target longitudinal running speed of the vehicle is generated by calculating the target longitudinal running speed of the vehicle according to the measurement data of the inertial sensor. And performing relative pose calculation based on the laser data and an initial longitudinal running speed corresponding to the target longitudinal running speed to generate a relative pose. And estimating the real-time speed of the vehicle based on the target longitudinal running speed and the course angle difference value corresponding to the relative pose, and generating the real-time speed of the vehicle. Vehicle pose estimation is carried out on the vehicle real-time speed, the course angle difference value and the initial vehicle pose corresponding to the vehicle real-time speed, and a target vehicle pose corresponding to the preset engineering vehicle is generated. The real-time vehicle pose of the vehicle is estimated only through inertial sensor measurement data and laser data recursion, and the method is simple, reliable, small in calculation amount and high in positioning result accuracy.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle positioning, and in particular to a method, system, equipment and medium for automatic positioning of a vehicle in a closed environment. Background Art

[0002] Through the automated modification of engineering vehicles such as pushers and rakes, unmanned operation of engineering vehicles in closed environments such as cabins can be realized to complete pre-set operation tasks, such as pushing and raking the coal around the cabin to the center of the cabin for easy crane grabbing. In the automatic operation process, the real-time acquisition of the speed of the engineering vehicle is crucial for closed-loop control and positioning of the vehicle speed. In order to obtain the vehicle speed, the most direct and effective method is to install a wheel speed machine or encoder on the vehicle, that is, to indirectly measure the vehicle speed by measuring the wheel speed or counting the motor rotor. However, on many different types of engineering vehicles, the installation of a wheel speed machine or encoder is often difficult to achieve or the cost of modification is very high and the measurement reading is unstable. For example, the crawler chassis of the pusher and rake often slips severely due to driving on the coal pile, resulting in unreliable vehicle speed readings obtained by the wheel speed machine. Therefore, the existing vehicle automatic positioning method obtains unreliable vehicle speed readings by measuring the wheel speed or counting the motor rotor, resulting in low accuracy of the obtained vehicle positioning data. Summary of the invention

[0003] The present invention provides a method, system, device and medium for automatic vehicle positioning in a closed environment, which solves the technical problem that the vehicle speed reading obtained by the existing automatic vehicle positioning method by measuring the wheel rotation speed or counting the motor rotor is unreliable, resulting in low accuracy of the obtained vehicle positioning data.

[0004] The present invention provides a method for automatic positioning of a vehicle in a closed environment, which is applied to a preset engineering vehicle. The method comprises:

[0005] Acquire laser data and inertial sensor measurement data of the preset engineering vehicle, calculate the target longitudinal running speed of the vehicle using the inertial sensor measurement data, and generate a target longitudinal running speed;

[0006] Perform relative posture calculation based on the laser data and the initial longitudinal running speed corresponding to the target longitudinal running speed to generate a relative posture;

[0007] Estimating the real-time speed of the vehicle based on the target longitudinal running speed and the heading angle difference value corresponding to the relative posture to generate the real-time speed of the vehicle;

[0008] The vehicle real-time speed, the heading angle difference value and the initial vehicle posture corresponding to the vehicle real-time speed are used to estimate the vehicle posture to generate a target vehicle posture corresponding to the preset engineering vehicle.

[0009] Optionally, the inertial sensor measurement data includes three-axis acceleration measurement values ​​and three-axis attitude angle measurement values; the step of calculating the vehicle target longitudinal running speed using the inertial sensor measurement data to generate the target longitudinal running speed includes:

[0010] Convert the three-axis attitude angle measurement value and the three-axis acceleration measurement value into sea level longitudinal acceleration to generate vehicle longitudinal acceleration;

[0011] A target longitudinal running speed is calculated according to the vehicle longitudinal acceleration and the corresponding initial longitudinal running speed.

[0012] Optionally, the laser data includes first laser data and second laser data; the step of performing relative posture calculation based on the laser data and an initial longitudinal running speed corresponding to the target longitudinal running speed to generate a relative posture includes:

[0013] Calculating the relative position of the target between the first laser data and the second laser data using a point cloud registration algorithm;

[0014] Calculating a two-dimensional relative pose between the target relative pose and the corresponding initial relative pose to generate a two-dimensional relative pose;

[0015] The relative posture is calculated according to the heading angle difference value in the two-dimensional relative posture and the initial longitudinal running speed corresponding to the target longitudinal running speed.

[0016] Optionally, the step of estimating the real-time speed of the vehicle based on the target longitudinal running speed and the heading angle difference value corresponding to the relative posture to generate the real-time speed of the vehicle comprises:

[0017] Calculating a target state prediction value and a target priori prediction variance according to an initial longitudinal running speed and a vehicle longitudinal acceleration corresponding to the target longitudinal running speed;

[0018] The real-time speed of the vehicle is generated by calculation based on the relative posture, the heading angle difference value corresponding to the relative posture, the target state prediction value and the target prior prediction variance.

[0019] Optionally, the step of estimating the vehicle posture according to the real-time speed of the vehicle, the heading angle difference value and the initial vehicle posture corresponding to the real-time speed of the vehicle to generate the target vehicle posture corresponding to the preset engineering vehicle includes:

[0020] The target vehicle posture corresponding to the preset engineering vehicle is calculated based on the initial longitudinal running speed, the initial longitudinal relative position difference, the initial lateral relative position difference, the initial heading angle difference value and the heading angle difference value in the heading angle difference values ​​corresponding to the real-time speed of the vehicle.

[0021] The present invention also provides a vehicle automatic positioning system in a closed environment, which is applied to a preset engineering vehicle, and the system comprises:

[0022] A target longitudinal running speed generating module is used to obtain the laser data and inertial sensor measurement data of the preset engineering vehicle, calculate the vehicle target longitudinal running speed based on the inertial sensor measurement data, and generate the target longitudinal running speed;

[0023] A relative posture generation module, used to perform relative posture calculation based on the laser data and the initial longitudinal running speed corresponding to the target longitudinal running speed, and generate a relative posture;

[0024] A vehicle real-time speed generating module, used to estimate the vehicle real-time speed based on the target longitudinal running speed and the heading angle difference value corresponding to the relative posture, and generate the vehicle real-time speed;

[0025] The target vehicle posture generation module is used to estimate the vehicle posture according to the real-time speed of the vehicle, the heading angle difference value and the initial vehicle posture corresponding to the real-time speed of the vehicle, and generate the target vehicle posture corresponding to the preset engineering vehicle.

[0026] Optionally, the inertial sensor measurement data includes three-axis acceleration measurement values ​​and three-axis attitude angle measurement values; the target longitudinal running speed generation module performs the following steps:

[0027] Convert the three-axis attitude angle measurement value and the three-axis acceleration measurement value into sea level longitudinal acceleration to generate vehicle longitudinal acceleration;

[0028] A target longitudinal running speed is calculated according to the vehicle longitudinal acceleration and the corresponding initial longitudinal running speed.

[0029] Optionally, the laser data includes first laser data and second laser data; and the relative posture generation module performs the following steps:

[0030] Calculating the relative position of the target between the first laser data and the second laser data using a point cloud registration algorithm;

[0031] Calculating a two-dimensional relative pose between the target relative pose and the corresponding initial relative pose to generate a two-dimensional relative pose;

[0032] The relative posture is calculated according to the heading angle difference value in the two-dimensional relative posture and the initial longitudinal running speed corresponding to the target longitudinal running speed.

[0033] The present invention also provides an electronic device, including a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of implementing any of the above-mentioned methods for automatic positioning of a vehicle in a closed environment.

[0034] The present invention also provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed, any of the above-mentioned methods for automatic positioning of a vehicle in a closed environment is implemented.

[0035] It can be seen from the above technical solutions that the present invention has the following advantages:

[0036] The present invention obtains laser data and inertial sensor measurement data of a preset engineering vehicle, calculates the target longitudinal running speed of the vehicle using the inertial sensor measurement data, and generates the target longitudinal running speed. Relative posture calculation is performed based on the laser data and the initial longitudinal running speed corresponding to the target longitudinal running speed to generate the relative posture. The real-time speed of the vehicle is estimated based on the heading angle difference value corresponding to the target longitudinal running speed and the relative posture to generate the real-time speed of the vehicle. The vehicle posture is estimated based on the vehicle real-time speed, the heading angle difference value and the initial vehicle posture corresponding to the vehicle real-time speed to generate the target vehicle posture corresponding to the preset engineering vehicle. The technical problem that the vehicle speed reading obtained by the existing vehicle automatic positioning method by measuring the wheel speed or counting the motor rotor is unreliable, resulting in low accuracy of the obtained vehicle positioning data is solved. The real-time longitudinal speed of the vehicle is estimated only by using laser inter-frame matching and data recursion collected by the inertial sensor, wherein the accuracy of the laser inter-frame matching only depends on the laser point cloud data of the two frames before and after, and is less affected by the dynamic changes of the environment, and is suitable for high-dynamic scenes caused by frequent pushing and raking of coal in the cabin. The inertial sensor measurement reading can also ensure long-term accuracy and effectiveness. This allows the preset engineering vehicle to always accurately and robustly estimate the real-time speed of the vehicle when it operates in a closed cabin for a long time, thereby obtaining the target vehicle posture corresponding to the preset engineering vehicle. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0038] Figure 1A flowchart of the steps of a method for automatic positioning of a vehicle in a closed environment provided in Embodiment 1 of the present invention;

[0039] Figure 2 A schematic diagram of short-term dead reckoning of a vehicle provided in Embodiment 1 of the present invention;

[0040] Figure 3 A principle block diagram of velocity estimation using laser and IMU provided in Embodiment 1 of the present invention;

[0041] Figure 4 This is a structural block diagram of a vehicle automatic positioning system in a closed environment provided by Embodiment 2 of the present invention. DETAILED DESCRIPTION

[0042] The embodiments of the present invention provide a method, system, device and medium for automatic vehicle positioning in a closed environment, which are used to solve the technical problem that the vehicle speed readings obtained by the existing automatic vehicle positioning methods by measuring the wheel rotation speed or counting the motor rotor are unreliable, resulting in low accuracy of the obtained vehicle positioning data.

[0043] In order to make the purpose, features and advantages of the present invention more obvious and easy to understand, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described below are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0044] Embodiment 1

[0045] See also Figure 1 , Figure 1 This is a flow chart of the steps of a method for automatic positioning of a vehicle in a closed environment provided in Embodiment 1 of the present invention.

[0046] Example 1 of the present invention provides a method for automatic positioning of a vehicle in a closed environment, which is applied to a preset engineering vehicle. The preset engineering vehicle is an engineering vehicle equipped with two laser sensors and an inertial sensor IMU (inertial measurement unit), and the type of engineering vehicle is usually a bulldozer. The inertial sensor used by the preset engineering vehicle is the BW-VG225E Beiwei inclination sensor, which can measure 3-axis acceleration and 3-axis angle relative to the horizontal plane. The output frequency of the sensor is set to 100Hz. The inertial sensor can be fixedly installed on the vehicle body, and its measurement readings can also ensure long-term accuracy and effectiveness. The laser sensor is a Sagitar 32-line laser RS-Helios-5515. The horizontal viewing angle of the laser is 360 degrees, the vertical viewing angle is 70 degrees, the horizontal angular resolution is set to 0.2 degrees, and the output frequency is set to 10Hz. By installing two 32-line lasers one after the other on the roof of the dozer, and then splicing the real-time point cloud data obtained by the two lasers, it is possible to scan and obtain a frame of point cloud data of the closed cabin environment every 100 milliseconds, basically covering the environment around the cabin, ensuring the accuracy of matching between laser frames.

[0047] Preferably, the preset engineering vehicle can also be installed with the Kecong MRC5000 controller, the I / O and communication interface of the controller ensure the real-time input of laser data and IMU data, and the speed estimation program also runs in the controller.

[0048] The method includes:

[0049] Step 101: Acquire laser data and inertial sensor measurement data of a preset engineering vehicle, calculate the target longitudinal running speed of the vehicle using the inertial sensor measurement data, and generate a target longitudinal running speed.

[0050] Further, the inertial sensor measurement data includes three-axis acceleration measurement values ​​and three-axis attitude angle measurement values. Step 101 may include the following sub-steps S11-S12:

[0051] S11. Convert the three-axis attitude angle measurement values ​​and the three-axis acceleration measurement values ​​into sea level longitudinal acceleration to generate vehicle longitudinal acceleration.

[0052] S12, substituting the vehicle longitudinal acceleration and the corresponding initial longitudinal running speed into a preset longitudinal running speed equation to calculate a target longitudinal running speed, that is, calculating the target longitudinal running speed according to the vehicle longitudinal acceleration and the corresponding initial longitudinal running speed.

[0053] In the embodiment of the present invention, a filtering fusion scheme for velocity estimation is designed according to the characteristics of the laser sensor and the inertial sensor IMU. The estimated state variable x is only the longitudinal velocity v of the vehicle. x :

[0054] x=v x ;

[0055] The x-axis of the preset engineering vehicle coordinate system is specified to point to the front of the vehicle, and the IMU is installed on the vehicle so that the x-axis of the IMU coordinate system coincides with the x-axis of the vehicle coordinate system. The IMU used can obtain 3-axis acceleration measurement values Among them are; is the x-axis acceleration measurement value; is the acceleration measurement value of the y-axis; It is the z-axis acceleration measurement value and the 3-axis attitude angle measurement value (roll angle α, pitch angle β, heading angle θ) According to the attitude angle measurement The acceleration measurement value can be Converted to sea level, the longitudinal acceleration of the vehicle at sea level, that is, the longitudinal acceleration of the vehicle, is:

[0056]

[0057] Among them, a x,k-1 t k-1 The longitudinal acceleration of the vehicle at the time is the x-axis acceleration measurement value; is the pitch angle; is the measured value of z-axis acceleration; is the heading angle.

[0058] Since the IMU output frequency is very high, about 100Hz, the three-axis attitude angle measurement value and the three-axis acceleration measurement value are converted into the sea level longitudinal acceleration to obtain the vehicle longitudinal acceleration. The vehicle longitudinal acceleration and the corresponding initial longitudinal running speed are used as the input of the state transfer equation, that is, the preset longitudinal running speed equation, so as to calculate the target longitudinal running speed. The preset longitudinal running speed equation is:

[0059] v x,k =v x,k-1 +a x,k-1 ·Δt+ε;

[0060] ε~N(0,Q);

[0061] Among them, v x,k v is the longitudinal speed of the car delivered at the current IMU input moment, i.e. the target longitudinal running speed; x,k-1 The longitudinal velocity of the car estimated at the last IMU input moment is the initial longitudinal running velocity; a x,k-1 t k-1The longitudinal acceleration of the vehicle at the moment; Δt is the time difference between the two consecutive IMU input moments, that is, the time difference between the two adjacent inertial sensor input moments; ε is the system noise with a normal distribution and the first covariance Q, and N is a normal distribution.

[0062] Step 102: Calculate the relative posture based on the laser data and the initial longitudinal running speed corresponding to the target longitudinal running speed to generate the relative posture.

[0063] Further, the laser data includes first laser data and second laser data. Step 102 may include the following sub-steps S21-S23:

[0064] S21, using a point cloud registration algorithm to calculate the relative position and orientation of the target between the first laser data and the second laser data.

[0065] S22. Calculate the two-dimensional relative posture between the target relative posture and the corresponding initial relative posture to generate a two-dimensional relative posture.

[0066] S23. Substitute the heading angle difference value in the two-dimensional relative posture and the initial longitudinal running speed corresponding to the target longitudinal running speed into the preset observation equation to calculate the longitudinal displacement, and generate the relative posture, that is, calculate the relative posture according to the heading angle difference value in the two-dimensional relative posture and the initial longitudinal running speed corresponding to the target longitudinal running speed.

[0067] In the embodiment of the present invention, the relative pose between the two frames of laser data can be calculated by the NDT registration algorithm, that is, the relative pose of the target between the first laser data and the second laser data is calculated by the point cloud registration algorithm. Thus, the two-dimensional relative pose between the target relative pose and the corresponding initial relative pose is calculated, and the vehicle at the current laser data acquisition time t is obtained. k Relative to the last laser data acquisition t k-1 The two-dimensional relative pose Δ at the moment p k =[Δp x,k ,Δp y,k ,Δp θ,k ] T , where Δp x,k , Δp y,k t k The relative position difference between the two moments before and after in the x and y axis directions, Δp θ,k t k The heading angle difference between the two moments before and after the moment. p k Indicates that at the previous time t k-1 The vehicle coordinate system is Figure 2 shown.

[0068] To construct the preset observation equation, you can use Figure 2 The short-term dead reckoning method in t k-1 Vehicle position at the moment p k-1 =[p x,k-1 ,p y,k-1 ,p θ,k-1 ] T Predict t k Vehicle position at the moment p k Specifically: Assume that the vehicle has only longitudinal velocity and no lateral velocity, that is, the vehicle velocity vector v k Parallel to the body:

[0069]

[0070] because The value is too small, which may cause problems in numerical calculation, so only the displacement Δp in the X direction is used here. x,k To list the default observation equation as:

[0071]

[0072] Among them, Δp x,k t k The longitudinal relative position difference at the moment is the relative posture; x,k-1 t k-1 The longitudinal running speed at the moment is the initial longitudinal running speed; ΔT is the time difference between the two lasers used as observation sources, that is, the time difference between the first laser and the second laser used as observation sources; Δp θ,k t k The heading angle difference value at the moment; μ is the measurement noise with normal distribution and the second covariance R.

[0073] The longitudinal displacement of the vehicle t is calculated by substituting the heading angle difference value in the two-dimensional relative posture and the initial longitudinal running speed corresponding to the target longitudinal running speed into the preset observation equation. k The longitudinal relative position difference at each moment is the relative posture.

[0074] Step 103: estimate the real-time speed of the vehicle based on the target longitudinal running speed and the heading angle difference value corresponding to the relative posture to generate the real-time speed of the vehicle.

[0075] Further, step 103 may include the following sub-steps S31-S32:

[0076] S31. Substitute the initial longitudinal running speed, the vehicle longitudinal acceleration, the first covariance, the initial prior prediction variance and the initial state prediction value corresponding to the target longitudinal running speed into the preset prediction equation for data prediction, and generate the target state prediction value and the target prior prediction variance, that is, calculate the target state prediction value and the target prior prediction variance based on the initial longitudinal running speed and the vehicle longitudinal acceleration corresponding to the target longitudinal running speed.

[0077] S32. Substitute the relative posture, the heading angle difference value corresponding to the relative posture, the target state prediction value and the target prior prediction variance into the preset update equation to estimate the real-time speed of the vehicle, and generate the real-time speed of the vehicle, that is, calculate the real-time speed of the vehicle based on the relative posture, the heading angle difference value corresponding to the relative posture, the target state prediction value and the target prior prediction variance.

[0078] In the embodiment of the present invention, since the preset longitudinal running speed equation and the preset observation equation are both linear equations, it is only necessary to use the linear Kalman filter algorithm to estimate the real-time speed and minimize the state estimation variance. The linear Kalman filter algorithm calculation equation is specifically as follows:

[0079] The prediction process is to substitute the initial longitudinal running speed, vehicle longitudinal acceleration, first covariance, initial prior prediction variance and initial state prediction value corresponding to the target longitudinal running speed into the preset prediction equation for data prediction, and obtain the target state prediction value and target prior prediction variance. The preset prediction equation is:

[0080]

[0081]

[0082] in, is the state quantity v at the previous moment. x,k-1 The state prediction value estimated by the preset longitudinal running speed equation is the state prediction value estimated by the preset longitudinal running speed equation from the initial longitudinal running speed, that is, the target state prediction value; v x,k-1 t k-1 The longitudinal running speed at the moment is the initial longitudinal running speed; x,k-1 t k-1 The longitudinal acceleration of the vehicle at the moment; Δt is the time difference between two adjacent inertial sensor input moments; is the prior prediction variance at the current moment, i.e., the target prior prediction variance; P k-1 t k-1 The prior prediction variance at time t is the initial prior prediction variance; Q is the first covariance.

[0083] The updating process is to estimate the real-time speed of the vehicle by substituting the relative posture, the heading angle difference value corresponding to the relative posture, the target state prediction value and the target prior prediction variance into the preset update equation to obtain the real-time speed of the vehicle. The preset update equation is:

[0084]

[0085] Among them, K k is the Kalman gain; is the updated posterior state variance, i.e., t k The a priori prediction variance of the moment; ΔT is the time difference between the first laser and the second laser as the observation source acquisition time; Δp θ,k t k The heading angle difference at the moment; R is the second covariance; v′ x,k The optimal estimate of the state quantity at the current moment calculated in the update step is the real-time speed of the vehicle; is the state prediction value estimated by the initial longitudinal running speed through the preset longitudinal running speed equation, i.e., the target state prediction value; Δp x,k t k The longitudinal relative position difference at each moment is the relative posture.

[0086] Step 104 , estimate the vehicle posture according to the vehicle real-time speed, the heading angle difference value and the initial vehicle posture corresponding to the vehicle real-time speed, and generate a target vehicle posture corresponding to the preset engineering vehicle.

[0087] Further, step 104 may include the following sub-step S41:

[0088] S41. Substitute the vehicle's real-time speed, the heading angle difference value and the initial vehicle posture corresponding to the vehicle's real-time speed into the preset posture estimation equation to estimate the vehicle posture, and generate the target vehicle posture corresponding to the preset engineering vehicle, that is, according to the initial longitudinal running speed, the initial longitudinal relative position difference, the initial lateral relative position difference, the initial heading angle difference value and the heading angle difference value in the heading angle difference value corresponding to the vehicle's real-time speed, calculate the target vehicle posture corresponding to the preset engineering vehicle.

[0089] In the embodiment of the present invention, the present invention uses the NDT algorithm as a method for inter-frame matching of the front and rear frame lasers. Of course, other inter-frame matching algorithms such as point-to-point ICP and point-to-line ICP can also be used, but there will be differences in the amount of calculation and convergence accuracy.

[0090] The idea of ​​NDT is very simple. It does not need to distinguish between points and lines, but only uses the local statistical information (mean and variance) of the point cloud as the basis for inter-frame matching. Specifically, it needs to first divide the target point cloud into several voxels at a certain resolution, then calculate the point cloud Gaussian distribution of each voxel, and then calculate the voxel registration residual of the point cloud of the previous and next two frames, and optimize the residual through Gauss-Newton or LM algorithm to iterate the best inter-frame matching posture.

[0091] Like all registration methods, the optimization result of the NDT algorithm depends on the initial estimate. If the initial estimate is inaccurate, it will cause an erroneous matching result. The 3D NDT algorithm used in the present invention requires an initial estimate of the 6-dimensional pose. Here, it is assumed that the vehicle's roll angle, pitch angle and position in the z direction do not change much, and its initial value estimate can be simply set to 0. Here, the estimated longitudinal velocity of the car and the IMU data are used recursively to calculate the initial estimate of the inter-frame displacement of the vehicle in the x and y directions and the inter-frame change of the heading angle Δp = [Δp x ,Δp y ,Δp θ ] T .

[0092] Each time the inter-frame matching is completed, the initial estimate, that is, the initial vehicle pose Δp, is set to 0, and then the estimated longitudinal velocity of the car and the IMU data are used to deduce the Δp at the next moment. p ′ is the target vehicle posture. The vehicle posture is estimated by substituting the vehicle real-time speed, the heading angle difference value and the initial vehicle posture corresponding to the vehicle real-time speed into the preset posture estimation equation to obtain the target vehicle posture corresponding to the preset engineering vehicle. The preset posture estimation equation is:

[0093]

[0094] in, is the target vehicle posture, Δp′ x,k t k The relative longitudinal position difference of the target at the moment, Δp′ y,k t k The relative lateral position difference of the target at the moment, Δp′ θ,k is the target heading angle difference; is the initial vehicle posture, Δp x,k-1 is the initial longitudinal relative position difference, Δp y,k-1 is the initial lateral relative position difference, Δp θ,k-l is the initial heading angle difference; Δθ k v is the heading angle difference on the horizontal plane measured by the IMU at two consecutive moments, that is, the heading angle difference on the horizontal plane measured by the inertial sensor at two adjacent moments; x,k-1 is the vehicle longitudinal velocity estimated by the filter in real time, t k-1The longitudinal running speed at the moment is the initial longitudinal running speed.

[0095] In the embodiment of the present invention, Figure 3 As shown, two lasers with a frequency of 10Hz are used to collect laser data of the preset engineering vehicle. The inertial sensor measurement data collected by the inertial sensor IMU with a frequency of 100Hz is used to predict the input value to obtain the target longitudinal running speed. The laser data is used for NDT inter-frame matching to obtain the observation value, that is, the relative posture, and then the speed is estimated through the Kalman filter, where the frequency is 100Hz, to obtain the real-time speed of the vehicle. Then, the initial estimation of inter-frame matching is performed to obtain the target vehicle posture corresponding to the preset engineering vehicle. Every time the inter-frame matching is completed, the initial estimate, that is, the initial vehicle posture, is set to 0, and the initial value is re-matched.

[0096] By acquiring the laser data and inertial sensor measurement data of the preset engineering vehicle, the inertial sensor measurement data is used to calculate the target longitudinal running speed of the vehicle, and the target longitudinal running speed is generated. The relative posture is calculated based on the laser data and the initial longitudinal running speed corresponding to the target longitudinal running speed, and the relative posture is generated. The real-time speed of the vehicle is estimated based on the heading angle difference value corresponding to the target longitudinal running speed and the relative posture, and the real-time speed of the vehicle is generated. The vehicle posture is estimated by the vehicle real-time speed, the heading angle difference value and the initial vehicle posture corresponding to the vehicle real-time speed, and the target vehicle posture corresponding to the preset engineering vehicle is generated. The technical problem that the vehicle speed reading obtained by the existing vehicle automatic positioning method by measuring the wheel speed or counting the motor rotor is unreliable, resulting in low accuracy of the obtained vehicle positioning data. Only laser frame matching and IMU recursion are used to estimate the real-time longitudinal speed of the vehicle. The Kalman filter algorithm is used to integrate laser frame matching and IMU recursion. The accuracy of laser frame matching depends only on the laser point cloud data of the previous and next two frames, and is less affected by the dynamic changes of the environment. It is suitable for high-dynamic scenes caused by frequent pushing and raking of coal in the cabin. On the other hand, the IMU can be fixedly installed on the vehicle body, and its measurement readings can also ensure long-term accuracy and effectiveness, so that when the preset engineering vehicle is working in a closed cabin for a long time, the real-time speed of the vehicle can always be accurately and robustly estimated.

[0097] Embodiment 2

[0098] See also Figure 4 , Figure 4 This is a structural block diagram of a vehicle automatic positioning system in a closed environment provided by Embodiment 2 of the present invention.

[0099] Example 2 of the present invention provides a vehicle automatic positioning system in a closed environment, which is applied to a preset engineering vehicle. The system includes:

[0100] The target longitudinal running speed generating module 401 is used to obtain the laser data and inertial sensor measurement data of a preset engineering vehicle, calculate the vehicle target longitudinal running speed based on the inertial sensor measurement data, and generate the target longitudinal running speed.

[0101] The relative posture generation module 402 is used to perform relative posture calculation based on the laser data and the initial longitudinal running speed corresponding to the target longitudinal running speed to generate the relative posture.

[0102] The vehicle real-time speed generating module 403 is used to estimate the vehicle real-time speed based on the target longitudinal running speed and the heading angle difference value corresponding to the relative posture, and generate the vehicle real-time speed.

[0103] The target vehicle posture generation module 404 is used to estimate the vehicle posture according to the vehicle real-time speed, the heading angle difference value and the initial vehicle posture corresponding to the vehicle real-time speed, and generate the target vehicle posture corresponding to the preset engineering vehicle.

[0104] Optionally, the inertial sensor measurement data includes three-axis acceleration measurement values ​​and three-axis attitude angle measurement values; the target longitudinal running speed generation module 401 performs the following steps:

[0105] Convert the three-axis attitude angle measurement value and the three-axis acceleration measurement value into the sea level longitudinal acceleration to generate the vehicle longitudinal acceleration;

[0106] The target longitudinal running speed is calculated according to the vehicle longitudinal acceleration and the corresponding initial longitudinal running speed.

[0107] Optionally, the laser data includes first laser data and second laser data; the relative posture generation module 402 performs the following steps:

[0108] The point cloud registration algorithm is used to calculate the relative position and posture of the target between the first laser data and the second laser data;

[0109] Calculate the two-dimensional relative pose between the target relative pose and the corresponding initial relative pose to generate a two-dimensional relative pose;

[0110] The relative posture is calculated based on the heading angle difference value in the two-dimensional relative posture and the initial longitudinal running speed corresponding to the target longitudinal running speed.

[0111] Optionally, the vehicle real-time speed generating module 403 performs the following steps:

[0112] According to the initial longitudinal running speed and the longitudinal acceleration of the vehicle corresponding to the target longitudinal running speed, the target state prediction value and the target prior prediction variance are calculated;

[0113] The real-time speed of the vehicle is calculated based on the relative posture, the heading angle difference value corresponding to the relative posture, the target state prediction value and the target prior prediction variance.

[0114] Optionally, the target vehicle posture generation module 404 performs the following steps:

[0115] The target vehicle posture corresponding to the preset engineering vehicle is calculated based on the initial longitudinal running speed, the initial longitudinal relative position difference, the initial lateral relative position difference, the initial heading angle difference value and the heading angle difference value corresponding to the vehicle's real-time speed.

[0116] An embodiment of the present invention further provides an electronic device, comprising: a memory and a processor, wherein a computer program is stored in the memory; when the computer program is executed by the processor, the processor executes a method for automatic positioning of a vehicle in a closed environment as described in any of the above embodiments.

[0117] The memory can be an electronic memory such as a flash memory, an EEPROM (electrically erasable programmable read-only memory), an EPROM, a hard disk or a ROM. The memory has a storage space for a program code for executing any method step in the above method. For example, the storage space for the program code may include individual program codes for implementing the various steps in the above method respectively. These program codes can be read from or written to one or more computer program products. These computer program products include program code carriers such as a hard disk, a compact disk (CD), a memory card or a floppy disk. The program code can be compressed, for example, in an appropriate form. When these codes are run by a computing and processing device, the computing and processing device is caused to execute the various steps in the vehicle automatic positioning method in the closed environment described above.

[0118] An embodiment of the present invention further provides a computer-readable storage medium on which a computer program is stored. When the computer program is executed by a processor, the method for automatic positioning of a vehicle in a closed environment as described in any of the above embodiments is implemented.

[0119] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0120] In the several embodiments provided in the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of units is only a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, which can be electrical, mechanical or other forms.

[0121] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0122] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0123] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods of each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0124] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for automatic vehicle positioning in a closed environment, characterized in that: Applied to preset engineering vehicles, the method comprises: Acquire laser data and inertial sensor measurement data of the preset engineering vehicle, calculate the target longitudinal running speed of the vehicle using the inertial sensor measurement data, and generate a target longitudinal running speed; Perform relative posture calculation based on the laser data and the initial longitudinal running speed corresponding to the target longitudinal running speed to generate a relative posture; Estimating the real-time speed of the vehicle based on the target longitudinal running speed and the heading angle difference value corresponding to the relative posture to generate the real-time speed of the vehicle; The vehicle real-time speed, the heading angle difference value and the initial vehicle posture corresponding to the vehicle real-time speed are used to estimate the vehicle posture to generate a target vehicle posture corresponding to the preset engineering vehicle.

2. The method for automatic vehicle positioning in a closed environment according to claim 1, characterized in that: The inertial sensor measurement data includes three-axis acceleration measurement values ​​and three-axis attitude angle measurement values; the step of calculating the target longitudinal running speed of the vehicle using the inertial sensor measurement data to generate the target longitudinal running speed includes: Convert the three-axis attitude angle measurement value and the three-axis acceleration measurement value into sea level longitudinal acceleration to generate vehicle longitudinal acceleration; A target longitudinal running speed is calculated according to the vehicle longitudinal acceleration and the corresponding initial longitudinal running speed.

3. The method for automatic vehicle positioning in a closed environment according to claim 1, characterized in that: The laser data includes first laser data and second laser data; the step of performing relative posture calculation based on the laser data and the initial longitudinal running speed corresponding to the target longitudinal running speed to generate the relative posture includes: Calculating the relative position of the target between the first laser data and the second laser data using a point cloud registration algorithm; Calculating a two-dimensional relative pose between the target relative pose and the corresponding initial relative pose to generate a two-dimensional relative pose; The relative posture is calculated according to the heading angle difference value in the two-dimensional relative posture and the initial longitudinal running speed corresponding to the target longitudinal running speed.

4. The method for automatic vehicle positioning in a closed environment according to claim 2, characterized in that: The step of estimating the real-time speed of the vehicle based on the target longitudinal running speed and the heading angle difference value corresponding to the relative posture to generate the real-time speed of the vehicle comprises: Calculating a target state prediction value and a target priori prediction variance according to an initial longitudinal running speed and a vehicle longitudinal acceleration corresponding to the target longitudinal running speed; The real-time speed of the vehicle is generated by calculation based on the relative posture, the heading angle difference value corresponding to the relative posture, the target state prediction value and the target prior prediction variance.

5. The method for automatic vehicle positioning in a closed environment according to claim 1, characterized in that: The step of estimating the vehicle posture according to the vehicle real-time speed, the heading angle difference value and the initial vehicle posture corresponding to the vehicle real-time speed to generate the target vehicle posture corresponding to the preset engineering vehicle comprises: The target vehicle posture corresponding to the preset engineering vehicle is calculated based on the initial longitudinal running speed, the initial longitudinal relative position difference, the initial lateral relative position difference, the initial heading angle difference value and the heading angle difference value in the heading angle difference values ​​corresponding to the real-time speed of the vehicle.

6. A vehicle automatic positioning system in a closed environment, characterized in that: Applied to a preset engineering vehicle, the system comprises: A target longitudinal running speed generating module is used to obtain the laser data and inertial sensor measurement data of the preset engineering vehicle, calculate the vehicle target longitudinal running speed based on the inertial sensor measurement data, and generate the target longitudinal running speed; A relative posture generation module, used to perform relative posture calculation based on the laser data and the initial longitudinal running speed corresponding to the target longitudinal running speed, and generate a relative posture; A vehicle real-time speed generating module, used to estimate the vehicle real-time speed based on the target longitudinal running speed and the heading angle difference value corresponding to the relative posture, and generate the vehicle real-time speed; The target vehicle posture generation module is used to estimate the vehicle posture according to the real-time speed of the vehicle, the heading angle difference value and the initial vehicle posture corresponding to the real-time speed of the vehicle, and generate the target vehicle posture corresponding to the preset engineering vehicle.

7. The vehicle automatic positioning system in a closed environment according to claim 6, characterized in that: The inertial sensor measurement data includes three-axis acceleration measurement values ​​and three-axis attitude angle measurement values; the target longitudinal running speed generation module performs the following steps: Convert the three-axis attitude angle measurement value and the three-axis acceleration measurement value into sea level longitudinal acceleration to generate vehicle longitudinal acceleration; A target longitudinal running speed is calculated according to the vehicle longitudinal acceleration and the corresponding initial longitudinal running speed.

8. The vehicle automatic positioning system in a closed environment according to claim 6, characterized in that: The laser data includes first laser data and second laser data; the relative posture generation module performs the following steps: Calculating the relative position of the target between the first laser data and the second laser data using a point cloud registration algorithm; Calculating a two-dimensional relative pose between the target relative pose and the corresponding initial relative pose to generate a two-dimensional relative pose; The relative posture is calculated according to the heading angle difference value in the two-dimensional relative posture and the initial longitudinal running speed corresponding to the target longitudinal running speed.

9. An electronic device, characterized in that: It comprises a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of the method for automatic positioning of a vehicle in a closed environment as described in any one of claims 1 to 5.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed, the method for automatic positioning of a vehicle in a closed environment as described in any one of claims 1 to 5 is implemented.