CN101762277A - 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 determined method

Technical field

The present invention relates to a kind of six-degree of freedom position and attitude and determine 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 being subjected to 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 by 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 finished.Therefore landmark navigation is one of emphasis problem of current scientific and technical personnel's concern from the major state acquiring method.

Carry out in the method that the six degree of freedom state resolves in the road sign Pixel Information of utilizing that has developed, technology [1] (Sharp C S formerly, 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, as 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 may 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 is at first according to navigation road sign and its imaging vegetarian refreshments and optical camera imaging aperture characteristics point-blank, utilize 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 determine method in order to solve.

Design philosophy of the present invention is: at the coupling of position element in the pixel observation equation and attitude element, consider the unchangeability of European conversion lower angle, formed angle between each navigation road sign observation sight line as observed quantity, with position, attitude state decoupling in the pixel observation equation, position, attitude state in the pixel observation equation are found the solution respectively, 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 determined 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 can be to navigation road sign imaging in scene, the pixel by extracting navigation road sign in the image, as line coordinates, and 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 is

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 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 extracted then _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 is utilized the pixel of the navigation road sign that step 1 obtains, as the focal distance f of line coordinates and optical camera, makes 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 the pixel p of any i and j road sign combination 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}}=\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

Determine 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 determine to observe the predicted value of angle matrix

And observing matrix H, 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 is

$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 is

$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^{*}}$

With

For auxiliary vector, be defined 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}}^{*}}$

With

Be respectively the unit sight line vector of i and j road sign

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

Wherein

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

${\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, determines the position of the relative scene of mobile vehicle

Utilize the observation angle matrix of measuring Observation angle matrix with forecast

Do difference and ask for observation angle matrix deviation Make up the view 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

Utilize this departure to the position priori estimates Improve, solve 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 determines 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]$

The mobile vehicle position that calculates based on step 4 Determine the unit pointing vector of each navigation road sign under the scene coordinate system And the matrix form of forming

$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 be 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 purpose of the present invention and advantage are described better, the present invention will be further described below in conjunction with the drawings and specific embodiments.

The concrete steps of present embodiment are as follows:

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

Mobile vehicle utilizes its optical camera that carries can be to navigation road sign imaging in scene, by 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 as shown in Figure 2.

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

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 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 extracted then _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 is

$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 is utilized the pixel of the navigation road sign that step 1 obtains, as the focal distance f of line coordinates and optical camera, makes up the observation angle matrix that optical camera is measured the navigation road sign

By (1) formula in the step 1 and (2) formula as seen, 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 the algorithm complexity 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)$

By geometric relationship as can be known, be to be axis to surface of position that should the angle measured value with two navigation road sign lines, rotation is by this one section toroid that circular arc obtains of 2, 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 _IjThe pass be

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

The center of circle is to P _iP _jDistance L be

L＝RcosA _ij

Wherein, ρ _iAnd ρ _jBe respectively vector

With

Mould

The also available vector equation expression of above-mentioned geometric description utilizes

With

Inner product relation, 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)$

As seen, 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

Determine 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^{*}}$

With

For auxiliary vector, be defined 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}}^{*}}$

With

Be respectively the unit sight line vector of i and j road sign

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

Wherein

Be 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 determine the predicted value A that takes measurement of an angle _Ij ^*And linear vector

Like this by 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 determine to observe the predicted value of angle matrix

And observing matrix H, 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 is

$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 is

$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, determines the position of the relative scene of mobile vehicle

Utilize the observation angle matrix of measuring

Observation angle matrix with forecast Do difference and ask for observation angle matrix deviation

Make up the view 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 Utilize this departure to the position priori estimates

Improve, solve 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 determines 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 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, determine that mobile vehicle with respect to the attitude transition matrix optimum solution of scene coordinate system is

$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 determine that mobile vehicle is with respect to six free space states such as the position of scene and attitudes.

Claims (1)

1. determine 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 can be to navigation road sign imaging in scene, the pixel by extracting navigation road sign in the image, as line coordinates, and 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 is

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 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 extracted then _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 is utilized the pixel of the navigation road sign that step 1 obtains, as the focal distance f of line coordinates and optical camera, makes 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:

$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)(i,j=\mathrm{1,2},\&CenterDot;\&CenterDot;\&CenterDot;,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}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ A_{n(n-1)}\end{array}\right]$

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

The predicted value of observation angle matrix

For

${\stackrel{\&RightArrow;}{A}}^{*}=\left[\begin{array}{c}{A}_{12}^{*}\\ A_{23}^{*}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ 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 is

$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 is

$H=\left[\begin{array}{c}{\stackrel{\&RightArrow;}{h}}_{12}^T\\ {\stackrel{\&RightArrow;}{h}}_{23}^T\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ {\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^{*}}$

With

For auxiliary vector, be defined 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}}^{*}}$

With

Be respectively the unit sight line vector of i and j road sign

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

Wherein

Be 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;

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

Utilize the observation angle matrix of measuring

Observation angle matrix with forecast

Do difference and ask for observation angle matrix deviation Make up the view 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

Utilize this departure to the position priori estimates

Improve, solve 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 determines 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

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

The mobile vehicle position that calculates based on step 4 Determine the unit pointing vector of each navigation road sign under the scene coordinate system

And the matrix form of forming

$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}& \&CenterDot;\&CenterDot;\&CenterDot;& \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 $N_{\mathrm{ba}}=N_b{N}_a^T{\left(N_a{N}_a^T\right)}^{-1};$ Obtain the attitude of mobile vehicle according to the attitude transition matrix with respect to the scene coordinate system.

Images