CN118091695B - Laser radar-based carriage boundary prediction method, system, device and medium - Google Patents
Laser radar-based carriage boundary prediction method, system, device and medium Download PDFInfo
- Publication number
- CN118091695B CN118091695B CN202410487636.7A CN202410487636A CN118091695B CN 118091695 B CN118091695 B CN 118091695B CN 202410487636 A CN202410487636 A CN 202410487636A CN 118091695 B CN118091695 B CN 118091695B
- Authority
- CN
- China
- Prior art keywords
- carriage
- boundary
- laser radar
- filtering algorithm
- iteration
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 238000001914 filtration Methods 0.000 claims abstract description 70
- 239000013598 vector Substances 0.000 claims abstract description 48
- 230000004927 fusion Effects 0.000 claims abstract description 10
- 239000011159 matrix material Substances 0.000 claims description 44
- 238000010276 construction Methods 0.000 claims description 10
- 238000005070 sampling Methods 0.000 claims description 4
- 230000009191 jumping Effects 0.000 claims description 3
- 238000005259 measurement Methods 0.000 abstract description 23
- 238000004891 communication Methods 0.000 description 8
- 238000012545 processing Methods 0.000 description 7
- 238000005457 optimization Methods 0.000 description 4
- 230000003044 adaptive effect Effects 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
Abstract
The invention provides a laser radar-based carriage boundary prediction method, a laser radar-based carriage boundary prediction system, a laser radar-based carriage boundary prediction device and a laser radar-based carriage boundary prediction medium, and belongs to the technical field of measurement and positioning in complex environments. The method comprises the following steps: measuring boundary information of a carriage to be measured through a laser radar; taking boundary information of a carriage to be detected as a state vector of a filtering algorithm, and constructing a state equation of the filtering algorithm and an observation equation of the filtering algorithm; constructing a second observation equation based on the observation equation of the filtering algorithm to counteract the influence of colored noise; and estimating the boundary of the carriage by utilizing the self-adaptive expected auxiliary Kalman algorithm based on the second observation equation, the state equation of the filtering algorithm and the observation equation of the filtering algorithm. According to the method, the influence of colored measurement noise is eliminated through reconstruction of the observation noise, the estimated accuracy of the data fusion filter under complex conditions can be effectively improved, and the estimated accuracy of the carriage boundary is further improved.
Description
Technical Field
The invention relates to the technical field of measurement and positioning in complex environments, in particular to a laser radar-based carriage boundary prediction method, a laser radar-based carriage boundary prediction system, a laser radar-based carriage boundary prediction device and a laser radar-based carriage boundary prediction medium.
Background
The laser radar plays an important role in the aspect of carriage boundary measurement by the characteristics of high precision and high resolution. Through the emission and the reception of the laser beam, the laser radar can construct a three-dimensional space model of the carriage, thereby realizing the recognition and the strategy of the carriage boundary. The method has important significance in the fields of automatic driving, intelligent logistics and the like, and is beneficial to improving the safety and efficiency of the vehicle.
However, lidar also has some technical drawbacks in car measurements. First, the measurement range and accuracy of lidar may be somewhat limited. In addition, the laser radar may have a large error in long-distance measurement, and error correction is required. Second, the scanning speed of lidar is relatively slow, and may not meet the measurement requirements in fast moving and highly dynamic scenarios. When taking measurements inside the car, if the car is in motion or if the internal objects are moving rapidly, the lidar may not be able to capture these changes in real time, resulting in a lag or inaccuracy in the measurement results.
In order to improve the accuracy of measurement, related personnel design various carriage boundary estimation algorithms and improve the accuracy and stability of the laser radar in carriage boundary estimation by optimizing algorithm parameters, improving a data processing method, introducing machine learning and other technologies.
However, the influence of colored measurement noise on measurement accuracy is often not fully considered in the existing carriage boundary estimation algorithm when laser radar data are processed. Colored measurement noise refers to the existence of correlation between the statistical characteristics (such as mean, variance or covariance) of noise and the measured value, and this correlation may cause the performance of the prediction algorithm to be degraded, so that an estimation error is generated in practical application. Colored measurement noise may originate from a variety of factors, such as hardware characteristics of the lidar, environmental disturbances, temperature variations, etc. These factors may lead to inaccuracy or instability in the measured data, which in turn affects the accuracy of the car boundary prediction.
Disclosure of Invention
Aiming at the problems, the invention aims to provide a laser radar-based compartment boundary estimation method, a laser radar-based compartment boundary estimation system, a laser radar-based compartment boundary estimation device and a laser radar-based medium.
The invention aims to achieve the aim, and the aim is achieved by the following technical scheme: a carriage boundary estimating method based on a laser radar comprises the following steps:
measuring boundary information of a carriage to be measured through a laser radar;
Taking boundary information of a carriage to be detected as a state vector of a filtering algorithm, and constructing a state equation of the filtering algorithm and an observation equation of the filtering algorithm;
constructing a second observation equation based on the observation equation of the filtering algorithm to counteract the influence of colored noise;
And estimating the boundary of the carriage by utilizing the self-adaptive expected auxiliary Kalman algorithm based on the second observation equation, the state equation of the filtering algorithm and the observation equation of the filtering algorithm.
Further, the measuring the boundary information of the carriage to be measured by the laser radar includes:
The position and speed of the lidar itself in the x, y and z directions and the position of the car boundary points in the x, y and z directions are measured by the lidar.
Further, the state equation of the filtering algorithm is specifically as follows:
；
Wherein, The position of the laser radar k in the x, y and z directions at the moment; /(I)The velocity of the laser radar k in the x, y and z directions; /(I)For sampling period,/>The system noise at time k is the covariance matrix of the system noise；/>Position information of a carriage boundary point k in x, y and z directions; /(I)Is a system matrix; /(I)Fusing the state vector of the model for the filtering algorithm data at the moment k; /(I)One-step pre-estimation of a model state vector is fused for the filtering algorithm data at the moment k; m is the number of car boundary points.
Further, the observation equation of the filtering algorithm is specifically as follows:
；
Wherein, Observation vector of data fusion model for filtering algorithm,/>Fusion of filtering algorithm data with observation matrix of model observation equation,/>The distance between the laser radar and the boundary point of the carriage is measured for the laser radar; /(I)Is colored observation noise; /(I)Is a color factor; /(I)Is white noise,/>For/>Is a covariance matrix of (2).
Further, the second observation equation is specifically as follows:
；
Wherein, For k time/>Jacobian matrix of (a); /(I)An observation matrix that is a second observation equation; /(I)White noise for the second observation equation,/>For/>Is a covariance matrix of (1); /(I)Is a system matrix;；
The observed noise of the second observation equation is:
。
further, the estimating the car boundary by using the adaptive expectation-assisting Kalman algorithm includes:
the following formula is used for one-step estimation:
；
；
Wherein, An error matrix at the moment k; /(I)One-step pre-estimation of an error matrix at the moment k;
Using the formula Performing initialization assignment;
Wherein, For the error vector of the internal iteration at time k,/>The internal iteration state vector at the moment k is marked with iteration times in brackets, and 0 is the initial time;
After the initialization assignment is completed, the filter is used for carrying out Step one, performing internal iteration;
The filter is specifically as follows:
；
；
；
；
Wherein, S is the iteration step number and s is the iteration step number; /(I)Representing an innovation matrix of the s+1st step of internal iteration at the k moment; /(I)Representing the filter gain of the s+1st step of the internal iteration of the k moment; /(I)The error vector of the s step is iterated for the inside of the k moment; /(I)The state vector of the s step is iterated for the inside of the k moment; /(I)The state vector of the s+1st step is iterated for the inside of the k moment;
After each iteration is completed, the formula is utilized Calculate the mahalanobis distance/>And compare/>A threshold door;
If it is Then, noise and error vector estimation is carried out; after the estimation is completed, the iteration step number S is added with 1, and internal iteration is continued until the iteration step number S meets the total iteration step number/>；
If it isDirectly jumping out of the internal iteration;
after the internal iteration is completed, the output of the filter is obtained according to the following formula:
；
；
and determining a carriage boundary estimation result according to the output of the filter.
Further, noise and error vector estimation is performed according to the following formula:
；
。
correspondingly, the invention discloses a carriage boundary estimating system based on a laser radar, which comprises the following steps:
the measuring module is used for measuring boundary information of the carriage to be measured through the laser radar;
The first equation construction module is used for taking boundary information of a carriage to be detected as a state vector of a filtering algorithm and constructing a state equation of the filtering algorithm and an observation equation of the filtering algorithm;
The second equation construction module is used for constructing a second observation equation based on the observation equation of the filtering algorithm so as to counteract the influence of colored noise;
And the estimating module is used for estimating the boundary of the carriage by utilizing the self-adaptive expected auxiliary Kalman algorithm based on the second observation equation, the state equation of the filtering algorithm and the observation equation of the filtering algorithm.
Correspondingly, the invention discloses a carriage boundary estimating device based on a laser radar, which comprises:
the storage is used for storing a carriage boundary estimation program based on the laser radar;
The processor is configured to implement the method for estimating a car boundary based on the laser radar according to any one of the above steps when executing the program for estimating a car boundary based on the laser radar.
Correspondingly, the invention discloses a readable storage medium, wherein a laser radar-based carriage boundary estimating program is stored on the readable storage medium, and the laser radar-based carriage boundary estimating program realizes the steps of the laser radar-based carriage boundary estimating method when being executed by a processor.
Compared with the prior art, the invention has the beneficial effects that: the invention discloses a laser radar-based carriage boundary estimation method, a laser radar-based carriage boundary estimation system, a laser radar-based carriage boundary estimation device and a laser radar-based carriage boundary estimation medium. The observation equation constructed by the invention fully considers the influence of the colored measurement noise on the measurement precision, and eliminates the influence of the colored measurement noise by reconstructing the observation noise. In the filtering process, the Markov distance is used for judging the performance of KF which is greatly expected to be estimated and assisted, so that the self-adaptive capacity of the algorithm is effectively improved, and the accuracy of the algorithm is further improved.
According to the method, the boundary information of the carriage to be measured is measured through the laser radar, and the accurate position information of the carriage can be obtained through the method. The boundary of the carriage can be estimated more accurately by combining the state equation and the observation equation of the filtering algorithm. Particularly, the influence of colored noise is counteracted by constructing a second observation equation, so that the accuracy of estimation is further improved.
The invention not only considers the position and the speed of the laser radar in the x, y and z directions, but also considers the position of the carriage boundary point in the x, y and z directions, thereby being capable of adapting to the conditions of different carriage shapes and sizes. In addition, the self-adaptive expectation-aided Kalman algorithm is adopted for prediction, so that the method can dynamically adjust parameters according to different conditions, and the adaptability is further improved.
When the system is faced with interference factors such as colored noise, the influence of the noise can be effectively counteracted by constructing the second observation equation, so that the robustness of the system is improved. In addition, the filter is utilized for internal iteration, so that errors can be further reduced, and the stability of the estimated result is improved.
In conclusion, the method realizes high-precision estimation of the carriage boundary through means of accurate measurement, filter algorithm optimization, noise processing and the like, and has the advantages of strong adaptability, high robustness, good real-time performance, algorithm optimization, flexibility and the like.
It can be seen that the present invention has outstanding substantial features and significant advances over the prior art, as well as the benefits of its implementation.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings that are required to be used in the embodiments or the description of the prior art will be briefly described below, and it is obvious that the drawings in the following description are only embodiments of the present invention, and that other drawings can be obtained according to the provided drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a method of an embodiment of the present invention.
FIG. 2 is a flow chart of estimating a car boundary using an adaptive desired assist Kalman algorithm according to an embodiment of the present invention.
Fig. 3 is a system configuration diagram of an embodiment of the present invention.
In the figure, 1, a measurement module; 2. a first equation construction module; 3. a second equation construction module; 4. and a pre-estimating module.
Detailed Description
In order to better understand the aspects of the present invention, the present invention will be described in further detail with reference to the accompanying drawings and detailed description. It will be apparent that the described embodiments are only some, but not all, embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
Referring to fig. 1, the present embodiment provides a laser radar-based car boundary estimation method, which includes the following steps:
S1: and measuring boundary information of the carriage to be measured by a laser radar.
In particular embodiments, the position and velocity of the lidar itself in the x, y and z directions, as well as the position of the car boundary points in the x, y and z directions, are measured by the lidar.
S2: and taking boundary information of the carriage to be detected as a state vector of a filtering algorithm, and constructing a state equation of the filtering algorithm and an observation equation of the filtering algorithm.
In the specific embodiment, the position and the speed of the laser radar in the x, y and z directions and the position of the boundary point of the carriage in the x, y and z directions are taken as state vectors of a filtering algorithm.
The state equation of the filtering algorithm is:
；
Wherein, The position of the laser radar k in the x, y and z directions at the moment; /(I)The velocity of the laser radar k in the x, y and z directions; /(I)For sampling period,/>The system noise at time k is the covariance matrix of the system noise；/>Position information of a carriage boundary point k in x, y and z directions; /(I)Is a system matrix; /(I)Fusing the state vector of the model for the filtering algorithm data at the moment k; /(I)One-step pre-estimation of a model state vector is fused for the filtering algorithm data at the moment k; m is the number of car boundary points.
The observation equation of the filtering algorithm is:
；
Wherein, Observation vector of data fusion model for filtering algorithm,/>Fusion of filtering algorithm data with observation matrix of model observation equation,/>The distance between the laser radar and the boundary point of the carriage is measured for the laser radar; /(I)Is colored observation noise; /(I)Is a color factor; /(I)Is white noise,/>For/>Is a covariance matrix of (2).
S3: based on the observation equation of the filtering algorithm, a second observation equation is constructed to counteract the effect of colored noise.
In a specific embodiment, the influence of colored noise is counteracted by constructing a second observation equation, which specifically comprises the following steps:
；
Wherein, For k time/>Jacobian matrix of (a); /(I)An observation matrix that is a second observation equation; /(I)White noise for the second observation equation,/>For/>Is a covariance matrix of (1); /(I)Is a system matrix;。
The observed noise of the second observation equation is:
。
s4: and estimating the boundary of the carriage by utilizing the self-adaptive expected auxiliary Kalman algorithm based on the second observation equation, the state equation of the filtering algorithm and the observation equation of the filtering algorithm.
In a specific embodiment, based on the second observation equation, the state equation and the second observation equation based on the filtering algorithm utilize the adaptive expectation-based auxiliary Kalman algorithm to estimate the boundary of the cabin, as shown in fig. 2, and the specific steps are as follows:
Firstly, one-step estimation is carried out:
；
；
Wherein, An error matrix at the moment k; /(I)Is one-step estimation of an error matrix at the moment k.
Secondly, carrying out initialization assignment:
；
Wherein, For the error vector of the internal iteration at time k,/>The state vector of the internal iteration at the time k is shown in brackets, the iteration number is shown in brackets, and 0 is the initial number.
On the basis, the filter is used forStep internal iteration, wherein,/>For the total number of iteration steps, s is the number of iteration steps, and the specific steps of the internal iteration are as follows:
；
；
；
；
Wherein, Representing an innovation matrix of the s+1st step of internal iteration at the k moment; /(I)Representing the filter gain of the s+1st step of the internal iteration of the k moment; /(I)The error vector of the s step is iterated for the inside of the k moment; /(I)The state vector of the s step is iterated for the inside of the k moment; /(I)The state vector of step s+1 is iterated internally for the k moment.
After each internal iteration is completed, the performance of the filter is evaluated by utilizing the mahalanobis distance, and a specific calculation formula of the mahalanobis distance is as follows:
；
in the evaluation, a threshold value door of the mahalanobis distance is preset according to experience. And then compare And a threshold door.
If it isAnd carrying out noise and error vector estimation. The noise and error vector prediction calculation steps are as follows:
；
。
After the estimation is completed, adding 1 to the iteration step number S, and continuing the internal iteration until the iteration step number S meets the iteration total step number 。
If it isThen the internal iteration is directly jumped out.
After the internal iteration is completed, the output of the filter is obtained according to the following formula:
；
。
And finally, determining a carriage boundary estimated result according to the output of the filter.
The invention provides a laser radar-based carriage boundary estimating method, which realizes high-precision carriage boundary estimation through means of accurate measurement, filter algorithm optimization, noise processing and the like, and has the advantages of strong adaptability, high robustness, good real-time performance, algorithm optimization, flexibility and the like.
Referring to fig. 3, the invention also discloses a laser radar-based carriage boundary estimating system, which comprises: a measurement module 1, a first equation construction module 2, a second equation construction module 3 and an estimation module 4.
And the measuring module 1 is used for measuring the boundary information of the carriage to be measured through the laser radar.
The first equation construction module 2 is configured to take boundary information of a to-be-detected carriage as a state vector of a filtering algorithm, and construct a state equation of the filtering algorithm and an observation equation of the filtering algorithm.
And a second equation construction module 3, configured to construct a second observation equation based on the observation equation of the filtering algorithm, so as to cancel the influence of the colored noise.
And the estimating module 4 is used for estimating the boundary of the carriage by utilizing the self-adaptive expected auxiliary Kalman algorithm based on the second observation equation, the state equation of the filtering algorithm and the observation equation of the filtering algorithm.
The specific implementation manner of the laser radar-based vehicle boundary estimation system in this embodiment is substantially identical to the specific implementation manner of the laser radar-based vehicle boundary estimation method described above, and will not be described herein.
The invention also discloses a carriage boundary estimating device based on the laser radar, which comprises a processor and a memory; the step of implementing the laser radar-based car boundary estimation method according to any one of the above steps when the processor executes the laser radar-based car boundary estimation program stored in the memory.
Further, the laser radar-based compartment boundary estimating device in this embodiment may further include:
The input interface is used for acquiring an externally imported laser radar-based compartment boundary estimation program, storing the acquired laser radar-based compartment boundary estimation program into the memory, and also can be used for acquiring various instructions and parameters transmitted by external terminal equipment and transmitting the various instructions and parameters into the processor so that the processor can develop corresponding processing by utilizing the various instructions and parameters. In this embodiment, the input interface may specifically include, but is not limited to, a USB interface, a serial interface, a voice input interface, a fingerprint input interface, a hard disk reading interface, and the like.
And the output interface is used for outputting various data generated by the processor to the terminal equipment connected with the output interface so that other terminal equipment connected with the output interface can acquire various data generated by the processor. In this embodiment, the output interface may specifically include, but is not limited to, a USB interface, a serial interface, and the like.
The communication unit is used for establishing remote communication connection between the laser radar-based carriage boundary estimating device and the external server so that the laser radar-based carriage boundary estimating device can mount the image file to the external server. In this embodiment, the communication unit may specifically include, but is not limited to, a remote communication unit based on a wireless communication technology or a wired communication technology.
And the keyboard is used for acquiring various parameter data or instructions input by a user by knocking the key cap in real time.
And the display is used for running the relevant information of the laser radar-based compartment boundary estimation process to display in real time.
A mouse may be used to assist a user in inputting data and to simplify user operations.
The invention also discloses a readable storage medium, which includes Random Access Memory (RAM), memory, read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, removable disk, CD-ROM, or any other form of storage medium known in the art. A laser radar-based car boundary estimation program is stored in the readable storage medium, and when the laser radar-based car boundary estimation program is executed by the processor, the steps of the laser radar-based car boundary estimation method according to any one of the above are implemented.
In summary, the influence of colored measurement noise is eliminated by reconstructing the observation noise, so that the estimation accuracy of the data fusion filter under complex conditions can be effectively improved, and the estimation accuracy of the carriage boundary is further improved.
In this specification, each embodiment is described in a progressive manner, and each embodiment is mainly described in a different point from other embodiments, so that the same or similar parts between the embodiments are referred to each other. For the method disclosed in the embodiment, since it corresponds to the system disclosed in the embodiment, the description is relatively simple, and the relevant points refer to the description of the method section.
Those of skill would further appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative elements and steps are described above generally in terms of functionality in order to clearly illustrate the interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the solution. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
In the several embodiments provided by the present invention, it should be understood that the disclosed systems, and methods may be implemented in other ways. For example, the system embodiments described above are merely illustrative, e.g., the division of the elements is merely a logical functional division, and there may be additional divisions when actually implemented, e.g., multiple elements or components may be combined or integrated into another system, or some features may be omitted or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed with each other may be through some interface, system or unit indirect coupling or communication connection, which may be in electrical, mechanical or other form.
The units described as separate units may or may not be physically separate, and units shown as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of 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.
In addition, each functional module in the embodiments of the present invention may be integrated in one processing unit, or each module may exist alone physically, or two or more modules may be integrated in one unit.
Similarly, each processing unit in the embodiments of the present invention may be integrated in one functional module, or each processing unit may exist physically, or two or more processing units may be integrated in one functional module.
The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied directly in hardware, in a software module executed by a processor, or in a combination of the two. The software modules may be disposed in Random Access Memory (RAM), memory, read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.
Finally, it is further noted that relational terms such as first and second, and the like are used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Moreover, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or apparatus that comprises the element.
The laser radar-based carriage boundary estimating method, the laser radar-based carriage boundary estimating system, the laser radar-based carriage boundary estimating device and the laser radar-based carriage boundary estimating method and the laser radar-based carriage boundary estimating device are described in detail. The principles and embodiments of the present invention have been described herein with reference to specific examples, the description of which is intended only to facilitate an understanding of the method of the present invention and its core ideas. It should be noted that it will be apparent to those skilled in the art that the present invention may be modified and practiced without departing from the spirit of the present invention.
Claims (4)
1. A laser radar-based carriage boundary prediction method is characterized by comprising the following steps:
measuring boundary information of a carriage to be measured through a laser radar;
Taking boundary information of a carriage to be detected as a state vector of a filtering algorithm, and constructing a state equation of the filtering algorithm and an observation equation of the filtering algorithm;
constructing a second observation equation based on the observation equation of the filtering algorithm to counteract the influence of colored noise;
Based on a second observation equation, a state equation of a filtering algorithm and an observation equation of the filtering algorithm, estimating the boundary of the carriage by using a self-adaptive expected auxiliary Kalman algorithm;
the measuring boundary information of the carriage to be measured by the laser radar comprises the following steps:
Measuring the position and the speed of the laser radar in the x, y and z directions and the position of the carriage boundary point in the x, y and z directions by the laser radar;
The state equation of the filtering algorithm is specifically as follows:
；
Wherein, The position of the laser radar k in the x, y and z directions at the moment; /(I)The velocity of the laser radar k in the x, y and z directions; /(I)For sampling period,/>The system noise at time k is the covariance matrix/>；Position information of a carriage boundary point k in x, y and z directions; /(I)Is a system matrix; /(I)Fusing the state vector of the model for the filtering algorithm data at the moment k; /(I)One-step pre-estimation of a model state vector is fused for the filtering algorithm data at the moment k; m is the number of carriage boundary points;
The observation equation of the filtering algorithm is specifically as follows:
；
Wherein, Observation vector of data fusion model for filtering algorithm,/>Fusion of filtering algorithm data with observation matrix of model observation equation,/>The distance between the laser radar and the boundary point of the carriage is measured for the laser radar; Is colored observation noise; /(I) Is a color factor; /(I)Is white noise,/>For/>Is a covariance matrix of (1);
the second observation equation is specifically as follows:
；
Wherein, For k time/>Jacobian matrix of (a); /(I)An observation matrix that is a second observation equation; /(I)White noise for the second observation equation,/>For/>Is a covariance matrix of (1); /(I)Is a system matrix; /(I)；
The observed noise of the second observation equation is:
；
The estimating the boundary of the carriage by using the self-adaptive expected auxiliary Kalman algorithm comprises the following steps:
the following formula is used for one-step estimation:
；
；
Wherein, An error matrix at the moment k; /(I)One-step pre-estimation of an error matrix at the moment k;
Using the formula Performing initialization assignment:
Wherein, For the error vector of the internal iteration at time k,/>The internal iteration state vector at the moment k is marked with iteration times in brackets, and 0 is the initial time;
After the initialization assignment is completed, the filter is used for carrying out Step one, performing internal iteration;
The filter is specifically as follows:
；
；
；
；
Wherein, S is the iteration step number and s is the iteration step number; /(I)Representing an innovation matrix of the s+1st step of internal iteration at the k moment; /(I)Representing the filter gain of the s+1st step of the internal iteration of the k moment; /(I)The error vector of the s step is iterated for the inside of the k moment; /(I)The state vector of the s step is iterated for the inside of the k moment; /(I)The state vector of the s+1st step is iterated for the inside of the k moment;
After each iteration is completed, the formula is utilized Calculating the mahalanobis distanceAnd compare/>A threshold door;
If it is Then, noise and error vector estimation is carried out; after the estimation is completed, the iteration step number S is added with 1, and internal iteration is continued until the iteration step number S meets the total iteration step number/>；
If it isDirectly jumping out of the internal iteration;
after the internal iteration is completed, the output of the filter is obtained according to the following formula:
；
；
determining a carriage boundary estimation result according to the output of the filter;
noise and error vector estimation is performed according to the following formula:
；
。
2. A lidar-based car boundary prediction system, comprising:
the measuring module is used for measuring boundary information of the carriage to be measured through the laser radar;
The first equation construction module is used for taking boundary information of a carriage to be detected as a state vector of a filtering algorithm and constructing a state equation of the filtering algorithm and an observation equation of the filtering algorithm;
The second equation construction module is used for constructing a second observation equation based on the observation equation of the filtering algorithm so as to counteract the influence of colored noise;
The estimating module is used for estimating the boundary of the carriage by utilizing the self-adaptive expected auxiliary Kalman algorithm based on the second observation equation, the state equation of the filtering algorithm and the observation equation of the filtering algorithm;
The state equation of the filtering algorithm is specifically as follows:
；
Wherein, The position of the laser radar k in the x, y and z directions at the moment; /(I)The velocity of the laser radar k in the x, y and z directions; /(I)For sampling period,/>The system noise at time k is the covariance matrix/>；Position information of a carriage boundary point k in x, y and z directions; /(I)Is a system matrix; /(I)Fusing the state vector of the model for the filtering algorithm data at the moment k; /(I)One-step pre-estimation of a model state vector is fused for the filtering algorithm data at the moment k; m is the number of carriage boundary points;
The observation equation of the filtering algorithm is specifically as follows:
；
Wherein, Observation vector of data fusion model for filtering algorithm,/>Fusion of filtering algorithm data with observation matrix of model observation equation,/>The distance between the laser radar and the boundary point of the carriage is measured for the laser radar; Is colored observation noise; /(I) Is a color factor; /(I)Is white noise,/>For/>Is a covariance matrix of (1);
the second observation equation is specifically as follows:
；
Wherein, For k time/>Jacobian matrix of (a); /(I)An observation matrix that is a second observation equation; /(I)White noise for the second observation equation,/>For/>Is a covariance matrix of (1); /(I)Is a system matrix; /(I)；
The observed noise of the second observation equation is:
；
The estimating the boundary of the carriage by using the self-adaptive expected auxiliary Kalman algorithm comprises the following steps:
the following formula is used for one-step estimation:
；
；
Wherein, An error matrix at the moment k; /(I)One-step pre-estimation of an error matrix at the moment k;
Using the formula Performing initialization assignment:
Wherein, For the error vector of the internal iteration at time k,/>The internal iteration state vector at the moment k is marked with iteration times in brackets, and 0 is the initial time;
After the initialization assignment is completed, the filter is used for carrying out Step one, performing internal iteration;
The filter is specifically as follows:
；
；
；
；
Wherein, S is the iteration step number and s is the iteration step number; /(I)Representing an innovation matrix of the s+1st step of internal iteration at the k moment; /(I)Representing the filter gain of the s+1st step of the internal iteration of the k moment; /(I)The error vector of the s step is iterated for the inside of the k moment; /(I)The state vector of the s step is iterated for the inside of the k moment; /(I)The state vector of the s+1st step is iterated for the inside of the k moment;
After each iteration is completed, the formula is utilized Calculating the mahalanobis distanceAnd compare/>A threshold door;
If it is Then, noise and error vector estimation is carried out; after the estimation is completed, the iteration step number S is added with 1, and internal iteration is continued until the iteration step number S meets the total iteration step number/>；
If it isDirectly jumping out of the internal iteration;
after the internal iteration is completed, the output of the filter is obtained according to the following formula:
；
；
determining a carriage boundary estimation result according to the output of the filter;
noise and error vector estimation is performed according to the following formula:
；
。
3. a laser radar-based car boundary estimation device, comprising:
the storage is used for storing a carriage boundary estimation program based on the laser radar;
a processor for implementing the laser radar-based car boundary estimation method according to claim 1 when executing the laser radar-based car boundary estimation program.
4. A readable storage medium, characterized by: the readable storage medium stores a laser radar-based car boundary estimation program, which when executed by a processor, implements the laser radar-based car boundary estimation method according to claim 1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410487636.7A CN118091695B (en) | 2024-04-23 | Laser radar-based carriage boundary prediction method, system, device and medium |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410487636.7A CN118091695B (en) | 2024-04-23 | Laser radar-based carriage boundary prediction method, system, device and medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN118091695A CN118091695A (en) | 2024-05-28 |
| CN118091695B true CN118091695B (en) | 2024-06-25 |
Family
ID=
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104619020A (en) * | 2015-02-06 | 2015-05-13 | 合肥工业大学 | RSSI and TOA distance measurement based WIFI indoor positioning method |
| CN110375772A (en) * | 2019-07-29 | 2019-10-25 | 中国航空工业集团公司北京长城计量测试技术研究所 | The ring laser stochastic error modeling of adaptive Kalman filter and compensation method |
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104619020A (en) * | 2015-02-06 | 2015-05-13 | 合肥工业大学 | RSSI and TOA distance measurement based WIFI indoor positioning method |
| CN110375772A (en) * | 2019-07-29 | 2019-10-25 | 中国航空工业集团公司北京长城计量测试技术研究所 | The ring laser stochastic error modeling of adaptive Kalman filter and compensation method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112113574B (en) | Method, apparatus, computing device and computer-readable storage medium for positioning | |
| CN114325667A (en) | External parameter calibration method and device for combined navigation equipment and laser radar | |
| CN110572139B (en) | Fusion filtering implementation method and device for vehicle state estimation, storage medium and vehicle | |
| CN113960622A (en) | Real-time positioning method and device fusing laser radar and IMU sensor information | |
| CN115046540A (en) | Point cloud map construction method, system, equipment and storage medium | |
| CN112180361A (en) | Vehicle-mounted radar target tracking method based on dynamic federal filtering | |
| CN115457510A (en) | Self-adaptive cooperative target tracking method based on variational Bayes | |
| CN117222915A (en) | System and method for tracking an expanded state of a moving object using a composite measurement model | |
| CN116500575A (en) | Extended target tracking method and device based on variable decibel leaf theory | |
| Melo et al. | A data‐driven particle filter for terrain based navigation of sensor‐limited autonomous underwater vehicles | |
| CN118091695B (en) | Laser radar-based carriage boundary prediction method, system, device and medium | |
| CN114063131A (en) | GNSS/INS/wheel speed combined positioning real-time smoothing method | |
| CN111736144B (en) | Maneuvering turning target state estimation method only by distance observation | |
| CN111735443B (en) | Dense target track correlation method based on assignment matrix | |
| CN110515060B (en) | Method for calibrating reflectivity of multi-line laser radar | |
| CN118091695A (en) | Laser radar-based carriage boundary prediction method, system, device and medium | |
| CN109655057B (en) | Filtering optimization method and system for accelerator measurement value of six-push unmanned aerial vehicle | |
| CN116929343A (en) | Pose estimation method, related equipment and storage medium | |
| CN116734860A (en) | Multi-AUV self-adaptive cooperative positioning method and system based on factor graph | |
| CN116772867A (en) | Multi-AUV self-adaptive co-location method and system based on node optimization of factor graph | |
| CN117012049A (en) | UWB-based vehicle searching guiding method and related device | |
| CN114741659B (en) | Adaptive model on-line reconstruction robust filtering method, device and system | |
| CN116182905A (en) | Laser radar and combined inertial navigation space-time external parameter calibration method, device and system | |
| CN113390376B (en) | Method, device and system for determining cabin displacement of wind generating set | |
| CN110412531B (en) | Amplitude information-based receiving station path optimization method under clutter condition |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant |