JP6260983B2 - Self-position estimation apparatus and method - Google Patents

Description

  The present invention relates to a self-position estimation apparatus and method, and is applied to, for example, a vehicle control system that controls a moving trajectory of a vehicle using NSS (Navigation Satellite Systems) and INS (Inertial Navigation System). And suitable.

  Conventionally, there is NSS as a technique for estimating the position of a robot, a vehicle, or a mobile terminal. NSS is a method of obtaining the distance from each positioning satellite by receiving radio waves from a plurality of positioning satellites and estimating the position of the receiver based on the obtained position of each positioning satellite. One commonly used NSS is the GPS (Global Positioning System) operated by the United States.

  When positioning the receiver using NSS, multipath occurs due to the influence of reflected waves from buildings, trees or the ground near the receiver, and the distance from the satellite is accurately determined by the influence of this multipath. It may not be required. Conventionally, the following two techniques have been proposed as a method of reducing the influence of such multipath.

  The first method is a method of discriminating whether a signal from each satellite is a direct wave or a reflected wave based on the self-position, the position of the satellite, and information on the entire surrounding shielding object. In this case, as a method for acquiring “information on all surrounding shields”, a method of acquiring from a geographic information database (Patent Document 1), a method of measuring the surroundings with an infrared / visible camera, etc. (Patent Document 2), etc. Has been proposed.

  In the second method, the positioning result by NSS is compared with the position estimation result by INS. When the difference is large, it is determined that the positioning result by NSS includes an error due to multipath, and the position estimation by INS is performed. It is a method of adopting the result. In this case, as a comparison method for comparing the positioning result by NSS and the position estimation result by INS, a method for comparing the position and orientation of a positioning target (for example, a vehicle) (Patent Document 3), and a method for comparing acceleration (patent) Document 4) has been proposed.

  In general, the INS is obtained by integrating the accelerometer information to obtain the distance, integrating the gyro information to obtain the direction, and combining the moving distance and the direction vector for each subdivision point. It is a system which calculates the movement distance and direction from. Some INS uses information such as a wheel encoder and a steer angle encoder to correct the movement distance and direction. Since INS does not directly measure the position of the positioning target, the measurement error of velocity, acceleration, and angular velocity is integrated, and the position error gradually increases. Therefore, when using INS, generally, the position estimation result by INS is periodically corrected based on the positioning result by NSS.

Japanese Patent No. 4418357 Japanese Patent No. 4391458 Japanese Patent No. 4803862 Japanese Patent No. 3338304

  By the way, the above-mentioned first method requires geographic data around the location where the positioning target exists, and for example, when using a geographic information database, data for the entire area into which the positioning target enters is necessary, and the calculation is accordingly performed. There is a problem that the amount increases. Further, in the first method, as in Patent Document 2, when it is attempted to acquire information on the surrounding shielding object by measuring the surroundings with a camera or the like, complicated processing such as recognition of the shielding object may be required. There is also a problem that it is easily affected by optical noise such as backlight.

  On the other hand, the second method described above has an advantage that multipath can be detected without processing a large amount of data. However, in position estimation by INS, a position error occurs due to the influence of noise such as tire slip and vibration. For this reason, when the positioning result by NSS and the position estimation result by INS do not coincide, the position estimation result by INS may have a larger error with respect to the actual position than the positioning result by NSS.

  In such a situation, when the position estimation result by INS is adopted as the processing result of the self-position estimation process when the positioning result by NSS and the position estimation result by INS do not match, correct positioning is performed by NSS. As a result, since the difference between the positioning result by NSS and the position estimation result by INS gradually increases, as a result, the position estimation result by INS is always adopted. There is a problem that the difference between the estimated position and the actual position is enlarged.

  The present invention has been made in consideration of the above points, and an object of the present invention is to propose a self-position estimation apparatus and method that can accurately estimate the self-position with a small amount of data and a small amount of calculation.

In order to solve such problems, in the present invention, an NSS (Navigation Satellite System) sensor that measures its own position based on radio waves from a positioning satellite, an internal sensor that detects its own state, and an output of the NSS sensor A position estimation unit that estimates its own position based on a first positioning result obtained based on the second positioning result obtained by integrating the output of the internal sensor, and the position estimation unit Is managing its own state as an internal state,
Based on the latest managed internal state, predicts the internal state at the time when the observation value of the internal state is acquired, and predicts at least the first positioning result based on the predicted internal state. The first abnormality determination process for determining whether or not a measurement abnormality has occurred in the NSS sensor based on the difference between the predicted first positioning result and the actual first positioning result is executed. If it is determined in the first abnormality determination process that a measurement abnormality has occurred in the NSS sensor, a correction process for changing the observation noise to a large value is executed, and then the NSS sensor measures again. When a second abnormality determination process for determining whether or not an abnormality has occurred is performed, and it is determined by the second abnormality determination process that no measurement abnormality has occurred in the NSS sensor, When the positioning result is corrected based on the first positioning result, it is estimated as its own position, and when it is determined that a measurement abnormality has occurred in the NSS sensor, the second positioning result is A position corrected with a smaller correction amount than the case where it is determined that no measurement abnormality has occurred in the NSS sensor is estimated as its own position.

In the present invention, in the self -position estimation method executed by the self-position estimation device, the output of the NSS (Navigation Satellite System) sensor that measures the position of the self based on the radio wave from the positioning satellite and the state of the self are detected. A first step of obtaining the output of the inner world sensor, a first positioning result obtained based on the output of the NSS sensor, and a second positioning result obtained by integrating the output of the inner world sensor And a second step of estimating the position of the self based on the self position, and the self position estimating device manages the self state as an internal state. In the second step, the self position estimating device Predicting the internal state at the time when the observation value of the internal state is acquired based on the latest internal state being managed, and reducing the internal state based on the predicted internal state At least the first positioning result is predicted, and whether or not a measurement abnormality has occurred in the NSS sensor based on the difference between the predicted first positioning result and the actual first positioning result is determined. A first abnormality determination process is performed, and if it is determined that a measurement abnormality has occurred in the NSS sensor by the first abnormality determination process, a correction process is performed to change the observation noise to a large value. After that, a second abnormality determination process for determining whether or not a measurement abnormality has occurred in the NSS sensor is executed again, and a measurement abnormality has occurred in the NSS sensor by the second abnormality determination process. If it is determined that there is no measurement error, the second positioning result is corrected based on the first positioning result and is estimated as its own position, and it is determined that a measurement abnormality has occurred in the NSS sensor. Before A second positioning result was what the abnormal measurement NSS sensor is fixed in a small correction amount than when it is determined not to have occurred so as to estimate the self location.

  According to the self-position estimation apparatus and the self-position estimation method of the present invention, since the second positioning result is always corrected based on the first positioning result, the second positioning result is finally the first positioning. Modified to approach the result. Therefore, when the second positioning result is adopted as the self-position estimation result, the first positioning result is not used after that, and the error between the self-position estimation result and the actual position increases. It is possible to effectively prevent the occurrence of the situation.

  Further, according to the self-position estimation apparatus and self-position estimation method of the present invention, it is possible to estimate the self-position with a small amount of data and calculation amount without requiring geographic data around the place where the self exists.

  According to the present invention, it is possible to realize a self-position estimation apparatus and method that can accurately estimate the self-position with a small amount of data and calculation amount.

It is a block diagram which shows schematic structure of the vehicle control system by 1st and 2nd embodiment. It is a flowchart which shows the process sequence of INS process. It is a flowchart which shows the process sequence of the update process in INS process. It is a flowchart which shows the process sequence of a NSS process. It is a flowchart which shows the process sequence of the update process in a NSS process. It is a flowchart which shows the process sequence of a 1st abnormality determination process. It is a flowchart which shows the process sequence of a 2nd abnormality determination process. It is a flowchart which shows the process sequence of a 3rd abnormality determination process. It is a flowchart which shows the process sequence of a normal correction process. It is a flowchart which shows the process sequence of a 1st abnormality time correction process. It is a flowchart which shows the process sequence of a 2nd abnormality correction process. It is a flowchart which shows the process sequence of a 3rd abnormality correction process. It is a flowchart which shows the process sequence of the update process by 2nd Embodiment. It is a flowchart which shows the process sequence of abnormality determination and a correction process.

  Hereinafter, an embodiment of the present invention will be described in detail with reference to the drawings.

(1) First Embodiment (1-1) System Configuration According to the Present Embodiment In FIG. 1, reference numeral 1 denotes a vehicle control system according to the present embodiment as a whole. The vehicle control system 1 includes a positioning device 2 that measures the position of a vehicle (not shown) on which the vehicle control system 1 is mounted, and a control device 3 that controls movement of the vehicle based on the positioning result of the positioning device 2. It consists of.

  The positioning device 2 has a self-position estimation function that estimates the position and orientation of the vehicle using both NSS and INS, and is a GPS / GPS compass as an NSS sensor (hereinafter referred to as an NSS sensor) 10. 13, an INS sensor (hereinafter referred to as an INS sensor) 11 includes a wheel encoder 14, a gyro sensor 15, and a steering angle encoder 16. In addition to the NSS sensor 10 and the INS sensor 11, the positioning device 2 includes a position estimation unit 12 that estimates a self-position based on sensor outputs of the NSS sensor 10 and the INS sensor 11.

  The GPS / GPS compass 13 is a sensor that uses two or more GPS antennas to measure not only the position but also the direction, and sends the position and orientation of the vehicle obtained by such measurement to the position estimation unit 12 as position / posture information. To do.

  The wheel encoder 14 is composed of a rotary encoder, and sends an output corresponding to the rotation angle of the vehicle wheel to the position estimating unit 12 as encoder information. The gyro sensor 15 detects the angular velocity of the vehicle, and sends the detection result to the position estimation unit 12 as angular velocity information. Further, the steer angle encoder 16 is an encoder that detects the rotation angle of the vehicle steering (hereinafter referred to as a steer angle), and sends the detection result to the position estimation unit 12 as steer angle information.

  The position estimation unit 12 includes information processing resources such as a CPU (Central Processing Unit) and a memory. The position estimation unit 12 is transmitted from the gyro sensor 15 and the vehicle speed obtained based on the encoder information transmitted from the wheel encoder 14. The position and posture of the vehicle are estimated by integrating the angular velocity of the vehicle obtained based on the angular velocity information obtained and the curvature of the moving trajectory of the vehicle obtained based on the steering angle information transmitted from the steer angle encoder 16. Further, the position estimation unit 12 corrects the position and posture of the vehicle obtained in this way based on the position / posture information transmitted from the GPS / GPS compass 13, and the position and posture of the vehicle thus obtained are corrected. The represented position / posture information is sent to the control device 3.

  The control device 3 includes an action planning unit 17, a vehicle control unit 18, and a robot drive unit 19. In the present embodiment, the action planning unit 17, the vehicle control unit 18, and the robot drive unit 19 are implemented by executing a corresponding program by a CPU (not shown) configuring the control device 3. However, the behavior planning unit 17, the vehicle control unit 18, and the robot drive unit 19 may be configured separately from each other.

  The action planning unit 17 includes a trajectory generation unit 17A that generates a trajectory of the vehicle toward a target point set in advance based on position / posture information given from the position estimation unit 12 of the positioning device 2. Information on the track generated by the generation unit 17A is sent to the vehicle control unit 18 as track information.

  Based on the trajectory information given from the action plan unit 17, the vehicle control unit 18 drives the robot drive unit so that the vehicle moves on the trajectory generated by the trajectory generation unit 17A (target value). Is provided with a command value calculation unit 18A. Then, the vehicle control unit 18 sends the command value calculated by the command value calculation unit 18A to the robot drive unit 19.

  The robot drive unit 19 includes, for example, a vehicle drive source such as an engine or a motor and a motor that rotationally drives the vehicle steering. And the robot drive part 19 drives a vehicle in the state according to the said command value by driving an engine, a motor, etc. with the rotation amount or rotation angle according to the command value given from the vehicle control part 18. FIG.

(1-2) Self-position estimation method (1-2-1) overview according to the present embodiment Next, a self-position estimation processing method (self-position estimation method) executed in the position estimation unit 12 of the positioning device 2 explain.

  In the self-position estimation method according to this embodiment, the self-position and orientation measured by the NSS and the self-position and orientation obtained by the INS are compared, and a measurement abnormality due to the influence of multipath or the like occurs in the NSS. One of the features is that it is determined whether or not it is. In addition, when the self-position estimation method obtains a determination result that no measurement abnormality has occurred in the NSS by such determination, the position and orientation of the vehicle obtained by the INS are corrected based on the measurement result of the NSS. On the other hand, when the determination result that the measurement abnormality has occurred is obtained, the position obtained by the INS is corrected with a smaller correction amount than when the determination result that the measurement abnormality has not occurred in the NSS is obtained. Another feature is the point to be corrected.

  As a means for realizing such a self-position estimation method, in this embodiment, Kalman filter theory is introduced into the self-position estimation process. That is, the position estimation unit 12 of the positioning device 2 estimates the position and orientation of the vehicle by the Kalman filter theory using the observation values obtained based on the sensor outputs of the NSS sensor 10 and the INS sensor 11.

  In the Kalman filter theory, parameters to be estimated such as position, orientation, velocity and angular velocity are set as internal states, and the internal states are sequentially used by using the observed values of the position, posture, velocity and angular velocity based on the sensor outputs of the NSS sensor 10 and the INS sensor 11. By updating, the internal state is converged to the correct value.

  In this case, since the Kalman filter theory also calculates the error variance of the estimated value, a measurement abnormality occurs in the NSS by determining whether or not the estimated value of the position and orientation by the NSS is within the error range. It is possible to determine whether or not.

  If a measurement abnormality occurs in the NSS, the position obtained by the INS is corrected with a smaller correction amount than when the determination result that the measurement abnormality does not occur in the NSS is obtained. Later, it is effective that the error between the position and posture of the self measured by the NSS and the position and posture of the self obtained by the INS will expand and the NSS measurement result will not be adopted. Can be prevented.

(1-2-2) Contents of Self-Position Estimation Processing Using Kalman Filter Theory Next, specific processing contents of self-position estimation processing according to the present embodiment using Kalman filter theory will be described.

  FIG. 2 shows the contents of the INS process executed by the position estimation unit 12 at a predetermined first update cycle (for example, 0.02 second cycle). The position estimation unit executes the INS process shown in FIG. 2 to update the internal state at the first update cycle based on the sensor output of each INS sensor 11.

  In practice, when the INS process shown in FIG. 2 is started, the position estimation unit 12 first obtains the current time (SP1), and thereafter transmits the current time from the wheel encoder 14, the gyro sensor 15, and the steering angle encoder 16, respectively. Encoder information, angular velocity information, and steering angle information are sequentially acquired (SP2 to SP4). Subsequently, the position estimation unit 12 updates the internal state based on the encoder information, angular velocity information, and steer angle information acquired in steps SP2 to SP4 (SP5), and thereafter returns to step SP1. The position estimation unit 12 repeatedly executes the above INS process at the first update cycle.

  FIG. 3 shows specific processing contents of the update processing executed by the position estimation unit 12 in step SP5 of the INS processing (FIG. 2).

  When the position estimation unit 12 proceeds to step SP5 of the INS process, the position estimation unit 12 starts this update process. First, based on the latest internal state held, each INS sensor 11 (the wheel encoder 14, the gyro sensor 15 and the An internal state prediction process for predicting the internal state at the time when the steer angle encoder 16) acquires the corresponding sensor value (hereinafter referred to as the observation time) is executed (SP10).

  Subsequently, the position estimation unit 12 determines the vehicle speed obtained based on the encoder information from the wheel encoder 14, the vehicle angular speed obtained based on the angular velocity information from the gyro sensor 15, and the steer angle from the steer angle encoder 16. Observation value prediction processing is performed to obtain the predicted value of the observed value of the vehicle trajectory curvature obtained based on the information (SP11), and thereafter, the predicted value of the observed value obtained in step SP11 and the actual observation A correction process for correcting the internal state based on the value is executed (SP12). The position estimation unit 12 then returns to the INS process.

  On the other hand, FIG. 4 shows the processing contents of the NSS processing executed by the position estimation unit 12 in a predetermined second update cycle (for example, 1 second cycle) in parallel with the above-described INS processing (FIG. 2). The position estimation unit 12 updates the internal state based on the sensor output of the NSS sensor 10 by executing the NSS process shown in FIG.

  In practice, when the NSS process shown in FIG. 4 is started, the position estimation unit 12 first acquires the current time (SP20), and then acquires position / posture information given from the GPS / GPS compass 13 (SP21). ). Subsequently, the position estimation unit 12 executes an update process for updating the internal state based on the position information included in the position / posture information acquired in step SP21 (SP22), and is further included in the position / posture information. After executing an update process for updating the internal state based on the attitude information (azimuth information) (SP23), the process returns to step SP20. The position estimation unit 12 repeatedly executes the above NSS processing at the second update cycle.

  FIG. 5 shows specific processing contents of the update processing executed by the position estimation unit 12 in step SP22 and step SP23 of the NSS processing (FIG. 4).

  When the position estimation unit 12 proceeds to step SP22 or step SP23 of the NSS process, the position estimation unit 12 starts this update process. First, the GPS / GPS compass 13 determines the position / posture based on the latest internal state held. An internal state prediction process for predicting the internal state at the positioning time (observation time) is executed (SP30).

  Subsequently, the position estimation unit 12 executes an observed value prediction process for obtaining a predicted value of the observed value of the GPS / GPS compass 13 and an error range of the predicted value (SP31). Thereafter, the position estimation unit 12 compares the predicted value of the observed position or orientation of the vehicle obtained in step SP30 with the observed value of the position or orientation of the vehicle observed by the actual GPS / GPS compass 13 ( The position is compared in the update process of step SP22, and the posture is compared in the update process of step SP23), and whether or not a measurement abnormality has occurred in the NSS depending on whether or not these differences exceed the error range of the predicted value obtained in step SP31. An abnormality determination process is performed to determine whether (SP32).

  And the position estimation part 12 correct | amends an internal state based on the process result of this abnormality determination process (SP33). Specifically, if the position estimation unit 12 determines that the observation value is normal as a result of the abnormality determination process in step SP32, the position estimation unit 12 corrects the internal state using the measurement result of the NSS, When it is determined that there is an abnormality, the internal state is corrected with a correction amount smaller than the correction amount when it is determined that there is no abnormality in the observed value. The position estimation unit 12 then returns to the NSS process.

(1-2-3) Details of update processing (1-2-3-1) Internal state and its variance / covariance Next, the internal state prediction processing, observation value prediction processing, abnormality determination processing, and correction processing described above will be described. Specific contents will be described. In the following description, the “target sensor” refers to the wheel encoder 14, the gyro sensor 15, and the steer angle encoder 16 during the INS process, and in the update process of step SP 22 of the NSS process (FIG. 4), GPS / A GPS sensor in the GPS compass 13 is indicated, and the GPS compass in the GPS / GPS compass 13 is indicated in the update process in step SP23 of the NSS process.

First, the internal state of the vehicle used in the following description and its dispersion / covariance will be described. The position estimator 12 sets these parameters to the following equations, where the vehicle position is (x, y), the attitude is θ, the velocity is vx, and the angular velocity is wz.
Is managed as an internal state vector X _t (hereinafter simply referred to as an internal state X _t ).

The position estimation unit 12, as a matrix equation representing the covariance of the error range of the estimated value of the internal state X _t, the following equation
A covariance / covariance matrix CovX _t (hereinafter, simply referred to as “dispersion / covariance CovX _t” ) is held in the memory. The diagonal term of the matrix in equation (2) represents the variance of each parameter, and the off-diagonal term represents the covariance between parameters.

The internal state _{X t} and covariance CovX _t of the vehicle is a value which changes any time. Hereinafter, the time representing the internal state X _t and covariance CovX _t modeled time t, each parameter model time denote the value of the time point of t.

Then, the position estimation unit 12 initializes the internal state _Xt as the following expression based on the position / posture information first given from the GPS / GPS compass 13.

The position estimation unit 12, together with the initialization of such internal state X _t, initializes the covariance CovX _t of the internal state X _t according to the following equation.

In the equations (3) and (4), t is set to the time when the GPS / GPS compass 13 measures the position and orientation of the vehicle. In Equation (4), σinit _{x to} σinit _wz are constants that are appropriately determined in consideration of the accuracy of position / posture information given from the GPS / GPS compass 13.

(1-2-3-2) in the internal state prediction processing the internal state prediction process, time acquired sensor output of the sensor of interest (hereinafter, referred to as observation time) internal state X _t and variance-co in Predict the variance CovX _t . This predicted value is obtained by setting the difference between the observation time and the model time as Δt (observation time−model time).
It can be obtained as follows.

In the equation (6), “A” represents the following equation:
Where “Q” is the following equation
It is a matrix represented by

(1-2-3-3) Observation value prediction processing In the observation value prediction processing,
In the observation equation represented by defining for each sensor of interest, and the prediction value X _{t + Delta] t} of the internal state X _t determined by the internal state prediction process described above, covariance of covariance CovX _t CovX _t + _Δt By substituting, the observation value of the target sensor is predicted.

Here, in equation (9), Y _t represents the predicted value of the observed value, and X _t ^* represents the internal state before the correction process (assuming that _t matches the observation time by the prediction process). . Similarly, hereinafter, “*” represents a value before correction processing.

In the observation value prediction process, for each target sensor, the variance / covariance CovY _t of the observation value prediction value Y _t of the sensor is expressed by the following equation.
Ask for. In the equation (10), B represents the following equation:
Represents the partial differential matrix of the observation equation given by (hereinafter referred to as the observation matrix).

Furthermore, in the observation value prediction process, for each target sensor:
Obtaining the variance expected value V _t that contains sensor noise by. In the equation (12), R is an observation noise indicating how much noise is included in the sensor value.

In the case of the present embodiment, for the GPS sensor included in the GPS / GPS compass 13, the function g (X _t ^* ), the observation matrix B, and the observation noise R are respectively given by
Is defined as follows.

For the GPS compass included in the GPS / GPS compass 13, the function g (X _t ^* ), the observation matrix B, and the observation noise R are respectively expressed by the following equations:
Is defined as follows.

Further, for the wheel encoder 14 and the gyro sensor 15, the function g (X _t ^* ), the observation matrix B, and the observation noise R are respectively expressed by the following equations.
For the steer angle encoder 16, the function g ( _Xt ^* ), the observation matrix B, and the observation noise R are respectively expressed by the following equations:
Is defined as follows.

Note that when the predicted value vx ^* of the vehicle before correction obtained based on the encoder information from the wheel encoder 14 is “0”, the predicted value of the steer angle cannot be calculated. The correction process by is not executed. Σgps_x to σ_κ are determined in consideration of the accuracy (variation) of each sensor.

Therefore, in the observation prediction process of the INS process (FIG. 2), the position estimation unit 12 predicts the observation value of the velocity vx obtained based on the encoder information from the wheel encoder 14 according to the equations (19) to (24). Predicted value Y _t of the observed value of angular velocity wz obtained based on the value Y _t , angular velocity information from the gyro sensor 15, and predicted value Y of the observed value of curvature κ obtained based on the steer angle information from the steer angle encoder 16 _t .

Further, in the observation prediction process in the update process of step SP22 of the NSS process (FIG. 4), the position estimation unit 12 is based on the position / attitude information from the GPS / GPS compass 13 according to the expressions (13) to (15). A predicted value Y _t of the observed value of the obtained position (x, y) is obtained, and in the observation prediction process in the update process of step SP23 of the NSS process, the GPS / GPS compass 13 is obtained by Expressions (16) to (18). The predicted value Y _t of the observed value of the posture θ obtained based on the position / posture information from is obtained.

(1-2-3-4) Abnormality determination process The abnormality determination process is a process executed only in the NSS process as described above. In this abnormality determination process, it is determined whether or not a measurement abnormality due to the influence of a multipath or the like has occurred in the GPS / GPS compass 13.

Here, as a determination method of such abnormality determination, a first abnormality determination method for evaluating the absolute value of the difference between the predicted value and the observed value, a second abnormality determination method for evaluating the magnitude of the Mahalanobis distance, A third abnormality determination method that evaluates the magnitude of the correction amount X _Corr of the state X _t can be considered.

Among these, the first abnormality determination method is the absolute value of the difference between the predicted value Y _t of the vehicle position calculated by the equation (13) and the observed value Y _s of the actual vehicle position (step SP22 of NSS processing). Or the absolute value of the difference between the predicted value Y _t of the vehicle posture calculated by the equation (16) and the observed value Y _s of the actual vehicle posture (in the case of step SP23 of the NSS process) This is a method for determining that a measurement abnormality has occurred in the GPS / GPS compass 13 when the position or orientation is larger than a threshold value set in advance.

  FIG. 6 shows a specific processing procedure of the position estimation unit 12 when the first abnormality determination method is adopted as a determination method for determining whether or not a measurement abnormality has occurred in the GPS / GPS compass 13.

In this case, according to the processing procedure shown in FIG.
Thus, with respect to the position or orientation of the vehicle, a difference (hereinafter referred to as an observation residual) Ey between the predicted value Y _t calculated by the expression (13) or the expression (16) and the actual observation value Y _s is obtained. Calculate (SP40).

Subsequently, the position estimation unit 12 uses the following equation for the observation residual Ey calculated by the equation (25).
As described above, it is determined whether or not the absolute value of the observation residual Ey is larger than a predetermined threshold value (Th_Ey) (SP41).

  The position estimation unit 12 determines that the normal correction process should be executed if a negative result is obtained in this determination (SP42), and if an affirmative result is obtained in this determination, the position estimation unit 12 corrects the abnormality. It is determined that the process should be executed (SP43).

  On the other hand, the second abnormality determination method is a method for determining that a measurement abnormality has occurred in the GPS / GPS compass 13 when the Mahalanobis distance is large.

If the measurement error of the motion error and sensors of the vehicle are all the normal distribution, the observation residuals Ey also mean "0", a normal distribution of variance expected value V _t. Therefore, the following equation is obtained by dividing the square of the error (observation residual Ey) by the error variance:
The Mahalanobis distance M calculated by the above is 99.7% with a probability of 3 or less (because the normal distribution error has a probability of entering the 3σ range is 99.7%). Can do.

Therefore, in the present embodiment, in step SP22 of the NSS process,
In step SP23 of NSS processing based on whether or not
Whether or not measurement abnormality has occurred in the GPS / GPS compass 13 is determined based on whether or not the above condition is satisfied.

  In Expression (28), Th_gxy represents a threshold value of a position predetermined for the Mahalanobis distance M, and Th_gth in Expression (29) represents a threshold value of a posture predetermined for the Mahalanobis distance M. These threshold values Th_gxy and Th_gth are threshold values determined with reference to actual experimental data. As described above, the Mahalanobis distance M is 3 or less with a probability of 99.7%, so it is determined in the range of 3-4. Will be.

  FIG. 7 shows a specific processing procedure of the position estimation unit 12 when this second abnormality determination method is adopted as a determination method for determining whether or not a measurement abnormality has occurred in the GPS / GPS compass 13. .

In this case, according to the processing procedure shown in FIG. 7, the position estimation unit 12 first determines the observation residual between the predicted value Y _t calculated by the equation (13) and the actual observation value Y _s for the vehicle position. Ey is calculated by equation (25) (SP50).

  Subsequently, the position estimation unit 12 calculates the Mahalanobis distance M by the equation (27) (SP51), and then in step SP22 of the NSS process, the threshold value for the position where the Mahalanobis distance M calculated for the position is set in advance. It is determined whether or not it is smaller than Th_gxy, and in step SP23 of the NSS process, it is determined whether or not the Mahalanobis distance M calculated for the posture is smaller than a threshold value Th_gth for a preset posture (SP52).

  The position estimation unit 12 determines that normal correction processing should be executed if a negative result is obtained in this determination (SP53), and if an affirmative result is obtained in this determination, the correction at the time of abnormality is performed. It is determined that the process should be executed (SP54).

On the other hand, the third abnormality determination method is not the Mahalanobis distance M in the abnormality determination,
In using the correction amount _{X Corr} internal state _{X t} represented a method of determining whether an abnormality measured GPS / GPS compass 13 has occurred.

In the equation (30), K is the following equation:
Represents the Kalman gain calculated by

When employing this third abnormality determination method, in step of NSS processing SP22, the correction amount _X each component of _{Corr (X Corr _x, X Corr} _y, X Corr _θ, X Corr _vx, X Corr _wz and X _Corr _Kappa ) Among the components X _Corr _x and X _Corr _y related to the position,
Based on whether meet, also in step of NSS processing SP23, among the components of the correction amount X _Corr, using the component X _Corr _θ about attitude, the following equation
Whether or not measurement abnormality has occurred in the GPS / GPS compass 13 is determined based on whether or not the above condition is satisfied.

In Expression (32), Th_gxy represents a threshold value of a position predetermined for the evaluation value V _Corr , and Th_gth in Expression (33) represents a threshold value of a posture predetermined for the evaluation value V _Corr .

  FIG. 8 shows a specific processing procedure of the position estimation unit 12 when this third abnormality determination method is adopted as a determination method for determining whether or not a measurement abnormality has occurred in the GPS / GPS compass 13. Show.

In this case, according to the processing procedure shown in FIG. 8, the position estimation unit 12 first determines the observation residual between the predicted value Y _t calculated by the expression (13) and the actual observation value Y _s for the vehicle position. Ey is calculated by the equation (25) (SP 60 ).

Subsequently, the position estimation unit 12 calculates the Kalman gain K from the equation (31) (SP61), and then calculates the correction amount X _Corr represented by the equation (30) (SP62). The position estimating unit 12, and thereafter, in step of NSS processing SP22, by utilizing the components X _Corr _x and X _Corr _y about the position of the correction amount _{X Corr} calculated in step SP62, satisfies the equation (32) whether the judged, in step of NSS processing SP23, by utilizing the components X _Corr _θ regarding the attitude of the correction amount _{X Corr} calculated in step SP62, it is determined whether satisfies the equation (33) ( SP63).

  The position estimation unit 12 determines that the normal correction process should be executed if a negative result is obtained in this determination (SP64), and if an affirmative result is obtained in this determination, the position estimation unit 12 corrects the abnormality. It is determined that the process should be executed (SP65).

(1-2-3-5) Correction process (1-2-3-5-1) Normal correction process The position estimation unit 12 determines that the normal correction process should be executed in the above-described abnormality determination process. If
The internal state _Xt is corrected by
Modifying the covariance CovX _t internal state _{X t} by. In the equations (34) and (35), K represents the Kalman gain calculated by the above equation (31).

  FIG. 9 shows a specific processing procedure of correction processing (hereinafter referred to as normal correction processing) executed by the position estimation unit 12 when normal processing is selected as correction processing in the abnormality determination processing described above. .

In this case, the position estimation unit 12 first calculates the Kalman gain K by executing the calculation of (31) (SP70). Subsequently, the position estimation unit 12 executes the operation of using the Kalman gain K calculated (34) in step SP71, and calculates a new internal state X _t, holds at that time inside the state _{X t,} corrects be replaced by a new internal state _{X t} calculated at this time (SP71).

Subsequently, the position estimation unit 12 calculates the variance / covariance CovX _t of the new internal state X _t by executing the calculation of the equation (36) using the Kalman gain K calculated in step SP71 ( SP72), the dispersion / covariance CovX _t held at that time is corrected to be replaced with the new dispersion / covariance CovX _t calculated at this time. Then, the position estimation unit 12 thereafter ends this normal correction process.

(1-2-3-5-2) Correction processing at the time of abnormality On the other hand, in the above-described abnormality determination processing, when it is determined that the correction processing at the time of abnormality should be executed, the correction amount is smaller than that at the time of normal correction processing. modifying the internal state _{X t} and covariance CovX _t.

Here, as a method of correcting the internal state X _t and its variance / covariance CovX _t with a correction amount smaller than that in the normal correction process, the first abnormal correction method for switching observation noise and correction by weighting are performed. A second abnormality correction method and a third abnormality correction method for performing upper limit processing of the correction amount are conceivable.

  Of these, the first abnormality correction method is a method of switching the observation noise R of the position / posture information output from the GPS / GPS compass 13 to a larger value than that during the normal correction process. For example, when an abnormality determination is obtained in the abnormality determination process in step SP32 (FIG. 5) of the update process executed in step SP22 of the NSS process (FIG. 4), “σgsp_x” in Expression (15) in the normal correction process and When the value of “σgsp_y” is both set to 0.3 [m], the values of “σgsp_x” and “σgsp_y” are both switched to 3 [m], which is 10 times that value. When abnormality determination is obtained in the abnormality determination process in step SP32 of the update process executed in step SP23 of the NSS process, the value of “σgsp_θ” in the equation (18) at the time of the normal correction process is set to 15 [degrees]. If it is, the value of “σgsp_θ” is switched to 150 [degrees], which is 10 times that value.

By switching the observation noise R of the position / posture information output from the GPS / GPS compass 13 to a large value in this way, the Kalman gain K expressed by the equation (31) can be reduced, and as a result, it is normally corrected. it is possible to modify the internal state X _t and covariance CovX _t with a smaller correction amount than during processing.

  FIG. 10 shows the position estimation unit 12 when the first abnormality correction method is adopted as a correction method when it is determined that a measurement abnormality has occurred in the GPS / GPS compass 13 during the abnormality determination process. A specific processing procedure is shown.

In this case, the position estimation unit 12 first changes the observation noise R to a preset value at the time of abnormal correction processing (a value larger than that at the time of normal correction processing) according to the processing procedure shown in FIG. , (12), the variance expected value V _t of the observation residual Ey is recalculated (SP80).

Next, the position estimating unit 12 uses the variance expected value _{V t} of the observation residuals Ey calculated in step SP80, calculates the Kalman gain K by equation (31) (SP81).

Then, the position estimating unit 12 uses the Kalman gain K calculated in step SP81 (34) to calculate the internal state X _t by formula, a new internal state of the internal state X _t held, calculated this time way to modify and replace with X _t (SP82).

The position estimation unit 12 uses a Kalman gain K calculated in step SP81 (35) to calculate the covariance CovX _t internal state _{X t} by formula, the variance-covariance CovX _t held, At this time fix be replaced by a new covariance CovX _t calculated (SP83).

On the other hand, the second abnormality correction method is as follows for the internal state _Xt.
As for the dispersion / covariance CovX _t ,
In this way, the correction amount (K × Ey or −K × B × CovX _t ^* ) is divided by the Mahalanobis distance M.

As the degree of NSS measurement abnormality increases, the Mahalanobis distance M also increases. Therefore, by dividing the correction amount by the Mahalanobis distance M as shown in Equations (35) and (36), it is less than that during normal correction processing. The internal state X _t and the variance / covariance CovX _t can be corrected with the correction amount.

  FIG. 11 shows the position estimation unit 12 when the second abnormality correction method is adopted as a correction method when it is determined that a measurement abnormality has occurred in the GPS / GPS compass 13 during the abnormality determination process. A specific processing procedure is shown.

  In this case, according to the processing procedure shown in FIG. 11, the position estimation unit 12 first calculates the normal Kalman gain K using the equation (31) (SP90), and calculates the calculated Kalman gain K using the equation (27). A coefficient divided by the given Mahalanobis distance M is calculated (SP91).

Next, the position estimating unit 12 uses the calculated coefficient in the step SP91 (34) to calculate the internal state X _t by formula, the internal state X _t which holds the new internal state X calculated this time Correction is made so as to replace _t (SP92).

In addition, the position estimation unit 12 calculates the variance / covariance CovX _t of the internal state X _t by using the coefficient calculated in step SP91, using the coefficient (35), and the retained variance / covariance CovX _t is obtained at this time. to replace the computed new covariance CovX _t was to modify (SP93).

On the other hand, the third abnormality correction method is as follows for the internal state _Xt.
As for the dispersion / covariance CovX _t ,
It is a method of correcting as follows. In Equations (38) and (39), Th represents the upper limit of the correction amount that can be accepted by the system, and X _Corr represents the correction amount in the normal correction process.

According to the third abnormal-time correction method, it is possible to modify the amount of internal state X _t, such an effect that it is possible to limit the processing below a preset threshold.

  FIG. 12 shows the position estimation unit 12 when the third abnormality correction method is adopted as a correction method when it is determined that a measurement abnormality has occurred in the GPS / GPS compass 13 during the abnormality determination process. A specific processing procedure is shown.

In this case, the position estimation unit 12 first calculates the Kalman gain K from the equation (31) according to the processing procedure shown in FIG. 12 (SP100), and uses the calculated Kalman gain K to calculate the internal state according to the equation (30). A correction amount X _Corr of X _t is calculated (SP101).

For example, when the third abnormality determination method described above is adopted as the abnormality determination method in the abnormality determination process, the Kalman gain K and the correction amount X _Corr are already calculated at the stage of the abnormality determination process ( Steps SP61 and SP62) and steps SP100 and SP101 in FIG. 8 can be omitted.

Subsequently, the position estimation unit 12
Thus, the coefficient α for reducing the correction amount X _Corr is calculated (SP102).

Next, the position estimation unit 12 calculates the internal state X _t by the equation (38) using the Kalman gain K calculated in step SP100 and the coefficient α calculated in step SP102, and holds the internal state X _t and corrects be replaced by a new internal state _{X t} calculated at this time (SP103).

The position estimation unit 12, and the Kalman gain K calculated in step SP100, by using the calculated coefficient α in step SP102 to calculate the covariance CovX _t internal state _{X t} by formula (39), and held The dispersion / covariance CovX _t that has been corrected is replaced with the new dispersion / covariance CovX _t calculated at this time (SP104).

(1-3) According to the vehicle control system 1 effects more in this embodiment, since the positioning result by the INS is always corrected based on the measurement results of the N SS, finally positioning result by the INS of NSS It is corrected to get closer to the positioning result. Therefore, when the positioning result by the INS is adopted as the estimation result of the self position, the positioning result by the NSS is not used after that, and the error between the estimation result of the self position and the actual position increases. Can be effectively and effectively prevented from occurring.

  Moreover, according to this vehicle control system 1, the self-location can be estimated with a small amount of data and a calculation amount without requiring geographic data around the place where the vehicle exists.

  Thus, according to the vehicle control system 1, the self-position can be estimated with high accuracy with a small amount of data and calculation amount.

(2) Second Embodiment In FIG. 1, reference numeral 20 denotes a vehicle control system according to a second embodiment as a whole. The vehicle control system 20 is configured in the same manner as the vehicle control system 1 of the first embodiment except that the processing content of the update process executed by the position estimation unit 22 of the positioning device 21 is different.

  FIG. 13 shows the processing contents of the update processing according to the present embodiment executed in place of the update processing described above with reference to FIG. 5 in step SP22 and step SP23 of the NSS processing (FIG. 4).

  When proceeding to step SP22 and step SP23 of the NSS process, the position estimation unit 22 starts this update process. First, the internal state prediction process of step SP110 and the observation value prediction process of step SP111 are performed as described above with reference to FIG. The same process as the internal state prediction process and the observation value prediction process (SP30, SP31) of the update process of the first embodiment is performed.

  Subsequently, the position estimation unit 22 determines whether or not a measurement abnormality occurs in the NSS, and when the measurement abnormality occurs in the NSS, an abnormality determination that corrects the internal state and its variance / covariance and The correction process is executed (SP112).

  FIG. 14 shows specific processing contents of the abnormality determination and correction processing executed by the position estimation unit 22 in step SP112 of the update processing.

  When proceeding to step SP112 of the update process (FIG. 13), the position estimation unit 22 starts the abnormality determination process and the correction process shown in FIG. 14, and first, step SP50 of the second abnormality determination process described above with reference to FIG. By processing step SP120 to step SP122 in the same manner as in step SP52, in step SP22 of the NSS process, it is determined whether or not the Mahalanobis distance M calculated for the position is smaller than a threshold value Th_gxy for a preset position. In step SP23 of the NSS process, it is determined whether the Mahalanobis distance M calculated for the posture is smaller than a threshold value Th_gth for the preset posture (SP120 to SP122).

When the position estimation unit 22 obtains a positive result in this determination, it corrects the observation noise R in advance in the same manner as in steps SP80 and SP81 of the first abnormality correction process described above with reference to FIG. After changing to the value at the time of processing (a value larger than that at the time of normal correction processing), the variance expected value V _t of the observation residual Ey is recalculated by the equation (12) (SP126), and further this variance expected value V _t Is used to calculate the Kalman gain K using the equation (31) (SP127).

  Subsequently, the position estimation unit 22 newly divides the Kalman gain K calculated in step SP127 by the Mahalanobis distance M given by equation (27), similarly to step SP91 of the second abnormality correction process described above with reference to FIG. A Kalman gain K is calculated (SP128).

Next, the position estimation unit 22 calculates the correction amount X _Corr using the new Kalman gain K calculated in step SP128 in the same manner as in step SP62 of the third abnormality correction process described above with reference to FIG. 8 (SP129). ).

Or after this, the position estimation unit 22, in step of NSS processing SP22, by utilizing the components X _Corr _x and X _Corr _y about the position of the correction amount _{X Corr} calculated in step SP62, satisfies the equation (32) determine whether, at steps NSS processing SP23, by utilizing the components X _Corr _θ regarding the attitude of the correction amount _{X Corr} calculated in step SP62, it is determined whether satisfies the equation (33) (SP130 ).

If the position estimation unit 22 obtains an affirmative result in this determination, the internal state X _t and its variance / covariance CovX _t are the same as in step SP102 to step SP104 of the third abnormality correction method described above with reference to FIG. Is corrected (SP131 to SP133), and then the updating process is terminated.

Position estimating unit 22 on the other hand, if the negative result is obtained in the determination at step SP130, calculates a new internal state X _t by the above equation (34) using the new Kalman gain K calculated in step SP128 together, the internal state _{X t} which holds modified by replacing the internal state _{X t} calculated at this time (SP 134).

Further, the position estimation unit 22 calculates a new variance / covariance CovX _t using the new Kalman gain K calculated in step SP128 by the above-described equation (35), and holds the variance / covariance CovX _t held therein. Is replaced by the variance / covariance CovX _t calculated at this time (SP13 5 ). Then, the position estimating unit 22 ends this update process.

According to the vehicle control system 20 of the present embodiment described above, when the abnormality determination process determines that an abnormality has occurred in the NSS positioning result, the correction process for changing the observation noise R to a large value is executed. in terms of the, again, the abnormality determination process performed, when an abnormality is detected in the abnormality determination process, so as to modify the internal state X _t and covariance CovX _t in further executing the corrective action Therefore, the self-position can be estimated with higher accuracy.

(3) Other Embodiments In the first and second embodiments described above, the case where the present invention is applied to a positioning device that estimates the position of a vehicle has been described. The present invention is not limited to this, and can be widely applied to various other self-position estimation apparatuses that estimate the position of a moving body other than a vehicle.

  In the first and second embodiments described above, in the NSS process (FIG. 4), an abnormality determination is made as to whether or not an abnormality has occurred in the measurement result for each position and orientation. As described above, the present invention is not limited to this, and the abnormality determination may be performed based on only one of the position and the posture.

Furthermore, in the first and second embodiments described above, the case where the three internal sensors of the wheel encoder 14, the gyro sensor 15, and the steering angle encoder 16 are applied as the INS sensor 11 has been described. The present invention is not limited to this, and it is not necessary to use all three internal sensors. For example, the gyro sensor 15 may be omitted by using only the wheel encoder 14 and measuring the speed and angular velocity from the rotation amounts of the left and right wheels, and using only the gyro sensor 15, the acceleration and angular velocity may be omitted. May be detected, and the internal state may be corrected based on these. However, by increasing the number (type) of the internal sensors or increasing the accuracy, the expected dispersion value V _t when calculating the Mahalanobis distance M becomes a small value, so that there is an advantage that it is easy to perform abnormality determination.

  Further, in the first and second embodiments described above, the case where the wheel encoder 14, the gyro sensor 15, and the steering angle encoder 16 are applied as the INS sensor 11 has been described, but the present invention is not limited thereto. Instead, for example, as shown in FIG. 1, an acceleration sensor 30 may be added as the INS sensor 11.

  The present invention can be applied to a positioning device that estimates the position of a vehicle.

  DESCRIPTION OF SYMBOLS 1,20 ... Vehicle control system, 2, 21 ... Positioning device, 3 ... Control device, 10 ... NSS sensor, 11 ... INS sensor, 12, 22 ... Position estimation part, 13 ... GPS / GPS Compass, 14 ... Wheel encoder, 15 ... Gyro sensor, 16 ... Steer angle encoder.

Claims (4)

NSS (Navigation Satellite System) sensor that measures its position based on radio waves from positioning satellites;
An internal sensor that detects its own state,
A position estimator that estimates the position of itself based on a first positioning result obtained based on the output of the NSS sensor and a second positioning result obtained by integrating the output of the internal sensor. ,
The position estimation unit
I manage my state as an internal state,
Based on the latest managed internal state, predict the internal state at the time when the observation value of the internal state was acquired,
Predicting at least the first positioning result based on the predicted internal state;
A first abnormality determination process for determining whether or not a measurement abnormality has occurred in the NSS sensor based on a difference between the predicted first positioning result and the actual first positioning result;
If it is determined in the first abnormality determination process that a measurement abnormality has occurred in the NSS sensor, a correction process for changing the observation noise to a large value is executed, and then a measurement abnormality is again detected in the NSS sensor. The second abnormality determination process for determining whether or not has occurred,
If it is determined in the second abnormality determination process that no measurement abnormality has occurred in the NSS sensor, the second positioning result corrected based on the first positioning result is taken as its own position. When it is estimated that the measurement abnormality has occurred in the NSS sensor, the second positioning result is corrected with a smaller correction amount than when it is determined that the measurement abnormality has not occurred in the NSS sensor. A self-position estimation apparatus characterized by estimating a corrected position as a self-position. The position estimation unit
In the first abnormality determination process, the absolute value of the difference between the predicted first positioning result and the actual first positioning result, the Mahalanobis distance of the difference, and the magnitude of the correction amount of the internal state 2. The self-position estimation apparatus according to claim 1, wherein a determination is made as to whether or not a measurement abnormality has occurred in the NSS sensor based on one of them. In the self-position estimation method executed by the self-position estimation device,
A first step of acquiring an output of an NSS (Navigation Satellite System) sensor that measures its own position based on radio waves from a positioning satellite and an output of an internal sensor that detects its own state;
A second step of estimating its own position based on a first positioning result obtained based on the output of the NSS sensor and a second positioning result obtained by integrating the output of the internal sensor;
With
The self-position estimation device
I manage my state as an internal state,
In the second step, the self-position estimation device includes:
Based on the latest managed internal state, predict the internal state at the time when the observation value of the internal state was acquired,
Predicting at least the first positioning result based on the predicted internal state;
A first abnormality determination process for determining whether or not a measurement abnormality has occurred in the NSS sensor based on a difference between the predicted first positioning result and the actual first positioning result;
If it is determined by the first abnormality determination process that a measurement abnormality has occurred in the NSS sensor, a correction process for changing the observation noise to a large value is executed, and then the measurement abnormality is again detected in the NSS sensor. Execute the second abnormality determination process to determine whether or not
If it is determined in the second abnormality determination process that no measurement abnormality has occurred in the NSS sensor, the second positioning result corrected based on the first positioning result is taken as its own position. When it is estimated that the measurement abnormality has occurred in the NSS sensor, the second positioning result is corrected with a smaller correction amount than when it is determined that the measurement abnormality has not occurred in the NSS sensor. Estimate the corrected position as self
A self-position estimation method characterized by the above. The self-position estimation device
In the first abnormality determination process, the absolute value of the difference between the predicted first positioning result and the actual first positioning result, the Mahalanobis distance of the difference, and the magnitude of the correction amount of the internal state Based on either, it is determined whether or not a measurement abnormality has occurred in the NSS sensor
The self-position estimation method according to claim 3.