CN117818754B - Course angle correction method and device, storage medium and electronic equipment - Google Patents
Course angle correction method and device, storage medium and electronic equipment Download PDFInfo
- Publication number
- CN117818754B CN117818754B CN202410238825.0A CN202410238825A CN117818754B CN 117818754 B CN117818754 B CN 117818754B CN 202410238825 A CN202410238825 A CN 202410238825A CN 117818754 B CN117818754 B CN 117818754B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- angle
- course
- course angle
- sets
- 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 64
- 238000012937 correction Methods 0.000 title claims abstract description 39
- 230000033001 locomotion Effects 0.000 claims abstract description 51
- 230000007613 environmental effect Effects 0.000 claims abstract description 16
- 239000011159 matrix material Substances 0.000 claims description 33
- 238000001914 filtration Methods 0.000 claims description 30
- 238000004422 calculation algorithm Methods 0.000 claims description 29
- 238000005259 measurement Methods 0.000 claims description 26
- 238000013507 mapping Methods 0.000 claims description 24
- 239000013598 vector Substances 0.000 claims description 23
- 230000008859 change Effects 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 10
- 230000008569 process Effects 0.000 description 12
- 238000010586 diagram Methods 0.000 description 11
- 238000012545 processing Methods 0.000 description 9
- 238000004364 calculation method Methods 0.000 description 6
- 238000004891 communication Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 5
- 230000007704 transition Effects 0.000 description 5
- 230000003287 optical effect Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000005286 illumination Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 102000006822 Agouti Signaling Protein Human genes 0.000 description 1
- 108010072151 Agouti Signaling Protein Proteins 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000001934 delay Effects 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000003064 k means clustering Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Landscapes
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
The application provides a course angle correction method, a course angle correction device, a storage medium and electronic equipment. The electronic equipment measures environment information outside the vehicle through the ultrasonic sensor; acquiring a first course angle of the vehicle at the current moment according to the environmental information; measuring the motion state information of the vehicle by a motion state sensor; obtaining a second course angle of the vehicle at the current moment according to the motion state information; and fusing the first course angle and the second course angle to obtain the current target course angle of the vehicle. Because the ultrasonic sensor and the motion state sensor are low in price and can adapt to various scenes, the stability and the accuracy of the course angle measuring result can be improved after the ultrasonic sensor and the motion state sensor are combined.
Description
Technical Field
The application relates to the field of automobiles, in particular to a course angle correction method, a course angle correction device, a storage medium and electronic equipment.
Background
The heading angle (also called direction angle) of a vehicle refers to the angle between the forward direction of the vehicle and the north direction. The development of the vehicle course angle calculation technology can be traced back to the early twentieth century. At that time, a mechanical instrument is used to measure the heading angle of the vehicle, which requires manual adjustment of the instrument, and errors easily occur in the measurement process. Later, with the development of electronic technology, electronic instruments are beginning to be used to measure the heading angle of a vehicle.
In a vehicle navigation system, a heading angle is one of key parameters of vehicle navigation, and therefore, the accuracy of calculation of the heading angle directly affects the accuracy and stability of the vehicle navigation system. However, in the practical process, the existing course angle measuring method with higher precision has the problems of over high hardware cost or poor stability. Therefore, there is a need to provide a low cost and high stability solution for heading angle measurement.
Disclosure of Invention
In order to overcome at least one defect in the prior art, the application provides a course angle correction method, a device, a storage medium and electronic equipment, which specifically comprise the following steps:
in a first aspect, the present application provides a heading angle correction method, the method including:
measuring environmental information outside the vehicle by an ultrasonic sensor;
acquiring a first course angle of the current moment of the vehicle according to the environment information;
Measuring the motion state information of the vehicle by a motion state sensor;
Obtaining a second course angle of the current moment of the vehicle according to the motion state information;
and fusing the first course angle and the second course angle to obtain the current target course angle of the vehicle.
With reference to the optional implementation manner of the first aspect, the fusing the first heading angle with the second heading angle to obtain a target heading angle of the vehicle includes:
acquiring a first error between the first course angle and the second course angle;
Obtaining a more accurate second error through a linear filtering algorithm according to the first course angle, the second course angle and the first error;
and correcting the second course angle by using the second error to obtain the target course angle.
With reference to the optional implementation manner of the first aspect, the deriving the second more accurate error from the first heading angle, the second heading angle and the first error by means of linear filtering includes:
Taking the first course angle as an observation value required by the linear filtering algorithm;
taking the second course angle as a predicted value required by the linear filtering algorithm;
Taking the first error as one of the state vectors required by the linear filtering algorithm;
according to the observed value, the predicted value and the state vector, obtaining an updated state vector after linear filtering;
and obtaining a more accurate second error from the updated state vector.
With reference to the optional implementation manner of the first aspect, the environmental information includes two sets of spatial coordinates obtained by measuring a plurality of feature points twice in succession during a period from a last measurement time to a current time of the vehicle, where the plurality of feature points represent a plurality of positions of an obstacle surface located in an environment, and the obtaining, according to the environmental information, a first heading angle of the current time of the vehicle includes:
Fitting a space mapping matrix between the two sets of space coordinates according to the two sets of space coordinates;
according to the space mapping matrix, obtaining a course change angle of the vehicle from the last measurement moment to the current moment;
And determining a first course angle at the current moment of the vehicle according to the course change angle and the course angle at the last measuring moment.
With reference to the optional implementation manner of the first aspect, the fitting a spatial mapping matrix between the two sets of spatial coordinates according to the two sets of spatial coordinates includes:
and fitting a space mapping matrix between the two sets of space coordinates by a least square method according to the two sets of space coordinates.
With reference to the optional implementation manner of the first aspect, the spatial mapping matrix is capable of converting one set of spatial coordinates of the two sets of spatial coordinates into mapped spatial coordinates, and a difference between the mapped spatial coordinates and the other set of spatial coordinates is minimal.
With reference to the optional implementation manner of the first aspect, the environmental information includes two sets of spatial coordinates obtained by measuring a plurality of feature points twice in succession during a period from a last measurement time to a current time of the vehicle, where the plurality of feature points represent a plurality of positions on a surface of the obstacle, and the measuring, by using the ultrasonic sensor, the environmental information outside the vehicle includes:
for each of the feature points, measuring at least two measured distances between the feature point and the vehicle by the ultrasonic sensor each time a measurement is made;
and determining the space coordinates of the feature points through a triangular positioning algorithm according to the at least two actually measured distances.
In a second aspect, the present application provides a heading angle correction apparatus, the apparatus comprising:
The first course module is used for measuring environment information outside the vehicle through the ultrasonic sensor; acquiring a first course angle of the current moment of the vehicle according to the environment information;
the second course module is used for measuring the motion state information of the vehicle by the motion state sensor; obtaining a second course angle of the current moment of the vehicle according to the motion state information;
and the course correction module is used for fusing the first course angle and the second course angle to obtain the current target course angle of the vehicle.
With reference to the optional implementation manner of the second aspect, the heading correction module is further specifically configured to:
acquiring a first error between the first course angle and the second course angle;
Obtaining a more accurate second error through a linear filtering algorithm according to the first course angle, the second course angle and the first error;
and correcting the second course angle by using the second error to obtain the target course angle.
With reference to the optional implementation manner of the second aspect, the heading correction module is further specifically configured to:
Taking the first course angle as an observation value required by the linear filtering algorithm;
taking the second course angle as a predicted value required by the linear filtering algorithm;
Taking the first error as one of the state vectors required by the linear filtering algorithm;
according to the observed value, the predicted value and the state vector, obtaining an updated state vector after linear filtering;
and obtaining a more accurate second error from the updated state vector.
With reference to the optional implementation manner of the second aspect, the environment information includes two sets of spatial coordinates obtained by measuring a plurality of feature points twice in succession during a period from a last measurement time to a current time of the vehicle, where the plurality of feature points represent a plurality of positions of an obstacle surface located in the environment, and the first heading module is further specifically configured to:
Fitting a space mapping matrix between the two sets of space coordinates according to the two sets of space coordinates;
according to the space mapping matrix, obtaining a course change angle of the vehicle from the last measurement moment to the current moment;
And determining a first course angle at the current moment of the vehicle according to the course change angle and the course angle at the last measuring moment.
With reference to the optional implementation manner of the second aspect, the first heading module is further specifically configured to:
and fitting a space mapping matrix between the two sets of space coordinates by a least square method according to the two sets of space coordinates.
With reference to an optional implementation manner of the second aspect, the spatial mapping matrix is capable of converting one set of spatial coordinates of the two sets of spatial coordinates into mapped spatial coordinates, and a difference between the mapped spatial coordinates and the other set of spatial coordinates is minimal.
With reference to the optional implementation manner of the second aspect, the environmental information includes two sets of spatial coordinates obtained by measuring a plurality of feature points twice in succession during a period from a last measurement time to a current time of the vehicle, where the plurality of feature points represent a plurality of positions located on the surface of the obstacle, and the first heading module is further specifically configured to:
for each of the feature points, measuring at least two measured distances between the feature point and the vehicle by the ultrasonic sensor each time a measurement is made;
and determining the space coordinates of the feature points through a triangular positioning algorithm according to the at least two actually measured distances.
In a third aspect, the present application provides a storage medium storing a computer program which, when executed by a processor, implements the heading angle correction method.
In a third aspect, the present application provides an electronic device, including a processor and a memory, where the memory stores a computer program, and the computer program, when executed by the processor, implements the heading angle correction method.
Compared with the prior art, the application has the following beneficial effects:
The application provides a course angle correction method, a course angle correction device, a storage medium and electronic equipment. The electronic equipment measures environment information outside the vehicle through the ultrasonic sensor; acquiring a first course angle of the vehicle at the current moment according to the environmental information; measuring the motion state information of the vehicle by a motion state sensor; obtaining a second course angle of the vehicle at the current moment according to the motion state information; and fusing the first course angle and the second course angle to obtain the current target course angle of the vehicle. Because the ultrasonic sensor and the motion state sensor are low in price and can adapt to various scenes, the stability and the accuracy of the course angle measuring result can be improved after the ultrasonic sensor and the motion state sensor are combined.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are needed in the embodiments will be briefly described below, it being understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered as limiting the scope, and other related drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flow chart of a method according to an embodiment of the present application;
fig. 2 is a schematic diagram of an ultrasonic ranging principle according to an embodiment of the present application;
FIG. 3 is a schematic diagram of a coordinate measurement principle according to an embodiment of the present application;
FIG. 4 is a schematic diagram of a geometric relationship provided by an embodiment of the present application;
fig. 5 is a schematic structural diagram of a virtual device according to an embodiment of the present application;
Fig. 6 is a schematic structural diagram of an electronic device according to an embodiment of the present application.
Icon: 101-an obstacle; 102-ultrasonic wave; 103-vehicle; 1011—feature points; 201-a first heading module; 202-a second heading module; 203, a course correction module; 301-memory; 302-a processor; 303-a communication unit; 304-a system bus.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present application more apparent, the technical solutions of the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present application, and it is apparent that the described embodiments are some embodiments of the present application, but not all embodiments of the present application. The components of the embodiments of the present application generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations.
Thus, the following detailed description of the embodiments of the application, as presented in the figures, is not intended to limit the scope of the application, as claimed, but is merely representative of selected embodiments of the application. All other embodiments, which can be made by those skilled in the art based on the embodiments of the application without making any inventive effort, are intended to be within the scope of the application.
It should be noted that: like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further definition or explanation thereof is necessary in the following figures.
In the description of the present application, it should be noted that the terms "first," "second," "third," and the like are used merely to distinguish between descriptions and are not to be construed as indicating or implying relative importance. Furthermore, 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.
Based on the above statement, as introduced in the background art, the existing heading angle measuring method with higher precision has the problems of overhigh hardware cost or poor stability. In this regard, it should be understood that the vehicle course angle calculation technology mainly includes a global positioning system, a laser radar, a vision sensor, and an inertial navigation system, but these methods have the following problems:
Global positioning system (Global Positioning System, GPS), which is a satellite-based positioning technology, can determine the position and direction of a vehicle from satellite signals. However, the GPS has a problem of signal interference, so that higher vehicle accuracy cannot be obtained in any scene, and the requirement of automatic driving of the automobile cannot be met. For example, in indoor, underground parking, etc. scenarios, GPS signals typically cannot penetrate buildings or underground structures and therefore cannot provide accurate location information.
Lidar, which is a technology for acquiring distance information by scanning a surrounding environment with a laser beam, is expensive at present and is not suitable for large-scale application and measuring a heading angle of a vehicle, although high-precision and stable positioning and map construction can be realized. In addition, if a lidar is used, a large amount of point cloud data needs to be processed, and if the computing power cannot keep up, delays or errors may be caused.
The visual sensor can acquire image information of surrounding environment through equipment such as a camera and the like, and the position and the direction of the vehicle are estimated. However, the vision sensor is sensitive to factors such as illumination conditions and weather, so that accurate information of course angle information cannot be provided stably in a scene with poor illumination environment. In addition, in the same manner as the lidar, if a vision sensor is used, a large amount of calculation is required for the image acquired by the vision sensor, and if the calculation force cannot keep up, delay or error may be caused.
The inertial navigation system calculates the position and the direction of the vehicle by measuring the acceleration and the angular velocity of the vehicle, and has higher precision and stability, but the inertial navigation system has a drift problem and can not stably provide accurate information of course angle information.
Based on the findings of the above technical problems, the inventors have made creative efforts to propose the following technical solutions to solve or improve the above problems. It should be noted that the above prior art solutions have drawbacks, which are obtained by the inventor after practice and careful study, and thus the discovery process of the above problems and the solutions to the problems provided by the embodiments of the present application below should be all contributions of the inventor to the application during the inventive process, and should not be construed as technical matters known to those skilled in the art.
In view of this, the present embodiment provides a heading angle correction method. In the method, the electronic equipment measures the environmental information outside the vehicle through an ultrasonic sensor; acquiring a first course angle of the vehicle at the current moment according to the environmental information; measuring the motion state information of the vehicle by a motion state sensor; obtaining a second course angle of the vehicle at the current moment according to the motion state information; and fusing the first course angle and the second course angle to obtain the current target course angle of the vehicle. Because the ultrasonic sensor and the motion state sensor are low in price and can adapt to various scenes, the stability and the accuracy of the course angle measuring result can be improved after the ultrasonic sensor and the motion state sensor are combined.
The electronic device for implementing the course angle correction method can be embedded device designed for course measurement, and can also be control device for intelligent driving of the vehicle.
In order to make the solution provided by this embodiment clearer, it is assumed that the electronic device is a control device responsible for intelligent driving of the vehicle, and the steps of the method will be explained in detail with reference to fig. 1. It should be understood that the operations of the flow diagrams may be performed out of order and that steps that have no logical context may be performed in reverse order or concurrently. Moreover, one or more other operations may be added to or removed from the flow diagrams by those skilled in the art under the direction of the present disclosure. As shown in fig. 1, the method includes:
S101, environmental information outside the vehicle is measured by an ultrasonic sensor.
The number of ultrasonic sensors in the present embodiment may be plural, for example, 12 ultrasonic sensors are provided around the vehicle. The control device senses an obstacle around the vehicle through the plurality of ultrasonic sensors that are configured. As shown in fig. 2, the distance measurement principle of the ultrasonic sensor is that the sensor emits a beam of ultrasonic pulses, and when the beam of ultrasonic waves encounters an obstacle 101, the beam of ultrasonic waves is reflected back by the obstacle 101. The sensor receives the reflected ultrasonic waves and calculates the distance of the obstacle 101 from the ultrasonic sensor by measuring the round trip time of the ultrasonic waves and the propagation speed of sound in the air.
Based on the distance measurement principle of ultrasonic waves, the environment information comprises two sets of space coordinates obtained by measuring a plurality of characteristic points twice successively during the period from the last measurement moment to the current moment of the vehicle, and the plurality of characteristic points represent a plurality of positions of the surface of the obstacle in the environment. This requires converting the distance between the feature point and the vehicle into spatial coordinates. In this regard, an alternative embodiment of step S102 includes:
s102-1, measuring at least two actual measurement distances between the characteristic points and the vehicle by the ultrasonic sensor each time for each characteristic point.
S102-2, determining the space coordinates of the feature points through a triangular positioning algorithm according to at least two measured distances.
For example, autonomous parking is taken as an example. As shown in fig. 3, the vehicle 103 is in two positions during the movement of the automatic parkingLocation,/>Position) of the obstacle 101 by transmitting ultrasonic waves 102 from the same ultrasonic sensor, and measuring the distances between the vehicle 103 and the characteristic points on the surface of the obstacle 101 at different positions based on the reflected signals generated by the characteristic points, which are expressed as/>, respectively、/>. Then, the distance/>, between the two positions of the vehicle 103 at the time of transmitting the ultrasonic wave is obtained according to the following vehicle movement distance formula：
；
In the method, in the process of the invention,Representing the initial speed of the vehicle,/>The expression slave/>Position movement to/>Time of location,/>Indicating the acceleration of the vehicle. As shown in fig. 4, which shows the distance/>, between the vehicle 103 and the characteristic point 1011 at different positions、And distance between two positions/>The geometric relationship in the coordinate system, so that the spatial coordinates of the feature point 1011 can be calculated by a triangulation algorithm according to the illustrated geometric relationship. Assumption/>The coordinates of the location in the global vehicle-based coordinate system are/>，/>The coordinates of the location in the global coordinate system are/>The coordinates of the obstacle in the global coordinate system are/>. Based on the collective relationship between these coordinates, the coordinates of the obstacle in the global coordinate system can be calculated as/>, by the expression：
；
；
。
Since the vehicle is provided with a plurality of ultrasonic sensors, the vehicle is driven byPosition movement to/>The position may measure the spatial coordinates of a plurality of feature points as a set of spatial coordinates. Similarly, repeating the above measurement procedure may yield another set of spatial coordinates.
It can be understood that the positions of the feature points in the physical space do not actually change, and the reason why the two sets of space coordinates are inconsistent is based on the change of the coordinate system of the vehicle, and the change of the positions between the coordinate systems exactly reflects the heading angle of the vehicle. In view of this, the two sets of spatial coordinates are clustered respectively, so as to determine the correspondence between the two sets of spatial coordinates. For example, a K-means clustering algorithm may be selected, and the spatial coordinates of the feature points are used as input data and divided into three clusters, which respectively correspond to the arc, the corner, and the head in the parking scene. For each cluster, its centroid may be calculated and then matched with the centroid of the cluster in another set of spatial coordinates, thereby establishing a correspondence between the two sets of spatial coordinates. For example, two feature points belonging to the corner have a correspondence relationship in the two sets of spatial coordinates.
Based on the description of the environmental information by the above implementation, with continued reference to fig. 1, the heading angle correction method provided in this embodiment further includes:
s102, according to the environment information, a first course angle of the current moment of the vehicle is obtained.
As a scientific research implementation mode, the control device can fit a space mapping matrix between two sets of space coordinates according to the two sets of space coordinates; and obtaining a first course angle of the current moment of the vehicle according to the space mapping matrix. In a specific embodiment, the control device may fit a spatial mapping matrix between two sets of spatial coordinates by using a least square method according to the two sets of spatial coordinates, where the spatial mapping matrix is capable of converting one set of spatial coordinates in the two sets of spatial coordinates into mapped spatial coordinates, and a difference between the mapped spatial coordinates and another set of spatial coordinates is the smallest. It can be understood that the spatial mapping matrix is used to perform matrix operation on one set of spatial coordinates, and the operation result is another set of spatial coordinates.
Illustratively, to obtain the change in the angle of two adjacent frames by feature point registration, a least squares method is generally used. In this method, the objective is to minimize the sum of squares of residuals, i.e. the sum of squares of errors between the spatial coordinates of one set of feature points and the other set of spatial coordinates after transformation by a spatial mapping matrix. Specifically, it is assumed that two sets of spatial coordinates are respectively expressed asAnd/>Wherein/>Representing the number of feature points, the spatial mapping matrix/>, of two sets of feature point times, can be calculated by the following formula：
；
In the method, in the process of the invention,Representing the two norms of the vector. While the minimization of the upper expression may be solved by matrix operations. In the course of the operation, the/>And/>Respectively expressed as/>In which,/>AndTransform matrix/>Can be expressed as/>At this time, the above formula may be converted into the following form:
；
In the method, in the process of the invention, The Frobenius norm of a matrix, i.e. the square root of the sum of the squares of the matrix elements, is represented. And determining the first course angle of the vehicle through the space mapping matrix.
With reference to the description of the first heading angle in the above embodiment, with continued reference to fig. 1, the method for providing more heading angle correction according to this embodiment further includes:
S103, the motion state information of the vehicle is detected by a motion state sensor.
S104, obtaining a second course angle of the current moment of the vehicle according to the motion state information.
In this embodiment, the motion sensor is used to measure the motion state of the vehicle itself. Wherein the motion state sensor includes at least one of a steering angle sensor and a gyroscope. The measured motion state information is the wheel rotation angle. Substituting the wheel rotation angle into the following expression to obtain a second course angle：
；
In the method, in the process of the invention,Representing the wheelbase of a vehicle,/>Representing the turning radius of the vehicle,/>Representing the track of the vehicle.
S105, fusing the first course angle and the second course angle to obtain the current target course angle of the vehicle.
It should be understood that in the related art, a fixed weight may be preset for the first heading angle and the second heading angle, and the weights of the first heading angle and the second heading angle may be used for weighted fusion. However, the course angle measured by the ultrasonic sensor has the characteristics of low frequency, no accumulated error and possibly a outlier error value (the error is extremely large), and the course angle measured by the vehicle motion state sensor has the characteristics of high frequency, accumulated error and no outlier error value; therefore, the fixed weights cannot accommodate the precision variations of the ultrasonic sensor and the motion state sensor. Theoretically, based on the change in the error of the motion state sensor, when the error is small, it is more prone to trust the heading angle measured by the motion state sensor, and conversely, it is more prone to trust the heading angle measured by the motion state sensor. Therefore, in the optional implementation of step S105, the first heading angle and the second heading angle are fused by means of linear filtering, and specific embodiments include:
s105-1, acquiring a first error between the first course angle and the second course angle.
S105-2, obtaining a more accurate second error through a linear filtering algorithm according to the first course angle, the second course angle and the first error.
In an alternative embodiment, the control device uses the first course angle as an observation value required by the linear filtering algorithm; taking the second course angle as a predicted value required by a linear filtering algorithm; taking the first error as one of the state vectors required by the linear filtering algorithm; according to the observed value, the predicted value and the state vector, obtaining an updated state vector after linear filtering; from the updated state vector, a more accurate second error is obtained.
Wherein the linear filtering algorithm may be a kalman filtering algorithm. The kalman filtering algorithm itself is the prior art, and details of implementation of this embodiment are not described herein. However, the method is different from the conventional Kalman filtering algorithm in calculation process, and the prediction state of the predicted object is obtained through a state transition matrix in the conventional Kalman filtering algorithm. With respect to the state transition matrix, it should be understood that the state transition matrix describes how the system state transitions to the next time at one time, which results in the state vector at the next time from the state vector at the current time by linear transformation. The state transition matrix can be used for predicting the state of the next moment by using the state of the current moment, so that the prediction of the predicted object is realized. In contrast, in the present embodiment, the second heading angle measured by the motion state sensor is used as a predicted value, and the updated state vector is obtained by using the modified state equation in the kalman filtering algorithm.
Conventional kalman filtering algorithms typically obtain filtered sensor measurements from updated state vectors, as the updated state vectors filter various noise in the measurements. In contrast, in the present embodiment, a more accurate second error is obtained from the updated state vector, and the second heading angle of the motion state sensor is corrected by using the second error. Namely, step S105 further includes:
S105-3, correcting the second course angle by using the second error to obtain the target course angle.
Therefore, through the implementation mode, the course angle measured by the sound wave sensor which is low in cost and can adapt to various scenes and the course angle measured by the motion state sensor are fused, so that the precision after fusion is higher than the measurement precision when any one sensor is independently used, and the advantage complementation is formed between the two sensors.
Based on the same inventive concept as the course angle correction method provided in the present embodiment, the present embodiment also provides a course angle correction device including at least one software function module that can be stored in a memory 301 or cured in an electronic device in a software form. A processor 302 in the electronic device is used to execute executable modules stored in the memory 301. For example, a software function module included in the heading angle correction device, a computer program, and the like. Referring to fig. 5, functionally divided, the apparatus may include:
A first heading module 201 for measuring environmental information outside the vehicle through an ultrasonic sensor; acquiring a first course angle of the vehicle at the current moment according to the environmental information;
a second heading module 202 for measuring motion state information of the vehicle itself through a motion state sensor; obtaining a second course angle of the vehicle at the current moment according to the motion state information;
And the course correction module 203 is configured to fuse the first course angle with the second course angle to obtain a current target course angle of the vehicle.
In the present embodiment, the first heading module 201 is used to implement steps S101 and S102 in fig. 1, the second heading module 202 is used to implement steps S103 and S104 in fig. 1, and the heading correction module 203 is used to implement step S105 in fig. 1. Therefore, the detailed description of each module may refer to the specific implementation manner of the corresponding step, and this embodiment will not be repeated.
In addition, functional modules in the embodiments of the present application may be integrated together to form a single part, or each module may exist alone, or two or more modules may be integrated to form a single part.
It should also be appreciated that the above embodiments, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored on a computer readable storage medium. Based on this understanding, the technical solution of the present application may be embodied essentially or in a part contributing to the prior art or in a part of the technical solution, in the form of a software product stored in a storage medium, comprising several instructions for causing a computer device (which may be a personal computer, a server, a network device, etc.) to perform all or part of the steps of the method according to the embodiments of the present application.
Accordingly, the present embodiment also provides a storage medium storing a computer program which, when executed by a processor, implements the heading angle correction method provided by the present embodiment. The storage medium may be a usb disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, etc. which may store the program code.
The embodiment provides an electronic device for implementing the course angle correction method. As shown in fig. 6, the electronic device may include a processor 302 and a memory 301. The memory 301 stores a computer program, and the processor reads and executes the computer program corresponding to the above embodiment in the memory 301 to realize the heading angle correction method provided in the present embodiment.
With continued reference to fig. 6, the electronic device further comprises a communication unit 303. The memory 301, the processor 302 and the communication unit 303 are electrically connected to each other directly or indirectly through a system bus 304 to realize data transmission or interaction.
The memory 301 may be an information recording device based on any electronic, magnetic, optical or other physical principle for recording execution instructions, data, etc. In some embodiments, the memory 301 may be, but is not limited to, volatile memory, non-volatile memory, storage drives, and the like.
In some embodiments, the volatile memory may be a random access memory (Random Access Memory, RAM); in some embodiments, the nonvolatile Memory may be Read Only Memory (ROM), programmable Read Only Memory (Programmable Read-Only Memory, PROM), erasable Read Only Memory (Erasable Programmable Read-Only Memory, EPROM), electrically erasable Read Only Memory (Electric Erasable Programmable Read-Only Memory, EEPROM), flash Memory, or the like; in some embodiments, the storage drive may be a magnetic disk drive, a solid state disk, any type of storage disk (e.g., optical disk, DVD, etc.), or a similar storage medium, or a combination thereof, etc.
The communication unit 303 is used for transmitting and receiving data through a network. In some embodiments, the network may include a wired network, a wireless network, a fiber optic network, a telecommunications network, an intranet, the internet, a local area network (Local Area Network, LAN), a wide area network (Wide Area Network, WAN), a wireless local area network (Wireless Local Area Networks, WLAN), a metropolitan area network (Metropolitan Area Network, MAN), a wide area network (Wide Area Network, WAN), a public switched telephone network (Public Switched Telephone Network, PSTN), a bluetooth network, a ZigBee network, a near field Communication (NEAR FIELD Communication, NFC) network, or the like, or any combination thereof. In some embodiments, the network may include one or more network access points. For example, the network may include wired or wireless network access points, such as base stations and/or network switching nodes, through which one or more components of the service request processing system may connect to the network to exchange data and/or information.
The processor 302 may be an integrated circuit chip with signal processing capabilities and may include one or more processing cores (e.g., a single-core processor or a multi-core processor). By way of example only, the Processor may include a central processing unit (Central Processing Unit, CPU), application SPECIFIC INTEGRATED Circuit (ASIC), special purpose instruction set Processor (Application Specific Instruction-set Processor, ASIP), graphics processing unit (Graphics Processing Unit, GPU), physical processing unit (Physics Processing Unit, PPU), digital signal Processor (DIGITAL SIGNAL Processor, DSP), field programmable gate array (Field Programmable GATE ARRAY, FPGA), programmable logic device (Programmable Logic Device, PLD), controller, microcontroller unit, reduced instruction set computer (Reduced Instruction Set Computing, RISC), microprocessor, or the like, or any combination thereof.
It will be appreciated that the structure shown in fig. 6 is merely illustrative. The electronic device may also have more or fewer components than shown in fig. 6, or have a different configuration than shown in fig. 6. The components shown in fig. 6 may be implemented in hardware, software, or a combination thereof.
It should be understood that the apparatus and method disclosed in the above embodiments may be implemented in other manners. The apparatus embodiments described above are merely illustrative, for example, of the flowcharts and block diagrams in the figures that illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods and computer program products according to various embodiments of the present application. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code, which comprises one or more executable instructions for implementing the specified logical function(s). It should also be noted that in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or the blocks may sometimes be executed in the reverse order, depending upon the functionality involved. It will also be noted that each block of the block diagrams and/or flowchart illustration, and combinations of blocks in the block diagrams and/or flowchart illustration, can be implemented by special purpose hardware-based systems which perform the specified functions or acts, or combinations of special purpose hardware and computer instructions.
The above description is merely illustrative of various embodiments of the present application, but the scope of the present application is not limited thereto, and any person skilled in the art can easily think about variations or substitutions within the scope of the present application, and the application is intended to be covered by the scope of the present application. Therefore, the protection scope of the present application shall be subject to the protection scope of the claims.
Claims (9)
1. A heading angle correction method, the method comprising:
measuring environment information outside a vehicle through an ultrasonic sensor, wherein the environment information comprises two sets of space coordinates obtained by measuring a plurality of characteristic points twice successively during the period from the last measurement moment to the current moment of the vehicle, and the characteristic points represent a plurality of positions of the surface of an obstacle in the environment;
Fitting a space mapping matrix between the two sets of space coordinates according to the two sets of space coordinates;
according to the space mapping matrix, obtaining a course change angle of the vehicle from the last measurement moment to the current moment;
determining a first course angle of the current moment of the vehicle according to the course change angle and the course angle of the last measuring moment;
Measuring the motion state information of the vehicle by a motion state sensor, wherein the motion state information is a wheel corner;
Obtaining a second course angle of the current moment of the vehicle according to the motion state information;
and fusing the first course angle and the second course angle to obtain the current target course angle of the vehicle.
2. The heading angle correction method as set forth in claim 1, wherein said fusing the first heading angle and the second heading angle to obtain a target heading angle of the vehicle includes:
acquiring a first error between the first course angle and the second course angle;
Obtaining a more accurate second error through a linear filtering algorithm according to the first course angle, the second course angle and the first error;
and correcting the second course angle by using the second error to obtain the target course angle.
3. The heading angle correction method as recited in claim 2, wherein the deriving the first heading angle, the second heading angle, and the first error by linear filtering to obtain a more accurate second error includes:
Taking the first course angle as an observation value required by the linear filtering algorithm;
taking the second course angle as a predicted value required by the linear filtering algorithm;
Taking the first error as one of the state vectors required by the linear filtering algorithm;
according to the observed value, the predicted value and the state vector, obtaining an updated state vector after linear filtering;
and obtaining a more accurate second error from the updated state vector.
4. The heading angle correction method as recited in claim 1, wherein said fitting a spatial mapping matrix between the two sets of spatial coordinates according to the two sets of spatial coordinates comprises:
and fitting a space mapping matrix between the two sets of space coordinates by a least square method according to the two sets of space coordinates.
5. The heading angle correction method as recited in claim 4, wherein the spatial mapping matrix is capable of converting one set of spatial coordinates of the two sets of spatial coordinates to mapped spatial coordinates, and wherein a difference between the mapped spatial coordinates and the other set of spatial coordinates is minimal.
6. The heading angle correction method according to claim 1, wherein the environmental information includes two sets of spatial coordinates obtained by measuring a plurality of feature points twice in succession during a period from a last measurement time to a current time of the vehicle, the plurality of feature points representing a plurality of positions located on a surface of the obstacle, the environmental information outside the vehicle being measured by the ultrasonic sensor, comprising:
for each of the feature points, measuring at least two measured distances between the feature point and the vehicle by the ultrasonic sensor each time a measurement is made;
and determining the space coordinates of the feature points through a triangular positioning algorithm according to the at least two actually measured distances.
7. A heading angle correction apparatus, characterized in that the apparatus comprises:
The system comprises a first course module, a second course module and a first control module, wherein the first course module is used for measuring environment information outside a vehicle through an ultrasonic sensor, the environment information comprises two sets of space coordinates obtained by measuring a plurality of characteristic points twice successively during the period from the last measurement moment to the current moment of the vehicle, and the plurality of characteristic points represent a plurality of positions of the surface of an obstacle in the environment;
The first course module is further used for fitting out a space mapping matrix between the two sets of space coordinates according to the two sets of space coordinates; according to the space mapping matrix, obtaining a course change angle of the vehicle from the last measurement moment to the current moment; determining a first course angle of the current moment of the vehicle according to the course change angle and the course angle of the last measuring moment;
The second course module is used for measuring the motion state information of the vehicle by the motion state sensor; obtaining a second course angle of the current moment of the vehicle according to the motion state information, wherein the motion state information is a wheel corner;
and the course correction module is used for fusing the first course angle and the second course angle to obtain the current target course angle of the vehicle.
8. A storage medium storing a computer program which, when executed by a processor, implements the heading angle correction method of any one of claims 1-6.
9. An electronic device comprising a processor and a memory, the memory storing a computer program which, when executed by the processor, implements the heading angle correction method of any one of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410238825.0A CN117818754B (en) | 2024-03-04 | 2024-03-04 | Course angle correction method and device, storage medium and electronic equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410238825.0A CN117818754B (en) | 2024-03-04 | 2024-03-04 | Course angle correction method and device, storage medium and electronic equipment |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117818754A CN117818754A (en) | 2024-04-05 |
| CN117818754B true CN117818754B (en) | 2024-05-28 |
Family
ID=90510099
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410238825.0A Active CN117818754B (en) | 2024-03-04 | 2024-03-04 | Course angle correction method and device, storage medium and electronic equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117818754B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113008244A (en) * | 2021-02-25 | 2021-06-22 | 广州导远电子科技有限公司 | Navigation information processing method and device, electronic equipment and storage medium |
| CN113391336A (en) * | 2021-06-17 | 2021-09-14 | 上海联适导航技术股份有限公司 | Course angle detection method, device, equipment and readable storage medium |
| CN115824229A (en) * | 2022-12-29 | 2023-03-21 | 苏州万集车联网技术有限公司 | Course angle correction method, device, equipment, storage medium and program product |
| CN115979288A (en) * | 2022-12-27 | 2023-04-18 | 浙江零跑科技股份有限公司 | Course angle determining method, electronic equipment and storage medium |
| CN116358603A (en) * | 2023-03-31 | 2023-06-30 | 深圳市正浩创新科技股份有限公司 | Course angle correction method, self-mobile device and storage medium |
| KR102618247B1 (en) * | 2023-08-03 | 2023-12-28 | (주)에스유엠 | Device for correcting localization heading error in autonomous car and operating methdo thereof |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111780756A (en) * | 2020-07-20 | 2020-10-16 | 北京百度网讯科技有限公司 | Vehicle dead reckoning method, device, equipment and storage medium |
-
2024
- 2024-03-04 CN CN202410238825.0A patent/CN117818754B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113008244A (en) * | 2021-02-25 | 2021-06-22 | 广州导远电子科技有限公司 | Navigation information processing method and device, electronic equipment and storage medium |
| CN113391336A (en) * | 2021-06-17 | 2021-09-14 | 上海联适导航技术股份有限公司 | Course angle detection method, device, equipment and readable storage medium |
| CN115979288A (en) * | 2022-12-27 | 2023-04-18 | 浙江零跑科技股份有限公司 | Course angle determining method, electronic equipment and storage medium |
| CN115824229A (en) * | 2022-12-29 | 2023-03-21 | 苏州万集车联网技术有限公司 | Course angle correction method, device, equipment, storage medium and program product |
| CN116358603A (en) * | 2023-03-31 | 2023-06-30 | 深圳市正浩创新科技股份有限公司 | Course angle correction method, self-mobile device and storage medium |
| KR102618247B1 (en) * | 2023-08-03 | 2023-12-28 | (주)에스유엠 | Device for correcting localization heading error in autonomous car and operating methdo thereof |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117818754A (en) | 2024-04-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR102463176B1 (en) | Device and method to estimate position | |
| KR101625486B1 (en) | Map-based positioning system and method thereof | |
| WO2022127532A1 (en) | Method and apparatus for calibrating external parameter of laser radar and imu, and device | |
| EP2133662B1 (en) | Methods and system of navigation using terrain features | |
| JP6354556B2 (en) | POSITION ESTIMATION DEVICE, POSITION ESTIMATION METHOD, POSITION ESTIMATION PROGRAM | |
| KR102627453B1 (en) | Method and device to estimate position | |
| CN111427061A (en) | Robot mapping method and device, robot and storage medium | |
| CN113933818A (en) | Method, device, storage medium and program product for calibrating laser radar external parameter | |
| CN111308415B (en) | Online pose estimation method and equipment based on time delay | |
| WO2020184013A1 (en) | Vehicle control device | |
| CN114111775B (en) | Multi-sensor fusion positioning method and device, storage medium and electronic equipment | |
| KR20180038154A (en) | Method for vehicle pose estimation using LiDAR | |
| CN115494533A (en) | Vehicle positioning method, device, storage medium and positioning system | |
| JPWO2020202522A1 (en) | Vehicle positioning device | |
| CN115236714A (en) | Multi-source data fusion positioning method, device and equipment and computer storage medium | |
| CN116399324A (en) | Picture construction method and device, controller and unmanned vehicle | |
| CN115290071A (en) | Relative positioning fusion method, device, equipment and storage medium | |
| CN112184906B (en) | Method and device for constructing three-dimensional model | |
| CN113405555A (en) | Automatic driving positioning sensing method, system and device | |
| WO2024001649A1 (en) | Robot positioning method, apparatus and computing readable storage medium | |
| CN117818754B (en) | Course angle correction method and device, storage medium and electronic equipment | |
| CN117268408A (en) | Laser slam positioning method and system | |
| KR102506411B1 (en) | Method and apparatus for estimation of location and pose on vehicle and record medium for this | |
| CN113495281B (en) | Real-time positioning method and device for movable platform | |
| CN112965076B (en) | Multi-radar positioning system and method for robot |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |