CN113865586B - Installation angle estimation method and device and automatic driving system - Google Patents
Installation angle estimation method and device and automatic driving system Download PDFInfo
- Publication number
- CN113865586B CN113865586B CN202111148161.1A CN202111148161A CN113865586B CN 113865586 B CN113865586 B CN 113865586B CN 202111148161 A CN202111148161 A CN 202111148161A CN 113865586 B CN113865586 B CN 113865586B
- Authority
- CN
- China
- Prior art keywords
- angle
- coordinate system
- predicted
- target
- quaternion
- 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
- 238000009434 installation Methods 0.000 title claims abstract description 101
- 238000000034 method Methods 0.000 title claims abstract description 47
- 239000011159 matrix material Substances 0.000 claims description 47
- 230000006870 function Effects 0.000 claims description 19
- 238000006243 chemical reaction Methods 0.000 claims description 12
- 230000021615 conjugation Effects 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 5
- 238000012546 transfer Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 abstract description 13
- 230000009977 dual effect Effects 0.000 description 15
- 238000004891 communication Methods 0.000 description 5
- 238000001914 filtration Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000000802 evaporation-induced self-assembly Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000002620 method output Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/166—Mechanical, construction or arrangement details of inertial navigation systems
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Abstract
The invention provides a method, a device and an automatic driving system for estimating an installation angle, which are used for acquiring a first observation angle of a double antenna of a navigation device in a first coordinate system corresponding to the navigation device, a first prediction angle of a target vehicle in the first coordinate system and a first calibration angle of the double antenna in a second coordinate system corresponding to the target vehicle; determining a second predicted angle of the dual-antenna in a first coordinate system according to the first predicted angle and the first calibration angle; based on the first observation angle and the second prediction angle, a target installation angle of the measuring device in a second coordinate system is estimated. In the mode, the observation angle and the prediction angle of the double antennas under the coordinate system corresponding to the navigation device are utilized to estimate the installation angle of the measurement device under the coordinate system corresponding to the target vehicle, so that the problem of inaccurate positioning caused by inaccurate calibration of the installation angle is avoided, the accuracy of the installation angle is improved, and the positioning accuracy of the vehicle is further improved.
Description
Technical Field
The invention relates to the technical field of multi-information fusion, in particular to a method and a device for estimating an installation angle and an automatic driving system.
Background
In vehicle autopilot, a positioning module is generally included, through which observations of an IMU (Inertial Measurement Unit, measurement unit), GNSS (Global Navigation Satellite System ) are filtered, and a final state of the vehicle, i.e., a state of an IMU center, is output. The state of the center of the IMU needs to be converted through the mounting angle of the IMU, and the mounting angle is the attitude of the IMU under the vehicle coordinate system, so that the accuracy of the mounting angle directly influences the accuracy of the final state of the vehicle output by the positioning module. In the related art, a calibration mode is generally adopted to obtain the installation angle, but the installation angle obtained through the calibration mode has a certain error within about 1 degree, the accuracy of determining the installation angle is low, and for a scene needing high-precision positioning, the mode cannot meet the requirement, so that the positioning state precision of the vehicle is low.
Disclosure of Invention
Accordingly, the present invention is directed to a method and apparatus for estimating an installation angle and an automatic driving system, so as to improve accuracy of the installation angle and further improve accuracy of vehicle positioning.
In a first aspect, an embodiment of the present invention provides a method for estimating an installation angle, where the method is applied to an autopilot system of a target vehicle; the automatic driving system comprises a measuring device and a navigation device; the method comprises the following steps: acquiring a first observation angle of a double antenna of the navigation device in a first coordinate system corresponding to the navigation device, a first prediction angle of a target vehicle in the first coordinate system, and a first calibration angle of the double antenna in a second coordinate system corresponding to the target vehicle; determining a second predicted angle of the dual-antenna in a first coordinate system according to the first predicted angle and the first calibration angle; based on the first observation angle and the second prediction angle, a target installation angle of the measuring device in a second coordinate system is estimated.
Further, the step of determining a second predicted angle of the dual antenna in the first coordinate system according to the first predicted angle and the first calibration angle includes: calculating the product value of the quaternion of the first prediction angle and the quaternion of the first calibration angle to obtain a quaternion which is the second prediction angle; and converting the quaternion of the second predicted angle into the second predicted angle according to the target conversion function.
Further, the step of estimating the target installation angle of the measuring device in the second coordinate system based on the first observation angle and the second prediction angle includes: calculating the product value of the conjugate of the quaternion of the second predicted angle and the quaternion of the first observation angle, and converting the product value into angle data of the product value according to the target conversion function; obtaining a target equation of subtracting the first observation angle from the second prediction angle according to the angle data; based on the target equation, a target installation angle of the measuring device in the second coordinate system is estimated.
Further, the step of estimating the target installation angle of the measuring device in the second coordinate system based on the target equation includes: determining an error estimation equation of the automatic driving system according to the target equation; the error estimation equation comprises an error estimation value of the target installation angle; and extracting an error estimated value of the target installation angle from the error estimated equation, and calculating a product value of a quaternion of the error estimated value and a quaternion of a third predicted angle of the measuring device in the second coordinate system to obtain the target installation angle.
Further, the objective equation is: wherein (1)>For the second prediction angle, +>For the second predicted pitch angle, +.>For the second predicted roll angle, +.>For a second predicted yaw angle; />For the first observation angle, +>For the first observation pitch angle, +.>For the first observation roll angle, +.>For a first observed yaw angle; quaterniodToAttitude is a target conversion function that converts quaternions into angles; />Quaternion for the first predicted angle; />The quaternion is the quaternion of the first calibration angle; />A quaternion for a first observation angle; t represents data conjugation; />Is a first order Jacobian matrix, wherein I 3×3 Representing a 3 x 3 dimensional identity matrix; 0 3×12 Zero matrix representing 3 x 12 dimensions, +.>Quaternion representing a second calibration angle of the target vehicle in the first coordinate system>A transformed rotation matrix; δx represents a state error; v denotes the observation error vector.
Further, the error estimation equation is:wherein (1)>Representing an error estimate of the autopilot system; />Is a second predicted angle; />Is a first observation angle; k=ph T (HPH T +R) -1 Wherein P is the error vector variance matrix of the state of the automatic driving system during prediction; />Is a first order Jacobian matrix, I 3×3 Representing a 3 x 3 dimensional identity matrix; 0 3×12 Zero matrix representing 3 x 12 dimensions, +.>Quaternion representing a second calibration angle of the target vehicle in the first coordinate system>A transformed rotation matrix; r is a variance matrix of the first observation angle; t represents the matrix transpose.
Further, after the step of extracting the error estimate of the target mounting angle from the error estimate equation, the method further includes: determining a second calibration angle of the measuring device in a second coordinate system as an initial third prediction angle; or determining the third predicted angle of the measuring device in the second coordinate system after the last correction as the third predicted angle of the measuring device in the second coordinate system.
In a second aspect, an embodiment of the present invention provides an apparatus for estimating an installation angle, the apparatus being provided in an automated driving system of a target vehicle; the automatic driving system comprises a measuring device and a navigation device; the device comprises: the acquisition module is used for acquiring a first observation angle of the double antennas of the navigation device in a first coordinate system corresponding to the navigation device, a first prediction angle of the target vehicle in the first coordinate system and a first calibration angle of the double antennas in a second coordinate system corresponding to the target vehicle; the determining module is used for determining a second predicted angle of the double antenna in the first coordinate system according to the first predicted angle and the first calibration angle; and the estimation module is used for estimating the target installation angle of the measuring device in the second coordinate system based on the first observation angle and the second prediction angle.
In a third aspect, an embodiment of the present invention provides an autopilot system including a processor and a memory, the memory storing machine executable instructions executable by the processor, the processor executing the machine executable instructions to implement the method of estimating the installation angle of any one of the first aspects.
In a fourth aspect, embodiments of the present invention provide a machine-readable storage medium storing machine-executable instructions that, when invoked and executed by a processor, cause the processor to implement a method of estimating an installation angle of any of the first aspects.
The embodiment of the invention has the following beneficial effects:
the invention provides a method, a device and an automatic driving system for estimating an installation angle, which are used for acquiring a first observation angle of a double antenna of a navigation device in a first coordinate system corresponding to the navigation device, a first prediction angle of a target vehicle in the first coordinate system and a first calibration angle of the double antenna in a second coordinate system corresponding to the target vehicle; determining a second predicted angle of the dual-antenna in a first coordinate system according to the first predicted angle and the first calibration angle; based on the first observation angle and the second prediction angle, a target installation angle of the measuring device in a second coordinate system is estimated. In the mode, the observation angle and the prediction angle of the double antennas under the coordinate system corresponding to the navigation device are utilized to estimate the installation angle of the measurement device under the coordinate system corresponding to the target vehicle, so that the problem of inaccurate positioning caused by inaccurate calibration of the installation angle is avoided, the accuracy of the installation angle is improved, and the positioning accuracy of the vehicle is further improved.
Additional features and advantages of the invention will be set forth in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. The objectives and other advantages of the invention will be realized and attained by the structure particularly pointed out in the written description and claims hereof as well as the appended drawings.
In order to make the above objects, features and advantages of the present invention more comprehensible, preferred embodiments accompanied with figures are described in detail below.
Drawings
In order to more clearly illustrate the embodiments of the invention or the technical solutions in the prior art, the drawings that are needed in the description of the embodiments or the prior art will be briefly described, it being obvious that the drawings in the description below are some embodiments of the invention and that other drawings may be obtained from these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a flowchart of a method for estimating an installation angle according to an embodiment of the present invention;
FIG. 2 is a flowchart of another method for estimating an installation angle according to an embodiment of the present invention;
fig. 3 is a schematic structural diagram of an installation angle estimation device according to an embodiment of the present invention;
fig. 4 is a schematic structural diagram of an automated driving system according to an embodiment of the present invention.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments. 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.
The installation angle of the inertial measurement device in the existing automatic driving system is generally obtained in a calibration mode, but the calibrated angle precision is within about 1 DEG, and the angle precision cannot meet the requirement for high-precision positioning, so that the accuracy of the installation angle can be accurately estimated, and the accuracy of the output positioning state of the positioning filter can be effectively improved. Based on the above, the method and the device for estimating the installation angle and the automatic driving system provided by the embodiment of the invention can be used for a vehicle with the automatic driving system.
For the convenience of understanding the present embodiment, first, a method for estimating an installation angle disclosed in the present embodiment is described in detail, and the method is applied to an automatic driving system of a target vehicle; the automatic driving system comprises a measuring device and a navigation device; the measurement device is usually an inertial measurement device, and the navigation device is usually a global navigation satellite device; as shown in fig. 1, the method comprises the steps of:
step S102, a first observation angle of a double antenna of a navigation device in a first coordinate system corresponding to the navigation device, a first prediction angle of a target vehicle in the first coordinate system, and a first calibration angle of the double antenna in a second coordinate system corresponding to the target vehicle are obtained;
in general, the first observation angle refers to the pose of the dual antenna of the navigation device in the navigation coordinate system; typical poses include roll, pitch and yaw. The first observation angle refers to an angle observation value of each frame of double antennas. The first observation angle may be expressed asWherein->Representing pitch angle, & lt & gt>Representing roll angle, & lt + & gt>Representing the yaw angle. Usually, the actual application gesture is not directly stored and calculated in the form of angles, and for the convenience of calculation, the first observation angle can be converted into four anglesThe number of elements may be determined by a first observation angle between the transfer functions attitudeToQuaterniod> Conversion to quaternion->And the quaternion of the first observation angle of the dual antenna dual in the first coordinate system n corresponding to the navigation device is represented.
The quaternion of the first predicted angle of the target vehicle in the first coordinate system can be expressed asThe quaternion of the first calibration angle of the dual antenna in the second coordinate system corresponding to the target vehicle may be expressed as +.>The dual antenna is rigidly calibrated during installation, so that the first calibration angle of the dual antenna in the second coordinate system corresponding to the target vehicle is a known value.
Step S104, determining a second predicted angle of the dual-antenna in a first coordinate system according to the first predicted angle and the first calibration angle;
specifically, the quaternion may be based on the first predicted angleAnd a quaternion of the first calibration angle +.>A second predicted angle of the dual antenna in the first coordinate system is calculated. The second predicted angle may be expressed as +.>
In additionThe second predicted angle of the dual antenna in the first coordinate system may be a quaternion based on a fourth predicted angle of the measuring device in the first coordinate systemFifth prediction angle of measuring device in second coordinate system +.>And a quaternion of the first calibration angle +.>And (5) determining. Wherein the quaternion of the fourth prediction angle +.>And a fifth prediction angle->Is the product of the conjugation of the first predicted angle, is the quaternion +.>
Step S106, estimating the target installation angle of the measuring device in the second coordinate system based on the first observation angle and the second prediction angle.
Quaternion which can be based on a first observation angleAnd the second predicted angle ++determined in step S104 above>Estimating the target mounting angle of the measuring device in the second coordinate system +.>I.e. the mounting angle of the measuring device in the coordinate system corresponding to the target vehicle.
Specifically, an error of a target installation angle of the measuring device in the second coordinate system can be determined according to an angle error between the first observation angle and the second prediction angle; and then determining the target installation angle of the measuring device in the second coordinate system according to the error of the target installation angle, namely the actual installation angle of the measuring device in the second coordinate system.
The invention provides an estimation method of an installation angle, which comprises the steps of obtaining a first observation angle of a double antenna of a navigation device in a first coordinate system corresponding to the navigation device, a first prediction angle of a target vehicle in the first coordinate system, and a first calibration angle of the double antenna in a second coordinate system corresponding to the target vehicle; determining a second predicted angle of the dual-antenna in a first coordinate system according to the first predicted angle and the first calibration angle; based on the first observation angle and the second prediction angle, a target installation angle of the measuring device in a second coordinate system is estimated. In the mode, the observation angle and the prediction angle of the double antennas under the coordinate system corresponding to the navigation device are utilized to estimate the installation angle of the measurement device under the coordinate system corresponding to the target vehicle, so that the problem of inaccurate positioning caused by inaccurate calibration of the installation angle is avoided, the accuracy of the installation angle is improved, and the positioning accuracy of the vehicle is further improved.
How the second predicted angle is determined is specifically described below, including: calculating the product value of the quaternion of the first prediction angle and the quaternion of the first calibration angle to obtain a quaternion which is the second prediction angle; and converting the quaternion of the second predicted angle into the second predicted angle according to the target conversion function.
The second predicted angle may be calculated specifically by:
wherein, quaternion toattitude represents the above objective transformation function, which is used to transform quaternion into a function of attitude angle.
The following describes in detail how to estimate the target installation angle of the measuring device in the second coordinate system, and specifically includes the following steps:
step 201, calculating the product value of the conjugate of the quaternion of the second predicted angle and the quaternion of the first observation angle, and converting the product value into angle data of the product value according to a target conversion function;
first calculating the product of the quaternion of the second predicted angle and the conjugate of the quaternion of the first observed angle, i.eWherein->Quaternion indicating the second predicted angle, < ->And conjugation of quaternion representing the first observation angle. The angle data may be expressed as
Step 202, obtaining a target equation of subtracting the first observation angle from the second prediction angle according to the angle data;
specifically, a target equation of subtracting the first observation angle from the second prediction angle is calculated according to the angle data; the target mode specifically comprises the following steps:
wherein,for the second prediction angle, +>For the second predicted pitch angle, +.>For the second predicted roll angle, +.>For a second predicted yaw angle; />For the first observation angle, +>For the first observation pitch angle, +.>For the first observation roll angle, +.>For a first observed yaw angle; quaterniodToAttitude is a target conversion function that converts quaternions into angles; />Quaternion for the first predicted angle; />The quaternion is the quaternion of the first calibration angle; />A quaternion for a first observation angle; t represents data conjugation; />Is a first order Jacobian matrix, wherein I 3×3 Representing a 3 x 3 dimensional identity matrix; 0 3×12 Zero matrix representing 3 x 12 dimensions, +.>Quaternion representing a second calibration angle of the target vehicle in the first coordinate system>Converted rotational momentAn array; δx represents a state error; v denotes the observation error vector.
The δx represents a state error, and generally includes an attitude angle error, a position error, a velocity error, an accelerometer zero bias, a gyroscope zero bias, an installation angle error, and the like; the above v represents an observation error vector of the first observation angle.
Step 203, estimating a target installation angle of the measuring device in the second coordinate system based on the target equation.
Specifically, an installation angle error is obtained from the target equation, and a target installation angle of the measuring device in the second coordinate system is estimated according to the installation angle error.
A preferred embodiment:
(1) Determining an error estimation equation of the automatic driving system according to the target equation; the error estimation equation comprises an error estimation value of the target installation angle;
(2) And extracting an error estimated value of the target installation angle from the error estimated equation, and calculating a product value of a quaternion of the error estimated value and a quaternion of a third predicted angle of the measuring device in the second coordinate system to obtain the target installation angle.
First from the objective equationIn the step (a), an error estimation equation is obtained through transformation, and the error estimation equation is as follows: />Wherein (1)>Representing an error estimate of the autopilot system; />Is a second predicted angle;is a first observation angle; k=ph T (HPH T +R) -1 Wherein P is the error vector variance matrix of the state of the automatic driving system during prediction; /> Is a first order Jacobian matrix, I 3×3 Representing a 3 x 3 dimensional identity matrix; 0 3×12 Zero matrix representing 3 x 12 dimensions, +.>Quaternion representing a second calibration angle of the target vehicle in the first coordinate system>A transformed rotation matrix; r is a variance matrix of the first observation angle; t represents the matrix transpose.
Then, an error estimation value of the target installation angle is extracted from the error estimation equationCalculating the quaternion of the error estimate>Calculating the product of the quaternion of the error estimate and the quaternion of the measuring device at a third angle of prediction in the second coordinate system, i.e.)>Obtaining the above target mounting angle->Specifically, the method can be expressed as: />
Further, the third predicted angle (i.e., the installation angle) of the measuring device in the second coordinate system is generally obtained by determining the second calibration angle of the measuring device in the second coordinate system as the initial third predicted angle; or determining the third predicted angle of the measuring device in the second coordinate system after the last correction as the third predicted angle of the measuring device in the second coordinate system.
Specifically, in an initial state, determining a second calibration angle of the measuring device in a second coordinate system as an initial third prediction angle; if the second calibration angle is corrected, the target installation angle after the last correction can be used as a third prediction angle after the system starts filtering.
In the mode, the observation value in the inertial navigation system is utilized, the integrity of system constraint is improved, the accuracy of the installation angle is greatly improved, and the accuracy of the positioning output state is improved. The problem that errors caused by inaccurate mounting angles are distributed to other state errors is avoided.
In addition, each effective double-antenna observation value participates in filtering, and besides the installation angle is estimated, the system state filtering function can be achieved. The installation angle estimated value is quickly converged under the condition of good observation value of the double antennas, and the accuracy of the installation angle state estimated by the final system can reach the accuracy level of 0.1 degrees.
Referring to another method of estimating the installation angle shown in fig. 2, the method outputs a positioning state of the vehicle based on an extended error state filter of the positioning, wherein the error state includes an attitude error, a position error, a speed error, an accelerometer zero bias, a gyroscope zero bias, and an installation angle error. Specifically, a measurement equation is constructed according to the angle observation value (corresponding to the first observation angle) of each frame of double antennas, then measurement and update are carried out by using the system prediction value (corresponding to the second prediction angle) to obtain an installation angle error estimation value, and finally installation angle update is carried out.
Firstly, constructing a measurement equation; the dual-antenna observation value is the gesture of the dual-antenna under the navigation coordinate system (corresponding to the first observation angle of the dual-antenna under the first coordinate system corresponding to the navigation device): roll angle phi, pitch angle theta, yaw angle phi. The observation equation is the direct observation of angle:further, the variance matrix of the observation angle is R. Wherein the gesture of the angular representation can be converted into a quaternion by the conversion function attitudeToQuaterniod>
The actual pose is then typically not stored directly in terms of angles, and is typically derived in quaternion storage.Quaternion representing the vehicle system (corresponding to the aforementioned target vehicle or the second coordinate system corresponding to the target vehicle) in the navigation coordinate system (corresponding to the aforementioned first coordinate system) can be predicted by the system to obtain +.>Representing the attitude of the IMU (corresponding to the measuring device) under the vehicle body system, the calibration value being regarded as the initial predicted value +.>After the system starts filtering, the installation angle after the last correction can be used as a predicted value; />Representing the quaternion of the dual antenna in the vehicle system (corresponding to the second coordinate system), the dual antenna is calibrated strictly during installation, and is a known value. The system predicts the pose of the dual antenna under the navigation system (corresponding to the aforementioned second predicted angle):
QuaterniodToAttitude represents a function that converts a quaternion into an attitude angle, and superscript T represents a quaternion conjugate. The measurement update equation (corresponding to the target equation described above) can then be expressed as:
I 3×3 representing a 3 x 3 dimensional array of units, 0 3×12 Representing a 3 x 12-dimensional zero matrix,representation->The transformed rotation matrix, v represents the observation error vector,/>Is a first order jacobian matrix.
Obtaining a state error estimate (corresponding to the error estimate equation) through the target equation:taking out the installation angle error estimated value +.>Can be converted into quaternion->Then updating to obtain the quaternion of the accurate target installation angle>
Corresponding to the method embodiment, the embodiment of the invention also provides an installation angle estimation device which is arranged in an automatic driving system of the target vehicle; the automatic driving system comprises a measuring device and a navigation device; as shown in fig. 3, the apparatus includes:
the obtaining module 31 is configured to obtain a first observation angle of the dual antenna of the navigation device in a first coordinate system corresponding to the navigation device, a first prediction angle of the target vehicle in the first coordinate system, and a first calibration angle of the dual antenna in a second coordinate system corresponding to the target vehicle;
a determining module 32, configured to determine a second predicted angle of the dual antenna in the first coordinate system according to the first predicted angle and the first calibration angle;
an estimating module 33, configured to estimate a target installation angle of the measuring device in the second coordinate system based on the first observation angle and the second prediction angle.
The invention provides an estimation device of an installation angle, which is used for acquiring a first observation angle of a double antenna of a navigation device in a first coordinate system corresponding to the navigation device, a first prediction angle of a target vehicle in the first coordinate system and a first calibration angle of the double antenna in a second coordinate system corresponding to the target vehicle; determining a second predicted angle of the dual-antenna in a first coordinate system according to the first predicted angle and the first calibration angle; based on the first observation angle and the second prediction angle, a target installation angle of the measuring device in a second coordinate system is estimated. In the mode, the observation angle and the prediction angle of the double antennas under the coordinate system corresponding to the navigation device are utilized to estimate the installation angle of the measurement device under the coordinate system corresponding to the target vehicle, so that the problem of inaccurate positioning caused by inaccurate calibration of the installation angle is avoided, the accuracy of the installation angle is improved, and the positioning accuracy of the vehicle is further improved.
Further, the determining module is further configured to: calculating the product value of the quaternion of the first prediction angle and the quaternion of the first calibration angle to obtain a quaternion which is the second prediction angle; and converting the quaternion of the second predicted angle into the second predicted angle according to the target conversion function.
Further, the estimation module is further configured to: calculating the product value of the conjugate of the quaternion of the second predicted angle and the quaternion of the first observation angle, and converting the product value into angle data of the product value according to the target conversion function; obtaining a target equation of subtracting the first observation angle from the second prediction angle according to the angle data; based on the target equation, a target installation angle of the measuring device in the second coordinate system is estimated.
Further, the estimation module is further configured to: determining an error estimation equation of the automatic driving system according to the target equation; the error estimation equation comprises an error estimation value of the target installation angle; and extracting an error estimated value of the target installation angle from the error estimated equation, and calculating a product value of a quaternion of the error estimated value and a quaternion of a third predicted angle of the measuring device in the second coordinate system to obtain the target installation angle.
Further, the objective equation is: wherein (1)>For the second prediction angle, +>For the second predicted pitch angle, +.>For the second predicted roll angle, +.>For a second predicted yaw angle; />For the first observation angle, +>For a first observation of the angle of elevation and depression,for the first observation roll angle, +.>For a first observed yaw angle; quaterniod ToAttitude is used for converting quaternionsA target transfer function for the angle; />Quaternion for the first predicted angle; />The quaternion is the quaternion of the first calibration angle; />A quaternion for a first observation angle; t represents data conjugation; />Is a first order Jacobian matrix, wherein I 3×3 Representing a 3 x 3 dimensional identity matrix; 0 3×12 Zero matrix representing 3 x 12 dimensions, +.>Quaternion representing a second calibration angle of the target vehicle in the first coordinate system>A transformed rotation matrix; δx represents a state error; v denotes the observation error vector.
Further, the error estimation equation is:wherein (1)>Representing an error estimate of the autopilot system; />Is a second predicted angle; />Is a first observation angle; k=ph T (HPH T +R) -1 Wherein P isThe state error vector variance matrix of the automatic driving system during prediction; />Is a first order Jacobian matrix, I 3×3 Representing a 3 x 3 dimensional identity matrix; 0 3×12 Zero matrix representing 3 x 12 dimensions, +.>Quaternion representing a second calibration angle of the target vehicle in the first coordinate system>A transformed rotation matrix; r is a variance matrix of the first observation angle; t represents the matrix transpose.
Further, the apparatus further includes a second determining module configured to: determining a second calibration angle of the measuring device in a second coordinate system as an initial third prediction angle; or determining the third predicted angle of the last corrected measuring device in the second coordinate system as the third predicted angle of the current measuring device in the second coordinate system.
The device for estimating the installation angle provided by the embodiment of the invention has the same technical characteristics as the method for estimating the installation angle provided by the embodiment, so that the same technical problems can be solved, and the same technical effects can be achieved.
The present embodiment also provides an autopilot system including a processor and a memory, the memory storing machine executable instructions executable by the processor, the processor executing the machine executable instructions to implement the above-described method of estimating an installation angle.
Referring to fig. 4, the autopilot system includes a processor 100 and a memory 101, the memory 101 storing machine executable instructions that can be executed by the processor 100, the processor 100 executing the machine executable instructions to implement the above-described method of estimating the installation angle.
Further, the autopilot system shown in fig. 4 further includes a bus 102 and a communication interface 103, the processor 100, the communication interface 103 and the memory 101 being connected by the bus 102.
The memory 101 may include a high-speed random access memory (RAM, random Access Memory), and may further include a non-volatile memory (non-volatile memory), such as at least one magnetic disk memory. The communication connection between the system network element and at least one other network element is implemented via at least one communication interface 103 (which may be wired or wireless), and may use the internet, a wide area network, a local network, a metropolitan area network, etc. Bus 102 may be an ISA bus, a PCI bus, an EISA bus, or the like. The buses may be classified as address buses, data buses, control buses, etc. For ease of illustration, only one bi-directional arrow is shown in FIG. 4, but not only one bus or type of bus.
The processor 100 may be an integrated circuit chip with signal processing capabilities. In implementation, the steps of the above method may be performed by integrated logic circuits of hardware in the processor 100 or by instructions in the form of software. The processor 100 may be a general-purpose processor, including a central processing unit (Central Processing Unit, CPU for short), a network processor (Network Processor, NP for short), etc.; but also digital signal processors (Digital Signal Processor, DSP for short), application specific integrated circuits (Application Specific Integrated Circuit, ASIC for short), field-programmable gate arrays (Field-Programmable Gate Array, FPGA for short) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components. The disclosed methods, steps, and logic blocks in the embodiments of the present invention may be implemented or performed. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. The steps of the method disclosed in connection with the embodiments of the present invention may be embodied directly in the execution of a hardware decoding processor, or in the execution of a combination of hardware and software modules in a decoding processor. The software modules may be located in a random access memory, flash memory, read only memory, programmable read only memory, or electrically erasable programmable memory, registers, etc. as well known in the art. The storage medium is located in the memory 101, and the processor 100 reads the information in the memory 101 and, in combination with its hardware, performs the steps of the method of the previous embodiment.
The embodiment of the invention also provides a machine-readable storage medium, which stores machine-executable instructions that, when being called and executed by a processor, cause the processor to implement the method for estimating the installation angle.
The method, apparatus and system for estimating an installation angle provided in the embodiments of the present invention include a computer readable storage medium storing program codes, where the instructions included in the program codes may be used to execute the method described in the foregoing method embodiment, and specific implementation may refer to the method embodiment and will not be repeated herein.
It will be clear to those skilled in the art that, for convenience and brevity of description, specific working procedures of the above-described system and apparatus may refer to corresponding procedures in the foregoing method embodiments, which are not described herein again.
In addition, in the description of embodiments of the present invention, unless explicitly stated and limited otherwise, the terms "mounted," "connected," and "connected" are to be construed broadly, and may be, for example, fixedly connected, detachably connected, or integrally connected; can be mechanically or electrically connected; can be directly connected or indirectly connected through an intermediate medium, and can be communication between two elements. The specific meaning of the above terms in the present invention will be understood by those skilled in the art in specific cases.
The functions, if implemented in the form of software functional units and sold or used as a stand-alone product, may be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention 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 invention. And the aforementioned storage medium includes: a U-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, or other various media capable of storing program codes.
In the description of the present invention, it should be noted that the directions or positional relationships indicated by the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc. are based on the directions or positional relationships shown in the drawings, are merely for convenience of describing the present invention and simplifying the description, and do not indicate or imply that the devices or elements referred to must have a specific orientation, be configured and operated in a specific orientation, and thus should not be construed as limiting the present invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance.
Finally, it should be noted that: the above examples are only specific embodiments of the present invention for illustrating the technical solution of the present invention, but not for limiting the scope of the present invention, and although the present invention has been described in detail with reference to the foregoing examples, it will be understood by those skilled in the art that the present invention is not limited thereto: any person skilled in the art may modify or easily conceive of the technical solution described in the foregoing embodiments, or perform equivalent substitution of some of the technical features, while remaining within the technical scope of the present disclosure; such modifications, changes or substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present invention, and are intended to be included in the scope of the present invention. Therefore, the protection scope of the invention is subject to the protection scope of the claims.
Claims (5)
1. A method of estimating an installation angle, the method being applied to an automated driving system of a target vehicle; the automatic driving system comprises a measuring device and a navigation device; the method comprises the following steps:
acquiring a first observation angle of a double antenna of the navigation device in a first coordinate system corresponding to the navigation device, a first prediction angle of the target vehicle in the first coordinate system, and a first calibration angle of the double antenna in a second coordinate system corresponding to the target vehicle;
determining a second predicted angle of the dual-antenna in the first coordinate system according to the first predicted angle and the first calibration angle;
estimating a target installation angle of the measuring device in the second coordinate system based on the first observation angle and the second prediction angle;
according to the first predicted angle and the first calibration angle, determining a second predicted angle of the dual-antenna in the first coordinate system comprises the following steps:
calculating the product value of the quaternion of the first prediction angle and the quaternion of the first calibration angle to obtain the quaternion of the second prediction angle;
converting the quaternion of the second predicted angle into the second predicted angle according to a target conversion function;
estimating a target installation angle of the measuring device in the second coordinate system based on the first observation angle and the second prediction angle, including:
calculating the product value of the conjugate of the quaternion of the second predicted angle and the quaternion of the first observation angle, and converting the product value into angle data of the product value according to a target conversion function;
obtaining a target equation of subtracting the first observation angle from the second prediction angle according to the angle data;
estimating a target installation angle of the measuring device in the second coordinate system based on the target equation;
estimating a target installation angle of the measuring device in the second coordinate system based on the target equation, including:
determining an error estimation equation of the automatic driving system according to the target equation; the error estimation equation comprises an error estimation value of the target installation angle;
extracting an error estimation value of the target installation angle from the error estimation equation, and calculating a product value of a quaternion of the error estimation value and a quaternion of a third prediction angle of the measuring device in the second coordinate system to obtain the target installation angle;
the objective equation is:
wherein,for the second prediction angle, +.>For the second predicted pitch angle, +.>For the second predicted roll angle, +.>For a second predicted yaw angle; />For the first observation angle, +.>For the first observation pitch angle, +.>For the first observation roll angle, +.>For a first observed yaw angle; />To convert quaternionA target transfer function for the angle;a quaternion for the first predicted angle; />A quaternion for the first calibration angle; />A quaternion for the first observation angle;Trepresenting data conjugation; />Is a first order Jacobian matrix, wherein +.>Representing a 3 x 3 dimensional identity matrix; />Zero matrix representing 3 x 12 dimensions, +.>Quaternion +.>A transformed rotation matrix; />Representing a state error; />Representing an observed error vector;
the error estimation equation is:
wherein,representing an error estimate of the autopilot system; />For the second predicted angle; />For the first observation angle; />Wherein, the method comprises the steps of, wherein,Pthe state error vector variance matrix of the automatic driving system is used for prediction;is a first order Jacobian matrix +.>Representing a 3 x 3 dimensional identity matrix; />Zero matrix representing 3 x 12 dimensions, +.>Quaternion +.>A transformed rotation matrix; />A variance matrix for a first observation angle;Trepresenting the matrix transpose.
2. The method of claim 1, wherein after the step of extracting the error estimate of the target mounting angle from the error estimate equation, the method further comprises:
determining a second calibration angle of the measuring device in the second coordinate system as an initial third prediction angle; or,
and determining the third predicted angle of the measuring device in the second coordinate system after the last correction as the third predicted angle of the measuring device in the second coordinate system.
3. An installation angle estimation device employing the installation angle estimation method according to claim 1, characterized in that the device is provided to an automated driving system of a target vehicle; the automatic driving system comprises a measuring device and a navigation device; the device comprises:
the acquisition module is used for acquiring a first observation angle of the double antenna of the navigation device in a first coordinate system corresponding to the navigation device, a first prediction angle of the target vehicle in the first coordinate system and a first calibration angle of the double antenna in a second coordinate system corresponding to the target vehicle;
the determining module is used for determining a second predicted angle of the dual-antenna in the first coordinate system according to the first predicted angle and the first calibration angle;
and the estimation module is used for estimating the target installation angle of the measuring device in the second coordinate system based on the first observation angle and the second prediction angle.
4. An autopilot system comprising a processor and a memory, the memory storing machine executable instructions executable by the processor, the processor executing the machine executable instructions to implement the method of estimating the mounting angle of any one of claims 1-2.
5. A machine-readable storage medium storing machine-executable instructions which, when invoked and executed by a processor, cause the processor to implement the method of estimating an installation angle of any of claims 1-2.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111148161.1A CN113865586B (en) | 2021-09-29 | 2021-09-29 | Installation angle estimation method and device and automatic driving system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111148161.1A CN113865586B (en) | 2021-09-29 | 2021-09-29 | Installation angle estimation method and device and automatic driving system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113865586A CN113865586A (en) | 2021-12-31 |
| CN113865586B true CN113865586B (en) | 2024-02-06 |
Family
ID=78992259
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111148161.1A Active CN113865586B (en) | 2021-09-29 | 2021-09-29 | Installation angle estimation method and device and automatic driving system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113865586B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115343730B (en) * | 2022-10-18 | 2023-02-17 | 苏州挚途科技有限公司 | GNSS antenna external parameter determination method, device and computer readable storage medium |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109945891A (en) * | 2017-12-21 | 2019-06-28 | 通用汽车环球科技运作有限责任公司 | System and method for the Inertial Measurement Unit being aligned in vehicle |
| CN111156994A (en) * | 2019-12-31 | 2020-05-15 | 西安航天华迅科技有限公司 | INS/DR & GNSS loose integrated navigation method based on MEMS inertial component |
| CN112146561A (en) * | 2020-09-09 | 2020-12-29 | 无锡卡尔曼导航技术有限公司 | Hall angle sensor installation angle offset estimation method |
| CN112362055A (en) * | 2020-12-01 | 2021-02-12 | 苏州挚途科技有限公司 | Attitude estimation method and device and electronic equipment |
| CN112781617A (en) * | 2020-12-28 | 2021-05-11 | 广州吉欧电子科技有限公司 | Error estimation method, integrated navigation processing system, and storage medium |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109059904A (en) * | 2018-06-01 | 2018-12-21 | 浙江亚特电器有限公司 | Combinated navigation method for mobile carrier |
-
2021
- 2021-09-29 CN CN202111148161.1A patent/CN113865586B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109945891A (en) * | 2017-12-21 | 2019-06-28 | 通用汽车环球科技运作有限责任公司 | System and method for the Inertial Measurement Unit being aligned in vehicle |
| CN111156994A (en) * | 2019-12-31 | 2020-05-15 | 西安航天华迅科技有限公司 | INS/DR & GNSS loose integrated navigation method based on MEMS inertial component |
| CN112146561A (en) * | 2020-09-09 | 2020-12-29 | 无锡卡尔曼导航技术有限公司 | Hall angle sensor installation angle offset estimation method |
| CN112362055A (en) * | 2020-12-01 | 2021-02-12 | 苏州挚途科技有限公司 | Attitude estimation method and device and electronic equipment |
| CN112781617A (en) * | 2020-12-28 | 2021-05-11 | 广州吉欧电子科技有限公司 | Error estimation method, integrated navigation processing system, and storage medium |
Non-Patent Citations (1)
| Title |
|---|
| 星图匹配/惯性组合导航技术研究;肖松;刘朝山;马涛;刘光斌;;光学技术(S1);286-288、290 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113865586A (en) | 2021-12-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113701745B (en) | External parameter change detection method, device, electronic equipment and detection system | |
| CN112835085B (en) | Method and device for determining vehicle position | |
| CN110440827B (en) | Parameter error calibration method and device and storage medium | |
| CN114136315B (en) | Monocular vision-based auxiliary inertial integrated navigation method and system | |
| CN115435817B (en) | MEMS inertial navigation installation error calibration method, storage medium and control computer | |
| CN113933818A (en) | Method, device, storage medium and program product for calibrating laser radar external parameter | |
| CN113865586B (en) | Installation angle estimation method and device and automatic driving system | |
| CN111026081A (en) | Error calculation method, device, equipment and storage medium | |
| CN111024067B (en) | Information processing method, device and equipment and computer storage medium | |
| CN114777768A (en) | High-precision positioning method and system for satellite rejection environment and electronic equipment | |
| CN112781617B (en) | Error estimation method, integrated navigation processing system and storage medium | |
| CN116242373A (en) | High-precision navigation positioning method and system for fusing multi-source data | |
| CN115856922A (en) | Loosely-coupled land combined navigation method and device, computer equipment and medium | |
| CN115950450A (en) | Calibration method and device of two-dimensional Doppler velocimeter, computer equipment and medium | |
| CN116222586A (en) | Fusion positioning method and device for automatic driving vehicle and electronic equipment | |
| CN115900705A (en) | Tightly-coupled land combined navigation method, device, computer equipment and medium | |
| CN114019954B (en) | Course installation angle calibration method, device, computer equipment and storage medium | |
| CN113959433B (en) | Combined navigation method and device | |
| CN115560744A (en) | Robot, multi-sensor-based three-dimensional mapping method and storage medium | |
| CN112835086B (en) | Method and device for determining vehicle position | |
| CN114322996B (en) | Pose optimization method and device of multi-sensor fusion positioning system | |
| CN113538699A (en) | Positioning method, device and equipment based on three-dimensional point cloud and storage medium | |
| CN113483762A (en) | Pose optimization method and device | |
| CN111678515A (en) | Device state estimation method and device, electronic device and storage medium | |
| CN116718153B (en) | Deformation monitoring method and system based on GNSS and INS |
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 | ||
| TA01 | Transfer of patent application right | ||
| TA01 | Transfer of patent application right |
Effective date of registration: 20220713 Address after: 215100 floors 11 and 12, Tiancheng Times Business Plaza, No. 58, qinglonggang Road, high speed railway new town, Xiangcheng District, Suzhou City, Jiangsu Province Applicant after: Suzhou Zhitu Technology Co.,Ltd. Applicant after: FAW JIEFANG AUTOMOTIVE Co.,Ltd. Address before: 215100 floors 11 and 12, Tiancheng Times Business Plaza, No. 58, qinglonggang Road, high speed railway new town, Xiangcheng District, Suzhou City, Jiangsu Province Applicant before: Suzhou Zhitu Technology Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |