CN105806365B  Rapid Alignment Technology between a kind of vehiclemounted inertial navigation based on Active Disturbance Rejection Control is advanced  Google Patents
Rapid Alignment Technology between a kind of vehiclemounted inertial navigation based on Active Disturbance Rejection Control is advanced Download PDFInfo
 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
Links
 238000005516 engineering processes Methods 0.000 title claims abstract description 16
 239000011159 matrix materials Substances 0.000 claims abstract description 73
 239000000969 carriers Substances 0.000 claims abstract description 51
 238000010276 construction Methods 0.000 claims abstract description 10
 238000004364 calculation methods Methods 0.000 claims description 29
 230000001133 acceleration Effects 0.000 claims description 21
 230000001131 transforming Effects 0.000 claims description 14
 230000000694 effects Effects 0.000 claims description 11
 238000000034 methods Methods 0.000 claims description 10
 239000000203 mixtures Substances 0.000 claims description 9
 238000004458 analytical methods Methods 0.000 claims description 7
 230000014509 gene expression Effects 0.000 claims description 7
 238000006467 substitution reactions Methods 0.000 claims description 6
 238000006243 chemical reactions Methods 0.000 claims description 5
 230000005484 gravity Effects 0.000 claims description 4
 238000000926 separation method Methods 0.000 claims description 4
 238000006880 crosscoupling reactions Methods 0.000 claims description 3
 238000001914 filtration Methods 0.000 claims description 3
 238000005070 sampling Methods 0.000 claims description 3
 230000001808 coupling Effects 0.000 claims description 2
 238000010168 coupling process Methods 0.000 claims description 2
 238000005859 coupling reactions Methods 0.000 claims description 2
 230000001340 slower Effects 0.000 claims 1
 238000009434 installation Methods 0.000 description 5
 239000000686 essences Substances 0.000 description 3
 241000208340 Araliaceae Species 0.000 description 2
 235000003140 Panax quinquefolius Nutrition 0.000 description 2
 235000005035 ginseng Nutrition 0.000 description 2
 235000008434 ginseng Nutrition 0.000 description 2
 239000006227 byproducts Substances 0.000 description 1
 230000000875 corresponding Effects 0.000 description 1
 230000002708 enhancing Effects 0.000 description 1
 230000000051 modifying Effects 0.000 description 1
 230000003068 static Effects 0.000 description 1
Classifications

 G—PHYSICS
 G01—MEASURING; TESTING
 G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
 G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
 G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or startingup of inertial devices

 G—PHYSICS
 G01—MEASURING; TESTING
 G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
 G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00  G01C19/00
 G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00  G01C19/00 by using measurements of speed or acceleration
 G01C21/12—Navigation; 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/16—Navigation; 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/18—Stabilised platforms, e.g. by gyroscope
Abstract
Description
One, technical field
Rapid Alignment Technology between the vehiclemounted inertial navigation based on Active Disturbance Rejection Control that the present invention relates to a kind of is advanced, belongs to vehiclemounted 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 highprecision 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 vehiclemounted inertial navigation is all the time all It is the hot spot of inertial navigation area research.In recent years, domestic vehiclemounted 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 vehiclemounted inertial navigation Initial Alignment Method in traveling process is studied.
Currently, initial alignment scheme is broadly divided into two kinds between vehiclemounted 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 highprecision, 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 vehiclemounted inertial navigation rapid alignment between traveling, this paper presents a kind of based on Active Disturbance Rejection Control Rapid Alignment Technology between vehiclemounted 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 vehiclemounted 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 realtime 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 nonthread character State error feeds back (NonLinearity 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 realtime 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 vehiclemounted 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 closeloop dynamic performance is good, can This kind of timevarying, multivariable, coupling and the uncertain system of external disturbance are directed at for handling between vehiclemounted 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 ω_{ie}Direct estimation Slave system (b system) arrives the initial attitude matrix of geographic coordinate system (t system)G and ω_{ie}Geographic coordinate system component all Be it is determining, they can be indicated are as follows:
g^{t}=[g_{E} g_{N} g_{U}]^{T}=[0 0g]^{T} (1)
G represents local gravitational acceleration in formula；ω_{ie}It is earth rotation angular speed；L represents local latitude；It reconstructs out The third vector r orthogonal with them^{t}=g^{t}×ω_{ie} ^{t}；Using component array of these three vectors in body system and Department of Geography, It can be in the hope of strapdown matrixExpression formula:
Formula (1), (2) are brought into abovementioned 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 rotationalangular 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 ω_{ie}And gravity acceleration g is known parameters；Using abovementioned 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 composition^{p}, 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},▽_{N}It represents The zero offset of accelerometer horizontal direction；ε_{E}It is the constant value drift of gyroscope east orientation；a_{dE},a_{dN}It is accelerometer horizontal direction Measurement error；ω_{dE}Represent the measurement error of gyroscope east orientation；G represents local gravitational acceleration；ω_{ie}It is earth rotation angle speed Rate；L represents local latitude；The error angle provided using abovementioned 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 " eastnorthday " (ENU) standard is used,It can be expressed asSo initial value of strapdown 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 V^{n}；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 carcarrying 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 abovementioned 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 Autodisturbancerejection 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 crosscouplings 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 formula_{E}It is east orientation speed error；V_{N}, L represent the inertial navigation moment resolving north orientation speed and latitude；R is the earth half Diameter；f_{U}It is accelerometer day to output；φ_{N}Indicate north orientation misalignment；u_{1}、u_{4}Represent feedback quantity；ε_{N}It is the north orientation drift of gyroscope； ▽_{E}Represent the east orientation zero bias of accelerometer；y_{1}It is the observed quantity of subsystem 1；v_{1}It is observation noise；Enable x_{1}=δ V_{E},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 y_{1}Represent the velocity error observation of east orientation；z_{1}(t) and z_{2}(t) the δ V of subsystem is provided in real time_{E}WithEstimated value；And z_{3}(t) then for estimating north gyro drift ε_{y}And all uncertain outer resultant actions disturbed； e_{01}It is the difference of actual observed value and estimated value；Fal power function is a kind of nonlinear function, and α and δ are its parameters；β_{11},β_{12},β_{13} It is the parameter of observer；After above system discretization, it can be used for numerical value calculating；U_{1}、U_{4}It 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 formula_{N}It is north orientation speed error；V_{E}, L represent the inertial navigation moment resolving east orientation speed and latitude；R is the earth half Diameter；f_{E},f_{U}It is accelerometer east orientation and day to output；φ_{E},φ_{U}Indicate east orientation and day to misalignment；u_{2},u_{3},u_{5}Represent feedback Amount；ε_{E},ε_{U}The east orientation and day for being gyroscope are to drift；▽_{N}Represent the north orientation zero bias of accelerometer；y_{2}It is the observation of subsystem 2 Amount；v_{2}It is observation noise；In the model of subsystem 2, x is enabled_{1}=δ V_{N},It can With transformation are as follows:
Extended state observer ESO_N is designed using above system:
Wherein y_{2}Represent 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 subsystem_{N},Estimated value；And θ_{4}(t) then for estimating Count east orientation gyroscopic drift ε_{x}And all uncertain outer resultant actions disturbed；e_{02}It is the difference of actual observed value and estimated value；α and δ It is the parameter of fal power function；β_{01},β_{02},β_{03},β_{04}It is the parameter of observer；U_{2}、U_{3}、U_{5}It is the feedback quantity after transformation；
C. nonlinear state error Feedback Design
By abovementioned 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 nonlinear letter Number fal function and error equation are write to arrange；Feedback Control Laws can indicate are as follows:
Wherein β_{1},β_{2},β_{3},β_{4},β_{5}Indicate undetermined parameter, other parameters are defined in abovementioned 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 abovementioned 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 autodisturbance 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 beta_{11}、β_{12}And β_{13}；α mainly determines nonlinear 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}、β_{12}And β_{13}Three 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, β_{11}Main control tracking velocity, β_{12}Represent gain, β_{13}Controlling curve smoothness； β is adjusted first_{11}So that z_{1}It (t) can faster steady tracking y_{1}(t), β is adjusted further according to estimated value_{12}Control z_{2}(t) size；Most It is further adjusted according to overall control effect afterwards, suitably turns β down_{13}But 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 function_{1},β_{2},β_{3},β_{4},β_{5}；Wherein α and δ have been analyzed in the above content, no longer superfluous here It states；And β_{1},β_{2},β_{3},β_{4},β_{5}In can separate and adjust, wherein β_{1}And β_{4}Control subsystem 1, and β_{2}、β_{3}And β_{5}Control is subsystem System 2；By taking subsystem 1 as an example, parameter beta_{1}And β_{4}Physical significance it is similar to differential coefficient in PID controller and gain coefficient；Work as β_{1} When larger, adjustment speed is slackoff, but curve is steady；Otherwise β_{1}When smaller, adjustment speed becomes faster, but the oscillation of curve becomes It is greatly or even unstable；β_{4}Be 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 vehiclemounted 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 strapdown 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 deviation_{1}Transition matrix between systemn_{1}Error angle between system and n system is exactly to be solved Misalignment；In abovementioned steps, observer can provide the estimated value of three misalignments are as follows:
Wherein φ_{N},φ_{E},φ_{U}It is true misalignment；It is the misalignment of automatic disturbance rejection controller estimation；▽_{E}, ▽_{N}Represent the zero offset of accelerometer horizontal direction；f_{U}It is accelerometer day to output；It is the longitude variation of inertial reference calculation Rate；ω_{ie}It is earth rotation angular speed；L represents local latitude；ε_{E}It is the drift of gyroscope east orientation；Because three misalignments are small Angle, therefore feedback matrix can be constructedCome to amendment strapdown 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；
Abovementioned 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 vehiclemounted during external disturbance Compensation problem.
The present invention has the advantages that
(1) rapid alignment scheme between the vehiclemounted 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 vehiclemounted 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) vehiclemounted 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 vehiclemounted inertial navigation under the conditions of quiet pedestal.
Serial number, code name, symbol description are as follows in figure:
In Fig. 1:
1inertial navigation subsystem 2odometer, 3automatic disturbance rejection controller
101inertial measurement cluster 102navigation calculation units
201202odometers of odometer install matrix
301extended state observer 302nonlinear state errors feedback
303ins error systemsThe attitude matrix ofinertial navigation
V^{n}The velocity information ofinertial reference calculationVelocity information under the measurement system ofodometer output
δV^{n} velocity error informationVelocity information under the navigation system ofodometer output
λ, L, hinertial reference calculation latitude, longitude and height
The evaluated error vector ofthe misaligned angle of the platform
The state variable of θ (t), z (t)expansion state observation
U (t)feedback rate control b_{0},b_{1} to setting parameter
In Fig. 2:
1analytic coarse alignment 2once correct coarse alignment
101inertial measurement cluster 102parsing alignment matrix construction units
201platform systems and navigation are error angle solving unit
202error matrix structural units
f^{b}The specific force standard value a thataccelerometer measures_{d} accelerometer measures error
The angular speed standard value ω thatgyroscope measures_{d} gyroscope measurement error
Gcarrier vehicle park place acceleration of gravity Lcarrier vehicle park place latitude
Transition matrix ofbody the system to platform systemTransition matrix ofplatform the system to true Department of Geography
Transition matrix ofbody the system to Department of GeographyError angle betweenplatform system and Department of Geography
Φ^{p}The antisymmetric matrix of error angle composition betweenplatform system and Department of Geography
Acceleration ofbody the system to Department of GeographyAngular speed ofbody the system relative to Department of Geography
In Fig. 3:
Transition matrix ofbody the system to platform systemTransition matrix ofbody the system to navigation system
δV^{n}The velocity error information ofinertial 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 vehiclemounted 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 " eastnorthday ", and body coordinate system is " on the right sidepreceding".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 strapdown 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 provided^{n}.Simultaneously odometer also it is exportable measurement coordinate under carrier vehicle velocity information, And by installation matrixAnd strapdown 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 vehiclemounted inertial navigation during traveling.
See Fig. 1, the present invention proposes Rapid Alignment Technology between a kind of vehiclemounted 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 navigation^{n}, position P^{n}And 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 carcarrying 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 calculationAbovementioned 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 vehiclemounted 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 ω_{ie}Direct estimation Slave system (b system) arrives the initial attitude matrix of geographic coordinate system (t system)G and ω_{ie}Geographic coordinate system component all Be it is determining, they can be indicated are as follows:
g^{t}=[g_{E} g_{N} g_{U}]^{T}=[0 0g]^{T} (1)
G represents local gravitational acceleration in formula；ω_{ie}It is earth rotation angular speed；L represents local latitude；It reconstructs out The third vector r orthogonal with them^{t}=g^{t}×ω_{ie} ^{t}；Using component array of these three vectors in body system and Department of Geography, It can be in the hope of strapdown matrixExpression formula:
Formula (1), (2) are brought into abovementioned 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 rotationalangular 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 ω_{ie}And gravity acceleration g is known parameters；Using abovementioned 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 composition^{p}, 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},▽_{N}It represents The zero offset of accelerometer horizontal direction；ε_{E}It is the constant value drift of gyroscope east orientation；a_{dE},a_{dN}It is accelerometer horizontal direction Measurement error；ω_{dE}Represent the measurement error of gyroscope east orientation；G represents local gravitational acceleration；ω_{ie}It is earth rotation angle speed Rate；L represents local latitude；The error angle provided using abovementioned 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 " eastnorthday " (ENU) standard is used,It can be expressed asSo initial value of strapdown 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 V^{n}；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 carcarrying machine system (b system) transition matrixFurther according to the attitude matrix of inertial reference calculationIt will be under abovementioned 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 Autodisturbancerejection 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 crosscouplings 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 formula_{E}It is east orientation speed error；V_{N}, L represent the inertial navigation moment resolving north orientation speed and latitude；R is the earth half Diameter；f_{U}It is accelerometer day to output；φ_{N}Indicate north orientation misalignment；u_{1}、u_{4}Represent feedback quantity；ε_{N}It is the north orientation drift of gyroscope； ▽_{E}Represent the east orientation zero bias of accelerometer；y_{1}It is the observed quantity of subsystem 1；v_{1}It is observation noise；Enable x_{1}=δ V_{E},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 y_{1}Represent the velocity error observation of east orientation；z_{1}(t) and z_{2}(t) the δ V of subsystem is provided in real time_{E}WithEstimated value；And z_{3}(t) then for estimating north gyro drift ε_{y}And all uncertain outer resultant actions disturbed； e_{01}It is the difference of actual observed value and estimated value；Fal power function is a kind of nonlinear function, and α and δ are its parameters；β_{11},β_{12},β_{13} It is the parameter of observer；After above system discretization, it can be used for numerical value calculating；U_{1}、U_{4}It 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 formula_{N}It is north orientation speed error；V_{E}, L represent the inertial navigation moment resolving east orientation speed and latitude；R is the earth half Diameter；f_{E},f_{U}It is accelerometer east orientation and day to output；φ_{E},φ_{U}Indicate east orientation and day to misalignment；u_{2},u_{3},u_{5}Represent feedback Amount；ε_{E},ε_{U}The east orientation and day for being gyroscope are to drift；▽_{N}Represent the north orientation zero bias of accelerometer；y_{2}It is the observation of subsystem 2 Amount；v_{2}It is observation noise；In the model of subsystem 2, x is enabled_{1}=δ V_{N},It can With transformation are as follows:
Extended state observer ESO_N is designed using above system:
Wherein y_{2}Represent 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 subsystem_{N},Estimated value；And θ_{4}(t) then for estimating Count east orientation gyroscopic drift ε_{x}And all uncertain outer resultant actions disturbed；e_{02}It is the difference of actual observed value and estimated value；α and δ It is the parameter of fal power function；β_{01},β_{02},β_{03},β_{04}It is the parameter of observer；U_{2}、U_{3}、U_{5}It is the feedback quantity after transformation；
C. nonlinear state error Feedback Design
By abovementioned 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 nonlinear letter Number fal function and error equation are write to arrange；Feedback Control Laws can indicate are as follows:
Wherein β_{1},β_{2},β_{3},β_{4},β_{5}Indicate undetermined parameter, other parameters are defined in abovementioned 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 abovementioned 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 autodisturbance 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 beta_{11}、β_{12}And β_{13}；α mainly determines nonlinear 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}、β_{12}And β_{13}Three 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, β_{11}Main control tracking velocity, β_{12}Represent gain, β_{13}Controlling curve smoothness； β is adjusted first_{11}So that z_{1}It (t) can faster steady tracking y_{1}(t), β is adjusted further according to estimated value_{12}Control z_{2}(t) size；Most It is further adjusted according to overall control effect afterwards, suitably turns β down_{13}But 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 function_{1},β_{2},β_{3},β_{4},β_{5}；Wherein α and δ have been analyzed in the above content, no longer superfluous here It states；And β_{1},β_{2},β_{3},β_{4},β_{5}In can separate and adjust, wherein β_{1}And β_{4}Control subsystem 1, and β_{2}、β_{3}And β_{5}Control is subsystem System 2；By taking subsystem 1 as an example, parameter beta_{1}And β_{4}Physical significance it is similar to differential coefficient in PID controller and gain coefficient；Work as β_{1} When larger, adjustment speed is slackoff, but curve is steady；Otherwise β_{1}When smaller, adjustment speed becomes faster, but the oscillation of curve becomes It is greatly or even unstable；β_{4}Be 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 vehiclemounted 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 strapdown 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 deviation_{1}Transition matrix between systemn_{1}Error angle between system and n system is exactly to be solved Misalignment；In abovementioned steps, observer can provide the estimated value of three misalignments are as follows:
Wherein φ_{N},φ_{E},φ_{U}It is true misalignment；It is the misalignment of automatic disturbance rejection controller estimation；▽_{E}, ▽_{N}Represent the zero offset of accelerometer horizontal direction；f_{U}It is accelerometer day to output；It is the longitude variation of inertial reference calculation Rate；ω_{ie}It is earth rotation angular speed；L represents local latitude；ε_{E}It is the drift of gyroscope east orientation；Because three misalignments are small Angle, therefore feedback matrix can be constructedCome to amendment strapdown 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 abovementioned 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 pointdevice model can solve the problems, such as inertial navigation rapid alignment in the automotive environment and it is vehiclemounted during external disturb Dynamic compensation problem.
Claims (1)
 Rapid Alignment Technology between 1. a kind of vehiclemounted 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 strapdown 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 rotationalangular velocity of the earth vector ω_{ie}Direct estimation Slave system, that is, b system is to geographic coordinate system, that is, t system initial attitude matrixG and ω_{ie}Geographic coordinate system component all It is determining, their expressions are as follows:g^{t}=[g_{E} g_{N} g_{U}]^{T}=[0 0 g]^{T} (1)G represents local gravitational acceleration in formula；ω_{ie}It is earth rotation angular speed；L represents local latitude；Reconstruct out third A vector r orthogonal with them^{t}=g^{t}×ω_{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 abovementioned 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 rotationalangular 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 ω_{ie}And gravity acceleration g is known parameters；Using abovementioned 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 asIt 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 composition^{p}, 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},▽_{N}It represents and adds The zero bias of speedometer horizontal direction；ε_{E}It is the east orientation drift of gyroscope；a_{dE},a_{dN}It is the measurement error of accelerometer horizontal direction； ω_{dE}Represent the measurement error of gyroscope east orientation；G represents local gravitational acceleration；ω_{ie}It is earth rotation angular speed；L representative is worked as The latitude on ground；The error angle provided using abovementioned 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 " eastnorthday " i.e. ENU standard is used,It is expressed asSo strapdown 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 strapdown matrix met the requirements is providedStep 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 V^{n}；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 carcarrying machine system, that is, b system using mount message construction odometer Transition matrixFurther according to the strapdown matrix of inertial reference calculationIt is to navigation by the rate conversion under abovementioned measurement coordinate system, It is denoted asSince 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 Autodisturbancerejection 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 timevarying, multivariable, coupling and the uncertain system of external disturbance between advancing for vehiclemounted inertial navigation, omits Two horizontal circuit crosscouplings 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_EThe 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 formula_{E}It is east orientation speed error；V_{N}, L represent the inertial navigation moment resolving north orientation speed and latitude；R is earth radius；f_{U} It is accelerometer day to output；φ_{N}Indicate north orientation misalignment；u_{1}、u_{4}Represent feedback quantity；ε_{N}It is the north orientation drift of gyroscope；▽_{E}Generation The east orientation zero bias of table accelerometer；y_{1}It is the observed quantity of subsystem 1；v_{1}It is observation noise；Enable x_{1}=δ V_{E},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 y_{1}Represent the velocity error observation of east orientation；z_{1}(t) and z_{2}(t) the δ V of subsystem 1 is provided in real time_{E}With's Estimated value；And z_{3}(t) then for estimating the north orientation drift ε of gyroscope_{N}And all uncertain outer resultant actions disturbed；e_{01}It is real The difference of border observation and estimated value；Fal power function is a kind of nonlinear function, and α and δ are its parameters；β_{11},β_{12},β_{13}It is observation The parameter of device；After abovementioned 1 discretization of subsystem, calculated for numerical value；U_{1}、U_{4}It is the feedback quantity after transformation；B. the design of north orientation extended state observer ESO_NThe 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 formula_{N}It is north orientation speed error；V_{E}, L represent the inertial navigation moment resolving east orientation speed and latitude；R is earth radius；f_{E}, f_{U}It is accelerometer east orientation and day to output；φ_{E},φ_{U}Indicate east orientation and day to misalignment；u_{2},u_{3},u_{5}Represent feedback quantity；ε_{E},ε_{U} The east orientation and day for being gyroscope are to drift；▽_{N}Represent the north orientation zero bias of accelerometer；y_{2}It is the observed quantity of subsystem 2；v_{2}It is to see Survey noise；Enable x_{1}=δ V_{N},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 y_{2}Represent 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 respectively_{N},Estimated value；And θ_{4}(t) then for estimating gyroscope East orientation drift ε_{E}And all uncertain outer resultant actions disturbed；e_{02}It is the difference of actual observed value and estimated value；α and δ is fal power The parameter of secondary function；β_{01},β_{02},β_{03},β_{04}It is the parameter of observer；U_{2}、U_{3}、U_{5}It is the feedback quantity after transformation；C. nonlinear state error Feedback DesignBy abovementioned 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 β_{1},β_{2},β_{3},β_{4},β_{5}Indicate undetermined parameter, other parameters are defined in abovementioned 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 abovementioned 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 autodisturbance 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 beta_{11}、β_{12}And β_{13}；α mainly determines nonlinear shape, the size of δ determining function linearly interval；α Empirically value can be chosen for α=1, δ=0.5 with δ；β_{11}、β_{12}And β_{13}Three parameters are to influence the dynamic characteristic of closedloop 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, β_{11}Main control tracking velocity, β_{12}Represent gain, β_{13}Controlling curve smoothness；β is adjusted first_{11}So that z_{1} It (t) can faster steady tracking y_{1}(t), β is adjusted further according to estimated value_{12}Control z_{2}(t) size；Finally according to overall control Effect further adjusts, and suitably turns β down_{13}Can 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 number_{1},β_{2},β_{3},β_{4},β_{5}；Wherein α and δ have been analyzed in the above content, and which is not described herein again；And β_{1},β_{2},β_{3},β_{4},β_{5}In separately adjust, wherein β_{1}And β_{4}Control subsystem 1, and β_{2}、β_{3}And β_{5}Control is subsystem 2；With subsystem For system 1, parameter beta_{1}And β_{4}Physical significance it is similar to differential coefficient in PID controller and gain coefficient；Work as β_{1}When larger, adjust Section slows, but curve is steady；Otherwise β_{1}When smaller, adjustment speed becomes faster, but the oscillation of curve becomes larger, or even unstable It is fixed；β_{4}It 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 adjusted_{1}, so that estimation is more excellent in speed and smoothness, then adjust β_{4}Make result and expection It is consistent；Step 5: the misalignment exported by observer, construction feedback matrix realize the vehiclemounted 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 strapdown 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 deviation_{1}Transition matrix between systemn_{1}Be n system between error angle be exactly to The misalignment of solution；In abovementioned steps, observer can provide the estimated value of three misalignments are as follows:Wherein φ_{N},φ_{E},φ_{U}It is true misalignment；It is the misalignment of automatic disturbance rejection controller estimation；▽_{E},▽_{N}Generation The zero bias of table accelerometer horizontal direction；f_{U}It is accelerometer day to output；It is the longitude change rate of inertial reference calculation；ω_{ie}It is Earth rotation angular speed；L represents local latitude；ε_{E}It is the drift of gyroscope east orientation；Because three misalignments are lowangles, Feedback matrix can be constructedTo correct strapdown 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.
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CN201610146652.5A CN105806365B (en)  20160315  20160315  Rapid Alignment Technology between a kind of vehiclemounted 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)  20160315  20160315  Rapid Alignment Technology between a kind of vehiclemounted inertial navigation based on Active Disturbance Rejection Control is advanced 
Publications (2)
Publication Number  Publication Date 

CN105806365A CN105806365A (en)  20160727 
CN105806365B true CN105806365B (en)  20190607 
Family
ID=56468396
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CN201610146652.5A CN105806365B (en)  20160315  20160315  Rapid Alignment Technology between a kind of vehiclemounted inertial navigation based on Active Disturbance Rejection Control is advanced 
Country Status (1)
Country  Link 

CN (1)  CN105806365B (en) 
Families Citing this family (7)
Publication number  Priority date  Publication date  Assignee  Title 

FR3057349B1 (en) *  20161011  20190712  Safran Electronics & Defense  IMPROVEMENTS IN INERTIAL PLANT ALIGNMENT METHODS 
CN106843214B (en) *  20170213  20200221  浙江工业大学  Tape guidance AGV tracking control method based on active disturbance rejection control 
CN107036630B (en) *  20170427  20200925  深圳市思拓通信系统有限公司  Automatic identification system and method for installation angle of vehicle driving early warning device 
CN107830873A (en) *  20171101  20180323  北京航空航天大学  A kind of highprecision vehicle positioning and orienting method combined based on uniaxial horizontal rotation inertial navigation with odometer 
CN108931247A (en) *  20180408  20181204  和芯星通科技(北京)有限公司  A kind of air navigation aid and device 
CN108562288A (en) *  20180508  20180921  北京航天时代激光导航技术有限责任公司  A kind of Laser strapdown used group of systemlevel online selfcalibration system and method 
CN109631939A (en) *  20181108  20190416  湖北三江航天红峰控制有限公司  A kind of rapid alignment method based on magnetometer and accelerometer 
Citations (3)
Publication number  Priority date  Publication date  Assignee  Title 

EP0474105A2 (en) *  19900830  19920311  Jacob, Heinrich G., Prof. Dr.Ing.  Inertial navigation system with a compensation filter for its alignment during flight 
CN103743414A (en) *  20140102  20140423  东南大学  Initial alignment method of speedometerassisted strapdown inertial navigation system during running 
CN104897156A (en) *  20150603  20150909  北京理工大学  Spin control method for rotation modulation inertial navigation system 

2016
 20160315 CN CN201610146652.5A patent/CN105806365B/en active IP Right Grant
Patent Citations (3)
Publication number  Priority date  Publication date  Assignee  Title 

EP0474105A2 (en) *  19900830  19920311  Jacob, Heinrich G., Prof. Dr.Ing.  Inertial navigation system with a compensation filter for its alignment during flight 
CN103743414A (en) *  20140102  20140423  东南大学  Initial alignment method of speedometerassisted strapdown inertial navigation system during running 
CN104897156A (en) *  20150603  20150909  北京理工大学  Spin control method for rotation modulation inertial navigation system 
NonPatent Citations (3)
Title 

"一种抗基座扰动的捷联惯导系统初始对准方法";高伟熙 等，;《北京理工大学学报》;20100630;第30卷;190194页 
"一种里程计辅助车载捷联惯导行进间对准方法";陈鸿跃 等，;《导弹与航天运载技术》;20131231(第5期);4450页 
"自抗扰控制技术在捷联惯导动基座初始对准中的应用研究";牛立 等，;《中国惯性技术学报》;20031231;第11卷(第6期);2733页 
Also Published As
Publication number  Publication date 

CN105806365A (en)  20160727 
Similar Documents
Publication  Publication Date  Title 

US10338261B2 (en)  Measurement of magnetic field gradients  
US20170074660A1 (en)  Magnetic Field Gradient Navigation Aid  
Kingston et al.  Realtime attitude and position estimation for small UAVs using lowcost 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 twoway 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 inflight alignment using equivalent error models  
CN102692225B (en)  Attitude heading reference system for lowcost small unmanned aerial vehicle  
CA2530903C (en)  Method and system for improving accuracy of inertial navigation measurements using measured and stored gravity gradients  
GoshenMeskin et al.  Unified approach to inertial navigation system error modeling  
CA1141008A (en)  Autonomous navigation system  
Georgy et al.  Modeling the stochastic drift of a MEMSbased gyroscope in gyro/odometer/GPS integrated navigation  
CN103575299B (en)  Utilize dualaxis 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 closedloop strapdown attitude system for unmanned aerial vehicle (UAV)  
CN103743414B (en)  Initial Alignment Method between the traveling of vehiclemounted SINS assisted by a kind of speedometer  
Barczyk et al.  Integration of a triaxial magnetometer into a helicopter UAV GPSaided 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.  Lowcost 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 