[go: up one dir, main page]

WO2017168588A1 - 測定装置、測定方法、及び、プログラム - Google Patents

測定装置、測定方法、及び、プログラム Download PDF

Info

Publication number
WO2017168588A1
WO2017168588A1 PCT/JP2016/060234 JP2016060234W WO2017168588A1 WO 2017168588 A1 WO2017168588 A1 WO 2017168588A1 JP 2016060234 W JP2016060234 W JP 2016060234W WO 2017168588 A1 WO2017168588 A1 WO 2017168588A1
Authority
WO
WIPO (PCT)
Prior art keywords
time
features
moving body
distance
traveling direction
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/JP2016/060234
Other languages
English (en)
French (fr)
Inventor
諒子 新原
加藤　正浩
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Pioneer Corp
Original Assignee
Pioneer Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Pioneer Corp filed Critical Pioneer Corp
Priority to PCT/JP2016/060234 priority Critical patent/WO2017168588A1/ja
Publication of WO2017168588A1 publication Critical patent/WO2017168588A1/ja
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B60W40/10—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models related to vehicle motion
    • B60W40/114—Yaw movement

Definitions

  • the present invention relates to a technique for detecting a change in the traveling direction of a moving object.
  • Patent Document 1 describes a method of recognizing a fixed object existing in front of a moving body and correcting a deviation amount of a yaw rate sensor based on a movement trajectory in which the fixed object moves relative to the host vehicle. ing.
  • An object of the present invention is to provide a measuring apparatus capable of calculating the azimuth angle of a moving body with one sensor and obtaining the amount of change in the traveling direction.
  • Invention of Claim 1 is a measuring device, Comprising: Each distance from a mobile body to two features in each of 1st time and 2nd time, and direction of these 2 features seen from the mobile body And a moving direction of the moving body from the first time to the second time based on an acquisition result of the first acquiring section. And a calculating unit that calculates the amount of change.
  • the invention according to claim 7 is a measuring method executed by a measuring device, wherein each distance from a moving body to two features at each of a first time and a second time and the distance from the moving body. Based on the acquisition result of the 1st acquisition process and the 1st acquisition process which acquires the angle which each direction of two features and the advancing direction of the above-mentioned moving body make, the above-mentioned from the 1st time to the 2nd time And a calculating step of calculating the amount of change in the traveling direction of the moving body.
  • the invention according to claim 8 is a program executed by a measuring apparatus including a computer, and each distance from a moving body to two features at each of a first time and a second time and from the moving body From the first time to the second time based on the acquisition result of the first acquisition unit and the first acquisition unit that acquire the angles formed by the direction of the two features and the traveling direction of the moving body.
  • the computer is caused to function as a calculation unit that calculates an amount of change in the traveling direction of the moving body.
  • the calculation method of the azimuth change amount according to the first example of the first embodiment will be described.
  • the calculation method of the azimuth change amount according to the second example of the first embodiment will be described. It is a block diagram which shows the structure of the coefficient update apparatus by 1st Example. It is a flowchart of the coefficient update process by 1st Example.
  • the calculation method of the azimuth change amount according to the second embodiment will be described. It is a figure explaining a map coordinate system and a vehicle coordinate system.
  • the calculation method of the azimuth angle of the vehicle by 2nd Example is shown.
  • It is a flowchart of the coefficient update process by 2nd Example The example of the positional relationship of three features and a moving vehicle is shown. The other example of the positional relationship of three features and a moving vehicle is shown.
  • the distance from the moving body to the two features at the first time and the second time, the direction of the two features viewed from the moving body, and the progress of the moving body respectively.
  • a first acquisition unit that acquires an angle formed by each direction, and a calculation that calculates an amount of change in the traveling direction of the moving body from the first time to the second time based on an acquisition result of the first acquisition unit A section.
  • the distance from the moving body to the two features at the first time and the second time, the direction of the two features viewed from the moving body, and the traveling direction of the moving body are determined. Get the angle between each. And based on the acquisition result of the said 1st acquisition part, the variation
  • One aspect of the measurement apparatus further includes a second acquisition unit that acquires a distance between the two features, and the calculation unit is based on the acquisition results of the first acquisition unit and the second acquisition unit, An amount of change in the traveling direction of the moving body from the first time to the second time is calculated.
  • the distance between two features is acquired, and the amount of change in the traveling direction of the moving object is calculated using this distance.
  • the second acquisition unit is based on the distance between the two features and the angle formed by the moving direction of the moving body and the direction of each of the two features. Get the distance between two features.
  • the second acquisition unit acquires a distance between the two features based on map information.
  • the output result at the second time of the angular velocity sensor mounted on the moving body is calculated based on the change amount in the traveling direction per unit time.
  • a correction unit that corrects the first information for calculating the yaw rate of the mobile body.
  • the first information for calculating the yaw rate of the moving body is corrected based on the amount of change in the traveling direction per unit time.
  • the first information includes sensitivity and offset of the angular velocity sensor.
  • the measuring method executed by the measuring device includes the distance from the moving body to the two features at the first time and the second time, and the distance from the moving body. Based on the acquisition result of the 1st acquisition process and the 1st acquisition process which acquires the angle which each direction of two features and the advancing direction of the above-mentioned moving body make, the above-mentioned from the 1st time to the 2nd time And a calculation step of calculating a change amount in the traveling direction of the moving body. Thereby, the amount of change in the traveling direction of the moving body can be calculated using any feature that can be measured from the moving body.
  • the program executed by the measuring device including the computer is the distance from the moving body to the two features and the moving body at the first time and the second time, respectively. From the first time to the second time based on the acquisition result of the first acquisition unit and the first acquisition unit that acquire the angles formed by the direction of the two features and the traveling direction of the moving body.
  • the computer is caused to function as a calculation unit that calculates the amount of change in the traveling direction of the moving body. Thereby, the amount of change in the traveling direction of the moving body can be calculated using any feature that can be measured from the moving body.
  • This program can be stored in a storage medium and used.
  • the sensitivity and offset of the gyro sensor have characteristics that change with temperature and vibration. Therefore, if the angular velocity is integrated in a state including these errors, the errors accumulate, and the calculated azimuth angle deviates greatly from the actual value. Therefore, in order to estimate the azimuth angle with high accuracy, it is necessary to continuously correct the sensitivity and offset of the gyro sensor.
  • the sensitivity and offset are corrected using the direction change amount based on GPS information. For this reason, the accuracy of correction deteriorates in an environment such as an urban area where the GPS reception state is poor. Moreover, it cannot correct
  • the coefficient updating apparatus calculates the azimuth change amount of the vehicle based on the measurement result of the feature by the external sensor without using the GPS information, and corrects the sensitivity and offset of the gyro sensor.
  • the external sensor include a camera, LiDAR (Light Detection And Ranging), and a millimeter wave radar.
  • the coefficient updating device corrects the sensitivity and offset of the gyro sensor through the following steps. (1) Two features are detected at time t-1 and time t. (2) Based on the feature measurement result at each time, the amount of change in the azimuth angle of the vehicle between time t-1 and time t is calculated. (3) The sensitivity coefficient and the offset coefficient are estimated by the successive least square method.
  • ⁇ L When the change amount of the azimuth angle of the vehicle (hereinafter referred to as “azimuth angle change amount”) ⁇ L can be obtained from the feature measurement value by the external sensor, the following is obtained by dividing this by the time interval ⁇ t.
  • the angular velocity is calculated as follows.
  • the angular velocity (also referred to as “measured yaw rate”) obtained from the feature measurement value by the external sensor is regarded as the true angular velocity, it can be expressed as follows.
  • the recursive least square method it is possible to obtain the ⁇ and beta. That is, if the azimuth angle change amount ⁇ of the vehicle can be acquired using an external sensor, the sensitivity and offset of the gyro sensor can be estimated.
  • the azimuth angle change amount ⁇ is obtained using the measured value of the feature by the external sensor.
  • FIG. 1 is a diagram for explaining a method of calculating the azimuth change amount of a vehicle according to the first example of the first embodiment.
  • the feature A and the feature B can be measured by the external sensor at time t-1 and time t.
  • it is necessary that the same two features can be measured by the external sensor at each time.
  • the azimuth angle variation ⁇ of the vehicle is expressed as follows using the azimuth angles ⁇ A, t-1 , ⁇ A, t of the feature A at time t-1 and time t and the angles ⁇ , ⁇ in FIG. Can be calculated.
  • angle ⁇ t ⁇ 1 is obtained from the cosine theorem.
  • the azimuth angles ⁇ A, t ⁇ 1 , ⁇ A, t obtained by the external sensor and the angles ⁇ t ⁇ 1 , ⁇ t obtained by the above equations (8), (9) are expressed by equation (7).
  • the azimuth angle change amount ⁇ can be calculated.
  • FIG. 2 is a diagram for explaining a method of calculating the azimuth change amount of the vehicle according to the second example of the first embodiment.
  • the feature A and the feature B can be measured by the external sensor at time t ⁇ 1 and time t.
  • the azimuth angle change amount ⁇ of the vehicle is expressed as follows using the azimuth angles ⁇ B, t ⁇ 1 , ⁇ B, t of the feature A at time t ⁇ 1 and time t and the angles ⁇ ′ and ⁇ ′ in FIG. It can be calculated as follows.
  • the angle ⁇ ′ t ⁇ 1 is obtained from the cosine theorem.
  • the relative distance r of the feature and the relative angle ⁇ of the feature can be obtained from the external sensor.
  • the relative position (x, y) of the feature can be obtained instead of the distance r and the relative angle ⁇
  • the relative distance r and the relative angle ⁇ can be obtained from the relative position (x, y) by the following formula and used. That's fine.
  • the distance R between the features can be calculated using the feature measurement value by the external sensor. Specifically, when the relative distance r and the relative angle ⁇ of the feature with respect to the host vehicle can be acquired from the external sensor, the distance R between the features can be calculated as follows using the cosine theorem.
  • the distance R between the features can be calculated as follows.
  • the distance R between the features may be acquired from the high-accuracy map.
  • the accuracy of the distance between features also changes depending on the measurement accuracy. That is, if the measurement accuracy is poor, the accuracy of the calculated distance between the features is also deteriorated, and the accuracy of the azimuth change amount of the vehicle to be calculated later is also deteriorated.
  • this problem can be avoided and the accuracy of the obtained azimuth angle change amount of the vehicle can be improved.
  • FIG. 3 is a block diagram showing the configuration of the coefficient update device 10 according to the first embodiment.
  • the coefficient update device 10 includes a gyro sensor 11, an external sensor 12, a traveling direction acquisition unit 13, a feature measurement unit 14, an inter-feature distance calculation unit 15, a coefficient calculation unit 16, And an azimuth angle variation calculation unit 17.
  • the traveling direction acquisition unit 13, the feature measurement unit 14, the feature distance calculation unit 15, the coefficient calculation unit 16, and the azimuth change amount calculation unit 17 execute a program prepared in advance by a computer such as a CPU. This can be realized.
  • the gyro sensor 11 supplies the detected angular velocity ⁇ t to the traveling direction acquisition unit 13 and the coefficient calculation unit 16.
  • the traveling direction acquisition unit 13 acquires the traveling direction Hd of the vehicle based on the angular velocity ⁇ t supplied from the gyro sensor 11 and supplies it to the feature measurement unit 14.
  • the external sensor 12 is, for example, a camera, LiDAR, millimeter wave radar, or the like.
  • the feature measuring unit 14 measures the distance to the feature and the angle with the feature based on the output of the external sensor 12. Specifically, the feature measuring unit 14 measures the distances (relative distances) r A, t ⁇ 1 , r B, t ⁇ 1 from the vehicle to the two features A, B at time t ⁇ 1. At the same time, the azimuth angles (relative angles) ⁇ A, t ⁇ 1 , ⁇ B, t ⁇ 1 of the two features A and B with respect to the traveling direction Hd t ⁇ 1 of the vehicle supplied from the traveling direction acquisition unit 13 are calculated.
  • the distance between the features is supplied to the distance calculation unit 15 and the azimuth angle change calculation unit 17.
  • the feature measurement unit 14 measures the distances r A, t , r B, t from the vehicle to the two features A, B at the time t, and the vehicle supplied from the traveling direction acquisition unit 13 two features a to the traveling direction Hd t, the azimuth angle phi a, t of B, calculates the ⁇ B, t, and supplies the feature distance calculation unit 15 and azimuth angle change amount calculation unit 17.
  • the inter-feature distance calculation unit 15 uses the above equation (15).
  • a distance R between the features A and B is calculated and supplied to the azimuth angle change amount calculation unit 17.
  • the distance calculation part 15 between features may acquire the distance R between features from a high precision map.
  • the azimuth angle variation calculation unit 17 includes the azimuth angles ⁇ A, t ⁇ 1 , ⁇ B, t ⁇ 1 , ⁇ A, t , ⁇ B, t , and distance between features supplied from the feature measurement unit 14. Based on the distance R between the features calculated by the calculation unit 15, the azimuth change amount ⁇ of the vehicle is calculated by the above formulas (7) to (9) or the formulas (11) to (13) to calculate the coefficient. 16 is supplied.
  • the feature measurement unit 14 is an example of the first acquisition unit of the present invention
  • the azimuth change amount calculation unit 17 is an example of the calculation unit of the present invention
  • the inter-feature distance calculation unit 15 is the main acquisition unit. It is an example of the 2nd acquisition part of invention, and a coefficient calculation part is an example of the correction
  • the azimuth angle change amount corresponds to the change amount in the traveling direction in the present invention.
  • FIG. 4 is a flowchart of the coefficient update process.
  • the first example of the method for calculating the azimuth angle change amount described above is used.
  • the coefficient updating unit 10 the external sensor 12 detects the feature A, B (Step S10), and calculates an angle theta t by the formula (9) described above (step S11).
  • step S12 YES
  • the features A and B are detected at two times and the angles ⁇ t ⁇ 1 and ⁇ t are obtained.
  • the azimuth angle change amount ⁇ is calculated (step S14).
  • the coefficient updating apparatus 10 calculates the true angular velocity (measured yaw rate) ⁇ _dot (t) from the azimuth change amount ⁇ and the time interval ⁇ t using the equation (2) (Step S15), and obtained. From the measured yaw rate ⁇ _dot (t) and the output ⁇ t of the gyro sensor, the sensitivity coefficient ⁇ and the offset coefficient ⁇ are calculated and updated by the above equation (3) (step S16).
  • FIG. 5 is a diagram for explaining a method of calculating the azimuth change amount according to the second embodiment.
  • the position of the vehicle is shown on the map coordinate system (X m , Y m ).
  • map coordinates system is defined by X m-axis and the Y m axis.
  • the vehicle coordinate system is defined by an Xv axis indicating the front-rear direction of the vehicle, a Yv axis indicating the left-right direction of the vehicle, and a Zv axis indicating the vertical direction of the vehicle. Is done.
  • the second embodiment first, at time t ⁇ 1, at least two features registered in the high-precision map are measured using an external sensor.
  • the feature A and the feature B are measured.
  • the azimuth angle ⁇ t ⁇ 1 of the vehicle is calculated from the position information of the features A and B obtained from the high-accuracy map and the measurement result by the external sensor.
  • At time t similarly, at least two features registered in the high-precision map are measured using the external sensor.
  • the feature C and the feature D are measured.
  • direction angle (psi) t of a vehicle is calculated from the positional information on the features C and D obtained from a highly accurate map, and the measurement result by an external sensor.
  • the external sensor since the azimuth angle of the vehicle can be calculated independently at each time, it is not necessary that the same two features can be detected by the external sensor at time t-1 and time t.
  • the external sensor detects the features A and B at time t ⁇ 1 and detects the features C and D different from the features A and B at time t ⁇ 1.
  • FIG. 7 shows a method of calculating the azimuth angle of the vehicle according to the second embodiment.
  • the position (x mA , y mA ) of the feature A and the position (x mB , y mB ) of the feature A can be acquired from the high-precision map, and the relative position ( x vA , y vA ) and the relative position (x vB , y vB ) of the feature B can be acquired.
  • the vector p from the feature B to the feature A is in the map coordinate system [X mA -x mB y mA -y mB ] T
  • [X vA -x vB y vA -y vB ] T It is expressed. Here, it is set as follows.
  • the direction cosine matrix C from the map coordinate system to the vehicle coordinate system is as follows.
  • the azimuth angle ⁇ of the vehicle can be calculated using the equations (22) to (24).
  • the vehicle azimuth angles ⁇ t-1 and ⁇ t are calculated at time t-1 and time t, respectively, and are substituted into equation (17), whereby the azimuth angle change amount ⁇ of the vehicle can be calculated.
  • FIG. 8 is a block diagram showing the configuration of the coefficient update device 20 according to the second embodiment.
  • the coefficient update device 20 includes a gyro sensor 21, an external sensor 22, a feature measurement unit 23, a coefficient calculation unit 24, an azimuth change amount calculation unit 25, and a map database (DB) 26. Is provided.
  • the feature measurement unit 23, the coefficient calculation unit 24, and the azimuth change amount calculation unit 25 can be realized by executing a program prepared in advance by a computer such as a CPU.
  • the gyro sensor 21 supplies the detected angular velocity ⁇ t to the coefficient calculation unit 24.
  • the external sensor 22 is, for example, a camera, LiDAR, millimeter wave radar, or the like.
  • the feature measurement unit 23 measures the relative position (x v , y v ) of the two features based on the output of the external sensor 22 and supplies the measured relative position (x v , y v ) to the azimuth angle change amount calculation unit 25.
  • the azimuth angle variation calculation unit 25 acquires the relative positions (x m , y m ) of the two features from the map DB 26 storing the high-accuracy map. Then, the azimuth angle change amount calculation unit 25 uses the relative positions (x v , y v ) and (x m , y m ) of the two features to calculate the azimuth angle ⁇ t according to equations (18) to (24). Is calculated. By performing this processing at two different times, the azimuth angle change amount calculation unit 25 calculates azimuth angles ⁇ t ⁇ 1 and ⁇ t , calculates the azimuth angle change amount ⁇ by equation (17), and calculates coefficients. To the unit 24.
  • the coefficient calculation unit 24 Based on the angular velocity ⁇ t supplied from the gyro sensor 21 and the azimuth angle change amount ⁇ supplied from the azimuth angle change amount calculation unit 25, the coefficient calculation unit 24 performs the following equations (2) to (3).
  • the sensitivity coefficient ⁇ and the offset coefficient ⁇ of the gyro sensor are calculated and updated.
  • FIG. 9 is a flowchart of the coefficient update process according to the second embodiment.
  • the coefficient update device 20 acquires the positions of two features by the external sensor 12 (step S20). Next, the coefficient update device 20 acquires the positions of these two features from the high-accuracy map stored in the map DB 26 (step S21). Next, the coefficient updating device 20 calculates the azimuth angle by the equations (18) to (24) based on the positions of the two features acquired from the external sensor and the positions of the two features acquired from the high-precision map. ⁇ is calculated (step S22).
  • step S23 YES
  • two features are detected at two times and azimuth angles ⁇ t ⁇ 1 and ⁇ t are obtained.
  • the azimuth angle change amount ⁇ is calculated from the equation (17) (step S25).
  • the coefficient updating device 20 calculates the true angular velocity (measured yaw rate) ⁇ _dot (t) from the azimuth angle change amount ⁇ and the time interval ⁇ t using the equation (2) (step S26). Then, the coefficient update device 20 calculates and updates the sensitivity coefficient ⁇ and the offset coefficient ⁇ from the measured yaw rate ⁇ _dot (t) and the output ⁇ t of the gyro sensor by the above-described equation (3) (step S27).
  • the azimuth angle change amount ⁇ between time t-1 and time t is calculated by the coefficient update process described above.
  • the azimuth angle change obtained by the combination of the feature 1 and the feature 2 is ⁇ 12
  • the azimuth change obtained by the combination of the feature 2 and the feature 3 is ⁇ 23
  • the feature 3 and the feature Assuming that the azimuth change amount obtained by the combination of 1 is ⁇ 31 , a value obtained by averaging these can be used as the azimuth change ⁇ as follows.
  • the coefficient updating device may obtain the azimuth angle change amount ⁇ by using two features that are closer to the vehicle, that is, the feature 1 and the feature 3.
  • the distance between the features is compared with the threshold value L th .
  • L 12 ⁇ L th , L 23 > L th , and L 31 > L th combinations in which the distance between the features is shorter than the threshold L th , that is, combinations of the features 1 and 2 are excluded, combinations of the features 2 and 3 and features 1 and features
  • the azimuth angle change amount ⁇ may be obtained using the combination of three. Specifically, considering the distance from the vehicle to the feature by the method (B), the feature update device is closer to the vehicle than the feature 2 (L 1 ⁇ L 2 ). Based on the combination of the feature 1 and the feature 3, the azimuth angle change amount ⁇ may be obtained.
  • the average value of ⁇ 31 may be used as the azimuth angle change amount ⁇ .
  • the predetermined distance is set as the threshold value L th in advance, but instead, an average value of three or more measured distances between features may be used as the threshold value L th .
  • either the method for calculating the distance R between the features from the measurement results of the two features or the method for obtaining the distance R between the features using the map data of the high-precision map may be used in combination.
  • the distance between features R is obtained using high-precision map data
  • the distance R between features is obtained from the measurement result of the features. May be.
  • the present invention can be used for an apparatus mounted on a moving body.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Physics (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Navigation (AREA)

Abstract

測定装置は、第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する。そして、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する。

Description

測定装置、測定方法、及び、プログラム
　本発明は、移動体の進行方向の変化を検出する技術に関する。
　移動体の前方に存在する物体を認識して、移動体のヨーレートを補正する手法が知られている。特許文献１は、移動体の前方に存在する固定物を認識し、その固定物が自車両に対して相対的に移動する移動軌跡に基づいて、ヨーレートセンサのずれ量を補正する手法を記載している。
特開２０１２－６６７７７号公報
　特許文献１に記載の手法では、上記の移動軌跡の他に、車速、旋回半径からのずれ量、固定物が横方向に相対的に移動する横移動量など、推定ヨーレートの算出に用いるパラメータが多く、これらのパラメータを取得するセンサのずれ量も考慮しなくてはならない。
　本発明が解決しようとする課題としては、上記のものが例として挙げられる。本発明は、１つのセンサで移動体の方位角を算出し、進行方向の変化量を得ることが可能な測定装置を提供することを目的とする。
　請求項１に記載の発明は、測定装置であって、第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する第１取得部と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出部と、を備えることを特徴とする。
　請求項７に記載の発明は、測定装置により実行される測定方法であって、第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する第１取得工程と、前記第１取得工程の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出工程と、を備えることを特徴とする。
　請求項８に記載の発明は、コンピュータを備える測定装置によって実行されるプログラムであって、第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する第１取得部、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出部、として前記コンピュータを機能させることを特徴とする。
第１実施例の第１例による方位角変化量の算出方法を示す。 第１実施例の第２例による方位角変化量の算出方法を示す。 第１実施例による係数更新装置の構成を示すブロック図である。 第１実施例による係数更新処理のフローチャートである。 第２実施例による方位角変化量の算出方法を示す。 地図座標系及び車両座標系を説明する図である。 第２実施例による車両の方位角の算出方法を示す。 第２実施例による係数更新装置の構成を示すブロック図である。 第２実施例による係数更新処理のフローチャートである。 ３つの地物と移動車両との位置関係の例を示す。 ３つの地物と移動車両との位置関係の他の例を示す。
　本発明の好適な実施形態では、第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する第１取得部と、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出部と、を備える。
　上記の測定装置は、第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する。そして、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する。これにより、移動体から計測できる任意の地物を利用して、移動体の進行方向の変化量を算出することができる。
　上記の測定装置の一態様は、前記２つの地物間の距離を取得する第２取得部を更に備え、前記算出部は、前記第１取得部及び前記第２取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する。この態様では、２つの地物間の距離を取得し、これを用いて移動体の進行方向の変化量を算出する。この場合の好適な例では、前記第２取得部は、前記２つの地物までの距離、及び、前記移動体の進行方向と前記２つの地物それぞれの方向とがなす角に基づいて、前記２つの地物間の距離を取得する。他の好適な例では、前記第２取得部は、地図情報に基づいて、前記２つの地物間の距離を取得する。
　上記の測定装置の他の一態様は、前記変化量に基づき算出される、単位時間あたりの前記進行方向の変化量に基づき、前記移動体に搭載される角速度センサの前記第２時刻における出力結果から前記移動体のヨーレートを算出するための第１情報を補正する補正部を更に備える。この態様では、単位時間あたりの前記進行方向の変化量に基づいて、移動体のヨーレートを算出するための第１情報を補正する。好適な例では、前記第１情報は、前記角速度センサの感度及びオフセットを含む。
　本発明の他の好適な実施形態では、測定装置により実行される測定方法は、第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する第１取得工程と、前記第１取得工程の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出工程と、を備える。これにより、移動体から計測できる任意の地物を利用して、移動体の進行方向の変化量を算出することができる。
　本発明の他の好適な実施形態では、コンピュータを備える測定装置によって実行されるプログラムは、第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する第１取得部、前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出部、として前記コンピュータを機能させる。これにより、移動体から計測できる任意の地物を利用して、移動体の進行方向の変化量を算出することができる。このプログラムは、記憶媒体に記憶して利用することができる。
　以下、図面を参照して本発明の好適な実施例について説明する。
　［１］背景
　従来のカーナビゲーション装置に搭載されている自車位置推定システムでは、車速パルスから算出される速度とジャイロセンサの出力から算出される方位角を用いて車両の移動状態を測定し、ＧＰＳ情報と統合することにより現在位置を推定する複合航法システムが用いられている。複合航法システムによる自車位置推定精度向上のためには、車体速度と方位角（ヨー角）を正確に把握する必要がある。
　ジャイロセンサの感度とオフセットは温度や振動などによって変化する特性がある。そのため、それらの誤差を含んだ状態で角速度を積分すると、誤差が蓄積し、計算される方位角は実際の値から大きくずれてしまう。従って、方位角を高精度に推定するためには、ジャイロセンサの感度とオフセットを継続的に補正することが必要である。
　従来のカーナビゲーション装置では、ＧＰＳ情報に基づく方位変化量を用いて感度とオフセットの補正を行っている。そのため、都市部のようなＧＰＳの受信状態の悪い環境では補正の精度が劣化する。また、トンネルや地下駐車場のようなＧＰＳ測位ができない環境では補正ができない。よって、ＧＰＳの受信状態に関わらず、継続的に感度とオフセットの補正を行うことが要求される。
　本実施例による係数更新装置は、ＧＰＳ情報を用いずに、外界センサによる地物の計測結果に基づいて車両の方位変化量を算出し、ジャイロセンサの感度とオフセットを補正する。外界センサとしては、カメラやＬｉＤＡＲ（Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　Ａｎｄ　Ｒａｎｇｉｎｇ）、ミリ波レーダーなどが挙げられる。
　具体的には、係数更新装置は、以下の工程によりジャイロセンサの感度及びオフセットを補正する。
（１）時刻ｔ－１及び時刻ｔにおいて、２つの地物を検出する。
（２）各時刻の地物計測結果に基づいて、時刻ｔ－１から時刻ｔの間の車両の方位角の変化量を算出する。
（３）逐次最小二乗法により、感度係数とオフセット係数を推定する。
　［２］ジャイロセンサの感度とオフセットの推定方法
　真の角速度Ψ＿ｄｏｔ（ｔ）と、ジャイロセンサで検出する角速度ωｔとの関係は、感度係数αとオフセット係数βを用いて次のように表すことができる。
Figure JPOXMLDOC01-appb-M000001
 　外界センサによる地物計測値から、車両の方位角の変化量（以下、「方位角変化量」と呼ぶ。）ΔΨＬを取得できた場合、これをその時間間隔Δｔで除することにより、次のように角速度を算出する。
Figure JPOXMLDOC01-appb-M000002
 　外界センサによる地物計測値から求めた角速度（「計測ヨーレート」とも呼ぶ。）を真の角速度とみなすと、次のように表すことができる。
Figure JPOXMLDOC01-appb-M000003
 　よって、次のように置くと、
Figure JPOXMLDOC01-appb-M000004
 以下の１次式で表すことができる。
Figure JPOXMLDOC01-appb-M000005
 　従って、ｘｔとｙｔの複数のデータがあれば、逐次最小二乗法等により、αとβを求めることができる。即ち、外界センサを用いて車両の方位角変化量ΔΨを取得できれば、ジャイロセンサの感度とオフセットを推定することができる。
　［３］方位角変化量の算出方法
　以下、外界センサを用いて車両の方位角変化量ΔΨを求める方法を説明する。
　［３．１］第１実施例
　第１実施例は、外界センサによる地物の測定値を用いて方位角変化量ΔΨを求めるものである。
　［３．１．１］方位角変化量の算出方法（第１例）
　時刻ｔ－１及び時刻ｔにおいて、同一の地物を２つ以上計測できたとき、その計測結果を用いて車両の方位角変化量を算出することができる。ここで、外界センサからは、自車両に対する地物の相対距離と相対角度（ｒ、φ）が取得できるものとする。図１は、第１実施例の第１例により車両の方位角変化量を算出する方法を説明する図である。図１の例では、時刻ｔ－１と時刻ｔにおいて、外界センサにより地物Ａと地物Ｂが計測できたものとする。なお、第１例では、各時刻において、外界センサにより同じ２つの地物を計測できることが必要である。
　車両の方位角変化量ΔΨは、時刻ｔ－１及び時刻ｔにおける地物Ａの方位角φＡ，ｔ－１、φＡ，ｔと、図１における角度Δθ、γを用いて、以下のように算出することができる。
　まず、三角形の内角と外角の関係から、
Figure JPOXMLDOC01-appb-M000006
 が得られる。従って、方位角変化量ΔΨは、
Figure JPOXMLDOC01-appb-M000007
 で得られる。
　ここで、角度θｔ－１は、余弦定理より、
となる。同様に、角度θｔは、
Figure JPOXMLDOC01-appb-M000009
 となる。
　よって、外界センサにより得られた方位角φＡ，ｔ－１、φＡ，ｔと、上記の式（８）、（９）により得られた角度θｔ－１、θｔを式（７）に代入することにより、方位角変化量ΔΨを算出することができる。
　［３．１．２］方位角変化量の算出方法（第２例）
　上記の第１例は地物Ａ側の角度を用いたが、地物Ｂ側の角度を用いても同様に方位角変化量ΔΨを算出することができる。図２は、第１実施例の第２例により車両の方位角変化量を算出する方法を説明する図である。図２の例でも、時刻ｔ－１と時刻ｔにおいて、外界センサにより地物Ａと地物Ｂが計測できたものとする。なお、第２例でも、各時刻において、外界センサにより同じ２つの地物を計測できることが必要である。
　車両の方位角変化量ΔΨは、時刻ｔ－１及び時刻ｔにおける地物Ａの方位角φＢ，ｔ－１、φＢ，ｔと、図２における角度Δθ’、γ’を用いて、以下のように算出することができる。
　まず、三角形の内角と外角の関係から、
Figure JPOXMLDOC01-appb-M000010
 が得られる。
　従って、方位角変化量ΔΨは、
Figure JPOXMLDOC01-appb-M000011
 で得られる。
　ここで、角度θ’ｔ－１は、余弦定理より、
Figure JPOXMLDOC01-appb-M000012
 となる。同様に、角度θ’ｔは、
Figure JPOXMLDOC01-appb-M000013
 となる。
　よって、外界センサにより得られた方位角φＢ，ｔ－１、φＢ，ｔと、上記の式（１２）、（１３）により得られた角度θ’ｔ－１、θ’ｔを式（１１）に代入することにより、方位角変化量ΔΨを算出することができる。
　［３．１．３］地物計測値
　上記の第１例及び第２例では、外界センサから地物の相対距離ｒと地物の相対角度φが取得できるものとしているが、地物の相対距離ｒと相対角度φではなく、地物の相対位置（ｘ，ｙ）が取得できる場合には、以下の式によって相対位置（ｘ，ｙ）から相対距離ｒ及び相対角度φを求めて使用すればよい。
Figure JPOXMLDOC01-appb-M000014
 　［３．１．４］地物間の距離
　上記の第１例及び第２例では、式（８）、（９）、（１２）、（１３）において２つの地物間の距離Ｒを使用している。地物間の距離Ｒは以下の方法により取得することができる。
　まず、地物間の距離Ｒは、外界センサによる地物計測値を用いて算出することができる。具体的に、外界センサから自車両に対する地物の相対距離ｒと相対角度φが取得できる場合、余弦定理により地物間の距離Ｒは以下のように算出できる。
Figure JPOXMLDOC01-appb-M000015
 　一方、外界センサから自車両に対する地物の相対位置（ｘ，ｙ）が取得できる場合、地物間の距離Ｒは以下のように算出できる。
Figure JPOXMLDOC01-appb-M000016
 　また、地物の位置情報を有する高精度地図を利用できる場合には、高精度地図から地物間の距離Ｒを取得しても良い。外界センサによる地物計測値から地物間距離を算出する場合は、計測精度によって地物間距離の精度も変わってしまう。つまり、計測精度が悪いと、算出される地物間距離の精度も悪くなり、後で計算する車両の方位角変化量の精度も悪くなる。高精度地図を用いると、この問題を回避することができ、得られる車両の方位角変化量の精度も向上させることができる。
　［３．１．５］係数更新装置
　次に、ジャイロセンサの感度係数α及びオフセット係数βを更新する係数更新装置について説明する。図３は、第１実施例による係数更新装置１０の構成を示すブロック図である。図示のように、係数更新装置１０は、ジャイロセンサ１１と、外界センサ１２と、進行方向取得部１３と、地物計測部１４と、地物間距離計算部１５と、係数計算部１６と、方位角変化量計算部１７とを備える。なお、進行方向取得部１３、地物計測部１４、地物間距離計算部１５、係数計算部１６、及び、方位角変化量計算部１７は、ＣＰＵなどのコンピュータが予め用意されたプログラムを実行することにより実現することができる。
　ジャイロセンサ１１は、検出した角速度ωｔを進行方向取得部１３及び係数計算部１６へ供給する。進行方向取得部１３は、ジャイロセンサ１１から供給された角速度ωｔに基づいて車両の進行方向Ｈｄを取得し、地物計測部１４に供給する。
　外界センサ１２は、例えばカメラ、ＬｉＤＡＲ、ミリ波レーダーなどである。地物計測部１４は、外界センサ１２の出力に基づいて地物までの距離及び地物との角度を計測する。具体的には、地物計測部１４は、時刻ｔ－１において、車両から２つの地物Ａ、Ｂまでの距離（相対距離）ｒＡ,ｔ－１、ｒＢ，ｔ－１を計測するとともに、進行方向取得部１３から供給された車両の進行方向Ｈｄｔ－１に対する２つの地物Ａ、Ｂの方位角（相対角度）φＡ，ｔ－１、φＢ，ｔ－１を算出し、地物間距離計算部１５及び方位角変化量計算部１７に供給する。また、地物計測部１４は、時刻ｔにおいて、車両から２つの地物Ａ、Ｂまでの距離ｒＡ,ｔ、ｒＢ，ｔを計測するとともに、進行方向取得部１３から供給された車両の進行方向Ｈｄｔに対する２つの地物Ａ、Ｂの方位角φＡ，ｔ、φＢ，ｔを算出し、地物間距離計算部１５及び方位角変化量計算部１７に供給する。
　地物間距離計算部１５は、地物計測部１４が計測した２つの地物Ａ、Ｂの距離ｒＡ、ｒＢ及び方位角φＡ、φＢに基づいて、前述の式（１５）により地物Ａ、Ｂ間の地物間距離Ｒを算出して方位角変化量計算部１７へ供給する。なお、高精度地図を利用できる場合は、地物間距離計算部１５は、高精度地図から地物間距離Ｒを取得しても良い。
　方位角変化量計算部１７は、地物計測部１４から供給された方位角φＡ，ｔ－１、φＢ，ｔ－１、φＡ，ｔ、φＢ，ｔ、及び、地物間距離計算部１５が算出した地物間距離Ｒに基づいて、前述の式（７）～（９）又は式（１１）～（１３）により、車両の方位角変化量ΔΨを算出して係数計算部１６に供給する。
　係数計算部１６は、ジャイロセンサ１１から供給された角速度ωｔと、方位角変化量計算部１７から供給された方位角変化量ΔΨに基づいて、前述の式（２）～（３）により、ジャイロセンサの感度係数α及びオフセット係数βを算出し、更新する。
　上記の構成において、地物計測部１４は本発明の第１取得部の一例であり、方位角変化量計算部１７は本発明の算出部の一例であり、地物間距離計算部１５は本発明の第２取得部の一例であり、係数計算部は本発明の補正部の一例である。また、方位角変化量は本発明における進行方向の変化量に相当する。
　［３．１．６］係数更新処理
　次に、上記の係数更新装置１０による係数更新処理について説明する。図４は、係数更新処理のフローチャートである。なお、以下の説明では、前述した方位角変化量の算出方法の第１例を使用するものとする。
　まず、係数更新装置１０は、外界センサ１２により地物Ａ、Ｂを検出し（ステップＳ１０）、前述の式（９）により角度θｔを算出する（ステップＳ１１）。次に、係数更新装置１０は、ｆｌａｇ＝１であるか否かを判定する（ステップＳ１２）。なお、「ｆｌａｇ」は処理の開始時に「０」にリセットされている。ｆｌａｇ＝１でない場合（ステップＳ１２：ＮＯ）、係数更新装置１０はｆｌａｇに「１」をセットし（ステップＳ１３）、ステップＳ１０へ戻る。
　一方、ｆｌａｇ＝１である場合（ステップＳ１２：ＹＥＳ）、２つの時刻において地物Ａ、Ｂが検出され、角度θｔ－１、θｔが得られているので、係数更新装置１０は、１時刻前の地物Ａの方位角φＡ，ｔ－１及び角度θｔ－１と、現在時刻の地物Ａの方位角φＡ，ｔ及び角度θｔから、前述の式（７）を用いて方位角変化量ΔΨを算出する（ステップＳ１４）。
　次に、係数更新装置１０は、方位角変化量ΔΨと時間間隔Δｔから、式（２）を用いて真の角速度（計測ヨーレート）Ψ＿ｄｏｔ（ｔ）を算出し（ステップＳ１５）、得られた計測ヨーレートΨ＿ｄｏｔ（ｔ）とジャイロセンサの出力ωｔから、前述の式（３）により感度係数αとオフセット係数βを算出して更新する（ステップＳ１６）。
　［３．２］第２実施例
　［３．２．１］方位角変化量の算出方法
　第２実施例では、高精度地図と外界センサによる地物計測結果とを用いて車両の方位角変化量を算出する。図５は、第２実施例による方位角変化量の算出方法を説明する図である。図５では、地図座標系（Ｘｍ，Ｙｍ）上に車両の位置を示している。ここで、地図座標系と車両座標系との関係を図６に示す。図６（Ａ）に示すように、地図座標系はＸｍ軸とＹｍ軸により規定される。一方、図６（Ｂ）に示すように、車両座標系は、車両の前後方向を示すＸｖ軸と、車両の左右方向を示すＹｖ軸と、車両の上下方向を示すＺｖ軸により規定される。
　図５に戻り、第２実施例では、まず、時刻ｔ－１で、外界センサを用いて高精度地図に登録されている地物を少なくとも２つ計測する。図５の例では、地物Ａと地物Ｂが計測される。そして、高精度地図から得られる地物Ａ、Ｂの位置情報と、外界センサによる計測結果から車両の方位角Ψｔ－１を算出する。
　次に、時刻ｔにおいても同様に、外界センサを用いて高精度地図に登録されている地物を少なくとも２つ計測する。図５の例では、地物Ｃと地物Ｄが計測される。そして、高精度地図から得られる地物Ｃ、Ｄの位置情報と、外界センサによる計測結果から車両の方位角Ψｔを算出する。
　次に、各時刻における車両の方位角Ψｔ－１、Ψｔの差分から、下記の式により、時刻ｔ－１から時刻ｔの間の車両の方位角の変化量ΔΨを求める。
Figure JPOXMLDOC01-appb-M000017
 　なお、第２実施例においては、車両の方位角は各時刻で独立に計算できるので、時刻ｔ－１と時刻ｔで外界センサにより同じ２つの地物を検出できる必要はない。図５の例では、外界センサは、時刻ｔ－１では地物Ａ、Ｂを検出しており、時刻ｔでは地物Ａ、Ｂとは異なる地物Ｃ、Ｄを検出している。
　次に、各時刻における車両の方位角の算出方法について説明する。図７は、第２実施例による車両の方位角の計算方法を示す。いま、高精度地図から地物Ａの位置（ｘｍＡ，ｙｍＡ）と地物Ｂの位置（ｘｍＢ，ｙｍＢ）を取得でき、外界センサを用いて自車両に対する地物Ａの相対位置（ｘｖＡ，ｙｖＡ）と地物Ｂの相対位置（ｘｖＢ，ｙｖＢ）を取得できるものとする。地物Ｂから地物Ａへのベクトルｐは、地図座標系においては、
　　　［ｘｍＡ－ｘｍＢ　ｙｍＡ－ｙｍＢ］Ｔ
と表され、車両座標系においては、
　　　［ｘｖＡ－ｘｖＢ　ｙｖＡ－ｙｖＢ］Ｔ
と表される。ここで、次のようにおく。
Figure JPOXMLDOC01-appb-M000018
 　地図座標系から車両座標系への方向余弦行列Ｃは、次のようになる。
Figure JPOXMLDOC01-appb-M000019
 　従って、
Figure JPOXMLDOC01-appb-M000020
 となる。
　式（２０）×ｄｘｍ＋式（２１）×ｄｙｍより、
Figure JPOXMLDOC01-appb-M000021
 が得られる。
　また、式（２０）×ｄｙｍ－式（２１）×ｄｘｍより、
Figure JPOXMLDOC01-appb-M000022
 が得られる。よって、式（２２）～（２４）を用いて、車両の方位角Ψを算出することができる。
　こうして、時刻ｔ－１及び時刻ｔにおいてそれぞれ車両の方位角Ψｔ－１、Ψｔを算出し、式（１７）に代入することにより、車両の方位角変化量ΔΨを算出することができる。
　［３．２．２］係数更新装置
　次に、第２実施例による係数更新装置について説明する。図８は、第２実施例による係数更新装置２０の構成を示すブロック図である。図示のように、係数更新装置２０は、ジャイロセンサ２１と、外界センサ２２と、地物計測部２３と、係数計算部２４と、方位角変化量計算部２５と、地図データベース（ＤＢ）２６とを備える。なお、地物計測部２３、係数計算部２４、及び、方位角変化量計算部２５は、ＣＰＵなどのコンピュータが予め用意されたプログラムを実行することにより実現することができる。
　ジャイロセンサ２１は、検出した角速度ωｔを係数計算部２４へ供給する。外界センサ２２は、例えばカメラ、ＬｉＤＡＲ、ミリ波レーダーなどである。地物計測部２３は、外界センサ２２の出力に基づいて、２つの地物の相対位置（ｘｖ，ｙｖ）を計測し、方位角変化量計算部２５へ供給する。
　方位角変化量計算部２５は、高精度地図を記憶した地図ＤＢ２６から、２つの地物の相対位置（ｘｍ，ｙｍ）を取得する。そして、方位角変化量計算部２５は、２つの地物の相対位置（ｘｖ，ｙｖ）、（ｘｍ，ｙｍ）を用いて、式（１８）～（２４）により方位角Ψｔを算出する。この処理を異なる２つの時刻で行うことにより、方位角変化量計算部２５は、方位角Ψｔ－１とΨｔを算出し、式（１７）により方位角変化量ΔΨを算出して係数計算部２４へ供給する。
　係数計算部２４は、ジャイロセンサ２１から供給された角速度ωｔと、方位角変化量計算部２５から供給された方位角変化量ΔΨに基づいて、前述の式（２）～（３）により、ジャイロセンサの感度係数α及びオフセット係数βを算出し、更新する。
　［３．２．３］係数更新処理
　次に、第２実施例による係数更新処理について説明する。図９は、第２実施例による係数更新処理のフローチャートである。
　まず、係数更新装置２０は、外界センサ１２により２つの地物の位置を取得する（ステップＳ２０）。次に、係数更新装置２０は、地図ＤＢ２６に記憶されている高精度地図から、それら２つの地物の位置を取得する（ステップＳ２１）。次に、係数更新装置２０は、外界センサより取得した２つの地物の位置と、高精度地図より取得した２つの地物の位置に基づいて、式（１８）～（２４）により、方位角Ψを算出する（ステップＳ２２）。
　次に、係数更新装置２０は、ｆｌａｇ＝１であるか否かを判定する（ステップＳ２３）。なお、「ｆｌａｇ」は処理の開始時に「０」にリセットされている。ｆｌａｇ＝１でない場合（ステップＳ２３：ＮＯ）、係数更新装置２０はｆｌａｇに「１」をセットし（ステップＳ２４）、ステップＳ２０へ戻る。
　一方、ｆｌａｇ＝１である場合（ステップＳ２３：ＹＥＳ）、２つの時刻においてそれぞれ２つの地物が検出され、方位角Ψｔ－１とΨｔが得られているので、係数更新装置２０は、式（１７）により方位角変化量ΔΨを算出する（ステップＳ２５）。
　次に、係数更新装置２０は、方位角変化量ΔΨと時間間隔Δｔから、式（２）を用いて真の角速度（計測ヨーレート）Ψ＿ｄｏｔ（ｔ）を算出する（ステップＳ２６）。そして、係数更新装置２０は、計測ヨーレートΨ＿ｄｏｔ（ｔ）とジャイロセンサの出力ωｔから、前述の式（３）により感度係数αとオフセット係数βを算出し、更新する（ステップＳ２７）。
　［４］３つ以上の地物を計測できた場合の処理
　上記の係数更新処理では２つの地物を計測しているが、同時に３つ以上の地物を計測できた場合には、以下の方法により方位角変化量ΔΨを算出することができる。
　（Ａ）平均値を使用する方法
　同時に３つ以上の地物を測定できた場合、方位角変化量ΔΨを複数通り計算し、それらを平均した値を係数の更新に使用することができる。
　例えば、地物を３個計測できた場合、図１０に示すように、地物１と地物２、地物２と地物３、地物３と地物１の組み合わせを選ぶことができる。それぞれの組み合わせにおいて、前述の係数更新処理により時刻ｔ－１から時刻ｔの間の方位角変化量ΔΨを計算する。地物１と地物２の組み合わせにより得られた方位角変化量をΔΨ１２とし、地物２と地物３の組み合わせにより得られた方位角変化量をΔΨ２３とし、地物３と地物１の組み合わせにより得られた方位角変化量をΔΨ３１とすると、以下のようにこれらを平均した値を方位角変化量ΔΨとして使用することができる。
Figure JPOXMLDOC01-appb-M000023
 これにより、方位角変化量ΔΨの精度を統計的に向上させることができる。
　（Ｂ）地物までの距離を考慮して２つの地物を選択する方法
　同時に３つ以上の地物を計測できた場合、そのうちの信頼度の高い２つの地物の組み合わせに基づいて方位角変化量を求めることにより、高精度で方位角変化量を得ることができる。一般的に、外界センサによる計測は、距離が遠いほど精度が下がる。従って、同時に３つ以上の地物を計測できた場合、車両からの距離が近い方から２つの地物を選択し、それらに基づいて第１実施例又は第２実施例の方法により方位角変化量ΔΨを求める。
　例えば、図１０の例のように３つの地物１～３が計測できた場合、車両からの３つの地物までの距離は、Ｌ１＜Ｌ３＜Ｌ２である。よって、係数更新装置は、車両からの距離が近い方の２つの地物、即ち、地物１及び地物３を用いて方位角変化量ΔΨを求めればよい。
　（Ｃ）地物間の距離を考慮して２つの地物を選択する方法
　通常、地物同士の位置が近すぎると計算精度が低下する。そこで、同時に３つ以上の地物を計測できた場合、予め決定した所定の閾値Ｌｔｈよりも近い２つの地物の組み合わせを排除する。
　例えば、図１１の例のように３つの地物１～３が計測できた場合、各地物間の距離を閾値Ｌｔｈと比較する。図１１の例では、Ｌ１２＜Ｌｔｈ、Ｌ２３＞Ｌｔｈ、Ｌ３１＞Ｌｔｈであるとする。この場合、地物間の距離が閾値Ｌｔｈよりも短い組み合わせ、即ち、地物１と地物２の組み合わせを除外し、地物２と地物３の組み合わせ、及び、地物１と地物３の組み合わせを使用して方位角変化量ΔΨを求めればよい。具体的には、上記の（Ｂ）の方法により車両から地物までの距離を考慮すると、地物２より地物１の方が車両から近い（Ｌ１＜Ｌ２）ので、係数更新装置は、地物１と地物３の組み合わせに基づいて方位角変化量ΔΨを求めればよい。
　その代わりに、上記の（Ａ）の方法を使用し、地物１と地物２の組み合わせにより得られる方位角変化量ΔΨ１２と地物１と地物３の組み合わせにより得られる方位角変化量ΔΨ３１との平均値を方位角変化量ΔΨとしても良い。なお、上記の方法では、予め所定距離を閾値Ｌｔｈとして定めているが、その代わりに、計測された３つ以上の地物間距離の平均値を閾値Ｌｔｈとして使用しても良い。
　［５］変形例
　上記の実施例では、２つの地物の計測結果から地物間距離Ｒを算出方法と、高精度地図の地図データを用いて地物間距離Ｒを取得する方法のいずれかを用いているが、両者を組み合わせて使用してもよい。例えば、高精度地図データが存在するエリアにおいては高精度地図データを使用して地物間距離Ｒを求め、高精度地図データが存在しないエリアでは地物の計測結果から地物間距離Ｒを求めてもよい。また、車両の状況に応じて、いずれか精度の高い方で得られた地物間距離Ｒを用いることとしてもよい。
　本発明は、移動体に搭載する装置に利用することができる。
　１１、２１　ジャイロセンサ
　１２、２２　外界センサ
　１３　進行方向取得部
　１４、２３　地物計測部
　１５　地物間距離計算部
　１６、２４　係数計算部
　１７、２５　方位角変化量計算部

Claims (9)

  1. 　第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する第１取得部と、
    　前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出部と、
    　を備えることを特徴とする測定装置。
  2. 　前記２つの地物間の距離を取得する第２取得部を更に備え、
    　前記算出部は、前記第１取得部及び前記第２取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出することを特徴とする請求項１に記載の測定装置。
  3. 　前記第２取得部は、前記２つの地物までの距離、及び、前記移動体の進行方向と前記２つの地物それぞれの方向とがなす角に基づいて、前記２つの地物間の距離を取得することを特徴とする請求項２に記載の距離推定装置。
  4. 　前記第２取得部は、地図情報に基づいて、前記２つの地物間の距離を取得することを特徴とする請求項２に記載の距離推定装置。
  5. 　前記変化量に基づき算出される、単位時間あたりの前記進行方向の変化量に基づき、前記移動体に搭載される角速度センサの前記第２時刻における出力結果から前記移動体のヨーレートを算出するための第１情報を補正する補正部を更に備えることを特徴とする請求項１乃至４のいずれか一項に記載の測定装置。
  6. 　前記第１情報は、前記角速度センサの感度及びオフセットを含むことを特徴とする請求項５に記載の測定装置。
  7. 　測定装置により実行される測定方法であって、
    　第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する第１取得工程と、
    　前記第１取得工程の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出工程と、
    　を備えることを特徴とする測定方法。
  8. 　コンピュータを備える測定装置によって実行されるプログラムであって、
    　第１時刻及び第２時刻それぞれにおける、移動体から２つの地物までのそれぞれの距離及び前記移動体からみた前記２つの地物の方向と前記移動体の進行方向とがそれぞれなす角度を取得する第１取得部、
    　前記第１取得部の取得結果に基づき、前記第１時刻から前記第２時刻までの前記移動体の進行方向の変化量を算出する算出部、
    　として前記コンピュータを機能させることを特徴とするプログラム。
  9. 　請求項８に記載のプログラムを記憶した記憶媒体。
PCT/JP2016/060234 2016-03-29 2016-03-29 測定装置、測定方法、及び、プログラム Ceased WO2017168588A1 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
PCT/JP2016/060234 WO2017168588A1 (ja) 2016-03-29 2016-03-29 測定装置、測定方法、及び、プログラム

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2016/060234 WO2017168588A1 (ja) 2016-03-29 2016-03-29 測定装置、測定方法、及び、プログラム

Publications (1)

Publication Number Publication Date
WO2017168588A1 true WO2017168588A1 (ja) 2017-10-05

Family

ID=59963733

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2016/060234 Ceased WO2017168588A1 (ja) 2016-03-29 2016-03-29 測定装置、測定方法、及び、プログラム

Country Status (1)

Country Link
WO (1) WO2017168588A1 (ja)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2020047210A (ja) * 2018-09-21 2020-03-26 トヨタ自動車株式会社 物体検出装置
CN110988989A (zh) * 2019-12-05 2020-04-10 大连民族大学 一种爆破振动速度幅值和方向修正的方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH08247775A (ja) * 1995-03-15 1996-09-27 Toshiba Corp 移動体の自己位置同定装置および自己位置同定方法
JP2006160116A (ja) * 2004-12-08 2006-06-22 Toyota Central Res & Dev Lab Inc 車両運動推定装置及び移動体検出装置
JP2008008783A (ja) * 2006-06-29 2008-01-17 Toyota Motor Corp 車輪速パルス補正装置
US20100017128A1 (en) * 2007-06-05 2010-01-21 Gm Global Technology Operations, Inc. Radar, Lidar and camera enhanced methods for vehicle dynamics estimation
JP2013072782A (ja) * 2011-09-28 2013-04-22 Aisin Aw Co Ltd 移動体位置検出システム、移動体位置検出装置、移動体位置検出方法及びコンピュータプログラム
JP2014098613A (ja) * 2012-11-14 2014-05-29 Kddi Corp ジャイロセンサの較正機能を備えた角速度測定装置、較正プログラム及び方法
US20150025786A1 (en) * 2012-02-28 2015-01-22 Continental Automotive Gmbh Method And Device For Determining The Speed And/Or Position Of A Vehicle

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH08247775A (ja) * 1995-03-15 1996-09-27 Toshiba Corp 移動体の自己位置同定装置および自己位置同定方法
JP2006160116A (ja) * 2004-12-08 2006-06-22 Toyota Central Res & Dev Lab Inc 車両運動推定装置及び移動体検出装置
JP2008008783A (ja) * 2006-06-29 2008-01-17 Toyota Motor Corp 車輪速パルス補正装置
US20100017128A1 (en) * 2007-06-05 2010-01-21 Gm Global Technology Operations, Inc. Radar, Lidar and camera enhanced methods for vehicle dynamics estimation
JP2013072782A (ja) * 2011-09-28 2013-04-22 Aisin Aw Co Ltd 移動体位置検出システム、移動体位置検出装置、移動体位置検出方法及びコンピュータプログラム
US20150025786A1 (en) * 2012-02-28 2015-01-22 Continental Automotive Gmbh Method And Device For Determining The Speed And/Or Position Of A Vehicle
JP2014098613A (ja) * 2012-11-14 2014-05-29 Kddi Corp ジャイロセンサの較正機能を備えた角速度測定装置、較正プログラム及び方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2020047210A (ja) * 2018-09-21 2020-03-26 トヨタ自動車株式会社 物体検出装置
JP7020353B2 (ja) 2018-09-21 2022-02-16 トヨタ自動車株式会社 物体検出装置
CN110988989A (zh) * 2019-12-05 2020-04-10 大连民族大学 一种爆破振动速度幅值和方向修正的方法

Similar Documents

Publication Publication Date Title
US10634777B2 (en) Radar odometry for vehicle
US8041472B2 (en) Positioning device, and navigation system
US9952597B2 (en) Apparatus for correcting vehicle location
JP4964047B2 (ja) 位置検出装置及び位置検出方法
EP3109589B1 (en) A unit and method for improving positioning accuracy
JP6354556B2 (ja) 位置推定装置、位置推定方法、位置推定プログラム
US20200072617A1 (en) Host vehicle position estimation device
WO2019188745A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
WO2012086401A1 (ja) 運転支援装置
JP2024103654A (ja) 計測精度算出装置、自己位置推定装置、制御方法、プログラム及び記憶媒体
JP6569572B2 (ja) 車両位置決定装置
JP2023164553A (ja) 位置推定装置、推定装置、制御方法、プログラム及び記憶媒体
WO2018212292A1 (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
WO2017109978A1 (ja) 距離推定装置、距離推定方法及びプログラム
JP6503477B2 (ja) 距離推定装置、距離推定方法及びプログラム
JP2019052955A (ja) 測定装置、測定方法、及び、プログラム
WO2017168588A1 (ja) 測定装置、測定方法、及び、プログラム
JP2017181195A (ja) 測定装置、測定方法、及び、プログラム
JP6925428B2 (ja) 測定装置、測定方法、及び、プログラム
JP6707627B2 (ja) 測定装置、測定方法、及び、プログラム
CN114279439A (zh) 一种惯导数据的修正方法、装置和系统
JP2023099851A (ja) 測定装置、測定方法およびプログラム
JP2017177962A (ja) 測定装置、測定方法、及び、プログラム
KR20190079470A (ko) 객체 측위 방법 및 장치
CN113167910B (zh) 一种用于估算车辆的姿态的方法

Legal Events

Date Code Title Description
NENP Non-entry into the national phase

Ref country code: DE

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 16896808

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 16896808

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: JP