Method and device for quickly estimating mounting angle of vehicle-mounted IMU (Inertial Measurement Unit)

Abstract

The invention relates to the technical field of integrated navigation systems, in particular to a method and a device for quickly estimating a mounting angle of a vehicle-mounted IMU (Inertial Measurement Unit). The method comprises the steps that original data of an IMU and observation data of navigation equipment are obtained when a moving vehicle moves linearly, and the original data comprise the angular velocity and the acceleration; performing error compensation on the original data to obtain a compensation angular velocity and a compensation acceleration, and obtaining first state information of the IMU according to the compensation angular velocity and the compensation acceleration; obtaining second state information of the vehicle through a Kalman filter according to the observation data; fusing the first state information and the second state information by using a Kalman filter to obtain a first speed vector of the IMU under the inertial navigation coordinate system and a second speed scalar of the IMU under the vehicle body coordinate system; and determining the mounting angle of the IMU according to a vehicle kinematics constraint condition met by the first velocity vector and the second velocity vector. According to the method, the vehicle-mounted IMU mounting angle can be estimated in real time, the implementation mode is simple, and engineering application and popularization are facilitated.

Description

Technical Field

[0001] The present invention relates to the technical field of integrated navigation systems, and in particular to a method and device for quickly estimating a vehicle-mounted IMU installation angle. Background Art

[0002] Vehicle-mounted LiDAR systems offer significant advantages in collecting 3D point cloud data due to their high precision and comprehensive information acquisition capabilities. The positioning and orientation components of these systems typically integrate the Global Navigation Satellite System (GNSS), the Inertial Navigation System (INS), and wheel speedometers. Under ideal conditions—a wide field of view and undisturbed GNSS signal interference—3D point cloud data can be collected with centimeter-level accuracy. However, due to the low position of the vehicle-mounted platform, it is susceptible to environmental factors (such as obstruction by trees, bridges, tunnels, or multipath effects), leading to GNSS signal loss and compromising the absolute accuracy of the point cloud data. In particular, when driving in long tunnels, GNSS lock can be lost for extended periods, causing the pure INS trajectory to rapidly diverge, resulting in distorted 3D point cloud data.

[0003] There are two main strategies for addressing poor GNSS data quality or signal loss in existing technologies: zero velocity UPdaTe (ZUPT) when the vehicle is stationary; and non-holonomic constraints (NHC) based on the vehicle's kinematics. ZUPT is suitable for error correction after a vehicle stops, while NHC suppresses inertial navigation data drift by exploiting the fact that the lateral velocity is zero when the vehicle is moving in a straight line. Specifically, constraint equations are established based on the fact that when the vehicle is moving in a straight line, only the forward velocity exists, while the left, right, and up and down velocities are zero. This effectively suppresses the divergence of pure inertial navigation. In comparison, ZUPT equations are simpler to establish, requiring only the vehicle to be stationary, while NHC needs to account for the slight mounting angle between the IMU and the vehicle.

[0004] However, the installation angle between the inertial navigation unit and the moving vehicle is small, making it difficult to measure directly. Existing methods assume a small installation angle, typically meaning that the installation deviation between the IMU and the vehicle is very small, close to 0 degrees. In this case, some simplifying assumptions can be used to perform kinematic calculations. However, this simplification also introduces a certain degree of error. To quantify and compensate for this error, an error model must be introduced and used to calculate the installation angle. This method is computationally complex.

[0005] In view of this, overcoming the defects of the prior art is an urgent problem to be solved in this technical field. Summary of the Invention

[0006] In response to the above defects or improvement needs of the existing technology, the present invention proposes a method and device for quickly estimating the installation angle of a vehicle-mounted IMU, which can estimate the installation angle of the vehicle-mounted IMU in real time, with a simple calculation formula and simple implementation.

[0007] The embodiment of the present invention adopts the following technical solutions: In a first aspect, the present invention provides a method for quickly estimating the installation angle of a vehicle-mounted IMU, specifically comprising: obtaining raw data from the IMU and observation data from a navigation device when a moving vehicle is in linear motion, wherein the raw data includes angular velocity and acceleration; The original data is error compensated to obtain a compensated angular velocity and a compensated acceleration, and the first state information of the IMU is obtained based on the compensated angular velocity and the compensated acceleration; the second state information of the vehicle is obtained through a Kalman filter based on the observation data; the first state information and the second state information are fused using the Kalman filter to obtain a first velocity vector of the IMU in the inertial navigation coordinate system and a second velocity scalar of the IMU in the vehicle coordinate system; and the installation angle of the IMU is determined based on the vehicle kinematic constraints satisfied by the first velocity vector and the second velocity scalar.

[0008] Preferably, an equation for the first velocity vector and the second velocity scalar when the mobile vehicle moves in a straight line is established based on the vehicle kinematic constraints; the first velocity vector is decomposed according to the equation to obtain a right direction velocity in the inertial navigation coordinate system, a forward direction velocity in the inertial navigation coordinate system, and an upward direction velocity in the inertial navigation coordinate system; An arc tangent calculation is performed on the right direction speed and the forward direction speed to obtain a heading installation angle; an arc sine calculation is performed on the right direction speed, the forward direction speed, and the upward direction speed to obtain a pitch installation angle.

[0009] Preferably, the IMU is installed on the INS, the integrated navigation system includes the INS and the navigation device, the first state information includes the speed, position and attitude of the IMU and the corresponding first optimal uncertainty estimate, the second state information includes the speed and position of the vehicle and the corresponding second optimal uncertainty estimate, and the method further includes: based on the speed, position, attitude, error model of the IMU at the current moment and the optimal uncertainty estimate of the integrated navigation system at the previous moment, obtaining the first predicted uncertainty estimate at the current moment and the predicted state at the current moment; based on the second state information at the current moment, calculating the measurement uncertainty estimate at the current moment and the measurement state at the current moment; based on the measurement state, the measurement uncertainty estimate, the first predicted uncertainty estimate and the predicted state, obtaining a first deviation and a first Kalman gain; according to the first Kalman gain, the predicted state at the current moment and the first deviation, obtaining the optimal speed, position and attitude of the integrated navigation system at the current moment and the first optimal uncertainty estimate at the current moment; according to the optimal speed, position and attitude, obtaining the first velocity vector of the IMU in the inertial navigation coordinate system and the second velocity scalar of the IMU in the vehicle coordinate system.

[0010] Prioritizing, the acceleration and the angular velocity measured by the accelerometer and the gyroscope are obtained, and the acceleration and the angular velocity are error compensated based on the error compensation model to obtain the compensated acceleration and the compensated angular velocity; based on the compensated angular velocity and the compensated acceleration, the IMU is mechanically arranged to obtain the updated posture of the IMU at the current moment and the updated speed of the IMU at the current moment; based on the current moment update speed, the previous moment update speed, the previous moment update posture and the previous moment update position, the current moment update position is obtained.

[0011] Prioritizing, calculating the velocity increment and angular increment from the previous moment to the current moment based on the compensated acceleration and the compensated angular velocity; The optimal speed, position and attitude output by the combined navigation filter at the previous moment and the angle increment and speed increment at the current moment are used to obtain the updated attitude and updated speed of the IMU at the current moment.

[0012] Prioritize, based on the observation data at the current moment and the empirical error, the observed speed, observed position and the observation uncertainty estimate of the vehicle at the current moment are calculated; based on the second state information of the vehicle at the previous moment and the time interval, the predicted speed, predicted position and the second predicted uncertainty estimate of the vehicle at the current moment are obtained; based on the observation uncertainty estimate and the second predicted uncertainty estimate, a second Kalman gain is obtained; based on the predicted speed, the predicted position, the observed speed and the observed position, a second deviation is obtained; based on the second Kalman gain, the predicted speed, the predicted position and the second deviation, the second state information of the vehicle at the current moment is obtained.

[0013] Prioritize, determine whether the first velocity vector satisfies a non-holonomic constraint; if the first velocity vector satisfies the non-holonomic constraint, determine the installation angle of the IMU based on the vehicle kinematic constraints satisfied by the first velocity vector and the second velocity scalar; if the first velocity vector does not satisfy the non-holonomic constraint, reacquire the raw data and observation data at the next moment to obtain a new first velocity vector based on the reacquired raw data and observation data.

[0014] Preferably, the navigation device is one or more of a GNSS receiver, a wheel speed odometer, a visual odometer, a lidar or a magnetometer.

[0015] In a second aspect, the present invention provides a device for quickly estimating the installation angle of a vehicle-mounted IMU, specifically comprising: at least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the processor to execute the method for quickly estimating the installation angle of the vehicle-mounted IMU in the first aspect.

[0016] In a third aspect, the present invention further provides a non-volatile computer storage medium, wherein the computer storage medium stores computer executable instructions, which are executed by one or more processors to complete the method provided by the method described in the first aspect.

[0017] Compared with the prior art, the beneficial effects of the present invention are as follows: an IMU is fixedly connected to the body of a mobile vehicle, and a navigation device is also installed on the body of the mobile vehicle to obtain the original data of the IMU and the observation data of the navigation device when the mobile vehicle moves in a straight line; error compensation is performed on the original data, and the first state information of the IMU is obtained based on mechanical arrangement; the observation data is processed using a Kalman filter to obtain the second state information of the vehicle; the first state information and the second state information are fused using a Kalman filter to obtain the first velocity vector of the IMU in the inertial navigation coordinate system and the second velocity scalar of the IMU in the vehicle body coordinate system; the installation angle of the IMU is determined based on the vehicle kinematic constraints satisfied by the first velocity vector and the second velocity scalar. Using this method, the real-time performance is strong, and no subsequent calculation is required. The installation angle of the vehicle-mounted IMU can be estimated by running a straight line trajectory. The calculation formula is simple, and the estimation of the installation angle of the vehicle-mounted IMU can be achieved more simply and accurately. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] To more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments of the present invention. Obviously, the drawings described below are only some embodiments of the present invention. Those skilled in the art can also derive other drawings based on these drawings without inventive effort.

[0019] Figure 1 This is a schematic diagram of a method for quickly estimating the installation angle of a vehicle-mounted IMU provided by an embodiment of the present invention; Figure 2 The embodiment of the present invention provides Figure 1 Flow chart of the specific implementation method of step 105; Figure 3 The embodiment of the present invention provides Figure 1 Flow chart of the specific implementation method of step 104; Figure 4 The embodiment of the present invention provides Figure 1 Flow chart of the specific implementation method of step 102; Figure 5 The embodiment of the present invention provides Figure 1 Flow chart of the specific implementation method of step 103; Figure 6 2 is a schematic diagram of an installation angle estimation curve of the IMAE method provided in an embodiment of the present invention; Figure 7 Schematic diagram of vehicle trajectory during the satellite shielding phase provided by an embodiment of the present invention; Figure 8 Schematic diagram of the eastward trajectory error calculated by three methods for the loss of lock segment provided by an embodiment of the present invention; Figure 9 Schematic diagram of trajectory northing error calculated by three methods for loss of lock segments provided by an embodiment of the present invention; Figure 10 Schematic diagram of trajectory antenna error calculated by three methods for loss of lock phase provided by an embodiment of the present invention; Figure 11 is a schematic diagram of a real-time trajectory diagram provided by an embodiment of the present invention; Figure 12 1 is a schematic diagram of a simulated real-time easting error provided by an embodiment of the present invention; Figure 13 1 is a schematic diagram of a simulated real-time northing error provided by an embodiment of the present invention; Figure 14 1 is a schematic diagram of a simulated real-time antenna direction error provided by an embodiment of the present invention; Figure 15 A schematic diagram of the structure of a vehicle-mounted IMU installation angle rapid estimation device provided by an embodiment of the present invention; The accompanying drawings are numerals as follows: 21: Processor; 22: Memory. DETAILED DESCRIPTION

[0020] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0021] Unless the context requires otherwise, throughout the specification and claims, the term "including" is to be interpreted as meaning open inclusion, that is, "including, but not limited to". In the description of the specification, the terms "one embodiment", "some embodiments", "exemplary embodiments", "example", "specific example" or "some examples" and the like are intended to indicate that the specific features, structures, materials or characteristics associated with the embodiment or example are included in at least one embodiment or example of the present disclosure. The schematic representation of the above terms does not necessarily refer to the same embodiment or example. In addition, the specific features, structures, materials or characteristics may be included in any one or more embodiments or examples in any appropriate manner, that is, although they may be carried in the embodiments or examples of the above terms due to reasons such as the order and position of appearance, it is not limited to that they can be carried in combination by one embodiment or example.

[0022] In the description of the present invention, the terms "first" and "second" are used for descriptive purposes only, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, the features defined as "first" and "second" may explicitly or implicitly include one or more of the features. In the description of the embodiments of the present disclosure, unless otherwise specified, "multiple" means two or more. In addition, for example, the description may also use the method of adding "A" and "B" at the end to describe the same type of nouns as two independent individuals. In this case, the corresponding features defined as "A" and "B" are only used to distinguish the description purposes of the same type of individuals, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated.

[0023] In the description of the present invention, the expression "A and / or B" (where A and B are used to formally represent specific characteristic contents) is involved, and the corresponding expressions include the following three combinations: only A, only B, and a combination of A and B.

[0024] As used herein, "about," "substantially," or "approximately" includes the stated value and an average value that is within an acceptable range of deviation from the particular value as determined by one of ordinary skill in the art taking into account the measurements in question and errors associated with measurement of the particular quantity (i.e., limitations of the measurement system).

[0025] In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0026] Navigation equipment (GNSS, for example) and INS are typically mounted on the body of a moving vehicle. However, due to uncertainties in mounting position and orientation, there is often an offset between the INS and the vehicle's body. This offset is also known as the mounting angle or mounting offset angle. While GNSS uses satellite signals to determine the vehicle's motion, INS utilizes the accelerometers and gyroscopes in the inertial measurement unit (IMU) to measure the vehicle's motion. Wheel speedometers and NHCs provide important correction functions, particularly at low speeds or when stationary, helping to reduce errors accumulated by the IMU. Wheel speedometers directly measure the speed of the vehicle's wheels, and their readings are based on the vehicle's body coordinate system. NHCs further utilize the vehicle's inherent kinematic characteristics (for example, ground vehicles cannot move sideways) to correct the data derived from the INS and GNSS, also relying on information from the vehicle's body coordinate system. Therefore, if the mounting angle between the INS and the vehicle is not accurately known, the system's estimation of the vehicle's true motion will be affected. Therefore, when using wheel speedometers or NHCs for calibration, accurate calibration of the mounting angle is essential.

[0027] In order to solve the above problems, the above embodiments of the present invention provide a method for quickly estimating the installation angle of a vehicle-mounted IMU, which can achieve the estimation of the installation angle of the vehicle-mounted IMU more simply and accurately.

[0028] Since the IMU is located inside the INS and is key to the INS's ability to measure the motion state of a moving vehicle, the IMU installation angle is subsequently used to represent the installation deviation between the INS and the moving vehicle's body. The following describes the specific implementation of a method for rapidly estimating the vehicle-mounted IMU installation angle, using accompanying figures and specific examples.

[0029] Embodiment 1: For ease of description, the embodiment of the present invention defines a set of spatial coordinate systems, but the method of the embodiment of the present invention itself is irrelevant to the selection of the coordinate system.

[0030] Navigation coordinate system (n-system): This uses the ENU (East North Up) geographic coordinate system, with its origin located at the center of mass of the vehicle. In this coordinate system, the X-axis points east along the local latitude, the Y-axis points north along the local meridian, and the Z-axis, perpendicular to the ground and pointing upward, forms a right-handed rectangular coordinate system. The plane formed by the X- and Y-axes represents the local horizontal plane, while the plane formed by the X- and Z-axes corresponds to the local meridian plane.

[0031] Vehicle coordinate system (V system): The right front upper coordinate system is used. This system is fixed on the vehicle body and moves with the vehicle body at all times. The X-axis points to the right of the vehicle body, the Y-axis points to the front of the vehicle body, and the Z-axis is perpendicular to the ground and points to the top of the vehicle body. XYZ constitutes a right-handed rectangular coordinate system.

[0032] Inertial navigation coordinate system (B system): uses the right front upper coordinate system, which is fixed on the IMU and moves with the IMU at all times. The X axis points to the right of the IMU, the Y axis represents the front of the IMU, and the Z axis is perpendicular to the ground and points above the IMU. XYZ constitutes a right-handed rectangular coordinate system.

[0033] Standing directly behind the moving vehicle and looking forward, the right front upper coordinate system can be regarded as: the positive direction of the X axis extends to the right, the positive direction of the Y axis is the direction in which the moving vehicle is traveling forward, and the positive direction of the Z axis is along the vertical axis of the moving vehicle, pointing from the ground to the sky.

[0034] like Figure 1 As shown, the method for quickly estimating the vehicle-mounted IMU installation angle provided in Example 1 of the present invention specifically includes the following steps: Step 101: Obtaining raw data from the IMU and observation data from the navigation device when the mobile vehicle moves in a straight line, wherein the raw data includes angular velocity and acceleration.

[0035] Among them, the moving vehicle can be a train or a motor vehicle. Compared with a motor vehicle, the motion conditions of a train are more consistent with the on-board kinematic constraint equations. Therefore, the method provided in the embodiment of the present invention is also applicable to the rapid estimation of the installation angle of the on-board IMU of a train, and theoretically has higher accuracy than that of a motor vehicle.

[0036] Specifically, linear motion refers to the movement of a mobile vehicle along the Y-axis of the v-frame, that is, the mobile vehicle travels forward or backward without displacement along the X-axis or Z-axis of the v-frame; the raw data is measured by the accelerometer and gyroscope in the IMU, where the acceleration is measured by the accelerometer, reflecting the linear acceleration of the mobile vehicle, and the angular velocity is measured by the gyroscope, reflecting the rotational speed of the mobile vehicle around the corresponding axis; the observation data is measured by GNSS, and the observation data includes pseudorange and carrier phase, etc. Among them, the pseudorange refers to the distance obtained by multiplying the signal propagation time from the receiver to the satellite by the speed of light. The carrier phase refers to the phase change of the carrier signal transmitted by the satellite relative to the reference signal inside the receiver, which can be accurately measured by the receiver.

[0037] Step 102: performing error compensation on the raw data to obtain compensated angular velocity and compensated acceleration, and obtaining first state information of the IMU based on the compensated angular velocity and the compensated acceleration.

[0038] Specifically, the IMU's raw data compensation items include: zero bias repeatability, zero bias instability, random walk, scale factor nonlinearity, and inter-axis non-orthogonality. In general, the raw data is compensated using the parameters given by the manufacturer to obtain compensated angular velocity and compensated acceleration. The inertial navigation mechanical arrangement is used to obtain the first state information of the IMU at the current moment, and the error amount at the current moment is synchronously updated for raw data error compensation at the next moment. The mechanical arrangement includes speed update, position update, and attitude update. The first state information includes the IMU's speed, position, and attitude, as well as the corresponding first optimal uncertainty estimate. The first optimal uncertainty estimate represents the error of the IMU's speed, position, and attitude.

[0039] Step 103 obtains second state information of the vehicle through a Kalman filter according to the observation data.

[0040] Specifically, the observation data must be preprocessed before being updated through a Kalman filter. This preprocessing includes checking the satellite signal-to-noise ratio, performing multipath calculations, detecting and correcting cycle slips, and eliminating gross errors. After preprocessing, the processed observation data is obtained. The Kalman filter then calculates the vehicle's second state information at the current moment. This second state information is then saved and used as the data source for the next prediction update. This second state information includes the vehicle's speed and position, along with the corresponding second-best uncertainty estimate, which represents the error in the vehicle's speed and position.

[0041] Step 104: Use a Kalman filter to fuse the first state information and the second state information to obtain a first velocity vector of the IMU in the inertial navigation coordinate system and a second velocity scalar in the vehicle coordinate system.

[0042] Specifically, the first state information and the second state information come from the IMU and the navigation device. The Kalman filter is used to weightedly fuse the information from the two different sources to give full play to the advantages of different information and obtain high-precision speed, position and attitude. The first velocity vector of the IMU in the inertial coordinate system and the second velocity scalar in the vehicle coordinate system are obtained through the rotation matrix and the lever arm.

[0043] Step 105: Determine the installation angle of the IMU according to the vehicle kinematic constraints satisfied by the first velocity vector and the second velocity scalar.

[0044] Specifically, a determination is made as to whether the first velocity vector satisfies a nonholonomic constraint. If so, the IMU installation angle is determined based on the vehicle kinematic constraints satisfied by the first velocity vector and the second velocity scalar. If not, the IMU raw data and navigation device observation data are reacquired at the next moment to obtain a new first velocity vector and second velocity scalar based on the reacquired raw data and observation data. The nonholonomic constraint utilizes the characteristic that the lateral velocity of a moving vehicle is zero when moving in a straight line to suppress data drift.

[0045] In this embodiment, an IMU and a navigation device are fixedly connected to the body of a moving vehicle to obtain the IMU's acceleration, angular velocity, and navigation device observation data during the vehicle's linear motion. These acceleration, angular velocity, and navigation device observation data are processed and fused using a corresponding algorithm to ultimately obtain a first velocity vector and a second velocity scalar. The IMU's mounting angle is then determined based on the kinematic constraints satisfied by these first and second velocity vectors. This method eliminates the need to assume a small mounting angle, making it easier to estimate the vehicle-mounted IMU's mounting angle.

[0046] The IMU installation angle includes the heading, pitch, and roll angles. The heading angle refers to the IMU's azimuth relative to the vehicle's coordinate system, the pitch angle refers to the IMU's vertical tilt relative to the vehicle's coordinate system, and the roll angle refers to the IMU's sideways tilt relative to the vehicle's coordinate system.

[0047] In one embodiment, since a moving vehicle usually travels on a horizontal road, the roll angle is theoretically unobservable and has no effect on the NHC and the wheel speedometer, so it can be ignored and not estimated. The subsequent estimation of the IMU installation angle mainly estimates the heading installation angle and the pitch installation angle.

[0048] like Figure 2 As shown, the specific implementation method of step 105 provided in Example 1 of the present invention specifically includes the following steps: Step 201: Based on the vehicle kinematic constraints, establish equations for the first velocity vector and the second velocity scalar when the mobile vehicle moves in a straight line; decompose the first velocity vector according to the equations to obtain a right direction velocity in the inertial navigation coordinate system, a forward direction velocity in the inertial navigation coordinate system, and an upward direction velocity in the inertial navigation coordinate system.

[0049] In one embodiment, a first velocity vector of the IMU in the inertial navigation coordinate system and a second velocity scalar of the IMU in the vehicle system are obtained, and the first velocity vector is decomposed to obtain the right direction velocity in the inertial navigation coordinate system, the forward direction velocity in the inertial navigation coordinate system, and the upward direction velocity in the inertial navigation coordinate system. By decomposing the first velocity vector, the motion state of the moving vehicle can be accurately described.

[0050] Step 202: performing arc tangent calculation on the right direction speed and the forward direction speed to obtain a heading installation angle.

[0051] In one embodiment, the heading installation angle is calculated using the following formula.

[0052]

[0053] in, is the heading installation angle, is the right direction velocity in the inertial navigation coordinate system, is the forward velocity in the inertial navigation coordinate system.

[0054] Step 203: performing arcsine calculation on the right direction velocity, the front direction velocity, and the upward direction velocity to obtain a pitch installation angle.

[0055] In one embodiment, the pitch installation angle is calculated using the following formula.

[0056]

[0057] in, is the pitch installation angle, is the upward velocity in the inertial navigation coordinate system.

[0058] In one embodiment, the specific implementation methods of step 103 and step 104 are both based on a Kalman filter. For the convenience of description, the embodiment of the present invention constructs a set of Kalman filters, and the subsequent estimation of the installation angle of the IMU is based on the combined navigation of GNSS and INS.

[0059] Specifically, the state prediction equation is: ; in, represents the predicted state vector at time t, represents the optimal predicted state vector at time t-1, and A represents the state transition matrix that is updated over time. The state prediction equation predicts the current integrated navigation state vector based on the state transition matrix and the optimal state vector at the previous moment. The state vector includes the integrated navigation speed deviation, which is the difference between the integrated navigation predicted speed and the observed speed.

[0060] The error prediction equation is: ; in, Represents the prior error covariance matrix at time t, that is, the uncertainty estimate of the forecast; represents the posterior error covariance matrix at time t-1, that is, the uncertainty estimate of the prediction at the previous moment; Q represents the process noise covariance matrix, which represents the uncertainty of the system model.

[0061] The Kalman gain equation is: ; Where K represents the Kalman gain, P represents the prior error covariance matrix, which is the uncertainty estimate of the prediction, H represents the observation state coefficient matrix, which maps the predicted values ​​in the state space to the observation space, and R represents the observation noise covariance matrix, which is the sensor observation error.

[0062] like Figure 3 As shown, the specific implementation method of step 104 provided in embodiment 1 of the present invention specifically includes the following steps: Step 301: Based on the speed, position, attitude, error model of the IMU at the current moment and the optimal first uncertainty estimate of the integrated navigation system at the previous moment, a first predicted uncertainty estimate at the current moment and a predicted state at the current moment are obtained.

[0063] In one embodiment, the raw data of the IMU is used to predict and update the integrated navigation filter, and the predicted state at time t and the first prediction uncertainty estimate are calculated based on the state vector at time t-1 and the state transfer matrix at time t.

[0064] Step 302: Based on the second state information at the current moment, calculate and obtain the measurement uncertainty estimate and the measurement state at the current moment.

[0065] In one embodiment, when IMU data is updated, the integrated navigation filter uses the IMU data for prediction updates. After receiving the vehicle speed and position transmitted by the navigation device filter, the integrated navigation filter begins to perform measurement updates. Based on the current vehicle speed and position and the measurement uncertainty estimate at the previous moment, the measurement state matrix of the integrated navigation filter is obtained, namely, the current measurement uncertainty estimate and the current measurement state.

[0066] Step 303: Obtain a first bias and a first Kalman gain based on the measurement state, the measurement uncertainty estimate, the first prediction uncertainty estimate, and the predicted state.

[0067] In one embodiment, the sampling frequency of the IMU is 100-1000 Hz, and the sampling frequency of the navigation device is 1-10 Hz. That is, the sampling frequency of the IMU is much higher than that of the navigation device. Therefore, when only the speed, position, and attitude provided by the IMU are available, the integrated navigation filter only performs a predictive update. When both the speed, position, and attitude provided by the IMU and the speed and position of the vehicle are received simultaneously, the integrated navigation filter first performs a predictive update and then a measurement update. The measurement state, the measurement uncertainty estimate, the first predictive uncertainty estimate, and the predicted state are acquired at the same time. The first deviation reflects the prior error between the current measurement state and the current predicted state. The first predictive uncertainty estimate reflects the credibility of the current predicted state. The measurement uncertainty estimate reflects the credibility of the current measurement state. The first Kalman gain reflects the filter's focus.

[0068] When the uncertainty estimate of the first prediction is large, the first Kalman gain will assign a larger weight to the current measurement state; otherwise, it will assign a smaller weight.

[0069] Step 304: Obtain the optimal speed, position, and attitude of the integrated navigation system at the current moment and a first optimal uncertainty estimate at the current moment based on the first Kalman gain, the current predicted state, and the first deviation.

[0070] In one embodiment, the combined navigation filter performs weighted fusion on the predicted state at the current moment and the measured state at the current moment according to the first Kalman gain, and obtains the optimal speed, position and attitude, the first optimal uncertainty estimate and the optimal error parameter of the combined navigation at the current moment after fusion, wherein the first optimal uncertainty estimate is used to participate in the calculation of the predicted uncertainty estimate at the next moment, and the optimal error parameter is used for Kalman filter closed loop error correction.

[0071] Step 305: According to the optimal speed, position and attitude, obtain a first velocity vector of the IMU in the inertial navigation coordinate system and a second velocity scalar of the IMU in the vehicle coordinate system.

[0072] In one embodiment, based on the current attitude and position, a transformation matrix between different coordinate systems can be calculated. After obtaining the transformation matrix between different coordinate systems, the corresponding vector of the velocity vector in each coordinate system can be obtained through the transformation matrix. Specifically, the optimal velocity, position, and attitude are generally expressed in the navigation coordinate system n-system. Therefore, based on the attitude and position, a rotation matrix is ​​calculated from the navigation coordinate system to the inertial navigation coordinate system. The first velocity vector of the IMU in the inertial navigation coordinate system is calculated based on the rotation matrix. Simultaneously, based on the constraints, the second velocity scalar of the IMU in the vehicle coordinate system is calculated.

[0073] like Figure 4 As shown, the specific implementation method of step 102 provided in embodiment 1 of the present invention specifically includes the following steps: Step 401: Acquire the acceleration and the angular velocity measured by the accelerometer and the gyroscope, and perform error compensation on the acceleration and the angular velocity based on an error compensation model to obtain compensated acceleration and compensated angular velocity.

[0074] Specifically, the current acceleration measured by the accelerometer and the current angular velocity measured by the gyroscope are obtained, the error compensation model and the iterative value of the combined navigation filter at the previous moment are determined according to the technical parameters given by the inertial navigation manufacturer, the acceleration and angular velocity are compensated based on the error compensation model, and the compensated acceleration and compensated angular velocity are calculated.

[0075] Step 402: Based on the compensated angular velocity and the compensated acceleration, the IMU is mechanically arranged to obtain the updated posture of the IMU at the current moment and the updated speed of the IMU at the current moment.

[0076] Specifically, the velocity increment and angular increment from the previous moment to the current moment are calculated based on the compensated acceleration and compensated angular velocity. The integrated navigation filter is a continuous closed-loop system. Once all errors have fully converged, the obtained velocity, position, and attitude are highly accurate. Using the optimal velocity, position, and attitude output by the integrated navigation filter at the previous moment, as well as the angular increment and velocity increment at the current moment, the IMU's current updated attitude and IMU's current update speed can be calculated. The IMU's current updated attitude is the attitude of the IMU at the current moment in step 301, and the IMU's current update speed is the speed of the IMU at the current moment in step 301.

[0077] Step 403: Based on the current update speed, the last update speed, the last update posture and the last update position, the current update position is obtained.

[0078] Specifically, since the sampling interval of the IMU is short, within one sampling period, the position increment can be obtained by multiplying the average of the current update speed and the previous update speed by the time interval. The position increment is added to the updated position at the previous moment to obtain the updated position of the IMU at the current moment, where the updated position of the IMU at the current moment is the position of the IMU at the current moment in step 301.

[0079] like Figure 5 As shown, the specific implementation method of step 103 provided in Example 1 of the present invention specifically includes the following steps: Step 501: Based on the observation data at the current moment and the empirical error, the observation speed and observation position of the vehicle at the current moment and the observation uncertainty estimate at the current moment are calculated.

[0080] Specifically, observation data is typically GNSS observation data. In actual processing, GNSS error ranges are typically established based on experience. These errors typically include pseudorange measurement error, carrier phase measurement error, tropospheric delay error, ephemeris error, and receiver clock error. GNSS observation data is generally redundant. Therefore, given the error size of each component, the current vehicle's observed speed and position, as well as the current observation uncertainty estimate, can be calculated using weighted least squares.

[0081] Step 502: Based on the second state information of the vehicle at the previous moment and the time interval, obtain the predicted speed and predicted position of the vehicle at the current moment and the second prediction uncertainty estimate at the current moment.

[0082] In one embodiment, the GNSS Kalman filter first performs a prediction update, which uses the second state information and time interval of the vehicle at the previous moment and the state transition matrix to calculate the predicted speed and predicted position of the vehicle at the current moment, and then uses a given noise matrix to update the second prediction uncertainty estimate at the current moment.

[0083] Step 503: Obtain a second Kalman gain based on the observation uncertainty estimate and the second prediction uncertainty estimate.

[0084] Specifically, the observation uncertainty estimate reflects the error distribution of the vehicle's observed speed and position at the current moment, while the second prediction uncertainty estimate reflects the error distribution of the predicted speed and position at the current moment. The error distribution is reflected as weights in the GNSS Kalman filter, and the second Kalman gain is the specific calculated value of the weights.

[0085] Step 504: Obtain a second deviation according to the predicted speed, the predicted position, the observed speed, and the observed position.

[0086] Specifically, the second deviation can be obtained by subtracting the observed speed and observed position of the vehicle at the current moment from the predicted speed and predicted position of the vehicle at the current moment.

[0087] Step 505: Obtain second state information of the vehicle at the current moment according to the second Kalman gain, the predicted speed, the predicted position and the second deviation.

[0088] Specifically, after obtaining the second Kalman gain and the second deviation, the second Kalman gain and the second deviation can be used to perform measurement update of the Kalman filter, and based on the predicted speed and the predicted position, the second state information of the vehicle at the current moment can be obtained.

[0089] In this embodiment, an IMU is fixedly attached to the body of a moving vehicle, and a navigation device is installed on the body of the moving vehicle. The IMU's acceleration and angular velocity, along with the navigation device's observation data, are acquired during the vehicle's linear motion. These acceleration, angular velocity, and navigation device observation data are processed and fused using a corresponding algorithm to ultimately obtain a first velocity vector and a second velocity scalar. The IMU's mounting angle is then determined based on the kinematic constraints satisfied by these first and second velocity scalars. This method makes it easier to estimate the mounting angle of a vehicle-mounted IMU.

[0090] Example 2: The following describes the computational simplicity of the method for quickly estimating the vehicle-mounted IMU installation angle proposed by the present invention.

[0091] In this embodiment, the ENU geographic coordinate system is selected as the navigation coordinate system, recorded as the n system; the right front upper coordinate system is selected as the inertial navigation coordinate system, recorded as the b system; and the right front upper coordinate system is selected as the vehicle body coordinate system, recorded as the v system.

[0092] Generally, the b- and v-frames are assumed to completely overlap. However, in actual installation, due to installation uncertainties, the b- and v-frames do not completely overlap, and an installation angle exists between them. In GNSS and INS integrated navigation, in addition to the mobile vehicle measurement data provided by the GNSS and INS themselves, wheel speedometer and NHC correction measurement data are also used. Because both the wheel speedometer and NHC use the vehicle coordinate system as the reference coordinate system, when using a wheel speedometer or NHC, the installation angle must be accurately estimated.

[0093] The installation angle of the B system and the V system is the heading installation angle , pitch installation angle , roll installation angle According to the installation angle, the direction cosine matrix from the b system to the v system can be obtained: (1) in, 、 , and so on. According to the above formula, we can get: (2) in, represents the speed of the moving vehicle in frame b, 、 and are the velocity components of the moving vehicle in the b system, namely, the right direction velocity of the moving vehicle in the b system, the front direction velocity of the moving vehicle in the b system, and the upward direction velocity of the moving vehicle in the b system; Represents the speed of the moving vehicle under the v system, 、 and They are respectively the velocity components of the moving vehicle under the V system, namely the right direction speed of the moving vehicle under the V system, the front direction speed of the moving vehicle under the V system and the upward direction speed of the moving vehicle under the V system.

[0094] When the vehicle is in straight-line motion and does not skid or drift, there is only forward velocity, and the velocities in the other two directions are zero, that is: (3) Substituting formula (3) into formula (2), we can obtain: (4) When calculating the installation angle, the existing method assumes that the installation angle is a small angle. The error model derivation equation is as follows (5) in .

[0095] Select the error state of the inertial navigation system as velocity error , attitude error vector , position error , gyro zero drift , plus zero , a total of 15 dimensions. Considering formula (5), the heading installation angle and pitch installation angle are also expanded as state variables, and the state variables of the vehicle dynamics model constraint-assisted integrated navigation model are expanded to 17 dimensions: (6) The speed output by the inertial navigation system Converted to the n system, the speed is , the transformation matrix is , then the speed under the v system It can be calculated as follows: (7) Then use the right direction speed under the V system and the upward velocity in the v system The constraint that this is zero constitutes the measurement value: (8) If the installation angle itself meets the small angle requirement, no pre-calibration is required. is the unit matrix. The measured value can be directly composed of the velocity components in the b frame decomposed by the inertial navigation system: (9) Performing total differentiation on formula (7) and considering formula (5), we can obtain: (10) This formula can also be written in another form: (11) From the result of formula (11), combined with the measured value to form formula (8) and the error model formula (5), the measurement equation for the constraint-assisted vehicle dynamics model can be obtained as follows: (12) in, To measure the equivalent noise, the measurement matrix H can be expressed as: (13) Using the above formula, the installation angle can be iteratively estimated using the Kalman filter when the vehicle is moving in a straight line. However, it should be noted that due to the influence of factors such as the installation arm in the inertial navigation system, the lateral velocity measurement error during the vehicle's steering process is large. Therefore, it is necessary to consider not using lateral velocity constraints for measurement updates during rapid steering.

[0096] The existing method estimates the installation angle through Kalman filter iteration. The calculation formula is complex and the installation angle needs to be assumed to be a small angle.

[0097] In the method provided in Example 1 of the present invention, considering that the IMU (Inertial Measurement Unit) is fixedly connected to the vehicle body and there is no relative motion, the velocity scalars of the two are the same, that is, .

[0098] because , then according to the above formula (3) we can get:

[0099] Substituting the above relationship into the above formula (4), we get:

[0100] According to the above formula, when the vehicle is moving in a straight line, the IMU installation angle can be directly calculated using the three-dimensional velocity of the b-axis. That is, when using GNSS / INS integrated navigation, if the GNSS signal is good and the error is fully converged, the b-axis velocity output by the integrated navigation system can be used to achieve real-time calculation of the installation angle. At the same time, considering factors such as vehicle body vibration, a certain time window can be set in engineering applications for averaging to eliminate interference from high-frequency errors. The calculation formula provided by the embodiment of the present invention is simple and does not require the installation angle to be assumed to be a small angle.

[0101] Example 3: To illustrate the difference in calculation accuracy between the vehicle-mounted IMU installation angle rapid estimation method proposed in this embodiment and existing methods, the following describes the calculation accuracy of the IMU installation angle, NHC testing, and real-time testing at large installation angles with reference to the accompanying figures.

[0102] According to the calculation formulas for the heading installation angle and the pitch installation angle, the following formula can be obtained: (14) in, , is the true value of the pitch installation angle, is the pitch installation angle estimation error, and the other terms are similar. From this, the installation angle estimation accuracy can be calculated: (15) In one embodiment, the accuracy of the IMU Mounting Angle Fast Estimation (IMAFE) method provided by an embodiment of the present invention is verified using multiple sets of on-vehicle measured data. At the same time, the reliability of the method provided by an embodiment of the present invention is verified by comparing the calculation results of the widely recognized commercial post-processing software Inertial Explorer (IE) and the IMU Mounting Angle Estimation (IMAE) method provided by the existing method.

[0103] The mobile vehicle was located in Jiangxia District, Wuhan. The GNSS board used was the Unicore UM982 high-precision positioning and orientation board, and the inertial navigation system used was the Honeywell tactical-grade MEMS HG4930CA51. The HG4930CA51's parameters are shown in Table 1. The arm between the inertial navigation system and the antenna was precisely calculated based on design drawings, with millimeter-level accuracy. Time synchronization was achieved using PPS signals. The entire system was installed above the rear wheels of the mobile vehicle.

[0104] Table 1 HG4930CA51 parameter indicators

[0105] There are a total of 9 groups of on-board measured data (see Table 2 for details). Among them, the first three groups of measured data were measured at different times after re-disassembly and reassembly, and were used to verify the consistency of the three methods after disassembly and reassembly; the last seven groups of measured data were measured on the same day with the same set of equipment without disassembly and reassembly, but with repeated startup, and were used to verify the stability of the equipment's repeated startup and the repeatability of the method.

[0106] The average speed of the 9 sets of moving vehicle data is about 15 m / s. According to Table 1 and formula (15), when the installation angle is small, the accuracy of the installation angle can be calculated as: (16) Both IE and IMAE are post-processing methods, requiring the vehicle trajectory to be calculated first and then estimating the installation angle based on the vehicle trajectory. IMAFE can estimate the installation angle either post-process or in real time. To ensure estimation accuracy, this embodiment uniformly adopts a post-processing approach, with the IE software using a bidirectional tight combination mode to calculate the vehicle trajectory.

[0107] Table 2 shows the calculated mounting angles based on vehicle trajectories. The first three data sets show that the three methods are essentially consistent, with the maximum differences in the heading and pitch mounting angles being 0.117° and 0.088°, respectively. Based on the last seven data sets, the standard deviations of the heading and pitch mounting angles for IE, IMAE, and IMAFE are calculated to be (0.055°, 0.098°), (0.085°, 0.039°), and (0.039°, 0.019°), respectively.

[0108] Table 2 Calculation of vehicle track installation angle

[0109] From the above results alone, we can conclude that IMAFE achieves the highest installation angle estimation accuracy in post-processing mode. However, the three methods utilize different data for actual installation angle estimation. IE uses all data, IMAE excludes the initial static portion, and IMAFE uses data filtered from all data using a threshold (speed ≥ 15 m / s, gyro three-axis output ≤ 15° / s). The selection of different data for each method is based on the following considerations: (1) IE is a pre-set software. Its internal parameters cannot be changed, and its calculation principle is not explained. Only the input parameters can be changed. This embodiment uses segmented processing on the same set of data and uses IE to calculate the heading and pitch installation angles of different segments. The results show that the maximum difference in the installation angles of different segments can reach 0.3°. This may be because the segmentation time is too short and the internal parameters have not fully converged. Therefore, this paper chooses to use IE to estimate the installation angle without changing the original data. (2) In the process of using the IMAE method in this embodiment, it was found that when the static time is long, the final result will converge to a local optimal solution, which is significantly different from the true value (about 0.5°). This phenomenon no longer occurs after eliminating the static stage. In addition, even after convergence, the estimated value of the installation angle will fluctuate. For details, see Figure 6 ,From the figure, we can see that the fluctuation of the installation angle after ,convergence is around 0.15°.

[0110] (3) When using the IMAFE method, the kinematic constraints of the vehicle must not be violated. At the same time, the higher the speed, the higher the accuracy.

[0111] Therefore, it is not rigorous to conclude that the IMAFE method has the highest estimation accuracy based solely on the existing results. Unlike IMAE, each sampling point in IMAFE can be used as an observation value to calculate the installation angle. If each observation value is independent of each other, according to the covariance propagation law, the estimation accuracy of the IMAFE installation angle is: (17) In the formula It is the accuracy of averaging the heading and pitching installation angles over multiple measurements. n is the number of observations. n are both greater than 10000. According to formula (16) and formula (17), the installation angle accuracy should be better than 0.001° in theory. However, in reality, the accuracy is only better than 0.05°. After analysis, the reasons for this phenomenon are: (1) The installation angle between the rear wheel contact point of the vehicle and the IMU is not always a fixed value. The bumps and vibrations during the vehicle's movement will cause the installation angle to change slightly.

[0112] (2) There is a certain correlation between the sampling points at different times in the same vehicle trajectory, and the observation values ​​are not independent of each other.

[0113] (3) The installation angle varies among different vehicle trajectories due to differences in vehicle loads and driving conditions.

[0114] (4) In the combined navigation solution, the installation angle and attitude error are not completely separated, and part of the installation angle component is brought into the Kalman filter.

[0115] Through the above analysis, this paper can draw the following conclusions: The installation angle accuracy estimated by IMAFE is at the same level as the existing methods, and its accuracy is better than 0.05° for tactical-grade MEMS.

[0116] In one embodiment, a moving vehicle is used for testing, and the instruments used are the same as those used in the above embodiments. Figure 7 As shown in the figure, the trajectory is approximately 17 kilometers long and lasts approximately 30 minutes. The GNSS maintains a fixed solution throughout the vehicle's movement. To verify the effect of the mounting angle on NHC, the following experimental steps were designed: (1) The bidirectional smoothed post-processing trajectory calculated by GNSS / INS integrated navigation is used as the true value, and the IMU installation angle is calculated afterwards using the IMAFE method; (2) Block all satellites in a certain section to simulate the situation where satellites lose lock in the tunnel; (3) There are three ways to solve the trajectory in the unlocked section: pure INS solution, NHC constraint without considering the installation angle, and NHC constraint with considering the installation angle.

[0117] Satellite shielding situations such as Figure 7 As shown, the total time is 420 seconds and the total length is about 6 kilometers. One of the purposes of selecting the middle section shield is to make the errors of the GNSS / INS combined system fully converge before the two-way solution enters the NHC stage. 、 They are -0.490° and -2.028° respectively.

[0118] The trajectory errors calculated by the three methods during the unlocked phase are as follows: Figure 8 、 Figure 9 and Figure 10 The error statistics are shown in Table 3. As can be seen from the table, when the NHC constraint is directly used without considering the installation angle, the position error increases rapidly. The main reason is that when the installation angle is not considered, the right and upward velocity components are not zero. Forcing the constraint will instead bring the velocity components into the integrated navigation filter in the form of errors, thereby increasing the position error.

[0119] When the installation angle is taken into account, it can be seen that NHC can effectively suppress the growth of errors, where the east error is reduced from 2.85m to 1.83m, the north error is reduced from 4.99m to 0.88m, and the celestial error is reduced from 1.03m to 0.18m. A careful observation shows that the improvement effect of the east error is not obvious. This is because NHC only has two velocity constraints: lateral and celestial. Assuming that the heading is 0º, the north velocity error is unconstrained and gradually accumulates, while the east velocity error is constrained; when the heading turns 90º, the north velocity error is constrained and converges, while the east velocity error is unconstrained. In the simulated loss of lock segment used in this embodiment, the velocity component is mainly in the east direction, so the east constraint is poor. When we change the simulated loss of lock segment to be dominated by the north velocity component, the east error is better than the north error. The specific experiment will not be repeated.

[0120] Table 3 Position error statistics of three solution methods

[0121] The above embodiments verify the accuracy and reliability of the IMAFE method. Compared with the existing methods, IMAFE has two advantages: it does not need to assume that the installation angle is a small angle and can be calculated in real time.

[0122] To verify the accuracy of the IMAFE at various mounting angles, this paper designed seven sets of mobile vehicle experiments. The inertial navigation system (INS) was mounted on a rotating platform (turntable), which was then rotated through seven angles (+30°, +20°, +10°, 0°, -10°, -20°, and -30°). A mobile vehicle test was performed at each angle. The test hardware was identical to that in the previous embodiment, resulting in seven sets of data. During each rotation, only the turntable was rotated, with no modifications made to the INS, to ensure test consistency.

[0123] Due to various interference factors, the +30° data set was partially lost, rendering it unusable. The remaining six data sets were first bidirectionally smoothed to obtain the POS trajectory. The installation angles for the corresponding trajectories were then estimated using the IMAFE method. The final results are shown in Table 4.

[0124] Table 4 Installation angle estimation at different rotation angles

[0125] According to Table 4, compensating the rotation angle to the heading installation angle yields six sets of data. The standard deviations for the heading and elevation installation angles are (0.061°, 0.068°). Although the inertial navigation system remains constant relative to the turntable during rotation, the turntable's fixing screws must be reinstalled before and after each rotation, which inevitably introduces corresponding errors. Furthermore, the turntable itself also has errors. These errors are incorporated into the installation angle as components. Therefore, the actual installation angle accuracy should be better than (0.061°, 0.068°). Ultimately, we conclude that the IMAFE can maintain high accuracy even at large installation angles, achieving an accuracy better than 0.07° for tactical-grade MEMS.

[0126] Both the IE and IMAE methods require a high-precision POS trajectory to be obtained beforehand. Once the integrated navigation system's errors converge, the IMAFE method can calculate the corresponding installation angle from any straight line segment. This means the IMAFE method doesn't require prior calibration and can provide results online in real time. The IMAFE method is particularly suitable for equipment that is frequently disassembled and assembled and requires the use of NHC or a wheel speedometer.

[0127] To validate the IMAFE method, this embodiment uses the data from the previous embodiment for real-time simulation testing. First, raw data is fed frame by frame into the integrated navigation system, simulating real-time data acquisition and transmission. Then, the system is initialized using a dynamic alignment scheme. Once the system has fully converged, a corresponding threshold is set to filter the data, and the filtered data is used to calculate the inertial navigation installation angle. Finally, a segment of data after the installation angle calculation is completed is selected, satellites are blocked, and pure inertial navigation solutions are performed with and without NHC constraints.

[0128] Specific as Figure 11 As shown in the figure, the real-time calculation results of the heading installation angle and the pitch installation angle are (-0.459°, -2.046°), which are basically consistent with the subsequent results. The satellite shielding time is 932 seconds in total.

[0129] The real-time simulation results are as follows: Figure 12 、 Figure 13 and Figure 14The statistical results are shown in Table 5. It can be seen that after adding NHC constraints, the accuracy in the three directions is greatly improved, which also proves the feasibility of the IMAFE method in real-time solution.

[0130] Table 5 Simulation real-time error statistics

[0131] Example 4: Based on the method for quickly estimating the installation angle of the vehicle-mounted IMU provided in the above embodiment, the present invention also provides a device for quickly estimating the installation angle of the vehicle-mounted IMU that can be used to implement the above method, such as Figure 15 The figure is a schematic diagram of the device architecture of an embodiment of the present invention. The device for quickly estimating the vehicle-mounted IMU installation angle of this embodiment includes one or more processors 21 and a memory 22. Figure 15 A processor 21 is taken as an example.

[0132] The processor 21 and the memory 22 may be connected via a bus or other means. Figure 15 The bus connection is taken as an example.

[0133] Memory 22, as a non-volatile computer-readable storage medium for the method for rapidly estimating the mounting angle of a vehicle-mounted IMU, can be used to store non-volatile software programs and non-volatile computer-executable programs, such as the method for rapidly estimating the mounting angle of a vehicle-mounted IMU described in the aforementioned embodiment. Processor 21 executes the non-volatile software programs, instructions, and modules stored in memory 22 to perform various functional applications and data processing of the device for rapidly estimating the mounting angle of a vehicle-mounted IMU, thereby implementing the method for rapidly estimating the mounting angle of a vehicle-mounted IMU described in the aforementioned embodiment.

[0134] The memory 22 may include high-speed random access memory and non-volatile memory, such as at least one disk storage device, flash memory device, or other non-volatile solid-state memory device. In some embodiments, the memory 22 may optionally include a memory remotely located relative to the processor 21, and such remote memory may be connected to the processor 21 via a network. Examples of such networks include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0135] The program instructions / modules are stored in the memory 22. When executed by one or more processors 21, the method for quickly estimating the vehicle-mounted IMU installation angle in the above embodiment is executed. For example, the method described above is executed. Figure 1-Figure 5 The steps shown.

[0136] An embodiment of the present invention further provides a non-volatile computer storage medium, wherein the computer storage medium stores computer executable instructions, and the computer executable instructions are executed by one or more processors, for example Figure 15 A processor 21 in the embodiment can enable the one or more processors to execute the method for quickly estimating the vehicle-mounted IMU installation angle in the aforementioned embodiment, for example, executing the method described above. Figure 1-Figure 5 The steps shown.

[0137] It is worth noting that the information interaction, execution process, etc. between the modules and units within the above-mentioned devices and systems are based on the same concept as the processing method embodiment of the present invention. The specific content can be found in the description of the method embodiment of the present invention and will not be repeated here.

[0138] A person skilled in the art will understand that all or part of the steps in the various methods of the embodiments can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium, which may include: read-only memory (ROM), random access memory (RAM), a disk or an optical disk, etc.

[0139] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for quickly estimating the installation angle of a vehicle-mounted IMU, characterized in that: An IMU and a navigation device are installed on a mobile vehicle, wherein the IMU is fixedly connected to a body of the mobile vehicle. The method includes: Acquire raw data from the IMU and observation data from the navigation device when the mobile vehicle is in linear motion, wherein the raw data includes angular velocity and acceleration; Performing error compensation on the raw data to obtain a compensated angular velocity and a compensated acceleration, and obtaining first state information of the IMU according to the compensated angular velocity and the compensated acceleration; Obtaining second state information of the vehicle through a Kalman filter according to the observation data; fusing the first state information and the second state information using a Kalman filter to obtain a first velocity vector of the IMU in an inertial navigation coordinate system and a second velocity scalar of the IMU in a vehicle coordinate system; An installation angle of the IMU is determined according to vehicle kinematic constraints satisfied by the first velocity vector and the second velocity scalar.

2. The method for quickly estimating the installation angle of the vehicle-mounted IMU according to claim 1, characterized in that: The IMU installation angle includes a heading installation angle and a pitch installation angle; The method further comprises: Establishing equations for the first velocity vector and the second velocity scalar when the mobile vehicle is in linear motion according to the vehicle kinematic constraints; decomposing the first velocity vector according to the equations to obtain a right velocity in the inertial navigation coordinate system, a forward velocity in the inertial navigation coordinate system, and an upward velocity in the inertial navigation coordinate system; Performing an arc tangent calculation on the right direction speed and the forward direction speed to obtain a heading installation angle; An arcsine calculation is performed on the right direction velocity, the front direction velocity, and the upward direction velocity to obtain a pitch installation angle.

3. The method for quickly estimating the installation angle of the vehicle-mounted IMU according to claim 1, characterized in that: The IMU is mounted on the INS, the integrated navigation system includes the INS and the navigation device, the first state information includes the speed, position and attitude of the IMU and the corresponding first optimal uncertainty estimate, the second state information includes the speed and position of the vehicle and the corresponding second optimal uncertainty estimate, The method further comprises: Obtaining a first prediction uncertainty estimate at the current moment and a prediction state at the current moment based on the velocity, position, attitude, and error model of the IMU at the current moment and a first optimal uncertainty estimate of the integrated navigation system at the previous moment; Based on the second state information at the current moment, calculating a measurement uncertainty estimate and a measurement state at the current moment; obtaining a first bias and a first Kalman gain based on the measurement state, the measurement uncertainty estimate, the first prediction uncertainty estimate, and the predicted state; Obtaining an optimal speed, position, and attitude of the integrated navigation system at a current moment and a first optimal uncertainty estimate at a current moment based on the first Kalman gain, the predicted state, and the first deviation; According to the optimal speed, position and attitude, a first velocity vector of the IMU in the inertial navigation coordinate system and a second velocity scalar of the IMU in the vehicle body coordinate system are obtained.

4. The method for quickly estimating the vehicle-mounted IMU installation angle according to claim 3, characterized in that: The method further comprises: Acquire the acceleration and the angular velocity measured by the accelerometer and the gyroscope, and perform error compensation on the acceleration and the angular velocity based on an error compensation model to obtain a compensated acceleration and a compensated angular velocity; Based on the compensation angular velocity and the compensation acceleration, the IMU is mechanically arranged to obtain an updated posture of the IMU at a current moment and an updated speed of the IMU at a current moment; Based on the current update speed, the last update speed, the last update posture and the last update position, the current update position is obtained.

5. The method for quickly estimating the vehicle-mounted IMU installation angle according to claim 4, characterized in that: The method further comprises: Calculate the velocity increment and angular increment from the previous moment to the current moment based on the compensated acceleration and compensated angular velocity; The optimal speed, position and attitude output by the combined navigation filter at the previous moment and the angle increment and speed increment at the current moment are used to obtain the updated attitude of the IMU at the current moment and the updated speed of the IMU at the current moment.

6. The method for quickly estimating the vehicle-mounted IMU installation angle according to claim 1, characterized in that: The method further comprises: Based on the observation data at the current moment and the empirical error, the observation speed and observation position of the vehicle at the current moment and the observation uncertainty estimate at the current moment are calculated; Obtaining a predicted speed and a predicted position of the vehicle at a current moment and a second prediction uncertainty estimate at a current moment based on the second state information of the vehicle at a previous moment and a time interval; obtaining a second Kalman gain based on the observation uncertainty estimate and the second prediction uncertainty estimate; obtaining a second deviation according to the predicted speed, the predicted position, the observed speed, and the observed position; Second state information of the vehicle at a current moment is obtained according to the second Kalman gain, the predicted speed, the predicted position, and the second deviation.

7. The method for quickly estimating the vehicle-mounted IMU installation angle according to claim 1, characterized in that: The method further comprises: determining whether the first velocity vector satisfies a nonholonomic constraint; If the first velocity vector satisfies a nonholonomic constraint, determining an installation angle of the IMU according to a vehicle kinematic constraint satisfied by the first velocity vector and the second velocity scalar; If the first velocity vector does not satisfy the non-holonomic constraint condition, the original data and the observation data at the next moment are re-acquired to obtain a new first velocity vector according to the re-acquired original data and the observation data.

8. The method for quickly estimating the vehicle-mounted IMU installation angle according to any one of claims 1 to 7, characterized in that: The navigation device is one or more of a GNSS receiver, a wheel speed odometer, a visual odometer, a lidar or a magnetometer.

9. A vehicle-mounted IMU installation angle rapid estimation device, characterized in that: The device comprises: At least one processor; and a memory communicatively connected to the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the processor to execute the method for quickly estimating the installation angle of the vehicle-mounted IMU described in any one of claims 1-8.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the computer executes the method for quickly estimating the installation angle of the vehicle-mounted IMU as described in any one of claims 1 to 8.

Images