JP6149699B2 - 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 restarting after the vehicle has stopped, the backed up values are read back and regarded as values one hour before, and the output value of the inertial sensor and the position and direction of the vehicle are corrected. Yes.

  However, for example, after the vehicle is parked in a multi-story parking lot where a turntable having a direction different from the entry direction and the exit direction by 180 degrees is installed, the vehicle is turned on while the vehicle travel locus calculation device is stopped. When rotating above, there is a difference between the direction backed up in the memory and the actual direction of the vehicle. In such a case, when the vehicle travel trajectory calculation device is restarted, the backed up heading and heading prediction error, even though the backed up heading is different from the actual heading of the vehicle. If the output value of the inertial sensor and the position and direction of the vehicle are corrected by assuming that the value is one hour ago, there is a problem that the travel locus is erroneously estimated.

As a conventional technique for the case where the vehicle rotates on the turntable while the vehicle travel locus calculation device is in a stopped state, the traveling direction information indicating the traveling direction of the own vehicle based on the direction change amount and the moving distance of the own vehicle is provided. A travel trajectory generating unit that acquires the travel trajectory of the host vehicle based on the travel direction information, and between the host vehicle position based on the host vehicle position information acquired by GPS positioning and the position of the head of the travel trajectory. A positional deviation detection unit that detects a positional deviation of a predetermined value or more, and a traveling locus matching processing unit that performs pattern matching processing between the road pattern and rigidly transforming the traveling locus when the positional deviation is detected. The pattern matching road trajectory is regarded as the head of the road pattern where the position of the host vehicle is the same as the true road trajectory. The origin of the rigid transformation vehicle direction change points for (e.g., a turntable installation site) is that so as to determine (for example, see Patent Document 2).
JP-A-8-68654 JP 2010-190721 A

  Since the device described in Patent Document 2 is calculated with the azimuth direction reversed immediately after leaving the turntable, the pattern matching process does not match the pattern, but after a while it is correct using the satellite navigation result. In order to return to the position, by tracing back the traveling trajectory so far from this return position, the position of the turntable in question is specified, but for a while after leaving the turntable, Since the direction is calculated in the reverse direction, there is a problem that malfunction occurs.

  Further, as described above, the vehicle position, orientation, inertia sensor correction amount, and error covariance matrix of each of these errors are backed up in a nonvolatile storage medium, and are backed up when the device is restarted. In a device that reads back each value and corrects the output value of the inertial sensor and the position and direction of the vehicle, if the vehicle rotates on the turntable while the device is stopped, the backed up direction is Despite being different from the vehicle's heading, the backed-up heading and heading prediction error is regarded as the value one hour before, and the output value of the inertial sensor and the vehicle position and heading are corrected. The trajectory is estimated incorrectly.

  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.

  As a result, compared to the satellite navigation output at the current time, it seems that the direction of the vehicle suddenly changed during one time, and the correction process tries to adjust the correction amount of the inertial sensor. The correction amount of the inertial sensor is erroneously estimated, and the output value of the inertial sensor is erroneously corrected.

  As described above, once the output value of the inertial sensor is erroneously corrected, there is a problem that it takes time before the travel locus can be accurately calculated again.

  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 rotates on a turntable during operation stop.

  In order to achieve the above object, the invention according to claim 1 is based on the output value of the inertial sensor (1, 2) mounted on the vehicle and the output value of the GPS receiver (3). And an error covariance matrix that includes a prediction error of each error as a component and estimates each error using a Kalman filter with each error relative to the correction amount of the vehicle position, direction and inertial sensor as a state quantity. Computation processing means (100 to 400) that calculates and corrects the position and direction of the vehicle and the output value of the inertial sensor using each error and the error covariance matrix, and the travel locus of the vehicle based on the position and direction of the vehicle A vehicle travel locus calculation device for calculating the vehicle position, orientation, and inertia sensor correction amount specified by the arithmetic processing means when the vehicle travel locus calculation device stops operating. And the backup means (50) for storing the error covariance matrix in the storage medium and the vehicle travel locus calculation device are activated, the arithmetic processing means is configured to determine the orientation of the error covariance matrix stored in the storage medium. Only the error prediction error is reset to a predetermined value, and other than the azimuth error prediction error, the pre-startup values stored in the storage medium are used to correct the vehicle position, azimuth, and output value of the inertial sensor. It is characterized by that.

  According to such a configuration, when the vehicle travel locus calculation device stops operating, the vehicle position, orientation, inertia sensor correction amount and error covariance matrix specified by the arithmetic processing means are stored in the storage medium. When the backup means (50) to be activated and the vehicle travel locus calculation apparatus are activated, the arithmetic processing means resets only the prediction error of the azimuth error to a predetermined value out of the error covariance matrix stored in the storage medium. In addition, other than the prediction error of the azimuth error, the vehicle position, azimuth, and output value of the inertial sensor are corrected using the pre-startup values stored in the storage medium. Thus, after the vehicle rotates on the turntable while the operation is stopped, 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 the navigation calculation.

First, the definition of the signal generation process will be described. Since the Kalman filter in the navigation calculation is intended to correct the error in the 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.
(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, navigation calculation using a 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 the navigation device.

  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 bearing correction, and the absolute position 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.
(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 Y component of the absolute position obtained by the navigation calculation and the Y component of the position output from the GPS ₃ , and εX _DRt −εX _GPSt is the absolute position obtained by the navigation calculation. And the difference between the X component of the position 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. The microcomputer of the navigation device performs initialization processing in step 20, resets the direction prediction error in step 30, and turns the accessory power supply of the vehicle from the off state to the on state in step 40. It is determined whether or not. 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”. When the microcomputer of the present navigation device determines in step 40 that the accessory power supply of the vehicle has been 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 50 is executed. 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.

  However, the processing of steps 20 to 30 will be described later, and the processing of step 40 and steps 100 to 400 will be described first.

  First, in step 40, it is determined whether or not the accessory power supply of the vehicle has changed from an on state to an 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, the determination in step 402 is YES, and the process proceeds to the Kalman filter calculation process in step 405 and thereafter. 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, correction of dead reckoning navigation error, that is, gyro 2 offset correction, vehicle speed sensor 1 distance coefficient correction, absolute azimuth correction, and absolute position correction are performed in step 411 by calculation shown in the figure. 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 more accurate dead reckoning navigation data. 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, prediction calculation of error covariance corresponding to an error when GPS3 positioning cannot be performed is performed, and the Kalman filter process performed when GPS3 can be positioned thereafter can be accurately performed.

  Here, returning to the description of FIG. 2, when the accessory power supply of the vehicle changes from the on state to the off state, the determination in step 40 becomes YES, and in step 50, the latitude of the state before the navigation device stops operating, The error, covariance matrix including the longitude, azimuth, offset correction amount, distance coefficient correction amount, and prediction error of each error is backed up to the nonvolatile storage medium, and the process is terminated.

  Here, when the navigation device is stopped and then restarted, in step 20, an initialization process according to a program stored in the ROM is performed and each value backed up in the nonvolatile storage medium is read back. I do. 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, the bearing prediction error is reset. Specifically, among the latitude, longitude, azimuth, offset correction amount, distance coefficient correction amount stored in the RAM in step 20 and an error covariance matrix including each error prediction error, the azimuth prediction error is predetermined. Reset to value.

For example, the initial value of the azimuth error prediction error is set to the following value. The azimuth prediction error can be set in the square of 0 to 180 degrees. Therefore, here, σ _AA ² is reset to 180 ² = 13240.

  Hereinafter, in the arithmetic processing of steps 100 to 400, among the error covariance matrix including the latitude, longitude, azimuth, offset correction amount, distance coefficient correction amount, and prediction error of each error stored in the RAM in step 20; Resetting the azimuth prediction error to a predetermined value, except for the azimuth prediction error (latitude, longitude, azimuth, offset correction amount, distance coefficient correction amount, and azimuth error prediction error included in the error covariance matrix) At step 20, each value read from the non-volatile storage medium and stored in the RAM is set to the state one hour before, and the position and direction of the vehicle at the current time are specified, and the output value of the position, direction and inertial sensor of the vehicle are specified. Make corrections.

  In the calculation of the travel trajectory using the Kalman filter, for example, after the vehicle is parked in a multi-story parking lot where turntables with different directions of entry and exit are 180 degrees, the navigation device stops operating. While the vehicle rotates on the turntable, the backed up direction and the prediction error of the direction are one hour before, even though the backed up direction is different from the actual direction of the vehicle. If the output value of the inertial sensor or the position and direction of the vehicle are corrected by assuming that the value is a value, the travel locus is erroneously estimated.

  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.

  As a result, compared to the satellite navigation output at the current time, it seems that the direction of the vehicle suddenly changed during one time, and the correction process tries to adjust the correction amount of the inertial sensor. The correction amount of the inertial sensor is erroneously estimated, and the output value of the inertial sensor is erroneously corrected.

  Therefore, in this embodiment, when the navigation device is restarted after being stopped, in step 30, the azimuth prediction error included in the error covariance matrix is reset to a value that maximizes the error, The direction error recognized by the Kalman filter is increased. This prevents erroneous correction of the output value of the inertial sensor using an azimuth prediction error that is significantly different from the actual azimuth error.

  In addition, when the navigation device is stopped and then restarted, if the azimuth value itself is reset every time to return to the initial value, the vehicle rotates on the turntable with the navigation device stopped. If it is not, the correct azimuth is not known, and the azimuth cannot be estimated promptly upon restart. For this reason, at the time of restart, a traveling locus that is traveling in the wrong direction is calculated. In most cases in actual operation, since the vehicle does not rotate with the navigation device stopped, it is a problem that it is impossible to quickly estimate the direction at the time of restart as described above.

  For this reason, in this embodiment, what is reset every time when the system is restarted is limited to the azimuth prediction error. The azimuth value itself is not reset, and the azimuth of the vehicle is promptly calculated using the value of the state before the navigation device is stopped, that is, the azimuth stored in the nonvolatile storage medium. .

  According to the configuration described above, the vehicle travel locus calculation device stores the corrected vehicle position, orientation, inertia sensor correction amount, and error covariance matrix when the vehicle travel locus calculation device stops operating. When the vehicle travel trajectory calculation device is activated and stored in the medium, the error covariance matrix stored in the storage medium is reset to a predetermined value only for the azimuth error prediction error, except for the azimuth error prediction error. (Vehicle position, heading, inertia sensor correction amount and vehicle position error prediction error, inertia sensor correction amount error prediction error) are calculated using the pre-startup values stored in the storage medium. In addition to identifying the vehicle position and direction at the time, and correcting the vehicle position, direction and output value of the inertial sensor, erroneous correction of the output value of the inertial sensor is prevented, and the vehicle is turned After rotating above, upon restart, the more quickly the travel locus can be accurately calculated.

  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 30, a value (180 ² ) that maximizes the error is set as the initial value of the prediction error of the azimuth error included in the error covariance matrix. It is not limited to a large value (180 ² ).

  In the above embodiment, the offset error is estimated as the error of the gyroscope correction amount. However, the offset error and the gain error may be estimated separately as the error of the gyroscope correction amount.

  The correspondence between the configuration of the above embodiment and the configuration of the claims will be described. Steps 100 to 400 correspond to arithmetic processing means, and step 50 corresponds to backup means.

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

Claims (1)

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. Computational processing means (100 to 400) that corrects the position and direction of the vehicle and the output value of the inertial sensor using a matrix, and for calculating the travel locus of the vehicle based on the position and direction of the vehicle A travel locus calculation device,
Backup means for storing in a storage medium the vehicle position and orientation, the correction amount of the inertial sensor, and the error covariance matrix specified by the arithmetic processing means when the vehicle travel locus calculating device stops operating. 50),
When the vehicle travel locus calculation device is activated, the arithmetic processing means resets only the prediction error of the azimuth error out of the error covariance matrix stored in the storage medium to a predetermined value, and the azimuth In addition to the prediction error of the error, the vehicle position and direction at the current time and the output value of the inertial sensor are corrected using each value before the activation stored in the storage medium. A vehicle travel locus calculation device.

Images