CN110879066A - Attitude calculation algorithm and device and vehicle-mounted inertial navigation system - Google Patents
Attitude calculation algorithm and device and vehicle-mounted inertial navigation system Download PDFInfo
- Publication number
- CN110879066A CN110879066A CN201911370123.3A CN201911370123A CN110879066A CN 110879066 A CN110879066 A CN 110879066A CN 201911370123 A CN201911370123 A CN 201911370123A CN 110879066 A CN110879066 A CN 110879066A
- Authority
- CN
- China
- Prior art keywords
- attitude
- current
- updating period
- quaternion
- module
- 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.)
- Pending
Links
Images
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/18—Stabilised platforms, e.g. by gyroscope
-
- 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
Abstract
The invention is suitable for the technical field of navigation, and provides an attitude calculation algorithm, an attitude calculation device and a vehicle-mounted inertial navigation system, wherein the algorithm comprises the following steps: obtaining speed related data of a carrier vehicle; calculating an equivalent rotation vector of the current attitude updating period according to the speed related data of the carrier vehicle; calculating the quaternion of the current attitude updating period according to the equivalent rotation vector of the current attitude updating period and the quaternion of the previous attitude updating period; and calculating the attitude angle of the carrier vehicle by using the quaternion of the current attitude updating period. The attitude calculation algorithm provided by the invention can control the calculated amount on the premise of ensuring the calculation precision by selecting a proper attitude updating period.
Description
Technical Field
The invention belongs to the technical field of navigation, and particularly relates to an attitude calculation algorithm, an attitude calculation device and a vehicle-mounted inertial navigation system.
Background
The inertial navigation system is an autonomous navigation system independent of external information, and information of a carrier in a navigation coordinate system is obtained by monitoring acceleration information and angular velocity information of the carrier and calculating. The core of the inertial navigation system is an attitude calculation algorithm.
At present, attitude calculation algorithms mainly used in an inertial navigation system comprise a quaternion method, an Euler angle method, a direction cosine method and the like, wherein the calculation accuracy of the quaternion method is insufficient; the Euler angle method has more trigonometric operation in the calculation process and larger calculation amount; the directional cosine method is also more computationally intensive.
Disclosure of Invention
In view of this, embodiments of the present invention provide an attitude calculation algorithm, an attitude calculation device, and a vehicle-mounted inertial navigation system, and aim to solve the problem that in the prior art, an attitude calculation algorithm of an inertial navigation system cannot give consideration to both calculation accuracy and calculation amount in a high dynamic environment.
A first aspect of an embodiment of the present invention provides an attitude calculation algorithm, including:
the method comprises the following steps: obtaining speed related data of a carrier vehicle;
step two: calculating an equivalent rotation vector of the current attitude updating period according to the speed related data of the carrier vehicle;
step three: calculating the quaternion of the current attitude updating period according to the equivalent rotation vector of the current attitude updating period and the quaternion of the previous attitude updating period;
step four: and calculating the attitude angle of the carrier vehicle by using the quaternion of the current attitude updating period.
A second aspect of the embodiments of the present invention provides an attitude resolver based on a vehicle-mounted inertial navigation system, including:
a speed data acquisition module for acquiring speed related data of the carrier vehicle;
the equivalent rotation vector calculation module is used for calculating an equivalent rotation vector of the current attitude updating period according to the speed related data of the carrier vehicle;
the quaternion updating and calculating module is used for calculating the quaternion of the current attitude updating period according to the equivalent rotating vector of the current attitude updating period and the quaternion of the previous attitude updating period;
and the attitude angle calculation module is used for calculating the attitude angle of the carrier vehicle by using the quaternion of the current attitude update period.
A third aspect of the embodiments of the present invention provides an in-vehicle terminal, including a memory, a processor, and a computer program stored in the memory and operable on the processor, where the processor executes the computer program to implement the steps of the method described above.
A fourth aspect of the embodiments of the present invention provides a computer-readable storage medium, which stores a computer program, wherein the computer program, when executed by a processor, implements the steps of the method as described above.
A fifth aspect of an embodiment of the present invention provides a vehicle-mounted inertial navigation system, including: the system comprises a sensor module, a self-checking module, a power supply module, a data transmission module and the attitude resolving device;
the self-checking module is respectively connected with the sensor module, the power supply module, the data transmission module and the attitude resolving device; the power supply module is also respectively connected with the sensor module, the data transmission module and the attitude resolving device; the sensor module is also connected with the attitude resolving device, and the attitude resolving device is also connected with the data transmission module;
the sensor module is used for acquiring speed related data of a carrier vehicle and outputting the speed related data to the attitude resolving device;
the attitude resolving device is used for resolving the attitude according to the speed related data;
the self-checking module is used for detecting whether the vehicle-mounted inertial navigation system is abnormal or not;
the power supply module is used for supplying power to the vehicle-mounted inertial navigation system;
and the data transmission module is used for realizing data transmission between the attitude resolving device and a third-party system in a normalized data packet format.
Compared with the prior art, the embodiment of the invention has the following beneficial effects:
the embodiment of the invention provides an attitude calculation algorithm, an attitude calculation device and a vehicle-mounted inertial navigation system, wherein the algorithm comprises the following steps: obtaining speed related data of a carrier vehicle; calculating an equivalent rotation vector of the current attitude updating period according to the speed related data of the carrier vehicle; calculating the quaternion of the current attitude updating period according to the equivalent rotation vector of the current attitude updating period and the quaternion of the previous attitude updating period; and calculating the attitude angle of the carrier vehicle by using the quaternion of the current attitude updating period. The attitude calculation algorithm provided by the invention can control the calculated amount on the premise of ensuring the calculation precision by selecting a proper attitude updating period.
Drawings
In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings needed to be used in the embodiments or the prior art descriptions will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without inventive exercise.
FIG. 1 is a flow chart of an attitude resolution algorithm provided by an embodiment of the present invention;
FIG. 2 is a block diagram of an attitude resolver provided in an embodiment of the present invention;
FIG. 3 is a system block diagram of a vehicle inertial navigation system provided by an embodiment of the invention;
fig. 4 is a schematic block diagram of a vehicle-mounted terminal according to an embodiment of the present invention.
Detailed Description
In the following description, for purposes of explanation and not limitation, specific details are set forth, such as particular system structures, techniques, etc. in order to provide a thorough understanding of the embodiments of the invention. It will be apparent, however, to one skilled in the art that the present invention may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present invention with unnecessary detail.
In order to explain the technical means of the present invention, the following description will be given by way of specific examples.
Referring to fig. 1, a first aspect of the embodiments of the present invention provides an attitude calculation algorithm, including:
s101: obtaining speed related data of a carrier vehicle;
s102: calculating an equivalent rotation vector of the current attitude updating period according to the speed related data of the carrier vehicle;
s103: calculating the quaternion of the current attitude updating period according to the equivalent rotation vector of the current attitude updating period and the quaternion of the previous attitude updating period;
s104: and calculating the attitude angle of the carrier vehicle by using the quaternion of the current attitude updating period.
In the present embodiment, the attitude update period is in a multiple relationship with the equivalent rotation vector calculation period.
In an embodiment of the present invention, before S102, the attitude calculation algorithm in this embodiment further includes:
carrying out error compensation on the speed related data, and carrying out zero offset correction on the speed related data after error compensation;
accordingly, S102 includes:
and calculating the equivalent rotation vector of the current attitude updating period according to the speed related data after the carrier vehicle zero offset correction.
In this embodiment, the error compensation includes zero offset compensation, scale factor compensation, perpendicularity error compensation, zero and scale factor compensation, and sensitivity compensation, and the processing of removing noise and filtering interference on data is realized.
Specifically, the compensation range of the zero position and the scale factor is-45-70 ℃, and the compensation is carried out by adopting a curve fitting method. During compensation, sampling and point taking are carried out on one point at every 10 ℃, wherein zero compensation adopts a 6 th-order function to carry out linear fitting, and scale factor compensation adopts a fourth-order function to carry out linear fitting. Optionally, curve fitting is used instead of interpolation for compensation, and since the curve obtained by interpolation includes all the provided data point coordinates, when the data error of part of the data is large, a large approximation error will result. And the fluctuation of part of sample points can be avoided by adopting curve fitting, so that a curve for describing the overall change rule of the sample can be better obtained.
Further, the curve fitting generally adopts a least square method, but because a Micro-Electro-Mechanical System (MEMS) device has poor repeatability and accuracy of data in a high and low temperature experimental sampling process, especially in a high dynamic environment, the obtained experimental fitting data has a relatively severe change. For example, due to the fact that the high-temperature and low-temperature curves cannot be well connected due to large differences, abnormal points occur at certain temperature points, and the abnormal conditions have large interference on the fitting estimation of the least square method, so that the design adopts the robust fitting. Compared with other regression methods, the robust fitting is less influenced by the abnormal value, the abnormal value can be automatically eliminated in the regression analysis, and a more robust regression coefficient is obtained. Calling a robust regression function in MATLAB to perform robust regression, and obtaining a fitting function curve more adaptive to the performance of the MEMS device aiming at the characteristics of large data change and low precision in the test process of the MEMS device, thereby better realizing the compensation of zero position and scale factor.
In this embodiment, the step of correcting the zero offset includes obtaining zero information through acceleration data and angular velocity data, and implementing real-time correction of the zero through kalman filtering.
In one embodiment of the present invention, the speed-related data includes angular speed data, and S102 includes:
performing quadratic parabolic fitting on angular velocity data on the current equivalent rotation vector calculation period according to the equivalent rotation vector differential equation to obtain a fitting equation;
and calculating the equivalent rotation vector of the current equivalent rotation vector calculation period and the equivalent rotation vector of the current attitude updating period according to the fitting equation.
In this embodiment, the sampling period of the angle increment fitting is T, the calculation period of the equivalent rotation vector is T, and the posture update period is h. In an equivalent rotation vector calculation period T corresponding to an attitude transformation quaternion, in order to reduce the drift of an attitude calculation algorithm, a multi-subsample algorithm is adopted, the more the selected subsamples are, the more accurate the calculation is, but the larger the calculation amount is. And updating an attitude change matrix between a carrier coordinate system and a navigation coordinate system caused by the attitude change of the carrier in each attitude updating period h, solving for an equivalent rotation vector with a small calculated amount, adopting high-frequency updating, solving for an attitude angle with a large calculated amount, adopting low-frequency updating, and setting the updating frequency according to the running speed of the carrier vehicle, thereby controlling the calculated amount on the basis of ensuring the calculating precision.
Alternatively, T ═ 3T, h ═ 2T.
In this embodiment, a period, i.e., a time interval [ t ] is calculated for the current equivalent rotation vector according to the differential equation of the equivalent rotation vectork-1,tk-1+T]Performing curve fitting on the angular velocity data, wherein the times of the curve are determined by the number of sub-samples of the equivalent rotating vector, and as T is 3T, the curve is the three-character equivalent rotating vector, and fitting by using quadratic parabola to obtain a fitting equation shown in formula (1);
in the formula (1), t is an angle increment fitting sampling period; t is tKFitting a sampling period for the kth angular increment; τ is a small time increment; n represents a navigation coordinate system, b represents a carrier vehicle coordinate system,
the component of the attitude transformation angle increment of the carrier vehicle relative to the navigation coordinate system in the axial direction of the carrier vehicle coordinate system at the time increment tau after the kth angle increment is fitted with the sampling period;
and
are respectively asAngular velocity fits the coefficients of the orders of the equation.
In this embodiment, the gesture update period, i.e., the time interval [ t ]k-1,tk-1+h]Equally dividing the time interval into 3 time intervals, and fitting an equation according to the angular velocity of the formula (1) to obtain an angle increment calculation equation as shown in the formula (2):
in the formula (2), the reaction mixture is,
for the angular increment in the ith time interval of the attitude update period,
and (3) the component of the attitude transformation angle increment of the carrier vehicle relative to the navigation coordinate system in the axial direction of the carrier vehicle coordinate system at the time increment tau after the k-th angle increment is fitted to the sampling period, which is shown in the formula (1).
Specifically, formula (1) is substituted for formula (2) to calculate and respectively calculate the angle increment in the ith time interval in the current attitude updating period
The following can be obtained:
obtaining an equivalent rotation vector calculation formula corresponding to the current equivalent rotation vector calculation period according to the angle increment in each interval in the current attitude update period, wherein the formula is shown as a formula (3):
in the formula (3), the reaction mixture is,
calculating an equivalent rotation vector corresponding to the period for the current equivalent rotation vector,
the angle increment in three time intervals in the current posture updating period shown in the equation (2) respectively.
Similarly, the equivalent rotation vector in two equivalent rotation vector calculation periods 2T, i.e. in one attitude update period h, is calculated
In one embodiment of the present invention, S103 includes:
constructing an attitude change quaternion corresponding to the current attitude updating period according to the equivalent rotation vector of the current attitude updating period;
and calculating the quaternion of the current attitude updating period according to the attitude change quaternion corresponding to the current attitude updating period and the quaternion of the previous attitude updating period.
In the embodiment, an attitude change quaternion corresponding to the current attitude updating period is constructed according to the equivalent rotation vector in the current attitude updating period, and a calculation formula of the attitude change quaternion is obtained and is shown as a formula (4);
in the formula (4), the reaction mixture is,
is a quaternion of the attitude change,
update week for one poseEquivalent rotation vector in period h.
According to
And quaternion of the previous attitude updating period, and a formula for calculating quaternion of the current attitude updating period is shown as formula (5):
in the formula (5), the reaction mixture is,
the quaternion for the period is updated for the current pose,
the quaternion for the previous attitude update period,
is an attitude change quaternion.
In one embodiment of the present invention, S104 includes:
determining an attitude transformation matrix of the current attitude updating period according to the quaternion of the current attitude updating period;
and determining an attitude angle corresponding to the current attitude updating period according to the attitude transformation matrix of the current attitude updating period.
In this embodiment, the attitude transformation matrix a of the current attitude update cycle is calculated from the quaternion of the current attitude update cycle as follows:
in the formula (6), AxyFor the elements with the subscript xy in the attitude transformation matrix A of the current attitude updating period, q0、q1、q2、q3Updating periodic attitude change quaternion for current attitude
Four components of (a).
In this embodiment, the attitude angles corresponding to the current attitude update period include a pitch angle θ, a yaw angle ψ, and a roll angle γ, and are calculated by equation (7) and table 1:
in the formula (7) and Table 1, θ is the pitch angle, γMaster and slaveFor calculating the intermediate value, psi, of the roll angleMaster and slaveFor calculating intermediate values of yaw angle, AxyThe elements with the subscript xy in the attitude transformation matrix a for the current attitude update period are provided by equation (6).
Where pitch is calculated directly from equation (7) and yaw and roll are calculated from the truth table provided in table 1.
TABLE 1
In one embodiment of the invention, the velocity-related data includes angular velocity data and acceleration data, and the attitude calculation algorithm further includes:
determining a rotation vector of a current navigation coordinate updating period according to the angular speed data and the acceleration data of the carrier vehicle;
determining an attitude transformation matrix of the current navigation coordinate updating period according to the rotation vector of the current navigation coordinate updating period;
calculating a transformation matrix of the carrier vehicle coordinate system and the navigation coordinate system in the current navigation coordinate updating period according to the attitude transformation matrix of the current navigation coordinate updating period;
and updating the navigation coordinate system in the current navigation coordinate updating period according to the carrier vehicle coordinate system and the conversion matrix of the navigation coordinate system in the current navigation coordinate updating period.
In this embodiment, the update period of the navigation coordinate system is D, and since the change of the navigation coordinate system is slow compared with the change of the vehicle attitude of the carrier, the update period of the navigation coordinate system D is longer than the attitude period, and several times of the attitude update period can be selected according to the vehicle running speed.
In a navigation coordinate updating period D, the equivalent rotation vector of the navigation coordinate system is calculated by the formula (8)
In formula (8), ηnEquivalent rotation vector of navigation coordinate system; delta lambda is longitude increment, delta L is latitude increment, and delta lambda and delta L are obtained by calculation of angular velocity data and acceleration data respectively; l is latitude.
Correspondingly, by jk-1Time of day navigation coordinate system to jk-1The attitude transformation matrix of the navigation coordinate system at + D time is calculated by equation (9):
in the formula (9), the reaction mixture is,
is composed of jk-1Time of day navigation coordinate system to jk-1The attitude transformation matrix of the navigation coordinate system at + D moment; delta lambda is longitude increment, delta L is latitude increment, and delta lambda and delta L are obtained by calculation of angular velocity data and acceleration data respectively; l is latitude.
jk-1+ D time carrier coordinate system to jk-1The transformation matrix of the time navigation coordinate system is calculated by the formula (10);
in the formula (10), the compound represented by the formula (10),
for calculating j from attitude quaternionk-1+ D time carrier coordinate system to jk-1The transformation matrix of the time of day navigation coordinate system,
updating the periodic attitude transformation matrix for the current attitude, AxyThe elements with the subscript xy in the attitude transformation matrix a for the current attitude update period are provided by equation (6).
At jk-1The conversion matrix for converting the + D time from the carrier coordinate system to the navigation coordinate system is calculated by equation (11):
in the formula (11), the reaction mixture is,
is at jk-1A conversion matrix for converting the carrier coordinate system to the navigation coordinate system at the + D moment;
is composed of jk-1Time of day navigation coordinate system to jk-1The attitude transformation matrix of the navigation coordinate system at the + D moment is calculated by the formula (9);
is jk-1+ D time carrier coordinate system to jk-1The transformation matrix of the time navigation coordinate system is calculated by equation (10).
And updating the navigation coordinate system by the carrier vehicle coordinate system and the conversion matrix of the navigation coordinate system.
In the embodiment of the invention, during practical application, the attitude calculation effect under the low dynamic environment is equivalent to the precision of the traditional quaternion algorithm, but under the condition that the algorithm complexity is slightly increased, the calculation precision is improved by one order of magnitude compared with that of the traditional quaternion method. The vehicle-mounted inertial navigation system provided by the embodiment of the invention is suitable for vehicles and other transportation tools in high-impact and high-dynamic environments and is used for providing high-precision navigation data. In an actual sports car test, compared with a conventional Picard method quaternion algorithm, the improved attitude calculation algorithm disclosed by the embodiment of the invention can reduce the output attitude angle error in a high dynamic environment, and meanwhile, the accuracy of the yaw angle, the pitch angle and the roll angle can be improved by one order of magnitude and is improved from about 5 degrees to about 0.5 degrees.
Referring to fig. 2, fig. 2 shows a structure of an attitude resolver 100 based on a vehicle-mounted inertial navigation system according to an embodiment of the present invention, including:
a speed data acquisition module 110 for acquiring speed related data of the carrier vehicle in real time;
the equivalent rotation vector calculation module 120 is configured to calculate an equivalent rotation vector corresponding to a current equivalent rotation vector calculation period according to speed related data of the carrier vehicle in the current equivalent rotation vector calculation period;
a quaternion updating calculation module 130, configured to update the quaternion according to the equivalent rotation vector of the current attitude updating period and the quaternion of the previous attitude updating period, where the attitude updating period is in a multiple relationship with the equivalent rotation vector calculation period;
and the attitude angle calculation module 140 is configured to determine an attitude transformation matrix by using the quaternion of the current attitude update period, and calculate an attitude angle of the carrier vehicle according to the attitude transformation matrix.
The attitude calculation device further comprises a compensation module 150, wherein the compensation module 150 is used for carrying out error compensation on the speed related data and carrying out zero offset correction on the speed related data after error compensation; the velocity-related data after the null offset correction is input to the equivalent rotation vector calculation module 120.
In one embodiment, the equivalent rotation vector calculation module 120 in fig. 2 includes:
the fitting unit is used for performing quadratic parabolic fitting on angular velocity data on the current equivalent rotation vector calculation period according to the equivalent rotation vector differential equation to obtain a fitting equation;
and the equivalent rotation vector calculation unit is used for calculating the equivalent rotation vector of the current equivalent rotation vector calculation period and the equivalent rotation vector of the current posture updating period according to the fitting equation.
In one embodiment, the quaternion update calculation module 130 in fig. 2 includes:
the attitude change quaternion constructing unit is used for constructing an attitude change quaternion corresponding to the current attitude updating period according to the equivalent rotating vector of the current attitude updating period;
and the quaternion calculating unit is used for calculating the quaternion of the current attitude updating period according to the attitude change quaternion corresponding to the current attitude updating period and the quaternion of the previous attitude updating period.
In one embodiment, the attitude angle calculation module 140 in fig. 2 includes:
the attitude transformation matrix calculation unit is used for determining an attitude transformation matrix of the current attitude updating period according to the quaternion of the current attitude updating period;
and the attitude angle calculation unit is used for determining the attitude angle corresponding to the current attitude updating period according to the attitude transformation matrix of the current attitude updating period.
In one embodiment, the attitude solver 100 further comprises:
the navigation coordinate system updating period rotating vector calculating unit is used for determining a rotating vector of a current navigation coordinate updating period according to the angular velocity data and the acceleration data of the carrier vehicle;
the attitude transformation matrix calculation unit of the navigation coordinate system updating period is used for determining the attitude transformation matrix of the current navigation coordinate updating period according to the rotating vector of the current navigation coordinate updating period;
the coordinate system transformation matrix calculation unit is used for calculating transformation matrices of the carrier vehicle coordinate system and the navigation coordinate system in the current navigation coordinate updating period according to the attitude transformation matrix of the current navigation coordinate updating period;
and the navigation coordinate system updating and calculating unit is used for updating the navigation coordinate system in the current navigation coordinate updating period according to the carrier vehicle coordinate system and the conversion matrix of the navigation coordinate system in the current navigation coordinate updating period.
Referring to fig. 3, an embodiment of the present invention provides a vehicle-mounted inertial navigation system, including: the attitude calculation device comprises a sensor module 200, a self-checking module 500, a power supply module 400, a data transmission module 300 and the attitude calculation device 100;
the self-test module 500 is respectively connected with the sensor module 200, the power module 400, the data transmission module 300 and the attitude resolver 100; the power module 400 is also connected with the sensor module 200, the data transmission module 300 and the attitude resolver 100 respectively; the sensor module 200 is also connected with the attitude calculation device 100, and the attitude calculation device 100 is also connected with the data transmission module 300;
the sensor module 200 is used for acquiring speed related data of the carrier vehicle and outputting the speed related data to the attitude calculation device 100;
the attitude calculation device 100 is used for performing attitude calculation according to the speed-related data;
the self-checking module 500 is used for detecting whether the vehicle-mounted inertial navigation system is abnormal or not;
the power module 400 is used for supplying power to the vehicle-mounted inertial navigation system;
the data transmission module 300 is configured to implement data transmission between the attitude resolver and the third-party system in a normalized data packet format.
In the present embodiment, the sensor module 200 includes a gyroscope and an accelerometer;
the gyroscope is used for detecting angular speed data of the carrier vehicle in three axial directions;
the accelerometer is used to detect acceleration data of the carrier vehicle in three axial directions.
Optionally, the number of the gyroscopes is three, and the gyroscopes include an X-axis gyroscope, a Y-axis gyroscope and a Z-axis gyroscope, and the gyroscopes are respectively used for detecting three axial angular velocity information;
optionally, the number of the accelerometers is three, and the three accelerometers are respectively used for detecting acceleration information in three axial directions;
optionally, the gyroscope is a three-axis gyroscope and is used for detecting angular velocity information of three axial directions;
optionally, the accelerometer is a three-axis accelerometer, and is configured to detect acceleration information in three axial directions.
In this embodiment, the gyroscope and the accelerometer are directly and fixedly connected to the carrier vehicle, and the outputs of the gyroscope and the accelerometer are in the forms of speed increment and angle increment so as to reduce the influence of the output noise of the gyroscope and the accelerometer on the accuracy of the system.
In this embodiment, the power module 404 is of a low power consumption dc structure, wherein when the sensor module 401 is powered, the 28V dc power is first converted into a 12V power, and then the positive 12V power is converted into a positive 5V power, so as to provide a 5V voltage bias for the sensor module 401.
In this embodiment, the functions of the self-test module 500 specifically include alarm and exception handling for sensor abnormality, operating voltage abnormality, over-range, large overload, and the like.
In this embodiment, the vehicle-mounted inertial navigation system further includes an operation and maintenance module, where the operation and maintenance module is used to implement functions such as version query, calibration data entry, and firmware burning.
In this embodiment, the vehicle-mounted inertial navigation system further includes a signal conversion module, and the signal conversion module is configured to convert the acceleration data and the angular velocity data acquired by the sensor module 200 into digital signals, and transmit the converted angular velocity data and the converted acceleration data to the attitude resolver 100.
In the present embodiment, the sensor module 200 and the signal conversion module in the vehicle-mounted inertial navigation system are fixed to the housing by screws, wherein the signal conversion module is a PCB board, and the fixing position of the screws is determined by modal analysis and simulation experiments. Meanwhile, a rubber damping part is arranged around the chip of the sensor module 200, and the hardness of the rubber material is optimized and selected through a simulation experiment. The high impact resistance of the vehicle-mounted inertial navigation system is ensured by the means.
Fig. 4 is a schematic diagram of a vehicle-mounted terminal according to an embodiment of the present invention. As shown in fig. 4, the in-vehicle terminal 40 of the embodiment includes: a processor 41, a memory 42 and a computer program 43 stored in said memory 42 and executable on said processor 41. The processor 41 implements the steps in the above-described method embodiments, such as the steps S101 to S104 shown in fig. 1, when executing the computer program 43. Alternatively, the processor 41 implements the functions of the modules/units in the above-mentioned device embodiments, such as the functions of the modules 110 to 140 shown in fig. 2, when executing the computer program 43.
Illustratively, the computer program 43 may be partitioned into one or more modules/units that are stored in the memory 42 and executed by the processor 41 to implement the present invention. The one or more modules/units may be a series of computer program instruction segments capable of performing specific functions, which are used to describe the execution process of the computer program 43 in the in-vehicle terminal 40.
The vehicle-mounted terminal 40 may be a desktop computer, a notebook, a palm computer, a cloud server, or other computing devices. The vehicle-mounted terminal may include, but is not limited to, a processor 41 and a memory 42. Those skilled in the art will appreciate that fig. 4 is merely an example of the in-vehicle terminal 40, and does not constitute a limitation of the in-vehicle terminal 40, and may include more or less components than those shown, or combine some components, or different components, for example, the in-vehicle terminal may further include an input-output device, a network access device, a bus, etc.
The Processor 41 may be a Central Processing Unit (CPU), other general purpose Processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), an off-the-shelf Programmable Gate Array (FPGA) or other Programmable logic device, discrete Gate or transistor logic, discrete hardware components, etc. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.
The storage 42 may be an internal storage unit of the in-vehicle terminal 40, such as a hard disk or a memory of the in-vehicle terminal 40. The memory 42 may also be an external storage device of the in-vehicle terminal 40, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card), and the like provided on the in-vehicle terminal 40. Further, the memory 42 may also include both an internal storage unit and an external storage device of the in-vehicle terminal 40. The memory 42 is used for storing the computer program and other programs and data required by the in-vehicle terminal. The memory 42 may also be used to temporarily store data that has been output or is to be output.
An embodiment of the present invention provides a computer-readable storage medium, in which a computer program is stored, and the computer program, when executed by a processor, implements the steps of the above method.
It will be apparent to those skilled in the art that, for convenience and brevity of description, only the above-mentioned division of the functional units and modules is illustrated, and in practical applications, the above-mentioned function distribution may be performed by different functional units and modules according to needs, that is, the internal structure of the apparatus is divided into different functional units or modules to perform all or part of the above-mentioned functions. Each functional unit and module in the embodiments may be integrated in one processing unit, or each unit may exist alone physically, or two or more units are integrated in one unit, and the integrated unit may be implemented in a form of hardware, or in a form of software functional unit. In addition, specific names of the functional units and modules are only for convenience of distinguishing from each other, and are not used for limiting the protection scope of the present application. The specific working processes of the units and modules in the system may refer to the corresponding processes in the foregoing method embodiments, and are not described herein again.
In the above embodiments, the descriptions of the respective embodiments have respective emphasis, and reference may be made to the related descriptions of other embodiments for parts that are not described or illustrated in a certain embodiment.
Those of ordinary skill in the art will appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware or combinations of computer software and electronic hardware. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
In the embodiments provided in the present invention, it should be understood that the disclosed apparatus/terminal device and method may be implemented in other ways. For example, the above-described embodiments of the apparatus/terminal device are merely illustrative, and for example, the division of the modules or units is only one logical division, and there may be other divisions when actually implemented, for example, a plurality of units or components may be combined or integrated into another system, or some features may be omitted, or not executed. In addition, the shown or discussed mutual coupling or direct coupling or communication connection may be an indirect coupling or communication connection through some interfaces, devices or units, and may be in an electrical, mechanical or other form.
The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.
In addition, functional units in the embodiments of the present invention may be integrated into one processing unit, or each unit may exist alone physically, or two or more units are integrated into one unit. The integrated unit can be realized in a form of hardware, and can also be realized in a form of a software functional unit.
The integrated modules/units, if implemented in the form of software functional units and sold or used as separate products, may be stored in a computer readable storage medium. Based on such understanding, all or part of the flow of the method according to the embodiments of the present invention may also be implemented by a computer program, which may be stored in a computer-readable storage medium, and when the computer program is executed by a processor, the steps of the method embodiments may be implemented. . Wherein the computer program comprises computer program code, which may be in the form of source code, object code, an executable file or some intermediate form, etc. The computer-readable medium may include: any entity or device capable of carrying the computer program code, recording medium, usb disk, removable hard disk, magnetic disk, optical disk, computer Memory, Read-Only Memory (ROM), Random Access Memory (RAM), electrical carrier wave signals, telecommunications signals, software distribution medium, and the like. It should be noted that the computer readable medium may contain content that is subject to appropriate increase or decrease as required by legislation and patent practice in jurisdictions, for example, in some jurisdictions, computer readable media does not include electrical carrier signals and telecommunications signals as is required by legislation and patent practice.
The above-mentioned embodiments are only used for illustrating the technical solutions of the present invention, and not for limiting the same; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; such modifications and substitutions do not substantially depart from the spirit and scope of the embodiments of the present invention, and are intended to be included within the scope of the present invention.
Claims (10)
1. An attitude resolution algorithm, comprising:
the method comprises the following steps: obtaining speed related data of a carrier vehicle;
step two: calculating an equivalent rotation vector of the current attitude updating period according to the speed related data of the carrier vehicle;
step three: calculating the quaternion of the current attitude updating period according to the equivalent rotation vector of the current attitude updating period and the quaternion of the previous attitude updating period;
step four: and calculating the attitude angle of the carrier vehicle by using the quaternion of the current attitude updating period.
2. The attitude calculation algorithm of claim 1, wherein prior to step two, the attitude calculation algorithm further comprises:
carrying out error compensation on the speed related data, and carrying out zero offset correction on the speed related data after the error compensation;
correspondingly, the second step comprises the following steps:
and calculating the equivalent rotation vector of the current attitude updating period according to the speed related data after the carrier vehicle zero offset correction.
3. The attitude calculation algorithm of claim 1 wherein the velocity-related data comprises angular velocity data, and wherein step two comprises:
performing quadratic parabolic fitting on angular velocity data on the current equivalent rotation vector calculation period according to the equivalent rotation vector differential equation to obtain a fitting equation;
and calculating the equivalent rotation vector of the current equivalent rotation vector calculation period and the equivalent rotation vector of the current attitude updating period according to the fitting equation.
4. The attitude calculation algorithm of claim 1, wherein step three comprises:
constructing an attitude change quaternion corresponding to the current attitude updating period according to the equivalent rotation vector of the current attitude updating period;
and calculating the quaternion of the current attitude updating period according to the attitude change quaternion corresponding to the current attitude updating period and the quaternion of the previous attitude updating period.
5. The attitude calculation algorithm of claim 1, wherein step four comprises:
determining an attitude transformation matrix of the current attitude updating period according to the quaternion of the current attitude updating period;
and determining an attitude angle corresponding to the current attitude updating period according to the attitude transformation matrix of the current attitude updating period.
6. The attitude calculation algorithm of claim 1 wherein the velocity-related data includes angular velocity data and acceleration data, the attitude calculation algorithm further comprising:
determining a rotation vector of a current navigation coordinate updating period according to the angular speed data and the acceleration data of the carrier vehicle;
determining an attitude transformation matrix of the current navigation coordinate updating period according to the rotation vector of the current navigation coordinate updating period;
calculating a transformation matrix of a carrier vehicle coordinate system and a navigation coordinate system in the current navigation coordinate updating period according to the attitude transformation matrix of the current navigation coordinate updating period;
and updating the navigation coordinate system in the current navigation coordinate updating period according to the carrier vehicle coordinate system and the conversion matrix of the navigation coordinate system in the current navigation coordinate updating period.
7. An attitude resolver based on a vehicle-mounted inertial navigation system, comprising:
a speed data acquisition module for acquiring speed related data of the carrier vehicle;
the equivalent rotation vector calculation module is used for calculating an equivalent rotation vector of the current attitude updating period according to the speed related data of the carrier vehicle;
the quaternion updating and calculating module is used for calculating the quaternion of the current attitude updating period according to the equivalent rotating vector of the current attitude updating period and the quaternion of the previous attitude updating period;
and the attitude angle calculation module is used for calculating the attitude angle of the carrier vehicle by using the quaternion of the current attitude update period.
8. An in-vehicle terminal comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that the processor implements the steps of the attitude calculation algorithm according to any one of claims 1 to 6 when executing the computer program.
9. A computer-readable storage medium storing a computer program, wherein the computer program when executed by a processor implements the steps of the attitude calculation algorithm of any of claims 1 to 6.
10. An in-vehicle inertial navigation system, comprising: the attitude calculation device comprises a sensor module, a self-test module, a power supply module, a data transmission module and the attitude calculation device as claimed in claim 7;
the self-checking module is respectively connected with the sensor module, the power supply module, the data transmission module and the attitude resolving device; the power supply module is also respectively connected with the sensor module, the data transmission module and the attitude resolving device; the sensor module is also connected with the attitude resolving device, and the attitude resolving device is also connected with the data transmission module;
the sensor module is used for acquiring speed related data of a carrier vehicle and outputting the speed related data to the attitude resolving device;
the attitude resolving device is used for resolving the attitude according to the speed related data;
the self-checking module is used for detecting whether the vehicle-mounted inertial navigation system is abnormal or not;
the power supply module is used for supplying power to the vehicle-mounted inertial navigation system;
and the data transmission module is used for realizing data transmission between the attitude resolving device and a third-party system in a normalized data packet format.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911370123.3A CN110879066A (en) | 2019-12-26 | 2019-12-26 | Attitude calculation algorithm and device and vehicle-mounted inertial navigation system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911370123.3A CN110879066A (en) | 2019-12-26 | 2019-12-26 | Attitude calculation algorithm and device and vehicle-mounted inertial navigation system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110879066A true CN110879066A (en) | 2020-03-13 |
Family
ID=69731559
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911370123.3A Pending CN110879066A (en) | 2019-12-26 | 2019-12-26 | Attitude calculation algorithm and device and vehicle-mounted inertial navigation system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110879066A (en) |
Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6219616B1 (en) * | 1998-04-23 | 2001-04-17 | Litton Systems Inc. | Method and apparatus for obtaining attitude updates in a strapdown inertial navigation system |
| CN101290229A (en) * | 2008-06-13 | 2008-10-22 | 哈尔滨工程大学 | Silicon micro-navigation attitude system inertia/geomagnetism assembled method |
| CN101294811A (en) * | 2008-05-29 | 2008-10-29 | 北京航空航天大学 | Strapdown inertial navigation system adopting strange perturbation method for taper cone error and rowing error compensation |
| CN102095424A (en) * | 2010-12-06 | 2011-06-15 | 国营红峰机械厂 | Attitude measuring method suitable for vehicle fiber AHRS (Attitude and Heading Reference System) |
| CN102506871A (en) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | Airborne double-fiber IMU (inertial measurement unit)/DGPS (differential global positioning system) integrated relative deformation attitude measurement device |
| CN103558415A (en) * | 2013-11-19 | 2014-02-05 | 中国兵器工业集团第二一四研究所苏州研发中心 | MEMS accelerometer with temperature compensation function |
| CN103759731A (en) * | 2014-01-16 | 2014-04-30 | 电子科技大学 | Single-subsample rotating vector posture method under angular rate input condition |
| CN104501819A (en) * | 2014-12-24 | 2015-04-08 | 滨州学院 | Vehicular inertial navigation system |
| CN105444764A (en) * | 2015-11-24 | 2016-03-30 | 大连楼兰科技股份有限公司 | Attitude measurement method based on assistance of speedometer of vehicle |
| CN105571590A (en) * | 2014-10-13 | 2016-05-11 | 北京自动化控制设备研究所 | Fusion compensation method |
| CN108873042A (en) * | 2018-06-13 | 2018-11-23 | 深圳市腾兴车联科技有限公司 | Vehicle intelligent terminal system and the method that planning driving path is recorded based on inertial navigation algorithm |
| CN109001787A (en) * | 2018-05-25 | 2018-12-14 | 北京大学深圳研究生院 | A kind of method and its merge sensor of solving of attitude and positioning |
| CN109000612A (en) * | 2018-06-19 | 2018-12-14 | 深圳市道通智能航空技术有限公司 | Angle evaluation method, device, camera assembly and the aircraft of equipment |
| CN110440797A (en) * | 2019-08-28 | 2019-11-12 | 广州小鹏汽车科技有限公司 | Vehicle attitude estimation method and system |
-
2019
- 2019-12-26 CN CN201911370123.3A patent/CN110879066A/en active Pending
Patent Citations (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6219616B1 (en) * | 1998-04-23 | 2001-04-17 | Litton Systems Inc. | Method and apparatus for obtaining attitude updates in a strapdown inertial navigation system |
| CN101294811A (en) * | 2008-05-29 | 2008-10-29 | 北京航空航天大学 | Strapdown inertial navigation system adopting strange perturbation method for taper cone error and rowing error compensation |
| CN101290229A (en) * | 2008-06-13 | 2008-10-22 | 哈尔滨工程大学 | Silicon micro-navigation attitude system inertia/geomagnetism assembled method |
| CN102095424A (en) * | 2010-12-06 | 2011-06-15 | 国营红峰机械厂 | Attitude measuring method suitable for vehicle fiber AHRS (Attitude and Heading Reference System) |
| CN102506871A (en) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | Airborne double-fiber IMU (inertial measurement unit)/DGPS (differential global positioning system) integrated relative deformation attitude measurement device |
| CN103558415A (en) * | 2013-11-19 | 2014-02-05 | 中国兵器工业集团第二一四研究所苏州研发中心 | MEMS accelerometer with temperature compensation function |
| CN103759731A (en) * | 2014-01-16 | 2014-04-30 | 电子科技大学 | Single-subsample rotating vector posture method under angular rate input condition |
| CN105571590A (en) * | 2014-10-13 | 2016-05-11 | 北京自动化控制设备研究所 | Fusion compensation method |
| CN104501819A (en) * | 2014-12-24 | 2015-04-08 | 滨州学院 | Vehicular inertial navigation system |
| CN105444764A (en) * | 2015-11-24 | 2016-03-30 | 大连楼兰科技股份有限公司 | Attitude measurement method based on assistance of speedometer of vehicle |
| CN109001787A (en) * | 2018-05-25 | 2018-12-14 | 北京大学深圳研究生院 | A kind of method and its merge sensor of solving of attitude and positioning |
| CN108873042A (en) * | 2018-06-13 | 2018-11-23 | 深圳市腾兴车联科技有限公司 | Vehicle intelligent terminal system and the method that planning driving path is recorded based on inertial navigation algorithm |
| CN109000612A (en) * | 2018-06-19 | 2018-12-14 | 深圳市道通智能航空技术有限公司 | Angle evaluation method, device, camera assembly and the aircraft of equipment |
| CN110440797A (en) * | 2019-08-28 | 2019-11-12 | 广州小鹏汽车科技有限公司 | Vehicle attitude estimation method and system |
Non-Patent Citations (2)
| Title |
|---|
| 谢中华，等: "《新编MATLAB/Simulink自学一本通》", 31 January 2018, 北京航空航天大学出版社 * |
| 赵欣，王仕成，等: "一种改进的高动态捷联惯导解算算法", 《中国惯性技术学报》 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109163721B (en) | Attitude measurement method and terminal equipment | |
| Batista et al. | Accelerometer calibration and dynamic bias and gravity estimation: Analysis, design, and experimental evaluation | |
| CN109030867B (en) | Method and apparatus for calculating angular velocity using acceleration sensor and geomagnetic sensor | |
| CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
| CN112577521B (en) | Combined navigation error calibration method and electronic equipment | |
| CN111415387B (en) | Camera pose determining method and device, electronic equipment and storage medium | |
| CN109443349A (en) | A kind of posture Course Measure System and its fusion method, storage medium | |
| CN110986988B (en) | Track calculation method, medium, terminal and device integrating multi-sensor data | |
| US20230366680A1 (en) | Initialization method, device, medium and electronic equipment of integrated navigation system | |
| CN110440827B (en) | Parameter error calibration method and device and storage medium | |
| WO2022174831A1 (en) | Continuous north-seeking apparatus and method, electronic device, and storage medium | |
| CN112744313A (en) | Robot state estimation method and device, readable storage medium and robot | |
| KR20070043009A (en) | Method and apparatus for calibrating the rotational relationship between two motion sensors of a sensor system | |
| CN113551690A (en) | Calibration parameter acquisition method and device, electronic equipment and storage medium | |
| US11150090B2 (en) | Machine learning zero-rate level calibration | |
| CN110595434A (en) | Quaternion fusion attitude estimation method based on MEMS sensor | |
| CN112731320A (en) | Method, device and equipment for estimating error data of vehicle-mounted radar and storage medium | |
| CN110940336B (en) | Strapdown inertial navigation simulation positioning resolving method and device and terminal equipment | |
| CN111998870B (en) | Calibration method and device of camera inertial navigation system | |
| CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
| CN110160530B (en) | Spacecraft attitude filtering method based on quaternion | |
| Hajdu et al. | Complementary filter based sensor fusion on FPGA platforms | |
| CN110879066A (en) | Attitude calculation algorithm and device and vehicle-mounted inertial navigation system | |
| Milovzorov et al. | Mathematical models of inclination parameter converters based on double-axis accelerometers | |
| CN115839726B (en) | Method, system and medium for jointly calibrating magnetic sensor and angular velocity sensor |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20200313 |