CN105806365B - Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced - Google Patents

Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced Download PDF

Info

Publication number
CN105806365B
CN105806365B CN201610146652.5A CN201610146652A CN105806365B CN 105806365 B CN105806365 B CN 105806365B CN 201610146652 A CN201610146652 A CN 201610146652A CN 105806365 B CN105806365 B CN 105806365B
Authority
CN
China
Prior art keywords
error
inertial navigation
subsystem
parameter
alignment
Prior art date
Application number
CN201610146652.5A
Other languages
Chinese (zh)
Other versions
CN105806365A (en
Inventor
王新龙
祝佳芳
Original Assignee
北京航空航天大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 北京航空航天大学 filed Critical 北京航空航天大学
Priority to CN201610146652.5A priority Critical patent/CN105806365B/en
Publication of CN105806365A publication Critical patent/CN105806365A/en
Application granted granted Critical
Publication of CN105806365B publication Critical patent/CN105806365B/en

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18Stabilised platforms, e.g. by gyroscope

Abstract

Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced, it is characterised in that: the following steps are included: initializing before the starting of one, carrier vehicle to inertial navigation, complete the coarse alignment under the conditions of the quiet pedestal of inertial navigation;Two, after carrier vehicle starting, observed quantity is constructed using the velocity information that inertial navigation and odometer export;Three, fine alignment circuit traveling is designed using observed quantity, ins error equation and Auto-disturbance-rejection Control;Four, the adjusting of automatic disturbance rejection controller parameter;Five, the misalignment exported by observer, construction feedback matrix realize the vehicle-mounted running initial alignment of inertial navigation;Through the above steps, coarse alignment method, observed quantity construction, Active Disturbance Rejection Control circuit design method before starting respectively to carrier vehicle etc. are described in detail;This method does not need the statistical property of known noise, do not need point-device model just yet and can solve inertial navigation rapid alignment problem in the automotive environment and it is vehicle-mounted during external disturbance compensation problem.

Description

Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced

One, technical field

Rapid Alignment Technology between the vehicle-mounted inertial navigation based on Active Disturbance Rejection Control that the present invention relates to a kind of is advanced, belongs to vehicle-mounted Inertial alignment technical field.

Two, background technique

Currently, inertial navigation system has very extensive answer in the fields such as Aeronautics and Astronautics, navigation and land navigation With.Especially in military field, inertial navigation system is even more the huge effect that plays.Vehicular weapons are important in the weapon system of land Component part.Modern vehicular weapons are typically equipped with high-precision inertial navigation equipment, for guiding weapon system, provide navigation Information.Inertial navigation system must be completed initially to be aligned before entering navigation, and the inertia measurement after being initially aligned to provides basis, The precision being initially aligned is to influence the key factor of navigation accuracy.Thus, the Initial Alignment of vehicle-mounted inertial navigation is all the time all It is the hot spot of inertial navigation area research.In recent years, domestic vehicle-mounted inertial navigation generallys use static initial alignment, i.e., starts in carrier vehicle Before, the initial alignment of quiet pedestal is completed using the methods of compass alignment, multiposition alignment and rotation modulation alignment.These sides Consuming time is long for method, alignment precision is low and can not eliminate and compensate interference and error in traveling process.For enhancing weapon Mobility and concealment adapt to modern war environment, accomplish " parking is just beaten " even " fight under way ", need to exist to vehicle-mounted inertial navigation Initial Alignment Method in traveling process is studied.

Currently, initial alignment scheme is broadly divided into two kinds between vehicle-mounted inertial navigation traveling: Transfer Alignment and introducing external auxiliary are believed The alignment of breath.Transfer Alignment is made reference using the main inertial navigation information of high-precision, and the information gap of sub- inertial navigation and main inertial navigation, estimation are utilized Misalignment between the two out, to realize the alignment to sub- inertial navigation.Transfer Alignment proposes very high want to main inertial navigation precision It asks, not only increases cost, and the Error Compensation Technology of Transfer Alignment is also difficult point place;The alignment master of external auxiliary information To use inertial navigation/GPS/ odometer assembled scheme.GPS system can provide the Position And Velocity information of degree of precision in real time, but Since GPS signal is easy to be investigated and interfered, the independence and concealment of system can be reduced.Inertial navigation system can export complete Navigation information is a kind of complete autonomous navigation system, but navigation error dissipates at any time, thus pure inertial navigation system is not able to satisfy Requirement of the vehicular weapons system to navigation accuracy.Odometer can provide accurate velocity information as speed observation device, Course information can be extracted.It has many advantages, such as that error diverging is slow, output accuracy is high and low in cost, therefore, mileage Alignment scheme seems more practical between inertial navigation under meter auxiliary is advanced.

It to complete to be aligned between advancing based on the inertial navigation under odometer auxiliary, it is necessary to establish corresponding Error Propagation Model, benefit State variable is estimated with filter or observer.In control theory, when system is considered into stochastic system, adopt With Kalman filter estimated state;When system is thought of as deterministic system, adoption status observer estimated state.It utilizes Kalman filter carries out the scheme that estimation is current most of scholar's favor to state.But not due to SYSTEM ERROR MODEL It improves and noise statistics is unknown, cause alignment speed slower, device estimation error effect difference and system are quick to noise The problems such as sense.When system model or noise characteristic change greatly, filter result diverging is resulted even in;For second scheme, The estimation adoption status observer of state.Classical control theory leads to control essence due to lacking the application to system model characteristic Spend the problems such as not high, convergence rate is slow, system robustness is not strong.Therefore, new controller is designed to improve control effect, improve Alignment speed is a more novel thinking in Initial Alignment Technique research field in recent years.

In order to meet the requirement of vehicle-mounted inertial navigation rapid alignment between traveling, this paper presents a kind of based on Active Disturbance Rejection Control Rapid Alignment Technology between vehicle-mounted inertial navigation is advanced.

Three, summary of the invention

Aiming at the problems existing in the prior art, between the present invention proposes that a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced Rapid Alignment Technology designs automatic disturbance rejection controller estimation and loses using odometer and the velocity error of inertial navigation as observed quantity Quasi- angle, the final speed and posture for correcting carrier vehicle, realizes the running alignment of inertial navigation.This method be mainly concerned with inertial navigation system, Odometer and automatic disturbance rejection controller.Relationship between them is: inertial navigation system calculates position, speed and the appearance of carrier vehicle State information, odometer are used to measure the velocity information of carrier vehicle, and automatic disturbance rejection controller is using the speed difference of the two as observed quantity, estimation Alignment is realized in misalignment, the disturbance of final compensation system inside and outside.

The inertial navigation system includes inertial measurement cluster IMU and navigation calculation unit.Inertial measurement cluster includes accelerating Degree meter and gyroscope, for measuring the specific force and angular velocity information of output carrier vehicle.These information send to navigation calculation unit it Afterwards, navigation calculation unit calculates the real-time speed of carrier vehicle, position and posture information according to the mechanization of inertial navigation;

The odometer is a kind of independent speed (mileage) measuring device, is mounted on carrier vehicle wheel, and carrier vehicle is measured Velocity information in measurement coordinate system (m system), by under installation matrix conversion to navigational coordinate system (n system);

The automatic disturbance rejection controller is a kind of novel controller, mainly includes Nonlinear Tracking Differentiator TD (Tracking Differentiator, TD), extended state observer ESO (Extended State Observer, ESO) and non-thread character State error feeds back (Non-Linearity State Error Feedback, NLESF).Nonlinear Tracking Differentiator is used to realize to one section The tracking and differential calculation of signal.And extended mode observer utilizes the input and output of system, the state variable of estimating system with And by the real-time effect amount of expansion state.State error improves system to the utilization rate of error with nonlinear combination, Construct Feedback Control Laws.

The present invention proposes Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced.Due to certainly anti- Disturbing control technology has many advantages, such as that fast response time, overshoot are small, precision is high, structure is simple and close-loop dynamic performance is good, can This kind of time-varying, multivariable, coupling and the uncertain system of external disturbance are directed at for handling between vehicle-mounted inertial navigation is advanced.Specifically The following steps are included:

Step 1: before carrier vehicle starting, inertial navigation is initialized, completes the coarse alignment under the conditions of the quiet pedestal of inertial navigation;

General land is all parked in the place of base or fixation with vehicular weapons before being initiated, it is believed that it is initial Position is the geographical location (L, λ, h) in location;Under the conditions of quiet pedestal, it is believed that the initial velocity in three directions of inertial navigation is zero;And Initial attitude matrixIt can be obtained by coarse alignment;Coarse alignment under the conditions of the quiet pedestal of inertial navigation includes that parsing is thick right again Quasi- and primary amendment coarse alignment;

Analytic coarse alignment is carried out first, and analytic coarse alignment relies on gravitational vectors g and earth rotation angular speed ωieDirect estimation Slave system (b system) arrives the initial attitude matrix of geographic coordinate system (t system)G and ωieGeographic coordinate system component all Be it is determining, they can be indicated are as follows:

gt=[gE gN gU]T=[0 0g]T (1)

G represents local gravitational acceleration in formula;ωieIt is earth rotation angular speed;L represents local latitude;It reconstructs out The third vector r orthogonal with themt=gt×ωie t;Using component array of these three vectors in body system and Department of Geography, It can be in the hope of strap-down matrixExpression formula:

Formula (1), (2) are brought into above-mentioned formula and can obtained:

In above formulaIt indicates the component in gravitational vectors three directions in geographic coordinate system, acceleration can be used The output in three directions of degree meterSubstitution;Indicate that rotational-angular velocity of the earth vector is sat in geography The component in three directions in mark system, can use the output in three directions of gyroSubstitution;The geographic latitude of starting point L, earth rotation angular speed ωieAnd gravity acceleration g is known parameters;Using above-mentioned formula, so that it may which rough calculatesValue;But what is obtained here is not genuine Department of Geography (t system), but one has the flat of angle difference with Department of Geography Platform coordinate system (p system), therefore the above results are expressed as

It carries out once correcting coarse alignment again;By the platform coordinate system (p system) that obtains after analytic coarse alignment with really Error angle between geographic coordinate system (t system) is denoted as Φ;The purpose of primary amendment coarse alignment is exactly to utilize gyroscope and acceleration The information of meter solves the antisymmetric matrix Φ of error angle Φ and error angle compositionp, to coarse alignment resultIt carries out further Amendment obtainsConsider that the error of the running interference of carrier and inertia device, error angle Φ expression formula are written as:

WhereinIndicate the output of accelerometer horizontal direction;It is the east orientation output of gyro;▽E,▽NIt represents The zero offset of accelerometer horizontal direction;εEIt is the constant value drift of gyroscope east orientation;adE,adNIt is accelerometer horizontal direction Measurement error;ωdERepresent the measurement error of gyroscope east orientation;G represents local gravitational acceleration;ωieIt is earth rotation angle speed Rate;L represents local latitude;The error angle provided using above-mentioned formula can construct platform system to the transition matrix of Department of GeographyDue to error angle be it is a small amount of, can simplify are as follows:

The result that thus can parse coarse alignment is modified:

In formulaIt is that the body system (b system) that coarse alignment resolves arrives the transition matrix of platform system (p system);It is once to repair The Department of Geography (t system) that positive coarse alignment resolves arrives the transition matrix of platform system (p system);Selecting navigation system (n system) is geographic coordinate system (t system) and " east-north-day " (E-N-U) standard is used,It can be expressed asSo initial value of strap-down matrixIt can directly be indicated with the above results;The characteristics of coarse alignment, is that speed is fast, precision is low, but the essence after being Alignment provides an initial transformation matrix met the requirements

Step 2: after carrier vehicle starting, observed quantity is constructed using the velocity information that inertial navigation and odometer export;

The input of automatic disturbance rejection controller is the velocity error of inertial navigation and odometer, therefore before designing control loop, must Observed quantity need be constructed;After carrier vehicle starting, inertial navigation can be according to the ratio force information that acceleration exports come solving speed; The specific force of output is transformed into navigation system first, the differential equation is recycled to acquire the acceleration information of carrier, finally by product Velocity information is obtained after point, is denoted as Vn;Odometer is mounted on the wheel of carrier vehicle, the circle turned over by wheel in the measurement short time Number obtains the driving speed information of carrier vehicle, and the velocity information of output is denoted as in the case where measuring coordinate system (m system)Odometer During the installation process, established angle is known, can construct odometer measurement coordinate system (m system) to car-carrying machine using mount message The transition matrix of system (b system)Further according to the attitude matrix of inertial reference calculationBy the rate conversion under above-mentioned measurement system to leading Boat system, is denoted as

Since the measurement error of odometer is very small, it can be considered thatApproximate representation is the ideal velocity of carrier vehicle, will The difference of the two speed is considered as observed quantity, can indicate are as follows:

WhereinIt is the horizontal velocity of inertial reference calculation;Represent the horizontal velocity of odometer output;

Step 3: using observed quantity, ins error equation and Auto-disturbance-rejection Control between fine alignment circuit traveling into Row design;

The design in fine alignment circuit is using the velocity error of two horizontal directions as observed quantity, using the error equation of inertial navigation as shape States model;Two horizontal circuit cross-couplings items in system are omitted, original system becomes two decoupled subsystems, respectively to two A subsystem designs extended state observer:

A. the design of east orientation extended state observer ESO_E

The subsystem that east orientation channel is made of east orientation speed error and north orientation misalignment, ignores the shadow in other channels It rings, only retains the variable that the subsystem is coupled, subsystem 1 can indicate are as follows:

δ V in formulaEIt is east orientation speed error;VN, L represent the inertial navigation moment resolving north orientation speed and latitude;R is the earth half Diameter;fUIt is accelerometer day to output;φNIndicate north orientation misalignment;u1、u4Represent feedback quantity;εNIt is the north orientation drift of gyroscope; ▽ERepresent the east orientation zero bias of accelerometer;y1It is the observed quantity of subsystem 1;v1It is observation noise;Enable x1=δ VE,First subsystem can be write as by transformation:

According to extended state observer principle, the ESO_E of subsystem 1 be can be designed as:

Wherein y1Represent the velocity error observation of east orientation;z1(t) and z2(t) the δ V of subsystem is provided in real timeEWithEstimated value;And z3(t) then for estimating north gyro drift εyAnd all uncertain outer resultant actions disturbed; e01It is the difference of actual observed value and estimated value;Fal power function is a kind of nonlinear function, and α and δ are its parameters;β111213 It is the parameter of observer;After above system discretization, it can be used for numerical value calculating;U1、U4It is the feedback after transformation Amount;

B. the design of north orientation extended state observer ESO_N

The subsystem that north orientation channel is made of north orientation speed error and east orientation misalignment, ignores the shadow in other channels It rings, only retains the variable that the subsystem is coupled, subsystem 2 can indicate are as follows:

δ V in formulaNIt is north orientation speed error;VE, L represent the inertial navigation moment resolving east orientation speed and latitude;R is the earth half Diameter;fE,fUIt is accelerometer east orientation and day to output;φEUIndicate east orientation and day to misalignment;u2,u3,u5Represent feedback Amount;εEUThe east orientation and day for being gyroscope are to drift;▽NRepresent the north orientation zero bias of accelerometer;y2It is the observation of subsystem 2 Amount;v2It is observation noise;In the model of subsystem 2, x is enabled1=δ VN,It can With transformation are as follows:

Extended state observer ESO_N is designed using above system:

Wherein y2Represent the velocity error observation of north orientation;It is the longitude change rate of inertial reference calculation;θ1(t), θ2(t) and θ3 (t) it is respectively δ V in second subsystemN,Estimated value;And θ4(t) then for estimating Count east orientation gyroscopic drift εxAnd all uncertain outer resultant actions disturbed;e02It is the difference of actual observed value and estimated value;α and δ It is the parameter of fal power function;β01020304It is the parameter of observer;U2、U3、U5It is the feedback quantity after transformation;

C. nonlinear state error Feedback Design

By above-mentioned analysis, the model of extended state observer has been obtained, in order to construct closed circuit, has needed design of feedback Control law;The characteristics of automatic disturbance rejection controller is not utilize state error directly, but carry out nonlinear combination, raising pair to error The utilization efficiency of error overcomes the contradiction between system rapidity and overshoot;Nonlinear combinational means are generally according to non-linear letter Number fal function and error equation are write to arrange;Feedback Control Laws can indicate are as follows:

Wherein β12345Indicate undetermined parameter, other parameters are defined in above-mentioned steps;In this way, just establishing Based on automatic disturbance rejection controller to quasi loop, the specific force provided according to the velocity error and inertial navigation that measure in real time and resolve Velocity information estimates the value of three variables:These three values are simultaneously The misalignment in not accurate three directions has the estimated value of error;Carefully analyze the estimation it is not difficult to find that misalignment Error is consistent with Kalman filtering;

Step 4: the adjusting of automatic disturbance rejection controller parameter;

The parameter that can be seen that automatic disturbance rejection controller by above-mentioned analysis is more, and these parameters are to the property of controller There can be crucial influence, it is therefore necessary to adjust to parameter;According to separation theorem, the ginseng of automatic disturbance rejection controller three parts Number can be independently arranged;

Extended state observer is a part the most key in auto-disturbance rejection technology, first to the setting method of its parameter It is illustrated;By taking the extended state observer of subsystem 1 as an example, the parameter for needing to adjust in formula (12) mainly includes fal function In parameter alpha and δ and three parameter beta11、β12And β13;α mainly determines non-linear shape, δ determining function linearly interval it is big It is small;Under normal circumstances, empirically value can be chosen for α=1, δ=0.5 to α and δ;β11、β12And β13Three parameters are to influence to close The major parameter of the dynamic characteristic of loop system needs to be adjusted according to the feature of object and actual observation information;It first has to Guarantee the state of accurate tracking observation object, β11Main control tracking velocity, β12Represent gain, β13Controlling curve smoothness; β is adjusted first11So that z1It (t) can faster steady tracking y1(t), β is adjusted further according to estimated value12Control z2(t) size;Most It is further adjusted according to overall control effect afterwards, suitably turns β down13But making to vibrate biggish curve of output becomes steady;? Parameter can be taken into according to the empirical equation provided in some engineerings:

H represents the sampling time interval of emulation in formula;

Nonlinear state error feedback effect is to generate a Nonlinear control law u (t), and the parameter for needing to design mainly is wrapped Include the parameter alpha and δ and β in fal function12345;Wherein α and δ have been analyzed in the above content, no longer superfluous here It states;And β12345In can separate and adjust, wherein β1And β4Control subsystem 1, and β2、β3And β5Control is subsystem System 2;By taking subsystem 1 as an example, parameter beta1And β4Physical significance it is similar to differential coefficient in PID controller and gain coefficient;Work as β1 When larger, adjustment speed is slack-off, but curve is steady;Otherwise β1When smaller, adjustment speed becomes faster, but the oscillation of curve becomes It is greatly or even unstable;β4Be equivalent to a gain, adjusting gain size can control feedback law, make as far as possible estimated state with Desired value is close;It was adjusting in, and was generally first adjusting β1, so that estimation is more excellent in speed and smoothness, then adjust β4 Result is set to be consistent with expection;

Step 5: the misalignment exported by observer, construction feedback matrix realize that vehicle-mounted inertial navigation is running initial right It is quasi-;

Inertial navigation during traveling using inertial measurement cluster IMU data carry out navigation calculation, provide carrier vehicle speed, Position and posture information;But the strap-down matrix of inertial reference calculationIt is not transition matrix of the accurate body system to navigation system, and It is and really navigation is the n for having deviation1Transition matrix between systemn1Error angle between system and n system is exactly to be solved Misalignment;In above-mentioned steps, observer can provide the estimated value of three misalignments are as follows:

Wherein φNEUIt is true misalignment;It is the misalignment of automatic disturbance rejection controller estimation;▽E, ▽NRepresent the zero offset of accelerometer horizontal direction;fUIt is accelerometer day to output;It is the longitude variation of inertial reference calculation Rate;ωieIt is earth rotation angular speed;L represents local latitude;εEIt is the drift of gyroscope east orientation;Because three misalignments are small Angle, therefore feedback matrix can be constructedCome to amendment strap-down matrix:

Inertial navigation is just corrected in this way, and platform system and navigation system will gradually be overlapped, and is achieved that the mesh being aligned between advancing 's;

Above-mentioned five steps, coarse alignment method, observed quantity construction before starting respectively to carrier vehicle, Active Disturbance Rejection Control circuit Design method etc. is described in detail.Method proposed by the present invention do not need the statistical property of known noise, do not need yet it is non- Often accurate model can solve the problems, such as inertial navigation rapid alignment in the automotive environment and it is vehicle-mounted during external disturbance Compensation problem.

The present invention has the advantages that

(1) rapid alignment scheme between the vehicle-mounted inertial navigation proposed by the present invention based on Active Disturbance Rejection Control is advanced, utilizes active disturbance rejection Controller can quickly cope with Initial Alignment of vehicle-mounted inertial navigation during traveling.Due to drawing for automatic disturbance rejection controller Enter, the estimation curve of misalignment has the characteristics that fast convergence rate, overshoot are small.

(2) vehicle-mounted inertial navigation is during traveling, since environment leads to the vibration of carrier and waves, disturbs from external Reason element is more.The unmodeled disturbance of system is expanded into new variable by the extended state observer in automatic disturbance rejection controller, is used Special feedback mechanism establishes observer, estimates these disturbances, and finally compensate, and reaches the mesh for eliminating disturbance 's.

(3) present invention is in the implementation process of scheme, it is only necessary to which the odometer velocity information of certain precision is as reference, no Need other external auxiliary information.Therefore, radiation signal is not generated in the aligning process, with the independence of height and hidden Property.And odometer is low in cost, easy to install and use, and the exploitativeness of scheme is stronger.

(4) although automatic disturbance rejection controller parameter is more, the offering question of parameter is comparatively laborious, although fixed according to separation It manages, extended state observer and the coefficient of nonlinear state error feedback are all that can separate to design in automatic disturbance rejection controller 's.Influence by analysis parameter to system performance, the parameter adjustment law summed up have certain reference with method and answer With value.

Four, Detailed description of the invention

Fig. 1 is the structure chart of system of the present invention.

Fig. 2 is the method for the invention flow chart.

Fig. 3 is coarse alignment method flow chart of heretofore described vehicle-mounted inertial navigation under the conditions of quiet pedestal.

Serial number, code name, symbol description are as follows in figure:

In Fig. 1:

1-inertial navigation subsystem 2-odometer, 3-automatic disturbance rejection controller

101-inertial measurement cluster 102-navigation calculation units

201-202-odometers of odometer install matrix

301-extended state observer 302-nonlinear state errors feedback

303-ins error systemsThe attitude matrix of-inertial navigation

VnThe velocity information of-inertial reference calculationVelocity information under the measurement system of-odometer output

δVn- velocity error informationVelocity information under the navigation system of-odometer output

λ, L, h-inertial reference calculation latitude, longitude and height

The evaluated error vector of-the misaligned angle of the platform

The state variable of θ (t), z (t)-expansion state observation

U (t)-feedback rate control b0,b1- to setting parameter

In Fig. 2:

1-analytic coarse alignment 2-once correct coarse alignment

101-inertial measurement cluster 102-parsing alignment matrix construction units

201-platform systems and navigation are error angle solving unit

202-error matrix structural units

fbThe specific force standard value a that-accelerometer measuresd- accelerometer measures error

The angular speed standard value ω that-gyroscope measuresd- gyroscope measurement error

G-carrier vehicle park place acceleration of gravity L-carrier vehicle park place latitude

Transition matrix of-body the system to platform systemTransition matrix of-platform the system to true Department of Geography

Transition matrix of-body the system to Department of GeographyError angle between-platform system and Department of Geography

ΦpThe antisymmetric matrix of error angle composition between-platform system and Department of Geography

Acceleration of-body the system to Department of GeographyAngular speed of-body the system relative to Department of Geography

In Fig. 3:

Transition matrix of-body the system to platform systemTransition matrix of-body the system to navigation system

δVnThe velocity error information of-inertial navigation and odometer

Five, specific embodiment

Below in conjunction with attached drawing, the present invention is described in further detail.

The present invention proposes Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced.Selection is geographical Coordinate system is navigation system, and specified three axis are oriented in " east-north-day ", and body coordinate system is " on the right side-preceding-".Firstly, using carrying Vehicle parks the geographical location (λ, L) in place as the initial position under the conditions of the quiet pedestal of carrier vehicle, and initial velocity is set as zero.It utilizes Output of inertial measurement component under the conditions of quiet pedestal carries out analytic coarse alignment to inertial navigation, obtains body system and turns to platform system Change matrixUsing primary amendment coarse alignment, the transition matrix of acquisition platform system to true Department of Geography (navigation system) Finally determine initial strap-down matrixAfter carrier vehicle starting, the solving unit of inertial navigation utilizes the defeated of inertial measurement cluster Out, the velocity information V of the carrier with error is providedn.Simultaneously odometer also it is exportable measurement coordinate under carrier vehicle velocity information, And by installation matrixAnd strap-down matrixConversion after obtainUsing the difference of the two as automatic disturbance rejection controller Observed quantity, and the error equation of inertial navigation and the principle of automatic disturbance rejection controller are combined, design fine alignment circuit.Expansion state is seen Device is surveyed by the observation to velocity error, it is estimated that the misalignment in three directions.The state estimation error of observer passes through Nonlinear state error feedback, can be designed that Feedback Control Laws u (t), feedback loop be formed, so that misalignment gradually tends to Zero, finally realize alignment of vehicle-mounted inertial navigation during traveling.

See Fig. 1, the present invention proposes Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced, packet Include inertial navigation subsystem 1, odometer subsystem 2, automatic disturbance rejection controller 3;

Inertial navigation subsystem 1 includes inertial measurement cluster 101 and navigation calculation unit 102;Inertial measurement cluster 101 is surveyed Carrier vehicle is obtained relative to the angular speed of inertial space and than force information, information is transferred to after inertial reference calculation unit, clearing unit benefit The speed V at carrier vehicle current time is provided with the mechanization algorithm and principle of inertial navigationn, position PnAnd attitude matrixDue to Inertial measurement cluster is interfered the influence of acceleration under the conditions of moving base, therefore these information all have error;

Odometer subsystem 2 includes odometer 201 and installation matrix 202.Odometer is mounted on the wheel of carrier vehicle, is passed through The circle number that wheel turns in the measurement short time obtains the driving speed information of carrier vehicle, and the speed of output is in the case where measuring coordinate systemThe transition matrix of car-carrying machine system (b system) is arrived by measurement coordinate system (m system)Obtain the speed under body system.Finally Utilize the attitude matrix of inertial reference calculationAbove-mentioned velocity information is transformed into navigation system, is denoted as

Automatic disturbance rejection controller 3 includes extended state observer 301, nonlinear state error feedback 302 and ins error System 303.Using the error equation of inertial navigation, system is divided into two subsystems, designs expansion state state observer.With Odometer and the velocity error of inertial navigation are input, are estimated respectively state θ (t), z (t).Nonlinear state error is fed back State error is made into nonlinear combination, then designs Nonlinear control law u (t).The effect of controller is exactly to realize three misalignments The estimation at angle and the estimation and compensation of external uncertain disturbance;

See Fig. 2, the present invention proposes Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced, tool Body process the following steps are included:

See that Fig. 3, coarse alignment mainly include analytic coarse alignment 1 and primary amendment coarse alignment 2.Analytic coarse alignment 1 includes used again Property measurement component 101 and parsing alignment matrix construction unit 102.Primary amendment coarse alignment 2 includes 201 He of error angle solving unit Error matrix structural unit 202.

Step 1: before carrier vehicle starting, inertial navigation is initialized, completes the coarse alignment under the conditions of the quiet pedestal of inertial navigation;

General land is all parked in the place of base or fixation with vehicular weapons before being initiated, it is believed that it is initial Position is the geographical location (L, λ, h) in location;Under the conditions of quiet pedestal, it is believed that the initial velocity in three directions of inertial navigation is zero;And Initial attitude matrixIt can be obtained by coarse alignment;Coarse alignment under the conditions of the quiet pedestal of inertial navigation includes that parsing is thick right again Quasi- and primary amendment coarse alignment, as shown in Figure 3;

Analytic coarse alignment is carried out first, and analytic coarse alignment relies on gravitational vectors g and earth rotation angular speed ωieDirect estimation Slave system (b system) arrives the initial attitude matrix of geographic coordinate system (t system)G and ωieGeographic coordinate system component all Be it is determining, they can be indicated are as follows:

gt=[gE gN gU]T=[0 0g]T (1)

G represents local gravitational acceleration in formula;ωieIt is earth rotation angular speed;L represents local latitude;It reconstructs out The third vector r orthogonal with themt=gt×ωie t;Using component array of these three vectors in body system and Department of Geography, It can be in the hope of strap-down matrixExpression formula:

Formula (1), (2) are brought into above-mentioned formula and can obtained:

In above formulaIt indicates the component in gravitational vectors three directions in geographic coordinate system, acceleration can be used The output in three directions of degree meterSubstitution;Indicate that rotational-angular velocity of the earth vector is sat in geography The component in three directions in mark system, can use the output in three directions of gyroSubstitution;The geographic latitude of starting point L, earth rotation angular speed ωieAnd gravity acceleration g is known parameters;Using above-mentioned formula, so that it may which rough calculatesValue;But what is obtained here is not genuine Department of Geography (t system), but one has the flat of angle difference with Department of Geography Platform coordinate system (p system), therefore the above results are expressed as

It carries out once correcting coarse alignment again;By the platform coordinate system (p system) that obtains after analytic coarse alignment with really Error angle between geographic coordinate system (t system) is denoted as Φ;The purpose of primary amendment coarse alignment is exactly to utilize gyroscope and acceleration The information of meter solves the antisymmetric matrix Φ of error angle Φ and error angle compositionp, to coarse alignment resultIt carries out further Amendment obtainsConsider that the error of the running interference of carrier and inertia device, error angle Φ expression formula are written as:

WhereinIndicate the output of accelerometer horizontal direction;It is the east orientation output of gyro;▽E,▽NIt represents The zero offset of accelerometer horizontal direction;εEIt is the constant value drift of gyroscope east orientation;adE,adNIt is accelerometer horizontal direction Measurement error;ωdERepresent the measurement error of gyroscope east orientation;G represents local gravitational acceleration;ωieIt is earth rotation angle speed Rate;L represents local latitude;The error angle provided using above-mentioned formula can construct platform system to the transition matrix of Department of GeographyDue to error angle be it is a small amount of, can simplify are as follows:

The result that thus can parse coarse alignment is modified:

In formulaIt is that the body system (b system) that coarse alignment resolves arrives the transition matrix of platform system (p system);It is once to repair The Department of Geography (t system) that positive coarse alignment resolves arrives the transition matrix of platform system (p system);Selecting navigation system (n system) is geographic coordinate system (t system) and " east-north-day " (E-N-U) standard is used,It can be expressed asSo initial value of strap-down matrixIt can directly be indicated with the above results;The characteristics of coarse alignment, is that speed is fast, precision is low, but the essence after being Alignment provides an initial transformation matrix met the requirements

Step 2: after carrier vehicle starting, observed quantity is constructed using the velocity information that inertial navigation and odometer export;

As shown in Figure 1, the input of automatic disturbance rejection controller is the velocity error of inertial navigation and odometer, therefore controlled back in design Before road, it is necessary to be constructed to observed quantity;Carrier vehicle starting after, inertial navigation can according to acceleration export ratio force information come Solving speed;The specific force of output is transformed into navigation system first, recycles the differential equation to acquire the acceleration information of carrier, most Velocity information is obtained after integral afterwards, is denoted as Vn;Odometer is mounted on the wheel of carrier vehicle, passes through wheel in the measurement short time The circle number turned over obtains the driving speed information of carrier vehicle, and the velocity information of output is denoted as in the case where measuring coordinate system (m system)Odometer during the installation process, established angle be it is known, using mount message can construct odometer measurement coordinate system (m System) arrive car-carrying machine system (b system) transition matrixFurther according to the attitude matrix of inertial reference calculationIt will be under above-mentioned measurement system Rate conversion is to be denoted as to navigation

Since the measurement error of odometer is very small, it can be considered thatApproximate representation is the ideal velocity of carrier vehicle, will The difference of the two speed is considered as observed quantity, can indicate are as follows:

WhereinIt is the horizontal velocity of inertial reference calculation;Represent the horizontal velocity of odometer output;

Step 3: using observed quantity, ins error equation and Auto-disturbance-rejection Control between fine alignment circuit traveling into Row design;

The design in fine alignment circuit is using the velocity error of two horizontal directions as observed quantity, using the error equation of inertial navigation as shape States model;Two horizontal circuit cross-couplings items in system are omitted, original system becomes two decoupled subsystems, respectively to two A subsystem designs extended state observer:

A. the design of east orientation extended state observer ESO_E

The subsystem that east orientation channel is made of east orientation speed error and north orientation misalignment, ignores the shadow in other channels It rings, only retains the variable that the subsystem is coupled, subsystem 1 can indicate are as follows:

δ V in formulaEIt is east orientation speed error;VN, L represent the inertial navigation moment resolving north orientation speed and latitude;R is the earth half Diameter;fUIt is accelerometer day to output;φNIndicate north orientation misalignment;u1、u4Represent feedback quantity;εNIt is the north orientation drift of gyroscope; ▽ERepresent the east orientation zero bias of accelerometer;y1It is the observed quantity of subsystem 1;v1It is observation noise;Enable x1=δ VE,First subsystem can be write as by transformation:

According to extended state observer principle, the ESO_E of subsystem 1 be can be designed as:

Wherein y1Represent the velocity error observation of east orientation;z1(t) and z2(t) the δ V of subsystem is provided in real timeEWithEstimated value;And z3(t) then for estimating north gyro drift εyAnd all uncertain outer resultant actions disturbed; e01It is the difference of actual observed value and estimated value;Fal power function is a kind of nonlinear function, and α and δ are its parameters;β111213 It is the parameter of observer;After above system discretization, it can be used for numerical value calculating;U1、U4It is the feedback after transformation Amount;

B. the design of north orientation extended state observer ESO_N

The subsystem that north orientation channel is made of north orientation speed error and east orientation misalignment, ignores the shadow in other channels It rings, only retains the variable that the subsystem is coupled, subsystem 2 can indicate are as follows:

δ V in formulaNIt is north orientation speed error;VE, L represent the inertial navigation moment resolving east orientation speed and latitude;R is the earth half Diameter;fE,fUIt is accelerometer east orientation and day to output;φEUIndicate east orientation and day to misalignment;u2,u3,u5Represent feedback Amount;εEUThe east orientation and day for being gyroscope are to drift;▽NRepresent the north orientation zero bias of accelerometer;y2It is the observation of subsystem 2 Amount;v2It is observation noise;In the model of subsystem 2, x is enabled1=δ VN,It can With transformation are as follows:

Extended state observer ESO_N is designed using above system:

Wherein y2Represent the velocity error observation of north orientation;It is the longitude change rate of inertial reference calculation;θ1(t), θ2(t) and θ3 (t) it is respectively δ V in second subsystemN,Estimated value;And θ4(t) then for estimating Count east orientation gyroscopic drift εxAnd all uncertain outer resultant actions disturbed;e02It is the difference of actual observed value and estimated value;α and δ It is the parameter of fal power function;β01020304It is the parameter of observer;U2、U3、U5It is the feedback quantity after transformation;

C. nonlinear state error Feedback Design

By above-mentioned analysis, the model of extended state observer has been obtained, in order to construct closed circuit, has needed design of feedback Control law;The characteristics of automatic disturbance rejection controller is not utilize state error directly, but carry out nonlinear combination, raising pair to error The utilization efficiency of error overcomes the contradiction between system rapidity and overshoot;Nonlinear combinational means are generally according to non-linear letter Number fal function and error equation are write to arrange;Feedback Control Laws can indicate are as follows:

Wherein β12345Indicate undetermined parameter, other parameters are defined in above-mentioned steps;In this way, just establishing Based on automatic disturbance rejection controller to quasi loop, the specific force provided according to the velocity error and inertial navigation that measure in real time and resolve Velocity information estimates the value of three variables:These three values are simultaneously The misalignment in not accurate three directions has the estimated value of error;Carefully analyze the estimation it is not difficult to find that misalignment Error is consistent with Kalman filtering;

Step 4: the adjusting of automatic disturbance rejection controller parameter;

The parameter that can be seen that automatic disturbance rejection controller by above-mentioned analysis is more, and these parameters are to the property of controller There can be crucial influence, it is therefore necessary to adjust to parameter;According to separation theorem, the ginseng of automatic disturbance rejection controller three parts Number can be independently arranged;

Extended state observer is a part the most key in auto-disturbance rejection technology, first to the setting method of its parameter It is illustrated;By taking the extended state observer of subsystem 1 as an example, the parameter for needing to adjust in formula (12) mainly includes fal function In parameter alpha and δ and three parameter beta11、β12And β13;α mainly determines non-linear shape, δ determining function linearly interval it is big It is small;Under normal circumstances, empirically value can be chosen for α=1, δ=0.5 to α and δ;β11、β12And β13Three parameters are to influence to close The major parameter of the dynamic characteristic of loop system needs to be adjusted according to the feature of object and actual observation information;It first has to Guarantee the state of accurate tracking observation object, β11Main control tracking velocity, β12Represent gain, β13Controlling curve smoothness; β is adjusted first11So that z1It (t) can faster steady tracking y1(t), β is adjusted further according to estimated value12Control z2(t) size;Most It is further adjusted according to overall control effect afterwards, suitably turns β down13But making to vibrate biggish curve of output becomes steady;? Parameter can be taken into according to the empirical equation provided in some engineerings:

H represents the sampling time interval of emulation in formula;

Nonlinear state error feedback effect is to generate a Nonlinear control law u (t), and the parameter for needing to design mainly is wrapped Include the parameter alpha and δ and β in fal function12345;Wherein α and δ have been analyzed in the above content, no longer superfluous here It states;And β12345In can separate and adjust, wherein β1And β4Control subsystem 1, and β2、β3And β5Control is subsystem System 2;By taking subsystem 1 as an example, parameter beta1And β4Physical significance it is similar to differential coefficient in PID controller and gain coefficient;Work as β1 When larger, adjustment speed is slack-off, but curve is steady;Otherwise β1When smaller, adjustment speed becomes faster, but the oscillation of curve becomes It is greatly or even unstable;β4Be equivalent to a gain, adjusting gain size can control feedback law, make as far as possible estimated state with Desired value is close;It was adjusting in, and was generally first adjusting β1, so that estimation is more excellent in speed and smoothness, then adjust β4 Result is set to be consistent with expection;

Step 5: the misalignment exported by observer, construction feedback matrix realize that vehicle-mounted inertial navigation is running initial right It is quasi-;

Inertial navigation during traveling using inertial measurement cluster IMU data carry out navigation calculation, provide carrier vehicle speed, Position and posture information;But the strap-down matrix of inertial reference calculationIt is not transition matrix of the accurate body system to navigation system, and It is and really navigation is the n for having deviation1Transition matrix between systemn1Error angle between system and n system is exactly to be solved Misalignment;In above-mentioned steps, observer can provide the estimated value of three misalignments are as follows:

Wherein φNEUIt is true misalignment;It is the misalignment of automatic disturbance rejection controller estimation;▽E, ▽NRepresent the zero offset of accelerometer horizontal direction;fUIt is accelerometer day to output;It is the longitude variation of inertial reference calculation Rate;ωieIt is earth rotation angular speed;L represents local latitude;εEIt is the drift of gyroscope east orientation;Because three misalignments are small Angle, therefore feedback matrix can be constructedCome to amendment strap-down matrix:

Inertial navigation is just corrected in this way, and platform system and navigation system will gradually be overlapped, and is achieved that the mesh being aligned between advancing 's;

Coarse alignment method, observed quantity construction, Active Disturbance Rejection Control by above-mentioned five steps, before starting respectively to carrier vehicle Circuit design method etc. is described in detail;Method proposed by the present invention does not need the statistical property of known noise, is not required to yet Want point-device model can solve the problems, such as inertial navigation rapid alignment in the automotive environment and it is vehicle-mounted during external disturb Dynamic compensation problem.

Claims (1)

  1. Rapid Alignment Technology between 1. a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced, it is characterised in that: it specifically includes Following steps:
    Step 1: before carrier vehicle starting, inertial navigation is initialized, completes the coarse alignment under the conditions of the quiet pedestal of inertial navigation;
    General land is all parked in the place of base or fixation with vehicular weapons before being initiated, it is believed that its initial position is institute In the geographical location (L, λ, h) on ground;λ, L and h respectively represent local longitude, latitude and height;Under the conditions of quiet pedestal, it is believed that used The initial velocity for leading three directions is zero;And the initial value of strap-down matrixIt is obtained by coarse alignment;Under the conditions of the quiet pedestal of inertial navigation Coarse alignment again include analytic coarse alignment and primary amendment coarse alignment;
    Analytic coarse alignment is carried out first, and analytic coarse alignment relies on gravitational vectors g and rotational-angular velocity of the earth vector ωieDirect estimation Slave system, that is, b system is to geographic coordinate system, that is, t system initial attitude matrixG and ωieGeographic coordinate system component all It is determining, their expressions are as follows:
    gt=[gE gN gU]T=[0 0 g]T (1)
    G represents local gravitational acceleration in formula;ωieIt is earth rotation angular speed;L represents local latitude;Reconstruct out third A vector r orthogonal with themt=gt×ωie t;Using component array of these three vectors in body system and geographic coordinate system, Just acquireExpression formula:
    Formula (1), (2) are brought into above-mentioned formula and obtained:
    In above formulaThe component for indicating gravitational vectors three directions in body system, with three directions of accelerometer OutputSubstitution;Indicate point in rotational-angular velocity of the earth vector three directions in body system Amount, with the output in three directions of gyroscopeSubstitution;Geographic latitude L, the earth rotation angular speed of starting point ωieAnd gravity acceleration g is known parameters;Using above-mentioned formula, rough can calculateValue;But here To be not genuine geographic coordinate system i.e. t system, a but platform coordinate system i.e. p for having angle difference with geographic coordinate system System, therefore the above results are expressed as
    It carries out once correcting coarse alignment again;By the platform coordinate system i.e. p system obtained after analytic coarse alignment and true geography Error angle between coordinate system, that is, t system is denoted as Φ;The purpose of primary amendment coarse alignment is exactly to utilize gyroscope and accelerometer Information solves the antisymmetric matrix Φ of error angle Φ and error angle compositionp, to analytic coarse alignment resultIt carries out further Amendment obtainsConsider that the error of the running interference of carrier vehicle and inertia device, error angle Φ expression formula are written as:
    WhereinIndicate the output of accelerometer horizontal direction;It is the east orientation output of gyroscope;▽E,▽NIt represents and adds The zero bias of speedometer horizontal direction;εEIt is the east orientation drift of gyroscope;adE,adNIt is the measurement error of accelerometer horizontal direction; ωdERepresent the measurement error of gyroscope east orientation;G represents local gravitational acceleration;ωieIt is earth rotation angular speed;L representative is worked as The latitude on ground;The error angle provided using above-mentioned formula constructs geographic coordinate system to the transition matrix of platform coordinate systemDue to Error angle is a small amount of, therefore is simplified are as follows:
    The result of analytic coarse alignment can be thus modified:
    In formulaIt is the body system i.e. b system that resolves of analytic coarse alignment to the platform coordinate system i.e. transition matrix of p system;It is one Geographic coordinate system, that is, t system that secondary amendment coarse alignment resolves is to platform coordinate system, that is, p system transition matrix;Selection navigation system is n system For geographic coordinate system, that is, t system and " east-north-day " i.e. E-N-U standard is used,It is expressed asSo strap-down matrix Initial valueJust directly indicated with the above results;The characteristics of coarse alignment, is that speed is fast, precision is low, but the fine alignment after being The initial value of one strap-down matrix met the requirements is provided
    Step 2: after carrier vehicle starting, observed quantity is constructed using the velocity information that inertial navigation and odometer export;
    The input of automatic disturbance rejection controller is the velocity error of inertial navigation and odometer, therefore before designing control loop, it is necessary to Observed quantity is constructed;After carrier vehicle starting, the ratio force information that inertial navigation is exported according to accelerometer is come solving speed;First will The specific force of output is transformed into navigation system, and the differential equation is recycled to acquire the acceleration information of carrier vehicle, finally after integral To velocity information, it is denoted as Vn;Odometer is mounted on the wheel of carrier vehicle, is obtained by the circle number that wheel in the measurement short time turns over The driving speed information of carrier vehicle, the velocity information of output are denoted as in the case where measuring coordinate system i.e. m systemOdometer is being installed In the process, established angle is known, measures coordinate system, that is, m system to car-carrying machine system, that is, b system using mount message construction odometer Transition matrixFurther according to the strap-down matrix of inertial reference calculationIt is to navigation by the rate conversion under above-mentioned measurement coordinate system, It is denoted as
    Since the measurement error of odometer is very small, therefore, it is considered thatApproximate representation is the ideal velocity of carrier vehicle, by inertial reference calculation Horizontal velocity and the difference of horizontal velocity of odometer output be considered as observed quantity, indicate are as follows:
    WhereinIt is the horizontal velocity of inertial reference calculation;Represent the horizontal velocity of odometer output;
    Step 3: fine alignment circuit traveling is set using observed quantity, ins error equation and Auto-disturbance-rejection Control Meter;
    The design in fine alignment circuit is using the velocity error of two horizontal directions as observed quantity, using the error equation of inertial navigation as state mould Type;It is directed at this kind of time-varying, multivariable, coupling and the uncertain system of external disturbance between advancing for vehicle-mounted inertial navigation, omits Two horizontal circuit cross-couplings items in system, original system become two decoupled subsystems, set respectively to two subsystems Count extended state observer:
    A. the design of east orientation extended state observer ESO_E
    The subsystem that east orientation channel is made of east orientation speed error and north orientation misalignment, ignores the influence in other channels, only Retain the variable that the subsystem is coupled, subsystem 1 indicates are as follows:
    δ V in formulaEIt is east orientation speed error;VN, L represent the inertial navigation moment resolving north orientation speed and latitude;R is earth radius;fU It is accelerometer day to output;φNIndicate north orientation misalignment;u1、u4Represent feedback quantity;εNIt is the north orientation drift of gyroscope;▽EGeneration The east orientation zero bias of table accelerometer;y1It is the observed quantity of subsystem 1;v1It is observation noise;Enable x1=δ VE,Son System 1 is write as by transformation:
    According to extended state observer principle, the ESO_E of subsystem 1 is designed are as follows:
    Wherein y1Represent the velocity error observation of east orientation;z1(t) and z2(t) the δ V of subsystem 1 is provided in real timeEWith's Estimated value;And z3(t) then for estimating the north orientation drift ε of gyroscopeNAnd all uncertain outer resultant actions disturbed;e01It is real The difference of border observation and estimated value;Fal power function is a kind of nonlinear function, and α and δ are its parameters;β111213It is observation The parameter of device;After above-mentioned 1 discretization of subsystem, calculated for numerical value;U1、U4It is the feedback quantity after transformation;
    B. the design of north orientation extended state observer ESO_N
    The subsystem that north orientation channel is made of north orientation speed error and east orientation misalignment, ignores the influence in other channels, only Retain the variable that the subsystem is coupled, subsystem 2 indicates are as follows:
    δ V in formulaNIt is north orientation speed error;VE, L represent the inertial navigation moment resolving east orientation speed and latitude;R is earth radius;fE, fUIt is accelerometer east orientation and day to output;φEUIndicate east orientation and day to misalignment;u2,u3,u5Represent feedback quantity;εEU The east orientation and day for being gyroscope are to drift;▽NRepresent the north orientation zero bias of accelerometer;y2It is the observed quantity of subsystem 2;v2It is to see Survey noise;Enable x1=δ VN,Subsystem 2 is write as by transformation:
    According to extended state observer principle, the ESO_N of subsystem 2 is designed are as follows:
    Wherein y2Represent the velocity error observation of north orientation;It is the longitude change rate of inertial reference calculation;θ1(t), θ2(t) and θ3(t) It is the δ V in subsystem 2 respectivelyN,Estimated value;And θ4(t) then for estimating gyroscope East orientation drift εEAnd all uncertain outer resultant actions disturbed;e02It is the difference of actual observed value and estimated value;α and δ is fal power The parameter of secondary function;β01020304It is the parameter of observer;U2、U3、U5It is the feedback quantity after transformation;
    C. nonlinear state error Feedback Design
    By above-mentioned analysis, the model of extended state observer has been obtained, in order to construct closed circuit, has needed design of feedback control Rule;The characteristics of automatic disturbance rejection controller is not utilize state error directly, but carry out nonlinear combination to error, is improved to error Utilization efficiency, overcome the contradiction between system rapidity and overshoot;Nonlinear combinational means are generally according to nonlinear function fal Function and error equation are write to arrange;Feedback Control Laws indicate are as follows:
    Wherein β12345Indicate undetermined parameter, other parameters are defined in above-mentioned steps;In this way, just establishing base In automatic disturbance rejection controller to quasi loop, according to the speed of the velocity error and the inertial navigation specific force provided and resolving that measure in real time Information estimates the value of three variables: These three values are not The misalignment in accurate three directions, but have the estimated value of error;Carefully analyze the evaluated error it is not difficult to find that misalignment It is consistent with Kalman filtering;
    Step 4: the adjusting of automatic disturbance rejection controller parameter;
    Found out by above-mentioned analysis, the parameter of automatic disturbance rejection controller is more, and these parameters have pass to the performance of controller The influence of key, it is therefore necessary to which parameter is adjusted;According to separation theorem, the parameter of automatic disturbance rejection controller three parts is independent It is arranged;
    Extended state observer is a part the most key in auto-disturbance rejection technology, is carried out first to the setting method of its parameter Explanation;By taking the extended state observer of subsystem 1 as an example, the parameter for needing to adjust in formula (12) mainly includes in fal function Parameter alpha and δ and three parameter beta11、β12And β13;α mainly determines non-linear shape, the size of δ determining function linearly interval;α Empirically value can be chosen for α=1, δ=0.5 with δ;β11、β12And β13Three parameters are to influence the dynamic characteristic of closed-loop system Major parameter, need to be adjusted according to the feature of object and actual observation information;It first has to guarantee accurately to track to see Survey the state of object, β11Main control tracking velocity, β12Represent gain, β13Controlling curve smoothness;β is adjusted first11So that z1 It (t) can faster steady tracking y1(t), β is adjusted further according to estimated value12Control z2(t) size;Finally according to overall control Effect further adjusts, and suitably turns β down13Can make to vibrate biggish curve of output becomes steady;Also according to being provided in some engineerings Empirical equation, parameter is taken into:
    H' represents the sampling time interval of emulation in formula;
    Nonlinear state error feedback effect is to generate a Nonlinear control law u (t), and the parameter for needing to design includes fal letter Parameter alpha and δ and β in number12345;Wherein α and δ have been analyzed in the above content, and which is not described herein again;And β12345In separately adjust, wherein β1And β4Control subsystem 1, and β2、β3And β5Control is subsystem 2;With subsystem For system 1, parameter beta1And β4Physical significance it is similar to differential coefficient in PID controller and gain coefficient;Work as β1When larger, adjust Section slows, but curve is steady;Otherwise β1When smaller, adjustment speed becomes faster, but the oscillation of curve becomes larger, or even unstable It is fixed;β4It is equivalent to a gain, adjusting gain size can control feedback law, make estimated state close with desired value as far as possible;? During adjustment, β is generally first adjusted1, so that estimation is more excellent in speed and smoothness, then adjust β4Make result and expection It is consistent;
    Step 5: the misalignment exported by observer, construction feedback matrix realize the vehicle-mounted running initial alignment of inertial navigation;
    Inertial navigation carries out navigation calculation using the data of inertial measurement cluster IMU during traveling, provides speed, the position of carrier vehicle And posture information;But the strap-down matrix of inertial reference calculationTransition matrix of the not accurate body system to navigation system, machine System to and true navigation system have the n of deviation1Transition matrix between systemn1Be n system between error angle be exactly to The misalignment of solution;In above-mentioned steps, observer can provide the estimated value of three misalignments are as follows:
    Wherein φNEUIt is true misalignment;It is the misalignment of automatic disturbance rejection controller estimation;▽E,▽NGeneration The zero bias of table accelerometer horizontal direction;fUIt is accelerometer day to output;It is the longitude change rate of inertial reference calculation;ωieIt is Earth rotation angular speed;L represents local latitude;εEIt is the drift of gyroscope east orientation;Because three misalignments are low-angles, Feedback matrix can be constructedTo correct strap-down matrix:
    Inertial navigation is just corrected in this way, and platform coordinate system and navigation system will gradually be overlapped, and is achieved that the mesh being aligned between advancing 's.
CN201610146652.5A 2016-03-15 2016-03-15 Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced CN105806365B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201610146652.5A CN105806365B (en) 2016-03-15 2016-03-15 Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610146652.5A CN105806365B (en) 2016-03-15 2016-03-15 Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced

Publications (2)

Publication Number Publication Date
CN105806365A CN105806365A (en) 2016-07-27
CN105806365B true CN105806365B (en) 2019-06-07

Family

ID=56468396

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610146652.5A CN105806365B (en) 2016-03-15 2016-03-15 Rapid Alignment Technology between a kind of vehicle-mounted inertial navigation based on Active Disturbance Rejection Control is advanced

Country Status (1)

Country Link
CN (1) CN105806365B (en)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
FR3057349B1 (en) * 2016-10-11 2019-07-12 Safran Electronics & Defense IMPROVEMENTS IN INERTIAL PLANT ALIGNMENT METHODS
CN106843214B (en) * 2017-02-13 2020-02-21 浙江工业大学 Tape guidance AGV tracking control method based on active disturbance rejection control
CN107036630B (en) * 2017-04-27 2020-09-25 深圳市思拓通信系统有限公司 Automatic identification system and method for installation angle of vehicle driving early warning device
CN107830873A (en) * 2017-11-01 2018-03-23 北京航空航天大学 A kind of high-precision vehicle positioning and orienting method combined based on uniaxial horizontal rotation inertial navigation with odometer
CN108931247A (en) * 2018-04-08 2018-12-04 和芯星通科技(北京)有限公司 A kind of air navigation aid and device
CN108562288A (en) * 2018-05-08 2018-09-21 北京航天时代激光导航技术有限责任公司 A kind of Laser strapdown used group of system-level online self-calibration system and method
CN109631939A (en) * 2018-11-08 2019-04-16 湖北三江航天红峰控制有限公司 A kind of rapid alignment method based on magnetometer and accelerometer

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0474105A2 (en) * 1990-08-30 1992-03-11 Jacob, Heinrich G., Prof. Dr.-Ing. Inertial navigation system with a compensation filter for its alignment during flight
CN103743414A (en) * 2014-01-02 2014-04-23 东南大学 Initial alignment method of speedometer-assisted strapdown inertial navigation system during running
CN104897156A (en) * 2015-06-03 2015-09-09 北京理工大学 Spin control method for rotation modulation inertial navigation system

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0474105A2 (en) * 1990-08-30 1992-03-11 Jacob, Heinrich G., Prof. Dr.-Ing. Inertial navigation system with a compensation filter for its alignment during flight
CN103743414A (en) * 2014-01-02 2014-04-23 东南大学 Initial alignment method of speedometer-assisted strapdown inertial navigation system during running
CN104897156A (en) * 2015-06-03 2015-09-09 北京理工大学 Spin control method for rotation modulation inertial navigation system

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
"一种抗基座扰动的捷联惯导系统初始对准方法";高伟熙 等,;《北京理工大学学报》;20100630;第30卷;190-194页
"一种里程计辅助车载捷联惯导行进间对准方法";陈鸿跃 等,;《导弹与航天运载技术》;20131231(第5期);44-50页
"自抗扰控制技术在捷联惯导动基座初始对准中的应用研究";牛立 等,;《中国惯性技术学报》;20031231;第11卷(第6期);27-33页

Also Published As

Publication number Publication date
CN105806365A (en) 2016-07-27

Similar Documents

Publication Publication Date Title
US10338261B2 (en) Measurement of magnetic field gradients
US20170074660A1 (en) Magnetic Field Gradient Navigation Aid
Kingston et al. Real-time attitude and position estimation for small UAVs using low-cost sensors
Fang et al. Predictive iterated Kalman filter for INS/GPS integration and its application to SAR motion compensation
Tang et al. INS/GPS integration: Global observability analysis
CN103759730B (en) The collaborative navigation system of a kind of pedestrian based on navigation information two-way fusion and intelligent mobile carrier and air navigation aid thereof
US5331578A (en) Procedure for measuring angles and trajectories by means of gyros and inertial systems
CN101413800B (en) Navigating and steady aiming method of navigation / steady aiming integrated system
Yu et al. Comparison of SDINS in-flight alignment using equivalent error models
CN102692225B (en) Attitude heading reference system for low-cost small unmanned aerial vehicle
CA2530903C (en) Method and system for improving accuracy of inertial navigation measurements using measured and stored gravity gradients
Goshen-Meskin et al. Unified approach to inertial navigation system error modeling
CA1141008A (en) Autonomous navigation system
Georgy et al. Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation
CN103575299B (en) Utilize dual-axis rotation inertial navigation system alignment and the error correcting method of External Observation information
US4930085A (en) Method for determining the heading of an aircraft
Hong Fuzzy logic based closed-loop strapdown attitude system for unmanned aerial vehicle (UAV)
CN103743414B (en) Initial Alignment Method between the traveling of vehicle-mounted SINS assisted by a kind of speedometer
Barczyk et al. Integration of a triaxial magnetometer into a helicopter UAV GPS-aided INS
Jung et al. Inertial attitude and position reference system development for a small UAV
CN106708066A (en) Autonomous landing method of unmanned aerial vehicle based on vision/inertial navigation
Li et al. Low-cost tightly coupled GPS/INS integration based on a nonlinear Kalman filtering design
CN102445200A (en) Microminiature personal combined navigation system as well as navigating and positioning method thereof
CN104698485B (en) Integrated navigation system and air navigation aid based on BD, GPS and MEMS
CN103196448B (en) A kind of airborne distributed inertia surveys appearance system and Transfer Alignment thereof

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant