Quaternion Optimization Initial Static Base Alignment Method for Isolating Line Motion Disturbance

Abstract

An embodiment of the present invention provides a quaternion-optimized initial static base alignment method for isolating line motion disturbances. In the embodiment of the present application, the initial attitude quaternion to be obtained is calculated through an initial alignment method based on quaternion optimization; the influence degree of the alignment result on line motion interference is determined; when the determination result is that the influence degree exceeds the error upper limit value, the occurrence time of the disturbance during the alignment process is further located to eliminate the disturbance influence at this point and improve the alignment accuracy of the initial static base. At the same time, a high-precision initial attitude is provided for the position and attitude system under the conditions of short alignment time and harsh external environment.

Description

Technical Field

[0001] The present invention belongs to the field of navigation, and particularly relates to a quaternion optimization initial static base alignment method for isolating the disturbance of line movement. Background Art

[0002] The position and attitude measurement system is a special SINS / DGPS integrated navigation system and is one of the key technologies for high-resolution airborne earth observation. Among them, the initial static base alignment is one of the key technologies for the position and attitude measurement system to achieve high precision.

[0003] Currently, the initial static base alignment mostly adopts the initial alignment method based on Kalman filtering. These methods generally use zero velocity as the velocity observation quantity and use the magnitude of the R matrix to reflect the velocity disturbance.

[0004] Such methods meet the application requirements of conventional SINS / DGPS, but the battlefield environment poses more stringent requirements for the position and attitude measurement system. It is required that the position and attitude system can obtain a high-precision alignment attitude under large environmental disturbances in a short time to ensure the accuracy of aerial remote sensing battlefield imaging.

[0005] However, the alignment method based on Kalman filtering has poor accuracy of the coarse alignment heading angle under large disturbances and requires a long time to gradually converge in the fine alignment stage. On the other hand, large disturbances lead to poor credibility of the zero-velocity observation quantity, which will also greatly extend the time required for fine alignment under certain accuracy requirements. The initial alignment method based on quaternion optimization is an analytical alignment method different from the conventional Kalman filtering, and has the advantages of strong robustness and high precision. However, the disturbance of line movement destroys the prerequisite that the linear acceleration of this method is zero, seriously affecting the alignment accuracy. Summary of the Invention

[0006] The present application provides a quaternion optimization initial static base alignment method for isolating the disturbance of line movement to solve the problems pointed out in the background art.

[0007] In a first aspect, the present invention provides a quaternion optimization initial static base alignment method for isolating the disturbance of line movement, and the method includes:

[0008] Step S100, calculating the required initial attitude quaternion by using the initial alignment method based on quaternion optimization;

[0009] Step S200, determining the influence degree of the line movement interference on the alignment result;

[0010] Step S300, when the determination result shows that the influence degree exceeds the error upper limit value, further locate the occurrence time of the disturbance during the alignment process to eliminate the influence of the disturbance at this point and improve the alignment accuracy of the initial static base.

[0011] The above technical solution provided by the embodiments of the present application has the following advantages compared with the prior art:

[0012] A quaternion-optimized initial static base alignment method for isolating line motion disturbances provided by the embodiments of the present application gives a determination method for the degree of influence of the alignment result of the quaternion-optimized alignment method by disturbances, proposes a selection principle for static time points, improves the alignment algorithm, and avoids the influence of external disturbances on the alignment result by isolating the disturbance time points in the calculation of the optimization index. Description of the Drawings

[0013] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation to the present application. In the drawings:

[0014] Figure 1 is a main step flow chart of a quaternion-optimized initial static base alignment method for isolating line motion disturbances according to an embodiment of the present application;

[0015] Figure 2 is a specific step flow chart of the quaternion-optimized initial static base alignment method for isolating line motion disturbances according to an embodiment of the present application;

[0016] Figure 3 is a specific step flow chart of the quaternion-optimized initial static base alignment method for isolating line motion disturbances according to an embodiment of the present application. Detailed Embodiments

[0017] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention. Apparently, the described embodiments are some but not all of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0018] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the technical field to which the present invention belongs. The terms used herein in the specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention.

[0019] Embodiment 1

[0020] Embodiment 1 of the present invention provides a quaternion-optimized initial static base alignment method for isolating line motion disturbances, and the method includes:

[0021] Step S100, calculate the initial attitude quaternion to be obtained through an initial alignment method optimized based on quaternions;

[0022] Step S200, determine the influence degree of the line motion interference of the alignment result;

[0023] Step S300, when the determination result is that the influence degree exceeds the error upper limit value, further locate the occurrence time of the disturbance during the alignment process, so as to eliminate the disturbance influence at this point and improve the alignment accuracy of the initial static base.

[0024] A quaternion-optimized initial static base alignment method for isolating line motion disturbances provided by the present invention realizes fast and high-precision initial alignment in an environment with large external disturbances. The method specifically includes the following steps:

[0025] Step S100, calculate the initial attitude quaternion to be obtained through an initial alignment method optimized based on quaternions, which specifically includes the following operations:

[0026] Step S101, during the measurement process of the position and attitude system, select the local geographic system as the navigation system. At the same time, under the navigation system, obtain the attitude update differential equation and the velocity differential equation; use the quaternion attitude update method to solve the attitude update differential equation respectively to obtain the initial attitude of the carrier coordinate system relative to the inertial coordinate system Solve the velocity differential equation to obtain the intermediate variables β(t) and α(t);

[0027] Step S102, perform quaternion operation conversion on the intermediate variables β(t) and α(t), and at the same time define the K matrix for formula rearrangement to obtain: L(q) = q T Kq - λ(q T q - 1);

[0028] Among them, L(q) represents the mean square error statistics of the difference between the acceleration and zero acceleration at each moment in navigation, and the minimum eigenvalue λ of the K matrix min That is, it represents the minimum value that L(q) can reach; when q takes the eigenvector corresponding to the minimum eigenvalue λ of the K matrix min of , L(q) obtains the minimum value λ min , at this time, is the initial attitude quaternion to be obtained;

[0029] The specific operations are as follows:

[0030] During the measurement process of the position and attitude system, select the local geographic system as the navigation system. Under the navigation system, the attitude update differential equation is as follows:

[0031]

[0032] Among them, Equation 1 is the attitude update differential equation, where i is the inertial coordinate system, n is the navigation coordinate system, and b is the vehicle coordinate system. is the attitude matrix of the vehicle coordinate system relative to the navigation coordinate system. is the transpose of; is the angular rate output by the gyroscope. is the cross product matrix of the vector ; is the projection of the earth's angular rotation rate ω ie in the navigation coordinate system. represents the angular rate of the navigation coordinate system relative to the earth coordinate system. represents the projection of the rotational angular velocity of the vehicle coordinate system relative to the navigation coordinate system on the vehicle coordinate system.

[0033]

[0034] Among them, Equation 2 is the velocity differential equation. Among them, is the motion velocity of the navigation coordinate system relative to the earth coordinate system expressed in the inertial space; f b is the specific force information sensed by the inertial measurement unit expressed in the inertial space. is the motion velocity of the vehicle relative to the earth expressed in the inertial space; g n is the earth gravity vector expressed in the inertial space.

[0035] After the solution of the attitude update differential equation, is in the following form:

[0036]

[0037] Among them, x n (0, t) is a function obtained by using for update solution. The update method adopts the quaternion general attitude update method. t is the time. is the initial attitude of the vehicle coordinate system relative to the inertial coordinate system.

[0038] Therefore, according to the above Equation 3, and can be expressed in the following form:

[0039]

[0040]

[0041] Among them, χ b (0, t) is a function of , and χ n (0, t) is a function of function. is the attitude transformation matrix of the i - system relative to the b - system. is the attitude transformation matrix of the i - system relative to the n - system.

[0042] According to the calculation relationship of the attitude matrix can be expressed as Substituting into the formula, we can get:

[0043]

[0044] Substituting Equation 5 into the velocity differential equation, that is, Equation 2, we can get

[0045]

[0046] where, β(t) and α(t) are respectively defined as

[0047]

[0048]

[0049] where, both β(t) and α(t) in Equation 6 are intermediate variables (without specific meaning, just variable representations in the derivation process); that is, set the above Equation 6 as the observation formula;

[0050] For convenience, rewrite the above formula into the form of quaternion operation as

[0051]

[0052] where, represents quaternion multiplication operation, and q is in quaternion form that is, the initial attitude quaternion.

[0053] Meanwhile, it is defined that q = [s η], q consists of a scalar part s and a vector part η, q * is the conjugate quaternion of q.

[0054] Therefore is expressed as

[0055]

[0056] According to the quaternion multiplication rule as follows

[0057]

[0058] where, two operations of quaternion are defined as follows:

[0059]

[0060] The formula can be transformed into

[0061]

[0062] Subsequently, it is transformed into

[0063]

[0064] According to the least squares rule to solve the formula, the initial attitude quaternion q satisfies

[0065]

[0066] Here, the K matrix is defined as follows:

[0067]

[0068] Therefore, the formula can be expressed as

[0069]

[0070] Meanwhile, q satisfies

[0071] q T q = 1; Equation 14;

[0072] In order to incorporate the condition into the operation to facilitate the solution, the Lagrange multiplier method is applied to define L(q) as follows:

[0073] L(q) = q T Kq - λ(q T q - 1); Equation 15;

[0074] Therefore, when L(q) reaches the minimum value, it should meet the following conditions

[0075]

[0076]

[0077] Since this equation is the natural property of the quaternion, only this equation needs to be considered. Rearranging this equation gives

[0078] (K + λI)q = 0; Equation 18;

[0079] Among them, L(q) represents the mean square error statistics of the difference between the acceleration and zero acceleration at each moment in navigation, and the minimum eigenvalue λ of the K matrix min i.e., represents the minimum value that L(q) can reach.

[0080] Therefore, when q takes the eigenvector corresponding to the minimum eigenvalue λ of the K matrix min then L(q) reaches the minimum value λ min , at this time, ​That is the initial attitude quaternion sought. Thus, the initial alignment result can be obtained. The above algorithm can achieve alignment analytically. However, observing β(t) in the formula, since the true value at each moment is unknown, the algorithm assumes that under static base alignment it is always zero. The negative effect of this treatment is that when there are obvious linear motion disturbances during the alignment process, it destroys the precondition of this algorithm, making the attitude alignment result, especially the alignment accuracy of the heading angle, seriously distorted in this case.

[0081] In the specific technical solution of the embodiment of the present application, by executing step S200, the influence degree of the linear motion interference on the alignment result is determined;

[0082] The specific operation is as follows:

[0083] Step S201, it is agreed that the four eigenvalues of matrix K are arranged in ascending order as λ min 、λ c 、λ cc 、λ max , and the corresponding quaternion eigenvectors are respectively

[0084] Step S202, set that when the ideal eigenvalue is 0, the corresponding true attitude quaternion eigenvector is q r , and solve the between and q r There is a small angle error q Δ ; q Δ That is, the difference between the quaternion q r representing the true attitude and the eigenvector min corresponding to the smallest eigenvalue λ of matrix K ;

[0085] Step S203, calculate the upper limit of the alignment heading angle error caused by the linear motion disturbance, and calculate the upper limit of the alignment horizontal attitude angle error caused by the linear motion disturbance; evaluate the upper limit of the error of the alignment result affected by the disturbance according to the above calculation methods of the alignment heading angle error upper limit and the alignment horizontal attitude angle error upper limit.

[0086] Analysis shows that: linear motion disturbances will lead to the degradation of alignment accuracy (that is, the algorithm assumes a speed of 0 during the alignment process. If there is a linear motion disturbance, the assumption of a speed of 0 does not hold, and thus the alignment accuracy will decrease), but the algorithm user cannot judge the influence degree of the disturbance on the alignment result and the credibility of the alignment result. Therefore, a method for determining the disturbance degree and the influence degree of the alignment accuracy is needed.

[0087] In the ideal case, the smallest eigenvalue of matrix K is zero (as derived above, when q takes the eigenvector corresponding to the smallest eigenvalue λ of matrix K min ). When \(L(q)\) reaches the minimum value \(\lambda\) min , at this time is the required initial attitude quaternion. ), at this time, in an ideal situation, the corresponding eigenvector is the ideal initial attitude. However, in practice, the minimum eigenvalue is often a decimal close to zero, which represents the degree to which the inertial device error and the perturbation error cause the acceleration to deviate from the zero acceleration, and also represents the uncertainty of the alignment attitude (that is, the uncertainty of the above alignment attitude means that the alignment attitude is not 0). It is agreed that the four eigenvalues of the \(K\) matrix are arranged in ascending order as \(\lambda\) min 、\(\lambda\) c 、\(\lambda\) cc 、\(\lambda\) max , and the corresponding quaternion eigenvectors are respectively However, when the ideal eigenvalue is 0, the corresponding true attitude quaternion eigenvector is \(q\) r . Obviously, due to errors and \(q\) r there is a small angular error \(q\) Δ (\(q_{\Delta}\) is the difference between the quaternion \(q\) r representing the true attitude and the eigenvector min corresponding to the minimum eigenvalue \(\lambda\) of the \(K\) matrix ), that is

[0088] Since the eigenvectors of the \(K\) matrix are orthogonal to each other, therefore, for any standard quaternion and \(q\) c expressed as the basis, \( a 2 +b 2 =1, substituting it into the expression of \(L(q)\), we can get

[0089]

[0090] From we get Similarly Therefore, we can get

[0091] Considering the worst-case scenario, that is, the minimum eigenvalue generated by the inertial device error and the perturbation is between the ideal zero eigenvalue and the second smallest eigenvalue. At this time, the relationship between the eigenvalue and the eigenvector is as follows:

[0092]

[0093] From the above two equations, we can get \(b = \lambda\) min / \(\lambda\) c , which represents the ratio of the minimum eigenvalue to the second smallest eigenvalue.

[0094] Since usually \(b = \lambda\)min / λ c <<1, so after approximation, we can get

[0095]

[0096] That is

[0097] The above formula can be regarded as a quaternion and q c Interpolating linearly according to the weight b. However, the four elements in the quaternion are not equivalent, making it difficult to simplify and interpret the above formula with a simple vector. However, through multiple simulations, it is found that there is a special relationship between the attitudes represented by the eigenvector quaternions corresponding to the minimum eigenvalue and the second minimum eigenvalue. The horizontal attitude angles between these two attitudes are approximately the same, and the heading attitude angles are approximately 180° different, which is consistent with our common sense, that is, the speed divergence caused by the heading angle error is slower, and the speed divergence caused by the horizontal attitude angle error is faster. Based on the above facts, ignoring the second-order small error caused by approximation, the q r expression can be approximately expressed as

[0098]

[0099] where q E is the unit matrix of q, that is, the quaternion representation of pitch angle, roll angle, and heading angle 0°, 0°, 0°. q fai(180) is the quaternion representation of the attitude with a heading angle of 180° and pitch and roll angles of 0°.

[0100] From the above formula, the upper limit of the alignment heading angle error caused by linear motion disturbance can be approximately expressed as λ min / λ c , in radians.

[0101] Similarly, the upper limit of the alignment horizontal attitude angle error caused by linear motion disturbance can be approximately expressed as λ min / λ cc , where λ cc is the eigenvalue smaller than the largest eigenvalue.

[0102] According to the above calculation method, the upper limit of the error of the alignment result affected by the disturbance can be evaluated (that is, the error upper limit value refers to q Δ ).

[0103] When the judgment result according to the principle of the previous step is poor (when the judgment result exceeds the error upper limit q Δ ), it is necessary to further locate the occurrence time of the disturbance during the alignment process, so as to eliminate the disturbance at this point in the algorithm and improve the alignment accuracy.

[0104] The minimum eigenvalue of the K matrix is the minimum value that the mean square error of the navigation acceleration error can possibly reach under any initial attitude. If the acceleration is constantly zero for a period of time, the minimum eigenvalue is zero. If there is a linear motion disturbance for a period of time, the minimum eigenvalue is greater than zero, and the more severe the disturbance, the larger the minimum eigenvalue.

[0105] Using this feature, the IMU data during the alignment process can be evaluated in segments. The K matrix of the IMU data during the time period [M·T N·T] is calculated as follows:

[0106]

[0107] where T is the IMU sampling period, and M, N = 1, 2, 3…, and the definitions of β and α are the same as above.

[0108] Through multiple simulations and experimental analyses, it is found that when the length of the time period is taken as 1 second, it can not only better reflect the degree of acceleration fluctuation but also take into account the operation efficiency.

[0109] The alignment data can be divided into several data segments in seconds for discrimination respectively, and the linear motion disturbance points can be removed. However, static and disturbance are only fuzzy descriptions, making it impossible to clearly define the boundary to judge the disturbance time point. Therefore, it is necessary to define the range of the minimum eigenvalue of the K matrix to characterize the degree of stillness in the static state.

[0110] According to the practical experience in engineering, it is found that usually, the minimum eigenvalue under the action of disturbance is often greater than twice the eigenvalue in the normal static state. Since there is no clear definition method, it is advisable to take twice the mean value of the minimum eigenvalue calculated in the ideal static state on the laboratory marble table as the standard. When the judgment result is greater than or equal to this value, this point is taken as the obvious disturbance point. When the judgment result is less than this value, this point is taken as the undisturbed static point.

[0111] Analyzing the above technical solution, it can be seen that the method for selecting the undisturbed static time point is given through steps 201 - 203. Using the above method, the original algorithm can be improved to overcome the interference of linear motion disturbance.

[0112] Analyzing the implementation process of the algorithm, the K matrix is numerically calculated as follows

[0113]

[0114] Since all sampling points are included when the original algorithm calculates the K matrix, the alignment result will inevitably be affected by the linear motion disturbance. To overcome this shortcoming, by judging that the output modulus of the accelerometer is greater than a certain threshold (the threshold is set according to engineering experience), the time points with obvious linear motion disturbance are obtained, and then the above-mentioned identified obvious disturbance time points are removed to isolate the influence of the linear motion disturbance. Therefore, the calculation of the K matrix at this time becomes the following form

[0115]

[0116] Among them, n1, n2, n3, etc. are the time points of the selected wireless motion disturbances.

[0117] The physical meaning of this determination method is that, according to the error propagation principle of the static base, in the case of no inertial device errors and external disturbances, there is only a set of initial attitudes (except when the location is at the South Pole and the North Pole, in which case the heading angle is arbitrary), such that the linear acceleration calculated by pure inertial navigation at each moment is always zero. In actual applications, due to the inevitable existence of various errors, even with an accurate initial attitude, the speed will still diverge slowly. However, the speed divergence of the initial attitude with large errors will be much faster. The determination method of the embodiments of this application is based on this basic understanding.

[0118] In the quaternion optimization alignment algorithm, the physical meaning of the eigenvalues of the K matrix is the mean square deviation statistic of the navigation acceleration deviating from zero acceleration when navigating with the corresponding eigenvector as the initial attitude. The physical meaning of the minimum eigenvalue is the minimum value of the above mean square deviation statistic when navigating with an arbitrary initial attitude.

[0119] Linear motion disturbances are manifested as speed changes, that is, non-zero accelerations appear. For a section of static data containing linear disturbances, due to the randomness of the disturbances, there is almost no initial attitude that can make the acceleration obtained by strapdown solution always zero. Based on this basic understanding, the K matrix in the original alignment algorithm can be used to evaluate the degree of speed fluctuation within a certain time period.

[0120] According to the practical experience in engineering, it is found that usually, the minimum eigenvalue under the action of disturbances is often greater than twice the eigenvalue in the normal static state. Since there is no clear definition method, it is advisable to use twice the mean value of the minimum eigenvalue calculated in the ideal static state on the laboratory marble table as the standard. When the determination result is greater than or equal to this value, this point is regarded as the obvious disturbance point. When the determination result is less than this value, this point is regarded as the undisturbed static point.

[0121] In summary, the quaternion optimization initial static base alignment method for isolating linear motion disturbances adopted in the embodiments of the present invention optimizes the initial alignment method based on quaternion optimization, overcomes the influence of linear motion interference on the alignment result when using this algorithm, and improves the application range of this method; at the same time, this method can effectively isolate the interference of linear motion, significantly improve the alignment accuracy, and provide a high-precision initial attitude for the position and attitude system under the conditions of short alignment time and harsh external environment.

[0122] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some or all of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A quaternion-optimized initial static base alignment method for isolating line motion disturbances, characterized in that The method includes: Step S100, calculating the initial attitude quaternion to be obtained through an initial alignment method optimized based on quaternions; Step S200, determining the influence degree of the line motion interference on the alignment result; Step S300, when the determination result is that the influence degree exceeds the error upper limit value, further locate the occurrence time of the disturbance during the alignment process to eliminate the influence of the disturbance on this point and improve the alignment accuracy of the initial stationary base; During step S100, calculating the initial attitude quaternion to be obtained through an initial alignment method optimized based on quaternions, which specifically includes the following operations: Step S101, during the measurement of the position and attitude system, select the local geodetic system as the navigation system. Meanwhile, under the navigation system, obtain the attitude update differential equation and the velocity differential equation; use the quaternion attitude update method to solve the attitude update differential equation respectively to obtain the initial attitude of the carrier coordinate system relative to the inertial coordinate system Solve the velocity differential equation to obtain the intermediate variables β(t) and α(t); Step S102, perform quaternion operation transformation on the intermediate variables β(t) and α(t), and at the same time define the K matrix and reorganize formula (16) to obtain: L(q) = q T Kq - λ(q T q - 1); Among them, L(q) represents the mean square error statistics of the difference between the acceleration and zero acceleration at each moment in navigation, and the minimum eigenvalue λ of the K matrix min represents the minimum value that L(q) can reach; when q takes the eigenvector corresponding to the minimum eigenvalue λ of the K matrix min L(q) reaches the minimum value λ min , and at this time is the initial attitude quaternion to be found;​ The specific operations are as follows: During the measurement process of the position and attitude system, select the local geographic system as the navigation system; under the navigation system, the attitude update differential equation is as follows: Among them, Equation 1 is the attitude update differential equation, where i is the inertial coordinate system, n is the navigation coordinate system, and b is the vehicle coordinate system. is the attitude matrix of the vehicle coordinate system relative to the navigation coordinate system. is the transpose of; is the angular rate output by the gyroscope. is the vector cross product matrix of; represents the projection of the earth's angular rotation rate ω ie in the navigation coordinate system. represents the angular rate of the navigation coordinate system relative to the earth coordinate system. represents the projection of the rotational angular velocity of the vehicle coordinate system relative to the navigation coordinate system on the vehicle coordinate system. Among them, Equation 2 is the velocity differential equation; among them, is the motion velocity of the navigation coordinate system relative to the earth coordinate system expressed in the inertial space; f b is the specific force information sensed by the inertial measurement unit expressed in the inertial space; is the motion velocity of the carrier relative to the earth expressed in the inertial space; g n is the earth gravity vector expressed in the inertial space; After calculating the solution of the attitude update differential equation, It is in the following form: where x n (0, t) is a function obtained by using updated solution. The update method adopts the quaternion general attitude update method. t is time, is the initial attitude of the vehicle coordinate system relative to the inertial coordinate system; Therefore, according to the above formula 3(3), and can be expressed in the following form: where, χ b (0, t) is a function of χ n (0, t) is a function of; is the attitude transformation matrix of the i-system relative to the b-system; is the attitude transformation matrix of the i-system relative to the n-system; According to the calculation relationship of the attitude matrix It can be expressed as Substituting into Equation (4), we can get: Substitute the above formula into the velocity differential equation, i.e., formula (2)2, to obtain where, β(t) and α(t) are respectively defined as where, both β(t) and α(t) in formula 6 are intermediate variables; that is, set the above formula 6 as the observation formula; Convert the above formula into the form of quaternion operation as Among them, represents the quaternion multiplication operation, and q is the initial attitude quaternion, that is; Meanwhile, define \(q = [s\eta]\), where \(q\) consists of a scalar part \(s\) and a vector part \(\eta\), and \(q\) * is the conjugate quaternion of \(q\); Therefore is represented as follows According to the quaternion multiplication rule as follows where, two operations of quaternions are defined as follows: Formula (7) can be transformed into and then transformed into Solve formula (10) according to the least squares rule, and the initial attitude quaternion q satisfies Here, define the K matrix as follows: Therefore, formula (12) can be expressed as At the same time, q satisfies q T q = 1; Equation 14; In order to incorporate condition (14) into the operation to facilitate the solution, use the Lagrange multiplier method to define L(q) as follows: L(q) = q T Kq - λ(q T q - 1); Equation 15; Therefore, when L(q) obtains the minimum value, it should meet the following conditions Since formula (17) is the natural property of quaternions, only consider formula (16), and rearrange formula (16) to obtain (K + λI)q = 0; formula 18; Among them, \(L(q)\) represents the mean square error statistics of the difference between the acceleration and zero acceleration at each moment in navigation, and the minimum eigenvalue \(\lambda\) of the \(K\) matrix min which represents the minimum value that \(L(q)\) can reach; Therefore, when q takes the eigenvector corresponding to the minimum eigenvalue λ of the K matrix min , , L(q) obtains the minimum value λ min . At this time, is the required initial attitude quaternion, and the initial alignment result can be obtained; During the execution of step S200, determine the influence degree of the line motion interference on the alignment result; Step S201, it is agreed that the four eigenvalues of the K matrix are arranged in ascending order as λ min , λ c , λ cc , λ max , and the corresponding quaternion eigenvectors are respectively Step S202, set that when the ideal eigenvalue is 0, the corresponding true attitude quaternion eigenvector is q r , solve and q r There is a small angle error q Δ ; q Δ That is, the quaternion q representing the true attitude r and the difference between the minimum eigenvalue λ of the K matrix min corresponding eigenvector ; Step S203, calculate the upper limit of the alignment course angle error caused by the line motion disturbance, and calculate the upper limit of the alignment horizontal attitude angle error caused by the line motion disturbance; evaluate the upper limit of the error of the alignment result affected by the disturbance according to the above calculation methods of the upper limit of the alignment course angle error and the upper limit of the alignment horizontal attitude angle error; Since the eigenvectors of the K matrix are pairwise orthogonal, for any standard quaternion and q c used as a basis, where a 2 +b 2 = 1, substituting it into the expression of L(q), we can get From we get Similarly Therefore, we can obtain Considering the worst case, the minimum eigenvalue generated by the inertial device error and the disturbance is between the ideal zero eigenvalue and the second smallest eigenvalue. At this time, the relationship between the eigenvalue and the eigenvector is as follows: q λmin = aq r + bq c , a·0 + bλ c = λ min From the above two equations, it can be obtained that b = λ min / λ c , representing the ratio of the minimum eigenvalue to the second minimum eigenvalue; Since usually b = λ min / λ c << 1, therefore, after approximate treatment, we can obtain: That is Ignoring the second-order small error caused by approximation, approximate the q r expression as follows: where q E is the identity matrix of q, i.e., the quaternion representation of pitch angle, roll angle, and heading angle of 0°, 0°, 0°; q fai(180) is the attitude quaternion with a heading angle of 180° and the quaternion representation of pitch angle and roll angle of 0°; As can be seen from the above formula, the upper limit of the alignment course angle error caused by the linear motion disturbance can be approximately expressed as λ min / λ c , in radians; Similarly, the upper limit of the alignment horizontal attitude angle error caused by the linear motion disturbance can be approximately expressed as λ min / λ cc , where λ cc is the eigenvalue smaller than the maximum eigenvalue; According to the above calculation method, the upper limit of the error of the alignment result affected by the disturbance can be evaluated.

2. The quaternion optimization initial static base alignment method for isolating line motion disturbances according to claim 1, wherein During the execution of step S300, when the determination result is that the influence degree exceeds the error upper limit value, further locate the occurrence time of the disturbance during the alignment process to eliminate the influence of the disturbance on this point and improve the alignment accuracy of the initial stationary base: Evaluate the IMU data in the alignment process in segments, and calculate the K matrix of the IMU data within the time period [M·T N·T] as follows: where, T is the IMU sampling period, where M, N = 1, 2, 3…, the definitions of β and α are the same as above; set the IMU sampling period to 1 second; divide the alignment data into several data segments in seconds for discrimination respectively, and eliminate the line motion disturbance points among them.

Images