Background technology
Along with the raising day by day of people's living standard, continuous real-time location has been proposed to higher requirement, but due to the complexity of environment for use, single satellite navigation and location system, be difficult to meet vehicle in real time, location requirement continuously.Current, the DR(Dead Reckoning that Vehicle Integrated Location System is used GPS receiver more and formed with gyroscope, vehicle speed pulse or accelerometer, dead reckoning) by microprocessor, carry out the system integration, form GPS/DR integrated positioning scheme, but these schemes all have certain limitation.
Wherein, in the situation that environment is severe, inertial navigation is by the great effect of performance.Inertial navigation is comprised of mutually orthogonal three axis accelerometer and the mutually orthogonal inertial sensors such as three-axis gyroscope conventionally, high accuracy three-dimensional position, three-dimensional velocity and 3 d pose information can be provided in a short time, it is not subject to external interference, independence is good, in fields such as vehicle-mounted, aviation, navigations, is used widely at present.Inertial sensor is arranged on carrier conventionally, in order to allow line motion and the angular motion information of the direct reaction carriers of measurement data of inertial sensor, it axially need be with dextrad, forward direction and the sky of carrier to parallel, therefore mounting means and space have been proposed to higher requirement, need to arrange artificially it axial simultaneously, complex operation, has limited the application of inertial navigation.Due to the impact of the factors such as carrier space and manual operation, during installation, can there is certain inclination angle, when inclination angle be can not ignore, can affect the precision of inertial navigation.When judgement sensor axial, if the angle of any diaxon of inertial sensor and acceleration of gravity is 45 degree, this two axial specific force absolute value is equal in theory, and the specific force of another axle is 0, in such cases axially can there is multiple situation.
According to inventor's understanding, when utilizing the technology of inertial navigation, at least there is following point in prior art:
1. according to the horizontal resultant velocity of satellite navigation system output, judge that whether carrier is static, this precision and stability to satellite navigation system has certain requirement.When a little less than satellite-signal or complete losing lock, satellite navigation system can not accurately judge that whether carrier is static, causes None-identified sky to axle and forward direction axle, and its application exists larger limitation;
2. prior art, when using, requires the mutually orthogonal any diaxon of inertial sensor parallel with forward direction axle with carrier right ward axis respectively.And when actual installation, due to factors such as carrier space and manual operations, right ward axis and the forward direction axle axial and carrier of inertial sensor can exist certain inclination angle, when inclination angle is very important, the applicability of prior art reduces greatly.
3. due to the specific force of accelerometer output, comprise acceleration of motion and the acceleration of gravity of carrier, in judging the process of carrier forward direction axle, consider not remove the impact of acceleration of gravity, directly degree of will speed up meter specific force output valve to the result of time integral as velocity amplitude.When the factors such as carrier inclined or non-level installation cause the component of acceleration of gravity can not ignore, there will be erroneous judgement.
Embodiment
Below in conjunction with better embodiment wherein, the present invention program is described in detail.As shown in Figure 1, inertial sensor comprises mutually orthogonal three-axis gyroscope and mutually orthogonal three axis accelerometer.There is respectively inclination angle in the diaxon of inertial sensor and the right ward axis of carrier and forward direction axle
with
.The original axial direction of inertial sensor is
x _{ s } y _{ s } z _{ s }, what correct identification was afterwards axial and carrier dextrad, forward direction exist inclination angle is axially
x _{ t } y _{ t } z _{ t }, through over-compensation inclination angle with after with carrier dextrad, forward direction with day to consistent be axially
x _{ b } y _{ b } z _{ b }.The data that the method adopts comprise the specific force output of satellite navigation velocity information, the output of gyrostatic angular velocity and accelerometer.When carrier is static, accelerometer is gravitate only, and carrier from static to motion short time in, carrier forward direction has larger acceleration of motion.Based on above feature, can automatically identify the axial of inertial sensor.
The schematic flow sheet of a kind of onboard combined navigation embodiment of the method for the present invention has been shown in Fig. 2.
As shown in Figure 2, the method in the present embodiment comprises step:
S101: under static state obtain the ratio force direction of carrier inertial sensor three axles first axle consistent with gravity acceleration g direction axial; Obtain N straight-line travelling from stationary state to motion state the second axle in constantly axial; According to described the first axle, axially axially to obtain the 3rd axle axial with described the second axle, and obtain the corresponding specific force value of three axles and the magnitude of angular velocities of described inertial sensor.
Wherein the axial determining step of the first axle is: by the specific force of inertial sensor three axles
,
,
respectively with local acceleration of gravity
grelatively:
,
,
.
,
with
be respectively accelerometer
x _{ s } y _{ s } z _{ s }the absolute value of the absolute value of axial ratio power and local acceleration of gravity gap.When
time,
x _{ s }axle with
z _{ t }axle is parallel.If
for just,
x _{ s }with
z _{ t }direction of principal axis is identical; If
for negative,
x _{ s }with
z _{ t }direction of principal axis is contrary.When
time,
y _{ s }axle with
z _{ t }axle is parallel.If
for just,
y _{ s }with
z _{ t }direction of principal axis is identical; If
for negative,
y _{ s }with
z _{ t }direction of principal axis is contrary.When
time,
z _{ s }axle with
z _{ t }axle is parallel.If
for just,
z _{ s }with
z _{ t }direction of principal axis is identical; If
for negative,
z _{ s }with
z _{ t }direction of principal axis is contrary.Wherein,
for critical threshold values.Meanwhile, preserve the specific force of other diaxon, the component that this specific force is acceleration of gravity
, and continuous computation of mean values.In theory, when static, the specific force of upward direction is g, and the specific force of downward direction is-g,
x _{ t } y _{ t } z _{ t }in
z _{ t }smaller with the angle making progress, this direction specific force and g are very approaching, and
xtwith
ytthe specific force of direction is very little, so in our observable accelerometer output,
x _{ s } y _{ s } z _{ s }the specific force of which axle of axle and g relatively approach to judge.Value smaller,
x _{ s } y _{ s } z _{ s }in three axles, there is and only have an axle to meet above condition.
As more excellent embodiment, in linear uniform motion situation, carrier linear acceleration is 0, and owing to being rectilinear motion, carrier angular acceleration has centripetal acceleration while being 0(turning), therefore, in this situation, the output of accelerometer is the component of acceleration of gravity, the same with gyrostatic output with accelerometer in static situation.Thereby, no matter under stationary state or linear uniform motion state, can judge the first axle axially.
The axial determination step of the second axle is: obtain carrier from stationary state to motion state
nthe acceleration of individual straight-line travelling second axle in the moment; Whether the number of times that the acceleration that judges the second axle is greater than default acceleration rate threshold surpasses preset times; If so, carrier is propulsion acceleration; If not, carrier is dextrad acceleration of motion.Because the direction of acceleration has positive and negative minute, when the propulsion acceleration of saying is here negative, propulsion acceleration can be interpreted as reverse acceleration; In like manner, dextrad acceleration of motion can be interpreted as left-hand acceleration of motion.With example, launch explanation below:
At carrier, by static, to line of motion, travelled
nin the individual moment, calculate each acceleration of motion of the second axle constantly:
,
fbe the specific force of the second axle,
for the component of acceleration of gravity on the second direction of principal axis.Because carrier in this process accelerates, in certain several moment, there will be propulsion acceleration ratio larger, and dextrad acceleration of motion is always smaller.Therefore, statistics acceleration of motion
absolute value be greater than
number
m.When
time, the second axle with
y _{ t }direction of principal axis is parallel, if
for just, the second axle with
y _{ t }direction is identical, if
for negative, the second axle with
y _{ t }opposite direction.At that time, the second axle with
x _{ t }direction of principal axis is parallel, if
for just, the second axle with
x _{ t }direction is identical, if
for negative, this axle with
x _{ t }opposite direction.
If judge successfully, obtain the second axle axial.
get empirical value.
According to the axial result of determination of the first axle and the second axle, according to the right-hand rule, can judge that the 3rd axle is axial.Then, according to the value of reading of inertial sensor, obtain the corresponding specific force value of three axles and the magnitude of angular velocity of inertial sensor.
S102: according to the inclination angle of the forward direction axle of the second axle of described inertial sensor, the 3rd axle and described carrier, right ward axis and, the corresponding specific force value of described inertial sensor three axles and magnitude of angular velocity are carried out to matrix conversion, obtain the corresponding specific force value of rear three axles of conversion and magnitude of angular velocities.
According to
x _{ t } y _{ t } z _{ t }axial result of determination, can be by the data of inertial sensor from original
x _{ s } y _{ s } z _{ s }axle is transformed into carrier dextrad, forward direction with day to there being inclination angle
x _{ t } y _{ t } z _{ t }axle.
Wherein,
,
,
for accelerometer
x _{ s } y _{ s } z _{ s }the original specific force output of axle,
f _{ tx },
f _{ ty }with
f _{ tz }for being transformed into
x _{ t } y _{ t } z _{ t }the specific force value of axle,
,
with
for gyroscope
x _{ s } y _{ s } z _{ s }the rudimentary horn speed output of axle,
,
with
for being transformed into
x _{ t } y _{ t } z _{ t }the angular velocity of axle, for from
x _{ s } y _{ s } z _{ s }axle arrives
x _{ t } y _{ t } z _{ t }3 row 3 row transition matrixes of axle, any a line and any to show two elements be 0, another element is 1 or-1.
The derivation of matrix, can be with
x _{ s }with
z _{ t }direction is identical,
z _{ s }with
y _{ t }opposite direction,
y _{ s }with
x _{ t }opposite direction is example, and the conversion of specific force i.e. (angular velocity is similar):, be expressed in matrix as,, any a line and any to show two elements be 0, another element is 1 or-1.Consider the diaxon of inertial sensor and the right ward axis of carrier and forward direction axle inclination angle and, general
x _{ t } y _{ t } z _{ t }inertial sensor data under axle is transformed into carrier dextrad, forward direction and sky to consistent
x _{ b } y _{ b } z _{ b }axle.
Wherein,
,
with
for being transformed into
x _{ b } y _{ b } z _{ b }the specific force value of axle,
,
with
for being transformed into
x _{ b } y _{ b } z _{ b }the angular velocity of axle, for from
x _{ t } y _{ t } z _{ t }axle arrives
x _{ b } y _{ b } z _{ b }3 row 3 row transition matrixes of axle.
Wherein, the calculating at inclination angle and the derivation of transition matrix:
x _{ t } y _{ t } z _{ t }with
x _{ b } y _{ b } z _{ b }do not overlap, have rotation relationship.
x _{ b } y _{ b } z _{ b }around
x _{ b }axle rotation obtains
x _{ 1 } y _{ 1 } z _{ 1 },
r _{ b }=[
r _{ bx } r _{ by } r _{ bz }] be vector
r?
x _{ b } y _{ b } z _{ b }projection under system,
r _{ 1 }=[
r _{ 1x } r _{ 1y } r _{ 1z }] be
r?
x _{ 1 } y _{ 1 } z _{ 1 }projection.Rotation relationship is as Fig. 3, and angle rotates clockwise as just.Due to around
x _{ b }axle rotation, so
x _{ b }with
x _{ 1 }direction or the same.
? , be expressed in matrix as
In like manner,
x _{ 1 } y _{ 1 } z _{ 1 }around
y _{ 1 }axle rotation obtains
x _{ t } y _{ t } z _{ t },
r _{ t }=[
r _{ tx } r _{ ty } r _{ tz }] be
r?
x _{ t } y _{ t } z _{ t }projection.Rotation relationship is as Fig. 4, and angle rotates clockwise as just, due to around
y _{ 1 }axle rotation, so
y _{ 1 }with
y _{ t }direction or the same.
?
, be expressed in matrix as
,
Twice rotation can be expressed as,
The calculating at inclination angle is mainly that inertial sensor is arranged on carrier, and carrier horizontal positioned can be calculated inclination angle by the specific force of static moment accelerometer
with
.According to axial judged result, can degree of will speed up meter the specific force of output be transformed into
x _{ t } y _{ t } z _{ t, }.?
Because the transformation matrix between three-dimensional cartesian coordinate system is unit orthogonal matrix,
，
，
Wherein, subscript-1 representing matrix is inverted, subscript
trepresenting matrix transposition.
Carrier horizontal positioned,
x _{ b }with
y _{ b }direction specific force is 0,
z _{ b }direction specific force is g,
So, inclination angle and be calculated as:
Wherein,
,
it is coordinate system
x _{ s } y _{ s } z _{ s }arrive
x _{ t } y _{ t } z _{ t }transition matrix,
,
,
for the specific force of inertial sensor three axles,
gfor acceleration of gravity.
S103: the position that after the initial position by carrier, conversion, the corresponding specific force values of three axles and magnitude of angular velocity calculate described carrier.
,
with
for carrier dextrad, forward direction and day to specific force, remove after the impact of acceleration of gravity, by integration can calculate carrier dextrad, forward direction and day to speed increment, quadratic integral can calculate carrier dextrad, forward direction and day to displacement increment.
,
with
for carrier is around dextrad, forward direction with day to the angular velocity of rotation, by integration, can calculate the angle step of carrier.And then according to initial position, initial velocity and initial angle, calculate current position, speed and the angle of carrier.Under stationary condition, initial velocity is defaulted as 0, and initial angle calculates by accelerometer specific force.And under linear uniform motion state, calculate current location, speed and the angle of carrier, need to obtain inertial navigation initial position, initial velocity and initial angle constantly.
By this technology, can accurately judge that whether carrier is static, and accurately identification sky, to axle and forward direction axle, has larger actual application value.Meanwhile, the characteristic by inertial sensor can greatly make up the bad defect of satellite navigation signals, realizes continuous seamless navigation.
As shown in Figure 5, the system module in the present embodiment comprises:
Inertial sensor information acquisition module, data processing module and display module, modules connects successively.
Inertial sensor information acquisition module is axial for obtaining the ratio force direction of carrier inertial sensor three axles under static state first axle consistent with gravity acceleration g direction; Obtain from stationary state to motion state
ndescribed in the individual straight-line travelling moment, the second axle is axial; According to described the first axle, axially axially to obtain the 3rd axle axial with described the second axle, and obtain the corresponding specific force value of three axles and the magnitude of angular velocities of described inertial sensor; Described data processing module is used for according to the forward direction axle of the second axle of described inertial sensor, the 3rd axle and described carrier, the inclination angle of right ward axis
with
, the corresponding specific force value of described inertial sensor three axles and magnitude of angular velocity are carried out to matrix conversion, obtain the corresponding specific force value of rear three axles of conversion and magnitude of angular velocities; The position of calculating described carrier by the corresponding specific force value of three axles after the initial position of carrier, described conversion and magnitude of angular velocity; Described the first axle, the second axle are vertical between two with the 3rd axle.
Display module, speed information and the position of for receiving data processing mould, transmitting, and export corresponding information for car-mounted terminal (carrier).
The application of system embodiment of the present invention is based on embodiment of the method, and the technical characterictic in also can using method embodiment solves corresponding problem, and the beneficial effect bringing is consistent with embodiment of the method, is not repeated in this description herein.
The above embodiment has only expressed several embodiment of the present invention, and it describes comparatively concrete and detailed, but can not therefore be interpreted as the restriction to the scope of the claims of the present invention.It should be pointed out that for the person of ordinary skill of the art, without departing from the inventive concept of the premise, can also make some distortion and improvement, these all belong to protection scope of the present invention.Therefore, the protection domain of patent of the present invention should be as the criterion with claims.