Sensor to sensor calibration of the integrated INS/vision navigation system

Abstract

Due to the essential limitation of optical sensors, its integration with traditional Inertial Navigation System (INS) tends to be the focus in indoor navigation applications. In a low-cost INS/vision integrated navigation system, the relative position and gestures between slave systems are important coefficients. In order to solve the initial bias estimation problems involved in INS/vision integrated system, this article proposes a novel alignment approach based on the time-domain constraints. At first, on the basis of traditional initial alignment model, the time-domain constrained model is deduced, in which the time-related states and measurements are all modeled. In order to verify the advantage of state evaluability, both the traditional alignment model and the corresponding time-domain constrained model are analyzed via the nonlinear observability analysis method. At the end of this article, two groups of numerical simulations are implemented, and the corresponding results validate the effectiveness of the proposed time-domain constrained model.

Introduction

Visual navigation has received a lot of research interest in recent years for a number of reasons. Cameras are cheap, light, and have low power requirements when compared to other sensors for localization such as laser scanners. ^1,2 This makes them attractive sensors for low-cost situations or for applications where size and weight need to be tightly controlled, such as for micro aerial vehicles in a global navigation satellite system (GNSS)-denied environment. ³ In addition, cameras are also passive sensors, unlike sonar, radar, and laser scanners, which makes them useful in surveillance applications as they are difficult to detect and do not interfere with the environment they are observing. In addition, a large amount of information can also be obtained from a sequence of images which allows the motion of the platform to be tightly constrained and for highly detailed maps to be constructed. This is especially useful when loop closure occurs as it can be easily detected by image matching.

Traditional visual navigation usually uses a stereo visual system, which can directly provide the 3-dimensional (3-D) information of circumstance, and the position of cameras can be easily estimated by utilizing the visual difference come from multiple vision sensors. ⁴ Whereas the accuracy of stereo visual navigation is limited by the length of baseline, this problem is crucial especially in applications that the baseline is seriously limited, such as remote sensing, micro-unmanned aerial vehicles (UAVs), and so on. Therefore, the monocular visual navigation tends to be more general and commonly used.

According to the previous contributions, monocular visual navigation has been carried out either by local optimization of key frames ⁵ or by filtering. ^{6 –8} Since cameras are projective sensors providing bearing-only observations, observations from a single image cannot provide an estimate of the range to features, and if a single camera is the only sensor used, the true scale of the position and mapping is not observable, no matter which class of navigation algorithm is chose. This is one of the main drawbacks of using cameras for navigation. ^9,10 Comparatively, an Inertial Navigation System (INS) is capable of tracking the position, velocity, and attitude of a vehicle. This dead-reckoning process, however, cannot be used over extended periods of time because the errors in the computed estimates continuously increase. The high-dynamic motion measurements of the inertial measurement unit (IMU) are used to support the vision algorithms by providing accurate predictions where features can be expected in the upcoming frame. ¹¹ The combination of vision and inertial sensors is very suitable for a wide range of robotics applications. ^12,13

In the aforementioned traditional integrated navigation approaches, due to the large number and rate of observations from cameras and inertial unit, there exists large amount of computationally intensive processing that needs to be performed on the images to extract and match feature points. ⁷ Yang and Shen ¹⁴ introduce a probabilistic, optimization-based initialization method, where the sensors work under a tightly coupled structure. This article proposes a novel time-domain-related model with less computational load. On the basis of this model, we also provide an effective solution about this nonlinear optimization problem.

Problem formulation

In this section, we will give a more formal formulation of the problem we are solving. The coordinate frames that are used are introduced as follows:

Camera frame (C): This coordinate frame is attached to the moving camera. Its origin is located in the optical center of the camera, with the z-axis pointing along the optical axis.

Body frame (B): This is the coordinate frame of the strapdown IMU, and it is rigidly connected to the C frame. All the inertial measurements are resolved in this coordinate frame.

Image frame (I): This is the two-dimensional coordinate frame of the camera images. It is located on the image plane, which is perpendicular to the optical axis.

World frame (W): This is the only static coordinate that is involved in this article, and it is seen as the reference frame. The pose of all the aforementioned frames is estimated with respect to W frame. The 3-D feature positions are, without loss of generality, assumed to be constant and known in this frame. It is fixed to the environment and can be aligned in any direction. However, preferably it should be vertically aligned.

$R_{\mathit{\text{a}}}^b$ = Direction cosine matrix between a and b frame.

$q_{\mathit{\text{a}}}^b$ = Rotation quaternion from a to b frame.

$r_{\mathit{\text{a}}}^b$ = Relative displacement vector between a and b frame.

${\mathit{\text{b}}}_g$ , ${\mathit{\text{b}}}_a$ = Instrumental bias of gyros and accelerators in b frame.

$V_{\mathit{\text{a}}}^b$ = Relative velocity vector between a and b frame, m/s.

${\mathbf{\omega }}_{\mathit{\text{a}}}^b$ = Relative angular velocity between a and b frame, rad/s.

${{}^{c}r}_{\mathit{\text{a}}}^b,{{}^{c}V}_{\mathit{\text{a}}}^b,{{}^c\omega }_{\mathit{\text{a}}}^b$ = Description of $r_{\mathit{\text{a}}}^b,V_{\mathit{\text{a}}}^b,{\omega }_{\mathit{\text{a}}}^b$ in c frame.

${\mathit{\text{C}}}_k$ = Camera optical-centered coordinate system in kth step.

${\mathit{\text{I}}}_k$ = Image coordinate system in kth step.

${\mathrm{\text{Π}}}_C(k)$ = Optical-centered plane, xoy plane of ${\mathit{\text{C}}}_k$ .

${\mathrm{\text{Π}}}_I(k)$ = Image plane, xoy plane of ${\mathit{\text{I}}}_k$ .

The traditional alignment algorithm usually contains two steps, which are named as filter initialization and estimation. The basic model of the integrated system is introduced as follows.

In this article, the state of the system is selected as

$\begin{array}{l}\mathit{\text{x}}\left(t\right)=[\begin{array}{ccccc}{\left({{}^W\mathit{\text{r}}}_W^B\left(t\right)\right)}^{\text{T}}& {\left({\mathit{\text{q}}}_{\mathit{\text{B}}}^W\left(t\right)\right)}^{\text{T}}& {\left({}^W\mathit{\text{v}}{\mathbf{\text{a}}}_B^W\left(t\right)\right)}^{\text{T}}& {\left({}^W\mathit{\text{g}}\right)}^{\text{T}}& {\left({\mathit{\text{b}}}_g\left(t\right)\right)}^{\text{T}}\end{array}\\ {\begin{array}{ccccc}{\left({\mathit{\text{b}}}_a\left(t\right)\right)}^{\text{T}}& {\left({}^C\mathit{\text{r}}{ }_C^B\right)}^{\text{T}}& {\left({\mathit{\text{q}}}_{\mathit{\text{C}}}^B\right)}^{\text{T}}& {\left({\mathit{\omega }}_B^{ }\left(t\right)\right)}^{\text{T}}& {\left({}^W\mathit{\text{a}}{\mathbf{\text{a}}}_B^W\left(t\right)\right)}^{\text{T}}]\end{array}}^{\text{T}}\end{array}$

1

The dynamic process of state can be depicted as

${}^W\dot{\mathit{\text{r}}}{ }_W^B={}^w\mathit{\text{v}}{ }_B^W,{\dot{\mathit{\text{q}}}}_{\mathit{\text{B}}}^W=\frac{1}{\text{2}}\Omega \left({\left({\mathit{\omega }}_B^{ }\right)}^{\text{T}}\right){\mathit{\text{q}}}_{\mathit{\text{B}}}^W,{}^W\dot{\mathit{\text{v}}}{ }_B^W={}^W\mathit{\text{a}}{ }_B^W$

2

${}^W\dot{\mathit{\text{g}}}={\mathit{0}}_{3\times 1},{\dot{\mathit{\text{b}}}}_g={\mathit{\text{n}}}_{g\mathit{\omega }},{\dot{\mathit{\text{b}}}}_a={\mathit{\text{n}}}_{a\mathit{\omega }},{{}^C\dot{\mathit{\text{r}}}}_C^B={\mathit{0}}_{3\times 1},{\dot{\mathit{\text{q}}}}_{\mathit{\text{C}}}^B={\mathit{0}}_{4\times 1}$

3

In equation (3), the bias of gyros and accelerators is assumed as first-order Markov processes, and then the derivative of them can be written as the white noise, as presented by ${\mathit{\text{n}}}_{g\omega }$ and ${\mathit{\text{n}}}_{a\omega }$ . The motion of the whole system is supposed to satisfy the invariant acceleration and invariant angular velocity model, and then the angular velocity and derivative of acceleration are supposed to be white noise, ${\mathit{\text{n}}}_{\omega }$ and ${\mathit{\text{n}}}_a$ . Then we have

${\mathit{\omega }}_B={\mathit{\text{n}}}_{\mathit{\omega }},{{}^W\dot{\mathit{\text{a}}}}_B^W={\mathit{\text{n}}}_a$

4

In equation (2), $\mathit{\Omega }\left(\mathit{\omega }\right)$ is the quaternion matrix, and it can be written as ¹⁵

$\Omega \left(\omega \right)\text{=}\left[\begin{array}{cc}0& -{\omega }^{\text{T}}\\ \omega & -\left[\omega \times \right]\end{array}\right]$

5

Equations (2) to (4) are the traditional state equations of the integrated system. In INS system, the angular velocity and the acceleration can be measured directly in the body coordinate

${\hspace{0.17em}}_m\mathit{\omega }{ }_B^{ }={\omega }_B^{ }+{\omega }_{\text{cmd}}\text{+}{b}_g$

6

${\hspace{0.17em}}_m\mathit{\text{a}}{ }_{\mathit{\text{B}}}^W={\left(R\left(q_{\mathit{\text{I}}}^w\right)\right)}^{\text{T}}\left({\hspace{0.17em}}^{W}a{ }_{\mathit{\text{B}}}^W-{}^{W}g\right)+b_a$

7

Herein, $R\left(q\right)$ is the rotation matrix corresponding to the quaternion and ${\omega }_{\text{cmd}}^{ }$ is the command angular velocity of the INS platform. As previous researchers did, we also select the feature points in sampled images as the measurements of the filter. Based on the pinhole camera model, the measurement equations can be written as

$y_i=\left[\begin{array}{c}{\mathit{\text{u}}}_i\\ {\mathit{\text{v}}}_i\\ 1\end{array}\right]=K\cdot {\left[\begin{array}{ccc}\frac{{\mathit{\text{x}}}_i}{{\mathit{\text{z}}}_i}& \frac{{\mathit{\text{y}}}_i}{{\mathit{\text{z}}}_i}& 1\end{array}\right]}^{\text{T}}$

8

and

${\left[\begin{array}{ccc}{\mathit{\text{x}}}_i& {\mathit{\text{y}}}_i& {\mathit{\text{z}}}_i\end{array}\right]}^{\text{T}}={}^{C}r{ }_C^{{\mathit{\text{p}}}_i}=R_{\mathit{\text{B}}}^C\cdot R_{\mathit{\text{W}}}^B\left({}^{W}r{ }_W^{{\mathit{\text{p}}}_i}-{}^{W}r{ }_W^B\right)+{}^{C}r{ }_C^B+{\eta }_i$

9

where K is the intrinsic matrix, ${\mathit{\text{η}}}_i$ is the measuring noise of feature points, which is supposed as white noise sequence with variance $R_i$ . If many features are measured, the corresponding measurement is $y={\left[\begin{array}{ccc}{y}_1^{\text{T}}& \mathrm{...}& y_n^{\text{T}}\end{array}\right]}^{\text{T}}$ , and the measuring noise matrix is $R=\text{diag}\left(R_1,\mathrm{...},R_n\right)$ . Now the whole measurements can be written as

$h_0={\left[\begin{array}{ccc}{\hspace{0.17em}}_m\mathit{\omega }{ }_B^{ }& {\hspace{0.17em}}_m\mathit{\text{a}}{ }_{\mathit{\text{B}}}^W& y_i\end{array}\right]}^{\text{T}}$

10

In the aforementioned integrated model, there exist some error elements, such as inertial device error, image feature errors, and so on. In order to obtain more accurate initial alignment result, this article considers a novel time-domain constrained optimized model in the following section.

Time-domain constraints description

In Figure 1, the relationship between the coordinate frames is illustrated. The camera and the IMU are rigidly connected, that is, $R_{\mathit{\text{B}}}^C$ and $r_{\mathit{\text{B}}}^C$ are constant.

Figure 1.

The sensor unit, shown at two time instants, t(k-1) and t(k), consists of an IMU (B frame) and a camera (C frame). These frames are rigidly connected. The position of the sensor unit with respect to the world frame (W) changes over time as the unit is moved.

According to the two time-step motion, the relationship between the pose coefficients can be modeled as follows.

At first, in two adjacent time steps, the relation of rotation matrix is

$R_{C_{k-1}}^{{\mathit{\text{C}}}_k}=R_{{\mathit{\text{B}}}_k}^{{\mathit{\text{C}}}_k}\cdot R_{B_{k-1}}^{{\mathit{\text{B}}}_k}\cdot R_{C_{k-1}}^{B_{k-1}}$

11

Note that every rotation matrix R is corresponding to a gesture vector, which is depicted as $\varphi \left(R_{C_{k-1}}^{{\mathit{\text{C}}}_k}\right)$ , for example. The conjunction of INS and camera is supposed as rigid; therefore, the INS and the camera have the same angular velocity, and we have

$\varphi \left(R_{C_{k-1}}^{{\mathit{\text{C}}}_k}\right)\text{=}{\omega }_B^{ }\cdot \Delta \mathit{\text{t}}$

12

Suppose the camera-gesture-related measurement is $h_1$ , then the corresponding measuring equation is

$h_1\text{=}{\mathit{\omega }}_B^{ }\cdot \Delta \mathit{\text{t}}$

13

In addition, the transition vector of this integrated system has the following constraints

$\begin{array}{c}{\hspace{0.17em}}^{\mathit{\text{W}}}{r}_{C_{k-1}}^{{\mathit{\text{C}}}_k}={\hspace{0.17em}}^W{r}_{C_{k-1}}^{B_{k-1}}+{\hspace{0.17em}}^W{r}_{B_{k-1}}^{{\mathit{\text{B}}}_k}+{\hspace{0.17em}}^W{r}_{{\mathit{\text{B}}}_k}^{{\mathit{\text{C}}}_k}\\ =R_{C_{k-1}}^W{\hspace{0.17em}}^{C_{k-1}}{r}_{C_{k-1}}^{B_{k-1}}-R_{{\mathit{\text{C}}}_k}^W{\hspace{0.17em}}^{{\mathit{\text{C}}}_k}{r}_{{\mathit{\text{C}}}_k}^{{\mathit{\text{B}}}_k}+({\hspace{0.17em}}^W{r}_W^{{\mathit{\text{B}}}_k}-{\hspace{0.17em}}^W{r}_W^{{\mathit{\text{B}}}_{\mathrm{k-1}}})\end{array}$

14

According to the rigid conjunction, suppose we have

${\hspace{0.17em}}^{C_{k-1}}{r}_{C_{k-1}}^{B_{k-1}}={\hspace{0.17em}}^{{\mathit{\text{C}}}_k}{r}_{{\mathit{\text{C}}}_k}^{{\mathit{\text{B}}}_k}={\hspace{0.17em}}^C{r}_{\mathit{\text{C}}}^B$

15

The second group of measurements is the camera transition–related constraints, which can be written as

$h_2\text{=}{R}_{C_{k-1}}^W\left(I_{3\times 3}-R_{{\mathit{\text{C}}}_k}^{C_{k\text{-}1}}\right){\hspace{0.17em}}^{{\mathit{\text{C}}}_k}{r}_{\mathit{\text{C}}}^B+{\hspace{0.17em}}^W{v}_{\mathit{\text{B}}}^W\cdot \Delta \mathit{\text{t}}$

16

Equations (13) and (16) form the measurements in traditional INS/vision integrated system. In fact, we can also utilize more time-domain constraints in longer image sequence to improve the ego-motion estimation accuracy

$h_1\{\begin{array}{c}{R}_{C_{k-1}}^{{\mathit{\text{C}}}_k}=R_{{\mathit{\text{B}}}_k}^{{\mathit{\text{C}}}_k}\cdot R_{B_{k-1}}^{{\mathit{\text{B}}}_k}\cdot R_{C_{k-1}}^{B_{k-1}}\\ R_{C_{k-2}}^{{\mathit{\text{C}}}_k}=R_{{\mathit{\text{B}}}_k}^{{\mathit{\text{C}}}_k}\cdot R_{B_{k-2}}^{{\mathit{\text{B}}}_k}\cdot R_{C_{k-2}}^{B_{k-2}}\\ \mathrm{...}\\ R_{C_{k-n}}^{{\mathit{\text{C}}}_k}=R_{{\mathit{\text{B}}}_k}^{{\mathit{\text{C}}}_k}\cdot R_{B_{k-n}}^{{\mathit{\text{B}}}_k}\cdot R_{C_{k-n}}^{B_{k-n}}\end{array}$

17

$h_2\{\begin{array}{c}{\hspace{0.17em}}^W{r}_{C_{k-1}}^{{\mathit{\text{C}}}_k}\text{=}{R}_{C_{k-1}}^W\left(I_{3\times 3}-R_{{\mathit{\text{C}}}_k}^{C_{k\text{-}1}}\right){\hspace{0.17em}}^{{\mathit{\text{C}}}_k}{r}_{\mathit{\text{C}}}^B+{\displaystyle {\int }_{k-1}^k{\hspace{0.17em}}^W{v}_{\mathit{\text{B}}}^W}\cdot \mathit{\text{d}}t\\ {\hspace{0.17em}}^W{r}_{C_{k-2}}^{{\mathit{\text{C}}}_k}\text{=}{R}_{C_{k-2}}^W\left(I_{3\times 3}-R_{{\mathit{\text{C}}}_k}^{C_{k\text{-2}}}\right){\hspace{0.17em}}^{{\mathit{\text{C}}}_k}{r}_{\mathit{\text{C}}}^B+{\displaystyle {\int }_{k-2}^k{\hspace{0.17em}}^W{v}_{\mathit{\text{B}}}^W}\cdot \mathit{\text{d}}t\\ \mathrm{...}\\ {\hspace{0.17em}}^W{r}_{C_{k-n}}^{{\mathit{\text{C}}}_k}\text{=}{R}_{C_{k-n}}^W\left(I_{3\times 3}-R_{{\mathit{\text{C}}}_k}^{C_{k\text{-n}}}\right){\hspace{0.17em}}^{{\mathit{\text{C}}}_k}{r}_{\mathit{\text{C}}}^B+{\displaystyle {\int }_{k-n}^k{\hspace{0.17em}}^W{v}_{\mathit{\text{B}}}^W}\cdot \mathit{\text{d}}t\end{array}$

18

Equations (17) and (18) constitute the time-domain constraint-related measurements.

Observability analysis

On the basis of the time-domain constrained model, the model estimation problem is analyzed in this section. In the state estimation process, system observability is an important issue in state estimation problem. Observability analysis provides a direct understanding of the fundamental limits of the obtainable solutions, regardless of process and measurement noises. The standard pose estimation formulation is a strong nonlinear process. ¹⁶ For this analysis, the nonlinear system will be approximated at each time step as a linear system, always along with time-varying coefficients. Researchers usually use the Hermann approach to analyze the observability of nonlinear system. ¹⁷

Unlike the traditional integrated model, the time-domain constrained model has more time-related measurements, as depicted in the previous section. The involved measurements can be presented as $h={\left[\begin{array}{cccc}{h}_0& h_1& h_2& h_3\end{array}\right]}^{\text{T}}$ . Herein, $h_0$ contains three elements: $h_{01}={\hspace{0.17em}}_m{\mathbf{\omega }}_B$ , $h_{02}={\hspace{0.17em}}_m{a}_B^{\mathit{\text{W}}}$ , and $h_{03}=y_i$ . $h_{\text{3}}$ contains two components: $h_{3\text{1}}={\left(q_{\mathit{\text{B}}}^W\right)}^{\text{T}}\left(q_{\mathit{\text{B}}}^W\right)$ and $h_{3\text{2}}={\left(q_{\mathit{\text{C}}}^B\right)}^{\text{T}}\left(q_{\mathit{\text{C}}}^B\right)$ . All the Lie derivatives of the above elements can be written as

$L^0{h}_{01}={\mathbf{\omega }}_B^{ }+b_g$

19

$L^0{h}_{02}=R{\left(q_{\mathit{\text{B}}}^w\right)}^{\text{T}}\left({\hspace{0.17em}}^W{a}_{\mathit{\text{B}}}^W-{}^{W}g\right)+b_a$

20

$L^0{h}_{03}=y_i$

21

$L^0{h}_1={\mathbf{\omega }}_B^{ }\cdot \Delta \mathit{\text{t}}$

22

$L^0{h}_2=R_{C_{k-1}}^W\left(I_{3\times 3}-R_{{\mathit{\text{C}}}_k}^{C_{k\text{-}1}}\right){\hspace{0.17em}}^C{r}_{\mathit{\text{C}}}^B+{\hspace{0.17em}}^W{V}_{\mathit{\text{B}}}^W\cdot \Delta \mathit{\text{t}}$

23

$L^0{h}_3={\left[\begin{array}{cc}{\left(q_{\mathit{\text{B}}}^W\right)}^{\text{T}}\left(q_{\mathit{\text{B}}}^W\right)& {\left(q_{\mathit{\text{C}}}^B\right)}^{\text{T}}\left(q_{\mathit{\text{C}}}^B\right)\end{array}\right]}^{\text{T}}$

24

Their gradients are as follows

$\nabla L^0{h}_{01}=\left[0_{3\times 3}{0}_{3\times 4}{0}_{3\times 3}{0}_{3\times 3}{I}_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}{0}_{3\times 4}{I}_{3\times 3}{0}_{3\times 3}\right]$

25

$\begin{array}{l}\nabla L^0{h}_{02}=[0_{3\times 3}{\left(J\left(R\left(q_{\mathit{\text{B}}}^w\right)\right)\right)}^{\text{T}}\cdot \left({\hspace{0.17em}}^W{a}_{\mathit{\text{B}}}^W-{}^{W}g\right)0_{3\times 3}\\ -R{\left(q_{\mathit{\text{B}}}^w\right)}^{\text{T}}{0}_{3\times 3}{I}_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}R{\left(q_{\mathit{\text{B}}}^w\right)}^{\text{T}}\text{]}\end{array}$

26

$\nabla L^0{h}_{03}=\left[\frac{\partial y_i}{{\partial }^W{r}_{\mathit{\text{W}}}^B}\frac{\partial y_i}{\partial q_{\mathit{\text{B}}}^W}{0}_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}\frac{\partial y_i}{{\partial }^W{r}_{\mathit{\text{C}}}^B}\frac{\partial y_i}{\partial q_{\mathit{\text{C}}}^B}{0}_{3\times 3}{0}_{3\times 3}\right]$

27

$\nabla L^0{h}_{\text{1}}=\left[0_{3\times 3}{0}_{3\times \text{4}}{0}_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}\Delta \mathit{\text{t}}\cdot I_{3\times 3}{0}_{3\times 3}\right]$

28

$\begin{array}{l}\nabla L^0{h}_2=\text{[}\hspace{0.17em}{0}_{3\times 3}\frac{\partial R_{C_{k-1}}^W}{\partial q_{\mathit{\text{B}}}^W}\left(I_{3\times 3}-R_{{\mathit{\text{C}}}_k}^{C_{k\text{-}1}}\right){\hspace{0.17em}}^C{r}_{\mathit{\text{C}}}^B\Delta \mathit{\text{t}}\cdot I_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}{0}_{3\times 3}\\ R_{C_{k-1}}^W\left(I_{3\times 3}-R_{{\mathit{\text{C}}}_k}^{C_{k\text{-}1}}\right)\frac{\partial R_{C_{k-1}}^W}{\partial q_{\mathit{\text{C}}}^B}\left(I_{3\times 3}-R_{{\mathit{\text{C}}}_k}^{C_{k\text{-}1}}\right){ }^{\mathit{\text{C}}}{r}_{\mathit{\text{C}}}^B{0}_{3\times 3}{0}_{3\times 3}]\end{array}$

29

$\begin{array}{l}\nabla L^0{h}_3=\text{[}{0}_{1\times 3}\hspace{0.17em}\text{2}\cdot {\left(q_{\mathit{\text{B}}}^W\right)}^{\text{T}}{0}_{1\times 3}{0}_{1\times 3}{0}_{1\times 3}{0}_{1\times 3}{0}_{1\times 3}{0}_{1\times 4}{0}_{1\times 3}{0}_{1\times 3}\\ 0_{1\times 3}{0}_{1\times 4}{0}_{1\times 3}{0}_{1\times 3}{0}_{1\times 3}{0}_{1\times 3}{0}_{1\times 3}\hspace{0.17em}\text{2}\cdot {\left(q_{\mathit{\text{C}}}^B\right)}^{\text{T}}{0}_{1\times 3}{0}_{1\times 3}\text{]}\end{array}$

30

Herein, $\frac{\partial y_i}{\partial q_{\mathit{\text{B}}}^W}\text{=}{R}_{\mathit{\text{B}}}^C\cdot J\left(R{\left(q_{\mathit{\text{B}}}^W\right)}^{\text{T}}\right)\left({ }^{\mathit{\text{W}}}{r}_W^{{\mathit{\text{p}}}_i}-{ }^{\mathit{\text{W}}}{r}_{\mathit{\text{W}}}^B\right)$ , $\frac{\partial y_i}{\partial q_{\mathit{\text{C}}}^B}\text{=}J\left(R{\left(q_{\mathit{\text{B}}}^C\right)}^{\text{T}}\right)\cdot R_{\mathit{\text{W}}}^B\cdot \left({}^{W}r{ }^{{\mathit{\text{p}}}_i}W-{}^{W}r{ }^{B}W\right)$ , $\frac{\partial y_i}{\partial { }^{\mathit{\text{W}}}{r}_{\mathit{\text{W}}}^B}\text{=}-R_{\mathit{\text{W}}}^C$ , and $\frac{\partial y_i}{\partial { }^{\mathit{\text{W}}}{r}_{\mathit{\text{C}}}^B}\text{=}{I}_{3\times 3}$ . $J\left(R\left(q\right)\right)$ is the Jacobin matrix. The nonzero elements of the above terms constitute the observability judgment matrix O

$O\text{=}{\left[\begin{array}{l}\begin{array}{ccccccc}\nabla L^0{h}_{01}& \nabla L^0{h}_{02}& \nabla L^0{h}_{03}& \nabla L^0{h}_{\text{1}}& \nabla L^0{h}_2& \nabla L^0{h}_3& \nabla L_f^1{h}_{0\text{2}}\text{,}\end{array}\\ \begin{array}{cccccccc}\nabla L_f^1{h}_{0\text{3}}& \nabla L_f^1{h}_{\text{2}}& \nabla L_f^1{h}_{\text{31}}& \nabla L_f^2{h}_{0\text{2}}& \nabla L_f^2{h}_{0\text{3}}& \nabla L_f^2{h}_{\text{2}}& \nabla L_f^2{h}_{\text{31}}& \nabla L_f^{\text{3}}{h}_{0\text{3}}\end{array}\end{array}\right]}^{\text{T}}$

31

Under the same conditions, the observability judgment matrix $O_{\text{1}}$ of the traditional nonconstraint integrated model is

$O_{\text{1}}\text{=}{\left[\begin{array}{cccccccc}\nabla L^0{h}_{01}& \nabla L^0{h}_{02}& \nabla L^0{h}_{03}& \nabla L_f^1{h}_{0\text{2}}& \nabla L_f^1{h}_{0\text{3}}& \nabla L_f^2{h}_{0\text{2}}& \nabla L_f^2{h}_{0\text{3}}& \nabla L_f^{\text{3}}{h}_{0\text{3}}\end{array}\right]}^{\text{T}}$

32

From the above judgment matrix, we can find that in the initial alignment process, the motion dynamic of the integrated platform will affect the observability results. Thus, we consider this problem in different dynamics. At first, we suppose the rotation velocity is ${\left[\begin{array}{ccc}\text{0}& \text{0}& \text{1}\end{array}\right]}^{\text{T}}$ rad/s, the transition velocity is zero, the initial position and the gesture quaternion are ${\left[\begin{array}{ccc}\text{1}& \text{1}& \text{1}\end{array}\right]}^{\text{T}}$ and ${\left[\begin{array}{cccc}\text{1}& \text{0}& \text{0}& \text{0}\end{array}\right]}^{\text{T}}$ , respectively, and the acceleration is supposed to be zero for simplicity. Then the observability can be obtained from the rank calculation of the judgment matrix. In this situation, the matrix rank of the traditional integrated model is 26, which suggests that the model is not completely observable. While in the time-domain constrained model, the matrix rank is 32, thus the judgment matrix is full rank and all the states are observable in the proposed model.

In addition, we also consider the more rigid situation, where the rotation velocity is zero during the initial alignment process. Under this condition, the two involved models are both not full rank. That’s to say, in order to estimate the initial alignment coefficients of the INS/vision integrated system successfully, the rotation is necessary in different approaches. Kelly proved that if the rotation and transition are both included in the system motion, the initial alignment model is observable completely, as described in the study by Kelly and Sukhatme. ¹⁸ Compared with Kelly’s work, this article can simplify the initial alignment process by introducing the time-domain-related measurements. Even in the zero velocity situations, if the rotation is provided, the complete observability is guaranteed. This approach takes more advantages in applications that the large range maneuver is inappropriate.

Experiments validation

Simulations

In order to verify the effectiveness of the proposed time-domain constrained approach, we implement two groups of simulations in different dynamics. At first, the simulations of the integrated navigation system are implemented, where the ground truth of sensor to sensor bias, instrument coefficients are all known. The basic indicator of INS is described in Table 1.

Table 1.

Accuracy parameters of inertial navigation system.

Gyros			Accelerators
Constant drift (°/h)	Random walk (°/h)	Related time (s)	Stochastic bias (μg)	Random drift (μg)	Related time (s)
10	5	3000	100	50	300

The intrinsic coefficients of the camera are supposed as

$\mathit{\text{K}}=\left[\begin{array}{ccc}\text{1635}& \text{0}& \text{639}\\ \text{0}& \text{1635}& \text{512}\\ \text{0}& \text{0}& \text{1}\end{array}\right]$

33

In the first group of experiments, the system is supposed as static, and the true value of relative position is set as ${ }^{\mathit{\text{W}}}{\widehat{r}}_{\mathit{\text{C}}}^B\text{=}{\left[\begin{array}{ccc}\text{2}& \text{2}& \text{2}\end{array}\right]}^{\text{T}}$ m and $G_{\mathit{\text{C}}}^B\text{=}{\left[\begin{array}{ccc}\text{5}& \text{5}& \text{5}\end{array}\right]}^{\text{T}}$ °. The initial guess of system state is ${ }^{\mathit{\text{W}}}{\widehat{r}}_{\mathit{\text{C}}}^B\left(\text{0}\right)\text{=}{\left[\begin{array}{ccc}\text{1}& \text{1}& \text{1}\end{array}\right]}^{\text{T}}$ m and ${\widehat{G}}_{\mathit{\text{C}}}^B\text{=}{\left[\begin{array}{ccc}\text{0}& \text{0}& \text{0}\end{array}\right]}^{\text{T}}$ . In this situation, the estimation results of relative position and gesture are depicted in Figure 2.

Figure 2.

Initial alignment results in static circumstance: (a) estimation results of relative position; (b) estimation results of relative gesture.

From Figure 2, if the system is static during the whole alignment process, the initial bias of state guess is unable to be corrected well no matter which algorithm is selected. That’s to say, in the static situation, it is hard to implement initial alignment successfully, and it is well identical with the observability analysis results in the former section. In the following second group of experiments, the rotation velocity is supposed to be 10°/s, and the equipment indicator is the same as that in the first simulation, as depicted in Table 1. Under this condition, the corresponding estimation results are depicted in Figure 3.

Figure 3.

Initial alignment results in dynamic circumstance: (a) estimation results of relative position; (b) estimation results of relative gesture.

From the aforementioned simulations, we can find that provided the rotation motion is guaranteed, the initial alignment is able to be implemented without other sensors. The proposed time-domain constrained model is able to obtain higher accuracy in the initial alignment process.

Experiments

In this section, we implement a group of experiment on the basis of the data sampled from real equipment. The integrated system contains a micro IMU and industrial camera. In the experimental system, the random walk of gyro is 60°/h, the stochastic bias of accelerator is 1 mg, the relative position between camera and IMU is ${\mathit{\varphi }}_{\mathit{\text{c}}}^b\text{=}\left[\begin{array}{ccc}0& 0& 0\end{array}\right]$ °, and $r_{\mathit{\text{c}}}^b\text{=}\left[\begin{array}{ccc}-14.5& -6.5& -\end{array}\right]$ mm. It is necessary to remind here that the reference truth of sensor to sensor biases, which contains both orientation bias and position references, is measured from equipment positions in install process. The internal structure drawing of micro IMU is also referred in the measuring process. Because of the lack of the optical center in optical axis, the position bias of these two sensors is unable to measure accurately. The corresponding experimental results are depicted in Figure 4.

Figure 4.

Initial alignment estimation results of relative position between INS and camera: (a) estimation results of relative position; (b) estimation results of relative gesture.

This article also provides a demo about the experimental process. Please see the attachment file for more details. From the experiments results, we can find that the time-domain constraints are able to improve the estimation accuracy of the initial alignment estimation.

Conclusion

According to the positions constraints existed in the INS and visual system during different time steps, this article proposes a novel constrained model on the basis of the traditional parameterization. In addition, the time-domain optimization is also designed. From this article, we can arrive at three conclusions as follows:

The proposed approach needs no more additional sensors and measuring equipment other than a commonly used chessboard.

The result of observability analysis on two integrated model suggests that the system observability is related to the moving dynamics. Higher dynamic motion is able to improve the state estimation accuracy through the higher observability.

Compared with the traditional initial alignment model, the time-domain constrained parameterization is able to improve the system observability. In the proposed alignment process, the system states are completely observable even only the rotation motion is guaranteed, which will improve the availability of INS/vision integrated system especially in the applications that the system is hard to implement large-scale maneuver.