JP6248559B2 - Vehicle trajectory calculation device - Google Patents

Description

  The present invention relates to a vehicle travel locus calculation apparatus.

  2. Description of the Related Art In addition to a navigation system, a vehicular travel trajectory calculation device that detects a current position and a travel trajectory of a vehicle has been widely used in a driving support system that realizes automatic travel of a vehicle. Systems that measure and control only the behavior of the vehicle (VSC (Vehicle Stability Control), ABS (Anti-lock Brake System), etc.) have been used in the past, but nowadays a system that controls vehicles according to the surrounding environment of the vehicle. Has already been commercialized. There are various types of vehicle control systems according to the surrounding environment, and among them, a vehicle control system based on map information (hereinafter referred to as map-use ADAS) has attracted attention.

  To realize the map-based ADAS (Map-based Advanced Driver Assistance System), technology to identify the location of the vehicle on the map is indispensable anytime and anywhere. Such a technique is called map matching, and mainly uses the degree of coincidence between the road map shape and the travel locus shape as an index. For this reason, in order to realize the map-based ADAS, a technique capable of measuring an accurate travel locus shape anytime and anywhere is the key.

  By the way, the traveling locus shape is obtained from an inertial sensor such as a gyroscope that detects a rotational angular velocity of a vehicle, a vehicle speed sensor that outputs a vehicle speed pulse signal corresponding to a moving distance, and a satellite navigation output using a GPS receiver. Satellite navigation alone is sufficient for places with good satellite radio wave environment (under open sky environment), but in places where trouble is occurring (in tunnels, buildings, underpasses, etc.), inertial sensors are also used to obtain a running trajectory shape. However, an inexpensive inertial sensor has a large individual difference in characteristics, and only a distorted traveling locus shape can be obtained as it is. For this reason, an automatic calibration function of an inertial sensor combined with satellite navigation is indispensable to obtain an accurate travel locus shape anytime and anywhere.

  Various auto-calibration functions have been developed by various companies. One of them is a characteristic estimation method using a sequential processing type filter (Kalman filter). As such a method, the position and direction of the vehicle at the current time are specified based on the output value of the inertial sensor and the satellite navigation output, and each error with respect to the correction value of the vehicle position, direction and inertial sensor is used as a state quantity. Each error is estimated using a Kalman filter, and an error covariance matrix including the prediction error of each error as a component is calculated, and automatic calibration of the inertial sensor is realized using each error and error covariance matrix. Yes (see, for example, Patent Document 1).

  At this time, considering the noise included in the satellite navigation results, the automatic calibration function needs to gradually learn over a long time. Therefore, the vehicle position, direction, inertia sensor correction amount, and error covariance matrix of each of these errors are backed up by a nonvolatile storage medium, and the vehicle travel locus calculation device is accompanied when the vehicle engine is stopped. When the vehicle is restarted after the vehicle is stopped, each backed up value is read back to correct the output value of the inertial sensor and the position and direction of the vehicle.

  However, if the vehicle travels while the vehicle travel locus calculation device is stopped, such as when the vehicle is transported by a car ferry or a tow truck, the actual vehicle position and orientation, and the backed up vehicle location and orientation A shift occurs in the following (back-up position / orientation). In such a case, when the vehicle travel locus calculation device is restarted, the position and direction one hour before are regarded as the backup position / direction, even though the actual position and direction of the vehicle are different. Thus, the above correction is performed. As a result, when compared with the satellite navigation output at the current time, it seems that the position / orientation changed suddenly during one time, and the correction process tries to adjust the correction amount of the inertial sensor. There is a problem that the correction amount of the inertial sensor is erroneously estimated.

  As a conventional technique for a case where the vehicle rotates on the turntable while the vehicle travel locus calculation device is stopped, the display of the first current position data measured by satellite navigation is canceled when the device is restarted. In this way, there is one that prevents the current position from being displayed in a distorted manner (for example, see Patent Document 2).

JP-A-8-68654 JP 2010-190721 A

  The device described in Patent Document 2 cancels the display of the first current position data measured by satellite navigation when the device is restarted. Regarding the erroneous estimation of the correction amount of the inertial sensor, Not mentioned.

  In addition, as described above, since the correction of the inertial sensor needs to proceed with learning over a long period, once the inertial sensor correction amount is erroneously estimated, the learning progresses again and the travel locus shape can be calculated with high accuracy. There is a problem that it takes time to become.

  The present invention has been made in view of the above problems, and an object of the present invention is to allow a travel locus to be calculated more quickly and accurately at the time of restart after a vehicle movement occurs during operation stop.

In order to achieve the above object, the invention described in claim 1 is based on output values of inertial sensors (1, 2) mounted on a vehicle and output values of a GPS receiver (3). In addition to specifying the position and direction, each error is estimated using a Kalman filter with each error with respect to the vehicle position, direction and correction amount of the inertial sensor as a state quantity, and a prediction error of each error is included as a component Computation processing means (steps 100 to 400) for calculating an error covariance matrix and correcting the position and orientation of the vehicle and the output value of the inertial sensor using the errors and the error covariance matrix, A vehicle travel trajectory calculation device for calculating a travel trajectory of the vehicle based on the position and orientation of the vehicle, and when the travel trajectory calculation device for the vehicle stops operating, The position of the vehicle corrected by, together with orientation, the correction amount of the inertia sensor, and a backup means for storing the error covariance matrix to the storage medium, when a the vehicle traveling path calculation device is activated, the GPS receiver The position of the vehicle is specified based on the output value of the machine, and the operation of the vehicle travel locus calculation device is stopped based on the position of the vehicle and the position of the vehicle before activation stored in the storage medium. Movement determination means for determining whether or not vehicle movement has occurred, and when the movement determination means determines that vehicle movement has occurred during the operation stop, the correction processing means outputs the output of the GPS receiver; The position and direction specified by the value are regarded as the position and direction of the vehicle at the current time, and the prediction error of each error of the position and direction of the vehicle in the error covariance matrix Only about the correction value of the inertial sensor and the prediction error of the error of the correction value of the inertial sensor, the vehicle position using each value before the activation stored in the storage medium, The azimuth and the output value of the inertial sensor are corrected.

  According to such a configuration, when it is determined that the vehicle movement has occurred while the vehicle travel locus calculation device is stopped, the position and direction specified by the output value of the GPS receiver are set to the current time of the vehicle. It is regarded as position and direction, and only the prediction error of each error of vehicle position and direction in the error covariance matrix is reset to a predetermined value, and the prediction error of the inertia sensor correction amount and the inertia sensor correction amount error Since the vehicle position, heading, and output value of the inertial sensor are corrected using the values before startup stored in the storage medium, erroneous correction of the output value of the inertial sensor is prevented, and the operation is stopped. After the vehicle movement occurs, the travel locus can be calculated more quickly and accurately when the vehicle is restarted.

  In addition, the code | symbol in the bracket | parenthesis of each means described in this column and the claim shows the correspondence with the specific means as described in embodiment mentioned later.

It is a schematic structure figure showing one embodiment of the present invention. It is a flowchart which shows the calculation process of the main routine of a navigation calculation. It is a flowchart which shows the calculation process of azimuth | direction change amount / movement distance. It is a flowchart which shows the calculation process of a relative locus. It is a flowchart which shows the calculation process of an absolute azimuth | direction and an absolute position. It is a flowchart which shows a composite process with GPS. It is a block diagram which shows the model of a Kalman filter.

  DESCRIPTION OF THE PREFERRED EMBODIMENTS Embodiments shown in the drawings will be described below. In this embodiment, a Kalman filter is used in order to combine navigation calculation and GPS. An outline of the Kalman filter will be described. This Kalman filter is divided into a signal generation process and an observation process as shown in FIG. In the figure, when there is a linear system (φ) and a part of X (t) related to the observation matrix H can be observed with respect to the state X (t) of the system, the filter of X (t) Gives the best estimate. Here, ω is noise generated in the signal generation process, and v is noise generated in the observation process. The input of this filter is Y (t) and the output is the optimal estimate of X (t).

  The optimum estimated value of the state X using information up to the time t, that is, the state quantity X (t | t) is obtained by Equation 1.

(Equation 1) X (t | t) = X (t | t−1) + K (t) {Y (t) −HX (t | t−1)}
Here, X (t | t−1) is a prior estimated value, and K (t) is a Kalman gain, which are expressed by Equations 2 and 3, respectively.

(Equation 2) X (t | t−1) = φX (t−1 | t−1)

(Equation 3) K (t) = P (t | t−1) H ^T (HP (t | t−1) H ^T + V) ⁻¹
Here, P is an error covariance matrix of the state quantity X, P (t | t−1) is a predicted value of the error covariance matrix, and P (t−1 | t−1) is an error covariance matrix. Are represented by Equation 4 and Equation 5, respectively.

(Equation 4) P (t | t−1) = φP (t−1 | t−1) φ ^T + W

(Equation 5) P (t−1 | t−1) = (I−K (t−1) H) P (t−1 | t−2)
V is a variance of noise v generated in the observation process, and W is a variance of noise ω generated in the signal process. A (i | j) represents an estimated value of A at time i based on information up to time j. The subscript ^T means a transposed matrix, and ^-1 means an inverse matrix. I is a unit matrix.

  Furthermore, V and W are white Gaussian noises with an average of 0 and are uncorrelated with each other. In the Kalman filter as described above, an appropriate error is given to the initial values of the state quantity X and the error covariance matrix P, and the above calculation is repeated each time a new observation is performed, whereby the state quantity X is accurately obtained. It is possible to estimate. This embodiment applies such a Kalman filter to dead reckoning navigation.

First, the definition of the signal generation process will be described. Since the Kalman filter in dead reckoning is intended to correct errors in navigation calculation, the state quantity X defines the following five error values. It is the process matrix φ that gives a temporal change in the error value.
(1) Offset error (εG)

(Equation 6) εG _t = εG _t-1 + ω ₀
There is no definite change, and noise is added to the previous error.
(2) Absolute heading error (εA)

(Equation 7) εA _t = T × εG _t-1 + εA _t-1 + ω ₁
An azimuth error and noise obtained by multiplying an offset error by an elapsed time from the previous time are added to the previous error.
(3) Distance coefficient error (εK)

(Equation 8) εK _t = εK _t-1 + ω ₂
There is no definite change, and noise is added to the previous error.
(4) Absolute position north direction error (εY)

(Equation 9) εY _t = sin (A _T + εA _t-1 + εG _t-1 × T / 2) × L × (1 + εK _t-1 )
-Sin (A _T ) × L + εY _t-1
An error caused by an azimuth error / distance error is added to the previous error.
(5) Absolute position eastward error (εX)

(Equation 10) εX _t = cos (A _T + εA _t-1 + εG _t-1 × T / 2) × L × (1 + εK _t-1 )
-Cos (A _T ) × L + εX _t-1
An error caused by an azimuth error / distance error is added to the previous error. In the above definition, _AT is a true absolute bearing, L is a moving distance from the previous time, and T is an elapsed time from the previous time.

  When each of the above equations is partially differentiated by the state quantity and linearized, the signal generation process is defined as follows.

上記Ａは、絶対方位Ａ_T ＋εＡ_t-1 ＋εＧ_t-1 ×Ｔ／２を意味する。この値は真の絶対方位Ａ_Tにセンサ誤差が加わったものであり、後述するように、方位変化量から求められる絶対方位Ａとする。また、ω₀は、オフセット雑音（温度ドリフト等によるオフセットの変動分）、ω₁は絶対方位雑音（ジャイロのゲイン的な誤差）、ω₂ は距離係数雑音（経年変化）を意味する。 (Equation 11)

The above A means the absolute orientation A _T + εA _t-1 + εG _t-1 × T / 2. This value is obtained by adding a sensor error to the true absolute azimuth _AT , and is assumed to be an absolute azimuth A obtained from the azimuth change amount, as will be described later. Also, ω ₀ means offset noise (offset fluctuation due to temperature drift or the like), ω ₁ means absolute azimuth noise (gyro gain error), and ω ₂ means distance coefficient noise (aging).

  Next, the definition of the observation process will be described. The observed value is obtained from the difference between the navigation calculation output and the GPS output. Since each output includes an error, the sum of the navigation calculation error and the GPS error is obtained in the observed value. The observed value Y and the state quantity X are related to each other and defined as shown in Equation 12.

但し、観測過程で発生する雑音ｖはＧＰＳの雑音であり、数１３のように定義される。 (Equation 12)

However, the noise v generated in the observation process is GPS noise and is defined as shown in Equation 13.

以上の定義を基に、カルマンフィルタを用いた推測航法について説明する。図１に、本発明の一実施形態に係る車両用走行軌跡算出装置の全体構成を示す。本車両用走行軌跡算出装置は、車両に搭載されるナビゲーション装置における現在位置検出機能を実現する現在位置検出部７として構成されている。 (Equation 13)

Based on the above definition, dead reckoning navigation using the Kalman filter will be described. FIG. 1 shows the overall configuration of a vehicle travel locus calculation apparatus according to an embodiment of the present invention. The vehicle travel locus calculation device is configured as a current position detection unit 7 that realizes a current position detection function in a navigation device mounted on a vehicle.

  The navigation device is configured to output a vehicle at the current time based on an output value of a vehicle speed sensor 1 and a gyroscope (hereinafter simply referred to as “gyro”) 2 and an output value of a GPS receiver (hereinafter simply referred to as “GPS”) 3. The position and direction of the vehicle are specified, and the position and direction of the vehicle and the output value of the inertial sensor are corrected, and the travel locus of the vehicle based on the position and direction of the vehicle is calculated. Further, the travel locus shape is calculated based on the travel locus.

  The navigation device is mounted on a vehicle and includes a vehicle speed sensor 1, a gyro 2, a GPS 3, a current position detection unit 7, and a navigation execution unit 8. The current position detection unit 7 and the navigation execution unit 8 are performed by a calculation process of a microcomputer. That is, this navigation apparatus includes a microcomputer, a RAM, a ROM, and a nonvolatile storage medium (none of which are shown) for realizing the functions of the current position detection unit 7 and the navigation execution unit 8.

  Here, the vehicle speed sensor 1 is a sensor for detecting the moving distance of the vehicle, and outputs a pulse signal at intervals corresponding to the moving distance.

  The gyro 2 is a sensor for detecting the yaw angular velocity (direction change amount) of the vehicle, and outputs a detection signal corresponding to the angular velocity of the rotational motion applied to the vehicle.

  The GPS 3 receives transmission radio waves from GPS satellites via a GPS antenna, and detects the position (latitude, longitude), azimuth (traveling direction), speed, and the like of the vehicle.

  Further, the current position detection unit 7 detects data for performing navigation calculation, such as an absolute position, an absolute direction, a vehicle speed, and a relative locus, based on outputs from the vehicle speed sensor 1, the gyro 2, and the GPS 3.

  Further, the navigation execution unit 8 identifies the current position based on the detection result of the current position detection unit 7, displays the position of the host vehicle on a map on a display screen (not shown), and sets the set purpose. Provides route guidance to the ground.

  As shown in FIG. 1, on the basis of signals from the vehicle speed sensor 1 and the gyro 2, calculation is performed in the relative trajectory calculation unit 4 and the absolute position calculation unit 5, and the vehicle speed, Relative trajectory, absolute position, and absolute direction are output. Further, the GPS 3 can output the position, direction and vehicle speed. The Kalman filter 6 corrects the distance coefficient of the vehicle speed sensor 1, the offset correction of the gyro 2, and the absolute position based on the vehicle speed, absolute position, and absolute direction information obtained by the navigation calculation and the vehicle speed, position, and direction information from the GPS 3. Correction and absolute azimuth correction are performed.

  When the Kalman filter is applied to such a navigation device, the prior estimation X (t | t−1) shown in Equation 2 by the distance coefficient correction of the vehicle speed sensor 1, the offset correction of the gyro 2, the absolute position correction, and the absolute direction correction. Becomes 0. Therefore, Equation 1 becomes as shown in Equation 14.

(Expression 14) X (t | t) = K (t) Y (t)
Therefore, the state quantity X based on the five error values defined in the signal generation process is obtained from the Kalman gain K (t) and the observed value Y (t) obtained from Equations 3 to 5.

  Here, the error covariance matrix P in Equation 3 is defined by Equation 15.

この誤差共分散行列Ｐは、状態量ｘの確からしさを表した行列である。この誤差共分散行列Ｐにおけるσ_GG ²はオフセット誤差の大きさの見積もりを表し、σ_AA ² は絶対方位誤差の大きさの見積もりを表し、σ_KK ²は距離係数誤差の大きさの見積もりを表し、σ_YY ² は絶対方位北方誤差の大きさの見積もりを表し、σ_XX ²は絶対方位東方誤差の大きさの見積もりを表す。それら以外のσ_ij ² はｉ行とｊ列の相互相関値を表す。例えばσ_AG ²はオフセット誤差と絶対方位誤差の相互相関値を表す。 (Equation 15)

This error covariance matrix P is a matrix representing the probability of the state quantity x. In this error covariance matrix P, σ _GG ² represents an estimate of the offset error magnitude, σ _AA ² represents an estimate of the absolute bearing error magnitude, and σ _KK ² represents an estimate of the distance coefficient error magnitude. , Σ _YY ² represents an estimate of the magnitude of the absolute azimuth north error, and σ _XX ² represents an estimate of the magnitude of the absolute azimuth east error. Σ _ij ² other than these represents a cross-correlation value between i rows and j columns. For example, σ _AG ² represents a cross-correlation value between the offset error and the absolute azimuth error.

The diagonal components σ _GG ² , σ _AA ² , σ _KK ² , σ _YY ² , and σ _XX ² in the error covariance matrix P are also called prediction errors. Also, it means that each variable σ _GG ² , σ _AA ² , σ _KK ² , σ _YY ² , σ _XX ² in the error covariance matrix P can be accurately estimated when it converges to be small.

The value of the error covariance matrix P is updated by the calculation of Equation 4. In the initial values, the values of σ _GG ² , σ _AA ² , σ _KK ² , σ _YY ² , and σ _XX ² are set to values that maximize the error, and the cross-correlation values are all 0. Set to. Further, H in Equation 3 uses the matrix shown in Equation 12, and V uses the one shown in Equation 13. Further, W in Equation 4 uses the dispersion of ω shown in Equation 11.

As observed value Y in the observation process, as shown in Equation 12, εA _DRt −εA _GPSt , εK _DRt −K _GPSt , εY _DRt −εY _GPSt , and εX _DRt −εX _GPSt are used. Here, the subscript _GPSt means a value obtained by navigation calculation based on signals from the vehicle speed sensor 1 and the gyro 2 at time t, and GPSt means a value output from the _{GPS 3} at time t.

εA _DRt −εA _GPSt is the difference between the absolute azimuth obtained by the navigation calculation and the azimuth output from the GPS 3, that is, the absolute azimuth obtained by the navigation calculation includes the true absolute azimuth and its error εA _DRt. Further, since the true absolute azimuth and its error εA _GPSt are included in the azimuth output from the _GPS 3, _{εA DRt} −εA _GPSt can be obtained by taking the difference between them.

Similarly, εK _DRt −εK _GPSt is a distance coefficient error obtained from the difference between the speed obtained by the navigation calculation and the speed output from the GPS 3. Specifically, (speed by the navigation calculation−speed by the GPS) / ( The speed is calculated by navigation calculation). ΕY _DRt −εY _GPSt is the difference between the absolute position (latitude) obtained by the navigation calculation and the position (latitude) output from the GPS ₃ , and εX _DRt −εX _GPSt is the absolute position obtained by the navigation calculation. It is the difference in error between (longitude) and the position (longitude) output from the GPS 3.

Further, the noise v generated in the observation process shown in Equation 13 is GPS3 noise, and is obtained as follows. The positioning accuracy is obtained by UERE × HDOP based on the relationship between the pseudorange measurement error (UERE) in GPS3 and HDOP (Horizontal Dilution of Precision), and v _2t and v _3t are obtained by squaring the positioning accuracy. . Further, the speed accuracy is obtained from the relationship between the Doppler frequency measurement error and HDOP, and the Doppler frequency measurement error × HDOP is obtained. The distance accuracy measurement error is obtained from this speed accuracy / vehicle speed, and the squared value is used to calculate v. _1t is required. Further, the azimuth accuracy is obtained by tan ⁻¹ (speed accuracy / Vc) from the vehicle speed Vc and the speed accuracy, and v _0t is obtained by squaring this azimuth accuracy.

Accordingly, εA _DRt −εA _GPSt , εK _DRt −εK _GPSt , εY _DRt −εY _GPSt , εX _DRt −εX _GPSt and the noise V in the observation process are input, and _Equations 3 to 5 and _Equation 1 are executed. The state quantity X based on the five error values defined in the signal generation process is obtained, and the distance coefficient correction of the vehicle speed sensor 1, the offset correction of the gyro 2, the absolute position correction, and the absolute azimuth correction are performed by these.

  Since the above-mentioned relative locus calculation, absolute position calculation, and Kalman filter are performed by calculation processing by a microcomputer (not shown) of the navigation apparatus, this will be described below. FIG. 2 shows the calculation process of the main routine of the navigation calculation.

  First, an outline of arithmetic processing of the main routine will be described. In step 20, the microcomputer of the navigation device performs initialization processing, and in step 30, determines whether or not the accessory power supply of the vehicle has changed from the off state to the on state. In steps 100 to 300, the vehicle position (latitude, longitude) one hour before and the vehicle position at the current time (by latitude) are calculated by navigation calculation based on the output value of the inertial sensor constituted by the vehicle speed sensor 1 and the gyro 2 ( Latitude, longitude) and azimuth are calculated. Then, in step 400, based on the vehicle position and direction at the current time and the output value of the GPS receiver 3, each error of the vehicle position, direction and inertial sensor correction amount (offset correction amount, distance coefficient correction amount) is calculated. Estimate and correct each error in the vehicle position, heading, and inertial sensor correction amount at the current time using a Kalman filter with the state value and the variance value (prediction error) of each error as a diagonal component of the error covariance matrix I do.

  In addition, the Kalman filter always requires “a state before one time”. If the microcomputer of the present navigation device determines in step 30 that the accessory power supply of the vehicle has changed from the on state to the off state so that the Kalman filter can be operated seamlessly even after the navigation device is restarted, step 40 Then, the latitude, longitude, azimuth, offset correction amount, distance coefficient correction amount, and error covariance matrix in the state before the operation of the navigation device is stopped are backed up to a nonvolatile storage medium. Then, when the navigation device restarts after the operation is stopped, the microcomputer of the navigation device reads back each value backed up in the nonvolatile storage medium in step 20, and each value read back Is regarded as a state one hour before, and the navigation calculation (relative trajectory calculation 4, absolute position calculation 5) and the Kalman filter 6 are restarted.

  Next, the details of the arithmetic processing of the main routine will be described with reference to FIG. The navigation device is always supplied with power from the vehicle battery. Further, when it is determined that the accessory power supply (ACC power supply) is turned off during operation in accordance with a user operation, the navigation device transitions to the low power consumption mode after a certain period of time and enters a stopped state. In addition, after the navigation apparatus is in a stopped state, when the accessory power supply is turned on in response to a user operation, the navigation apparatus transitions to the normal mode and enters an operating state. When the accessory power supply is turned on in response to a user operation, the microcomputer of the navigation device performs the process shown in FIG.

  First, in step 20, an initialization process according to a program stored in the ROM is performed, and each value backed up by a nonvolatile storage medium is read back. Specifically, a predetermined initialization process is performed and the latitude, longitude, azimuth, offset correction amount, distance coefficient correction amount, and error covariance matrix backed up by a nonvolatile storage medium are read back and stored in the RAM. Let

  In the next step 30, it is determined whether or not the accessory power supply of the vehicle has changed from the on state to the off state. Here, the processing of steps 100 to 400 is repeatedly performed until the accessory power supply of the vehicle changes from the on state to the off state.

First, in step 100, the azimuth change amount / movement distance is calculated. Details of this processing are shown in FIG. First, at step 101, the azimuth change amount is calculated by multiplying the output angular velocity of the gyro 2 by the start cycle T _M of the main routine. In the next step 102, the offset change amount is subtracted from the offset change amount (this correction amount will be described later) multiplied by the main routine start cycle T _M to correct the offset of the azimuth change amount. In the next step 103, the moving distance is calculated by multiplying the number of vehicle speed pulses from the vehicle speed sensor 1 by a distance coefficient (this distance coefficient will also be described later).

  Following this step 100, a relative trajectory calculation process of step 200 is performed. Details of this processing are shown in FIG. First, in step 201, the relative azimuth is updated based on the azimuth change amount (obtained in step 102). The relative position coordinates are updated in step 202 based on the updated relative orientation and the movement distance obtained in step 103. This update is performed by adding the X and Y components of the relative direction with respect to the moving distance to the relative position coordinates so far. The relative position coordinates are used to obtain a relative trajectory, and so-called map matching is performed based on the relationship between the relative trajectory and the road shape.

  After step 200, the absolute azimuth / absolute position calculation process of step 300 is performed. Details of this processing are shown in FIG. First, in step 301, the absolute azimuth is updated based on the azimuth change amount (obtained in step 102).

  In the next step 302, the absolute position coordinates are updated in step 202 based on the updated absolute azimuth and the movement distance obtained in step 103. The absolute azimuth A and the absolute position updated in the process of step 200 are used in a composite process with GPS described later.

  FIG. 6 shows details of the step 400 for performing the combination processing with the GPS. First, in step 402, it is determined whether or not there is positioning data from GPS3. If there is positioning data from the GPS 3, it is determined in step 403 whether or not a ferry jump has occurred. The ferry jump means that the vehicle has moved while the navigation device is stopped. Specifically, the position of the vehicle is specified based on the positioning data from the GPS 3, and the position (latitude, longitude) of the vehicle specified based on the positioning data and the navigation device read back from the nonvolatile storage medium are operated. Whether the position of the vehicle specified based on the positioning data and the position of the vehicle before the operation of the navigation device is more than a predetermined distance Based on this, it is determined whether or not a ferry jump has occurred.

  Here, it is assumed that the ferry jump is not performed, and the case where the ferry jump is performed will be described later.

If the ferry jump is not performed, the determination in step 403 is NO, and the process proceeds to the Kalman filter calculation process in step 405 and subsequent steps. First, in step 405, the observed value Y is calculated. This is the speed of the vehicle based on the speed, position, direction data output from the GPS 3, and the absolute direction, absolute position obtained in the processing of step 300 in the navigation calculation, and the vehicle speed pulse from the vehicle speed sensor 1 by the speed calculation process (not shown). ΕA _DRt −εA _GPSt , εK _DRt −εK _GPSt , εY _DRt −εY _GPSt , εX _DRt −εX _{GPSt shown in} Equation ₁₂ and noise v generated in the observation process shown in _Equation 13 Calculate based on GPS3 positioning data.

  In step 406, the process matrix φ is calculated. This is the process shown in Equation 11 based on the movement distance L from the previous process matrix calculation time, the elapsed time T (these are separately obtained by a measurement process not shown) and the absolute direction A obtained in step 301. Find the matrix φ. Based on the observation value Y and the process matrix φ calculated in this way, the above-described calculations of Equations 3 to 5 are performed to obtain the state quantity X shown in Equation 14.

  In step 407, the prediction calculation of the error covariance matrix P is performed according to Equation 3. In step 408, the Kalman gain K is calculated by equation (4). In step 409, the error covariance matrix P is calculated by Equation 5.

  Thereafter, based on the Kalman gain K and the observed value Y, the state quantity X is obtained by calculation of Equation 14 in step 410. As shown on the left side of Equation 11, the state quantity X includes an offset error (εG), an absolute azimuth error (εA), a distance coefficient error (εK), an absolute position north direction error (εY), and an absolute position east direction error ( εX).

  Due to these errors, in step 411, the navigation calculation error is corrected by the calculation shown in the figure, that is, the gyro 2 offset correction, the vehicle speed sensor 1 distance coefficient correction, the absolute azimuth correction, and the absolute position correction are performed. The offset correction amount used in step 102 is corrected by the offset correction of the gyro 2, the distance coefficient used in step 103 is corrected by the distance coefficient correction of the vehicle speed sensor 1, and in step 301 by the absolute azimuth correction. The absolute azimuth A to be used is corrected, and the absolute position used in step 302 is corrected by the absolute position correction.

  The above processing is repeated every time there is positioning data from the GPS 3, and the error correction is performed to obtain a more accurate travel locus. If there is no positioning data, the determination in step 402 is NO and the process proceeds to steps 412 and 413 to calculate the process matrix φ and predictive calculation of the error covariance matrix P. As a result, the prediction calculation of the error covariance matrix corresponding to the error in the case where the positioning of the GPS 3 cannot be performed is performed, and the Kalman filter process performed when the GPS 3 can be positioned thereafter can be accurately performed.

  In addition, the case of a ferry jump is as follows. The movement of the vehicle occurs while the navigation device is stopped, and the position of the vehicle specified based on the positioning data and the position before the navigation device read back from the nonvolatile storage medium stops operating are a predetermined distance or more. If it is away, the determination at step 403 is YES, and the past state is corrected at step 404.

  Here, the position (latitude, longitude) and azimuth of the vehicle one hour before used in the calculation of the absolute azimuth and absolute position in step 300 are overwritten with the positioning value by satellite navigation using GPS3. As described above, when the position (latitude, longitude) and direction of the vehicle one hour before are corrected, the calculation is performed when the calculation of the absolute direction and absolute position in step 300 is performed at the next time. A value is used. Specifically, the corrected absolute azimuth is used as the absolute azimuth indicated on the right side of the equation in step 301 in FIG. 5, and the corrected vehicle position (latitude and longitude) is used in step 302 in FIG. Are used as absolute positions (abs.x and abs.y) shown on the right side of the equation.

Of the error covariance matrix to be input in Step 407 and Step 413 described later, the value of the component σ _AA ² corresponding to the azimuth prediction error, the component σ _YY ² corresponding to the latitude prediction error, and the longitude The component σ _XX ² corresponding to the prediction error is reset to a value (initial value) that maximizes the error. However, the correction value of the inertial sensor and the prediction error σ _GG ^{2 and} the value of σ _KK ² of the error of the correction value of the inertial sensor are not corrected.

For example, initial values of position and orientation prediction errors are set to the following values. The azimuth prediction error can be set in the square of 0 to 180 degrees. Therefore, here, σ _AA ² is reset to 180 ² = 13240. The component σ _YY ² corresponding to the latitude prediction error and the component σ _XX ² corresponding to the longitude prediction error are represented by the square of the distance. Thus, here, sigma _YY ² and sigma initial value of _XX ^2, resets to the square = 100000 ² of 100 each km.

  Thereafter, the process returns to the process of step 30 in FIG. 2, and the processes of steps 100 to 400 are repeated until the accessory power supply of the vehicle changes from the on state to the off state. In the processing of steps 100 to 400, various calculations are performed using the values corrected in step 404.

  In the correction of the output value of the inertial sensor using the Kalman filter, for example, when the vehicle is transported by a car ferry or a tow truck, etc. Deviation (hereinafter referred to as actual error) occurs between the set position and orientation and the actual position and orientation.

  The Kalman filter can accurately estimate the state quantity when an appropriate prediction error can be estimated (relative to the actual error), but in this case, it is expected that the actual error >> the prediction error. If a small prediction error is used even though the actual error is large, the estimation of the state quantity is erroneous, and the vehicle position and orientation and the output value of the inertial sensor are erroneously corrected.

  Therefore, in the present embodiment, when a ferry jump is observed, a process for overwriting each value of the position and direction of the vehicle with each value of the position and direction of the vehicle output from the GPS receiver is provided. Since the position and direction of the vehicle output from the GPS 3 are not necessarily correct due to the influence of the observation error, the prediction error related to the position and direction of the vehicle is made sufficiently large (so that it does not become an actual error >> prediction error) (initial value) To reset).

  However, even if vehicle movement occurs while the vehicle position detection device is in a stopped state, the correction amount of the inertial sensor does not change.Therefore, the correction value of the inertial sensor before starting is used for correcting the output value of the inertial sensor. I use it.

  According to the above-described configuration, when it is determined that the vehicle movement has occurred while the vehicle travel locus calculation device is not operating, the position and direction output from the GPS receiver are set as the position and direction of the vehicle at the current time. In the error covariance matrix, only the prediction error of each error in the position and direction of the vehicle is reset to a predetermined value, and the prediction error of the inertia sensor correction amount and the inertia sensor correction amount error is stored in the storage medium. Is used to correct the position and orientation of the vehicle and the output value of the inertial sensor, so that erroneous correction of the output value of the inertial sensor is prevented, and the vehicle moves while the operation is stopped. After the occurrence, the traveling locus can be calculated more quickly and accurately at the time of restart. The predetermined value can be a predetermined initial value.

  The present invention is not limited to the above-described embodiment, and various modifications can be made as follows without departing from the spirit of the present invention.

  For example, in the above embodiment, in step 404, the prediction error of each position and orientation error included in the error covariance matrix is corrected to be reset to a predetermined initial value. If the value-based satellite navigation provides a prediction error for each position and orientation error of the vehicle, in step 404, the prediction error for each position and orientation error included in the error covariance matrix is You may make it correct to the prediction error of each error of the position and direction of a vehicle specified by navigation. In this way, the current position can be estimated more quickly and accurately than when the prediction error of each position and orientation error included in the error covariance matrix is reset to a predetermined initial value. It is possible.

  In the above embodiment, the offset error of the gyroscope and the distance coefficient error of the vehicle speed sensor are estimated as the error of the inertia sensor, and the output value of the inertia sensor is corrected. However, the offset error of the gyroscope and the vehicle speed sensor The inertial sensor error may be estimated so as to include an error other than the distance coefficient error (for example, a gyroscope gain error), and the output value of the inertial sensor may be corrected.

  Further, in the above-described embodiment, the position of the vehicle and the position of the azimuth and the position of the vehicle using the error covariance matrix using the Kalman filter with each error relative to the correction amount of the inertial sensor as a state quantity. Although the azimuth is corrected, an error other than the vehicle position, azimuth, and inertia sensor correction amount may be added to the state quantity.

  The correspondence between the configuration in the above embodiment and the configuration in the claims will be described. Steps 100 to 400 correspond to arithmetic processing means, the nonvolatile storage medium corresponds to storage means, and step 403 moves. It corresponds to the determination means.

1 Vehicle speed sensor 2 Gyroscope 3 GPS receiver 7 Current position detection unit 8 Navigation execution unit

Claims (3)

Based on the output values of the inertial sensors (1, 2) mounted on the vehicle and the output value of the GPS receiver (3), the position and direction of the vehicle at the current time are specified, and the position, direction and Each error is estimated using a Kalman filter with each error relative to the correction amount of the inertial sensor as a state quantity, and an error covariance matrix including a prediction error of each error as a component is calculated, and each error and the error covariance are calculated. A vehicle that includes arithmetic processing means (steps 100 to 400) that corrects the position and orientation of the vehicle and the output value of the inertial sensor using a matrix, and calculates a travel locus of the vehicle based on the position and orientation of the vehicle A travel locus calculation device for
When the vehicle travel locus calculation device stops operating, a backup for storing the correction amount of the inertial sensor and the error covariance matrix in the storage medium together with the position and orientation of the vehicle corrected by the arithmetic processing means. Means,
When the vehicle travel locus calculation device is activated, the vehicle position is specified based on the output value of the GPS receiver, and the vehicle position before the activation stored in the storage medium is stored in the storage medium. And a movement determination means for determining whether or not a vehicle movement has occurred during the operation stop of the vehicle travel locus calculation device,
When it is determined by the movement determination means that vehicle movement has occurred during the operation stop, the arithmetic processing means determines the position and direction specified by the output value of the GPS receiver as the position of the vehicle at the current time, It is regarded as an azimuth, and among the error covariance matrix, only the prediction error of each error of the position and azimuth of the vehicle is reset to a predetermined value, and the correction amount of the inertial sensor and the correction amount of the inertial sensor are predicted. As for the error, the vehicle travel locus calculation apparatus is characterized by correcting the position and direction of the vehicle and the output value of the inertial sensor using the values before the activation stored in the storage medium.   The vehicle travel locus calculation apparatus according to claim 1, wherein the predetermined value is an initial value at which an error is maximized.   The vehicle travel locus calculation apparatus according to claim 1, wherein the predetermined value is a prediction error specified based on an output value of the GPS receiver.

Images