An iterative filtering based coagulation system alignment method, device and storage medium

Abstract

The application provides a coagulation system alignment method and device based on iterative filtering and a storage medium. The method comprises the following steps: collecting the earth rotation angular velocity, the local latitude, the local gravity acceleration value, the gyro signal data and the accelerometer signal data of an inertial navigation system in real time; calculating the initial conversion matrix Ci0_n(t), the inertial system second integral vector VG2_i0(t) of the gravity acceleration, the carrier system second integral vector VF2_ib0(t) of the accelerometer and the intermediate conversion matrix Cb_ib0(t) based on the collected data; calculating the Euler angle tlt(t) in real time according to the obtained VG2_i0(t) and VF2_ib0(t); calculating the updated conversion matrix Cib0_i0(t) based on the Euler angle tlt(t); and finally calculating the obtained Ci0_n(t), Cb_ib0(t) and Cib0_i0(t) to obtain the attitude matrix, so as to obtain the pitch angle, the roll angle and the heading angle information of the target object and complete the alignment. In addition, the application further provides a coagulation system alignment device based on iterative filtering and a storage medium. The application can not only store a large amount of intermediate data, but also can calculate the attitude angle in real time, and is simple and efficient to implement.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of inertial measurement, and more particularly, relates to a coagulation system alignment method based on iterative filtering, a device and a storage medium. BACKGROUND

[0002] Coagulation system alignment can effectively isolate environmental disturbances of angular motion and is a commonly used alignment method for inertial navigation. Currently, inertial navigation products have higher and higher requirements for rapidity, and sometimes need to switch to navigation at any time during alignment. The conventional coagulation system alignment method generally uses a double-vector attitude determination method to calculate a coagulation matrix. However, the double-vector attitude determination method generally selects two vectors at the end of alignment and a certain point in the middle of alignment, and the effect is best at half of the alignment time.

[0003] However, this method still has some defects. When used in a special environment, it needs to switch to navigation at any time, especially in the case of long-time alignment. Not only is it necessary to store multiple intermediate point data, but also it is impossible to calculate the attitude angle in real time. When there is a sudden state, the attitude angle can only be calculated after alignment is completed, which leads to inconvenience in use. SUMMARY

[0004] In view of at least one defect or improvement requirement of the prior art, the present application provides a coagulation system alignment method based on iterative filtering, a device and a storage medium. The coagulation system alignment method based on iterative filtering does not need to store a large amount of intermediate data, and can calculate the attitude angle in real time, which is simple and efficient to implement.

[0005] To achieve the above purpose, according to a first aspect of the present application, a coagulation system alignment method based on iterative filtering is provided, which comprises:

[0006] real-time collection of earth rotation angular velocity, local latitude, local gravity acceleration value, gyro information data and accelerometer information data of an inertial navigation system;

[0007] calculating an initial conversion matrix Cio_n(t) at a coagulation time based on the earth rotation angular velocity and the local latitude;

[0008] real-time iterative calculation of an inertial system second integral vector VG2_i0(t) of gravity acceleration at the coagulation time based on the earth rotation angular velocity, the local latitude and the local gravity acceleration value;

[0009] real-time iterative calculation of a carrier system second integral vector VF2_ib0(t) of an accelerometer at the coagulation time based on the gyro information data and the accelerometer information data of the inertial navigation system;

[0010] calculating an intermediate conversion matrix Cb_ib0(t) based on the gyro information data of the inertial navigation system in real time;

[0011] calculating Euler angles tlt(t) based on the inertial system second integral vector VG2_i0(t) and the carrier system second integral vector VF2_ib0(t) in real time;

[0012] calculating an updated conversion matrix Cib0_i0(t) based on the Euler angles tlt(t) in real time;

[0013] calculating a posture matrix Cb_n(t) based on the initial conversion matrix Ci0_n(t), the intermediate conversion matrix Cb_ib0(t) and the updated conversion matrix Cib0_i0(t) in real time;

[0014] calculating pitch(t), roll(t) and yaw(t) information of the target object based on the posture matrix Cb_n(t) in real time to complete alignment.

[0015] Further, the calculation formula of the initial conversion matrix Ci0_n(t) is:

[0016]

[0017] Wherein, sint=sin(Wie×T); cost=cos(Wie×T); sinL=sin(Lat); cosL=cos(Lat); Wie represents the earth rotation angular velocity, which is 7.292115e-5; Lat represents the local latitude; T represents the cumulative time, T=T+Ts, the initial value of T is set to 0, t represents the iteration solving time, and Ts represents the solving period.

[0018] Further, the iteration calculation process of the inertial system second integral vector VG2_i0(t) is:

[0019] S21: calculating the value of the inertial system component Gi0(t) of the gravitational acceleration at the solidification time; wherein, the inertial system component g represents the local gravitational acceleration value;

[0020] S22: calculating the value of the inertial system first integral vector VG_i0(t) of the gravitational acceleration at the solidification time; wherein, the inertial system first integral vector VG_i0(t)=VG_i0(t)+Gi0(t)×Ts, the initial value of the inertial system first integral vector VG_i0(t) is set to

[0021] S23: calculating the value of the inertial system second integral vector VG2_i0(t); wherein the inertial system second integral vector VG2_i0(t) = VG2_i0(t) + VG_i0(t) x Ts;

[0022] S24: repeating S21, S22 and S23 in turn to obtain the inertial system second integral vector VG2_i0(t) of the gravity acceleration at the solidification time through continuous iteration calculation.

[0023] Further, the iteration calculation process of the inertial system second integral vector VF2_ib0(t) is:

[0024] S31: calculating the value of the carrier system component Fib0(t) of the accelerometer at the solidification time; wherein the carrier system component Fib0(t) = Fib0(t) + Cb_ib0(t) x Acc(t); The initial value of the intermediate conversion matrix Cb_ib0(t) is set as The Acc(t) represents the accelerometer information data, and the accelerometer information data The Gyro(t) represents the gyro information data of the inertial navigation system, and the gyro information data of the inertial navigation system

[0025] S32: calculating the value of the carrier system first integral vector VF_ib0(t) of the accelerometer at the solidification time; wherein the carrier system first integral vector VF_ib0(t) = VF_ib0(t) + Fib0(t), and the initial value of the carrier system first integral vector VF_ib0(t) is set as

[0026] S33: calculating the value of the carrier system second integral vector VF2_ib0(t); wherein the carrier system second integral vector VF2_ib0(t) = VF2_ib0(t) + VF_ib0(t) x Ts;

[0027] S34: repeating S31, S32 and S33 in turn to obtain the carrier system second integral vector VF2_ib0(t) of the accelerometer at the solidification time through continuous iteration calculation.

[0028] Further, the iteration calculation process of the intermediate conversion matrix Cb_ib0(t) is:

[0029] S41: calculating the value of the norm0(t) of the gyro information data of the inertial navigation system; wherein the norm0(t) = norm0(t) + Gyro(t) x Ts;

[0030]

[0031] S42: calculating the value of the rotation vector qb(t) of the gyro information data of the inertial navigation system; wherein the rotation vector qb(t) = qb(t) + Gyro(t) x Ts; Where qb(t)_i represents the i-th term of the rotation vector qb(t), where i = 1, 2, 3, 4;

[0032] S43: Calculate the value of the quaternion Q_ib0(t) of the gyroscope information data of the inertial navigation system; wherein, the quaternion Q_ib0(t-1)_i represents the i-th term of Q_ib0(t-1), where i = 1, 2, 3, 4; Q_ib0(t-1) represents the quaternion Q_ib0(t) at the time of the previous calculation cycle, and the initial value of the quaternion Q_ib0(t) is set to...

[0033] S44: Calculate the intermediate transformation matrix Cb_ib0(t); wherein the formula for calculating the intermediate transformation matrix Cb_ib0(t) is:

[0034]

[0035] Qij represents the i-th term of the quaternion Q_ib0(t) multiplied by the j-th term of the quaternion Q_ib0(t);

[0036] S45: Repeat S41, S42, S43 and S44 in sequence to iteratively calculate and obtain the intermediate transformation matrix Cb_ib0(t).

[0037] Furthermore, the iterative calculation process for the Euler angles is as follows:

[0038] S51: Calculate the value of the measurement vector Z(t); wherein, the measurement vector Z(t) = VG2_i0(t) - Cib0_i0(t-1) × VF2_ib0(t), and the initial value of the update transformation matrix Cib0_i0(t) is set to...

[0039] S52: Calculate the value of the inertial frame projection Vib0_i0(t) of the second integral vector VF2_ib0(t) of the carrying system at the solidification time; wherein, the inertial frame projection Vib0_i0(t) = Cib0_i0(t-1) × VF2_ib0(t);

[0040] S53: Calculate the value of the measurement matrix H(t); wherein, the measurement matrix Vib0_i0(t)_i represents the i-th term of the inertial frame projection Vib0_i0(t), where i = 1, 2, 3;

[0041] S54: Calculate the value of the gain K(t); where the gain K(t) = P(t) × H(t) TH (t) x P (t) x H (t) T + R) -1; the P (t) represents a variance matrix, the variance matrix The initial value of the variance matrix P (t) is set as The H (t) T is a transpose matrix of the measurement matrix H (t); R represents a noise matrix, the noise matrix (H (t) x P (t) x H (t) T + R) -1 represents an inverse operation of H (t) x P (t) x H (t) T + R;

[0042] S55: Calculate the value of the Euler angle tlt (t) ; wherein, the Euler angle tlt (t) = K (t) x (Z (t) - H (t) x tlt (t-1) ), the initial value of the Euler angle tlt (t) is set as

[0043] S56: Repeat S51, S52, S53, S54 and S55 in turn to continuously iterate to obtain the Euler angle tlt (t).

[0044] Further, the iterative calculation process of the updated conversion matrix Cib0_i0 (t) is:

[0045] S61: Calculate the value of the intermediate variable norml (t) ; wherein, the intermediate variable norml (t) = tlt_1 (t) 2 + tlt_2 (t) 2 + tlt_3 (t) 2 ;

[0046] S62: Calculate the value of the intermediate coefficient a (t) and the intermediate coefficient b (t) ; wherein, the intermediate coefficient The intermediate coefficient

[0047] S63: Calculate the value of the updated conversion matrix Cib0_i0 (t) ; wherein, the calculation formula of the updated conversion matrix Cib0_i0 (t) is:

[0048] Cib0_i0 (t) = dlt (t) x Cib0_i0 (t-1) ;

[0049] The tlt (t) i represents the i-th term of tlt (t), at this time i = 1, 2, 3;

[0050] S64: Repeat S61, S62 and S63 in turn to continuously iterate to obtain the updated conversion matrix Cib0_i0 (t).

[0051] Further, the calculation formula of the attitude matrix Cb_n(t) is: Cb_n(t)=C10_n(t)xC10_i0(t)xCb_ib0(t);

[0052] The calculation formula of the pitch angle is: pitch(t)=arcsin(Cb_n(t)_32);

[0053] The calculation formula of the roll angle is:

[0054] The calculation formula of the heading angle is:

[0055] Wherein, Cb_n(t)_ij represents the i-th row and the j-th column of the attitude matrix Cb_n(t).

[0056] According to the second aspect of the present application, there is also provided a coagulation system alignment device based on iterative filtering, comprising at least one processing unit and at least one storage unit, wherein the storage unit stores a computer program which, when executed by the processing unit, causes the processing unit to perform the steps of any of the above-mentioned methods.

[0057] According to the third aspect of the present application, there is also provided a coagulation system alignment storage medium based on iterative filtering, which stores a computer program executable by an access authentication device, which, when running on the access authentication device, causes the access authentication device to perform the steps of any of the above-mentioned methods.

[0058] Overall, compared with the prior art, the above technical solutions conceived by the present application can achieve the following beneficial effects: the coagulation system alignment method, device and storage medium based on iterative filtering provided by the present application obtain the inertial system second integral vector and the carrier system second integral vector at the coagulation moment by using the iterative filtering method, iteratively calculate the Euler angle based on the two second integral vectors, and thus obtain the update conversion matrix from the carrier coordinate system at the coagulation moment to the inertial coordinate system at the coagulation moment; in addition, the initial conversion matrix of the navigation system at the coagulation moment and the intermediate conversion matrix from the carrier system to the carrier system at the coagulation moment are obtained through the collected data, the conversion matrices under different systems are used to obtain the attitude matrix, and the attitude angle is finally calculated in real time. The present application can not only calculate the attitude angle in real time, but also can directly overwrite the previous data when new data is collected, without storing multiple intermediate point data and occupying additional resources; moreover, the present application is suitable for alignment state in special situations where navigation needs to be switched at any time, is simple and ingenious, more intelligent, more practical, meets actual needs, and can be widely used. BRIEF DESCRIPTION OF DRAWINGS

[0059] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings required in the embodiments. Obviously, the drawings described below only illustrate some of the embodiments of the present application, and for those skilled in the art, other drawings can also be obtained from these drawings without creative labor.

[0060] Figure 1 A method flowchart of a coagulation system alignment method, device and storage medium based on iterative filtering provided by the present application. DETAILED DESCRIPTION

[0061] In order to make the objects, technical solutions and advantages of the present application clearer, the following will further describe the present application in combination with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application, and are not used to limit the present application. In addition, the technical features involved in each embodiment of the present application described below can be combined with each other as long as they do not conflict with each other.

[0062] The terms "include" and "have" and any variations thereof in the specification and claims of the present application and the above drawings are intended to cover the non-exclusive inclusion. For example, a process, method, system, product or device including a series of steps or units is not limited to the listed steps or units, but can optionally include steps or units not listed, or can optionally include other steps or units inherent to the process, method, product or device.

[0063] Figure 1 A method flowchart of a coagulation system alignment method, device and storage medium based on iterative filtering provided by the present application. This application not only does not need to store a large amount of intermediate data, but also can calculate the attitude angle in real time, and is simple and efficient to implement.

[0064] The local geographic position is taken as the navigation coordinate system; the target carrier is taken as the carrier coordinate system; the inertial navigation system is taken as the inertial coordinate system; the three axes of the carrier coordinate system are defined as right-front-up, and the rotation order is 3-1-2.

[0065] The earth rotation angular velocity Wie in the navigation coordinate system, the local latitude Lat, the local gravity acceleration value g, the gyro information data of the inertial navigation system and the accelerometer information data are collected in real time. The earth rotation angular velocity Wie in the navigation coordinate system, the local latitude Lat, the local gravity acceleration value g, the gyro information data of the inertial navigation system and the accelerometer information data are collected in real time. The earth rotation angular velocity Wie is 7.292115e-5.

[0066] Firstly, the initial conversion matrix Ci0_n(t) in the navigation coordinate system at the solidification time, the inertial system second integral vector VG2_i0(t) of the gravity acceleration in the inertial coordinate system at the solidification time, the carrier system second integral vector VF2_ib0(t) of the accelerometer in the carrier coordinate system at the solidification time, and the intermediate conversion matrix Cb_ib0(t) from the carrier coordinate system to the carrier coordinate system at the solidification time are calculated based on the collected data.

[0067] Specifically, the initial conversion matrix Ci0_n(t) in the navigation coordinate system at the solidification time is calculated based on the earth rotation angular velocity and the local latitude. As an embodiment of the present application, the calculation formula of the initial conversion matrix Ci0_n(t) is:

[0068]

[0069] Wherein, sint=sin(Wie×T); cost=cos(Wie×T); sinL=sin(Lat); cosL=cos(Lat); Wie represents the earth rotation angular velocity, which is 7.292115e-5; Lat represents the local latitude; T represents the cumulative time, T=T+Ts, the initial value of T is set to 0, t represents the iteration solving time, and Ts represents the solving period.

[0070] The inertial system second integral vector VG2_i0(t) of the gravity acceleration in the inertial coordinate system at the solidification time is calculated based on the earth rotation angular velocity, the local latitude and the local gravity acceleration value.

[0071] As an embodiment of the present application, the iteration calculation process of the inertial system second integral vector VG2_i0(t) is: firstly, the value of the inertial system component Gi0(t) of the gravity acceleration in the inertial system at the solidification time is calculated; then the value of the inertial system first integral vector VG_i0(t) of the gravity acceleration in the inertial system at the solidification time is calculated; finally, the value of the inertial system second integral vector VG2_i0(t) is calculated. It should be noted that the values of the inertial system component Gi0(t), the inertial system first integral vector VG_i0(t) and the inertial system second integral vector VG2_i0(t) are repeatedly calculated in sequence every time new data is collected, and the final inertial system second integral vector VG2_i0(t) of the gravity acceleration in the inertial coordinate system at the solidification time is obtained through continuous iteration calculation.

[0072] Wherein, the inertial system component g represents the local gravity acceleration value; the inertial system first integral vector VG_i0(t)=VG_i0(t)+Gi0(t)×Ts, the initial value of the inertial system first integral vector VG_i0(t) is set to Inertial system second integral vector VG2_i0(t) = VG2_i0(t) + VG_i0(t) x Ts.

[0073] Based on the gyro information data and the accelerometer information data of the inertial navigation system, the accelerometer body second integral vector VF2_ib0(t) in the carrier coordinate system at the solidification time is calculated in real time through iteration.

[0074] As an embodiment of the present application, the iteration calculation process of the accelerometer body second integral vector VF2_ib0(t) is as follows: first, the value of the accelerometer body component Fib0(t) in the carrier coordinate system at the solidification time is calculated, then the value of the accelerometer body first integral vector VF_ib0(t) in the carrier coordinate system at the solidification time is calculated, and finally the value of the accelerometer body second integral vector VF2_ib0(t) is calculated. It should be noted that the values of the accelerometer body component Fib0(t), the accelerometer body first integral vector VF_ib0(t) and the accelerometer body second integral vector VF2_ib0(t) are repeatedly calculated in sequence whenever new data is collected, and the final accelerometer body second integral vector VF2_ib0(t) in the carrier coordinate system at the solidification time is obtained through continuous iteration calculation.

[0075] The accelerometer body component Fib0(t) is calculated as follows: The initial value of the intermediate conversion matrix Cb_ib0(t) is set as follows: Acc(t) represents the accelerometer information data, and the accelerometer information data Acc(t) is calculated as follows: Gyro(t) represents the gyro information data of the inertial navigation system, and the gyro information data Gyro(t) is calculated as follows: The accelerometer body first integral vector VF_ib0(t) = VF_ib0(t) + Fib0(t), and the initial value of the accelerometer body first integral vector VF_ib0(t) is set as follows: The accelerometer body second integral vector VF2_ib0(t) = VF2_ib0(t) + VF_ib0(t) x Ts.

[0076] Based on the gyro information data of the inertial navigation system, the intermediate conversion matrix Cb_ib0(t) from the carrier coordinate system to the carrier coordinate system at the solidification time is calculated in real time through iteration.

[0077] As an embodiment of the present application, the iterative calculation process of the intermediate conversion matrix Cb_ib0(t) is as follows: first, the value of the norm of the gyro information data of the inertial navigation system norm0(t) is calculated, then the value of the rotation vector qb(t) of the gyro information data of the inertial navigation system is calculated, then the value of the quaternion Q_ib0(t) of the gyro information data of the inertial navigation system is calculated, and finally the value of the intermediate conversion matrix Cb_ib0(t) is calculated. It should be noted that the values of the norm norm0(t), the rotation vector qb(t), the quaternion Q_ib0(t) and the intermediate conversion matrix Cb_ib0(t) are repeatedly calculated in sequence whenever new data is collected, and the intermediate conversion matrix Cb_ib0(t) from the carrier coordinate system to the carrier coordinate system at the solidification time is obtained through continuous iterative calculation.

[0078] wherein the norm norm0(t) is calculated as follows: The rotation vector qb(t) is calculated as follows: qb(t)_i represents the ith term of the rotation vector qb(t), and i = 1, 2, 3, 4 at this time; the quaternion Q_ib0(t) is calculated as follows:

[0079] Q_ib0(t-1)_i represents the ith term of Q_ib0(t-1), and i = 1, 2, 3, 4 at this time; Q_ib0(t-1) represents the quaternion Q_ib0(t) at the previous calculation period, and the initial value of the quaternion Q_ib0(t) is set as follows:

[0080] It should be noted that the calculation formula of the intermediate conversion matrix Cb_ib0(t) is as follows:

[0081]

[0082] wherein Qij represents the ith term of the quaternion Q_ib0(t) multiplied by the jth term of the quaternion Q_ib0(t);

[0083] Then, the Euler angle tlt(t) is obtained through real-time iterative calculation according to the inertial system second integral vector VG2_i0(t) of the gravity acceleration in the inertial coordinate system at the solidification time and the carrier system second integral vector VF2_ib0(t) of the accelerometer in the carrier coordinate system at the solidification time; and the update conversion matrix Cib0_i0(t) from the carrier coordinate system at the solidification time to the inertial coordinate system at the solidification time is obtained through real-time iterative calculation according to the Euler angle tlt(t).

[0084] As an embodiment of the present application, the iterative calculation process of the Euler angle is as follows: firstly, the value of the measurement vector Z(t) is calculated, then the value of the inertial system projection Vib0_i0(t) of the carrier system second integral vector VF2_ib0(t) of the accelerometer in the carrier coordinate system at the solidification moment in the inertial coordinate system at the solidification moment is calculated, then the values of the measurement matrix H(t) and the gain K(t) are calculated, and finally the value of the Euler angle tlt(t) is calculated. It should be noted that the values of the measurement vector Z(t), the inertial system projection Vib0_i0(t), the measurement matrix H(t), the gain K(t) and the Euler angle tlt(t) are sequentially repeated for calculation whenever new data is collected, and the Euler angle tlt(t) at the calculation moment is obtained through continuous iterative calculation.

[0085] wherein the measurement vector Z(t) = VG2_i0(t) - Cib0_i0(t-1) x VF2_ib0(t), the initial value of the conversion matrix Cib0_i0(t) is set to the inertial system projection Vib0_i0(t) = Cib0_i0(t-1) x VF2_ib0(t); the measurement matrix Vib0_i0(t)_i represents the i-th term of the inertial system projection Vib0_i0(t), at this time i = 1, 2, 3; the gain K(t) = P(t) x H(t) T x (H(t) x P(t) x H(t) T + R)^-1; P(t) represents a variance matrix, the variance matrix The initial value of the variance matrix P(t) is set to H(t) T is the transpose matrix of the measurement matrix H(t); R represents a noise matrix, the noise matrix (H(t) x P(t) x H(t) T + R) is an inverse operation of H(t) x P(t) x H(t) T + R. It should be noted that the Euler angle tlt(t) = K(t) x (Z(t) - H(t) x tlt(t-1)), and the initial value of the Euler angle tlt(t) is set to

[0086] As an embodiment of the present application, the iterative calculation process of the update conversion matrix Cib0_i0(t) is as follows: first, the value of the intermediate variable norml(t) is calculated, then the values of the intermediate coefficient a(t) and the intermediate coefficient b(t) are calculated, and finally the value of the update conversion matrix Cib0_i0(t) is calculated. It should be noted that the values of the intermediate variable norml(t), the intermediate coefficient a(t), the intermediate coefficient b(t) and the update conversion matrix Cib0_i0(t) are repeatedly calculated in turn whenever new data is collected, and the update conversion matrix Cib0_i0(t) from the carrier coordinate system at the solidification time to the inertial coordinate system at the solidification time is obtained through continuous iterative calculation.

[0087] wherein the intermediate variable norml(t) = tlt_1(t) 2 + tlt_2(t) 2 + tlt_3(t) 2 ; the intermediate coefficient the intermediate coefficient

[0088] It is worth noting that the calculation formula of the update conversion matrix Cib0_i0(t) is:

[0089] Cib0_i0(t) = dlt(t) x Cib0_i0(t-1);

[0090] wherein, The i-th term of tlt(t) is shown, and at this time i = 1, 2, 3.

[0091] Finally, the initial conversion matrix Ci0_n(t) in the navigation coordinate system at the solidification time, the intermediate conversion matrix Cb_ib0(t) from the carrier coordinate system to the carrier coordinate system at the solidification time, and the update conversion matrix Cib0_i0(t) from the carrier coordinate system at the solidification time to the inertial coordinate system at the solidification time are calculated in real time to obtain the attitude matrix Cb_n(t), and then the pitch angle pitch(t), the roll angle roll(t) and the heading angle yaw(t) information of the target object are calculated in real time according to the attitude matrix Cb_n(t) to complete the alignment.

[0092] It is worth noting that the calculation formula of the attitude matrix Cb_n(t) is:

[0093] Cb_n(t) = Ci0_n(t) x Cib0_i0(t) x Cb_ib0(t);

[0094] The calculation formula of the pitch angle is: pitch(t) = arcsin(Cb_n(t)_32);

[0095] The calculation formula of the roll angle is:

[0096] The formula for calculating the heading angle is:

[0097] Where Cb_n(t)_ij represents the i-th row and j-th column of the attitude matrix Cb_n(t).

[0098] According to a second aspect of the present application, the present application also provides a coagulation system alignment device based on iterative filtering, comprising at least one processing unit and at least one storage unit, wherein the storage unit stores a computer program which, when executed by the processing unit, causes the processing unit to perform the steps of the above method.

[0099] According to a third aspect of the present application, the present application also provides a computer readable storage medium based on iterative filtering of the coagulation system alignment, which stores a computer program, and the program is executed by the processor to realize the steps of the above method. The computer readable storage medium can include but is not limited to any type of disk, including floppy disk, optical disk, DVD, CD-ROM, micro drive, and magneto-optical disk, ROM, RAM, EPROM, EEPROM, DRAM, VRAM, flash memory device, magnetic card or optical card, nanosystem (including molecular memory IC), or any type of medium or device suitable for storing instructions and / or data.

[0100] It should be noted that for the foregoing method embodiments, in order to simply describe, they are all expressed as a series of action combinations, but those skilled in the art should know that the present application is not limited by the order of the described actions, because according to the present application, certain steps can be performed in other order or simultaneously. Secondly, those skilled in the art should know that the embodiments described in the specification all belong to preferred embodiments, and the actions and modules involved are not necessarily necessary for the present application.

[0101] In the above embodiments, the description of each embodiment has its own emphasis, and the parts not described in detail in a certain embodiment can be referred to the related description of other embodiments.

[0102] In the several embodiments provided by the present application, it should be understood that the disclosed device can be implemented in other ways. For example, the device embodiments described above are only schematic. The division of the units is only a logical function division. There can be another division manner for actual implementation, for example, a plurality of units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed mutual couplings or direct couplings or communication connections between the units can be indirect couplings or communication connections through some interfaces, devices or units, and can be electrical or other forms.

[0103] The units described as separate components may or may not be physically separate, and the components displayed as units may or may not be physical units, i.e. may be located in one place, or may be distributed on multiple network units. Part or all of the units may be selected according to actual needs to achieve the purpose of the embodiment.

[0104] In addition, each functional unit in various embodiments of the present application can be integrated in one processing unit, or each unit can exist physically, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of a software functional unit.

[0105] If the integrated unit is realized in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer readable storage medium. Based on this understanding, the technical solutions of the present application essentially or the part that contributes to the prior art or the whole or part of the technical solutions can be embodied in the form of a software product. The computer software product is stored in a storage medium, including a plurality of instructions for causing a computer device (which can be a personal computer, a server or a network device, etc.) to execute all or part of the steps of the various embodiment methods of the present application. The aforementioned storage medium includes: a U disk, a read-only memory (Read-Only Memory, ROM), a random access memory (Random Access Memory, RAM), a mobile hard disk, a magnetic disk or an optical disk, and various media that can store program codes.

[0106] Those of ordinary skill in the art can understand that all or part of the steps in the various methods of the above embodiments can be instructed by a program to complete the relevant hardware, and the program can be stored in a computer readable storage medium, which can include: a flash disk, a read-only memory (Read-Only Memory, ROM), a random access memory (Random Access Memory, RAM), a magnetic disk or an optical disk, etc.

[0107] The above is only an exemplary embodiment of the present disclosure, and cannot limit the scope of the present disclosure. That is, any equivalent changes and modifications made in accordance with the teachings of the present disclosure are still within the scope of the present disclosure. Those skilled in the art will easily think of embodiments of the present disclosure after considering the specification and practicing the disclosure herein. The present application is intended to cover any variations, uses or adaptations of the present disclosure that follow the general principles of the present disclosure and include common knowledge or conventional techniques in the art that are not described in the present disclosure. The specification and examples are only considered as exemplary, and the scope and spirit of the present disclosure are defined by the claims.

[0108] Any technical features in the above embodiments can be combined, and for the sake of brevity, not all possible combinations are described above, however, any combination of the technical features is considered to be within the scope of the present application.

[0109] It is to be understood that the above description is intended to be illustrative and not restrictive. Many other embodiments will be apparent to those of skill in the art upon reviewing the above description. The scope of the application should, therefore, be determined not with reference to the above description, but instead with reference to the appended claims, along with their full scope of equivalents.

Claims

1. An iterative filter based coagulation system alignment method, characterized by, The method comprises: Real-time acquisition of the earth rotation angular velocity, local latitude, local gravity acceleration value, gyroscope information data and accelerometer information data of the inertial navigation system; An initial conversion matrix of a freezing time point is calculated based on the earth rotation angular velocity and the local latitude ; wherein the initial conversion matrix is calculated by the following formula: ； Wherein, the ; the ; the ; the ; the Indicate the earth rotation angular velocity, for ; the Indicate the local latitude; the Indicate the cumulative time, , The initial value of 0 is set, Indicate the iteration time, Indicate the calculation period; The inertial system second integral vector of the gravity acceleration at the solidification time is calculated in real time based on the earth rotation angular velocity, the local latitude and the local gravity acceleration value ; wherein the iterative calculation process of the inertial system second integral vector is that: firstly, the value of the inertial system component of the gravity acceleration at the solidification time in the inertial system is calculated; then, the value of the inertial system first integral vector of the gravity acceleration at the solidification time in the inertial system is calculated; finally, the value of the inertial system second integral vector is calculated; every time new data is collected, the values of the inertial system component , the inertial system first integral vector and the inertial system second integral vector are repeatedly calculated in sequence, and the final inertial system second integral vector is obtained through continuous iterative calculation; Real-time iterative calculation of the accelerometer's body coordinate system second integral vector at the freezing time based on the gyroscope information data and the accelerometer information data of the inertial navigation system ; wherein the iterative calculation process of the accelerometer's body coordinate system second integral vector is as follows: firstly, the value of the accelerometer's body coordinate system component at the freezing time is calculated, then the value of the accelerometer's body coordinate system first integral vector at the freezing time is calculated, and finally the value of the body coordinate system second integral vector is calculated; every time new data is collected, the values of the body coordinate system component , the body coordinate system first integral vector and the body coordinate system second integral vector are repeatedly calculated in sequence, and the final body coordinate system second integral vector is obtained through continuous iterative calculation; intermediate conversion matrix is obtained by real-time iterative calculation based on gyro information data of the inertial navigation system ; According to the inertial system quadratic integral vector and the carrier system quadratic integral vector Real-time iterative calculation of Euler angles ; According to the Euler angles Real-time iterative computation results in an updated transformation matrix ; According to the initial conversion matrix , the intermediate conversion matrix , and the update conversion matrix , the pose matrix is calculated in real time ; based on the attitude matrix the pitch angle , the roll angle and the heading angle information is completed.

2. A coagulation system alignment method based on iterative filtering as claimed in claim 1, characterized by, The inertial system quadratic integral vector The iterative calculation process is: S21 : calculating the value of the inertial frame component of the gravitational acceleration at the instant of solidification; wherein said inertial frame component , , represents said local value of the gravitational acceleration. S22: calculating a value of an inertial system first integral vector of the gravitational acceleration at the solidification time; wherein the initial value of the inertial system first integral vector is set as . . . . S23: calculating a value of the inertial system quadratic integral vector ; wherein the inertial system quadratic integral vector ; S24: iteratively calculating the inertial system second integral vector of the gravitational acceleration at the solidification time point by sequentially repeating S21, S22 and S23 .

3. An iterative filter based coagulation system alignment method as claimed in claim 1, wherein, The carrier system secondary integral vector The iterative calculation process is: S31: calculating the carrier body component of the accelerometer at the solidification moment ; wherein the carrier body component , the initial value of the intermediate conversion matrix is set as ; the represents the accelerometer information data, the accelerometer information data ; the represents the gyro information data of the inertial navigation system, the gyro information data of the inertial navigation system ; S32: calculate a carrier system first integral vector of the accelerometer at the solidification time point; wherein, the value of the carrier system first integral vector , the initial value of the carrier system first integral vector , is set as ;​ S33: Calculate the second integral vector of the load system. The value of ; where the quadratic integral vector of the carrying system is . ; S34: repeatedly iteratively calculating the secondary integral vector of the carrier system of the accelerometer at the solidification time point by sequentially repeating S31, S32 and S33 .

4. A coagulation system alignment method based on iterative filtering as claimed in claim 3, characterized in that, The intermediate conversion matrix The iterative computation process for the intermediate conversion matrix is: S41: calculate a modulus value of the gyro information data of the inertial navigation system ; wherein the modulus value ; S42: calculate a rotation vector of the gyro information data of the inertial navigation system ; wherein the rotation vector ; wherein, represents the first term of the rotation vector ; wherein ; S43: calculating a quaternion of gyro information data of the inertial navigation system ; wherein the quaternion ; represents the first term of the quaternion ; the quaternion represents the quaternion at the last calculation period time point , and the initial value of the quaternion is set as ; S44: calculate the intermediate conversion matrix ; wherein the intermediate conversion matrix is calculated according to the following formula: ； Qij represents the ith term of the quaternion Qj multiplied by the jth term of the quaternion Q S45: iteratively calculating the intermediate conversion matrix by repeating S41, S42, S43 and S44 in sequence .

5. An iterative filter based coagulation system alignment method as claimed in claim 1, wherein, The iteration calculation process of the Euler angle is: S51: calculate a measurement vector ; wherein the measurement vector is updated by ; wherein the initial value of the updated transformation matrix is set to S52: calculating the secondary integral vector of the carrier system inertial system projection at the instant of solidification ; wherein the inertial system projection ; S53: compute measurement matrix ; wherein the measurement matrix ; represents the first entry of the inertia system projection of the inertia system projection ; S54: calculating a gain ; wherein the gain ; the gain represents a variance matrix, the variance matrix , the variance matrix is set as an initial value of ; the gain is a transpose matrix of the measurement matrix ; R represents a noise matrix, the noise matrix ; represents an inverse operation on ; S55: calculating the value of the Euler angle ; wherein the Euler angle , the initial value of the Euler angle is set to ; S56: iteratively calculating the Euler angles by repeating S51, S52, S53, S54 and S55 in sequence .

6. An iterative filter based coagulation system alignment method as claimed in claim 5, wherein, The update conversion matrix The iterative computation process for the update conversion matrix S61 : Calculate an intermediate variable ; wherein the intermediate variable ; S62: Calculate intermediate coefficients and the value of the intermediate coefficients ; wherein the intermediate coefficients ; the intermediate coefficients ; S63: calculate the updated conversion matrix ; wherein the updated conversion matrix is calculated according to the following formula: ； The ; denotes the th ; S64: iteratively calculating the updated conversion matrix by repeating S61, S62 and S63 in sequence .

7. An iterative filter based coagulation system alignment method as claimed in claim 1, wherein, The pose matrix The calculation formula is: ； The calculation formula of the pitch angle is: ; The formula for calculating the roll angle is: ; The formula for calculating the heading angle is: ; wherein, denotes the i-th row, j-th column of the pose matrix .

8. An iterative filter based coagulation system alignment apparatus, characterized by, A computer program product comprising at least one processing unit and at least one memory unit, wherein the memory unit stores a computer program which, when executed by the processing unit, causes the processing unit to perform the steps of the method according to any one of claims 1-7.

9. An iterative filter based coagulation system alignment storage medium, characterized by, A computer program product comprising at least one processing unit and at least one memory unit, wherein the memory unit stores a computer program which, when executed by the processing unit, causes the processing unit to perform the steps of the method according to any one of claims 1-7.

Images