CN101762277B - Six-degree of freedom position and attitude determination method based on landmark navigation - Google Patents

Abstract

The invention relates to a six-degree of freedom position and attitude determination method based on landmark navigation, belonging to the information processing field of the automatic control system. The invention utilizes the coupling of position elements and attitude elements in pixel observation equation, considers the invariability of angle under European style transformation and uses included angles formed between navigation landmark observation visual line as observed quantity to decouple the position and attitude states in pixel observation equation and separately solve the position and attitude states in pixel observation equation, thus reducing the complexity of algorithm, fast determining the result and increasing the solution accuracy. The applicability of the method can be increased from three navigation landmarks to a plurality of landmarks.

Description

Six-degree of freedom position and attitude based on landmark navigation is confirmed method

Technical field

The present invention relates to a kind of six-degree of freedom position and attitude and confirm method, belong to the field of information processing in the automatic control system based on landmark navigation.

Background technology

Autonomous positioning navigation is one of the most basic, most important functions of autonomous mobile vehicle; Optical sensor is by the abundant environmental information of perception having received common concern because of it; Many existing vision positioning methods have all adopted the mode of navigating based on road sign optical information, and have obtained certain success in fields such as mobile robot, Aero-Space, automobile navigations.Based on the road sign optical information this mode of navigating; The main image that relies on optical camera to take mobile vehicle scene of living in; Extract the Pixel Information of relevant road sign in the image; These road signs position in scene is in advance known, like this through the processing of road markings Pixel Information and its known positional information, can calculate mobile vehicle with respect to space six degree of freedom states such as the position of scene, attitudes.Utilizing the road sign Pixel Information that the spatiality of mobile vehicle is resolved is key link in the autonomous landmark navigation; The six degree of freedom state estimation is a strong nonlinearity, fuzzy problem; It resolves result's correctness and the autonomous positioning ability that calculation accuracy has all directly influenced mobile vehicle, has determined the success ratio that the mobile vehicle inter-related task is accomplished.Therefore landmark navigation is one of emphasis problem of current scientific and technical personnel's concern from the major state acquiring method.

The road sign Pixel Information of utilizing having developed is carried out in the method that the six degree of freedom state resolves; Formerly technological [1] (Sharp C S; Shakernia O; Sastry S S.A vision system for landing anunmanned aerial vehicle [C] //IEEE International Conference on Roboticsand Automation, 2001,2:1720-1727.); Directly utilize the pixel observation equation that it is carried out linearization; The approximately linear observation equation that utilization obtains is found the solution the state of mobile vehicle, its mathematical method that adopts in finding the solution can be the least square fitting algorithm also can be other pass through the preface disposal route, like the Kalman filtering algorithm.But because the strong nonlinearity and the ambiguity of pixel observation equation self; Caused in the linear process truncation error bigger; It is low that this has caused that directly the pixel observation equation is carried out linearizing method solving precision, and initial value chooses even possibly cause dispersing of derivation algorithm improperly.

Formerly technology [2] is (referring to Chen Ying; The digital photogrammetry of remote sensing image. Shanghai: publishing house of Tongji University; 2003.) based on the pyramid principle location status and the attitude state of mobile vehicle are found the solution respectively; This method at first according to navigation road sign and its imaging vegetarian refreshments and optical camera imaging aperture characteristics point-blank, utilizes the range equation group to find the solution the location status of mobile vehicle earlier, then at the attitude state of finding the solution mobile vehicle based on this.This method has overcome the correlativity between location status and the attitude state effectively, and algorithm is found the solution simply, convenience.But this method only is suitable for finding the solution based on the spatiality of 3 navigation mark informations; When the road sign pixel that photographs during greater than 3; This method can only utilize wherein 3 road signs to find the solution, and the information that this obviously can not make full use of other road signs can't improve navigation accuracy.

Summary of the invention

The objective of the invention is to utilize the road sign Pixel Information to carry out problems such as precision is low, poor for applicability in the six degree of freedom state calculation method at present, provide a kind of six-degree of freedom position and attitude and confirm method in order to solve.

Design philosophy of the present invention is: to the coupling of position element in the pixel observation equation and attitude element; Consider the unchangeability of European conversion lower angle;, find the solution formed angle between each navigation road sign observation sight line respectively position, attitude state decoupling in the pixel observation equation as observed quantity position, attitude state in the pixel observation equation; Reduce the complicacy of algorithm, try to achieve the result fast and improve solving precision.The applicability of this algorithm can also expand to a plurality of road signs by 3 navigation road signs.

A kind of six-degree of freedom position and attitude is confirmed method, and concrete steps are:

Step 1 reads in the optical camera photographic images pixel coordinate of navigation road sign

Mobile vehicle utilizes its optical camera that carries to form images to the navigation road sign in scene, and through the pixel of navigation road sign in the extraction image, as line coordinates, the pointing direction of road sign under mobile vehicle camera coordinates system can obtain to navigate.Order is under the scene coordinate system, and the position vector of i navigation road sign does

The mobile vehicle camera coordinates is that the position vector and the transition matrix of relative scene coordinate system is respectively

And C _Ba, then under mobile vehicle camera coordinates system, the position vector of i navigation road sign For

${\stackrel{\→}{r}}_i^b=C_{\mathrm{ba}}(\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_i)$

Wherein, because of the scene coordinate system is a three-dimensional system of coordinate, transition matrix C _BaBe triplex row three column matrix.

The pixel p of i the navigation road sign that is then extracted _i, as line l _iSatisfy the following formula relation between the position of coordinate figure and mobile vehicle and the attitude

$p_i=f\frac{c_{11}(x-x_i)+c_{12}(y-y_i)+c_{13}(z-z_i)}{c_{31}(x-x_i)+c_{32}(y-y_i)+c_{33}(z-z_i)}$

$l_i=f\frac{c_{21}(x-x_i)+c_{22}(y-y_i)+c_{23}(z-z_i)}{c_{31}(x-x_i)+c_{32}(y-y_i)+c_{33}(z-z_i)}$

X wherein, y, z are the three shaft position coordinates of mobile vehicle under the scene coordinate system, x _i, y _i, z _iBe the three shaft position coordinates of i navigation road sign under the scene coordinate system, c _Ba(a=1,2,3; B=1,2,3) be transition matrix C _BaMiddle respective element, f is the focal length of optical camera.

Step 2; Utilize the pixel of the navigation road sign that step 1 obtains, as the focal distance f of line coordinates and optical camera, make up the observation angle matrix

of optical camera to the navigation road sign

If the optical camera of mobile vehicle is A to i with j the formed observation angle of navigation road sign observation sight line _Ij, then

$\cos A_{\mathrm{ij}}=\frac{{\stackrel{\→}{r}}_i\·{\stackrel{\→}{r}}_j}{r_i{r}_j}=\frac{{\stackrel{\→}{r}}_i^b\·{\stackrel{\→}{r}}_j^b}{r_i{r}_j}$

In the following formula

Be the relatively move position vector of carrier of i under the scene coordinate system and j navigation road sign, r _i, r _jIt is the distance between i and j navigation road sign and the mobile vehicle.

Utilize any i pixel p that makes up with j road sign in n the navigation road sign _i, p _jWith picture line l _i, l _jThe focal distance f of coordinate and optical camera can make up the observation angle of measurement

$A_{\mathrm{Ij}}=\mathrm{Arccos}\left(\frac{p_i{p}_j+l_i{l}_j+f^2}{\left|(p_i,l_i,f)\right|\left|(p_j,l_j,f)\right|}\right)(i,j=\mathrm{1,2},\·\·\·,n)$ And (i ≠ j)

For n navigation road sign; Any two road signs make up in twos can construct n (n-1)/2 observation angle altogether, and this n (n-1)/2 observation angle constituted the observation angle matrix of measuring

$\stackrel{\→}{A}=\left[\begin{array}{c}{A}_{12}\\ A_{23}\\ .\\ .\\ .\\ A_{n(n-1)}\end{array}\right]$

Step 3 is utilized mobile vehicle current location priori estimates

Confirm the predicted value A of observation angle _Ij ^*And observing matrix H

The mobile vehicle current location priori estimates

that utilizes the forecast of mobile vehicle self-position to provide can confirm to observe the predicted value

and the observing matrix H of angle matrix, wherein

${\stackrel{\→}{A}}^{*}=\left[\begin{array}{c}{A}_{12}^{*}\\ A_{23}^{*}\\ .\\ .\\ .\\ A_{n(n-1)}^{*}\end{array}\right]$

A _Ij ^*Make up the observation angle of pairing forecast for any i with j road sign, its expression formula does

$A_{\mathrm{ij}}^{*}=\arccos \left(\frac{({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i)\·({\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j)}{|{\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i||{\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j|}\right)(i,j=\mathrm{1,2},\·\·\·,n)$

The structure formula of observing matrix does

$H=\left[\begin{array}{c}{\stackrel{\→}{h}}_{12}^T\\ {\stackrel{\→}{h}}_{23}^T\\ .\\ .\\ .\\ {\stackrel{\→}{h}}_{n(n-1)}^T\end{array}\right]$

Wherein

${\stackrel{\→}{h}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{m}}_{\mathrm{ij}}}{r_i^{*}}+\frac{{\stackrel{\→}{m}}_{\mathrm{ji}}}{r_j^{*}}$

and

is auxiliary vector, and definition as follows

${\stackrel{\→}{m}}_{\mathrm{ij}}=\frac{{\stackrel{\→}{n}}_j-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_i}{\sin A_{\mathrm{ij}}^{*}}$ ${\stackrel{\→}{m}}_{\mathrm{ji}}=\frac{{\stackrel{\→}{n}}_i-({\stackrel{\→}{n}}_i\·{\stackrel{\→}{n}}_j){\stackrel{\→}{n}}_j}{\sin A_{\mathrm{ij}}^{*}}$

and

respectively, of the i-th and j-th unit of the line of sight vector signs

${\stackrel{\→}{n}}_i=\frac{{\stackrel{\→}{r}}_i^{*}}{r_i^{*}}$ ${\stackrel{\→}{n}}_j=\frac{{\stackrel{\→}{r}}_j^{*}}{r_j^{*}}$

Where

respectively Dir scene coordinates i and j points signs signs moving carrier relative position vector prediction

${\stackrel{\→}{r}}_i^{*}={\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_i$ ${\stackrel{\→}{r}}_j^{*}={\stackrel{\→}{r}}^{*}-{\stackrel{\→}{\mathrm{\ρ}}}_j$

r _i ^*, r _j ^*It is the distance of the forecast between i and j navigation road sign and the mobile vehicle.

Step 4 is utilized the observation angle and the observing matrix that obtain in the step 2,3, confirms the position of the relative scene of mobile vehicle

Using the measured angle observation matrix

and forecasts observation angle matrix do poor angle strike observation matrix deviation

building observation angle deviation matrix

$\mathrm{\δ}\stackrel{\→}{A}=\stackrel{\→}{A}-{\stackrel{\→}{A}}^{*}$

And then utilize iterative

$\mathrm{\δ}\stackrel{\→}{r}={\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\δ}\stackrel{\→}{A}$

Interative computation position deviation amount

utilizes this departure that position priori estimates is improved, and solves current mobile vehicle position:

$\stackrel{\→}{r}={\stackrel{\→}{r}}^{*}+{\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\δ}\stackrel{\→}{A}$

Step 5 is utilized the position result of the relative scene of mobile vehicle that step 4 obtains, and confirms the attitude of the relative scene of mobile vehicle

Utilize the pixel p of n navigation road sign _i, as line l _iThe focal distance f of coordinate and optical camera makes up the unit pointing vector of each navigation road sign under the mobile vehicle coordinate system

And the matrix form of forming

${\stackrel{\→}{n}}_i^b=\frac{(p_i,l_i,f)}{\left|(p_i,l_i,f)\right|}$ $N_b=\left[\begin{array}{cccc}{\stackrel{\→}{n}}_1^b& {\stackrel{\→}{n}}_2^b& ...& {\stackrel{\→}{n}}_n^b\end{array}\right]$

Step 4 calculated based mobile carrier position

OK scene coordinate system units each navigation signposts pointing vector

and the composition of the matrix form

$N_a=\left[\begin{array}{cccc}\frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_1}{r_1}& \frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_2}{r_2}& ...& \frac{\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_n}{r_n}\end{array}\right]$

Utilize N _aAnd N _b, find the solution the attitude transition matrix optimum solution C of mobile vehicle with respect to the scene coordinate system _BaFor:

$C_{\mathrm{ba}}=\frac{1}{2}{N}_{\mathrm{ba}}(3I-N_{\mathrm{ba}}^T{N}_{\mathrm{ba}})$

Wherein $N_{\mathrm{Ba}}=N_b{N}_a^T{\left(N_a{N}_a^T\right)}^{-1}.$ Just can obtain the attitude of mobile vehicle according to the attitude transition matrix with respect to the scene coordinate system.

Beneficial effect

Method provided by the present invention is considered the unchangeability of European conversion lower angle; Utilize the formed angle that respectively navigates between the road sign observation sight line as observed quantity; With position, attitude state decoupling in the pixel observation equation; The non-linear intensity and the complexity of reduction recording geometry make that linear equation after the decoupling zero finds the solution that the linearity is good, iteration efficient height and computing be simple; Weakening of nonlinear degree makes truncation error reduce, thereby improves calculation accuracy; The raising of iteration efficient makes the resolving time diminish, and the real-time of system is improved.

This method is applicable to the situation that has navigation road sign more than 3 and 3 in the automatic positioning navigation system; Can find the solution six free space positions, the state of mobile vehicle in real time, great application value arranged in engineering fields such as space exploration, satnav, unmanned planes.

Description of drawings

Fig. 1 is the process flow diagram of the inventive method.

Fig. 2 is the navigation road sign imaging relations synoptic diagram among the present invention.

Fig. 3 is an angle measuring position face synoptic diagram in the specific embodiment of the invention.

Embodiment

For the object of the invention and advantage are described better, the present invention is further specified below in conjunction with accompanying drawing and embodiment.

The concrete steps of present embodiment are following:

Step 1 reads in the optical camera photographic images pixel coordinate of navigation road sign

Mobile vehicle utilizes its optical camera that carries to form images to the navigation road sign in scene; Through the pixel of navigation road sign in the extraction image, as line coordinates; The pointing direction of road sign under the mobile vehicle coordinate system that can obtain to navigate, navigation road sign imaging relations is as shown in Figure 2.

Order is under the scene coordinate system, and the position vector of i navigation road sign does

The mobile vehicle camera coordinates is that the position vector priori estimates and the transition matrix of relative scene coordinate system is respectively

And C _Ba, then under mobile vehicle camera coordinates system, the position vector of i navigation road sign For

${\stackrel{\→}{r}}_i^b=C_{\mathrm{ba}}(\stackrel{\→}{r}-{\stackrel{\→}{\mathrm{\ρ}}}_i)$

Wherein, because of the scene coordinate system is a three-dimensional system of coordinate, transition matrix C _BaBe triplex row three column matrix.

The pixel p of i the navigation road sign that is then extracted _i, as line l _iSatisfy the following formula relation between the position of coordinate figure and mobile vehicle and the attitude

$p_i=f\frac{c_{11}(x-x_i)+c_{12}(y-y_i)+c_{13}(z-z_i)}{c_{31}(x-x_i)+c_{32}(y-y_i)+c_{33}(z-z_i)}$

$l_i=f\frac{c_{21}(x-x_i)+c_{22}(y-y_i)+c_{23}(z-z_i)}{c_{31}(x-x_i)+c_{32}(y-y_i)+c_{33}(z-z_i)}---\left(1\right)$

X wherein, y, z are the three shaft position coordinates of mobile vehicle under the scene coordinate system, x _i, y _i, z _iBe the three shaft position coordinates of i navigation road sign under the scene coordinate system, c _Ba(a=1,2,3; B=1,2,3) be transition matrix C _BaMiddle respective element, f is the focal length of optical camera.If total n of the navigation road sign that tracking observation arrives, then corresponding observation equation does

$y=h(\stackrel{\&RightArrow;}{r},C_{\mathrm{ba}})=\left[\begin{array}{ccccc}{p}_1& {\mathrm{<figure-callout\; id="1"\; label="l"\; filenames="HSA00000009008500021.png"\; state="\{\{state\}\}">l</figure-callout>}}_1& ...& p_n& l_n\end{array}\right]---\left(2\right)$

Step 2; Utilize the pixel of the navigation road sign that step 1 obtains, as the focal distance f of line coordinates and optical camera, make up the observation angle matrix

that optical camera is measured the navigation road sign

Visible by (1) formula in the step 1 and (2) formula, in observation equation, represent the x of positional information, y, z and represent the c of attitude information _BaHave serious coupling, observation equation has stronger nonlinear characteristic simultaneously, directly utilizes (2) formula to estimate to cause complex algorithm loaded down with trivial details to the six degree of freedom state of mobile vehicle, even divergence problem occurs.(2) complicacy found the solution of formula is mainly caused by position, attitude coupling; Notice the unchangeability of European conversion lower angle coordinate; The conversion that is angle coordinate and quadrature attitude in the geometric space is irrelevant; Utilize this characteristic below, introducing optical camera is that observed quantity is carried out decoupling zero to mobile vehicle position, attitude and found the solution to the angle between the navigation road sign observation sight line.

If formed view angle is A to the optical camera of mobile vehicle with j road sign observation sight line to i _Ij, then

$\cos A_{\mathrm{ij}}=\frac{{\stackrel{\&RightArrow;}{r}}_i\&CenterDot;{\stackrel{\&RightArrow;}{r}}_j}{r_i{r}_j}=\frac{{\stackrel{\&RightArrow;}{r}}_i^b\&CenterDot;{\stackrel{\&RightArrow;}{r}}_j^b}{r_i{r}_j}$

In the following formula

Be the relatively move position vector of carrier of i under the scene coordinate system and j navigation road sign, r _i, r _jIt is the distance between i and j navigation road sign and the mobile vehicle.

This view angle can utilize pixel in the optical imagery, represent as line coordinates, promptly

$A_{\mathrm{ij}}=\arccos \left(\frac{p_i{p}_j+l_i{l}_j+f^2}{\left|(p_i,l_i,f)\right|\left|(p_j,l_j,f)\right|}\right)$

Can be known by geometric relationship, be to be axis with two navigation road sign lines to surface of position that should the angle measured value, and rotating tee is crossed this one section circular arc of 2 and the toroid that obtains, and promptly A is all satisfied in view angle on this toroid _IjValue, as shown in Figure 3.The center O of this section circular arc is at two road sign P _iP _jOn the perpendicular bisector in sideline, distance and A between arc radius R and two road signs _IjRelation do

$R=\frac{{\mathrm{\&rho;}}_i-{\mathrm{\&rho;}}_j}{2\sin A_{\mathrm{ij}}}$

The center of circle is to P _iP _jDistance L do

L＝RcosA _ij

Wherein, ρ _iAnd ρ _jBe respectively vector

With

Mould

The also available vector equation expression of above-mentioned geometric description; Utilize the inner product relation of

and

, have

$\left(\stackrel{\&RightArrow;}{r}{-\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i\right)\&CenterDot;(\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j)=|\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i||\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j|\cos A_{\mathrm{ij}}---\left(3\right)$

It is thus clear that following formula is the mobile vehicle position

With the measurement included angle A _IjRelational expression, and irrelevant with the attitude state of carrier, therefore, can utilize following formula that the location status of carrier is found the solution separately.

For n navigation road sign; Any two road signs make up in twos can construct n (n-1)/2 observation angle altogether, and this n (n-1)/2 observation angle constituted the observation angle matrix of measuring

$\stackrel{\&RightArrow;}{A}=\left[\begin{array}{c}{A}_{12}\\ A_{23}\\ .\\ .\\ .\\ A_{n(n-1)}\end{array}\right]$

Step 3 is utilized mobile vehicle current location priori estimates Confirm to measure the predicted value A of angle _Ij ^*And observing matrix H

Consider that formula (3) is nonlinear equation, directly find the solution the comparison difficulty, below under the condition of little deviation linearization hypothesis, in mobile vehicle location-prior estimated value

The place derives to its linearization measurement equation, can obtain the position deviation amount

With observation angle departure δ A _IjBetween linear approximate relationship

$\mathrm{\&delta;}{A}_{\mathrm{ij}}={\stackrel{\&RightArrow;}{h}}_{\mathrm{ij}}\&CenterDot;\mathrm{\&delta;}\stackrel{\&RightArrow;}{r}$

Wherein

${\stackrel{\&RightArrow;}{h}}_{\mathrm{ij}}=\frac{{\stackrel{\&RightArrow;}{m}}_{\mathrm{ij}}}{r_i^{*}}+\frac{{\stackrel{\&RightArrow;}{m}}_{\mathrm{ji}}}{r_j^{*}}$

and is auxiliary vector, and definition as follows

${\stackrel{\&RightArrow;}{m}}_{\mathrm{ij}}=\frac{{\stackrel{\&RightArrow;}{n}}_j-({\stackrel{\&RightArrow;}{n}}_i\&CenterDot;{\stackrel{\&RightArrow;}{n}}_j){\stackrel{\&RightArrow;}{n}}_i}{\sin A_{\mathrm{ij}}^{*}}$ ${\stackrel{\&RightArrow;}{m}}_{\mathrm{ji}}=\frac{{\stackrel{\&RightArrow;}{n}}_i-({\stackrel{\&RightArrow;}{n}}_i\&CenterDot;{\stackrel{\&RightArrow;}{n}}_j){\stackrel{\&RightArrow;}{n}}_j}{\sin A_{\mathrm{ij}}^{*}}$

and respectively, of the i-th and j-th unit of the line of sight vector signs

${\stackrel{\&RightArrow;}{n}}_i=\frac{{\stackrel{\&RightArrow;}{r}}_i^{*}}{r_i^{*}}$ ${\stackrel{\&RightArrow;}{n}}_j=\frac{{\stackrel{\&RightArrow;}{r}}_j^{*}}{r_j^{*}}$

Wherein

is respectively under the scene coordinate system the relatively move position vector of forecast of carrier of i road sign and j road sign

${\stackrel{\&RightArrow;}{r}}_i^{*}={\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i$ ${\stackrel{\&RightArrow;}{r}}_j^{*}={\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j$

r _i ^*, r _j ^*It is the distance of the forecast between i and j navigation road sign and the mobile vehicle.In actual moving process, the mobile vehicle current location priori estimates that utilizes location prediction to provide

Can confirm the predicted value A that takes measurement of an angle _Ij ^*And linear vector

Like this through measured deviation amount δ A _Ij, utilize $\mathrm{\&delta;}{A}_{\mathrm{Ij}}={\stackrel{\&RightArrow;}{h}}_{\mathrm{Ij}}\&CenterDot;\mathrm{\&delta;}\stackrel{\&RightArrow;}{r}$ Formula can be improved the current location of mobile vehicle.If in image, extract n navigation road sign altogether, measure the angle combination for then total n (n-1)/2 kind, promptly can simultaneous n (n-1)/2 as $\mathrm{\&delta;}{A}_{\mathrm{Ij}}={\stackrel{\&RightArrow;}{h}}_{\mathrm{Ij}}\&CenterDot;\mathrm{\&delta;}\stackrel{\&RightArrow;}{r}$ The equation of form is formed the position of system of equations to mobile vehicle

Find the solution.

The mobile vehicle current location priori estimates

that utilizes the forecast of mobile vehicle self-position to provide can confirm to observe the predicted value

and the observing matrix H of angle matrix, wherein

${\stackrel{\&RightArrow;}{A}}^{*}=\left[\begin{array}{c}{A}_{12}^{*}\\ A_{23}^{*}\\ .\\ .\\ .\\ A_{n(n-1)}^{*}\end{array}\right]$

A _Ij ^*Make up the observation angle of pairing forecast for any i with j road sign, its expression formula does

$A_{\mathrm{ij}}^{*}=\arccos \left(\frac{({\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i)\&CenterDot;({\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j)}{|{\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i||{\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j|}\right)(i,j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,n)$

The structure formula of observing matrix does

$H=\left[\begin{array}{c}{\stackrel{\&RightArrow;}{h}}_{12}^T\\ {\stackrel{\&RightArrow;}{h}}_{23}^T\\ .\\ .\\ .\\ {\stackrel{\&RightArrow;}{h}}_{n(n-1)}^T\end{array}\right]$

Step 4 is utilized the observation angle and the observing matrix that obtain in the step 2,3, confirms the position of the relative scene of mobile vehicle

Using the measured angle observation matrix

and forecasts observation angle matrix

do poor angle strike observation matrix deviation

Building observation angle deviation matrix

$\mathrm{\&delta;}\stackrel{\&RightArrow;}{A}=\stackrel{\&RightArrow;}{A}-{\stackrel{\&RightArrow;}{A}}^{*}=\left[\begin{array}{c}\mathrm{\&delta;}{A}_{12}\\ \mathrm{\&delta;}{A}_{23}\\ .\\ .\\ .\\ \mathrm{\&delta;}{A}_{n(n-1)}\end{array}\right]$

And then utilize iterative

$\mathrm{\&delta;}\stackrel{\&RightArrow;}{r}{=\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\&delta;}\stackrel{\&RightArrow;}{A}$

Interative computation position deviation amount

utilizes this departure that position priori estimates

is improved, and solves current mobile vehicle position:

$\stackrel{\&RightArrow;}{r}={\stackrel{\&RightArrow;}{r}}^{*}+{\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\&delta;}\stackrel{\&RightArrow;}{A}$

Step 5 is utilized the position result of the relative scene of mobile vehicle that step 4 obtains, and confirms the attitude of the relative scene of mobile vehicle

Because under mobile vehicle camera coordinates system, the position of navigation road sign can be expressed as

${\stackrel{\&RightArrow;}{r}}_i^b=C_{\mathrm{ba}}(\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i)---\left(4\right)$

Formula (4) is carried out unit normalization, can get

${\stackrel{\&RightArrow;}{n}}_i^b=C_{\mathrm{ba}}\frac{\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i}{r_i}$

Wherein,

utilize the navigation road sign pixel, can be expressed as as line coordinates

${\stackrel{\&RightArrow;}{n}}_i^b=\frac{(p_i,l_i,f)}{\left|(p_i,l_i,f)\right|}$

Utilize the mobile vehicle position that obtains in the step 4, and many vectors decide the appearance principle, confirm that mobile vehicle with respect to the attitude transition matrix optimum solution of scene coordinate system does

$C_{\mathrm{ba}}=\frac{1}{2}{N}_{\mathrm{ba}}(3I-N_{\mathrm{ba}}^T{N}_{\mathrm{ba}})$

Wherein

$N_{\mathrm{ba}}=N_b{N}_a^T{\left(N_a{N}_a^T\right)}^{-1}$

$N_a=\left[\begin{array}{cccc}\frac{\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_1}{r_1}& \frac{\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_2}{r_2}& ...& \frac{\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_n}{r_n}\end{array}\right]$ $N_b=\left[\begin{array}{cccc}{\stackrel{\&RightArrow;}{n}}_1^b& {\stackrel{\&RightArrow;}{n}}_2^b& ...& {\stackrel{\&RightArrow;}{n}}_n^b\end{array}\right]$

Utilize the position of step 4 to find the solution formula like this

$\stackrel{\&RightArrow;}{r}={\stackrel{\&RightArrow;}{r}}^{*}+{\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\&delta;}\stackrel{\&RightArrow;}{A}$

Attitude matrix with step 5

$C_{\mathrm{ba}}=\frac{1}{2}{N}_{\mathrm{ba}}(3I-N_{\mathrm{ba}}^T{N}_{\mathrm{ba}})$

Can confirm that mobile vehicle is with respect to six free space states such as the position of scene and attitudes.

Claims (1)

1. confirm method based on the six-degree of freedom position and attitude of landmark navigation, it is characterized in that: comprise following steps:

Step 1 reads in the optical camera photographic images pixel coordinate of navigation road sign

Mobile vehicle utilizes its optical camera that carries to form images to the navigation road sign in scene; Through the pixel of navigation road sign in the extraction image, as line coordinates; The pointing direction of road sign under mobile vehicle camera coordinates system can obtain to navigate; Order is under the scene coordinate system, and the position vector of i navigation road sign does

I=1,2...n, the mobile vehicle camera coordinates is that relative scene coordinate system position vector and transition matrix are respectively

And C _Ba, then under mobile vehicle camera coordinates system, the position vector of i navigation road sign

For

${\stackrel{\&RightArrow;}{r}}_i^b=C_{\mathrm{ba}}({\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i)$

Wherein, because of the scene coordinate system is a three-dimensional system of coordinate, transition matrix C _BaBe triplex row three column matrix;

The pixel p of i the navigation road sign that is then extracted _i, as line l _iSatisfy the following formula relation between the position of coordinate figure and mobile vehicle and the attitude

$p_i=f\frac{c_{11}\left({x-x}_i\right)+c_{12}\left({y-y}_i\right)+c_{13}\left({z-z}_i\right)}{c_{31}\left({x-x}_i\right)+c_{32}\left({y-y}_i\right)+c_{33}\left({z-z}_i\right)}$

$l_i=f\frac{c_{21}\left({x-x}_i\right)+c_{22}\left({y-y}_i\right)+c_{23}\left({z-z}_i\right)}{c_{31}\left({x-x}_i\right)+c_{32}\left({y-y}_i\right)+c_{33}\left({z-z}_i\right)}$

X wherein, y, z are the three shaft position coordinates of mobile vehicle under the scene coordinate system, x _i, y _i, z _iBe the three shaft position coordinates of i navigation road sign under the scene coordinate system, c _Ba, a=1,2,3; B=1,2,3, be transition matrix C _BaMiddle respective element, f is the focal length of optical camera;

Step 2; Utilize the pixel of the navigation road sign that step 1 obtains, as the focal distance f of line coordinates and optical camera, make up the observation angle matrix of measuring

If the optical camera of mobile vehicle is A to i with j the formed observation angle of navigation road sign observation sight line _Ij:

$A_{\mathrm{Ij}}=\mathrm{Arccos}\left(\frac{p_i{p}_j+l_i{l}_j+f^2}{\left|(p_i,l_i,f)\right|\left|(p_j,l_j,f)\right|}\right),i,j=\mathrm{1,2},...,n$ And i ≠ j

For n navigation road sign; Any two road signs make up in twos can construct n (n-1)/2 observation angle altogether, and this n (n-1)/2 observation angle constituted the observation angle matrix of measuring

$\stackrel{\&RightArrow;}{A}=\left[\begin{array}{c}{A}_{12}\\ A_{23}\\ .\\ .\\ .\\ A_{n(n-1)}\end{array}\right]$

Step 3; Utilize mobile vehicle current location priori estimates

, confirm the predicted value

and the observing matrix H of observation angle

The predicted value

of observation angle matrix does

${\stackrel{\&RightArrow;}{A}}^{*}=\left[\begin{array}{c}{A}_{12}^{*}\\ A_{23}^{*}\\ .\\ .\\ .\\ A_{n(n-1)}^{*}\end{array}\right]$

makes up the observation angle of pairing forecast for any i with j road sign, and its expression formula does

$A_{\mathrm{ij}}^{*}=\arccos \left(\frac{({\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i)\&CenterDot;({\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j)}{|{\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i||{\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j|}\right),i,j=\mathrm{1,2},...,n$

The structure formula of observing matrix does

$H=\left[\begin{array}{c}{\stackrel{\&RightArrow;}{h}}_{12}^T\\ {\stackrel{\&RightArrow;}{h}}_{23}^T\\ .\\ .\\ .\\ {\stackrel{\&RightArrow;}{h}}_{n(n-1)}^T\end{array}\right]$

Wherein

${\stackrel{\&RightArrow;}{h}}_{\mathrm{ij}}=\frac{{\stackrel{\&RightArrow;}{m}}_{\mathrm{ij}}}{r_i^{*}}+\frac{{\stackrel{\&RightArrow;}{m}}_{\mathrm{ji}}}{r_j^{*}}$

and

is auxiliary vector, and definition as follows

${\stackrel{\&RightArrow;}{m}}_{\mathrm{ij}}=\frac{{\stackrel{\&RightArrow;}{n}}_j-({\stackrel{\&RightArrow;}{n}}_i\&CenterDot;{\stackrel{\&RightArrow;}{n}}_j){\stackrel{\&RightArrow;}{n}}_i}{\sin A_{\mathrm{ij}}^{*}}$ ${\stackrel{\&RightArrow;}{m}}_{\mathrm{ji}}=\frac{{\stackrel{\&RightArrow;}{n}}_i-({\stackrel{\&RightArrow;}{n}}_i\&CenterDot;\stackrel{\&RightArrow;}{n})\stackrel{\&RightArrow;}{n}}{\sin A_{\mathrm{ij}}^{*}}$

and

, respectively, of the i-th and j-th unit of the line of sight vector signs

${\stackrel{\&RightArrow;}{n}}_i=\frac{{\stackrel{\&RightArrow;}{r}}_i^{*}}{r_i^{*}}$ ${\stackrel{\&RightArrow;}{n}}_j=\frac{{\stackrel{\&RightArrow;}{r}}_j^{*}}{r_j^{*}}$

Wherein

is respectively under the scene coordinate system the relatively move position vector of forecast of carrier of i road sign and j road sign

${\stackrel{\&RightArrow;}{r}}_i^{*}={\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_i$ ${\stackrel{\&RightArrow;}{r}}_j^{*}={\stackrel{\&RightArrow;}{r}}^{*}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_j$

is the distance of the forecast between i and j navigation road sign and the mobile vehicle;

Step 4 is utilized the observation angle and the observing matrix that obtain in the step 2,3, confirms the position of the relative scene of mobile vehicle

Using the measured angle observation matrix

and forecasts observation angle matrix

do poor angle strike observation matrix deviation

Building observation angle deviation matrix

$\mathrm{\&delta;}\stackrel{\&RightArrow;}{A}=\stackrel{\&RightArrow;}{A}-{\stackrel{\&RightArrow;}{A}}^{*}$

And then utilize iterative

$\mathrm{\&delta;}\stackrel{\&RightArrow;}{r}={\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\&delta;}\stackrel{\&RightArrow;}{A}$

Interative computation position deviation amount

utilizes this departure that position priori estimates

is improved, and solves current mobile vehicle position:

$\stackrel{\&RightArrow;}{r}={\stackrel{\&RightArrow;}{r}}^{*}+{\left(H^{T}H\right)}^{-1}{H}^T\mathrm{\&delta;}\stackrel{\&RightArrow;}{A}$

Step 5 is utilized the position result of the relative scene of mobile vehicle that step 4 obtains, and confirms the attitude of the relative scene of mobile vehicle

Make up the unit pointing vector

of each navigation road sign under the mobile vehicle coordinate system and the matrix form of forming thereof

${\stackrel{\&RightArrow;}{n}}_i^b=\frac{(p_i,l_i.f)}{\left|(p_i,l_i,f)\right|}$ $N_b=\left[\begin{array}{cccc}{\stackrel{\&RightArrow;}{n}}_1^b& {\stackrel{\&RightArrow;}{n}}_2^b& ...& {\stackrel{\&RightArrow;}{n}}_n^b\end{array}\right]$

Step 4 calculated based mobile carrier position

OK scene coordinate system units each navigation signposts pointing vector

and the composition of the matrix form

$N_a=\left[\begin{array}{cccc}\frac{\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_1}{r_1}& \frac{\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_2}{r_2}& ...& \frac{\stackrel{\&RightArrow;}{r}-{\stackrel{\&RightArrow;}{\mathrm{\&rho;}}}_n}{r_n}\end{array}\right]$

Utilize N _aAnd N _b, find the solution the attitude transition matrix optimum solution C of mobile vehicle with respect to the scene coordinate system _BaFor:

$C_{\mathrm{ba}}=\frac{1}{2}{N}_{\mathrm{ba}}(3I-N_{\mathrm{ba}}^T{N}_{\mathrm{ba}})$

Wherein

obtains the attitude of mobile vehicle with respect to the scene coordinate system according to the attitude transition matrix.

Images