当前位置：网站首页>Analyze the pre integration of vio with less rigorous but logical mathematical theory

Analyze the pre integration of vio with less rigorous but logical mathematical theory

2022-07-23 12:13:00 【AutoGalaxy】

One . IMU Measurement model

Two . IMU Motion models （ Clear the goal first ： What the motion model does is from k The state of the moment recurs k+1 The state of the moment , Obviously, Markov chains ）

Let me introduce you The basic idea of traditional strapdown inertial navigation ：

Use Niuer , The derivative of position is equal to that of velocity , The derivative of velocity equals acceleration , Assume that the initial velocity and initial position of the carrier in the reference coordinate system are known , Using the acceleration information in the process of carrier motion , You can continue to perform integral operations , Real time update speed and location .

The above is only an ideal situation , Actually, we need to update the posture , Because the acceleration is measured by the accelerometer fixedly connected with the carrier , The acceleration at each moment is obtained under the current load system , Integrate velocity and position , The premise is to unify the acceleration into a coordinate system . The role of updating posture Right here ： Update posture in real time , The attitude of the current carrier system relative to the reference system can be obtained , Thus, the acceleration of the load system is projected under the reference system .

Quote... Here Huber A paragraph in the document written , Introduce the following knowledge points ：

（1） Recursive algorithm of traditional strapdown inertial navigation （ Important questions ： Handling gravity ）

（2） be based on BA Visual fusion algorithm （ There are still many doubtful points in this part , Absolutely impressive , Is there a more intuitive explanation for improving the optimization speed ？ At present, there is an explanation, but I don't know whether it is right , It has been posted on Zhihu , It's best to express it in mathematical formula , We will make a specific analysis later ）

IMU And visual integration makes IMU Of bias Considerable （ How to understand this sentence ？）

IMU And camera redundant measurement to suppress cumulative errors （ How to understand this sentence ？）

2.1 Preliminary knowledge

2.1.1 The derivative of the rotation matrix is derived by the spinor method

Let's talk first Rotation speed of rigid body It means ~~（ Spinor method ）

generally speaking , We use vectors to represent the position of objects , Use the rotation matrix to express the attitude of the object . The combination of vector and rotation matrix can use a homogeneous matrix （4*4） To express , Used to represent the position and posture of objects .

Velocity is the derivative of posture , Contains Derivative of position and Derivative of attitude . The derivative of position can be expressed by linear velocity , You can take the derivative of the vector and get , This kind of linear velocity understanding is more intuitive . But the rotational speed of the object （ Or the derivative of posture , Or the derivative of the rotation matrix ） How to express ？

If you use $\dot{R}$ Indicates the rotation speed , that $\dot{R}$ What is sacred ？ Is it right? R Take the derivative of each element in ？ The answer must be No , So how do we express the rotation speed ？ This is a key issue , To solve this problem , Please see the analysis below ！

Suppose we have an object , It performs a rotational motion in space , We don't consider its linear velocity , Only consider the angular velocity of its rotational motion ; We establish a fixed space coordinate system in space S（ Generally, it is the world coordinate system / Ignoring the rotation of the earth can also be called inertial system ）, Establish an object coordinate system on the object B, So we can Use the coordinate system B be relative to S The movement of Come on describe Motion of objects in space .

We know such a fact （ In quaternion （quaternion.pdf (krasjet.github.io)） One article also talked about ）： The object rotates freely in space , It can be equivalent to the rotation movement around a certain rotation axis at a certain speed .

Suppose the direction of the rotation angular velocity of the object is the unit vector $\vec {w}_{s}$ , Its rotation speed is $\dot{\theta}$ ; Then vector $\vec{w}=\dot{\theta}\vec {w}_{s}$ The direction of indicates the direction of angular velocity of rotation , The module length indicates the magnitude of the rotational angular velocity .

According to the robotics teacher of Taiwan University , A rotation matrix can be expressed by the following formula

$R=\[ \begin{matrix} \vec{X}_{B} &\vec{Y}_{B}&\vec{Z}_{B} \end{matrix} \]$

The above rotation matrix represents ： Coordinate system B The attitude of the . You can put B Coordinates in a coordinate system , Multiply the rotation matrix left , hold B The coordinates in the coordinate system change to S Coordinate system .

So show , Describe R The derivative of , Is to describe $\vec{X}_{B}\ \vec{Y}_{B}\ \vec{Z}_{B}$ The derivative of ; Three vectors , With $\vec{X}_{B}$ For example , $\vec{X}_{B}$ representative B Coordinate system X The unit vector of the axis in the reference coordinate system ; Find the derivative of the rotation matrix , It turns into the problem of finding the derivatives of three vectors . Or say ： The derivatives of these three vectors , combined , It becomes the derivative of the rotation matrix . The derivative of a vector is naturally easy to find .

We know , A vector $\vec{e}$ Around an axis $\vec{w}$ When rotating , The linear velocity of this vector $\dot{\vec{e}}$ It can be expressed as follows ：

$\dot{\vec{e}} = \vec{w} \times \vec{e}$

Considering that someone may not know the source , Now give a specific proof （ Proof given from the perspective of Pure Mathematics ）：

Proof：
Symbol description ： $R_{B}^{A}$ : The coordinate system B Relative to the coordinate system A The attitude of the
: $P^{B}$ ： Represents an object in the coordinate system B The coordinates under
It is known that ：
$RR^{T}=I$
Both sides have a
$\dot{R}R^{T}+R\dot{R}^{T}=0$
There are
$\dot{R}R^{T}=-R\dot{R}^{T}$
At this time
$S=\dot{R}R^{T}$
that S It's an antisymmetric matrix . hypothesis B There is an object in the coordinate system , Coordinate for $P^{B}$
$P^{A}=R_{B}^{A}P^{B}$
Suppose this object is relative to B The coordinate system never moves , So there are two ways to find the derivative of time （ commonly VIO in , There is body Coordinate system ,IMU Coordinate system , World coordinate system , Objects are relative B The coordinate system never moves , It's kind of similar IMU relative body Never move ）
$\dot{P}^{A}=\dot{R}_{B}^{A}P^{B}$
And because of $P^{B}=R_{A}^{B}P^{A}=(R_{B}^{A})^{T}P^{A}$ , Substituting into the above formula, we can get
$\dot{P}^{A}=\dot{R}_{B}^{A}(R_{B}^{A})^{T}P^{A}$
Again because $\dot{P}^{A}$ Defined as a vector $P^{A}$ Linear velocity of , therefore , The above formula can be further rewritten as
$\vec{V}^{A}=SP^{A}$
because S It's an antisymmetric matrix , So there is w^ = S（w It's a three-dimensional vector ,^ Stands for taking inverse symmetry ）
therefore , We can get it
$\vec{V}^{A}=\vec{w} \times P^{A}$
The meaning of the above formula is , The object is in A The derivative of the position under the system , be equal to , Coordinate system B Angular speed of rotation （ This part seems to be unable to explain intuitively through this way of proof , But that's what it actually means , undetermined ） And The object is in A Coordinate system position of the system Of Cross riding

thus , The above formula is proved .

below , Let's go back to the original question ： We asked for The derivative of the rotation matrix form , At present, the only thing we can use is $\dot{\vec{e}} = \vec{w} \times \vec{e}$ , And the above original problem can be equivalent to solving the rotation matrix $\vec{X}_{B}\ \vec{Y}_{B}\ \vec{Z}_{B}$ The derivative of , Then please , The following formula is established in sequence ：

$\dot{\vec{X}}_{B}=\vec{w} \times \vec{X}_{B}$

$\dot{\vec{Y}}_{B}=\vec{w} \times \vec{Y}_{B}$

$\dot{\vec{Z}}_{B}=\vec{w} \times \vec{Z}_{B}$

Let's write it in a more compact form ：

$[\dot{\vec{X}}_{B}\ \dot{\vec{Y}}_{B}\ \dot{\vec{Z}}_{B}]=[w \times \vec{X}_{B}\ w \times \vec{Y}_{B}\ w \times \vec{Z}_{B}]$

Defined here ：

$\dot{R} = [\dot{\vec{X}}_{B}\ \dot{\vec{Y}}_{B}\ \dot{\vec{Z}}_{B}]$

Because cross multiplication is antisymmetric to left multiplication matrix

therefore , In the end, you can get ：

$\dot{R}=w^{\wedge} R$

You think it's over ？ In fact, this is just an expression derived from the spinor method , With most SLAM The forms required inside are different , The above part is just a warm-up , post , For all of you to learn .

2.1.2 The derivative of rotation matrix is solved by Lie Algebraic Perturbation Model

$\frac{dR}{dt} \triangleq \lim_{\delta t \rightarrow 0} \frac{Re^{(w\delta t)^{\wedge}} - R}{\delta t}$

$\lim_{\delta t \rightarrow 0} \frac{Re^{(w\delta t)^{\wedge}} - R}{\delta t} = \lim_{\delta t \rightarrow 0} \frac{R(I+(w\delta t)^{\wedge}) - R}{\delta t}= \lim_{\delta t \rightarrow 0} \frac{R(w\delta t)^{\wedge}}{\delta t} = Rw^{\wedge}$

So let's i You can get ： $\dot{R}=Rw^{\wedge}$

2.2 IMU The motion model of （ We need to be clear about our goals ： according to k Acceleration value and angular velocity value at time and k The speed of the moment 、 Location 、 Posture , To push k+1 The speed of the moment 、 Location 、 Posture ）

2.2.1 Differential equation of motion model

combination 2.1.2 The proof of , And the basic idea of traditional strapdown inertial navigation , Can lead to IMU Differential equation of motion model ：

$\left\{ \begin{aligned} \dot{R}_{wb_{t}} &= R_{wb_{t}} (w^{b_{t}})^{\wedge}\\ \dot{v}_{t}^{w} &= a_{t}^{w} \\ \dot{p}_{wb_{t}} &= v_{t}^{w} \end{aligned} \right.$

2.2.2 Discretized by Euler method IMU Model recurrence equation

The two ends of the upper type are $[t,t+\Delta t]$ integral , There is the following formula （ Be careful , In the posture update part, you just intuitively understand , The more specific derivation and proof of estimation depends on the book of inertial navigation , At present, I don't understand ）

At this time will be IMU The measurement model is substituted into the discrete equation of motion ：

Be careful , In the middle of the above formula $n_{t}^{a}$ and $n_{t}^{b}$ It is a discrete random variable that obeys Gaussian distribution , The variance differs from the original continuous time by one $\Delta t$ factor , The discretization here will not be derived for the time being , I'll add later , One thing is for sure , This step can be rigorously deduced .

set up $\Delta t$ Fix , namely IMU The sampling frequency is fixed , Then the above formula can be written in a more general discrete form ：

3、 ... and . Officially began to introduce , Based on discrete Euler integral IMU Motion model IMU Preintegration

3.1 Computational analysis without pre integration

With IMU The time point of obtaining data is timestamp , Yes ... i,i+1,i+2,....,j-1,j,j+1... wait , however IMU The frequency of obtaining data is too fast , The camera is only i Time and j All the time , By analogy , interval (j-i) Time units appear once .

According to Euler integral , Can be （ Input ） i Moment to j-1 All the time IMU Measured value , as well as i The posture of the moment $R_{wb_{i}}$ , Location $p_{i}^{w}$ , Speed $v_{i}^{w}$ , obtain （ Output ）j The posture of the moment , Position and speed . Pay attention to input and output .

From the above formula , It can be seen that , Every time we optimize the algorithm , Will update $R_{wb_{i}}$ , $p_{i}^{w}$ and $v_{i}^{w}$ .

For attitude update , We asked for j The posture of the moment , Have to recalculate j-i Sub matrix multiplication can get j The posture of the moment , But also record the posture in the middle , Because the following speed update requires an intermediate posture （ Total records j-i A gesture ）;

For speed update , Use the posture update to save the posture , We have to calculate j-i Submatrix vector multiplication , as well as j-i Time in addition , To get it j The speed of the moment , At the same time, record the speed in the middle （ Total records j-i Speed ）;

For location update , To calculate 2*(j-i） Time in addition ,j-i Submatrix vector multiplication , To get the final j The location of the moment .

in summary , Update the posture every time 、 Speed and position , So in order to get the posture of the next moment 、 Speed and position , You have to calculate At least 3(j-i) Time in addition ,3(j-i) Submatrix vector multiplication above , To complete the recursion after an optimization update .

The above paragraph , It can be expressed in a more refined sentence ： Do not use pre integration , It will cause one update per time $R_{wb_{i}}$ , $p_{i}^{w}$ and $v_{i}^{w}$ , You have to calculate from scratch $R_{wb_{j}}$ , $p_{j}^{w}$ and $v_{j}^{w}$

（ tip ： In fact, the attitude and speed stored above imply a message , That is to do coordinate transformation , Every time we try to calculate intuitively in the world coordinate system , This leads to an increase in the amount of computation , Change the way of thinking , First relative calculation , Finally, change to the world coordinate system , Because those quantities in the middle are not of special concern to us , We only care about i and j What is the state of time in the world coordinate system , As for other moments , In fact, we are not particularly concerned about , The pre product component makes use of this ,（ Personal understanding is not too rigorous ））

About the formula of the above picture , Let me give some more explanations here , Maybe I can't understand it here ：

First of all, we need to Clear objectives ： Need by i,i+1,i+2,...,j-1 The moment IMU data , as well as i The state of the moment , Recurrence j The state of the moment .

The condition we know at present is 2.2.2 The general discretization form in section ：

That is, we can use Euler method from i moment Recurrence Out i+1 moment , And so on , To push j The moment , Here is a less rigorous proof of mathematical induction to illustrate ：

Proof：
from 2.2.2 Recursive form of the last graph in
We first deduction $R_{wb_{j}}$ How from the previous moment IMU The measured value and status are indicated
It is known that ：
$R_{wb_{i+1}}=R_{wb_{i}} e^{((\widetilde{w}_{i}^{b}-n_{k}^{g}-b_{k}^{g})\Delta t)^{\wedge}}$
$R_{wb_{i+2}}=R_{wb_{i+1}} e^{((\widetilde{w}_{i+1}^{b}-n_{k}^{g}-b_{k}^{g})\Delta t)^{\wedge}}$
......
By analogy , You can get
Let's deduce $v_{j}^{w}$ How is it from the previous IMU The measured value and the status indicate ：
It is known that
By analogy , In the end, you can get ：
Among them $\Delta t_{ij} = (j-i)\Delta t$ .
Finally, we can deduce $p_{j}^{w}$ How is it from the previous IMU The measured value and status are indicated ：
It is known that ：

$p_{i+1}^{w}=p_{i}^{w}+v_{i}^{w}\Delta t + \frac{1}{2}(R_{wb{i}}(a_{i}^{b}-b_{i}^{a}-n_{i}^{a})-g^{w})\Delta t^{2}$
$p_{i+2}^{w}=p_{i+1}^{w}+v_{i+1}^{w}\Delta t + \frac{1}{2}(R_{wb{i+1}}(a_{i+1}^{b}-b_{i+1}^{a}-n_{i+1}^{a})-g^{w})\Delta t^{2}$
......
By analogy , In the end, you can get ：

thus , Everything is certified .

3.2 In order to solve the problem of the above calculation amount , The pre product component is introduced ( It's defined in i Frame j Between frames )

Before starting the detailed description , I still want to emphasize one point ： After introducing the pre product component , our ultimate objective Yes, I hope that according to the following input ：

Input ：（1） Pre product component

（2）i The posture of the moment 、 Speed 、 Location

Only based on the above existing data , The following output is obtained

Output ：（1）j The posture of the moment 、 Speed 、 Location

You can find , At this time, the input has an extra pre product component . Next we need to construct these inputs

3.2.1 Construct the pre product component

In order to make after introducing the pre product component , It can reduce the amount of calculation , Then relative relationship is very important at this time . In fact, it's really like this when you think about it intuitively , hypothesis IMU stay i There is always a coordinate system , stay j There is always a coordinate system , that j The coordinate system is relative to i In fact, the coordinate system is always the same .

Let's first look at the discretized Euler integral based IMU Motion models ：（3.1 In the model ）

（1） The first pre product component we want to introduce is the blue box of the first expression above

Defined as ：

basis i,...,j-1 The moment IMU Gyroscope data can be used to calculate this pre integration component .

（2） The second pre product component introduced is part of the yellow box of the second expression above

First deform from the Yellow frame ：

At this time, let the velocity pre product component be

In the above formula , First equation , need $R_{b_{i}b_{k}}$ , as well as i To j-1 The moment Accelerometer value , To find the velocity pre product component ;

that $R_{b_{i}b_{k}}$ How can I ask ？

We might as well expand and change the shape $R_{b_{i}b_{i+1}},R_{b_{i}b_{i+2}},...$ ：

from 3.1 The recurrence formula of the middle posture can get ：

$R_{wb_{k}}=R_{wb_{i}}R_{b_{i}b_{k}}$

So we can get ：

therefore , I guess when calculating the attitude pre product component , Do you have to open up a piece of memory to record ： $R_{b_{i}b_{i+1}},R_{b_{i}b_{i+2}},...$

In fact, there is such a relationship ： $R_{b_{i}b_{k}}=(R_{wb_{i}})^{T}R_{wb_{k}}=\Delta R_{ik}$

（3） The third pre product component is not so intuitive , The position pre product component is defined in the following red box ,

To calculate this pre product score, you need The acceleration 、 $R_{b_{i}b_{k}}$ , $\Delta v_{ik}$

The acceleration can be read , $R_{b_{i}b_{k}}$ stay （2） It has been stated in （1） What you want , Now, now $\Delta v_{ik}$ How can I ask ？ I guess I also use （2） The pre integral formula in is calculated in advance

thus , The pre product component has been derived , How to obtain the input value calculation is also roughly explained , Bring in the pre product component to get IMU The recurrence formula of the motion model is as follows ：

Obviously , The pre product component is a fixed value , Only the state quantity changes , therefore , The amount of calculation is greatly reduced .

Four . Pre integrated measurement value and measurement noise （ The only goal of this section ： The calculation formula of noise covariance of pre integration measurement is given ）

Accuracy of recursion , It is also required that the pre product component be measured accurately ; Here, the pre integral is regarded as a random variable , Have ideal value ” Add “ The form of white noise .

Before derivation , Made the following assumptions ： In the interval of pre integral calculation bias They are equal. （ The interval of pre integration calculation is usually between two frames of the camera ）

4.1 Pre integration of rotation

The definition is given directly here , Qiu Bo's documents have been written quite clearly , I'll make further details next , You can get the final result

Finally, we get the following formula （ What we care about , The pre product component can be determined by the measured value ” Add “ Measuring noise ）

4.2 Velocity pre product component

Specific proof , Qiu Bo's document has clear proof , I will not explain here

4.3 Position pre product component

Empathy , Qiu Bo's documentation is very clear , No more explanation here

4.4 Measured value = Ideal value + noise

Go through the above steps , In the end, I got IMU Pre product component Form like Measured value = Ideal value + noise In the form of

4.5 Analyze the noise of pre integration （ Clear objectives ： Give the calculation expression of covariance ）

To be continued .....

原网站

版权声明
本文为[AutoGalaxy]所创，转载请带上原文链接，感谢
https://yzsam.com/2022/204/202207230538571149.html