CN112464432B - Optimization method of inertial pre-integration based on double-speed numerical integration structure - Google Patents

Optimization method of inertial pre-integration based on double-speed numerical integration structure Download PDF

Info

Publication number
CN112464432B
CN112464432B CN202011161744.3A CN202011161744A CN112464432B CN 112464432 B CN112464432 B CN 112464432B CN 202011161744 A CN202011161744 A CN 202011161744A CN 112464432 B CN112464432 B CN 112464432B
Authority
CN
China
Prior art keywords
vector
speed
matrix
integration
double
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011161744.3A
Other languages
Chinese (zh)
Other versions
CN112464432A (en
Inventor
汤传业
葛红飞
陈建锋
陈龙
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu University
Original Assignee
Jiangsu University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Jiangsu University filed Critical Jiangsu University
Priority to CN202011161744.3A priority Critical patent/CN112464432B/en
Publication of CN112464432A publication Critical patent/CN112464432A/en
Priority to PCT/CN2021/101261 priority patent/WO2022088700A1/en
Application granted granted Critical
Publication of CN112464432B publication Critical patent/CN112464432B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/10—Numerical modelling

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Remote Sensing (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Pure & Applied Mathematics (AREA)
  • Software Systems (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • Computing Systems (AREA)
  • Computer Hardware Design (AREA)
  • Geometry (AREA)
  • Automation & Control Theory (AREA)
  • Complex Calculations (AREA)

Abstract

The invention provides an inertial pre-integration optimization method based on a double-speed numerical integration structure, which is a very key link in a system in the application fields of automatic driving of a vehicle, attitude control of an unmanned aerial vehicle, motion capture, augmented reality and the like. According to the invention, a double-speed numerical integration structure is introduced into the inertial pre-integration of the combined motion measurement system, the double-speed numerical integration structure comprises a double-speed rotation integration structure, a double-speed integration structure and a double-speed position integration structure, and parameters in linear integration items and nonlinear integration items are optimally designed by implementing parameter optimization of the double-speed numerical integration structure. The method disclosed by the invention can be used for remarkably improving the motion estimation precision, the calculation instantaneity and the numerical stability of the inertial base combined motion measurement system.

Description

Optimization method of inertial pre-integration based on double-speed numerical integration structure
Technical Field
The invention relates to the technical fields of intelligent vehicle autonomous navigation positioning, unmanned plane autonomous navigation positioning and attitude control, carrier motion capturing and augmented reality and the like, in particular to an optimization method of inertial pre-integration based on a double-speed numerical integration structure.
Background
In recent years, the automotive industry and technology for automatic driving have rapidly developed, and a navigation and positioning system, which is one of the cores of automatic driving, is an essential component for realizing automatic driving. On the other hand, unmanned aerial vehicle technology and industry application rapidly develop in recent years, and the unmanned aerial vehicle is rapidly applied and expanded in the fields of agricultural plant protection, disaster relief, express delivery, mapping, aerial photography, news reporting, electric power inspection, film and television shooting and the like. The high dynamics, uncertainty and complexity of the unmanned aerial vehicle flight environment are major problems faced by unmanned aerial vehicle autonomous control. The precise and reliable real-time pose measurement of the unmanned carrier is not available for both the navigational positioning required by automatic driving and the pose control required by unmanned aerial vehicle autonomous flight. In addition, the application fields of augmented reality, medical rehabilitation, simulated training, animation production and the like have strong demands for motion capture technologies, and are increasingly remarkable.
In the current technical field of autonomous navigation positioning, pose measurement, motion capture and other motion measurement, a motion measurement scheme mainly adopts a combination mode of multi-sensing information fusion, and among various sensing devices on which the motion measurement scheme depends, an inertial measurement unit is almost indispensable because the inertial measurement unit can provide rich high-speed and high-dynamic motion information. The usual inertial based combined motion measurement system is: an inertial/vision combination system, an inertial/satellite combination system, an inertial/odometer combination system, etc.
Inertial pre-integration, a key component in inertial based combined motion measurement systems, is typically performed over a relatively large time interval to form a relative motion constraint. However, inertial pre-integration implemented in the form of standard integration is inefficient. This problem is solved by the linear inertial pre-integration method based on Euler angle expression, SO 3 manifold expression and quaternion expression, proposed by Lupton et al, forster et al, and Eckenhoff et al. However, studies by Eckenhoff et al and Nisar et al indicate that there is a significant higher order nonlinear error in the linear inertial pre-integration, the more intense the carrier dynamic motion intensity, the more significant the nonlinear error. In high-intensity dynamic motion environments including strong vibration, large maneuvers, etc., nonlinear errors of the current linearized inertial pre-integral are more pronounced.
Disclosure of Invention
Aiming at the defects in the prior art, the invention provides an optimization method of inertial pre-integration based on a double-speed numerical integration structure, and the motion estimation precision, the calculation instantaneity and the numerical stability of an inertial-based combined motion measurement system are improved by optimally designing parameters in the numerical integration structure.
The present invention achieves the above technical object by the following means.
An optimization method of inertial pre-integration based on a double-speed numerical integration structure introduces the double-speed numerical integration structure in the design process of inertial pre-integration of a combined motion measurement system, and implements parameter optimization of the double-speed numerical integration structure;
the double-speed numerical integration structure comprises a double-speed rotation integration structure, a double-speed integration structure and a double-speed position integration structure;
The parameter optimization of the double-speed numerical integration structure comprises linear integration parameter optimization and nonlinear integration parameter optimization.
Further, the double-speed rotation integration structure is phi ij＝φ1ij+φ2ij, Where φ 1 ij is a linear integral term of the rotation vector, φ 2 ij is a rotation vector nonlinear integral compensation term of bilateral form, ω k is an ideal inertial measurement angular velocity raw sample, ω k has a sampling interval δt, ω 'm and ω' n are angular velocity samples extracted from the raw samples { ω k |k=i, i+1,..A k is the rotation vector linear integral fit coefficient, al m and ar n are rotation vector nonlinear integral compensation coefficients, Σ is the sum operator, x is the vector cross.
Further, the two-speed integral structure is DeltaV ij＝ΔV1ij+ΔV2ij, Wherein Δv1 ij is a linear integral term of a velocity vector, Δv ij is a speed vector nonlinear integral compensation term in a bilateral form, a k is an ideal inertial measurement acceleration original sample, a k has a sampling interval δt, ω 1,m is an angular velocity sample extracted from the original samples { ω k |k=i, i+1,..j }, and ω 1,m has a sampling interval/>The number M 1,a1,n is the acceleration sample extracted from the original sample { a k |k=i, i+1,..j } with a 1,n sample interval/>The number N 1,Bk is the linear integral fitting coefficient of the velocity vector, and the Bl m and Br n are the nonlinear integral compensation coefficients of the velocity vector.
Further, the two-speed position integration structure is DeltaP ij＝ΔP1ij+ΔP2ij, Wherein Δp1 ij is a linear integral term of a position vector, Δp2 ij is a nonlinear integral compensation term of a position vector in a bilateral form, a k is an ideal inertial measurement acceleration original sample, a k has a sampling interval δt, ω 2,m is an angular velocity sample extracted from the original samples { ω k |k=i, i+1,..The number M 2,a2,n is the acceleration sample extracted from the original sample { a k |k=i, i+1,..j } with a 2,n sample interval/>The number N 2,Ck is the linear integral fitting coefficient of the position vector, and Cl m and Cr n are the nonlinear integral compensation coefficients of the position vector.
Further, the linear integral structure parameter optimization step comprises the following steps:
(1) Defining an inertia vector of a multi-segment polynomial expression:
Wherein ω (t) is an ideal measured inertia vector at time t, M is the number of terms of each segment polynomial, N is the number of segments of the piecewise polynomial function, g j,i (i=1, 2,..m, j=1, 2,..n) is the coefficient vector of the j-th segment polynomial, t 0 is the initial time, and the polynomial order is M-1;
Defining equal time intervals δt sampling ω (t) over a period of time [ t 0,t0 + (m+n-2) δt ]:
Wherein ω j (j=1, 2,., m+n-1) is the sampling of ω (t) at time t 0 + (j-1) δt;
The matrix of the relationship between ω j and g j,i is expressed as:
W＝TωG
Wherein, W= [ W 1 T W2 … WN]T, [] T Is the transpose operator of the matrix, pi is the product operator, O is the null matrix ;W1＝[(ωj)j,1]M×1(j＝1,2,...,M),Wj-(M-1)＝[(ωj)1,1]1×1(j＝M+1,M+2,...,M+N-1);Gj＝[(gj,iδti-1)i,1]M×1(j＝1,2,...,N),/>A1×1 unit pixel matrix constituted by the last element of the column matrix G j; t gω＝[((j-1)i-1)j,i]M×M,TM is a row matrix formed by taking the M th row of matrix T gω,/> To take the (M-1) × (M-1) matrix consisting of the first M-1 rows and the first M-1 columns of matrix T gω,/>For the (M-1) x M matrix formed by non-1 st column of the matrix T gω, T' M is the (M-1) x 1 matrix formed by the elements of the first M-1 row of the M column of the matrix T gω, and [ (-1 ] is the inverse operator of the matrix;
(2) The inertia vector omega (t) is integrated once from the time t 0 to the time t 0 + (M+N-2) delta t to obtain an integrated vector delta alpha:
The matrix of the relationship between Δα and g j,i is expressed as:
Δα＝δt·OnesTαG
wherein the content of O nes＝[(1)1,i]1×N is at least one of, O nes is a1 XN row matrix with 1 as element,/>
(3) The inertia vector omega (t) is integrated twice from the time t 0 to the time t 0 + (M+N-2) delta t to obtain an integrated vector delta alpha:
The matrix of the relationship between ΔΑ and g j,i is expressed as:
ΔΑ＝δt2·OnesTΑG
Wherein the method comprises the steps of
(4) The relationship between Δα, ΔΑ and W is:
Δα＝δt·TrαW,ΔΑ＝δt2·TrΑW
wherein the method comprises the steps of ,Trα＝OnesTαTω -1,TrΑ＝OnesTΑTω -1;
(5) The value of the rotated linear integral fit coefficient a k is obtained by a φ＝Trα＝OnesTαTω -1, the value of the linear integral fit coefficient B k is obtained by B V＝Trα＝OnesTαTω -1, and the value of the position linear integral fit coefficient C k is obtained by C P＝TrΑ＝OnesTΑTω -1, wherein ,Aφ＝[Ai Ai+1 … Aj],BV＝[Bi Bi+1 … Bj],CP＝[Ci Ci+1 … Cj].
Further, the nonlinear integral structure parameter optimization step comprises the following steps:
(1) The traditional rotation vector nonlinear integral compensation term expression, the traditional speed vector nonlinear integral compensation term expression and the traditional position vector nonlinear integral compensation term expression are respectively equivalent to the rotation vector nonlinear integral compensation term expression introducing the bilateral form, the speed vector nonlinear integral compensation term expression introducing the bilateral form and the position vector nonlinear integral compensation term expression introducing the bilateral form, namely:
(2) The constraint relation is as follows:
Βc11Βcij-Βc1jΒci1＝0(2≤i≤M1,2≤j≤N1)
Cc11Ccij-Cc1jCci1＝0(2≤i≤M2,2≤j≤N2)
(3) Under the limitation of the constraint relation, rotation vector nonlinear integral compensation coefficients ai m and par n, speed vector nonlinear integral compensation coefficients ai m and Br n, and position vector nonlinear integral compensation coefficients Cl m and Cr n are designed.
Still further, the conventional rotation vector nonlinear integral compensation term expression isThe traditional speed vector nonlinear integral compensation term expression is/>The traditional position vector nonlinear integral compensation term expression is/>Wherein, ac mn is a conventional rotation vector nonlinear integral compensation coefficient, b c mn is a conventional velocity vector nonlinear integral compensation coefficient, and Cc mn is a conventional position vector nonlinear integral compensation coefficient.
Compared with the prior art, the invention has the following advantages:
(1) According to the invention, a double-speed numerical integration structure is introduced into the inertial pre-integration of the inertial base combined motion measurement system for the first time, and inertial measurement sampling with different frequencies is adopted to implement linear integration and nonlinear integration in parallel, so that the calculation instantaneity is higher.
(2) According to the invention, the linear integral optimization design is carried out by using the assumption of the multi-segment polynomial for the first time, and the assumption of the single-segment polynomial is not adopted, so that the designed numerical integral has stronger adaptability, higher integral precision and better numerical stability.
(3) The invention uses the nonlinear integral structure in the form of bilateral compensation in the inertial pre-integral of the inertial base combined motion measurement system for the first time, and is not a traditional nonlinear integral structure, so that the real-time performance of the designed numerical integral calculation is higher.
Drawings
FIG. 1 is a flow chart of an optimization method of inertial pre-integration based on a double-speed numerical integration structure.
Detailed Description
For the purposes of clarity, technical solutions and advantages of embodiments of the present invention, the following descriptions will be taken with reference to the accompanying drawings and detailed description to clearly illustrate the spirit of the present disclosure, and any person skilled in the art, after having knowledge of the embodiments of the present disclosure, may make alterations and modifications by the techniques taught by the present disclosure without departing from the spirit and scope of the present disclosure. The exemplary embodiments of the present invention and the descriptions thereof are intended to illustrate the present invention, but not to limit the present invention.
Fig. 1 is a flowchart of the present embodiment, which is an optimization method of inertial pre-integration based on a two-speed numerical integration structure, specifically including the following steps:
Step (1), defining inertial pre-integration of a combined motion measurement system
The rigid body motion differential equation form is defined as follows:
Wherein, wa、w V and w P are the pose (quaternion) of the b-system relative to the w-system, the projection of the acceleration of the b-system relative to the w-system on the w-system, the projection of the b-system relative to the velocity of the w-system on the w-system, and the projection of the displacement of the b-system relative to the w-system on the w-system, respectively,/>And/>Respectively/> w V and w P derivatives over time,/>At time b, the rotational angular velocity of the system with respect to the system w,/>Is a quaternion multiplication.
By integrating the variables on both sides of formulas (1) - (3) over [ t i tj ], we can obtain:
Vj＝Vi+ΔVg+RiΔVij (5)
Pj＝Pi+ΔPv+ΔPg+RiΔPij (6)
Wherein, V j and P j are the pose (quaternion), speed and position, respectively, at time t j,/>V i and P i are the pose (quaternion), speed and position, respectively, at time t i,/>Δv ij and Δp ij are the attitude change (quaternion) from time t i to time t j, the velocity change due to the measured acceleration, and the position change due to the measured acceleration, respectively, Δv g and Δp g are the velocity change and the position change due to the gravitational acceleration from time t i to time t j, respectively, R i is the rotation matrix from b-line to w-line from time t i, and Δp v is the position change due to V i from time t i to time t j.
Step (2), introducing a double-speed numerical integration structure
Setting up and setting upThe equivalent rotation vector is phi ij, and the rotation vector phi ij is calculated as a two-speed rotation numerical integral, i.e.:
φij＝φ1ij+φ2ij (7)
Wherein, phi 1 ij and phi 2 ij are the linear integral term of the rotation vector and the rotation vector nonlinear integral compensation term in bilateral form, respectively, ω k (k=i, i+1,) j is the ideal inertial measurement angular velocity raw sample (the sampling interval of ω k is δt), ω ' m and ω ' n are the angular velocity samples extracted from the raw samples { ω k |k=i, i+1,., j } (the sampling intervals of ω ' m and ω ' n are δt φ,ω'm and the number of ω ' n are) ) A k is a rotation vector linear integral fitting coefficient, al m and ar n are rotation vector nonlinear integral compensation (also known as cone effect compensation) coefficients, Σ is a sum operator, and x is vector cross.
In addition, ΔV ij and ΔP ij are calculated using the following two-speed and two-speed position numerical integration forms, respectively:
ΔVij＝ΔV1ij+ΔV2ij (10)
ΔPij＝ΔP1ij+ΔP2ij (13)
Where Δv1 ij and Δv2 ij are the linear integral term and the bilateral form of the velocity vector nonlinear integral compensation term, respectively, Δp1 ij and Δp2 ij are the linear integral term and the nonlinear integral compensation term of the position vector, respectively, a k (k=i, i+1,., j) is the ideal inertial measurement acceleration raw sample (the sampling interval of a k is δt), ω 1,m is the angular velocity sample extracted from the raw samples { ω k |k=i, i+1,., j } (the sampling interval of ω 1,m is The number of ω 1,m is M 1),a1,n for the acceleration samples extracted from the original samples { a k |k=i, i+1,..j } (the sampling interval of a 1,n is/>The number of a 1,n is N 1),ω2,m, which is angular velocity samples extracted from the original samples { ω k |k=i, i+1,..The number of ω 2,m is M 2),a2,n for the acceleration samples extracted from the original samples { a k |k=i, i+1,..j } (the sampling interval of a 2,n is/>The number of a 2,n is N 2),Bk, C k, bl m and Br n, cl m and Cr n, and B.sub.L. are the linear integral fitting coefficients of speed vectors, the linear integral fitting coefficients of position vectors, the non-linear integral compensation (also known as rotation effect and pitch effect compensation) coefficients of speed vectors, and the non-linear integral compensation (also known as rotation effect and scroll effect compensation) coefficients of position vectors.
Step (3), optimally designing parameters of linear integral structure
The following provides a parameter optimization design process based on a linear integral structure, and the design process and the design method have generality.
First, an inertial vector definition of a multi-segment polynomial expression is given:
or written as:
Where ω (t) is the ideal measured inertia vector at time t, M is the number of terms of each segment polynomial (polynomial order is M-1), N is the number of segments of the piecewise polynomial function, g j,i (i=1, 2, M) is the coefficient vector of the j (j=1, 2, N) th segment polynomial, and t 0 is the initial time.
Let ω (t) be sampled at equal time intervals (denoted δt) over a period of time [ t 0,t0 + (m+n-2) δt ], namely:
or written as:
Where ω j (j=1, 2,., m+n-1) is the sampling of ω (t) at time t 0 + (j-1) δt.
Further, a matrix or vector is defined:
W1＝[(ωj)j,1]M×1,j＝1,2,...,M (20)
Wj-(M-1)＝[(ωj)1,1]1×1,j＝M+1,M+2,...,M+N-1 (21)
Tgω＝[((j-1)i-1)j,i]M×M (22)
TM＝Tgω(M,:) (23)
T'M＝Tgω(1:M-1,M) (26)
Gj＝[(gj,iδti-1)i,1]M×1,j＝1,2,...,N (27)
Wherein W 1 is an M x 1 column matrix with ω j (j=1, 2,., M) as the element, W j-(M-1) (j=m+1, m+2,., m+n-1) is a 1x 1 unitary matrix with ω j as the element, T gω is an M×M matrix with (j-1) i-1 as an element, T M is a row matrix formed by taking the M th row of matrix T gω, To take the (M-1) x (M-1) matrix of the first M-1 rows and the first M-1 columns of matrix T gω,For an (M-1) x M matrix formed by non-1 st column of the matrix T gω, T' M is an (M-1) x1 matrix formed by the elements of the first M-1 rows of the M th column of the matrix T gω, G j is an M x1 column matrix with G j,iδti-1 as the element,/>For taking the (M-1) x 1 matrix of the first M-1 elements of column matrix G j,/>Is a1 x1 matrix of unitary elements formed with the last element of the column matrix G j.
According to formulas (19) to (29), there are:
W1＝TgωG1 (30)
Further, supplementing the constraint relationship with respect to g j,i:
Then there are:
Wherein [ (-1) is the inverse operator of the matrix.
Further defining a matrix:
From the formula (31), the formula (33), the formula (36) and the formula (37), it can be deduced that:
further integrating equations (30) and (38), a matrix equation can be obtained:
W＝TωG (39)
W＝[W1 T W2 … WN]T (40)
Wherein [ (T) is a transpose operator taking a matrix, pi is a product operator, and O is a null matrix.
Next, the integral of the inertia vector defined by equation (16) or equation (17) is solved as follows:
or written as:
Wherein α (t) is the integral of the inertia vector ω (t) from time t 0 to time t (t 0≤t≤t0 + (m+n-2) δt).
From equation (43), the integral of the inertia vector ω (t) (denoted Δα) from time t 0 to time t 0 + (m+n-2) δt is obtained:
Further, a matrix is defined:
Wherein T gα is as follows Is a1 XM matrix of elements, T' gα is expressed as/>Is a1×m row matrix of elements.
Then formula (45) can be written as follows:
Further, a matrix is defined:
Ones＝[(1)1,i]1×N (49)
Wherein, O nes is a1 XN row matrix with 1 as an element.
Then equation (48) may be further written as follows:
Δα＝δt·OnesTαG (51)
Again, the integral (noted ΔΑ) of vector α (t) described by equation (43) or equation (44) over [ t 0,t0 + (m+n-2) δt ] is solved directly as follows:
And defining a matrix:
Wherein T Α1 is as follows Is a1 XM matrix of elements, T Α2 is the/>Is a1×m row matrix of elements.
Then equation (52) can be written as follows:
Further, a matrix is defined:
then formula (57) may be further written as follows:
ΔΑ＝δt2·OnesTΑG (59)
finally, the formula (39) is brought into the formulas (51) and (59), and the following can be obtained:
Δα＝δt·OnesTαTω -1W (60)
ΔΑ＝δt2·OnesTΑTω -1W (61)
formulas (60) and (61) may be further written as:
Δα＝δt·TrαW (62)
ΔΑ＝δt2·TrΑW (63)
Wherein,
Trα＝OnesTαTω -1 (64)
TrΑ＝OnesTΑTω -1 (65)
Let t i＝t0,tj＝t0 + (m+n-2) δt, and:
Aφ＝[Ai Ai+1 … Aj] (66)
BV＝[Bi Bi+1 … Bj] (67)
CP＝[Ci Ci+1 … Cj] (68)
If the inertial vector defined in equation (16) or equation (17) is taken as the inertial angular velocity, the value of the rotational linear integral fit coefficient a k,Ak in equation (8) is designed to be calculated using the following equation:
Aφ＝Trα＝OnesTαTω -1 (69)
If the inertial vector defined in equation (16) or equation (17) is the inertial acceleration, the values of the velocity linear integral fit coefficient B k in equation (11) and the position linear integral fit coefficients C k,Bk and C k in equation (14) are calculated by the following equations:
BV＝Trα＝OnesTαTω -1 (70)
CP＝TrΑ＝OnesTΑTω -1 (71)
Step (4), optimally designing nonlinear integral structure parameters
Conventional rotation vector nonlinear integral compensation terms typically take the form:
wherein, ac mn is a conventional rotation vector nonlinear integral compensation (also called cone effect compensation) coefficient.
For designing coefficients ai m and ar n in a bilateral form of a rotation vector nonlinear integral compensation term defined by formula (9) on the basis of a traditional rotation vector nonlinear integral compensation coefficient design method, letting:
From equation (73), the following relationship can be obtained:
Under the constraint relation determined by the formula (74), a rotation vector nonlinear integral compensation coefficient Abc mn is designed by adopting a traditional method, and then Ab m and Ab n are calculated according to the relation determined by the formula (73).
Further, the conventional velocity vector nonlinear integral compensation term is typically of the form:
the beta c mn is a conventional speed vector nonlinear integral compensation (also called rotation effect and pitch effect compensation) coefficient.
For designing coefficients beta m and Br n in a bilateral-form speed vector nonlinear integral compensation term defined by formula (12) on the basis of a traditional speed vector nonlinear integral compensation coefficient design method, letting:
According to equation (76), the following relationship can be obtained:
Βc11Βcij-Βc1jΒci1＝0,2≤i≤M1,2≤j≤N1 (77)
Under the limitation of the constraint relation determined by the formula (77), a speed vector nonlinear integral compensation coefficient beta c mn is designed by adopting a traditional method, and then beta m and Br n are calculated according to the relation determined by the formula (76).
Further, conventional position vector nonlinear integral compensation terms typically have the following form:
Wherein Cc mn is a conventional form of position vector nonlinear integral compensation (also known as rotation effect and scroll effect compensation) coefficient.
For designing coefficients Cl m and Cr n in the bilateral form of the position vector nonlinear integral compensation term defined by equation (15) on the basis of the conventional position vector nonlinear integral compensation coefficient design method, let:
From equation (79), the following relationship can be obtained:
Cc11Ccij-Cc1jCci1＝0,2≤i≤M2,2≤j≤N2 (80)
under the limitation of the constraint relation determined by the formula (80), the nonlinear integral compensation coefficient Cc mn of the position vector is designed by adopting a traditional method, and Cl m and Cr n are calculated according to the relation determined by the formula (79).
The examples are preferred embodiments of the present invention, but the present invention is not limited to the above-described embodiments, and any obvious modifications, substitutions or variations that can be made by one skilled in the art without departing from the spirit of the present invention are within the scope of the present invention.

Claims (3)

1. The optimization method of the inertial pre-integration based on the double-speed numerical integration structure is characterized in that the double-speed numerical integration structure is introduced in the design process of the inertial pre-integration of the combined motion measurement system, and the parameter optimization of the double-speed numerical integration structure is implemented;
the double-speed numerical integration structure comprises a double-speed rotation integration structure, a double-speed integration structure and a double-speed position integration structure;
the parameter optimization of the double-speed numerical integration structure comprises linear integration parameter optimization and nonlinear integration parameter optimization;
The double-speed rotation integrated structure is phi ij＝φ1ij+φ2ij,
Where φ 1 ij is a linear integral term of the rotation vector, φ 2 ij is a rotation vector nonlinear integral compensation term of bilateral form, ω k is an ideal inertial measurement angular velocity raw sample, ω k has a sampling interval δt, ω 'm and ω' n are angular velocity samples extracted from the raw samples { ω k |k=i, i+1,..A k is a rotation vector linear integral fitting coefficient, al m and ar n are rotation vector nonlinear integral compensation coefficients, Σ is a sum operator, x is a vector cross;
The two-speed integral structure is deltav ij＝ΔV1ij+ΔV2ij,
Wherein Δv1 ij is a linear integral term of a velocity vector, Δv ij is a speed vector nonlinear integral compensation term in a bilateral form, a k is an ideal inertial measurement acceleration original sample, a k has a sampling interval δt, ω 1,m is an angular velocity sample extracted from the original samples { ω k |k=i, i+1,..j }, and ω 1,m has a sampling interval/>The number M 1,a1,n is the acceleration sample extracted from the original sample { a k |k=i, i+1,..j } with a 1,n sample interval/>The number N 1,Bk is the linear integral fitting coefficient of the velocity vector, and Bl m and Br n are the nonlinear integral compensation coefficients of the velocity vector;
The two-speed position integration structure is deltap ij＝ΔP1ij+ΔP2ij,
Wherein Δp1 ij is a linear integral term of a position vector, Δp2 ij is a nonlinear integral compensation term of a position vector in a bilateral form, a k is an ideal inertial measurement acceleration original sample, a k has a sampling interval δt, ω 2,m is an angular velocity sample extracted from the original samples { ω k |k=i, i+1,..The number M 2,a2,n is the acceleration sample extracted from the original sample { a k |k=i, i+1,..j } with a 2,n sample interval/>The number N 2,Ck is the linear integral fitting coefficient of the position vector, and Cl m and Cr n are the nonlinear integral compensation coefficients of the position vector.
2. The optimization method of inertial pre-integration based on a two-speed numerical integration structure according to claim 1, wherein the linear integration parameter optimization step is:
(1) Defining an inertia vector of a multi-segment polynomial expression:
Wherein ω (t) is the ideal measured inertia vector at time t; m is the number of terms of each segment of polynomial; n is the number of segments of the piecewise polynomial function; g j,i is the coefficient vector of the j-th polynomial, and i=1, 2, M, j =1, 2, N; t 0 is the initial time; the order of the polynomials is M-1;
Defining equal time intervals δt sampling ω (t) over a period of time [ t 0,t0 + (m+n-2) δt ]:
Wherein ω j is the sampling of ω (t) at time t 0 + (j-1) δt, and j=1, 2.
The matrix of the relationship between ω j and g j,i is expressed as:
W＝TωG
Wherein, W= [ W 1 T W2 … WN]T, [] T Is the transpose operator of the matrix, pi is the product operator, O is the null matrix; w 1＝[(ωj)j,1]M×1 and j=1, 2,; w j-(M-1)＝[(ωj)1,1]1×1, and j=m+1, m+2,..m+n-1; g j＝[(gj,iδti-1)i,1]M×1 and j=1, 2,. -%, N; /(I)A 1x 1 unitary matrix of elements consisting of the last element of column matrix G j, where j=2, 3,..n; t gω＝[((j-1)i-1)j,i]M×M,TM is a row matrix formed by taking the M th row of matrix T gω,/> And j=2, 3, …, N-1; /(I) To take the (M-1) × (M-1) matrix consisting of the first M-1 rows and the first M-1 columns of matrix T gω,/>For the (M-1) x M matrix formed by non-1 st column of the matrix T gω, T' M is the (M-1) x 1 matrix formed by the elements of the first M-1 row of the M column of the matrix T gω, and [ (-1 ] is the inverse operator of the matrix;
(2) The inertia vector omega (t) is integrated once from the time t 0 to the time t 0 + (M+N-2) delta t to obtain an integrated vector delta alpha:
The matrix of the relationship between Δα and g j,i is expressed as:
Δα＝δt·OnesTαG
wherein the content of O nes＝[(1)1,i]1×N is at least one of, O nes is a1 x N row matrix with 1 as element,
(3) The inertia vector omega (t) is integrated twice from the time t 0 to the time t 0 + (M+N-2) delta t to obtain an integrated vector delta alpha:
The matrix of the relationship between ΔΑ and g j,i is expressed as:
ΔΑ＝δt2·OnesTΑG
Wherein the method comprises the steps of And j=2, 3, …, N,/>
(4) The relationship between Δα, ΔΑ and W is:
Δα＝δt·TrαW,ΔΑ＝δt2·TrΑW
wherein the method comprises the steps of ,Trα＝OnesTαTω -1,TrΑ＝OnesTΑTω -1;
(5) The value of the rotated linear integral fit coefficient a k is obtained by a φ＝Trα＝OnesTαTω -1, the value of the linear integral fit coefficient B k is obtained by B V＝Trα＝OnesTαTω -1, and the value of the position linear integral fit coefficient C k is obtained by C P＝TrΑ＝OnesTΑTω -1, wherein ,Aφ＝[Ai Ai+1 … Aj],BV＝[Bi Bi+1 … Bj],CP＝[Ci Ci+1 … Cj].
3. The optimization method of inertial pre-integration based on a two-speed numerical integration structure according to claim 2, wherein the nonlinear integration parameter optimization step is:
(1) The traditional rotation vector nonlinear integral compensation term expression, the traditional speed vector nonlinear integral compensation term expression and the traditional position vector nonlinear integral compensation term expression are respectively equivalent to the rotation vector nonlinear integral compensation term expression introducing the bilateral form, the speed vector nonlinear integral compensation term expression introducing the bilateral form and the position vector nonlinear integral compensation term expression introducing the bilateral form, namely:
(2) The constraint relation is as follows:
Βc11Βcij-Βc1jΒci1＝0,2≤i≤M1,2≤j≤N1
Cc11Ccij-Cc1jCci1＝0,2≤i≤M2,2≤j≤N2
(3) Under the limitation of constraint relation, rotation vector nonlinear integral compensation coefficients, namely, ab m and Ab n, speed vector nonlinear integral compensation coefficients, namely, ab m and Br n, and position vector nonlinear integral compensation coefficients, namely, cl m and Cr n are designed;
The expression of the traditional rotation vector nonlinear integral compensation term is The traditional speed vector nonlinear integral compensation term expression is/>The traditional position vector nonlinear integral compensation term expression is/>Wherein, ac mn is a conventional rotation vector nonlinear integral compensation coefficient, b c mn is a conventional velocity vector nonlinear integral compensation coefficient, and Cc mn is a conventional position vector nonlinear integral compensation coefficient.
CN202011161744.3A 2020-10-27 2020-10-27 Optimization method of inertial pre-integration based on double-speed numerical integration structure Active CN112464432B (en)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202011161744.3A CN112464432B (en) 2020-10-27 2020-10-27 Optimization method of inertial pre-integration based on double-speed numerical integration structure
PCT/CN2021/101261 WO2022088700A1 (en) 2020-10-27 2021-06-21 Dual-speed numerical integration structure based inertial pre-integration optimization method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011161744.3A CN112464432B (en) 2020-10-27 2020-10-27 Optimization method of inertial pre-integration based on double-speed numerical integration structure

Publications (2)

Publication Number Publication Date
CN112464432A CN112464432A (en) 2021-03-09
CN112464432B true CN112464432B (en) 2024-05-14

Family

ID=74835526

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011161744.3A Active CN112464432B (en) 2020-10-27 2020-10-27 Optimization method of inertial pre-integration based on double-speed numerical integration structure

Country Status (2)

Country Link
CN (1) CN112464432B (en)
WO (1) WO2022088700A1 (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112464432B (en) * 2020-10-27 2024-05-14 江苏大学 Optimization method of inertial pre-integration based on double-speed numerical integration structure

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110455301A (en) * 2019-08-01 2019-11-15 河北工业大学 A kind of dynamic scene SLAM method based on Inertial Measurement Unit
CN111256688A (en) * 2019-11-21 2020-06-09 中国航空工业集团公司西安飞行自动控制研究所 Pre-integration algorithm of inertial navigation system

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107909614B (en) * 2017-11-13 2021-02-26 中国矿业大学 Positioning method of inspection robot in GPS failure environment
JP6663610B2 (en) * 2017-12-08 2020-03-13 日章電機株式会社 Method and apparatus for measuring moment of inertia
CN109544638B (en) * 2018-10-29 2021-08-03 浙江工业大学 Asynchronous online calibration method for multi-sensor fusion
CN110044354B (en) * 2019-03-28 2022-05-20 东南大学 Binocular vision indoor positioning and mapping method and device
CN111780781B (en) * 2020-06-23 2022-05-06 南京航空航天大学 Template matching vision and inertia combined odometer based on sliding window optimization
CN112464432B (en) * 2020-10-27 2024-05-14 江苏大学 Optimization method of inertial pre-integration based on double-speed numerical integration structure

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110455301A (en) * 2019-08-01 2019-11-15 河北工业大学 A kind of dynamic scene SLAM method based on Inertial Measurement Unit
CN111256688A (en) * 2019-11-21 2020-06-09 中国航空工业集团公司西安飞行自动控制研究所 Pre-integration algorithm of inertial navigation system

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Suspension Force Modeling for a Bearingless Permanent Magnet Synchronous Motor Using Maxwell Stress Tensor Method;Xiaodong Sun等;IEEE;全文 *
基于双目视觉-惯性导航的轻型无人机导航算法;刘全攀等;兵工学报;全文 *
车辆模型对质心侧偏角估计的影响研究;陈建锋等;重庆理工大学学报(自然科学);全文 *

Also Published As

Publication number Publication date
WO2022088700A1 (en) 2022-05-05
CN112464432A (en) 2021-03-09

Similar Documents

Publication Publication Date Title
CN106774378B (en) A kind of UAV Flight Control and localization method
CN106595640A (en) Moving-base-object relative attitude measuring method based on dual-IMU-and-visual fusion and system
CN111551175B (en) Complementary filtering attitude resolving method of navigation attitude reference system
WO2020107931A1 (en) Pose information determination method and apparatus, and visual point cloud construction method and apparatus
CN108170297B (en) Real-time six-degree-of-freedom VR/AR/MR device positioning method
CN107063262A (en) A kind of complementary filter method resolved for UAV Attitude
CN111207745B (en) Inertial measurement method suitable for vertical gyroscope of large maneuvering unmanned aerial vehicle
CN112630813A (en) Unmanned aerial vehicle attitude measurement method based on strapdown inertial navigation and Beidou satellite navigation system
JPH10318778A (en) Method and device for finding position of satellite in the inside of it independently
CN112113566B (en) Inertial navigation data correction method based on neural network
CN106767767A (en) A kind of micro-nano multimode star sensor system and its data fusion method
CN109724597B (en) Inertial navigation resolving method and system based on function iteration integral
CN107664498A (en) A kind of posture fusion calculation method and system
CN111189474A (en) Autonomous calibration method of MARG sensor based on MEMS
CN108645404B (en) Method for resolving attitude of small multi-rotor unmanned aerial vehicle
CN103630146A (en) Laser gyroscope IMU (inertial measurement unit) calibration method combining discrete analysis and Kalman filtration
CN113587925B (en) Inertial navigation system and full-attitude navigation resolving method and device thereof
CN112464432B (en) Optimization method of inertial pre-integration based on double-speed numerical integration structure
CN108489485B (en) Error-free strapdown inertial navigation value updating method
CN113138608B (en) Four-rotor unmanned aerial vehicle vision servo control method using disturbance observer and nonlinear speed observer
DE2818202A1 (en) NAVIGATION DEVICE FOR LAND, AIR OR SEA VEHICLES
CN108416387B (en) Height filtering method based on fusion data of GPS and barometer
CN111207734B (en) EKF-based unmanned aerial vehicle integrated navigation method
WO2022057350A1 (en) Inertial pre-integration method for combined motion measurement system based on nonlinear integral compensation
JPS5910109B2 (en) Image geometric distortion correction device

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant