JP4925909B2

JP4925909B2 - Position detection apparatus and position detection method

Info

Publication number: JP4925909B2
Application number: JP2007121490A
Authority: JP
Inventors: 高之星崎; 隆行渡辺
Original assignee: Alpine Electronics Inc
Current assignee: Alpine Electronics Inc
Priority date: 2007-05-02
Filing date: 2007-05-02
Publication date: 2012-05-09
Anticipated expiration: 2027-05-02
Also published as: JP2008275530A

Description

本発明は、車両の現在位置を検出する位置検出装置および位置検出方法に関わり、特に、処理装置の負荷を軽減しつつ自律航法により計算される位置データの精度を向上できる位置検出装置および位置検出方法に関する。 The present invention relates to a position detection device and a position detection method for detecting a current position of a vehicle, and in particular, a position detection device and a position detection capable of improving the accuracy of position data calculated by autonomous navigation while reducing the load on a processing device. Regarding the method.

車載用ナビゲーション装置は、自律航法センサーを用いた自律航法（Dead Reckoning）とGPS（Global Positioning System）レシーバを用いたGPS航法を併用している。
自律航法は、車両の加速度を検出する加速度センサーや車両の方位変化量を検出する相対方位センサー（ジャイロ等）、車両の速度（距離）を検出する距離センサー（車速センサー等）の出力を用いて、車両の位置・方位・車速等を検出する方法である。しかし、自律航法の出力（位置・方位・車速等）には、センサーの誤差が含まれるため、誤差が生じる。特に、位置、方位はセンサー出力を積算して算出するため、誤差が徐々に累積する。一方、GPSは、最大位置誤差が通常環境で３０m程度で絶対的な位置、方位、車速を求めることができる。このため、GPS受信時に自律航法の出力を該GPSの出力に整合させることにより、累積により誤差が大きくなったときの補正が可能である。例えば、自律航法で得られた位置を周知のマップマッチング法により道路地図上の道路位置に自車位置を修正した時の位置と、GPSで得られた位置との差が所定値より大きくなった時に、道路地図上の位置をGPSで得られた位置に修正する。 The vehicle-mounted navigation device uses both autonomous navigation (Dead Reckoning) using an autonomous navigation sensor and GPS navigation using a GPS (Global Positioning System) receiver.
Autonomous navigation uses the output of an acceleration sensor that detects the acceleration of the vehicle, a relative direction sensor (such as a gyro) that detects the amount of change in the direction of the vehicle, and a distance sensor (such as a vehicle speed sensor) that detects the speed (distance) of the vehicle. This is a method for detecting the position / orientation / vehicle speed of the vehicle. However, the output of the autonomous navigation (position, direction, vehicle speed, etc.) includes an error of the sensor, and thus an error occurs. In particular, since the position and direction are calculated by integrating the sensor outputs, errors gradually accumulate. On the other hand, the GPS can obtain an absolute position, direction, and vehicle speed with a maximum position error of about 30 m in a normal environment. For this reason, it is possible to correct when the error becomes large due to accumulation by matching the output of the autonomous navigation with the output of the GPS during GPS reception. For example, the difference between the position when the position obtained by autonomous navigation is corrected to the road position on the road map by the well-known map matching method and the position obtained by GPS is larger than the predetermined value Sometimes, the position on the road map is corrected to the position obtained by GPS.

ところで、自律航法は、上述のようにGPSの出力により補正することができるが、GPS非受信時においてセンサー出力の誤差、センサー取り付け誤差により自律航法の誤差が累積し出力精度が悪くなる問題が生じる。特に、立体駐車場や地下駐車場ではGPS信号が届かないため、１００m程の最大位置誤差が発生し、また、都心部では反射したGPS信号を受けることが多く、マルチパスが発生すると、３００ｍ程の最大位置誤差が発生する。
以上より、センサー出力の誤差を修正して現在位置を求める方法が提案されている。第１従来技術（特許文献１参照）は、自律推測航法から求められる車両の位置、方位、車速の情報とGPSから出力される車両の位置、方位、車速の情報により、カルマンフィルタにて、オフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差を求めて、自律航法におけるそれぞれの補正を行う。
第２従来技術（特許文献２参照）は、車両の前後方向加速度に対応した加速度信号を出力する加速度センサーと、車両の移動距離に対応した距離信号を出力する距離センサーと、カルマンフィルタ部を備え、該カルマンフィルタ部が加速度信号および距離信号に基づいてカルマンフィルタ処理を行い、車両の姿勢角（水平面に対するピッチ角）、速度を離散時刻毎に算出し、該姿勢角を用いて傾斜走行時における位置誤差を補正する。
第１従来技術はGPS受信時に前記自律航法におけるオフセット誤差、距離係数誤差、絶対方位誤差、絶対位置誤差の補正をするものである。しかし、GPSの測位周期は１秒（１Hz）である。このため、１秒毎に上記補正を行うことになるが、補正周期が長すぎ、補正が不十分となり精度の高い位置検出ができない問題がある。また、第１従来技術は、二次元位置、二次元速度の４つのパラメータを用いるものであり、車両ピッチ角や自律航法用センサーの車両への取り付け角（車両に対する取り付けピッチ角、取り付けヨー角）の補正ができない問題がある。
第２従来技術は、三次元速度パラメータを用いて車両の姿勢角（水平面に対するピッチ角）、前後方向の速度を離散時刻毎に算出し、該姿勢角を用いて傾斜走行時における位置誤差を補正する。また、第２従来技術は、GPSの三次元位置データを用いて高さを含む位置誤差を補正する。しかし、第２従来技術の前者はGPSの三次元位置データを用いて位置誤差を補正するものではないため、誤差が累積して位置精度が落ちる問題がある。また、第２従来技術の後者はGPSから位置情報が得られる周期（１秒）毎に補正を行うことになるため、補正周期が長すぎて補正が不十分となり精度の高い位置検出ができない問題がある。また、第２従来技術は自律航法用センサーの取り付けヨー角の補正ができない問題がある。 By the way, as described above, autonomous navigation can be corrected by the output of GPS, but when GPS is not received, there is a problem that the accuracy of autonomous navigation accumulates due to errors in sensor output and sensor attachment errors, resulting in poor output accuracy. . Especially in multistory parking lots and underground parking lots, GPS signals do not reach, so a maximum position error of about 100m occurs, and reflected GPS signals are often received in the city center. When multipath occurs, about 300m. The maximum position error occurs.
From the above, a method for obtaining the current position by correcting the error of the sensor output has been proposed. The first prior art (see Patent Document 1) uses an offset error by a Kalman filter based on vehicle position, direction, and vehicle speed information obtained from autonomous dead reckoning navigation and vehicle position, direction, and vehicle speed information output from GPS. The distance coefficient error, the absolute bearing error, and the absolute position error are obtained, and each correction in the autonomous navigation is performed.
The second prior art (see Patent Document 2) includes an acceleration sensor that outputs an acceleration signal corresponding to the longitudinal acceleration of the vehicle, a distance sensor that outputs a distance signal corresponding to the moving distance of the vehicle, and a Kalman filter unit. The Kalman filter unit performs Kalman filter processing based on the acceleration signal and the distance signal, calculates the posture angle (pitch angle with respect to the horizontal plane) and speed of the vehicle for each discrete time, and uses the posture angle to calculate the position error during tilting. to correct.
The first prior art corrects offset error, distance coefficient error, absolute heading error, and absolute position error in the autonomous navigation when receiving GPS. However, the GPS positioning cycle is 1 second (1 Hz). For this reason, although the above correction is performed every second, there is a problem that the correction cycle is too long, the correction is insufficient, and the position cannot be detected with high accuracy. In addition, the first prior art uses four parameters of two-dimensional position and two-dimensional velocity, and the mounting angle of the vehicle pitch angle and the autonomous navigation sensor to the vehicle (the mounting pitch angle and the mounting yaw angle with respect to the vehicle). There is a problem that cannot be corrected.
The second conventional technique calculates the vehicle attitude angle (pitch angle with respect to the horizontal plane) and the speed in the front-rear direction at each discrete time using the three-dimensional speed parameter, and corrects the position error during the inclined running using the attitude angle. To do. In addition, the second conventional technique corrects a position error including a height using GPS three-dimensional position data. However, since the former of the second prior art does not correct the position error using the three-dimensional position data of GPS, there is a problem that the error is accumulated and the position accuracy is lowered. Further, the latter of the second prior art performs correction every period (1 second) at which position information is obtained from GPS. Therefore, the correction period is too long and correction is insufficient, so that the position cannot be detected with high accuracy. There is. In addition, the second prior art has a problem that it cannot correct the mounting yaw angle of the autonomous navigation sensor.

このため、本発明者は、精度の高い位置検出を可能にする位置検出装置及び位置検出方法を提案している(特許文献３)。この提案方法によれば、(1)第１の周期で高速に自律航法により位置計算し、(2)該第１の周期より長い周期であって、GPSの測位周期より短い周期で自律航法の位置計算に使用するパラメータ(車両速度、ピッチ角、センサー取り付け姿勢角など)を補正する第１補正処理を行ない、(3)GPSの測位周期（第３の周期）毎にGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行うことにより、精度の高い位置検出を可能にしている。
この提案方法によれば、第１周期、第２周期を短くするほど車両位置検出精度を向上することができるが、ナビゲーション制御用プロセッサ(CPU)の負荷増大し、ナビゲーション処理のための他の処理が実行できなくなる恐れがある。このため、位置検出精度を維持しつつ、CPUの負荷を軽減することが必要である。従来技術として、ナビゲーションにおける走行距離などの物理量を算出するために、短時間誤差推定手段と長時間誤差推定手段を備えた装置が提案されている(特許文献４)。しかし、この従来技術は、予め定めた時間ごとに短時間誤差推定と長時間誤差推定を行って物理量を算出するものであり、補正処理を行う処理部の負荷を軽減するための考慮、例えば補正処理に要する時間を短縮するための考慮がなされていない。
特開平８−６８６５５号公報特開２００３−７５１７２号公報特願２００７−５１１５２号特許第３５８１３９２号 For this reason, the inventor has proposed a position detection device and a position detection method that enable highly accurate position detection (Patent Document 3). According to this proposed method, (1) the position is calculated by autonomous navigation at high speed in the first period, and (2) the autonomous navigation is performed in a period longer than the first period and shorter than the positioning period of GPS. Performs first correction processing to correct parameters used for position calculation (vehicle speed, pitch angle, sensor mounting attitude angle, etc.), and (3) autonomously using GPS data for each GPS positioning cycle (third cycle) By performing the second correction process for correcting the calculation result by navigation, it is possible to detect the position with high accuracy.
According to the proposed method, the vehicle position detection accuracy can be improved as the first period and the second period are shortened. However, the load on the navigation control processor (CPU) increases, and other processes for the navigation process are performed. May not be able to run. For this reason, it is necessary to reduce the load on the CPU while maintaining the position detection accuracy. As a conventional technique, an apparatus including a short-time error estimation unit and a long-time error estimation unit has been proposed in order to calculate a physical quantity such as a travel distance in navigation (Patent Document 4). However, this conventional technique calculates a physical quantity by performing short-time error estimation and long-time error estimation at predetermined time intervals. Consideration for reducing the load on the processing unit that performs correction processing, for example, correction No consideration is given to shortening the time required for processing.
JP-A-8-68655 JP 2003-75172 A Japanese Patent Application No. 2007-51152 Japanese Patent No. 3581392

以上から、本発明の目的は、位置検出精度を維持しつつ、CPUの負荷を軽減することである。
本発明の別の目的は、位置検出精度を維持しつつ、自律航法の位置計算に使用するパラメータ(車両速度、ピッチ角、センサー取り付け姿勢角など)を補正する補正処理や自律航法演算に要する負荷を軽減することである。 From the above, an object of the present invention is to reduce the load on the CPU while maintaining the position detection accuracy.
Another object of the present invention is a load required for correction processing and autonomous navigation calculation for correcting parameters (vehicle speed, pitch angle, sensor mounting attitude angle, etc.) used for autonomous navigation position calculation while maintaining position detection accuracy. It is to reduce.

・位置検出方法
本発明の第１の態様は車両の現在位置を検出する位置検出方法である。
第１の位置検出方法は、第１の周期で高速に自律航法により位置計算するステップ、該第１の周期以上の長さの第２の周期で自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角などを補正する第１補正処理を行なうステップ、GPSの測位周期である第３の周期でGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行うステップ、補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて少なくとも前記第２周期の長さを制御するステップを有している。
第２の位置検出方法は、(1)車両の加速度や車両の方位変化量に応じた信号を出力する自律航法用センサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに車両移動距離検出部により検出された車両移動距離を用いて、自律航法部において第1の周期で緯度、経度、高さ方向の車両位置を計算すると共に、前記センサーから出力する加速度信号を用いて車両速度を計算する第１ステップ、(2)前記移動距離検出部の出力信号を用いて第１の周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した前記車両速度との速度差に基づいて、自律航法部で計算した車両速度、前記ピッチ角θ、前記センサーの取り付け姿勢角を補正する第２ステップ、(3)GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２の周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第３ステップ、(4)前記複数の補正対象のうち所定の補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて前記第２周期の長さを制御する第４ステップ、を備えている。
上記位置検出方法の前記第３ステップにおいて、カルマンフィルタ処理に基づいて前記補正対象の値を補正し、前記第４ステップにおいて該カルマンフィルタ処理の過程で演算される共分散行列の対角要素を用いて前記推定誤差を計算する。
また、上記位置検出方法の第４ステップにおいて、前記差分に基づいて前記第２周期の長さを制御すると共に、第１周期の長さを制御する。この場合、前記推定誤差が目標誤差より小さければ、前記第１、第２周期を長くし、該推定誤差が目標誤差より大きければ、前記第１、第２周期を短くする。
上記位置検出方法の第４ステップにおける前記目標誤差として、車両位置に対する目標誤差、あるいは車両速度に対する目標誤差、あるいは姿勢角に対する目標誤差を採用する。 -Position detection method The 1st mode of the present invention is a position detection method which detects the present position of vehicles.
The first position detecting method includes a step of calculating a position by autonomous navigation at a high speed in a first cycle, and a vehicle speed and pitch used for calculating a position of autonomous navigation in a second cycle having a length equal to or longer than the first cycle. A step of performing a first correction process for correcting an angle, a sensor mounting posture angle, etc., a step of performing a second correction process for correcting a calculation result by autonomous navigation using GPS data in a third period which is a positioning period of GPS, The method includes a step of calculating an estimation error to be corrected and controlling at least the length of the second period based on a difference between the estimation error and the target error.
The second position detection method is as follows: (1) The pitch angle θ, the yaw angle Y with respect to the horizontal plane of the sensor for autonomous navigation that outputs a signal according to the acceleration of the vehicle and the direction change of the vehicle, and the mounting attitude angle of the sensor with respect to the vehicle In addition, using the vehicle movement distance detected by the vehicle movement distance detection unit, the autonomous navigation unit calculates the vehicle position in the latitude, longitude, and height directions in the first cycle, and uses the acceleration signal output from the sensor. A first step of calculating the vehicle speed, and (2) calculating the vehicle speed in a second period longer than the first period by using the output signal of the movement distance detecting unit, Second step of correcting the vehicle speed calculated by the autonomous navigation unit, the pitch angle θ, and the mounting attitude angle of the sensor based on the speed difference with the vehicle speed calculated by the navigation unit, (3) Output from the GPS receiver Do A third position having a length equal to or longer than the second period is obtained using the vehicle position and vehicle speed in the degree, longitude, and height directions, and the latitude, longitude, and vehicle position and vehicle speed in the height direction output from the autonomous navigation unit. A third step of correcting the latitude, longitude, vehicle position in the height direction, vehicle speed, the pitch angle θ, the yaw angle Y, the sensor mounting posture angle, and the sensor offset value calculated by the autonomous navigation unit with a period of (4) a fourth step of calculating an estimation error of a predetermined correction object among the plurality of correction objects and controlling the length of the second period based on a difference between the estimation error and a target error. Yes.
In the third step of the position detection method, the correction target value is corrected based on Kalman filter processing, and using the diagonal elements of the covariance matrix calculated in the process of the Kalman filter processing in the fourth step, Calculate the estimation error.
In the fourth step of the position detection method, the length of the second period is controlled based on the difference, and the length of the first period is controlled. In this case, if the estimation error is smaller than the target error, the first and second periods are lengthened, and if the estimation error is larger than the target error, the first and second periods are shortened.
As the target error in the fourth step of the position detection method, a target error for the vehicle position, a target error for the vehicle speed, or a target error for the attitude angle is employed.

・位置検出装置
本発明の第２の態様は車両の現在位置を検出する位置検出装置である。
第１の位置検出装置は、車両の移動距離を測定する移動距離検出部、車両の加速度を検出する加速度センサー、車両の方位変化量に応じた信号を出力する相対方位センサー、GPS衛星からの衛星電波を受信して緯度、経度、高さ方向の車両位置および車両速度情報を出力するGPSレシーバ、第１の周期で高速に自律航法により位置計算する自律航法部、該第１の周期以上の長さの第２の周期で自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角などを補正する第１補正処理を行なう第１の補正部、GPSの測位周期である第３の周期でGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行う第２の補正部、補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて少なくとも前記第２周期の長さを制御する周期制御部、を備えている。
本発明の第２の位置検出装置は、車両の移動距離を測定する移動距離検出部、車両の加速度を検出する加速度センサー、車両の方位変化量に応じた信号を出力する相対方位センサー、GPS衛星からの衛星電波を受信して緯度、経度、高さ方向の車両位置および車両速度情報を出力するGPSレシーバ、第１の周期で、前記自律航法用のセンサーの水平面に対するピッチ角θ、ヨー角Yおよび車両に対する前記センサーの取り付け姿勢角並びに前記移動距離を用いて緯度、経度、高さ方向の車両位置を計算すると共に、前記加速度センサーから出力する加速度信号を用いて車両速度を計算する自律航法部、前記移動距離検出部の出力信号を用いて第１周期以上の長さの第２の周期で車両速度を計算し、該車両速度と前記自律航法部で計算した車両速度との速度差に基づいて前記自律航法部で計算している車両速度、前記ピッチ角θおよび前記センサーの取り付け姿勢角を補正する第１の補正部、前記GPSレシーバが出力する緯度、経度、高さ方向の車両位置と車両速度、および前記自律航法部が出力する緯度、経度、高さ方向の車両位置と車両速度を用いて、第２周期以上の長さの第３の周期で前記自律航法部が計算する緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、前記ヨー角Y、前記センサーの取り付け姿勢角およびセンサーオフセット値を補正する第２の補正部、前記複数の補正対象のうち所定の補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて前記第２周期の長さを制御する周期制御部を備えている。
前記第２の補正部は、カルマンフィルタ処理に基づいて前記補正対象の値を補正し、前記周期制御部は該カルマンフィルタ処理の過程で演算される各補正対象の共分散行列の対角要素を用いて前記推定誤差を計算する。
前記周期制御部は、前記差分に基づいて前記第２周期の長さを制御すると共に、第１周期の長さを制御する。また、前記周期制御部は、前記推定誤差が目標誤差より小さければ、前記第１、第２周期を長くし、該推定誤差が目標誤差より大きければ、前記第１、第２周期を短くする。また、前記周期制御部は前記目標誤差として、車両位置に対する目標誤差、あるいは車両速度に対する目標誤差、あるいは姿勢角に対する目標誤差を採用する。 -Position detection apparatus The 2nd mode of the present invention is a position detection apparatus which detects the present position of vehicles.
The first position detecting device includes a moving distance detecting unit that measures a moving distance of the vehicle, an acceleration sensor that detects the acceleration of the vehicle, a relative azimuth sensor that outputs a signal corresponding to the azimuth change amount of the vehicle, and a satellite from a GPS satellite. A GPS receiver that receives radio waves and outputs latitude, longitude, vehicle position and vehicle speed information in the height direction, an autonomous navigation unit that calculates positions by autonomous navigation at high speed in the first period, and a length that is longer than the first period A first correction unit that performs a first correction process for correcting the vehicle speed, pitch angle, sensor mounting posture angle, and the like used for autonomous navigation position calculation in the second period, and a third GPS positioning period. A second correction unit for performing a second correction process for correcting a calculation result by autonomous navigation using GPS data at a period; an estimation error of a correction target is calculated; and at least the first error based on a difference between the estimation error and a target error 2 cycle length And a period controller, which controls the.
A second position detection apparatus of the present invention includes a movement distance detection unit that measures the movement distance of a vehicle, an acceleration sensor that detects the acceleration of the vehicle, a relative direction sensor that outputs a signal corresponding to the amount of change in the direction of the vehicle, and a GPS satellite. A GPS receiver that receives satellite radio waves and outputs latitude, longitude, vehicle position and vehicle speed information in the height direction, pitch angle θ and yaw angle Y with respect to the horizontal plane of the sensor for autonomous navigation in the first period And an autonomous navigation unit that calculates the vehicle position in the latitude, longitude, and height directions using the mounting attitude angle of the sensor with respect to the vehicle and the moving distance, and calculates the vehicle speed using the acceleration signal output from the acceleration sensor The vehicle speed is calculated in a second period longer than the first period using the output signal of the moving distance detection unit, and the vehicle speed and the vehicle speed calculated by the autonomous navigation unit are calculated. A first correction unit that corrects the vehicle speed calculated by the autonomous navigation unit based on the speed difference, the pitch angle θ, and the mounting posture angle of the sensor, the latitude, longitude, and height direction output by the GPS receiver Using the vehicle position and vehicle speed of the vehicle and the vehicle position and vehicle speed in the latitude, longitude, and height directions output by the autonomous navigation unit, the autonomous navigation unit has a third period longer than the second period. A second correction unit that corrects the latitude, longitude, vehicle position in the height direction, vehicle speed, pitch angle θ, yaw angle Y, sensor mounting attitude angle, and sensor offset value to be calculated; A period controller that calculates an estimation error of a predetermined correction target and controls the length of the second period based on the difference between the estimation error and the target error.
The second correction unit corrects the correction target value based on Kalman filter processing, and the period control unit uses diagonal elements of each correction target covariance matrix calculated in the process of the Kalman filter processing. The estimation error is calculated.
The cycle control unit controls the length of the second cycle and the length of the first cycle based on the difference. The cycle control unit lengthens the first and second cycles if the estimation error is smaller than the target error, and shortens the first and second cycles if the estimation error is larger than the target error. Further, the cycle control unit employs a target error for the vehicle position, a target error for the vehicle speed, or a target error for the attitude angle as the target error.

本発明によれば、所定の補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて、自律航法の位置計算周期や、自律航法の位置計算に使用するパラメータの補正処理周期を制御するため、これら自律航法位置計算やパラメータ補正処理に要する負荷を軽減することができる。
また、所定の補正対象の推定誤差を算出し、該推定誤差と目標誤差の差分に基づいて、自律航法の位置計算周期や、自律航法の位置計算に使用するパラメータの補正処理周期を制御するため、位置検出精度を維持しつつ、自律航法位置計算やパラメータ補正処理に要する負荷を軽減することができる。
また、本発明によれば、カルマンフィルタ処理に基づいて前記補正対象値を補正し、該カルマンフィルタ処理の過程で演算される各補正対象の共分散行列の対角要素を用いて推定誤差を計算するため、推定誤差を計算するための処理の増加を少なくできる。
また、本発明によれば、目標誤差として車両位置に対する目標誤差あるいは車両速度に対する目標誤差あるいは姿勢角に対する目標誤差などを適宜採用することができる。 According to the present invention, an estimation error of a predetermined correction target is calculated, and based on a difference between the estimation error and a target error, a position calculation period for autonomous navigation or a correction process period for parameters used for position calculation for autonomous navigation Therefore, the load required for these autonomous navigation position calculation and parameter correction processing can be reduced.
In addition, to calculate an estimation error of a predetermined correction target, and to control the position calculation cycle of autonomous navigation and the correction processing cycle of parameters used for position calculation of autonomous navigation based on the difference between the estimation error and the target error The load required for autonomous navigation position calculation and parameter correction processing can be reduced while maintaining position detection accuracy.
Further, according to the present invention, the correction target value is corrected based on the Kalman filter processing, and the estimation error is calculated using the diagonal elements of the respective covariance matrices calculated in the course of the Kalman filter processing. Therefore, the increase in processing for calculating the estimation error can be reduced.
Further, according to the present invention, a target error for the vehicle position, a target error for the vehicle speed, a target error for the attitude angle, or the like can be appropriately employed as the target error.

(A) 本発明の概要
図１は本発明の位置検出装置の概要説明図であり、位置検出部１０および周期制御部３０を備えている。位置検出部１０において、自律航法部１２は自律センサー１１の出力信号を用いて第１の周期で高速に自律航法により車両位置を計算して出力する。補正部１５の第１補正部２１は該第１の周期より長い周期であって、GPSの測位周期より短い周期で自律航法の位置計算に使用するパラメータを補正し、第２の補正部２２はGPSの測位周期（第３の周期）毎にGPSデータを用いて自律航法による計算結果を補正する。周期制御部３０は目標誤差と推定誤差の差分に基づいて上記第１、第２の周期を制御する。
周期制御部３０において、推定誤差算出部３１は該カルマンフィルタ処理の過程で演算される共分散行列の対角要素を用い、差分演算部３２は、車両位置の該推定誤差と目標誤差(例えば１０ｍ)との差分を演算し、周期変更部３３は該差分に基づいて前記第１、第２周期を制御する。すなわち、周期変更部３３は推定誤差が目標誤差より小さければ、第１、第２周期を長くし、該推定誤差が目標誤差より大きければ、第１、第２周期を短くする。
図２は第２周期の制御例であり、最初、第２周期として１秒(１Hz)でゆっくりと第１の補正制御を行う。しかし、自律線センサーやGPSの検出精度及び車両の動き（Motion）の激しさなどにより、推定誤差が大きくなり目標誤差(＝１０m)を超えると、目標変更部３３は第２周期として０．２５秒(４Hz)で補正処理を行う。補正周期を短くしたことにより推定誤差が減少する。そして、第２周期として０．２５秒(４Hz)で補正処理を行うと検出精度が良すぎるため、目標変更部３３は第２周期として０．５秒(２Hz)で補正処理を行う。
以上のようにすれば、図２に示すように、目標誤差を満たしつつ、補正周期を長くして処理装置の負荷を減少することが可能になる。 (A) Outline of the Present Invention FIG. 1 is a schematic explanatory diagram of a position detection apparatus according to the present invention, which includes a position detection unit 10 and a cycle control unit 30. In the position detection unit 10, the autonomous navigation unit 12 calculates and outputs the vehicle position by autonomous navigation at high speed in the first cycle using the output signal of the autonomous sensor 11. The first correction unit 21 of the correction unit 15 corrects parameters used for autonomous navigation position calculation at a cycle longer than the first cycle and shorter than the GPS positioning cycle, and the second correction unit 22 The calculation result by autonomous navigation is corrected using GPS data at every GPS positioning period (third period). The cycle control unit 30 controls the first and second cycles based on the difference between the target error and the estimation error.
In the cycle control unit 30, the estimation error calculation unit 31 uses diagonal elements of the covariance matrix calculated in the process of the Kalman filter process, and the difference calculation unit 32 uses the estimation error and target error (for example, 10 m) of the vehicle position. And the period changing unit 33 controls the first and second periods based on the difference. That is, the period changing unit 33 lengthens the first and second periods if the estimation error is smaller than the target error, and shortens the first and second periods if the estimation error is larger than the target error.
FIG. 2 shows a control example of the second period. First, the first correction control is performed slowly in 1 second (1 Hz) as the second period. However, if the estimation error becomes large and exceeds the target error (= 10 m) due to the detection accuracy of the autonomous line sensor or GPS and the intensity of the motion of the vehicle, the target changing unit 33 sets 0.25 as the second period. Correction processing is performed in seconds (4 Hz). The estimation error is reduced by shortening the correction period. If the correction process is performed at 0.25 seconds (4 Hz) as the second period, the detection accuracy is too good. Therefore, the target changing unit 33 performs the correction process at 0.5 seconds (2 Hz) as the second period.
As described above, as shown in FIG. 2, it is possible to lengthen the correction cycle and reduce the load on the processing apparatus while satisfying the target error.

(B)位置検出部
図３は本発明を適用できる位置検出部１０の詳細な構成図である。この位置検出部には、自律航法用センサーとして、車両の移動距離を測定する移動距離検出部、たとえば車両が所定距離走行する毎に１個のパルスを発生する車速センサー１１a、車両の方位変化量に応じた信号を出力する相対方位センサーであるジャイロ１１b、車両の加速度を検出する加速度センサー１１cが設けられている。車速センサー１１aは車輪に取り付けられ、ジャイロ１１bおよび加速度センサー１１cは一体にダッシュボードの所定位置に装着される。ジャイロ１１bおよび加速度センサー１１cは、側面から見たとき、車両方向と平行して車両に取り付けられるのが理想であるが、図４（Ａ）に示すように取り付け誤差があり、センサー方向は車両方向に角度A（取り付けピッチ角）を成して取り付けられる。なお、水平方向とセンサー方向の角度θをピッチ角といい、ピッチ角は傾斜角と取り付けピッチ角の和である。また、ジャイロ１１bおよび加速度センサー１１cは、平面に投影したとき、車両方向と一致して車両に取り付けられるのが理想であるが、取り付け誤差があり、図４（Ｂ）に示すように、センサー方向は車両方向に角度A2（取り付けヨー角）を成して取り付けられる。なお、北方向とセンサー方向の角度Ｙをヨー角といい、ヨー角Ｙは車両方向と取り付けヨー角の和である。 (B) Position Detection Unit FIG. 3 is a detailed configuration diagram of the position detection unit 10 to which the present invention can be applied. The position detection unit includes a movement distance detection unit that measures the movement distance of the vehicle as an autonomous navigation sensor, for example, a vehicle speed sensor 11a that generates one pulse every time the vehicle travels a predetermined distance, and a direction change amount of the vehicle. A gyro 11b, which is a relative orientation sensor that outputs a signal corresponding to the above, and an acceleration sensor 11c that detects the acceleration of the vehicle are provided. The vehicle speed sensor 11a is attached to the wheel, and the gyro 11b and the acceleration sensor 11c are integrally mounted at a predetermined position on the dashboard. The gyro 11b and the acceleration sensor 11c are ideally attached to the vehicle in parallel with the vehicle direction when viewed from the side, but there is an attachment error as shown in FIG. 4A, and the sensor direction is the vehicle direction. Is attached at an angle A (mounting pitch angle). The angle θ between the horizontal direction and the sensor direction is referred to as a pitch angle, and the pitch angle is the sum of an inclination angle and a mounting pitch angle. Further, the gyro 11b and the acceleration sensor 11c are ideally attached to the vehicle in accordance with the vehicle direction when projected onto a plane, but there is an attachment error, and as shown in FIG. Is mounted at an angle A2 (mounting yaw angle) in the vehicle direction. The angle Y between the north direction and the sensor direction is referred to as a yaw angle, and the yaw angle Y is the sum of the vehicle direction and the mounting yaw angle.

自律航法部１２は、各自律センサーからの出力信号を用いて高速度で、たとえば25Hzの周期で前後方向の車両速度Vsp(k)、車両の３次元位置（緯度方向距離N(k)、経度方向距離E(k)、下方向距離D（ｋ））を計算して出力する。図５は加速度センサー１１cから出力する加速度信号を用いて車両速度Vsp(k)を計算する方法の説明図である。車両CARには鉛直方向に重力加速度Gが加わっており、取り付けピッチ角Ａが０の場合、（Ａ）に示すように、その傾斜方向成分G0は
G0＝G×sinβ
である。したがって、加速度センサー１１cが測定する加速度Accは、車両の移動に伴う進行方向の加速度G1と重力の傾斜方向成分の和となり、次式
Acc= G×sinβ＋G1
で表現できる。取り付けピッチ角Ａが０でない場合、（Ｂ）に示すように、加速度センサー１１ｃはピッチ角θ（＝β＋Ａ）方向の加速度Accが測定される。したがって、（C）に示すようにピッチ角方向の重力加速度成分はG×sinθとなり、ピッチ角方向の車両移動に伴う加速度成分はG1×cosAとなり、次式
Acc= G×sinθ+ G1×cosA
が成立し、取り付けヨー角A2を考慮すると、次式
Acc= G×sinθ+ G1×cosA×cosA2
が成立する。この結果、傾斜方向の加速度G1は次式
G1=( Acc−G×sinθ)/ (cosA×cosA2） (1)
で表現できる。加速度測定周期をT1とすれば、変化速度ΔVは次式
ΔV=T1×( Acc−G×sinθ)/ (cosA×cosA2)
で与えられる。したがって、速度Vsp(k+1)は1つ前の離散時刻kにおける速度Vsp(k)とΔVより
Vsp(k+1)＝Vsp(k)＋T１×( Acc−G×sinθ)/ (cosA×cosA2) （2）
で与えられる。尚、加速度Accのオフセットをα_OFとすれば加速度センサーの出力信号Accからα_OFを差し引いた値をAccとして(２)式の演算を行う。すなわち、
Acc=Acc−α_OF
とする。 The autonomous navigation unit 12 uses an output signal from each autonomous sensor at a high speed, for example, a vehicle speed Vsp (k) in the front-rear direction at a period of 25 Hz, a three-dimensional position of the vehicle (latitude direction distance N (k), longitude The direction distance E (k) and the downward distance D (k)) are calculated and output. FIG. 5 is an explanatory diagram of a method for calculating the vehicle speed Vsp (k) using the acceleration signal output from the acceleration sensor 11c. When the vehicle CAR is subjected to gravitational acceleration G in the vertical direction and the mounting pitch angle A is 0, as shown in FIG.
G0 ＝ G × sinβ
It is. Accordingly, the acceleration Acc measured by the acceleration sensor 11c is the sum of the acceleration G1 in the traveling direction accompanying the movement of the vehicle and the component of the inclination direction of gravity.
Acc = G × sinβ + G1
Can be expressed as When the mounting pitch angle A is not 0, the acceleration sensor 11c measures the acceleration Acc in the direction of the pitch angle θ (= β + A) as shown in FIG. Therefore, as shown in (C), the gravitational acceleration component in the pitch angle direction is G × sinθ, and the acceleration component accompanying the vehicle movement in the pitch angle direction is G1 × cosA.
Acc = G × sinθ + G1 × cosA
If the mounting yaw angle A2 is considered,
Acc = G × sinθ + G1 × cosA × cosA2
Is established. As a result, the acceleration G1 in the tilt direction is
G1 = (Acc−G × sinθ) / (cosA × cosA2) (1)
Can be expressed as If the acceleration measurement cycle is T1, the change rate ΔV is expressed by the following equation: ΔV = T1 x (Acc-G x sin θ) /
Given in. Therefore, the speed Vsp (k + 1) is calculated from the speed Vsp (k) and ΔV at the previous discrete time k.
Vsp (k + 1) = Vsp (k) + T1 x (Acc-G x sinθ) / (cosA x cosA2) (2)
Given in. If the offset of the acceleration Acc is α _OF , the calculation of equation (2) is performed with the value obtained by subtracting α _OF from the output signal Acc of the acceleration sensor as Acc. That is,
Acc = Acc−α _OF
And

また、自律航法部１２は、車両の３次元位置（緯度N(k)、経度E(k)、高さD（ｋ））を次式により計算して出力する。
N(k+1)= N(k) +S(cosθcosY cosAcosA2＋sinY sinA2＋sinθcosY sinAcosA2)
E(k+1)= E(k) +S(cosθsinY cosAcosA2−cosY sinA2＋sinθsinY sinAcosA2)
D(k+1)= D(k) +S(−sinθcosAcosA2＋cosθ sinAcosA2) (3)
ただし、S=（サンプル時間T1あたりの車速パルス数×パルス間距離）
=車がサンプル時間当たりに車両方向に進んだ距離
であり、４つの角度（θ、A、Y、A2）を使って、SをN-E-D座標系(North−East−Down座標系）に投影している。 In addition, the autonomous navigation unit 12 calculates and outputs the three-dimensional position (latitude N (k), longitude E (k), height D (k)) of the vehicle according to the following expression.
N (k + 1) = N (k) + S (cosθcosY cosAcosA2 + sinY sinA2 + sinθcosY sinAcosA2)
E (k + 1) = E (k) + S (cosθsinY cosAcosA2−cosY sinA2 ++ sinθsinY sinAcosA2)
D (k + 1) = D (k) + S (−sinθcosAcosA2 + cosθ sinAcosA2) (3)
However, S = (number of vehicle speed pulses per sample time T1 x distance between pulses)
= The distance the car has traveled in the direction of the vehicle per sample time, and project S to the NED coordinate system (North-East-Down coordinate system) using four angles (θ, A, Y, A2) Yes.

速度計算部１３は、所定のサンプル時間T2(たとえば10Hzの周期に相当する0.1秒)で車速センサー１１aから出力するパルス数Nと1パルスあたりの移動距離Lを用いて次式
Vx＝N×L／T2 (4)
により車速度を計算する。
GPSレシーバ１4はGPS測位周期、たとえば1秒間隔でGPS衛星から受信した信号に基づいて三次元位置（緯度、経度、高さ）、三次元速度(北方向速度、東方向速度、上下方向速度)を計算して出力する。 The speed calculation unit 13 uses the number N of pulses output from the vehicle speed sensor 11a at a predetermined sample time T2 (for example, 0.1 second corresponding to a period of 10 Hz) and the movement distance L per pulse as
Vx ＝ N × L / T2 (4)
Calculate the vehicle speed by
The GPS receiver 14 is based on a GPS positioning cycle, for example, a signal received from a GPS satellite at 1 second intervals, 3D position (latitude, longitude, height), 3D speed (north speed, east speed, vertical speed) Is calculated and output.

カルマンフィルタ部１５は、ジャイロオフセット補正部２０と第１の補正部２１と第２の補正部２２を備えている。
ジャイロオフセット補正部２０は、速度Vxが零（即ち、停車中）の時、停車中における角速度信号が[オフセット＋ノイズ]であることを利用し、該角速度信号出力ω_OFと自律航法部１２で計算する角速度信号オフセットとの差をとり、後述のカルマンフィルタ処理により角速度信号オフセットω_OFの補正を短時間で行う。
自律航法部１２はジャイロ１１bの出力信号を用いて計測した角速度信号ωから方位変化Δω(k）を次式
Δω（k）＝（ω−ω_OF）×T1
により計算し、また周知の慣性航法システムの技術から導出される次式に基づいてピッチ角θとヨー角Yを求めて更新する。
ｃ₀₀=cosθ(k+1) ×cosY(k+1) =−sinY(k)× Δω（ｋ）
ｃ₁₀=cosθ(k+1) ×sinY(k+1) = cosY(k) ×Δω（ｋ） (5)
なお、自律航法部１２はセンサー取り付けピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットω_OF、加速度信号オフセットα_OFについては次式により、
A(k+1)=A(k)
A2(k+1)=A2(k)
ω_OF(k+1)=ω_OF(k)
α_OF(k+1)=α_OF(k） (6)
補正されるまで一定とする。 The Kalman filter unit 15 includes a gyro offset correction unit 20, a first correction unit 21, and a second correction unit 22.
Gyro offset correcting unit 20, the speed Vx is zero (i.e., the vehicle is stopped) when the angular velocity signal during stopping is utilized that the Offset + Noise, the angular velocity signal output omega _OF autonomous navigation unit 12 The difference from the calculated angular velocity signal offset is taken, and the angular velocity signal offset ω _OF is corrected in a short time by the Kalman filter processing described later.
The autonomous navigation unit 12 calculates an azimuth change Δω (k) from the angular velocity signal ω measured using the output signal of the gyro 11b as follows: Δω (k) = (ω−ω _OF ) × T1
Further, the pitch angle θ and the yaw angle Y are obtained and updated based on the following equations derived from the known inertial navigation system technology.
c ₀₀ = cosθ (k + 1) × cosY (k + 1) = − sinY (k) × Δω (k)
c ₁₀ = cosθ (k + 1) × sinY (k + 1) = cosY (k) × Δω (k) (5)
The autonomous navigation unit 12 uses the following equations for the sensor mounting pitch angle A, sensor mounting yaw angle A2, angular velocity signal offset ω _OF , and acceleration signal offset α _OF :
A (k + 1) = A (k)
A2 (k + 1) = A2 (k)
ω _OF (k + 1) = ω _OF (k)
α _OF (k + 1) = α _OF (k) (6)
Keep constant until corrected.

カルマンフィルタ部の第１の補正部２１は、第１の周期(たとえば１０Hz周期)で第1のカルマンフィルタ処理を行う。第1のカルマンフィルタ処理において第１の補正部２１は、速度計算部１３が計算した車両速度Vxと自律航法部１２が計算した車両速度Vspとの差に基づいて、該自律航法部で計算している車両速度Vsp、ピッチ角θ、センサー取り付けピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットω_OF、加速度信号オフセットα_OFを補正する。
カルマンフィルタ部の第２の補正部２２は、第１の周期より長い第２の周期(たとえば1Hz周期)でGPSレシーバ１４が出力する三次元の車両位置と三次元の車両速度、ならびに自律航法部１２が出力する三次元の車両位置と三次元の車両速度を用いて、該自律航法部で計算している緯度、経度、高さ方向の車両位置、車両速度、前記ピッチ角θ、センサー取り付けピッチ角A、前記ヨー角Y、センサー取り付けヨー角A2、角速度信号オフセットω_OF、加速度信号オフセットα_OF（自律航法で計算している全てのパラメータ）を補正する。なお、第１、第２の補正部２１，２２によるカルマンフィルタ処理の詳細は後述する。
自律航法部１２は第１の補正部２１により１０Hz周期で更新されるピッチ角θ、車両取り付けピッチ角A、センサー取り付けヨー角A2を用いて車両速度や車両位置を（2）、（3）式により演算し、また、第２の補正部２２により1Hz周期で更新されるピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2を用いて車両速度や車両位置を（2）、（3）式により演算して出力する。 The first correction unit 21 of the Kalman filter unit performs the first Kalman filter process at a first period (for example, a 10 Hz period). In the first Kalman filter processing, the first correction unit 21 calculates the autonomous navigation unit based on the difference between the vehicle speed Vx calculated by the speed calculation unit 13 and the vehicle speed Vsp calculated by the autonomous navigation unit 12. Vehicle speed Vsp, pitch angle θ, sensor mounting pitch angle A, sensor mounting yaw angle A2, angular velocity signal offset ω _OF , and acceleration signal offset α _OF are corrected.
The second correction unit 22 of the Kalman filter unit includes a three-dimensional vehicle position and a three-dimensional vehicle speed output from the GPS receiver 14 in a second period (for example, 1 Hz period) longer than the first period, and the autonomous navigation unit 12. Using the three-dimensional vehicle position and three-dimensional vehicle speed output from the vehicle, the latitude, longitude, vehicle position in the height direction, vehicle speed, pitch angle θ, sensor mounting pitch angle calculated by the autonomous navigation unit A, the yaw angle Y, the sensor mounting yaw angle A2, the angular velocity signal offset ω _OF , and the acceleration signal offset α _OF (all parameters calculated by autonomous navigation) are corrected. Details of the Kalman filter processing by the first and second correction units 21 and 22 will be described later.
The autonomous navigation unit 12 uses the pitch angle θ, the vehicle mounting pitch angle A, and the sensor mounting yaw angle A2 that are updated by the first correction unit 21 in a cycle of 10 Hz to calculate the vehicle speed and the vehicle position (2) and (3) Further, the vehicle speed and the vehicle position are calculated by using the pitch angle θ, the vehicle mounting pitch angle A, the yaw angle Y, and the vehicle mounting yaw angle A2 that are updated by the second correction unit 22 in a cycle of 1 Hz (2). , (3) Calculate and output.

（C）位置検出部の処理
図６は図３の位置検出部１０の全体の処理フローである。
はじめに、自律航法部１２に３次元車両位置N、E、D、車両速度Vsp、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、ジャイロ１１ｂのオフセットω_OF、加速度センサーのオフセットα_OFの初期値を設定する（ステップ１０１）。以後、自律航法部１２は車速センサー１１ａ、ジャイロ１１ｂ、加速度センサー１１ｃの出力を取り込み（ステップ１０２）、第１周期（２５Hz周期）で（2）、（3）、(5)式の演算を行って車両速度Vsp（k+1）及び車両の３次元位置（緯度N(k+1)、経度E(k+1)、高さD(k+1)、ピッチ角θとヨー角Yに関する2つの値：
cosθ(k+1) ×cosY(k+1)、
cosθ(k+1) ×sinY(k+1)
を演算して出力する(ステップ１０３)。ついで、第２周期(１０Hz周期)になったかチェックし(ステップ１０４)、第２周期になっていなければ、ステップ１０２以降の処理を繰り返す。
第２周期になっていれば、車両速度Vxが零である状態が2秒以上続いていたかどうかによって停車判定を行う（ステップ１０５）。
停車中でなければ、第３周期(1Hz周期=GPS測位周期)になっているかチェックし（ステップ１０６）、第３周期でなければカルマンフィルタ１５の第1補正部２１は速度計算部１３が(4)式により計算した車両速度Vxと自律航法部１２が(2)式より計算した車両速度Vsp（ｋ）を用いてカルマンフィルタ処理により車両速度、前記ピッチ角θ、センサー取りつけピッチ角A、センサー取り付けヨー角A2、角速度信号オフセットω_OF、加速度信号オフセットα_OFを補正する（ステップ１０７）。このステップ１０７では後述するカルマンフィルタの観測行列H1を用いた第１補正処理が行われる。
ステップ１０６において、第３周期であればカルマンフィルタ１５の第２補正部２２はGPSレシーバ１４が出力する３次元の車両位置（N_GPS、E_GPS、D_GPS)と３次元の車両速度（VN_GPS、VE_GPS、VD_GPS）を用いて、車両位置、車両速度、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、角速度信号オフセットω_OF、加速度信号オフセットα_OFを補正する(ステップ１０８)。このステップ１０８では後述するカルマンフィルタの観測行列H2を用いた第２補正処理が行われる。
ステップ１０５において、停車中であれば、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１０９）、第３周期でなければカルマンフィルタ１５の第１補正部２１は前記ステップ１０７の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１０）。このステップ１１０では後述するカルマンフィルタの観測行列H3を用いた第３補正処理が行われる。
ステップ１０９において、第３周期であれば、カルマンフィルタ１５の第２補正部２２は前記ステップ１０８の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１）。このステップ１１１では後述するカルマンフィルタの観測行列H4を用いた第４補正処理が行われる。（ステップ１１１） (C) Processing of Position Detection Unit FIG. 6 is an overall processing flow of the position detection unit 10 of FIG.
First, the three-dimensional vehicle position N, E, D, vehicle speed Vsp, pitch angle θ, vehicle mounting pitch angle A, yaw angle Y, vehicle mounting yaw angle A2, gyro 11b offset ω _OF , acceleration sensor The initial value of the offset α _OF is set (step 101). Thereafter, the autonomous navigation unit 12 takes in the outputs of the vehicle speed sensor 11a, the gyro 11b, and the acceleration sensor 11c (step 102), and performs calculations of equations (2), (3), and (5) in the first period (25 Hz period). Vehicle speed Vsp (k + 1) and the three-dimensional position of the vehicle (latitude N (k + 1), longitude E (k + 1), height D (k + 1), pitch angle θ and yaw angle Y One value:
cosθ (k + 1) x cosY (k + 1),
cosθ (k + 1) × sinY (k + 1)
Is calculated and output (step 103). Next, it is checked whether the second cycle (10 Hz cycle) has been reached (step 104). If the second cycle has not been reached, the processing from step 102 onward is repeated.
If it is in the second period, the stoppage determination is performed based on whether or not the state where the vehicle speed Vx is zero continues for 2 seconds or more (step 105).
If the vehicle is not stopped, it is checked whether it is in the third period (1 Hz period = GPS positioning period) (step 106). If it is not in the third period, the first correction unit 21 of the Kalman filter 15 uses the speed calculation unit 13 (4 ) And vehicle speed Vx calculated by equation (2) and the vehicle speed Vsp (k) calculated by equation (2) by the autonomous navigation unit 12 by the Kalman filter process, the vehicle speed, the pitch angle θ, the sensor mounting pitch angle A, the sensor mounting yaw The angle A2, the angular velocity signal offset ω _OF , and the acceleration signal offset α _OF are corrected (step 107). In step 107, a first correction process using a Kalman filter observation matrix H1 described later is performed.
In step 106, if it is the third period, the second correction unit 22 of the Kalman filter 15 outputs the three-dimensional vehicle position (N _GPS , E _GPS , D _GPS ) output from the GPS receiver 14 and the three-dimensional vehicle speed (VN _GPS , VE _GPS , VD _GPS ) to correct vehicle position, vehicle speed, pitch angle θ, vehicle mounting pitch angle A, yaw angle Y, vehicle mounting yaw angle A2, angular velocity signal offset ω _OF , acceleration signal offset α _OF (Step 108). In this step 108, a second correction process using a Kalman filter observation matrix H2 described later is performed.
In step 105, if the vehicle is stopped, it is checked whether the third period (1 Hz period = GPS positioning period) is reached (step 109). If it is not the third period, the first correction unit 21 of the Kalman filter 15 performs step 107. And an angular velocity offset correction based on the difference between the gyro angular velocity output signal and the angular velocity signal offset calculated by the autonomous navigation unit 12 (step 110). In step 110, a third correction process using an observation matrix H3 of a Kalman filter, which will be described later, is performed.
In step 109, if it is the third period, the second correction unit 22 of the Kalman filter 15 performs the correction process of step 108 and determines the difference between the angular velocity output signal of the gyro and the angular velocity signal offset calculated by the autonomous navigation unit 12. Based on this, angular velocity offset correction is performed (step 111). In step 111, a fourth correction process using an observation matrix H4 of a Kalman filter, which will be described later, is performed. (Step 111)

以上の位置検出処理によれば、GPSによる推定誤差補正より早い周波数で、第１補正部２１が誤差累積の補正を行うため精度の高い位置検出ができる。図７はGPS受信時とGPS非受信時における位置検出誤差の説明図であり、比較のために第１補正処理をしない場合（従来技術という）の位置検出誤差も示している。図３の位置検出部によれば、GPS受信時に第１の補正部２１は１０Hz周期でピッチ角、センサー取り付けピッチ角、センサー取り付けヨー角の補正を行い、第２の補正部２１は１Hz周期(GPS測位周期)で補正を行うため、誤差の累積を小さくできる。なお、１Hz周期(GPS測位周期)でGPS測位データを用いて補正処理する従来技術では、１Hz周期で累積誤差がリセットされるがその間の累積誤差が大きくなる。また、図３の位置検出部によれば、GPS非受信時でも第１の補正部２１は１０Hz周期でピッチ角、センサー取り付けピッチ角、センサー取り付けヨー角の補正を行うため、誤差の累積度合を小さくできる。しかし、GPS測位データでのみ補正処理する従来技術では補正ができず、誤差の累積度合が大きく誤差が大きくなる。 According to the above position detection process, the first correction unit 21 performs error accumulation correction at a frequency faster than the estimated error correction by GPS, so that highly accurate position detection can be performed. FIG. 7 is an explanatory diagram of a position detection error when GPS is received and when GPS is not received, and also shows a position detection error when the first correction process is not performed (referred to as the prior art) for comparison. According to the position detection unit of FIG. 3, the first correction unit 21 corrects the pitch angle, sensor mounting pitch angle, and sensor mounting yaw angle in a 10 Hz cycle during GPS reception, and the second correction unit 21 performs a 1 Hz cycle ( Since correction is performed at the GPS positioning cycle, the error accumulation can be reduced. In the prior art in which correction processing is performed using GPS positioning data at a 1 Hz period (GPS positioning period), the accumulated error is reset at a 1 Hz period, but the accumulated error during that period increases. Further, according to the position detection unit of FIG. 3, the first correction unit 21 corrects the pitch angle, the sensor mounting pitch angle, and the sensor mounting yaw angle in a 10 Hz cycle even when GPS is not received. Can be small. However, the correction cannot be performed by the conventional technique in which correction processing is performed only with GPS positioning data, and the error accumulation degree is large and the error becomes large.

図８はGPS受信が不可能な都庁の立体駐車場をぐるぐる回って出てきたときの車両の走行軌跡を示すもので、(A)は図３の位置検出部をナビゲーションシステムに適用した場合の走行軌跡、（B）はマップマッチング機能を備えた従来のナビゲーションシステムの走行軌跡である。図３の位置検出部を備えたナビゲーションシステムによれば、GPSの届かない立体駐車場内での方向ずれが小さく、しかも立体駐車場出口での方向ずれが小さく、更には、自律航法の精度が高くGPSマルチパスが起きても位置精度の劣化が少ない。しかし、従来技術では、GPSの届かない立体駐車場内での方向ずれが大きく、しかも立体駐車場出口での方向ずれが大きく、更には、GPSマルチパスが起きると間違った道路にマップマッチングしてしまう。
図９は地下の立体駐車場内における図３の位置検出部を備えたナビゲーションシステムによる走行軌跡拡大図であり、In方向から進入して地下に降り、しかる後地下で数回周回して一階に戻りOut方向から駐車場より出た場合の走行軌跡を示している。図９より図３の位置検出部を備えたナビゲーションシステムによれば、高さ方向変化(ピッチ角、高さ位置)も正確にトラックでき、地下階層が判断できる。 Fig. 8 shows the driving trajectory of the vehicle when it goes around the multi-storey parking lot of the Tokyo Metropolitan Government where GPS reception is impossible. (A) shows the case where the position detection unit of Fig. 3 is applied to the navigation system. A traveling locus, (B), is a traveling locus of a conventional navigation system having a map matching function. According to the navigation system provided with the position detection unit of FIG. 3, the direction deviation in the multilevel parking area where GPS does not reach is small, the direction deviation at the multilevel parking area exit is small, and the accuracy of autonomous navigation is high. Even if GPS multipath occurs, there is little degradation of position accuracy. However, in the conventional technology, the direction deviation in the multilevel parking lot where GPS does not reach is large, the direction deviation at the multilevel parking lot exit is also large, and furthermore, if GPS multipath occurs, map matching is done on the wrong road .
FIG. 9 is an enlarged view of the travel locus by the navigation system having the position detection unit of FIG. 3 in the underground parking lot. The vehicle enters from the In direction and descends to the basement, and then laps several times in the basement to the first floor. The travel locus when exiting the parking lot from the return direction is shown. According to the navigation system provided with the position detection unit of FIG. 3 from FIG. 9, the height direction change (pitch angle, height position) can be accurately tracked, and the underground level can be determined.

（Ｄ）カルマンフィルタ処理
カルマンフィルタ処理は各時刻における予測値と観測値との誤差を修正しながら、各時刻における最適な推定値を逐次求める方法である。カルマンフィルタ処理においては、予め、ある値を予測するための算出式を設定し、この算出式を用いて観測値が得られる時刻ｎまで予測を繰り返す。時刻nで観測値が取得できれば、該観測値を用いて時刻ｎでの推定値について確率論的に定義された誤差を最小化させるような推定値補正計算を行う。
図１０はカルマンフィルタ処理の概要説明図である。カルマンフィルタにおいては、図１０に示すように、信号生成過程３１と観測過程４１に分けられる。図において、線形システムFがあり、そのシステムの状態をX(t)とするとき、観測行列Hを介してX(t)の一部が観測できる場合、カルマンフィルタはX(t)の最適な推定値を与える。ここで、ｗは信号生成過程にて発生する雑音であり、ｖは観測過程にて発生する雑音である。カルマンフィルタは、入力をZ(t)としてカルマン処理を所定周期で繰り返し実行することにより、最適推定値X(t)を求める。 (D) Kalman Filter Processing Kalman filter processing is a method for successively obtaining an optimum estimated value at each time while correcting an error between a predicted value and an observed value at each time. In the Kalman filter process, a calculation formula for predicting a certain value is set in advance, and the prediction is repeated until time n at which an observation value is obtained using this calculation formula. If the observed value can be acquired at time n, an estimated value correction calculation is performed using the observed value so as to minimize an error defined stochastically for the estimated value at time n.
FIG. 10 is a schematic explanatory diagram of Kalman filter processing. The Kalman filter is divided into a signal generation process 31 and an observation process 41 as shown in FIG. In the figure, when there is a linear system F and the state of the system is X (t), if a part of X (t) can be observed through the observation matrix H, the Kalman filter is an optimal estimate of X (t) Give value. Here, w is noise generated in the signal generation process, and v is noise generated in the observation process. The Kalman filter obtains the optimum estimated value X (t) by repeatedly executing the Kalman process at a predetermined cycle with the input as Z (t).

本発明のカルマンフィルタ処理におけるシステムモデルの状態式は次式
δX（k+１）＝F（k）δX（k）＋w(k） (7)
で表され、システム状態変数δXは
δX＝[δN、δE、δD、δVbx、δc₀₀、δc₁₀、δc₂₀、δp₀₀、δp₁₀、δp₂₀、bwz、bax]
である。但し、Vbx=Vsp((２)式参照)、bwz=ω_OF、bax=α_OFとしている。また、c₀₀〜p₂₀のパラメータは座標変換行列の要素で、
ｃ₀₀＝cosθcosY
ｃ₁₀＝cosθsinY
ｃ₂₀=−sinθ
ｐ₀₀＝cosAcosA2
ｐ₁₀＝cosAsinA2
p20＝−sinA
である。(7)式における線形システムFは(２)、(３)、(５)式のシステムモデルを表す式より図１１に示す行列で表現でき、太枠内が行列要素である。なお、c_ijはセンサー座標系からN−E−D座標系への座標変換行列要素、p_ijはセンサー座標系から車両固定座標系への座標変換行列要素であり、それぞれ次式により表現される。

また、本発明のカルマンフィルタの観測式は
δZ(k)=H(k)δX(k)+v(k) (8)
で表される。(8)式の観測行列Hは図１２に示す行列で表される。図１２において、観測行列Hの行列部分(1)は10Hz周期での速度誤差δVbxを計算する部分、(2)は10Hz周期での停車時における角速度オフセット誤差bwzを計算する部分、(3)は1Hz周期でのGPSの車両位置誤差δN,δE,δDおよび車両速度誤差δvnx,δvny,δvnzを計算する部分である。なお、観測行列Hは

と表現する。
カルマンフィルタは、Z(t) が観測できるタイミングで(8)式によりZ(t)(=δZ(t))を計算し、計算した値と観測された値の差に基づいてX(t)(＝δ（X）)を推定し、以後、次のZ（t)が観測されるまでX(t)を(7)式により更新する。そして、Z（t)が観測されると上記差を再び演算し、該差に基づいてX(t)(＝δ（X）)を推定し、以後、同様の処理を繰り返す。 The state equation of the system model in the Kalman filter processing of the present invention is the following equation: δX (k + 1) = F (k) δX (k) + w (k) (7)
In expressed, the system state variable [delta] X is δX = [δN, δE, δD , δVbx, δc 00, δc 10, δc 20, δp 00, δp 10, δp 20, bwz, bax]
It is. However, Vbx = Vsp (see equation (2)), bwz = ω _OF , and bax = α _OF . The parameters c _{00 to} p ₂₀ are elements of the coordinate transformation matrix,
c ₀₀ = cosθcosY
c ₁₀ = cosθsinY
c ₂₀ = −sinθ
p ₀₀ = cosAcosA2
p ₁₀ = cosAsinA2
p20 = −sinA
It is. The linear system F in the equation (7) can be expressed by the matrix shown in FIG. 11 from the equations representing the system models of the equations (2), (3), and (5), and the inside of the thick frame is a matrix element. C _ij is a coordinate transformation matrix element from the sensor coordinate system to the N-E-D coordinate system, and p _ij is a coordinate transformation matrix element from the sensor coordinate system to the vehicle fixed coordinate system. .

The observation formula of the Kalman filter of the present invention is δZ (k) = H (k) δX (k) + v (k) (8)
It is represented by The observation matrix H in equation (8) is represented by the matrix shown in FIG. In FIG. 12, the matrix part (1) of the observation matrix H is a part for calculating a speed error δVbx at a 10 Hz period, (2) is a part for calculating an angular speed offset error bwz when the vehicle is stopped at a 10 Hz period, and (3) is This is a part for calculating GPS vehicle position errors ΔN, ΔE, ΔD and vehicle speed errors Δvnx, Δvny, Δvnz in a 1 Hz cycle. The observation matrix H is

It expresses.
The Kalman filter calculates Z (t) (= δZ (t)) according to Eq. (8) at the timing when Z (t) can be observed, and X (t) (based on the difference between the calculated value and the observed value. = Δ (X)), and thereafter, X (t) is updated by equation (7) until the next Z (t) is observed. Then, when Z (t) is observed, the above difference is calculated again, X (t) (= δ (X)) is estimated based on the difference, and thereafter the same processing is repeated.

観測行列Hの行列部分(1)は図６の処理ステップ１０７の第１補正処理において使用するカルマンフィルタの観測行列H1を構成し、

である。また、観測行列Hの行列部分(1)、（3）は図６の処理ステップ１０８の第２補正処理において使用するカルマンフィルタの観測行列H2を構成し、

である。また、観測行列Hの行列部分(1)、（2）は図６の処理ステップ１１０の第３補正処理において使用するカルマンフィルタの観測行列H3を構成し、

である。また、観測行列Hの行列部分(1)、（2）、(3)は図6の処理ステップ１１１の第４補正処理において使用するカルマンフィルタの観測行列H4を構成し、

である。 The matrix portion (1) of the observation matrix H constitutes the observation matrix H1 of the Kalman filter used in the first correction processing of the processing step 107 in FIG.

It is. Moreover, the matrix parts (1) and (3) of the observation matrix H constitute the observation matrix H2 of the Kalman filter used in the second correction process of the processing step 108 in FIG.

It is. Further, the matrix parts (1) and (2) of the observation matrix H constitute the observation matrix H3 of the Kalman filter used in the third correction process in the processing step 110 of FIG.

It is. Moreover, the matrix parts (1), (2), and (3) of the observation matrix H constitute the observation matrix H4 of the Kalman filter used in the fourth correction process in the processing step 111 of FIG.

It is.

カルマンフィルタは、入力をZ(t)(=δZ(t))として以下の(9)式を所定周期（Z(t)の入力周期）で繰り返し実行することにより、最適推定値X(t｜t)(=δX(t｜t))を求める。ただし、時刻ｊまでの情報に基づく時刻iでのAの推定値をA(i｜j)と表記する。
X(t｜t)＝X(t｜t−1)＋K(t)[Z(t)−HX(t｜t−1)] (9)
ここでX(t｜t−1)は事前推定値、K(t)はカルマンゲインであり、それぞれ
X(t｜t−1)＝FX(t−1｜t−1) (9)′
K(t)＝P(t｜t−1)H^T(HP(t｜t−1)HT＋V)^-1
と表現でき、事前推定値X(t｜t−1)は、Z(t)の入力周期より短い周期で(9)′式により更新される。
また、Pは状態量Xの誤差共分散行列であり、P(t｜t−1)は誤差共分散の予測値、P(t−1｜t−1)は誤差共分散であり、それぞれ
P(t｜t−1)＝FP(t−1｜t−1)F^T＋W
P(t−1｜t−1)＝(I−K(t−1)H)P(t−1｜t−2)
である。Vは観測過程で発生する雑音ｖの分散、Ｗは信号過程で発生する雑音wの分散である。添字の（・）^Tは転置行列を意味し、(・)^-1は逆行列を意味する。また、Ｉは単位行列である。さらに、VとWは平均０の白色ガウス雑音であり、互いに無相関である。上記のようなカルマンフィルタにおいて、状態量Xと誤差共分散Pの初期値に適当な誤差を与えてやり、新しい観測が行われる度に(７)式の計算を繰り返し行うことにより、状態量Xの精度を向上することができる。 The Kalman filter executes the following equation (9) repeatedly with a predetermined period (Z (t) input period) with Z (t) (= δZ (t)) as an input, thereby obtaining an optimum estimated value X (t | t ) (= ΔX (t | t)). However, the estimated value of A at time i based on information up to time j is denoted as A (i | j).
X (t | t) = X (t | t−1) + K (t) [Z (t) −HX (t | t−1)] (9)
Where X (t | t−1) is the prior estimate and K (t) is the Kalman gain,
X (t | t−1) = FX (t−1 | t−1) (9) ′
K (t) = P (t | t−1) H ^T (HP (t | t−1) HT + V) ⁻¹
The prior estimated value X (t | t−1) is updated by the equation (9) ′ at a period shorter than the input period of Z (t).
P is the error covariance matrix of the state quantity X, P (t | t−1) is the predicted value of the error covariance, and P (t−1 | t−1) is the error covariance,
P (t | t−1) = FP (t−1 | t−1) F ^T + W
P (t−1 | t−1) = (I−K (t−1) H) P (t−1 | t−2)
It is. V is a variance of noise v generated in the observation process, and W is a variance of noise w generated in the signal process. 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 not correlated 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 P, and the calculation of the expression (7) is repeated each time a new observation is performed, thereby Accuracy can be improved.

共分散行列Pは次式で与えられる。

この共分散行列Pの対角要素は、誤差δN、δE、δD、δVbx、δc₀₀、δc₁₀、δc₂₀、δp₀₀、δp₁₀、δp₂₀、bwz、baxの分散であり、

と表現する。
以上では、カルマンフィルタを使用して各パラメータを補正する場合であるが、カルマンフィルタに限らず、Hインフィニティフィルタ、パーティクルフィルタなど、確率論に基づくフィルタリングシステムを利用して補正することが可能である。 The covariance matrix P is given by

Diagonal elements of the covariance matrix P error δN, δE, δD, δVbx, δc 00, δc 10, δc 20, δp 00, δp 10, δp 20, bwz, the variance of bax,

It expresses.
In the above, each parameter is corrected using the Kalman filter. However, the correction is not limited to the Kalman filter but can be performed using a filtering system based on probability theory, such as an H-infinity filter or a particle filter.

（E）周期制御
図１３はGPSの測位状態が良好のときの自律航法演算周波数HF、第１補正周波数LFを変えたときの走行軌跡とGPS測位軌跡である。GPSの測位状態が良好のとき、(1)自律航法部１２の自律航法演算周波数HFを25Hz、第１補正部２１の補正周波数LFを５Hzとした場合の走行軌跡RTRとGPS測位軌跡GTRは図１３(A)に示すようになり、(2)自律航法演算周波数HFを１Hz、第１補正周波数LFを１Hzとした場合の走行軌跡RTRとGPS測位軌跡GTRは図１３(B)に示すようになる。
この図１３（A）,(B)より、GPSの測位状態が良好であれば、周期を長くしても短い場合と同程度の位置精度を得ることが可能になる。すなわち、GPSの測位状態が良好であれば、自律航法演算周波数HFを１Hz、第１補正周波数LFを１Hzにして計算負荷を著しく削減することができる。
図１４は車両が立体駐車場内に入りGPSの測位状態が不良のときの自律航法演算周波数HF、第１補正周波数LFを変えたときの走行軌跡とGPS測位軌跡である。GPSの測位状態が不良な立体駐車場内では、(1)自律航法演算周波数HFを２５Hz、第１補正周波数LFを５Hzとした場合の走行軌跡RTRとGPS測位軌跡GTRは図１４(A)に示すようになり、(2)HF=5Hz,LF=5Hzの場合は図１４(B)に示すようになり、(3)HF=1Hz,LF=1Hzの場合は、図１４(C)に示すようになる。この図１４(A)〜(C)より、車両が立体駐車場内に入りGPSの測位状態が不良になると、HF=1Hz，LF=1Hzでは位置誤差が大きくなり採用できない。一方、HF=５Hz，LF=５HzであればHF=25Hz,LF=5Hzの場合と遜色なく位置精度が得られている。このため、GPSの測位状態が不良になったら自律航法演算周波数HFを25Hz、第１補正周波数LFを5Hzと極端に早くする必要はなく、HF=5Hz，LF=5Hzを採用できる。このようにすれば、位置精度を維持しつつ計算負荷を削減することができる。 (E) Cycle control FIG. 13 shows a traveling locus and a GPS positioning locus when the autonomous navigation calculation frequency HF and the first correction frequency LF are changed when the GPS positioning state is good. When the GPS positioning state is good, (1) the driving locus RTR and the GPS positioning locus GTR when the autonomous navigation calculation frequency HF of the autonomous navigation unit 12 is 25 Hz and the correction frequency LF of the first correction unit 21 is 5 Hz are shown in the figure. 13 (A), and (2) the traveling locus RTR and GPS positioning locus GTR when the autonomous navigation calculation frequency HF is 1 Hz and the first correction frequency LF is 1 Hz are as shown in FIG. 13 (B). Become.
From FIGS. 13A and 13B, if the GPS positioning state is good, it is possible to obtain the same position accuracy as when the period is long even if the period is long. That is, if the GPS positioning state is good, the calculation load can be significantly reduced by setting the autonomous navigation calculation frequency HF to 1 Hz and the first correction frequency LF to 1 Hz.
FIG. 14 shows the driving locus and the GPS positioning locus when the autonomous navigation calculation frequency HF and the first correction frequency LF are changed when the vehicle enters the multistory parking lot and the GPS positioning state is poor. In a multilevel parking garage where GPS positioning is poor, (1) The driving trajectory RTR and GPS positioning trajectory GTR when the autonomous navigation calculation frequency HF is 25 Hz and the first correction frequency LF is 5 Hz are shown in FIG. (2) When HF = 5Hz and LF = 5Hz, the result is as shown in FIG. 14B. When (3) HF = 1Hz and LF = 1Hz, the result is as shown in FIG. 14 (C). become. 14A to 14C, when the vehicle enters the multistory parking lot and the GPS positioning state becomes poor, the position error becomes large at HF = 1 Hz and LF = 1 Hz, which cannot be adopted. On the other hand, when HF = 5 Hz and LF = 5 Hz, the position accuracy is as good as when HF = 25 Hz and LF = 5 Hz. For this reason, when the GPS positioning state becomes poor, it is not necessary to set the autonomous navigation calculation frequency HF to 25 Hz and the first correction frequency LF to 5 Hz, and HF = 5 Hz and LF = 5 Hz can be adopted. In this way, it is possible to reduce the calculation load while maintaining the position accuracy.

図１５は本発明の位置検出装置の構成図であり、図１と同一部分には同一符号を付している。尚、位置検出部１０はカルマンフィルタ処理部１５のみ示している。
カルマンフィルタ処理部１５の第２推定部は１Hz周期(GPS測位周期)で第２の補正処理を行い、その補正処理の過程で得られる共分散行列Pの対角要素（（10）式参照）、すなわち、
δN、δE、δD、δVbx、δc₀₀、δc₁₀、δc₂₀、δp₀₀、δp₁₀、δp₂₀、bwz、bax
の分散を出力する。
推定誤差算出部３１は、目標位置からの最大誤差(例えば１０m)が目標誤差εtとして入力されているものとすれば、カルマンフィルタ処理部１５から出力する緯度方向及び経度方向の位置誤差δN、δEの分散

を用いて車両位置の推定誤差を演算する。すなわち、推定誤差算出部３１は、次式

により車両位置の推定誤差εiを演算する。
差分演算部３２は、車両位置の該推定誤差と目標誤差(例えば１０ｍ)との差分を演算し、周期変更部３３は該差分に基づいて前記第１、第２周期（自律航法演算周波数HFと第１補正周波数LF）を制御する。すなわち、周期変更部３３は推定誤差が目標誤差より小さければ、第１、第２周期を長くし、該推定誤差が目標誤差より大きければ、第１、第２周期を短くする。これを関数を用いて行うことが可能であり、また、予め自律航法演算周波数HFと第１補正周波数LFの組み合わせを複数個記憶しておき、その中から所定の組み合わせを選択してHF,LFを制御することも可能である。関数の例としては、正のゲインGを仮定し、
出力周期＝[1−G(εt−εi)]×元の周期
等が考えられる。組み合わせの例としては、
(1) HF=25Hｚ、LF=5Hz、
(2) HF=5Hz、LF=5Hｚ、
(3) HF=2Hz、LF=1Hz、
(4) HF=１Hz、LF=1Hz
が考えられる。
以上では自律航法演算周波数HFと第１補正周波数LFの両方を制御する場合を説明したがが、第１補正周波数LFのみ制御することもできる。また、以上の説明では、目標誤差として、二次元の車両位置に関して目標誤差を設定したが、三次元位置、あるいは車両速度、あるいは姿勢角等に関して目標誤差を設定することもできる。三次元位置に関して目標誤差を設定する場合、次式

により車両位置の推定誤差εiを演算する。また、車両速度に関して目標誤差を設定する場合には、次式

により車両位置の推定誤差εiを演算する。また、車両位置と車両速度とに関して目標誤差を設定する場合には、次式

により車両位置の推定誤差εiを演算し、他の場合にも同様に推定誤差εiを演算する。ここでα，β，γは正の重み係数である。 FIG. 15 is a block diagram of the position detection apparatus of the present invention, and the same parts as those in FIG. The position detection unit 10 shows only the Kalman filter processing unit 15.
The second estimation unit of the Kalman filter processing unit 15 performs a second correction process at a 1 Hz period (GPS positioning period), and a diagonal element of the covariance matrix P obtained in the course of the correction process (see equation (10)), That is,
δN, δE, δD, δVbx, δc 00, δc 10, δc 20, δp 00, δp 10, δp 20, bwz, bax
Output variance of.
If the maximum error (for example, 10 m) from the target position is input as the target error εt, the estimation error calculation unit 31 calculates the position errors δN and δE in the latitude and longitude directions output from the Kalman filter processing unit 15. dispersion

Is used to calculate the estimation error of the vehicle position. That is, the estimation error calculating unit 31

To calculate the vehicle position estimation error εi.
The difference calculation unit 32 calculates a difference between the estimation error of the vehicle position and a target error (for example, 10 m), and the period changing unit 33 calculates the first and second periods (autonomous navigation calculation frequency HF and The first correction frequency LF) is controlled. That is, the period changing unit 33 lengthens the first and second periods if the estimation error is smaller than the target error, and shortens the first and second periods if the estimation error is larger than the target error. This can be performed using a function, and a plurality of combinations of the autonomous navigation calculation frequency HF and the first correction frequency LF are stored in advance, and a predetermined combination is selected from among them and HF, LF It is also possible to control. As an example of the function, assuming a positive gain G,
Output cycle = [1−G (εt−εi )] X original period, etc. Examples of combinations are:
(1) HF = 25Hz, LF = 5Hz,
(2) HF = 5Hz, LF = 5Hz,
(3) HF = 2Hz, LF = 1Hz,
(4) HF = 1Hz, LF = 1Hz
Can be considered.
Although the case where both the autonomous navigation calculation frequency HF and the first correction frequency LF are controlled has been described above, only the first correction frequency LF can be controlled. In the above description, the target error is set for the two-dimensional vehicle position as the target error. However, the target error may be set for the three-dimensional position, the vehicle speed, the attitude angle, or the like. When setting the target error for the 3D position,

To calculate the vehicle position estimation error εi. When setting the target error for vehicle speed,

To calculate the vehicle position estimation error εi. When setting the target error for the vehicle position and vehicle speed,

Is used to calculate the estimation error εi of the vehicle position, and the estimation error εi is calculated in the same manner in other cases. Here, α, β, and γ are positive weighting factors.

図１６は本発明の位置検出装置の全体の処理フローであり、図１を参照して説明する。
自律航法部１２は自律航法（Dead Reckoning）センサー１１の出力を取り込み(ステップ２０１)、予め設定した第１周波数HFで自律航法により車両位置を計算して出力する(ステップ２０２)。上記自律航法計算と並行して、カルマンフィルタ処理部１５の第１補正部２１は該第１周波数以下の第２の周波数LFで自律航法の位置計算に使用する車両速度、ピッチ角、センサー取り付け姿勢角などを補正する第１補正処理を行ない(ステップ２０３)、第２補正部２２はGPSの測位周期(１秒＝１Hz)である第３の周波数でGPSデータを用いて自律航法による計算結果を補正する第２補正処理を行う(ステップ２０４)。周期制御部３０は、例えば車両位置に関する推定誤差を(12)式により算出し、該推定誤差と目標誤差の差分に基づいて第１周波数HF、第２周波数LFを制御し(ステップ２０５)、位置検出装置は上記補正処理の結果である推定状態量Xを出力する(ステップ２０６)。
以上本発明を実施例に従って説明したが、本発明は実施例に限定されるものではない。 FIG. 16 shows the overall processing flow of the position detection apparatus of the present invention, which will be described with reference to FIG.
The autonomous navigation unit 12 takes in the output of the autonomous navigation (Dead Reckoning) sensor 11 (step 201), calculates the vehicle position by autonomous navigation at a preset first frequency HF, and outputs it (step 202). In parallel with the autonomous navigation calculation, the first correction unit 21 of the Kalman filter processing unit 15 uses the second frequency LF equal to or lower than the first frequency to use the vehicle speed, pitch angle, and sensor mounting attitude angle used for the autonomous navigation position calculation. The first correction process is performed to correct the above (step 203), and the second correction unit 22 corrects the calculation result by the autonomous navigation using the GPS data at the third frequency that is the GPS positioning cycle (1 second = 1 Hz). A second correction process is performed (step 204). For example, the cycle control unit 30 calculates an estimation error related to the vehicle position by the equation (12), and controls the first frequency HF and the second frequency LF based on the difference between the estimation error and the target error (step 205). The detection device outputs an estimated state quantity X that is the result of the correction process (step 206).
Although the present invention has been described with reference to the embodiments, the present invention is not limited to the embodiments.

本発明の位置検出装置の概要説明図であるIt is outline | summary explanatory drawing of the position detection apparatus of this invention. 本発明の第２周期の制御例であるIt is an example of control of the 2nd period of the present invention. 本発明の位置検出部の構成図である。It is a block diagram of the position detection part of this invention. 姿勢パラメータ(ピッチ角θ、センサー取り付けピッチ角A、ヨー角Y、センサー取り付けヨー角A2)説明図である。FIG. 6 is an explanatory diagram of posture parameters (pitch angle θ, sensor mounting pitch angle A, yaw angle Y, sensor mounting yaw angle A2). 加速度センサーから出力する加速度信号を用いて車両速度Vsp(k)を計算する方法の説明図である。It is explanatory drawing of the method of calculating vehicle speed Vsp (k) using the acceleration signal output from an acceleration sensor. 位置検出部の全体の処理フローである。It is the whole processing flow of a position detection part. GPS受信時とGPS非受信時における位置検出誤差の説明図である。It is explanatory drawing of the position detection error at the time of GPS reception, and GPS non-reception. GPS受信が不可能な都庁の立体駐車場をぐるぐる回って出てきたときの車両の走行軌跡説明図である。It is traveling locus explanatory drawing of a vehicle when it goes around around the multistory parking lot of the Tokyo Metropolitan Government where GPS reception is impossible. 地下の立体駐車場内におけるナビゲーションシステムによる走行軌跡拡大図である。It is a driving locus enlarged view by a navigation system in a multilevel parking lot in the basement. カルマンフィルタ処理の概要説明図である。It is outline | summary explanatory drawing of a Kalman filter process. カルマンフィルタの線形システムFを示す行列例である。It is an example of a matrix which shows the linear system F of a Kalman filter. カルマンフィルタの観測行列例である。It is an example of an observation matrix of a Kalman filter. GPSの測位状態が良好のときの自律航法演算周波数HF、第１補正周波数LFを変えたときの走行軌跡とGPS測位軌跡である。These are the driving locus and the GPS positioning locus when the autonomous navigation calculation frequency HF and the first correction frequency LF are changed when the GPS positioning state is good. GPSの測位状態が不良のときの自律航法演算周波数HF、第１補正周波数LFを変えたときの走行軌跡とGPS測位軌跡である。である。These are the driving locus and the GPS positioning locus when the autonomous navigation calculation frequency HF and the first correction frequency LF are changed when the GPS positioning state is poor. It is. 本発明の位置検出装置の構成図である。It is a block diagram of the position detection apparatus of this invention. 本発明の位置検出装置の全体の処理フローである。It is the whole processing flow of the position detection apparatus of this invention.

Explanation of symbols

１１自律センサー
１２自律航法部
１５カルマンフィルタ処理部
２１第１の補正部
２２第２の補正部
３０周期制御部
３１推定誤差算出部
３２誤差演算部
３３周期変更部 DESCRIPTION OF SYMBOLS 11 Autonomous sensor 12 Autonomous navigation part 15 Kalman filter process part 21 1st correction | amendment part 22 2nd correction | amendment part 30 Period control part 31 Estimation error calculation part 32 Error calculation part 33 Period change part

Claims

In a position detection method for detecting the current position of a vehicle,
Position calculated by autonomous navigation in a first period,
Performing a first correction process for correcting the vehicle speed, pitch angle, sensor mounting posture angle, etc. used for the position calculation of the autonomous navigation in the second period longer than the first period;
Performing a second correction process for correcting a calculation result by autonomous navigation using GPS data in a GPS positioning period which is a third period longer than the second period ;
Calculating an estimation error of the correction target, and controlling at least the length of the second period based on the difference between the estimation error and the target error;
A position detection method characterized by the above.

In a position detection method for detecting the current position of a vehicle,
The vehicle detected by the vehicle movement distance detection unit and the pitch angle θ, the yaw angle Y with respect to the horizontal plane of the autonomous navigation sensor that outputs a signal corresponding to the acceleration of the vehicle and the amount of azimuth change of the vehicle, the yaw angle Y with respect to the vehicle A first step of calculating a vehicle position in the latitude, longitude, and height direction in the first period in the autonomous navigation unit using the moving distance, and calculating a vehicle speed using an acceleration signal output from the sensor;
The vehicle speed is calculated in a second period longer than the first period using the output signal of the movement distance detection unit, and the speed difference between the vehicle speed and the vehicle speed calculated by the autonomous navigation unit is calculated. Based on the second step of correcting the vehicle speed calculated by the autonomous navigation unit, the pitch angle θ, the mounting posture angle of the sensor,
Using the latitude, longitude, vehicle position and vehicle speed in the height direction output from the GPS receiver, and the vehicle position and vehicle speed in the latitude, longitude, and height direction output from the autonomous navigation unit, the length of the second cycle or more. The latitude, longitude, vehicle position in the height direction, vehicle speed, pitch angle θ, yaw angle Y, sensor mounting attitude angle and sensor offset value calculated by the autonomous navigation unit in the third period are corrected. The third step,
A fourth step of calculating an estimation error of a predetermined correction object among the plurality of correction objects, and controlling a length of the second period based on a difference between the estimation error and a target error;
A position detection method comprising:

In the third step, the correction target value is corrected based on Kalman filter processing, and the estimation error is calculated using diagonal elements of each correction target covariance matrix calculated in the course of the Kalman filter processing.
The position detection method according to claim 2.

The mounting posture angle of the sensor includes a sensor mounting pitch angle A and a sensor mounting yaw angle A2, and the sensor offset value includes an angular velocity signal offset and an acceleration signal offset.
The position detection method according to claim 2 or 3, wherein

In the fourth step, the length of the second period is controlled based on the difference, and the length of the first period is controlled.
The position detection method according to claim 2 or 3, wherein

If the estimation error is smaller than the target error, the first and second periods are lengthened. If the estimation error is larger than the target error, the first and second periods are shortened.
The position detection method according to claim 5.

4. The position detection method according to claim 2, wherein a target error with respect to a vehicle position, a target error with respect to a vehicle speed, or a target error with respect to an attitude angle is adopted as the target error.

In the position detection device that detects the current position of the vehicle,
A travel distance detector for measuring the travel distance of the vehicle,
An acceleration sensor that detects vehicle acceleration,
A relative direction sensor that outputs a signal according to the amount of change in the direction of the vehicle,
GPS receiver that receives satellite radio waves from GPS satellites and outputs latitude, longitude, vehicle position in the height direction and vehicle speed information,
Autonomous navigation unit for position calculation by the autonomous navigation in the first period,
A first correction unit that performs a first correction process for correcting a vehicle speed, a pitch angle, a sensor attachment posture angle, and the like used for position calculation of autonomous navigation in a second period that is longer than the first period;
A second correction unit that performs a second correction process for correcting a calculation result by autonomous navigation using GPS data in a GPS positioning period that is a third period longer than the second period ;
A period controller that calculates an estimation error of a correction target and controls at least the length of the second period based on a difference between the estimation error and a target error;
A position detection device comprising:

In the position detection device that detects the current position of the vehicle,
A travel distance detector for measuring the travel distance of the vehicle,
An acceleration sensor that detects vehicle acceleration,
A relative direction sensor that outputs a signal according to the amount of change in the direction of the vehicle,
GPS receiver that receives satellite radio waves from GPS satellites and outputs latitude, longitude, vehicle position in the height direction and vehicle speed information,
In the first period, the vehicle position in the latitude, longitude, and height directions is calculated using the pitch angle θ with respect to the horizontal plane of the sensor for autonomous navigation, the yaw angle Y, the mounting attitude angle of the sensor with respect to the vehicle, and the movement distance. And an autonomous navigation unit that calculates a vehicle speed using an acceleration signal output from the acceleration sensor,
Based on the speed difference between the vehicle speed and the vehicle speed calculated by the autonomous navigation unit, the vehicle speed is calculated in a second period longer than the first period using the output signal of the movement distance detection unit. A first correction unit that corrects the vehicle speed, the pitch angle θ, and the sensor mounting posture angle calculated by the autonomous navigation unit;
Using the latitude, longitude, vehicle position and vehicle speed in the height direction output from the GPS receiver, and the vehicle position and vehicle speed in the latitude, longitude, height direction output from the autonomous navigation unit, the second period or more The latitude, longitude, height vehicle position, vehicle speed, pitch angle θ, yaw angle Y, sensor mounting attitude angle and sensor offset value calculated by the autonomous navigation unit in the third period of length A second correction unit to correct,
A cycle control unit that calculates an estimation error of a predetermined correction target among the plurality of correction targets and controls the length of the second cycle based on a difference between the estimation error and a target error;
A position detection device comprising:

The second correction unit corrects the correction target value based on a Kalman filter process, and calculates the estimation error using a diagonal element of each correction target covariance matrix calculated in the process of the Kalman filter process. To
The position detection device according to claim 9.

The cycle control unit controls the length of the second cycle based on the difference and controls the length of the first cycle.
The position detection device according to claim 9 or 10, characterized in that

The cycle control unit lengthens the first and second cycles if the estimation error is smaller than the target error, and shortens the first and second cycles if the estimation error is larger than the target error.
The position detection device according to claim 11.

13. The position detection device according to claim 9, wherein a target error for a vehicle position, a target error for a vehicle speed, or a target error for an attitude angle is adopted as the target error.