CN101173858B - Three-dimensional posture fixing and local locating method for lunar surface inspection prober - Google Patents

Abstract

The invention relates to a three-dimensional gesture determining and local positioning method of a lunar surface rover, which comprises the following steps: (1) ascertaining the rolling and pitching angles by use of a triaxial accelerometer with sensitivity while the rover is still; (2) determining the drift angle gesture by means of a sun sensor; (3) using the axial gesture and the gyro deviation as the state quantity, the rolling and pitching angles established by the triaxial accelerometer, the drift angle determined by the sun sensor as well as three gyro outputs as the measuring information, building a state equation and a measuring equation, and estimating the triaxial and gyro deviations by means of extended Kalman filter; (4) after compensating the gyro outputs by virtue of the estimated gyro deviations while the rover is in motion, calculating the gesture changes of the rover, finishing the preestimation of the gyro gesture, and fulfilling gesture update; (5) acquiring the information about the rotation speed of the driving wheels of the rover, the rotating angle of the steering wheel, the rotating angle of the left and right rocker arms, and getting the position increment of the rover in the body coordinate system by use of the forward kinematics relationship. The invention has the advantages of high precision of gesture determining and positioning, simple calculation and easy implementation of the engineering.

Description

A kind of three-dimensional posture fixing of inspection tour prober for moon surface and local locating method

Technical field

The present invention relates to a kind of inspection tour prober for moon surface three-dimensional posture fixing and local locating method, be applicable to that the autonomous of detector decided the appearance location in the complicated unknown landform such as lunar surface, martian surface, or the independent navigation of the open-air vehicle in ground.

Background technology

Inspection tour prober for moon surface is that a class is implemented to make an inspection tour the detector of reconnoitring at lunar surface, also claims moon craft, lunar rover etc., also inspection tour prober for moon surface, planet car can be referred to as probe vehicles.In a broad sense, inspection tour prober for moon surface is a kind of can moving at moonscape, finishes the spacecraft of tasks such as detection, sampling, delivery.Say that narrowly inspection tour prober for moon surface is to adapt to lunar environment, carry the scientific exploration instrument is maked an inspection tour detection at lunar surface spacecraft.Therefore, inspection tour prober for moon surface is the special spacecraft of a class, is different from traditional satellite, airship, and before the landing lunar surface, inspection tour prober for moon surface is the useful load of lander, after the landing be independently, complete mobile detector.

The location of inspection tour prober for moon surface comprises overall situation location and local positioning.Overall situation positioning instant is determined the absolute position of moon craft under the menology coordinate system.Local positioning is promptly determined the location expression of moon craft with respect to navigation coordinate system (coordinate origin is got lander landing loca usually).Because the singularity of lunar surface environment, the air navigation aid that ground mobile robot and independent navigation vehicle adopt is not suitable for inspection tour prober for moon surface.As can't detecting gps signal on the lunar surface, so GPS can't be used for the location of inspection tour prober for moon surface; Utilize importance in star map recognition to carry out the precision very low (hundreds of rice～several kilometers) of celestial navigation; It is also unavailable to relate to methods such as the map match location of known environment map and external information (for example radar beacon and ultrasound wave navigation) and navigation beacon, because these methods need sufficient environment priori, be not suitable for the inspection tour prober for moon surface that in circumstances not known, moves.

Up to now, the lunar rover of successful foreign emission has the LRV of the U.S. and Lunokhod1, the Lunokhod2 of USSR (Union of Soviet Socialist Republics).The LRV of the U.S. is manned lunar rover, and the Lunokhod1 of USSR (Union of Soviet Socialist Republics) and Lunokhod2 adopt distant mode of operation.The Marsokhod of successful foreign is the MER (comprising Spirit and Opportunity) of the U.S. Sojourner in 1997 and 2004.Also carried out the development of ground principle prototype in addition both at home and abroad, FIDO, Rocky 7, Marsokhod, Nomad etc. are more typically arranged, wherein adopting more traditional instruments such as stadimeter, inclinator, gyro, inertia meter and GPS to combine to position except that Nomad, other have all adopted IMU, sun sensor and train scrambler three class sensors to obtain means as the main appearance locating information of deciding, but system configuration, to decide appearance location algorithm aspect different.

Except that the above technology that has been applied, it is also many in the document of publishing in recent years the moon and martian surface inspection prober to be decided the research of appearance localization method.

But the shortcoming that above-mentioned existing method exists is mainly reflected in: (1) utilizes the output of two axis accelerometers to find the solution the angle of inclination, and this is bigger at the angle of inclination, or the conventional value of local gravitational acceleration is when bigger variation takes place, and the error of generation is bigger; (2) utilize yaw axis gyro to measure crab angle, do not have to consider to change rolling and the luffing angle that causes by topographic relief; (3) utilize horizontal two-dimensional position information, or utilize the relation of foundation of left and right sides two-wheeled motion geometric relationship and yaw-position, utilize sun sensor output that the position is revised; Though this method is simple, is suitable for subdued topography, model error is big, is not suitable for complicated rugged topography; (4) decide the appearance bearing accuracy in order to improve, choose many quantity of states as treating estimated value, as site error, attitude error and inertial sensor deviation etc., state equation of setting up and measurement equation more complicated, decide appearance and local bearing accuracy though improved to a certain extent, but because complexity of calculation, engineering is used and is realized that difficulty is bigger; (5) Chang Yong dead reckoning method is only utilized the revolution speed calculating distance travelled of left and right wheels, does not consider that Terrain Elevation changes the influence that brings, and this method can cause than mistake under complex-terrain.

Summary of the invention

Technology of the present invention is dealt with problems and is: overcome the deficiencies in the prior art part, a kind of inspection tour prober for moon surface three-dimensional posture fixing and local locating method are provided, this method can obtain high decide appearance and bearing accuracy, calculates simply simultaneously, and Project Realization is easy.

Technical solution of the present invention is: a kind of three-dimensional posture fixing of inspection tour prober for moon surface and local locating method, and its characteristics are that step is as follows:

(1) when the lunar surface inspection prober is in static state, utilizes responsive definite the rolling and the angle of pitch of three axis accelerometer;

(2) utilize the output of sun sensor and the rolling and the angle of pitch of above-mentioned steps (1) acquisition to determine the crab angle attitude;

(3) with three-axis attitude and gyro bias as quantity of state, the rolling that three axis accelerometer is determined, the angle of pitch and the crab angle of determining by sun sensor, and three gyro outputs are as metrical information, set up state equation and measure equation, utilize EKF to estimate three-axis attitude and gyro bias;

(4) when the lunar surface inspection prober moves, utilization is estimated the gyro bias obtain output is compensated to gyro by step (3) after, the attitude of calculating inspection tour prober for moon surface changes, and finishes the gyro attitude prediction, realize posture renewal, obtain to be tied to the attitude matrix of body coordinate system by navigation coordinate;

(5) gather each driving wheel rotating speed of inspection tour prober for moon surface, steering wheel angle, left and right sides rocking arm corner information, utilize the full motion relation of six wheel speeds, four-wheel corner and major-minor rocking arm corner to obtain the positional increment of detector in body coordinate system;

(6) with the positional increment in the positional increment transformation navigation coordinate system of step (5) acquisition inspection tour prober for moon surface in body coordinate system, finding the solution inspection tour prober for moon surface is three speed and positional informations on the direction at navigation coordinate.

The present invention's advantage compared with prior art is:

Set up simple state equation when (1) the present invention utilizes static state, and utilize inertial sensor measure output and at that time the relation between the attitude set up gyro and accelerometer static measurement equation, again by expansion Kalman filtering technique, overcome the random noise of sun sensor and inertial sensor, thereby obtain the attitude estimated value of degree of precision, improved accuracy of attitude determination; When determining, attitude also finished estimation to gyro bias.Utilize gyro output carrying out attitude prediction in the time of dynamically, this moment, last gyro bias estimated value was used for that output compensates to gyro, had improved the precision of gyro attitude prediction when dynamic.

(2) it is definite to utilize the full motion relation of detector and attitude information to carry out local location.

With only adopt single-wheel or about the localization method of two wheel speeds compare, utilization of the present invention is found the solution the speed of detector in body coordinate system based on the kinematic relation of whole driving wheel rotating speeds, steering wheel angle and rocking arm corner, has reduced the error that the wheel slippage is caused; In addition, the introducing of attitude information has realized the three-dimensional localization under the rugged topography, can obtain elevation information, simplifies to handle with the general plane of ignoring height change and compares, and bearing accuracy improves.

(3) positional information that obtains of the present invention can be used as further the basis based on the ground adjustment processing of Vision information processing.When being used for the independent navigation of surface car, can adopt high precision obliquity sensors such as Electrolyte type or dynamic balance servo type to obtain rolling and two level inclinations of pitching, the absolute course of associating high precision sensor (as sun sensor or electronic compass etc.) obtains crab angle, can increase metrical information like this, and reduced the complexity of measurement functions, be convenient to online application.

Adopted full motion relation when (4) position is determined based on six wheel speeds, four-wheel corner and major-minor rocking arm corner, and introduced attitude information, this method has following three advantages: (1) has reduced the some or several slippage errors of rotor speed when positioning of taking turns of independent employing; (2) reduced sliding influence; (3) overcome Terrain Elevation and changed the influence that brings.

In a word, the present invention verifies that by internal field and the field trial of domestic desert first method is feasible in the inspection tour prober for moon surface principle prototype, and engineering easily realizes therefore having practicality.

Description of drawings

Fig. 1 is inspection tour prober for moon surface three-dimensional posture fixing of the present invention and local positioning process flow diagram;

Fig. 2 is the Filtering Estimation value of the roll angle of employing the inventive method acquisition;

The roll angle value of Fig. 3 for not adopting the present invention directly to measure;

Fig. 4 is the Filtering Estimation value of the angle of pitch of employing the inventive method acquisition;

The angle of pitch value of Fig. 5 for not adopting the present invention directly to measure;

Fig. 6 is the Filtering Estimation value of the crab angle of employing the inventive method acquisition;

The crab angle value of Fig. 7 for not adopting the present invention directly to measure;

The axis of rolling gyro bias estimated value of Fig. 8 for adopting the inventive method to obtain;

The pitch axis gyro bias estimated value of Fig. 9 for adopting the inventive method to obtain;

The yaw axis gyro bias estimated value of Figure 10 for adopting the inventive method to obtain.

Embodiment

Embodiment 1:

Definition " east-north-sky " month reason coordinate is a navigation coordinate system, is example with 3-2-1 (promptly elder generation is around the Z axle, again around Y-axis, X-axis then) commentaries on classics preface, defines the Eulerian angle that the relative navigation coordinate of inspection prober body coordinate system is

θ, ψ are the three-axis attitude angle, and navigation coordinate is that the position of initial point is a local location relatively.It is example that inspection tour prober for moon surface is taken turns rocker-arm with six, six wheel drive wherein, four-wheel steering.

As shown in Figure 1, concrete steps of the present invention are as follows:

(1) when the lunar surface inspection prober is in static state, utilize the responsive local gravitational acceleration of three axis accelerometer, after changing, obtain to roll and luffing angle.

If the output of three accelerometers installing along the body coordinate system quadrature is respectively [f _Mx, f _My, f _Mz] (not considering to measure noise and alignment error), then roll angle And pitching angle theta _mCan find the solution by following formula:

$g_m=\sqrt{{f_{\mathrm{mx}}}^2+{f_{\mathrm{my}}}^2+{f_{\mathrm{mz}}}^2}$

θ _m＝arcsin(-f _mx/g _m)

(2) utilize the output of sun sensor to calculate crab angle.

At first utilize the measurement output α of sun sensor _s, β _s, it is as follows to obtain the coordinate of solar vector in measurement coordinate system:

$s_{\mathrm{ss}}^x=\frac{\tan {\mathrm{\&beta;}}_{\mathrm{<figure-callout\; id="1"\; label="s"\; filenames="H2007101232006E00052.png"\; state="\{\{state\}\}">s</figure-callout>}}}{\sqrt{1+{\tan }^2{\mathrm{\&alpha;}}_s+{\tan }^2{\mathrm{\&beta;}}_s}}$

$s_{\mathrm{ss}}^y=\frac{\tan {\mathrm{\&alpha;}}_{\mathrm{<figure-callout\; id="1"\; label="s"\; filenames="H2007101232006E00052.png"\; state="\{\{state\}\}">s</figure-callout>}}}{\sqrt{1+{\tan }^2{\mathrm{\&alpha;}}_s+{\tan }^2{\mathrm{\&beta;}}_s}}$

$s_{\mathrm{ss}}^z=\frac{1}{\sqrt{1+{\tan }^2{\mathrm{\&alpha;}}_s+{\tan }^2{\mathrm{\&beta;}}_s}}$

Definition OX _SsY _SsZ _SsBe the measurement coordinate system of sun sensor, wherein Z _SsAlong the normal direction on code-disc plane, X _SsAxle is along the slit direction of sunshine incident, Y _SsAxle is by right-handed coordinate system definition, then α _sThe expression sun sensor measured angular, for solar vector at Y _SsZ _SsProjection on the plane and Z _SsThe angle of axle, β _sThe expression sun sensor advance optic angle, for solar vector at X _SsZ _SsProjection on the plane and Z _SsThe angle of axle, both can draw by the vertical range of code wheel reading, refraction coefficient and sensitive axes and code-disc.Solar vector is transformed in the detector body coordinate system describes:

${\left[\begin{array}{ccc}{s}_{\mathrm{bx}}& s_{\mathrm{by}}& s_{\mathrm{bz}}\end{array}\right]}^T=C_{\mathrm{ss}}^b{\left[\begin{array}{ccc}{s}_{\mathrm{ss}}^x& s_{\mathrm{ss}}^y& s_{\mathrm{ss}}^z\end{array}\right]}^T$

C wherein _Ss ^bBeing the allocation matrix of sun sensor, is that Matrix C is installed _b ^SsInverse matrix.C _b ^SsForm as follows:

$C_b^{\mathrm{ss}}=\left[\begin{array}{ccc}{\mathrm{ss}}_{11}& {\mathrm{ss}}_{12}& {\mathrm{ss}}_{13}\\ {\mathrm{ss}}_{21}& {\mathrm{ss}}_{22}& {\mathrm{ss}}_{23}\\ {\mathrm{ss}}_{31}& {\mathrm{ss}}_{32}& {\mathrm{ss}}_{33}\end{array}\right]$

C _b ^SsIn the numerical value of each element determine by the installation site of sun sensor.

Simultaneously, utilize almanac data to calculate and obtain the coordinate S of solar vector in the local geographic coordinate system of inspection prober _Gx, S _Gy, S _Gz, this coordinate S _Gx, S _Gy, S _GzAs known input value;

Utilize roll angle

And pitching angle theta _m, find the solution crab angle:

${\mathrm{\&psi;}}_m=\mathrm{arctg}\left(\frac{({\mathrm{\&epsiv;}}_3{\mathrm{\&epsiv;}}_5-{\mathrm{\&epsiv;}}_1{\mathrm{\&epsiv;}}_6)({\mathrm{\&epsiv;}}_1{\mathrm{\&epsiv;}}_4-{\mathrm{\&epsiv;}}_2{\mathrm{\&epsiv;}}_3)}{({\mathrm{\&epsiv;}}_2{\mathrm{\&epsiv;}}_3-{\mathrm{\&epsiv;}}_1{\mathrm{\&epsiv;}}_4)({\mathrm{\&epsiv;}}_4{\mathrm{\&epsiv;}}_5-{\mathrm{\&epsiv;}}_2{\mathrm{\&epsiv;}}_6)}\right)$

Wherein:

When (3) static, get three-axis attitude and gyro bias as quantity of state, get rolling, the angle of pitch and the crab angle of determining by three axis accelerometer of determining by sun sensor, three gyro outputs as metrical information, set up state equation and measure equation, utilize EKF to estimate three-axis attitude and gyro bias.

At first, get three attitude informations to be estimated

The deviation b of θ, ψ and three gyros _Gx, b _Gy, b _GzDefinition becomes a sextuple status Bar vector, and is as follows:

Thereby set up state equation be:

X(k)＝Φ(k，k-1)X(k-1)+w(k-1)

Wherein, w (k) is the dynamic noise vector, and k represents the k time sampling.

In the above-mentioned state equation, (k k-1) can require to decide Φ according to sensor characteristic that is adopted and modeling accuracy.Model when inspection tour prober for moon surface is in static state is analyzed, and can think and wait to estimate that attitude and sensor deviation all are constant, all be identical promptly in its value of any one sampling instant.So desirable Φ (k, k-1)=I _{N * n}

Then, get the three-axis attitude measured value that obtains by static output of accelerometer and sun sensor

θ _m, ψ _m, the static state output g of three gyros _Mx, g _My, g _MzDefinition becomes a sextuple status Bar vector, and is as follows:

Thereby set up measurement equation:

Y(k)＝h(X(k))+v(k)

Wherein v (k) is the measurement noise relevant with the measuring accuracy of sensor, and k represents the k time sampling equally.H (X (k)) is a measurement functions, with current quantity of state with to be tied to the posture changing matrix of body coordinate system from navigation coordinate relevant.If local longitude and latitude is respectively λ, η, in this case, deriving, it is as follows to obtain function h (X (k)):

Wherein, ω ₀Be moon angle of rotation speed.

After having set up state equation and observation equation, utilize EKF to estimate three-axis attitude and gyro bias.

(4) when the lunar surface inspection prober moves, utilization is estimated the gyro bias obtain output is compensated to gyro by step (3) after, the attitude of calculating inspection tour prober for moon surface changes, and finishes the gyro attitude prediction, finish posture renewal, obtain to be tied to the attitude matrix C of body coordinate system by navigation coordinate _Bn

Above-mentioned solution procedure is as follows:

Ask for current attitude angle speed ω by output of the gyro after the compensation and last attitude matrix _Gb ^b, i.e. the component of angular velocity in body coordinate system of the relative navigation coordinate of lunar surface inspection prober body coordinate system system:

Wherein:

ω _Eg ^bBe the navigation coordinate system component of angular velocity in body coordinate system of month solid coordinate system relatively;

ω _Ie ^bBe the component of month angular velocity (front-month revolutions angular velocity) of solid coordinate system relative inertness coordinate system in the system of body border.

Utilize ω _Gb ^bFind the solution the hypercomplex number differential equation, carry out hypercomplex number and upgrade:

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{q}}_1\\ {\stackrel{\&CenterDot;}{q}}_2\\ {\stackrel{\&CenterDot;}{q}}_3\\ {\stackrel{\&CenterDot;}{q}}_4\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& {\mathrm{\&omega;}}_{\mathrm{gbz}}^b& -{\mathrm{\&omega;}}_{\mathrm{gby}}^b& {\mathrm{\&omega;}}_{\mathrm{gbx}}^b\\ -{\mathrm{\&omega;}}_{\mathrm{gbz}}^{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="H2007101232006E00021.png,H2007101232006E00031.png"\; state="\{\{state\}\}">b</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{gbx}}^b& {\mathrm{\&omega;}}_{\mathrm{gby}}^b\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_{\mathrm{gbx}}^{\mathrm{<figure-callout\; id="0"\; label="b"\; filenames="H2007101232006E00021.png,H2007101232006E00031.png"\; state="\{\{state\}\}">b</figure-callout>}}& 0& {\mathrm{\&omega;}}_{\mathrm{gbz}}^b\\ -{\mathrm{\&omega;}}_{\mathrm{gbx}}^b& -{\mathrm{\&omega;}}_{\mathrm{gby}}^b& -{\mathrm{\&omega;}}_{\mathrm{gbz}}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ {\mathrm{<figure-callout\; id="4"\; label="q"\; filenames="H2007101232006E00052.png"\; state="\{\{state\}\}">q</figure-callout>}}_4\end{array}\right]$

Ask for attitude matrix according to hypercomplex number:

$C_{\mathrm{bn}}=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]=\left[\begin{array}{ccc}{{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="H2007101232006E00052.png"\; state="\{\{state\}\}">q</figure-callout>}}_1}^2-{q_2}^2-{q_3}^2+{{\mathrm{<figure-callout\; id="4"\; label="q"\; filenames="H2007101232006E00052.png"\; state="\{\{state\}\}">q</figure-callout>}}_4}^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -{{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="H2007101232006E00052.png"\; state="\{\{state\}\}">q</figure-callout>}}_1}^2+{q_2}^2-{q_3}^2+{{\mathrm{<figure-callout\; id="4"\; label="q"\; filenames="H2007101232006E00052.png"\; state="\{\{state\}\}">q</figure-callout>}}_4}^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -{{\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="H2007101232006E00052.png"\; state="\{\{state\}\}">q</figure-callout>}}_1}^2-{q_2}^2+{q_3}^2+{{\mathrm{<figure-callout\; id="4"\; label="q"\; filenames="H2007101232006E00052.png"\; state="\{\{state\}\}">q</figure-callout>}}_4}^2\end{array}\right]$

According to C _BnObtain attitude parameter with the relation of attitude angle

θ, ψ.Changeing preface with 3-2-1 is example, C _BnWith the pass of attitude angle be:

Other changes preface and does corresponding the modification.

(5) gather each driving wheel rotating speed, steering wheel angle, left and right sides rocking arm corner information, utilize the positive motion of mover system to learn the component of relation acquisition position of detector increment in body coordinate system

Δx _b，Δy _b，Δz _b：

Δy _b＝R(sinδ ₁dθ ₁+sinδ ₂dθ ₂+sinδ ₅dθ ₅+sinδ ₆dθ ₆)

Wherein:

Be intermediate variable,

β ₁, ρ ₁, β ₂, ρ ₂Be respectively left and right sides major-minor rocking arm corner; D θ ₁～d θ ₆Be six driving wheel rotating speeds, obtain by the scrambler of installing on each wheel; δ ₁, δ ₂, δ ₅, δ ₆Be left front, right front, left back and right back four steering wheel angles.

(6) with the positional increment in the positional increment transformation navigation coordinate system of step (5) acquisition detector in body coordinate system, finding the solution inspection prober is three speed and positional informations on the direction at navigation coordinate.

Positional increment is relevant with the current attitude of detector, and changes with attitude, can not be directly used in navigation calculating, need utilize current attitude information to be transformed into component Δ x in navigation coordinate system _n, Δ y _n, Δ z _n

$\left[\begin{array}{c}\mathrm{\&Delta;}{x}_n\\ \mathrm{\&Delta;}{y}_n\\ \mathrm{\&Delta;}{z}_n\end{array}\right]=C_{\mathrm{nb}}\left[\begin{array}{c}\mathrm{\&Delta;}{x}_b\\ \mathrm{\&Delta;}{y}_b\\ \mathrm{\&Delta;}{z}_b\end{array}\right]$

Wherein $C_{\mathrm{nb}}=C_{\mathrm{bn}}^T.$

This positional increment is transformed into after navigation coordinate system is described, and finding the solution inspection prober is three speed v on the direction at navigation coordinate _xv _y, v _zWith positional information x _{N, k}, y _{N, k}, z _{N, k}

v _x＝Δx _n/Δt

v _y＝Δy _n/Δt

v _z＝Δz _n/Δt

x _n，k＝x _n，k-1+Δx _n

y _n，k＝y _n，k-1+Δy _n

z _n，k＝z _n，k-1+Δz _n

(7) the demand moon craft because of task will switch between stationary state and motion state, in the moon craft entire work process, moon craft repeats three-dimensional posture fixing shown in Figure 1 and local positioning flow process, thereby omnidistance moon craft attitude and the positional information more accurately of obtaining of realization work, the assurance moon craft is finished the task of scientific exploration safely and effectively.

Fig. 2 is the Filtering Estimation value of the roll angle of employing the inventive method acquisition, the roll angle value of Fig. 3 for not adopting the present invention directly to measure, comparison diagram 2, Fig. 3 find, the roll angle that the use method that the present invention introduced is obtained, the roll angle of comparing and not adopting the inventive method and using the direct measurement of sensor to calculate, precision can obtain bigger raising.

Fig. 4 is the Filtering Estimation value of the angle of pitch of employing the inventive method acquisition, the angle of pitch value that Fig. 5 does not adopt the present invention directly to measure, comparison diagram 4, Fig. 5 find, the angle of pitch that the use method that the present invention introduced is obtained, the angle of pitch of comparing and not adopting the inventive method and using the direct measurement of sensor to calculate, precision can obtain bigger raising.

Fig. 6 is the Filtering Estimation value of the crab angle of employing the inventive method acquisition, the crab angle value of Fig. 7 for not adopting the present invention directly to measure, comparison diagram 6, Fig. 7 find, the crab angle that the use method that the present invention introduced is obtained, the crab angle of comparing and not adopting the inventive method and using the direct measurement of sensor to calculate, precision can obtain bigger raising.

The axis of rolling gyro bias estimated value of Fig. 8 for adopting the inventive method to obtain can be found by Fig. 8, and the use method that the present invention introduced can estimate the gyro bias on the axis of rolling direction more exactly.

The pitch axis gyro bias estimated value of Fig. 9 for adopting the inventive method to obtain can be found by Fig. 9, and the use method that the present invention introduced can estimate the gyro bias on the pitch axis direction more exactly.

The yaw axis gyro bias estimated value that Figure 10 adopts the inventive method to obtain can be found by Figure 10, and the use method that the present invention introduced can estimate the gyro bias on the yaw axis direction more exactly.

Embodiment 2

When changeing preface definition attitude angle by other, analytic process is the same, can draw analog result.In addition to the inspection tour prober for moon surface of different configurations, need according in the kinematics model establishment step (5) with the kinematic relation of each wheel, be used for speed and position and determine that its method is similar to embodiment 1.

Claims (5)

1. the three-dimensional posture fixing of an inspection tour prober for moon surface and local locating method is characterized in that step is as follows:

(1) when the lunar surface inspection prober is in static state, utilizes responsive definite the rolling and the angle of pitch of three axis accelerometer;

(2) utilize the output of sun sensor and the rolling and the angle of pitch of above-mentioned steps (1) acquisition to determine the crab angle attitude;

(3) with three-axis attitude and gyro bias as quantity of state, the rolling that three axis accelerometer is determined, the angle of pitch and the crab angle of determining by sun sensor, and three gyro outputs are as metrical information, set up state equation and measure equation, utilize EKF to estimate three-axis attitude and gyro bias;

(4) when the lunar surface inspection prober moves, utilization is estimated the gyro bias obtain output is compensated to gyro by step (3) after, the attitude of calculating inspection tour prober for moon surface changes, and finishes the gyro attitude prediction, realize posture renewal, obtain to be tied to the attitude matrix of body coordinate system by navigation coordinate;

(5) gather each driving wheel rotating speed of inspection tour prober for moon surface, steering wheel angle, left and right sides rocking arm corner information, utilize the full motion relation of six wheel speeds, four-wheel corner and major-minor rocking arm corner to obtain the positional increment of detector in body coordinate system;

(6) step (5) is obtained the positional increment of inspection tour prober for moon surface in body coordinate system and convert positional increment in the navigation coordinate system to, finding the solution inspection tour prober for moon surface is three speed and positional informations on the direction at navigation coordinate.

2. the three-dimensional posture fixing of inspection tour prober for moon surface according to claim 1 and local locating method is characterized in that: described step (1) utilize three axis accelerometer responsive determine to roll and the angle of pitch as follows: utilize three axis accelerometer sensitivity local gravitational acceleration g _m, after changing, to roll and luffing angle, the output of promptly establishing three accelerometers installing along the body coordinate system quadrature is respectively [f _Mx, f _My, f _Mz], roll angle then

And pitching angle theta _mCan find the solution by following formula:

$g_m=\sqrt{{f_{\mathrm{mx}}}^2+{f_{\mathrm{my}}}^2+{f_{\mathrm{mz}}}^2}$

θ _m＝arcsin(-f _mx/g _m)。

3. the three-dimensional posture fixing of inspection tour prober for moon surface according to claim 1 and local locating method, it is characterized in that: the method for definite crab angle attitude of described step (2) is:

A, at first utilize the measurement output α of sun sensor _s, β _s, it is as follows to obtain the coordinate of solar vector in measurement coordinate system:

$s_{\mathrm{ss}}^x=\frac{\tan {\mathrm{\&beta;}}_s}{\sqrt{1+{\tan }^2{\mathrm{\&alpha;}}_s+{\tan }^2{\mathrm{\&beta;}}_s}}$

$s_{\mathrm{ss}}^y=\frac{\tan {\mathrm{\&alpha;}}_s}{\sqrt{1+{\tan }^2{\mathrm{\&alpha;}}_s+{\tan }^2{\mathrm{\&beta;}}_s}}$

$s_{\mathrm{ss}}^z=\frac{1}{\sqrt{1+{\tan }^2{\mathrm{\&alpha;}}_s+{\tan }^2{\mathrm{\&beta;}}_s}}$

α _sThe measured angular of expression sun sensor, β _sThe optic angle that advances of representing sun sensor;

B, solar vector is transformed in the detector body coordinate system describes:

${\left[\begin{array}{ccc}{s}_{\mathrm{bx}}& s_{\mathrm{by}}& s_{\mathrm{bz}}\end{array}\right]}^T=C_{\mathrm{ss}}^b{\left[\begin{array}{ccc}{s}_{\mathrm{ss}}^x& s_{\mathrm{ss}}^y& s_{\mathrm{ss}}^z\end{array}\right]}^T$

C wherein _Ss ^bBeing the allocation matrix of sun sensor, is that Matrix C is installed _b ^SsInverse matrix, C _b ^SsForm as follows:

$C_b^{\mathrm{ss}}=\left[\begin{array}{ccc}{\mathrm{ss}}_{11}& {\mathrm{ss}}_{12}& {\mathrm{ss}}_{13}\\ {\mathrm{ss}}_{21}& {\mathrm{ss}}_{22}& {\mathrm{ss}}_{23}\\ {\mathrm{ss}}_{31}& {\mathrm{ss}}_{32}& {\mathrm{ss}}_{33}\end{array}\right]$

C _b ^SsIn the numerical value of each element determine by the installation site of sun sensor;

C, utilize almanac data calculate to obtain the coordinate S of solar vector in the local geographic coordinate system of inspection tour prober for moon surface _Gx, S _Gy, S _Gz

D, the result who utilizes above-mentioned B and C and roll angle

Pitching angle theta _m, find the solution crab angle:

${\mathrm{\&psi;}}_m=\mathrm{arctg}\left(\frac{({\mathrm{\&epsiv;}}_3{\mathrm{\&epsiv;}}_5-{\mathrm{\&epsiv;}}_1{\mathrm{\&epsiv;}}_6)({\mathrm{\&epsiv;}}_1{\mathrm{\&epsiv;}}_4-{\mathrm{\&epsiv;}}_2{\mathrm{\&epsiv;}}_3)}{({\mathrm{\&epsiv;}}_2{\mathrm{\&epsiv;}}_3-{\mathrm{\&epsiv;}}_1{\mathrm{\&epsiv;}}_4)({\mathrm{\&epsiv;}}_4{\mathrm{\&epsiv;}}_5-{\mathrm{\&epsiv;}}_2{\mathrm{\&epsiv;}}_6)}\right)$

Wherein:

4. the three-dimensional posture fixing of inspection tour prober for moon surface according to claim 1 and local locating method is characterized in that: in the described step (3)

State equation is: X (k)=φ (k, k-1) X (k-1)+w (k-1),

Wherein w (k) is the dynamic noise vector, and k represents the k time sampling, and (k k-1) can require to determine φ according to sensor characteristic that is adopted and modeling accuracy;

Measure equation: Y (k)=h (X (k))+v (k), wherein v (k) is the measurement noise relevant with the measuring accuracy of sensor, the same expression of k is sampled for the k time, and h (X (k)) is a measurement functions, with current quantity of state with to be tied to the posture changing matrix of body coordinate system from navigation coordinate relevant.

5. the three-dimensional posture fixing of inspection tour prober for moon surface according to claim 1 and local locating method, it is characterized in that: the solution procedure of described step (4) is as follows:

(1) asks for current attitude angle speed ω by output of the gyro after the compensation and last attitude matrix _Gb ^b, i.e. the component of angular velocity in body coordinate system of the relative navigation coordinate of lunar surface inspection prober body coordinate system system:

Wherein:

ω _Eg ^gBe the navigation coordinate system component of angular velocity in navigation coordinate system of month solid coordinate system relatively;

ω _IeBe to consolidate the angular velocity front-month revolutions angular velocity of coordinate system relative inertness coordinate system the moon at the component of consolidating by the moon in the coordinate system;

It is the latitude value of lunar rover position;

g _Mx, g _My, g _MzBe respectively the static state output of three gyros;

b _Gx, b _Gy, b _GzIt is the deviation of three gyros;

(2) utilize ω _Gb ^bFind the solution the hypercomplex number differential equation, carry out hypercomplex number and upgrade:

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{q}}_1\\ {\stackrel{\&CenterDot;}{q}}_2\\ {\stackrel{\&CenterDot;}{q}}_3\\ {\stackrel{\&CenterDot;}{q}}_4\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& {\mathrm{\&omega;}}_{\mathrm{gbz}}^b& -{\mathrm{\&omega;}}_{\mathrm{gby}}^b& {\mathrm{\&omega;}}_{\mathrm{gbx}}^b\\ -{\mathrm{\&omega;}}_{\mathrm{gbz}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{gbx}}^b& {\mathrm{\&omega;}}_{\mathrm{gby}}^b\\ {\mathrm{\&omega;}}_y& -{\mathrm{\&omega;}}_{\mathrm{gbx}}^b& 0& {\mathrm{\&omega;}}_{\mathrm{gbz}}^b\\ -{\mathrm{\&omega;}}_{\mathrm{gbx}}^b& -{\mathrm{\&omega;}}_{\mathrm{gby}}^b& -{\mathrm{\&omega;}}_{\mathrm{gbz}}^b& 0\end{array}\right]\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]$

(3) ask for attitude matrix according to hypercomplex number:

$C_{\mathrm{bn}}=\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\end{array}\right]\left[\begin{array}{ccc}{q_1}^2-{q_2}^2-{q_3}^2+{q_4}^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -{q_1}^2+{q_2}^2-{q_3}^2+{q_4}^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -{q_1}^2-{q_2}^2+{q_3}^2+{q_4}^2\end{array}\right].$

Images