JP2022115927A

JP2022115927A - Estimation device, control method, program, and storage media

Info

Publication number: JP2022115927A
Application number: JP2022074663A
Authority: JP
Inventors: 正浩加藤; Masahiro Kato
Original assignee: Pioneer Electronic Corp
Current assignee: Pioneer Corp
Priority date: 2018-03-23
Filing date: 2022-04-28
Publication date: 2022-08-09
Also published as: JP2024016186A; JPWO2019182082A1; WO2019182082A1

Abstract

PROBLEM TO BE SOLVED: To provide an estimation device capable of suitably estimating an attitude of a measuring unit for measuring distance to an object relative to a moving body.

SOLUTION: An in-vehicle device at least estimates an attitude of a rider who measures distance to an object relative to a vehicle, and performs processing of estimating the attitude of the rider relative to the vehicle based on detection results of an acceleration sensor for the rider provided on the rider when the vehicle is traveling while accelerating or decelerating.

SELECTED DRAWING: Figure 12

Description

本発明は、計測部の姿勢を推定する技術に関する。 The present invention relates to technology for estimating the posture of a measuring unit.

従来から、レーダやカメラなどの計測部の計測データに基づいて、自車位置推定などを行う技術が知られている。例えば、特許文献１には、計測センサの出力と、予め地図上に登録された地物の位置情報とを照合させることで自己位置を推定する技術が開示されている。また、特許文献２には、カルマンフィルタを用いた自車位置推定技術が開示されている。 2. Description of the Related Art Conventionally, there has been known a technique for estimating a position of a vehicle based on measurement data from measurement units such as radar and cameras. For example, Patent Literature 1 discloses a technique of estimating a self-position by collating the output of a measurement sensor with the position information of a feature registered in advance on a map. Further, Patent Document 2 discloses a vehicle position estimation technique using a Kalman filter.

特開２０１３－２５７７４２号公報JP 2013-257742 A 特開２０１７－７２４２２号公報JP 2017-72422 A

レーダやカメラなどの計測部から得られるデータは、計測部を基準とした座標系の値であり、車両に対する計測部の姿勢等に依存したデータとなっているため、車両を基準とした座標系の値に変換する必要がある。従って、計測部の姿勢にずれが生じた場合には、そのずれを的確に検知して計測部のデータに反映させる必要がある。 Data obtained from measurement units such as radar and cameras are values in a coordinate system based on the measurement unit, and are data dependent on the attitude of the measurement unit relative to the vehicle. must be converted to the value of Therefore, when the posture of the measurement unit deviates, it is necessary to accurately detect the deviation and reflect it in the data of the measurement unit.

本発明は、上記のような課題を解決するためになされたものであり、対象物に対する距離を計測する計測部の移動体に対する姿勢を好適に推定可能な推定装置を提供することを主な目的とする。 SUMMARY OF THE INVENTION The present invention has been made to solve the problems described above, and a main object of the present invention is to provide an estimating apparatus capable of suitably estimating the posture of a measuring unit that measures the distance to an object relative to a moving object. and

請求項に記載の発明は、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定する推定装置であって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定部を有する。 A claimed invention is an estimating device for estimating a posture of a measuring unit for measuring a distance to an object with respect to a moving object, wherein the measuring unit detects a An estimating unit for estimating the posture of the measuring unit with respect to the moving body based on the detection result of the provided acceleration detecting unit.

また、請求項に記載の発明は、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定する推定装置が実行する制御方法であって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定工程を有する。 Further, the claimed invention is a control method executed by an estimating device for estimating a posture of a measuring unit for measuring a distance to an object with respect to a moving body, wherein the moving body is traveling while accelerating or decelerating. an estimating step of estimating the posture of the measuring unit with respect to the moving object based on the detection result of an acceleration detecting unit provided in the measuring unit when the time is set.

また、請求項に記載の発明は、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定するコンピュータが実行するプログラムであって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定部として前記コンピュータを機能させる。 Further, the claimed invention is a computer-executable program for estimating the posture of a measuring unit for measuring a distance to an object with respect to a moving body, wherein the computer executes the program when the moving body is accelerating and decelerating. and causing the computer to function as an estimating section for estimating the attitude of the measuring section with respect to the moving body based on the detection result of the acceleration detecting section provided in the measuring section.

運転支援システムの概略構成図である。1 is a schematic configuration diagram of a driving support system; FIG. 車載機の機能的構成を示すブロック図である。3 is a block diagram showing a functional configuration of an in-vehicle device; FIG. ２次元座標により表された車両座標系とライダ座標系との関係を示す図である。FIG. 2 is a diagram showing the relationship between a vehicle coordinate system and a rider coordinate system represented by two-dimensional coordinates; ３次元座標により表された車両座標系とライダ座標系との関係を示す図である。FIG. 2 is a diagram showing the relationship between a vehicle coordinate system and a rider coordinate system represented by three-dimensional coordinates; 車両座標系及びライダ座標系における重力加速度のベクトルを示した図である。FIG. 3 is a diagram showing vectors of gravitational acceleration in a vehicle coordinate system and a rider coordinate system; ライダのｚ方向位置の変化前後において平坦な路面を車両が走行中のときにライダにより計測された路面のｚ方向の計測値を示した図である。FIG. 5 is a diagram showing measured values in the z-direction of the road surface measured by the rider while the vehicle is running on a flat road surface before and after the change in the z-direction position of the rider; 上り坂の始点の前後を走行中の車両のライダのｚ方向の計測値の大きさを示す図である。FIG. 4 is a diagram showing the magnitude of z-direction measurements of the rider of a vehicle traveling before and after the starting point of an uphill; 車両の走行時に得られる計測値及び車体ピッチ角の時間変化を示すグラフである。4 is a graph showing changes over time in measured values and vehicle body pitch angles obtained while the vehicle is running. バンプの前後を走行中の車両のライダのｚ方向の計測値の大きさを示す図である。FIG. 4 is a diagram showing the magnitude of the z-direction measurements of the rider of a vehicle traveling in front of and behind a bump; 車両の走行時に得られる計測値及び車体ピッチレートの時間変化を示すグラフである。4 is a graph showing changes over time in measured values and vehicle body pitch rates obtained while the vehicle is running. 旋回中の車両に生じる作用を概略的に示した図である。FIG. 4 is a diagram schematically showing the action that occurs on a vehicle during turning; ライダの出力を補正する処理の手順を示すフローチャートの一例である。6 is an example of a flowchart showing a procedure of processing for correcting the output of a rider;

本発明の好適な実施形態によれば、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定する推定装置であって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定部を有する。推定装置は、この態様により、車両に対するヨー方向における計測部の姿勢を好適に推定することができる。なお、「加減速して走行している」態様とは、加速して走行している態様、及び、減速して走行している態様のいずれの態様も含む。 According to a preferred embodiment of the present invention, there is provided an estimating apparatus for estimating the attitude of a measuring unit for measuring a distance to an object with respect to a moving object, wherein the above-described An estimating unit for estimating the posture of the measuring unit with respect to the moving body based on the detection result of the acceleration detecting unit provided in the measuring unit. With this aspect, the estimating device can preferably estimate the posture of the measuring unit in the yaw direction with respect to the vehicle. It should be noted that the mode of "running with acceleration and deceleration" includes both modes of running with acceleration and running with deceleration.

上記推定装置の一態様では、前記推定部は、前記移動体が所定速度により走行または停止しているときの前記加速度検出部が出力する加速度データに基づいて、前記計測部のロール方向及びピッチ方向の姿勢を推定し、前記移動体が加減速して走行しているときの前記加速度検出部が出力する加速度データ並びに前記推定されたロール方向及びピッチ方向の姿勢に基づいて、前記計測部のヨー方向の姿勢を推定する。この態様により、推定装置は、移動体が所定速度により走行または停止しているときには車両に対するロール方向及びピッチ方向における計測部の姿勢を好適に推定し、移動体が加減速して走行しているときには車両に対するヨー方向における計測部の姿勢を好適に推定することができる。 In one aspect of the estimating device, the estimating unit detects the roll direction and the pitch direction of the measuring unit based on the acceleration data output by the acceleration detecting unit when the mobile body is running or stopped at a predetermined speed. and based on the acceleration data output from the acceleration detection unit when the moving object is accelerating and decelerating and the estimated postures in the roll direction and the pitch direction, the yaw of the measuring unit is determined. Estimate the pose of the direction. According to this aspect, the estimating device suitably estimates the posture of the measuring unit in the roll direction and the pitch direction with respect to the vehicle when the moving object is traveling or stopping at a predetermined speed, and the moving object is traveling while accelerating or decelerating. Sometimes, the posture of the measurement unit in the yaw direction with respect to the vehicle can be estimated favorably.

上記推定装置の他の一態様では、前記推定部は、推定した前記計測部の姿勢と、記憶部に記憶された前記計測部の姿勢とに基づき、前記姿勢の変化量を推定する。これにより、推定装置は、記憶部に記憶された標準の姿勢に対する現在の姿勢の変化量を好適に推定することができる。 In another aspect of the estimation device, the estimation unit estimates the amount of change in the posture based on the estimated posture of the measurement unit and the posture of the measurement unit stored in the storage unit. Accordingly, the estimation device can suitably estimate the amount of change in the current posture with respect to the standard posture stored in the storage unit.

上記推定装置の他の一態様では、前記推定部は、高さ方向における路面の位置を示す前記計測部の計測データに基づいて、前記計測部の前記高さ方向の位置を推定する。この態様により、推定装置は、計測部の高さ方向における位置を好適に推定することができる。 In another aspect of the estimating device, the estimating unit estimates the position of the measuring unit in the height direction based on the measurement data of the measuring unit that indicates the position of the road surface in the height direction. With this aspect, the estimating device can preferably estimate the position of the measuring unit in the height direction.

上記推定装置の他の一態様では、前記推定部は、勾配が変化する道路地点が前記計測部により計測されたときの前記道路地点と前記移動体との距離に基づいて、前記移動体の前後方向における前記計測部の位置を推定する。この態様により、推定装置は、移動体の前後方向における計測部の位置を好適に推定することができる。 In another aspect of the estimating device, the estimating unit measures the distance between the moving object and the road spot where the gradient changes, based on the distance between the road spot and the moving object when measured by the measuring unit. Estimate the position of the measurement unit in a direction. According to this aspect, the estimating device can preferably estimate the position of the measuring unit in the front-rear direction of the moving object.

上記推定装置の他の一態様では、前記推定部は、前記移動体のピッチ方向の傾きを検出する傾き検出部が出力するデータの変化と、前記計測部が出力する計測データの変化との時間差に基づき、前記距離を算出する。この態様により、推定装置は、勾配が変化する道路地点が計測部により計測されたときの道路地点と移動体との距離を好適に算出し、移動体の前後方向における計測部の位置を推定に用いることができる。 In another aspect of the estimating device, the estimating unit includes a time difference between a change in data output by a tilt detecting unit that detects a tilt in the pitch direction of the moving object and a change in measurement data output by the measuring unit. to calculate the distance. According to this aspect, the estimating device suitably calculates the distance between the road point and the moving body when the road point where the gradient changes is measured by the measuring unit, and estimates the position of the measuring unit in the front-rear direction of the moving body. can be used.

上記推定装置の他の一態様では、前記推定部は、前記移動体の旋回中において前記加速度検出部が出力する前記移動体の左右方向の加速度データと、前記移動体に搭載された加速度センサが出力する前記移動体の左右方向の加速度データと、前記移動体に搭載されたジャイロセンサが出力する前記移動体のヨーレートとに基づいて、前記移動体の左右方向における前記計測部の位置を推定する。この態様により、推定装置は、移動体の左右方向における計測部の位置を好適に推定することができる。 In another aspect of the estimating device, the estimating unit includes acceleration data in the horizontal direction of the moving body output by the acceleration detecting unit while the moving body is turning, and an acceleration sensor mounted on the moving body. estimating the position of the measuring unit in the left-right direction of the moving body based on output acceleration data in the left-right direction of the moving body and a yaw rate of the moving body output by a gyro sensor mounted on the moving body; . According to this aspect, the estimating device can suitably estimate the position of the measuring unit in the left-right direction of the moving object.

上記推定装置の他の一態様では、前記推定部は、推定した前記計測部の位置と、記憶部に記憶された前記計測部の位置とに基づき、前記位置の変化量を推定する。これにより、推定装置は、記憶部に記憶された計測部の標準の位置に対する現在の位置の変化量を好適に推定することができる。 In another aspect of the estimation device, the estimation unit estimates the amount of change in the position based on the estimated position of the measurement unit and the position of the measurement unit stored in the storage unit. Thereby, the estimating device can suitably estimate the amount of change in the current position of the measuring unit from the standard position stored in the storage unit.

上記推定装置の他の一態様では、前記変化量に基づき、前記計測部が出力する計測データを補正する補正部をさらに備える。推定装置は、この態様により、計測部の姿勢や位置のずれが生じた場合であっても、ずれの影響が生じないように計測部の計測データを補正することができる。 In another aspect of the estimation device, the estimation device further includes a correction unit that corrects the measurement data output from the measurement unit based on the amount of change. With this aspect, the estimating device can correct the measurement data of the measuring unit so that the influence of the displacement does not occur even when the attitude or position of the measuring unit is displaced.

上記推定装置の他の一態様では、前記変化量が所定量以上である場合、前記計測部が出力する計測データに基づく処理を停止する停止制御部をさらに備える。この態様により、推定装置は、姿勢や位置のずれが大きい計測部の計測データを用いることによる当該計測データを用いる種々の処理の精度が低下するのを確実に抑制することができる。 In another aspect of the estimation apparatus, the apparatus further includes a stop control unit that stops processing based on the measurement data output by the measurement unit when the amount of change is equal to or greater than a predetermined amount. According to this aspect, the estimating device can reliably suppress deterioration in the accuracy of various processes using the measurement data due to the use of the measurement data of the measurement unit with large deviations in orientation and position.

本発明の他の好適な実施形態によれば、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定する推定装置が実行する制御方法であって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定工程を有する。推定装置は、この制御方法を用いることで、車両に対するヨー方向における計測部の姿勢を好適に推定することができる。 According to another preferred embodiment of the present invention, there is provided a control method executed by an estimating device for estimating a posture of a measuring unit for measuring a distance to an object with respect to a moving object, wherein the moving object accelerates or decelerates to travel. an estimating step of estimating the posture of the measuring unit with respect to the moving body based on the detection result of the acceleration detecting unit provided in the measuring unit when the moving body is moving. By using this control method, the estimating device can suitably estimate the posture of the measuring unit in the yaw direction with respect to the vehicle.

本発明の他の好適な実施形態によれば、対象物に対する距離を計測する計測部の移動体に対する姿勢を推定するコンピュータが実行するプログラムであって、前記移動体が加減速して走行しているときの、前記計測部に設けられた加速度検出部の検出結果に基づいて、前記移動体に対する前記計測部の姿勢を推定する推定部として前記コンピュータを機能させる。コンピュータは、このプログラムを実行することで、車両に対するヨー方向における計測部の姿勢を好適に推定することができる。好適には、上記プログラムは、記憶媒体に記憶される。 According to another preferred embodiment of the present invention, there is provided a computer-executable program for estimating the posture of a measuring unit for measuring a distance to an object with respect to a moving object, wherein the moving object accelerates or decelerates and travels. The computer functions as an estimating unit that estimates the posture of the measuring unit with respect to the moving body based on the detection result of the acceleration detecting unit provided in the measuring unit when the moving object is moving. By executing this program, the computer can suitably estimate the posture of the measurement unit in the yaw direction with respect to the vehicle. Preferably, the program is stored in a storage medium.

以下、図面を参照して本発明の好適な各実施例について説明する。 Preferred embodiments of the present invention will be described below with reference to the drawings.

［概略構成］
図１は、本実施例に係る運転支援システムの概略構成図である。図１に示す運転支援システムは、車両に搭載され、車両の運転支援に関する制御を行う車載機１と、ライダ（Ｌｉｄａｒ：ＬｉｇｈｔＤｅｔｅｃｔｉｏｎａｎｄＲａｎｇｉｎｇ、または、ＬａｓｅｒＩｌｌｕｍｉｎａｔｅｄＤｅｔｅｃｔｉｏｎＡｎｄＲａｎｇｉｎｇ）２と、ジャイロセンサ３と、車体用加速度センサ４と、ライダ用加速度センサ５とを有する。 [Outline configuration]
FIG. 1 is a schematic configuration diagram of a driving support system according to this embodiment. The driving support system shown in FIG. 1 is mounted on a vehicle and includes an in-vehicle device 1 that performs control related to driving support of the vehicle, a lidar (Light Detection and Ranging, or Laser Illuminated Detection And Ranging) 2, and a gyro sensor 3. , a vehicle body acceleration sensor 4 and a rider acceleration sensor 5 .

車載機１は、ライダ２、ジャイロセンサ３、車体用加速度センサ４、及びライダ用加速度センサ５と電気的に接続し、これらの出力データを取得する。また、道路データ及び道路付近に設けられた地物に関する地物情報などを記憶した地図データベース（ＤＢ：ＤａｔａＢａｓｅ）１０を記憶している。そして、車載機１は、上述の出力データ及び地図ＤＢ１０に基づき、車両の位置（「自車位置」とも呼ぶ。）の推定を行い、自車位置の推定結果に基づいて自動運転制御などの車両の運転支援に関する制御などを行う。また、車載機１は、ライダ２、ジャイロセンサ３、車体用加速度センサ４、及びライダ用加速度センサ５の出力に基づいて、ライダ２の姿勢及び位置の推定を行う。そして、車載機１は、この推定結果に基づいて、ライダ２が出力する点群データの各計測値を補正する処理などを行う。車載機１は、本発明における「推定装置」の一例である。 The in-vehicle device 1 is electrically connected to the rider 2, the gyro sensor 3, the vehicle acceleration sensor 4, and the rider acceleration sensor 5, and acquires output data from them. It also stores a map database (DB: DataBase) 10 that stores road data and feature information about features provided near roads. Then, the in-vehicle device 1 estimates the position of the vehicle (also referred to as “own vehicle position”) based on the output data and the map DB 10 described above, and controls the vehicle such as automatic driving control based on the estimation result of the own vehicle position. control related to driving support. The vehicle-mounted device 1 also estimates the attitude and position of the rider 2 based on the outputs of the rider 2 , the gyro sensor 3 , the vehicle body acceleration sensor 4 , and the rider acceleration sensor 5 . Based on this estimation result, the in-vehicle device 1 performs processing such as correcting each measurement value of the point cloud data output by the rider 2 . The in-vehicle device 1 is an example of the "estimation device" in the present invention.

ライダ２は、水平方向および垂直方向の所定の角度範囲に対してパルスレーザを出射することで、外界に存在する物体までの距離を離散的に測定し、当該物体の位置を示す３次元の点群情報を生成する。この場合、ライダ２は、照射方向を変えながらレーザ光を照射する照射部と、照射したレーザ光の反射光（散乱光）を受光する受光部と、受光部が出力する受光信号に基づくスキャンデータを出力する出力部とを有する。スキャンデータは、受光部が受光したレーザ光に対応する照射方向と、上述の受光信号に基づき特定される当該レーザ光のその照射方向での物体までの距離とに基づき生成され、車載機１へ供給される。本実施例では、一例として、ライダ２は、車両のフロント部分とリア部分とにそれぞれ設けられている。ライダ２は、本発明における「計測部」の一例である。 The lidar 2 discretely measures the distance to an object existing in the outside world by emitting a pulsed laser over a predetermined angular range in the horizontal and vertical directions, and generates a three-dimensional point indicating the position of the object. Generate group information. In this case, the lidar 2 includes an irradiation unit that irradiates laser light while changing the direction of irradiation, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and scan data based on light receiving signals output by the light receiving unit. and an output unit for outputting The scan data is generated based on the irradiation direction corresponding to the laser beam received by the light receiving unit and the distance to the object in the irradiation direction of the laser beam specified based on the above-mentioned light receiving signal, and is sent to the vehicle-mounted device 1. supplied. In this embodiment, as an example, the riders 2 are provided at the front portion and the rear portion of the vehicle. The rider 2 is an example of a "measurement section" in the present invention.

ジャイロセンサ３は、車両に設けられ、車体のヨーレートに相当する出力信号を車載機１へ供給する。車体用加速度センサ４は、車両に設けられた３軸加速度センサであり、車体の進行方向、側面方向、高さ方向に相当する３軸の加速度データに相当する検出信号を車載機１へ供給する。ジャイロセンサ３及び車体用加速度センサ４は、本発明における「傾き検出部」の一例である。ライダ用加速度センサ５は、各ライダ２に設けられた３軸加速度センサであり、設置されたライダ２の３軸の加速度データに相当する検出信号をそれぞれ車載機１へ供給する。ライダ用加速度センサ５は、本発明における「加速度検出部」の一例である。 The gyro sensor 3 is provided in the vehicle and supplies an output signal corresponding to the yaw rate of the vehicle body to the vehicle-mounted device 1 . The vehicle body acceleration sensor 4 is a 3-axis acceleration sensor provided in the vehicle, and supplies detection signals corresponding to 3-axis acceleration data corresponding to the traveling direction, side direction, and height direction of the vehicle body to the in-vehicle device 1. . The gyro sensor 3 and the vehicle body acceleration sensor 4 are examples of the "tilt detector" in the present invention. The rider acceleration sensor 5 is a 3-axis acceleration sensor provided for each rider 2 and supplies detection signals corresponding to 3-axis acceleration data of the installed rider 2 to the vehicle-mounted device 1 . The rider acceleration sensor 5 is an example of the "acceleration detector" in the present invention.

図２は、車載機２の機能的構成を示すブロック図である。車載機２は、主に、インターフェース１１と、記憶部１２と、入力部１４と、制御部１５と、情報出力部１６と、を有する。これらの各要素は、バスラインを介して相互に接続されている。 FIG. 2 is a block diagram showing the functional configuration of the vehicle-mounted device 2. As shown in FIG. The in-vehicle device 2 mainly has an interface 11 , a storage section 12 , an input section 14 , a control section 15 and an information output section 16 . These elements are interconnected via bus lines.

インターフェース１１は、ライダ２、ジャイロセンサ３、車体用加速度センサ４、及びライダ用加速度センサ５などのセンサから出力データを取得し、制御部１５へ供給する。また、インターフェース１１は、制御部１５が生成した車両の走行制御に関する信号を車両の電子制御装置（ＥＣＵ：ＥｌｅｃｔｒｏｎｉｃＣｏｎｔｒｏｌＵｎｉｔ）へ供給する。 The interface 11 acquires output data from sensors such as the rider 2 , the gyro sensor 3 , the vehicle body acceleration sensor 4 , and the rider acceleration sensor 5 , and supplies the output data to the control unit 15 . In addition, the interface 11 supplies a signal related to vehicle travel control generated by the control unit 15 to an electronic control unit (ECU) of the vehicle.

記憶部１２は、制御部１５が実行するプログラムや、制御部１５が所定の処理を実行するのに必要な情報を記憶する。本実施例では、記憶部１２は、地図ＤＢ１０と、ライダ設置情報ＩＬとを有する。ライダ設置情報ＩＬは、ある基準時（例えばライダ２のアライメント調整直後などの姿勢・位置ずれが生じていない時）における各ライダ２の相対的な３次元位置と姿勢に関する情報である。本実施例では、ライダ２等の姿勢を、ロール角、ピッチ角、ヨー角（即ちオイラー角）により表すものとする。ライダ設置情報ＩＬは、上述の基準時において実測された位置及び姿勢に関する情報であってもよく、後述するライダ２の位置及び姿勢の推定処理により車載機１によって推定されたライダ２の位置及び姿勢に関する情報であってもよい。 The storage unit 12 stores programs executed by the control unit 15 and information necessary for the control unit 15 to execute predetermined processing. In this embodiment, the storage unit 12 has a map DB 10 and rider installation information IL. The rider installation information IL is information about the relative three-dimensional position and attitude of each rider 2 at a certain reference time (for example, when there is no attitude/positional deviation such as immediately after alignment adjustment of the rider 2). In this embodiment, the posture of the rider 2 and the like is represented by roll angle, pitch angle, and yaw angle (that is, Euler angle). The rider installation information IL may be information about the position and attitude actually measured at the reference time described above, and the position and attitude of the rider 2 estimated by the vehicle-mounted device 1 by the estimation processing of the position and attitude of the rider 2 to be described later. It may be information about

入力部１４は、ユーザが操作するためのボタン、タッチパネル、リモートコントローラ、音声入力装置等であり、経路探索のための目的地を指定する入力、自動運転のオン及びオフを指定する入力などを受け付ける。情報出力部１６は、例えば、制御部１５の制御に基づき出力を行うディスプレイやスピーカ等である。 The input unit 14 is a button, touch panel, remote controller, voice input device, etc. for user operation, and accepts input specifying a destination for route search, input specifying ON and OFF of automatic driving, etc. . The information output unit 16 is, for example, a display, a speaker, or the like that outputs based on the control of the control unit 15 .

制御部１５は、プログラムを実行するＣＰＵなどを含み、車載機１の全体を制御する。制御部１５は、インターフェース１１から供給される各センサの出力信号及び地図ＤＢ１０に基づき、自車位置の推定を行い、自車位置の推定結果に基づいて自動運転制御を含む車両の運転支援に関する制御などを行う。このとき、制御部１５は、ライダ２の出力データを用いる場合には、ライダ２が出力する計測データを、ライダ設置情報ＩＬに記録されたライダ２の姿勢及び位置を基準として、ライダ２を基準とした座標系から車両を基準とした座標系に変換する。さらに、本実施例では、制御部１５は、車両に対するライダ２の現在（即ち処理基準時）の位置及び姿勢を推定することで、ライダ設置情報ＩＬに記録された位置及び姿勢に対する変化量を算出し、当該変化量に基づきライダ２が出力する計測データを補正する。これにより、制御部１５は、ライダ２の位置又は姿勢にずれが生じた場合であっても、当該ずれの影響を受けないようにライダ２が出力する計測データを補正する。制御部１５は、本発明における「推定部」、「補正部」、「停止制御部」及びプログラムを実行する「コンピュータ」の一例である。 The control unit 15 includes a CPU that executes programs and the like, and controls the entire in-vehicle device 1 . The control unit 15 estimates the position of the vehicle based on the output signal of each sensor supplied from the interface 11 and the map DB 10, and performs control related to vehicle driving support including automatic driving control based on the estimated result of the vehicle position. etc. At this time, when the output data of the rider 2 is used, the control unit 15 sets the measurement data output by the rider 2 based on the attitude and position of the rider 2 recorded in the rider installation information IL. The coordinate system based on the vehicle is converted into a coordinate system based on the vehicle. Furthermore, in this embodiment, the control unit 15 estimates the current position and orientation of the rider 2 with respect to the vehicle (that is, at the time of the processing reference), thereby calculating the amount of change with respect to the position and orientation recorded in the rider installation information IL. Then, the measurement data output by the rider 2 is corrected based on the amount of change. As a result, even if the position or posture of the rider 2 deviates, the control unit 15 corrects the measurement data output by the rider 2 so as not to be affected by the deviation. The control unit 15 is an example of an "estimation unit", a "correction unit", a "stop control unit", and a "computer" that executes a program in the present invention.

［ライダの位置及び姿勢推定］
次に、ライダ２の位置及び姿勢の推定方法について説明する。車載機１は、以下に示す処理を、ライダ２ごとに実行する。 [Lidar Position and Posture Estimation]
Next, a method for estimating the position and orientation of the rider 2 will be described. The in-vehicle device 1 executes the following processing for each rider 2.

（１）座標系の変換
ライダ２により取得される３次元点群データの各計測点が示す３次元座標は、ライダ２の位置及び姿勢を基準とした座標系（「ライダ座標系」とも呼ぶ。）で表されており、車両の位置及び姿勢を基準とした座標系（「車両座標系」とも呼ぶ。）に変換する必要がある。ここでは、まず、ライダ座標系と車両座標系との変換について説明する。 (1) Transformation of coordinate system
The three-dimensional coordinates indicated by each measurement point of the three-dimensional point cloud data acquired by the rider 2 are expressed in a coordinate system (also referred to as a "rider coordinate system") based on the position and orientation of the rider 2, It is necessary to convert to a coordinate system based on the position and orientation of the vehicle (also called a "vehicle coordinate system"). Here, first, conversion between the lidar coordinate system and the vehicle coordinate system will be described.

図３は、２次元座標により表された車両座標系とライダ座標系との関係を示す図である。ここでは、車両座標系は、車両の中心を原点とし、車両の進行方向に沿った座標軸「ｘ_ｂ」と車両の側面方向に沿った座標軸「ｙ_ｂ」を有する。また、ライダ座標系は、ライダ２の正面方向（矢印Ａ２参照）に沿った座標軸「ｘ_Ｌ」とライダ２の側面方向に沿った座標軸「ｙ_Ｌ」を有する。 FIG. 3 is a diagram showing the relationship between the vehicle coordinate system and the rider coordinate system represented by two-dimensional coordinates. Here, the vehicle coordinate system has the center of the vehicle as the origin, and has a coordinate axis " _xb " along the traveling direction of the vehicle and a coordinate axis " _yb " along the lateral direction of the vehicle. The lidar coordinate system also has a coordinate axis “x _L ” along the front direction of the rider 2 (see arrow A2) and a coordinate axis “y _L ” along the lateral direction of the rider 2 .

ここで、車両座標系に対するライダ２のヨー角を「Ｌ_ψ０」、ライダ２の位置を［Ｌ_ｘ０、Ｌ_ｙ０］^Ｔとした場合、車両座標系から見た時刻「ｋ」の計測点［ｘ_ｂ（ｋ）、ｙ_ｂ（ｋ）］^Ｔは、回転行列「Ｃ_ψ０」を用いた以下の式（１）によりライダ座標系の座標［ｘ_Ｌ（ｋ）、ｙ_Ｌ（ｋ）］^Ｔへ変換される。 Here, when the yaw angle of the rider 2 with respect to the vehicle coordinate system is “L _ψ0 ” and the position of the rider 2 is [L _x0 , L _y0 ] ^T , the measurement point [x _b (k), y _b (k)] ^T is converted to coordinates [x _L (k), y _L (k)] ^T in the lidar coordinate system by the following equation (1) using the rotation matrix “C _ψ0 ” converted.

一方、ライダ座標系から車両座標系への変換は、回転行列の逆行列（転置行列）を用いればよい。よって、ライダ座標系で取得した時刻ｋの計測点［ｘ_Ｌ（ｋ）、ｙ_Ｌ（ｋ）］^Ｔは、以下の式（２）により車両座標系の座標［ｘ_ｂ（ｋ）、ｙ_ｂ（ｋ）］^Ｔに変換することが可能である。

On the other hand, the inverse matrix (transposed matrix) of the rotation matrix may be used for conversion from the rider coordinate system to the vehicle coordinate system. Therefore, the measurement point [x _L (k), y _L (k)] ^T at time k acquired in the rider coordinate system is obtained by the following equation (2), and the coordinates [x _b (k), y _b (k)] can be transformed into ^T.

図４は、３次元座標により表された車両座標系とライダ座標系との関係を示す図である。ここでは、座標軸ｘ_ｂ、ｙ_ｂに垂直な座標軸を「ｚ_ｂ」、座標軸ｘ_Ｌ、ｙ_Ｌに垂直な座標軸を「ｚ_Ｌ」とする。

FIG. 4 is a diagram showing the relationship between the vehicle coordinate system and the rider coordinate system represented by three-dimensional coordinates. Here, the coordinate axis perpendicular to the coordinate axes _xb and _yb is " _zb ", and the coordinate axis perpendicular to the coordinate axes _xL and _yL is " _zL ".

車両座標系に対するライダ２のロール角を「Ｌ_φ０」、ピッチ角を「Ｌ_θ０」、ヨー角を「Ｌ_ψ０」とし、ライダ２の座標軸ｘ_ｂにおける位置が「Ｌ_ｘ０」、座標軸ｙ_ｂにおける位置が「Ｌ_ｙ０」、座標軸ｚ_ｂにおける位置が「Ｌ_ｚ０」とした場合、車両座標系から見た時刻「ｋ」の計測点［ｘ_ｂ０（ｋ）、ｙ_ｂ０（ｋ）、ｚ_ｂ０（ｋ）］^Ｔは、ロール、ピッチ、ヨーに対応する各回転行列「Ｃ_φ０」、「Ｃ_θ０」、「Ｃ_ψ０」により表される方向余弦行列「Ｃ_０」を用いた以下の式（３）により、ライダ座標系の座標［ｘ_Ｌ０（ｋ）、ｙ_Ｌ０（ｋ）、ｚ_Ｌ０（ｋ）］^Ｔへ変換される。 Let the roll angle of the rider 2 with respect to the vehicle _coordinate system be “L _φ0 ”, the _pitch angle be “L _θ0 ”, and the _yaw angle be “L _ψ0 ”. When the position is “L _y0 ” and the position on the coordinate axis z _b is “L _z0 ”, the measurement point [x _b0 (k), y _b0 (k), z _b0 ( _k )] ^T is the _following _equation ( ₃ ) into coordinates [x _L0 (k), y _L0 (k), z _L0 (k)] ^T in the lidar coordinate system.

一方、ライダ座標系から車両座標系への変換は、方向余弦行列の逆行列（転置行列）を用いればよい。よって、ライダ座標系で取得した時刻ｋの計測点［ｘ_Ｌ０（ｋ）、ｙ_Ｌ０（ｋ）、ｚ_Ｌ０（ｋ）］^Ｔは、以下の式（４）により車両座標系の座標［ｘ_ｂ０（ｋ）、ｙ_ｂ０（ｋ）、ｚ_ｂ０（ｋ）］^Ｔに変換することが可能である。

On the other hand, the inverse matrix (transposed matrix) of the direction cosine matrix may be used for conversion from the rider coordinate system to the vehicle coordinate system. Therefore, the measurement point [x _L0 (k), y _L0 (k), z _L0 (k)] ^T at time k acquired in the lidar coordinate system is the coordinate [x _b0 (k), y _b0 (k), z _b0 (k)] ^T.

なお、以後では、車両座標系における各座標軸ｘ_ｂ、ｙ_ｂ、ｚ_ｂに沿った方向を、それぞれ単に「ｘ方向」、「ｙ方向」、「ｚ方向」とも呼ぶ。

Hereinafter, the directions along the coordinate axes _xb , _yb , and zb in the vehicle coordinate system will also be simply referred to as " _x direction,""ydirection," and "z direction," respectively.

（２）ロール角及びピッチ角の推定
次に、ライダ２のロール角Ｌ_φ０及びピッチ角Ｌ_θ０の推定方法について説明する。以下に説明するように、車載機１は、対象のライダ２に設置されたライダ用加速度センサ５から得られる３軸の加速度出力値に基づき、ライダ２のロール角Ｌ_φ０及びピッチ角Ｌ_θ０を推定する。以後では、説明便宜上、ライダ用加速度センサ５は、ライダ座標系の３軸の加速度を計測するものとする。 (2) Estimation of roll angle and pitch angle
Next, a method for estimating the roll angle L _φ0 and the pitch angle L _θ0 of the rider 2 will be described. As described below, the in-vehicle device 1 determines the roll angle L _φ0 and the pitch angle L _θ0 of the rider 2 based on the three-axis acceleration output values obtained from the rider acceleration sensor 5 installed on the target rider 2. presume. Hereinafter, for convenience of explanation, it is assumed that the rider acceleration sensor 5 measures three-axis acceleration of the rider coordinate system.

車両が水平な場所で停車中あるいは一定速度で走行中の場合、車両座標系における加速度は、ｚ方向の重力加速度ｇのみである。図５（Ａ）は、車両座標系における重力加速度ｇのベクトルを示した図であり、図５（Ｂ）は、ライダ座標系における重力加速度ｇのベクトルを示した図である。よって，ライダ用加速度センサ５のライダ座標系の出力値［α_ｘ,α_ｙ,α_ｚ］^Ｔは以下の式（５）が成り立つ。 When the vehicle is stopped on a level surface or is traveling at a constant speed, the acceleration in the vehicle coordinate system is only the gravitational acceleration g in the z direction. FIG. 5A is a diagram showing the vector of the gravitational acceleration g in the vehicle coordinate system, and FIG. 5B is a diagram showing the vector of the gravitational acceleration g in the rider coordinate system. Therefore, the output value [α _x , α _y , α _z ] ^T of the rider coordinate system of the rider acceleration sensor 5 satisfies the following equation (5).

ここで、式（５）のα_ｙ,α_ｚを用いて「α_ｙ／α_ｚ」を計算すると、以下の式（６）が得られる。

Here, the following equation (6) is obtained by calculating “α _y /α _z ” using α _y and α _z of equation (5).

よって、ライダ２のロール角Ｌ_φ０は、重力加速度ｇを用いない以下の式（７）により表される。

Therefore, the roll angle _Lφ0 of the rider 2 is expressed by the following equation (7) without using the gravitational acceleration g.

また、式（５）のα_ｙ,α_ｚを用いて「α_ｙ ^２＋α_ｚ ^２」を計算すると、以下の式（８）が得られ、さらに式（８）と式（５）のα_ｘを用いて重力加速度ｇを消去すると、以下の式（９）が得られる。

Further, when calculating “α _y ² +α _z ² ” using α _y and α _z in equation (5), the following equation (8) is obtained, and α _x in equations (8) and (5) is used to eliminate the gravitational acceleration g, the following equation (9) is obtained.

よって、ライダ２のピッチ角Ｌ_θ０は、重力加速度ｇを用いない以下の式（１０）により表される。

Therefore, the pitch angle L _θ0 of the rider 2 is expressed by the following equation (10) without using the gravitational acceleration g.

以上により、車載機１は、水平な場所で、停止あるいは一定速度で走行しているときに、式（７）及び式（１０）を参照することで、ライダ用加速度センサ５の出力値に基づき、ライダ２のロール角Ｌ_φ０及びピッチ角Ｌ_θ０を算出することができる。なお、車載機１は、現在位置が水平な場所か否かを、ジャイロセンサ３又は車体用加速度センサ４の出力に基づき判定してもよく、車両の現在位置に相当する道路の道路データの傾斜角度に関する情報を地図ＤＢ１０から参照することで判定してもよい。また、車載機１は、車両が停止あるいは一定速度で走行しているか否かを、車体用加速度センサ４の出力に基づき判定してもよく、図示しない車速センサの出力に基づき判定してもよい。

As described above, the in-vehicle device 1 can refer to the equations (7) and (10) when stopped or traveling at a constant speed on a level surface, based on the output value of the rider acceleration sensor 5. , the roll angle L _φ0 and the pitch angle L _θ0 of the rider 2 can be calculated. The in-vehicle device 1 may determine whether or not the current position is horizontal based on the output of the gyro sensor 3 or the vehicle body acceleration sensor 4. You may judge by referring the information regarding an angle from map DB10. Further, the in-vehicle device 1 may determine whether the vehicle is stopped or traveling at a constant speed based on the output of the vehicle acceleration sensor 4, or based on the output of a vehicle speed sensor (not shown). .

（３）ヨー角の推定
次に、ライダ２のヨー角Ｌ_ψ０の推定方法について説明する。以下に説明するように、車載機１は、算出したライダ２のロール角Ｌ_φ０及びピッチ角Ｌ_θ０を用いて、車両が直進道路を加速中又は減速中に、ライダ用加速度センサ５から得られる３軸の加速度出力値に基づき、ヨー角Ｌ_ψ０を推定する。 (3) Estimation of yaw angle
Next, a method for estimating the yaw angle L _ψ0 of the rider 2 will be described. As described below, the in-vehicle device 1 uses the calculated roll angle L _φ0 and pitch angle L _θ0 of the rider 2 to obtain from the rider acceleration sensor 5 while the vehicle is accelerating or decelerating on a straight road. Based on the three-axis acceleration output values, the yaw angle L _ψ0 is estimated.

車両が直進道路を加速度「α」により加速中又は減速中の場合、車両座標系において、ｘ方向において加速度αが発生すると共に、ｚ方向において重力加速度ｇが発生するため、ライダ用加速度センサ５のライダ座標系の出力値［α_ｘ,α_ｙ,α_ｚ］^Ｔについて、以下の式（１１）が成り立つ。 When the vehicle is accelerating or decelerating on a straight road due to the acceleration "α", the acceleration α is generated in the x direction and the gravitational acceleration g is generated in the z direction in the vehicle coordinate system. The following equation (11) holds for the output values [α _x , α _y , α _z ] ^T in the lidar coordinate system.

ここで、加速度αと重力加速度ｇを消去するため、以下に説明する演算を行う。

Here, in order to eliminate the acceleration α and the gravitational acceleration g, the calculation described below is performed.

まず、式（１１）のα_ｙ,α_ｚを用いると、それぞれ以下の式（１２）、式（１３）が成立する。 First, using α _y and α _z in Equation (11), the following Equations (12) and (13) are respectively established.

そして、式（１２）を式（１３）により減算すると、以下の式（１４）が得られる。 Subtracting the equation (12) from the equation (13) yields the following equation (14).

同様に、式（１１）のα_ｙ,α_ｚを用いると、それぞれ以下の式（１５）、式（１６）が成立する。 Similarly, using α _y and α _z in Equation (11), the following Equations (15) and (16) are respectively established.

そして、式（１５）と式（１６）とを加算すると、以下の式（１７）が得られる。

Then, by adding the equations (15) and (16), the following equation (17) is obtained.

さらに、式（１７）にｓｉｎＬ_θ０を乗じた式と、式（１１）のα_ｘにｃｏｓＬ_θ０を乗じた式とを加算すると、以下の式（１８）が得られる。

Furthermore, the following equation (18) is obtained by adding the equation obtained by multiplying equation (17) by sinL _θ0 and the equation obtained by multiplying α _x of equation (11) by cosL _θ0 .

そして、式（１４）を式（１８）により割ると、以下の式（１９）が得られる。

Then, by dividing the equation (14) by the equation (18), the following equation (19) is obtained.

よって、ライダ２のヨー角Ｌ_ψ０は、加速度α及び重力加速度ｇを用いない以下の式（２０）により表される。

Therefore, the yaw angle L _ψ0 of the rider 2 is expressed by the following equation (20) without using the acceleration α and the gravitational acceleration g.

以上により、車載機１は、直進道路を加減速中に得られたライダ用加速度センサ５の出力値に基づき、式（２０）を参照することで、ライダ２のヨー角Ｌ_ψ０を算出することができる。なお、車載機１は、車両が直進道路を走行中か否かを、車体用加速度センサ４の出力に基づき判定してもよく、現在位置に相当する道路の道路データを地図ＤＢ１０から参照することで判定してもよい。また、車載機１は、車両が加減速中であるか否かを、車体用加速度センサ４の出力に基づき判定してもよく、図示しない車速センサの出力に基づき判定してもよい。

As described above, the in-vehicle device 1 calculates the yaw angle L _ψ0 of the rider 2 by referring to the expression (20) based on the output value of the rider acceleration sensor 5 obtained while accelerating or decelerating on the straight road. can be done. The in-vehicle device 1 may determine whether or not the vehicle is traveling on a straight road based on the output of the vehicle body acceleration sensor 4, and refers to the road data of the road corresponding to the current position from the map DB 10. can be determined by Further, the in-vehicle device 1 may determine whether or not the vehicle is accelerating or decelerating based on the output of the vehicle body acceleration sensor 4 or based on the output of a vehicle speed sensor (not shown).

（４）姿勢の変化量の算出
次に、ライダ設置情報ＩＬの生成時から現在時刻である処理基準時点までのライダ２のピッチ角、ロール角、ヨー角の変化量の算出について補足説明する。以下では、ライダ設置情報ＩＬに記録された（即ちライダ２の姿勢・位置ずれが生じてない初期時の）ライダ２のピッチ角、ロール角、ヨー角を、それぞれ、「Ｌ_φ０」、「Ｌ_θ０」、「Ｌ_ψ０」とする。 (4) Calculation of posture change amount
Next, a supplementary description will be given of calculation of the amount of change in the pitch angle, roll angle, and yaw angle of the rider 2 from the generation of the rider installation information IL to the processing reference time, which is the current time. In the following, the pitch angle, roll angle, and yaw angle of the rider 2 recorded in the rider installation information IL (that is, at the initial time when the attitude/positional deviation of the rider 2 has not occurred) will be expressed as “L _φ0 ” and “L _θ0 ” and “L _ψ0 ”.

何らかの影響により、以下の式（２１）に示されるように、ライダ２のロール角が「ΔＬ_φ」、ピッチ角が「ΔＬ_θ」、ヨー角が「ΔＬ_ψ」だけライダ設置情報ＩＬの生成時からそれぞれ変化し、処理基準時点のロール角が「Ｌ_φ」、ピッチ角が「Ｌ_θ」、ピッチ角が「Ｌ_ψ」になったとする。 Due to some influence, as shown in the following equation (21), the roll angle of the rider 2 is "ΔL _φ ", the pitch angle is "ΔL _θ ", and the yaw angle is "ΔL _ψ ". , and the roll angle at the processing reference time is "L _φ ", the pitch angle is "L _θ ", and the pitch angle is "L _ψ ".

同様に、以下の式（２２）に示されるように、ライダ２の座標軸ｘ_ｂにおける位置が「ΔＬ_ｘ０」、座標軸ｙ_ｂにおける位置が「ΔＬ_ｙ０」、座標軸ｚ_ｂにおける位置が「ΔＬ_ｚ０」だけライダ設置情報ＩＬの生成時からそれぞれ変化し、処理基準時点の座標軸ｘ_ｂにおける位置が「Ｌ_ｘ」、座標軸ｙ_ｂにおける位置が「Ｌ_ｙ」、座標軸ｚ_ｂにおける位置が「Ｌ_ｚ」になったとする。

Similarly, as shown in the following formula (22), the position of the rider 2 on the coordinate axis _xb is " _ΔLx0 ", the position on the coordinate axis _yb is " _ΔLy0 ", and the position on the coordinate axis _zb is " _ΔLz0 ". are changed from the generation of the rider installation information IL, and the position on the coordinate axis _xb at the processing reference time is "Lx", the position on the coordinate _axis _yb is " _Ly ", and the position on the coordinate axis _zb is " _Lz ". Suppose it happened.

また、ライダ２の上述の姿勢及び位置の変化に起因して、ライダ２から得られる時刻ｋの計測点が［ｘ_Ｌ０（ｋ）、ｙ_Ｌ０（ｋ）、ｚ_Ｌ０（ｋ）］^Ｔから［ｘ_Ｌ（ｋ）、ｙ_Ｌ（ｋ）、ｚ_Ｌ（ｋ）］^Ｔになったとする。この場合、時刻ｋの計測点の車両座標系からライダ座標系への変換は、式（２３）のようになる。

Also, due to the above-described changes in attitude and position of the rider 2, the measurement points at time k obtained from the rider 2 change from [x _L0 (k), y _L0 (k), z _L0 (k)] ^T to [ _xL (k), _yL (k), _zL (k)] ^T . In this case, the transformation of the measurement point at time k from the vehicle coordinate system to the rider coordinate system is given by equation (23).

ここで、式（２３）は、式（３）と同じ形式であるため、式（７）、式（１０）、式（２０）と同様の式を用いて、ライダ２のロール角Ｌ_φ、ピッチ角Ｌ_θ、ヨー角Ｌ_ψを算出することができる。よって、車載機１は、これらのロール角Ｌ_φ、ピッチ角Ｌ_θ、ヨー角Ｌ_ψと、ライダ設置情報ＩＬに記録されたロール角Ｌ_φ０、ピッチ角Ｌ_θ０、ヨー角Ｌ_ψ０との差をそれぞれ算出することで、ロール角の変化量ΔＬ_φ、ピッチ角の変化量ΔＬ_θ、ヨー角の変化量ΔＬ_ψを好適に算出することができる。なお、車載機１は、ライダ２の車両への取付け（即ちアライメント調整）後からライダ２の姿勢の推定処理が最初に実行可能なタイミングで式（７）、式（１０）、式（２０）に基づき推定したライダ２のロール角Ｌ_φ０、ピッチ角Ｌ_θ０、ヨー角Ｌ_ψ０を、ライダ設置情報ＩＬとして記憶してもよい。

Here, since equation (23) has the same form as equation (3), using equations similar to equations (7), (10), and (20), the roll angle L _φ of the rider 2 , The pitch angle L _θ and yaw angle L _ψ can be calculated. Therefore, the in-vehicle device 1 determines the difference between these roll angle L _φ , pitch angle L _θ , and yaw angle L _ψ and the roll angle L _φ0 , pitch angle L _θ0 , and yaw angle L _ψ0 recorded in the rider installation information IL. , the variation ΔL _φ in the roll angle, the variation ΔL _θ in the pitch angle, and the variation ΔL _ψ in the yaw angle can be preferably calculated. Note that the in-vehicle device 1 calculates equations (7), (10), and (20) at the timing when the processing for estimating the posture of the rider 2 can be executed for the first time after the rider 2 is attached to the vehicle (that is, the alignment adjustment). may be _stored as the _rider installation information _IL .

（５）ｚ方向の位置変化量の算出
次に、ｚ方向におけるライダ２の位置変化量の算出方法について説明する。車載機１は、平坦道路で車両が停止中又は一定速度走行中に、道路面を計測したライダ２の計測点のｚ方向の値に基づき、ｚ方向におけるライダ２の位置変化量「ΔＬ_ｚ」を算出する。 (5) Calculation of position change amount in z direction
Next, a method for calculating the amount of change in the position of the rider 2 in the z direction will be described. The in-vehicle device 1 determines the amount of positional change "ΔL _z " of the rider 2 in the z-direction based on the z-direction value of the measurement point of the rider 2 that measures the road surface while the vehicle is stopped or traveling at a constant speed on a flat road. Calculate

処理基準時点での車両座標系の時刻ｋの計測点の座標［ｘ_ｂ（ｋ）、ｙ_ｂ（ｋ）、ｚ_ｂ（ｋ）］^Ｔは、式（４）により示される座標［ｘ_ｂ０（ｋ）、ｙ_ｂ０（ｋ）、ｚ_ｂ０（ｋ）］^Ｔと同様、方向余弦行列「Ｃ」を用いて、以下の式（２４）により表される。 The coordinates [x _b (k), y _b (k), z _b (k)] ^T of the measurement point at time k in the vehicle coordinate system at the processing reference time are the coordinates [x _b0 ( k), y _b0 (k), z _b0 (k)] Similar to ^T , it is represented by the following equation (24) using the direction cosine matrix “C”.

ここで、処理基準時点のロール角Ｌ_φ、ピッチ角Ｌ_θ、ピッチ角Ｌ_ψが正確に求められている場合、「Ｃ^－１［ｘ_Ｌ（ｋ）、ｙ_Ｌ（ｋ）、ｚ_Ｌ（ｋ）］^Ｔ」は、車両座標系の値に正しく変換されている。よって、平坦な路面で車両が停車中あるいは一定速度で走行している時は、車両のピッチ変動が少ないため、道路面を照射したライダ２のz方向の計測値は、ライダ２の搭載位置から道路面までの高さ（即ち位置Ｌ_ｚ）となる。

Here, when the roll angle L _φ , pitch angle L _θ , and pitch angle L _ψ at the processing reference time are accurately obtained, "C ⁻¹ [x _L (k), y _L (k), z _L ( k)] ^T '' are correctly transformed to values in the vehicle coordinate system. Therefore, when the vehicle is stopped or traveling at a constant speed on a flat road surface, there is little variation in the pitch of the vehicle. It becomes the height to the road surface (that is, the position L _z ).

図６（Ａ）は、ライダ２の位置Ｌ_ｚの変化前において平坦な路面を車両が走行中のときにライダ２により計測された路面のｚ方向の計測値ｚ_ｂ０（ｋ）を示し、図６（Ｂ）は、ライダ２の位置Ｌ_ｚの変化後において平坦な路面を車両が走行中のときにライダ２により計測された路面のｚ方向の計測値ｚ_ｂ（ｋ）を示す。図６（Ａ）、（Ｂ）に示すように、計測値ｚ_ｂ０（ｋ）、計測値ｚ_ｂ（ｋ）は、それぞれ、ライダ２のｚ方向の位置Ｌ_ｚ０、Ｌ_ｚとそれぞれ同一長となっている。 FIG. 6A shows the measured value z _b0 (k) of the road surface in the z direction measured by the rider 2 while the vehicle is traveling on a flat road surface before the position L _z of the rider 2 changes. 6(B) shows the measured value z _b (k) of the road surface in the z direction measured by the rider 2 while the vehicle is running on a flat road surface after the position L _z of the rider 2 is changed. As shown in FIGS. 6A and 6B, the measured value z _b0 (k) and the measured value z _b (k) have the same length as the positions L _z0 and L _z of the rider 2 in the z direction, respectively. It's becoming

よって、式（４）で計算される道路面の計測値ｚ_ｂ０（ｋ）と、姿勢変化後の道路面の計測値ｚ_ｂ（ｋ）との差分は、ｚ方向の変化量ΔＬ_ｚと等しくなることがわかる。この場合に用いる計測値ｚ_ｂ０（ｋ）及び計測値ｚ_ｂ（ｋ）は、複数の走査ラインでの平均及び時間平均であることが望ましい。 Therefore, the difference between the measured value z _b0 (k) of the road surface calculated by Equation (4) and the measured value z _b (k) of the road surface after the attitude change is equal to the amount of change ΔL _z in the z direction. I know it will be. The measured value z _b0 (k) and the measured value z _b (k) used in this case are desirably averaged over a plurality of scan lines and time averaged.

以上を勘案し、車載機１は、以下の式（２５）に基づき、変化量ΔＬ_ｚを算出する。 Taking the above into consideration, the vehicle-mounted device 1 calculates the amount of change ΔL _z based on the following equation (25).

従って、車載機１は、ライダ２の初期位置Ｌ_ｚ０の計測後に、平坦な路面で車両が停車中あるいは一定速度で走行している時の路面の計測値ｚ_ｂ０（ｋ）の平均を算出し、ライダ２の初期位置Ｌ_ｚ０と共にライダ設置情報ＩＬに記録しておく。これにより、車載機１は、ライダ設置情報ＩＬを参照することで、処理基準時の位置Ｌ_ｚ及び変化量ΔＬ_ｚを好適に算出することができる。

Therefore, after measuring the initial position L _z0 of the rider 2, the in-vehicle device 1 calculates the average of the road surface measurement values z _b0 (k) when the vehicle is stopped or traveling at a constant speed on a flat road surface. , and the initial position _Lz0 of the rider 2 are recorded in the rider installation information IL. As a result, the vehicle-mounted device 1 can suitably calculate the position _Lz and the change amount _ΔLz at the time of the processing reference by referring to the rider installation information IL.

なお、車載機１は、初期位置Ｌ_ｚ０の計測時と処理基準時とでの車両のサスペンションのストローク量（即ち伸び切り位置からの沈み量）をそれぞれ推定し、推定したストローク量の差に基づき、z方向の変化量ΔＬ_ｚを補正してもよい。この場合、車載機１は、車両のサスペンションに設けられたストロークセンサ等に基づき、サスペンションのストローク量を計測してもよく、車両の搭乗人数に基づき、サスペンションのストローク量を推定してもよい。これにより、より正確に変化量ΔＬ_ｚを算出することが可能である。 In addition, the in-vehicle device 1 estimates the stroke amount of the suspension of the vehicle (that is, the amount of sinking from the fully extended position) at the time of measuring the initial position _Lz0 and at the time of the processing reference, and based on the difference between the estimated stroke amounts , the amount of change ΔL _z in the z direction may be corrected. In this case, the in-vehicle device 1 may measure the stroke amount of the suspension based on a stroke sensor or the like provided in the suspension of the vehicle, or may estimate the stroke amount of the suspension based on the number of passengers in the vehicle. Thereby, it is possible to calculate the amount of change ΔL _z more accurately.

（６）ｘ方向の位置変化量の算出
次に、ｘ方向におけるライダ２の位置変化量の算出方法について説明する。車載機１は、坂道の始点又は終点付近において、あるいは路面上のバンプを通過するときにおいて、ライダ２のｚ方向計測値の変化とジャイロセンサ３から得られるピッチレートの変化との時間差に基づき、ｘ方向の位置変化量Ｌ_ｘを算出する。 (6) Calculation of position change amount in x direction
Next, a method of calculating the amount of change in the position of the rider 2 in the x direction will be described. Based on the time difference between the change in the z-direction measurement value of the rider 2 and the change in the pitch rate obtained from the gyro sensor 3, the in-vehicle device 1, near the start or end point of a slope or when passing a bump on the road surface, A position change amount L _x in the x direction is calculated.

図７（Ａ）～図７（Ｇ）は、上り坂の始点５０の前後を走行中の車両のライダ２の特定のスキャンラインのｚ方向の計測値ｚ_ｂ（ｋ）の大きさを線分５１～５７により表した図である。また、図８（Ａ）は、図７（Ａ）～図７（Ｇ）に示す車両の走行時に計測される計測値ｚ_ｂ（ｋ）の時間変化を示すグラフであり、図８（Ｂ）は、図８（Ａ）と同一期間における車体ピッチ角（ピッチレートの積分値）の時間変化を示すグラフである。なお、図８（Ａ）及び図８（Ｂ）における番号５１～５７は、図７（Ａ）～図７（Ｇ）の線分５１～５７が示す計測値ｚ_ｂ（ｋ）に対応する位置をそれぞれ指し示している。 7(A) to 7(G) show the magnitude of the measured value z _b (k) in the z direction for a particular scan line of the rider 2 of the vehicle traveling before and after the starting point 50 of the uphill. Figure 5 is a diagram represented by 51-57; FIG. 8(A) is a graph showing temporal changes in measured values z _b (k) measured while the vehicle shown in FIGS. 7(A) to 7(G) is running, and FIG. FIG. 8 is a graph showing the change over time of the vehicle body pitch angle (integrated value of pitch rate) during the same period as in FIG. 8(A); Note that numbers 51 to 57 in FIGS. 8(A) and 8(B) are positions corresponding to the measured values z _b (k) indicated by line segments 51 to 57 in FIGS. 7(A) to 7(G). are indicated respectively.

上り坂や下り坂などの勾配路面の始まりや終わりの直前では、道路面を計測点とするｚ方向のライダ２の計測値は、坂の始点又は終点付近において変化する。図７の例では、図８（Ａ）に示すように、ライダ２が始点５０（即ち勾配の変化点）を照射した時刻「ｔ１」（図７（Ｂ）参照）から計測値ｚ_ｂ（ｋ）が徐々に変化し、車体の前輪が始点５０に差し掛かる時刻「ｔ２」（図７（Ｄ）参照）において計測値ｚ_ｂ（ｋ）が最小となる。その後、徐々に計測値ｚ_ｂ（ｋ）は大きくなり、後輪が始点５０に差し掛かるとき（図７（Ｆ）参照）の計測値ｚ_ｂ（ｋ）は平坦な路面を走行するときの計測値ｚ_ｂ（ｋ）と同じになる。 Immediately before the start or end of a sloped road such as an uphill or downhill, the measured value of the rider 2 in the z direction with the road surface as the measurement point changes near the start point or end point of the slope. In the example of FIG. 7, as shown in FIG. 8A, the measured value z _b (k ) gradually changes, and the measured value z _b (k) becomes minimum at time “t2” (see FIG. 7(D)) when the front wheels of the vehicle body approach the starting point 50 . After that, the measured value z _b (k) gradually increases, and the measured value z b (k) when the rear wheels approach the starting point 50 (see FIG. 7(F)) is the same as the measured value z _b (k) when traveling on a flat road surface. will be the same as the value z _b (k).

以上を勘案し、車載機１は、計測値ｚ_ｂ（ｋ）が減少し始める時刻ｔ１（図７（Ｂ）参照）から計測値ｚ_ｂ（ｋ）が最小となる時刻ｔ２（図７（Ｄ）参照）までの時間間隔「Δｔ」を算出する。 Considering the above, the vehicle-mounted device 1 changes the measured value z _b (k) from the time t1 (see FIG. 7B) when the measured value z _b (k) starts to decrease to the time t2 (see FIG. 7 (D )) is calculated.

ここで、時間間隔Δｔは、図７（Ａ）に示す距離ｄを車両が走行するのに要する時間に相当し、距離ｄは、特定のスキャンラインにおいて坂道の始点５０を検出したときの当該始点５０と車両の前輪までの距離に相当する。時間間隔Δｔは、本発明における「時間差」の一例である。 Here, the time interval Δt corresponds to the time required for the vehicle to travel the distance d shown in FIG. 50 and the distance to the front wheels of the vehicle. The time interval Δt is an example of "time difference" in the present invention.

そして、車載機１は、以下の式（２６）に示すように、車速パルスなどから車両の走行速度「ｖ」を求めて時間間隔Δtを乗じることで、距離ｄを算出する。 Then, the vehicle-mounted device 1 calculates the distance d by obtaining the running speed "v" of the vehicle from the vehicle speed pulse or the like and multiplying it by the time interval Δt, as shown in the following equation (26).

なお、図８（Ｂ）に示すように、車載機１は、車体の前輪が勾配の変化点（図７では始点５０）に差し掛かる時刻ｔ２を、車体又はライダ２に搭載されたジャイロセンサ３により計測したピッチ角（図８（Ｂ）では車体ピッチ角）により判定することもできる。

Note that, as shown in FIG. 8B, the vehicle-mounted device 1 detects time t2 at which the front wheels of the vehicle body approach the changing point of the gradient (starting point 50 in FIG. It can also be determined by the pitch angle (vehicle body pitch angle in FIG. 8(B)) measured by .

また、車載機１は、距離ｄを、路面上のバンプを通過したときにも同様に計測することが可能である。図９（Ａ）～図９（Ｇ）は、バンプ６０の前後を走行中の車両のライダ２のｚ方向の計測値ｚ_ｂ（ｋ）の大きさを線分６１～６６により表した図である。また、図１０（Ａ）は、図９（Ａ）～図９（Ｇ）に示す車両の走行時に得られる計測値ｚ_ｂ（ｋ）の時間変化を示すグラフであり、図１０（Ｂ）は、図１０（Ａ）と同一期間における車体ピッチレートの時間変化を示すグラフである。なお、図１０（Ａ）及び図１０（Ｂ）における番号６１～６６は、図９（Ａ）～図９（Ｇ）の線分６１～６６が示す計測値ｚ_ｂ（ｋ）が対応する位置をそれぞれ指し示している。この場合、図１０（Ａ）に示すように、車両のライダ２のｚ方向の計測値ｚ_ｂ（ｋ）は、バンプ６０を照射する時刻「ｔ３」（図９（Ｂ）参照）において計測値ｚ_ｂ（ｋ）が一時的に減少し、車体の前輪がバンプ６０を超える時刻「ｔ４」（図９（Ｄ）参照）において計測値ｚ_ｂ（ｋ）一時的に増加する。従って、車載機１は、計測値ｚ_ｂ（ｋ）が一時的に下がった時刻ｔ３（図９（Ｂ）参照）から計測値ｚ_ｂ（ｋ）が一時的に大きくなった時刻ｔ４（図９（Ｄ）参照）までの時間間隔を、時間間隔Δｔとして算出する。これによっても、車載機１は、式（２６）に基づき距離ｄを好適に算出することができる。なお、図１０（Ｂ）に示すように、車載機１は、車体の前輪がバンプ６０を超える時刻ｔ４を、車体又はライダ２に搭載されたジャイロセンサ３により計測したピッチレート（図１０（Ｂ）では車体ピッチレート）により判定することもできる。 In addition, the vehicle-mounted device 1 can similarly measure the distance d when passing through a bump on the road surface. 9A to 9G are diagrams showing the magnitude of the measured value z _b (k) in the z direction of the rider 2 of the vehicle running in front of and behind the bump 60 by line segments 61 to 66. FIG. be. FIG. 10(A) is a graph showing temporal changes in measured values z _b (k) obtained when the vehicle shown in FIGS. 9(A) to 9(G) is running, and FIG. 10A is a graph showing a change in the vehicle body pitch rate over time during the same period as in FIG. 10A. Note that numbers 61 to 66 in FIGS. 10(A) and 10(B) are positions corresponding to the measured values z _b (k) indicated by line segments 61 to 66 in FIGS. 9(A) to 9(G). are indicated respectively. In this case, as shown in FIG. 10(A), the measured value z _b (k) in the z direction of the vehicle rider 2 is the measured value z _b (k) temporarily decreases, and the measured value z _b (k) temporarily increases at time “t4” (see FIG. 9(D)) when the front wheels of the vehicle body cross the bump 60 . Therefore, the in-vehicle device 1 changes from time t3 (see FIG. 9B) when the measured value z _b (k) temporarily decreases to time t4 (see FIG. 9B) when the measured value z _b (k) increases temporarily. (D)) is calculated as the time interval Δt. Also by this, the vehicle-mounted device 1 can suitably calculate the distance d based on Formula (26). As shown in FIG. 10(B), the in-vehicle device 1 measures the pitch rate (see FIG. 10(B) ) can also be determined by the vehicle body pitch rate).

次に、距離ｄから位置変化量ΔＬ_ｘを算出する方法について説明する。車載機１は、ライダ２の位置が変化する前の距離「ｄ_０」を記憶しておき，以下の式（２７）に示すように、距離ｄとの差分を取ることで、ライダ２のｘ方向の位置変化量ΔＬ_ｘを算出することができる。 Next, a method for calculating the position change amount ΔL _x from the distance d will be described. The in-vehicle device 1 stores the distance “d ₀ ” before the position of the rider 2 changes, and obtains the difference from the distance d as shown in the following equation (27) to obtain x A direction position change amount ΔL _x can be calculated.

この場合、例えば、車載機１は、ライダ２の初期位置Ｌ_ｘ０の計測後、最初に坂道の始点又は終点付近を通過したとき、あるいは路面上のバンプを通過したときに距離ｄ_０を算出し、ライダ２の初期位置Ｌ_ｘ０と共にライダ設置情報ＩＬに記録しておく。これにより、車載機１は、ライダ設置情報ＩＬを参照することで、式（２７）に基づき、処理基準時の位置Ｌ_ｘ及び変化量ΔＬ_ｘを好適に算出することができる。

In this case, for example, after measuring the initial position _Lx0 of the rider 2, the in-vehicle device ₁ calculates the distance d0 when the rider 2 first passes near the starting point or the end point of the slope, or when passing through a bump on the road surface. , and the initial position _Lx0 of the rider 2 are recorded in the rider installation information IL. As a result, the vehicle-mounted device 1 can suitably calculate the position L _x and the change amount ΔL _x at the time of the processing reference based on the equation (27) by referring to the rider installation information IL.

（７）ｙ方向の位置変化量の算出
次に、ｙ方向におけるライダ２の位置変化量の算出方法について説明する。車載機１は、車両の旋回中において、ライダ用加速度センサ５のｙ方向出力値と、車体用加速度センサ４のｙ方向出力値と、ジャイロセンサ３の出力値から、ｙ方向のライダ２の位置Ｌ_ｙを算出する。 (7) Calculation of position change amount in y direction
Next, a method for calculating the amount of positional change of the rider 2 in the y direction will be described. The in-vehicle device 1 determines the y-direction position of the rider 2 from the y-direction output value of the rider acceleration sensor 5, the y-direction output value of the vehicle body acceleration sensor 4, and the output value of the gyro sensor 3 while the vehicle is turning. Calculate _Ly .

図１１は、旋回中の車両に生じる作用を概略的に示した図である。一般に，旋回中の車両重心点の速度「Ｖ_Ｇ」と向心加速度「α_Ｇ」は、以下の式（２８）及び式（２９）で与えられる。 FIG. 11 is a diagram schematically showing the action that occurs on the vehicle during turning. In general, the velocity “V _G ” and the centripetal acceleration “α _G ” of the center of gravity of the vehicle during turning are given by the following equations (28) and (29).

ここで，式（２８）の「ｒ_Ｇ」は旋回中心点から車両重心点までの距離を示している。また，速度Ｖ_Ｇと向心加速度α_Ｇの向きは直交している。剛体であれば，角速度はどの場所でも同じであるため，重心点から［Ａ_ｘ、Ａ_ｙ］^Ｔ離れた位置に設定した車両座標原点のＡ点の速度「Ｖ_Ａ」と向心加速度「α_Ａ」は，以下の式（３０）及び式（３１）で与えられる。

Here, “r _G ” in Equation (28) indicates the distance from the center of turning to the center of gravity of the vehicle. Also, the directions of the velocity _VG and the centripetal acceleration _αG are orthogonal. If the body is a rigid body, the angular velocity is the same everywhere. Therefore, the _velocity "V _A " and the _centripetal ^acceleration "α _A ” is given by the following equations (30) and (31).

図１１より，向心加速度α_Ａと車両横方向成分「α_Ａｙ」との関係は，以下の式（３２）に示される関係となる。

From FIG. 11, the relationship between the centripetal acceleration α _A and the vehicle lateral direction component "α _Ay " is represented by the following equation (32).

よって、車両横方向成分α_Ａｙは、以下の式（３３）により表される。

Therefore, the vehicle lateral direction component α _Ay is represented by the following equation (33).

この式（３３）の左辺の車両横方向成分α_Ａｙは、車両のＡ点の位置に加速度センサを搭載すると、ｙ軸方向の出力として計測できる。ここで、Ａ点から［Ｌ_ｘ、Ｌ_ｙ］^Ｔ離れた位置にライダ２が搭載されているとすると、ライダ２の位置での車両横方向成分「α_{（Ｌ＋Ａ）ｙ}」に関し、式（３３）と同様の以下の式（３４）が成立し、この式（３４）と式（３３）を用いることで、さらに以下の式（３５）が得られる。

The vehicle lateral direction component α _Ay on the left side of this equation (33) can be measured as an output in the y-axis direction if an acceleration sensor is mounted at the position of point A on the vehicle. Here, assuming that the rider 2 is mounted at a position separated by [L _x , _Ly ] ^T from the point A, the vehicle lateral direction component “α _(L+A)y ” at the position of the rider 2 is expressed by Equation (33) ) is established, and the following equation (35) is obtained by using the equations (34) and (33).

よって、位置Ｌ_ｙは、以下の式（３６）により表される。

Therefore, the position _Ly is represented by the following equation (36).

式（３６）は，車両に搭載した車両用加速度センサ４とライダ用加速度センサ５の出力の差分を車両のヨーレートの２乗で割れば、それが車両座標系原点に対するライダ２のｙ方向の位置になることを示している。従って、車載機１は、式（３６）を計算することで、ライダ２の位置Ｌ_ｙを把握することができる。また、車載機１は、ライダ設置情報ＩＬに記憶された初期位置Ｌ_ｙ０を参照することで、変化量ΔＬ_ｙを算出することもできる。

Equation (36) is such that if the difference between the outputs of the vehicle-mounted vehicle acceleration sensor 4 and the rider's acceleration sensor 5 is divided by the square of the yaw rate of the vehicle, the position of the rider 2 in the y direction with respect to the origin of the vehicle coordinate system is obtained. is shown to be Therefore, the vehicle-mounted device 1 can grasp the position _Ly of the rider 2 by calculating the expression (36). The vehicle-mounted device 1 can also calculate the amount of change ΔL _y by referring to the initial position Ly ₀ stored in the rider installation information IL.

［処理フロー］
図１２は、ライダ２の出力を補正する処理の手順を示すフローチャートの一例である。車載機１は、図１２に示す処理を、所定のタイミングにおいて繰り返し実行する。 [Processing flow]
FIG. 12 is an example of a flowchart showing a procedure of processing for correcting the output of the rider 2. FIG. The in-vehicle device 1 repeatedly executes the process shown in FIG. 12 at a predetermined timing.

まず、車載機１は、水平な道路で車両が停止中又は一定速度走行中に、ライダ用加速度センサ５の出力値からライダ２のロール角の変化量ΔＬ_φ及びピッチ角の変化量ΔＬ_θを算出する（ステップＳ１０１）。この場合、車載機１は、式（７）及び（１０）と同等の式に基づきライダ２のロール角Ｌ_φ、ピッチ角Ｌ_θを算出し、ライダ設置情報ＩＬに記録されたロール角Ｌ_φ０、ピッチ角Ｌ_θ０との差を、変化量ΔＬ_φ、ΔＬ_θとして算出する。 First, the in-vehicle device 1 calculates the amount of change ΔL _φ in the roll angle and the amount ΔL _θ of the pitch angle of the rider 2 from the output value of the rider acceleration sensor 5 while the vehicle is stopped or traveling at a constant speed on a level road. Calculate (step S101). In this case, the in-vehicle device 1 calculates the roll angle L _φ and the pitch angle L _θ of the rider 2 based on formulas equivalent to formulas (7) and (10), and calculates the roll angle L _φ0 recorded in the rider installation information IL. , and the pitch angle L _θ0 are calculated as the amounts of change ΔL _φ and ΔL _θ .

次に、車載機１は、車両が直進道路を加減速中に、ライダ用加速度センサ５の出力値からライダ２のヨー角の変化量ΔＬ_ψを算出する（ステップＳ１０２）。この場合、車載機１は、式（２０）と同等の式に基づきライダ２のヨー角Ｌ_ψを算出し、ライダ設置情報ＩＬに記録されたヨー角Ｌ_ψ０との差を、変化量ΔＬ_ψとして算出する。 Next, the in-vehicle device 1 calculates the amount of change ΔL _ψ in the yaw angle of the rider 2 from the output value of the rider acceleration sensor 5 while the vehicle is accelerating or decelerating on the straight road (step S102). In this case, the in-vehicle device 1 calculates the yaw angle L _ψ of the rider 2 based on an equation equivalent to equation (20), and calculates the difference from the yaw angle L _ψ0 recorded in the rider installation information IL as the amount of change ΔL _ψ Calculate as

次に、車載機１は、車両が平坦道路で停止中又は一定速度走行中に、道路面を照射するライダ２のｚ方向の計測値から、ｚ方向の位置変化量ΔＬ_ｚを算出する（ステップＳ１０３）。この場合、車載機１は、平坦な路面で車両が停車中あるいは一定速度で走行している時の路面の計測値ｚ_ｂ（ｋ）の平均を算出し、ライダ設置情報ＩＬに記録された同条件での路面の計測値ｚ_ｂ０（ｋ）の平均との差分をとることで、変化量ΔＬｚを算出する（式（２５）参照）。 Next, the in-vehicle device 1 calculates the position change amount ΔL _z in the z direction from the measured value in the z direction of the rider 2 illuminating the road surface while the vehicle is stopped on a flat road or traveling at a constant speed (step S103). In this case, the in-vehicle device 1 calculates the average of the measured values z _b (k) of the road surface when the vehicle is stopped or traveling at a constant speed on a flat road surface, By taking the difference from the average of the measured values z _b0 (k) of the road surface under the conditions, the amount of change ΔLz is calculated (see formula (25)).

次に、車載機１は、坂道の始点又は終点付近において、あるいは路面上のバンプを通過するときにおいて、ライダ２のｚ方向計測値の変化とジャイロセンサの出力値の変化との時間間隔Δｔを算出することで、ｘ方向の位置変化量ΔＬ_ｘを算出する（ステップＳ１０４）。この場合、車載機１は、時間間隔Δｔに車両の走行速度ｖを乗じることで距離ｄを算出し、あらかじめライダ設置情報ＩＬに記憶された距離ｄ_０との差分を、変化量ΔＬ_ｚとして算出する（式（２６）参照）。 Next, the in-vehicle device 1 determines the time interval Δt between the change in the z-direction measurement value of the rider 2 and the change in the output value of the gyro sensor near the start or end of a slope or when passing a bump on the road surface. By calculating, the position change amount ΔL _x in the x direction is calculated (step S104). In this case, the in-vehicle device ₁ calculates the distance d by multiplying the time interval Δt by the running speed v of the vehicle, and calculates the difference from the distance d0 stored in advance in the rider installation information IL as the amount of change ΔL _z . (see formula (26)).

次に、車載機１は、車両の旋回中において、ライダ用加速度センサ５のｙ方向出力値と、車体用加速度センサ４のｙ方向出力値と、ジャイロセンサ３の出力値から、ｙ方向の位置変化量ΔＬ_ｙを算出する（ステップＳ１０５）。具体的には、車載機１は、式（３６）に基づき処理基準時の位置Ｌ_ｙを算出し、ライダ設置情報ＩＬに記憶された初期位置Ｌ_ｙ０との差分を、ｙ方向の位置変化量ΔＬ_ｙとして算出する。 Next, while the vehicle is turning, the vehicle-mounted device 1 determines the position in the y-direction based on the y-direction output value of the rider acceleration sensor 5, the y-direction output value of the vehicle body acceleration sensor 4, and the output value of the gyro sensor 3. A change amount ΔL _y is calculated (step S105). Specifically, the vehicle-mounted device 1 calculates the position _{Ly at the time of the processing reference based on the equation (36), and calculates the difference from the initial position Ly0} _stored in the rider installation information IL as the position change amount in the y direction. Calculate as ΔL _y .

次に、車載機１は、ステップＳ１０１～Ｓ１０５で算出した変化量ΔＬ_φ、ΔＬ_θ、ΔＬ_ψ、ΔＬ_ｘ、ΔＬ_ｙ、ΔＬ_ｚのうち、所定の閾値以上のものが存在するか否か判定する（ステップＳ１０６）。上述の閾値は、後述するステップＳ１０８でのライダ２の計測データの補正処理を行うことで引き続きライダ２の計測データを使用できるか否かを判定するための閾値であり、例えば予め実験等に基づき設定される。そして、車載機１は、ステップＳ１０１～Ｓ１０５で算出した変化量ΔＬ_φ、ΔＬ_θ、ΔＬ_ψ、ΔＬ_ｘ、ΔＬ_ｙ、ΔＬ_ｚのうち、所定の閾値以上のものが存在する場合（ステップＳ１０６；Ｙｅｓ）、対象のライダ２の出力データの使用（即ち障害物検知や自車位置推定等への利用）を中止し、対象のライダ２について再度のアライメント調整を行う必要がある旨の警告を情報出力部１６により出力する（ステップＳ１０７）。これにより、事故等により著しく姿勢・位置のずれが生じたライダ２の計測データを用いることによる安全性低下等を確実に抑制する。上述の閾値は、本発明における「所定量」の一例である。 Next, the in-vehicle device 1 determines whether any of the variation amounts ΔL _φ , ΔL _θ , ΔL _ψ , ΔL _x , ΔL _y , and ΔL _z calculated in steps S101 to S105 are equal to or larger than a predetermined threshold. (step S106). The above-mentioned threshold value is a threshold value for determining whether or not the measurement data of the rider 2 can be used continuously by performing the correction processing of the measurement data of the rider 2 in step S108 described later, and is based on, for example, an experiment or the like in advance. set. Then, the in-vehicle device 1 determines that any of the variation amounts ΔL _φ , ΔL _θ , ΔL _ψ , ΔL _x , ΔL _y , and ΔL _z calculated in steps S101 to S105 are equal to or larger than a predetermined threshold value (step S106; Yes), stop using the output data of the target rider 2 (that is, use it for obstacle detection, self-vehicle position estimation, etc.), and issue a warning to the effect that it is necessary to perform alignment adjustment again for the target rider 2. Output by the output unit 16 (step S107). As a result, the use of the measurement data of the rider 2, whose attitude and position have significantly deviated due to an accident or the like, can reliably prevent a decrease in safety or the like. The above threshold is an example of the "predetermined amount" in the present invention.

一方、車載機１は、変化量ΔＬ_φ、ΔＬ_θ、ΔＬ_ψ、ΔＬ_ｘ、ΔＬ_ｙ、ΔＬ_ｚのうち、所定の閾値以上のものが存在しない場合（ステップＳ１０６；Ｎｏ）、これらの変化量に基づき、ライダ２が出力する点群データの各計測値を補正する（ステップＳ１０８）。この場合、車載機１は、例えば、各変化量の大きさごとの計測値の補正量を示すマップ等を記憶しておき、当該マップ等を参照することで、上述の計測値を補正する。また、変化量の所定の割合の値を計測値の補正量として計測値を補正してもよい。 On the other hand, if none of the amounts of change ΔL _φ , ΔL _θ , ΔL _ψ , ΔL _x , ΔL _y , and ΔL _z are greater than or equal to a predetermined threshold (step S106; No), the vehicle-mounted device 1 Based on, each measurement value of the point cloud data output by the rider 2 is corrected (step S108). In this case, the in-vehicle device 1 stores, for example, a map or the like indicating the amount of correction of the measured value for each amount of change, and refers to the map or the like to correct the above-described measured value. Further, the measured value may be corrected by using a value of a predetermined ratio of the amount of change as the correction amount of the measured value.

以上説明したように、本実施例における車載機１は、対象物に対する距離を計測するライダ２の車両に対する姿勢を少なくとも推定するものであって、車両が加減速して走行しているときの、ライダ２に設けられたライダ用加速度センサ５の検出結果に基づいて、車両に対するライダ２の姿勢を推定する処理などを行う。これにより、車載機１は、ライダ２が出力する計測データを車両座標系に変換する処理を高精度に実行したり、ライダ２の使用可否の判定を行ったりすることができる。 As described above, the in-vehicle device 1 in this embodiment estimates at least the posture of the rider 2, which measures the distance to the object, with respect to the vehicle. Based on the detection result of the rider acceleration sensor 5 provided in the rider 2, a process of estimating the posture of the rider 2 with respect to the vehicle is performed. As a result, the in-vehicle device 1 can perform a process of converting the measurement data output by the rider 2 into the vehicle coordinate system with high accuracy, and can determine whether the rider 2 can be used.

［変形例］
以下、実施例に好適な変形例について説明する。以下の変形例は、組み合わせて実施例に適用してもよい。 [Modification]
Modifications suitable for the embodiment will be described below. The following modifications may be combined and applied to the embodiment.

（変形例１）
図１２のステップＳ１０８において、車載機１は、ステップＳ１０１～Ｓ１０５で算出した各変化量に基づきライダ２が出力する点群データの各計測値を補正する代わりに、ステップＳ１０１～Ｓ１０５で算出するライダ２の処理基準時の姿勢及び位置の各推定値に基づき、各計測値を車両座標系に変換してもよい。 (Modification 1)
In step S108 of FIG. 12, the vehicle-mounted device 1 corrects each measurement value of the point cloud data output by the rider 2 based on each change amount calculated in steps S101 to S105. Each measured value may be transformed into the vehicle coordinate system based on each estimated value of attitude and position at the time of processing reference in 2.

この場合、車載機１は、ステップＳ１０１～Ｓ１０５で算出したロール角Ｌ_φ、ピッチ角Ｌ_θ、ヨー角Ｌ_ψ、ｘ方向位置Ｌ_ｘ、ｙ方向位置Ｌ_ｙ、ｚ方向位置Ｌ_ｚを用いて、式（２４）に基づき、ライダ２が出力する点群データの各計測値をライダ座標系から車体座標系に変換し、変換後のデータに基づいて、自車位置推定や自動運転制御などを実行してもよい。 In this case, the vehicle-mounted device 1 uses the roll angle L _φ , pitch angle L _θ , yaw angle L _ψ , x-direction position L _x , _y -direction position Ly, and z-direction position L _z calculated in steps S101 to S105. , Based on equation (24), each measurement value of the point cloud data output by the rider 2 is converted from the rider coordinate system to the vehicle body coordinate system, and based on the converted data, vehicle position estimation, automatic driving control, etc. may be executed.

他の例では、車載機１は、各ライダ２の姿勢及び位置を修正するためのアクチュエータなどの調整機構が各ライダ２に備わっている場合には、ステップＳ１０８の処理に代えて、ステップＳ１０１～Ｓ１０５で算出した各変化量の分だけライダ２の姿勢及び位置を修正するように調整機構を駆動させる制御を行ってもよい。 In another example, if each rider 2 is provided with an adjustment mechanism such as an actuator for correcting the attitude and position of each rider 2, the vehicle-mounted device 1 performs steps S101 to S101 instead of step S108. Control may be performed to drive the adjustment mechanism so as to correct the posture and position of the rider 2 by the amount of each change calculated in S105.

（変形例２）
図１に示す運転支援システムの構成は一例であり、本発明が適用可能な運転支援システムの構成は図１に示す構成に限定されない。例えば、運転支援システムは、車載機１を有する代わりに、車両の電子制御装置が図１２等に示す処理を実行してもよい。この場合、ライダ設置情報ＩＬは、例えば車両内の記憶部に記憶され、車両の電子制御装置は、ライダ２などの各種センサの出力データを受信可能に構成される。 (Modification 2)
The configuration of the driving assistance system shown in FIG. 1 is an example, and the configuration of the driving assistance system to which the present invention is applicable is not limited to the configuration shown in FIG. For example, the driving support system may execute the processing shown in FIG. In this case, the rider installation information IL is stored, for example, in a storage unit within the vehicle, and the electronic control unit of the vehicle is configured to be able to receive output data from various sensors such as the rider 2 .

１車載機
２ライダ
３ジャイロセンサ
４車体用加速度センサ
５ライダ用加速度センサ
１０地図ＤＢ 1 On-vehicle device 2 Rider 3 Gyro sensor 4 Acceleration sensor for vehicle body 5 Acceleration sensor for rider 10 Map DB

Claims

An estimating device for estimating a posture of a measuring unit for measuring a distance to an object with respect to a moving object,
An estimating apparatus having an estimating unit for estimating the posture of the measuring unit with respect to the moving object based on the detection result of the acceleration detecting unit provided in the measuring unit when the moving object is traveling while accelerating or decelerating. .