JP7238490B2 - Sensor error correction device and sensor error correction program - Google Patents

Sensor error correction device and sensor error correction program Download PDF

Info

Publication number
JP7238490B2
JP7238490B2 JP2019043173A JP2019043173A JP7238490B2 JP 7238490 B2 JP7238490 B2 JP 7238490B2 JP 2019043173 A JP2019043173 A JP 2019043173A JP 2019043173 A JP2019043173 A JP 2019043173A JP 7238490 B2 JP7238490 B2 JP 7238490B2
Authority
JP
Japan
Prior art keywords
sensor
error
travel information
angle
pitch angle
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.)
Active
Application number
JP2019043173A
Other languages
Japanese (ja)
Other versions
JP2020144093A (en
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.)
Toyota Central R&D Labs Inc
Original Assignee
Toyota Central R&D Labs Inc
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 Toyota Central R&D Labs Inc filed Critical Toyota Central R&D Labs Inc
Priority to JP2019043173A priority Critical patent/JP7238490B2/en
Publication of JP2020144093A publication Critical patent/JP2020144093A/en
Application granted granted Critical
Publication of JP7238490B2 publication Critical patent/JP7238490B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Navigation (AREA)

Description

本発明は、センサ誤差補正装置、及びセンサ誤差補正プログラムに関する。 The present invention relates to a sensor error correction device and a sensor error correction program.

走行中の車両は、路面に対して前後運動、横運動及び旋回運動の3軸方向の運動を行う。当該運動の際に、車両は、地球座標系(すなわち大地)に対する角度である姿勢角が動的に変化する。例えば、旋回中の車体には、車体が左右方向に傾くロール角が生じる。また、加速している車体、及び減速している車体には、車体の進行方向に車体が傾くピッチ角が生じる。 A vehicle in motion performs motions in three axial directions, that is, longitudinal motion, lateral motion, and turning motion, with respect to a road surface. During the motion, the vehicle dynamically changes its attitude angle, which is the angle with respect to the earth coordinate system (ie, the ground). For example, a vehicle body that is turning has a roll angle that causes the vehicle body to tilt in the left-right direction. In addition, the accelerating vehicle body and the decelerating vehicle body have a pitch angle at which the vehicle body is tilted in the traveling direction of the vehicle body.

車両のロール角及びピッチ角を把握できれば、車体の制御に係る操舵角、アクセル開度及びブレーキの強度の各々を適切に決定でき、車両の自動運転において、より安定した制御が可能となる。また、車体の姿勢角の的確な推定により、車両の地図上での移動量がより正確に算出できるので、自動運転において、車両の自己位置の推定がより適切に行える。 If the roll angle and pitch angle of the vehicle can be grasped, the steering angle, accelerator opening degree, and brake strength related to vehicle control can be appropriately determined, and more stable control becomes possible in automatic driving of the vehicle. In addition, accurate estimation of the attitude angle of the vehicle body enables more accurate calculation of the amount of movement of the vehicle on the map, so that the self-position of the vehicle can be estimated more appropriately in automatic driving.

しかし、斜面等を走行する際に、車両が旋回運動を行うと左右方向の運動を検出するセンサに誤差が生じてしまうことがある。左右方向の運動を検出するセンサ誤差を適切に補正する技術について提案が行われている。 However, when the vehicle makes a turning motion while traveling on a slope or the like, an error may occur in the sensor that detects the motion in the left-right direction. Techniques for appropriately correcting sensor errors that detect motion in the left-right direction have been proposed.

例えば、計測したヨーレート及び推定したロール角を用いて水平面ヨーレートを推定し、水平面ヨーレートの変化量と、速度ベクトルの変化量とを比較して、左右方向の加速度の補正をする技術がある(特許文献1参照)。 For example, there is a technology that estimates the horizontal plane yaw rate using the measured yaw rate and the estimated roll angle, compares the amount of change in the horizontal plane yaw rate and the amount of change in the velocity vector, and corrects the acceleration in the horizontal direction (patented Reference 1).

特開2018-112520号公報JP 2018-112520 A

しかしながら、特許文献1に記載の技術では、実際のロール角(真値)の絶対値と、推定されたロール角の絶対値とを用いて大小関係を比較しているため、ロール角の大きさを比較するに留まり、ロール角の向きを十分考慮されていなかった。そのため、特許文献1に記載の技術では、傾斜している斜面に対して逆方向に旋回する逆バンク又は遠心力等の影響で、旋回方向とは逆向きに車両が傾く場合、左右方向の運動を検出するセンサの値を必ずしも精度よく補正できるとは限らなかった。 However, in the technique described in Patent Document 1, the magnitude relationship is compared using the absolute value of the actual roll angle (true value) and the estimated absolute value of the roll angle. was only compared, and the direction of the roll angle was not sufficiently considered. Therefore, in the technique described in Patent Document 1, when the vehicle tilts in the opposite direction to the turning direction due to the reverse bank turning in the opposite direction to the inclined slope or the influence of centrifugal force, etc., the left-right motion It was not always possible to accurately correct the value of the sensor that detects .

本開示は、以上の事情を鑑みて成されたものであり、逆バンク又は遠心力等の影響で、旋回方向と逆方向に車体が傾く場合であっても、左右方向の車両の傾き及び加速度を精度よく補正できるセンサ誤差補正装置、及びセンサ誤差補正プログラムを提供することを目的とする。 The present disclosure has been made in view of the above circumstances. It is an object of the present invention to provide a sensor error correction device and a sensor error correction program capable of accurately correcting .

上記目的を達成するために、請求項1に記載のセンサ誤差補正装置は、移動体に搭載された、移動体の回転運動を検出する角速度センサ、移動体の並進運動を検出する加速度センサ、及び移動体の車輪の速度を検出する車輪速センサの各々から検出した値を含む走行情報を取得する取得部と、走行情報の角速度センサから検出した値を用いて、移動体のピッチ角方向の回転量を示す第一の角度変化量を推定し、走行情報の加速度センサ及び車輪速センサから検出した値を用いて、移動体のピッチ角方向の回転量を示す第二の角度変化量を推定する推定部と、第一の角度変化量及び前記第二の角度変化量の差分から走行情報の誤差を導出し、誤差に基づいて、取得部で取得した走行情報を補正する補正部と、を備え、走行情報の誤差は、移動体の車幅方向に沿う左右方向の加速度センサの誤差であり、補正部は、走行情報の左右方向の加速度センサの値を補正するTo achieve the above object, the sensor error correction device according to claim 1 comprises an angular velocity sensor mounted on a mobile body for detecting rotational motion of the mobile body, an acceleration sensor for detecting translational motion of the mobile body, and Rotation in the pitch angle direction of the mobile object using an acquisition unit that acquires travel information including values detected by each of the wheel speed sensors that detect the speed of the wheels of the mobile object, and the values detected by the angular velocity sensors of the travel information. estimating a first angle change amount indicating the amount of rotation, and using the values detected by the acceleration sensor and wheel speed sensor of the travel information, estimating a second angle change amount indicating the amount of rotation of the moving body in the pitch angle direction an estimating unit; and a correcting unit that derives an error in travel information from the difference between the first angle change amount and the second angle change amount, and corrects the travel information acquired by the acquisition unit based on the error. , the error of the travel information is the error of the acceleration sensor in the lateral direction along the vehicle width direction of the moving object, and the correction unit corrects the value of the acceleration sensor in the lateral direction of the travel information.

また、請求項2に記載の発明は、請求項1に記載の発明において、走行情報は、移動体が旋回を始めてから終了するまでの期間に取得部によって取得される。 Further, according to the second aspect of the invention, in the first aspect of the invention, the traveling information is acquired by the acquisition unit during a period from when the moving object starts to when it finishes turning.

また、請求項3に記載の発明は、請求項1又は2の発明において、補正部は、導出した走行情報の誤差と、前回に補正部によって導出された走行情報の誤差とを用いて、走行情報をさらに補正する。 Further, according to the invention of claim 3, in the invention of claim 1 or 2, the correcting unit uses the error of the derived traveling information and the error of the traveling information previously derived by the correcting unit to correct the travel. Further correct the information.

また、請求項に記載の発明は、請求項1からの何れか一項に記載の発明において、推定部は、走行情報からロール角をさらに推定し、補正部は、ロール角の誤差をさらに導出し、ロール角を補正する。 Further, the invention according to claim 4 is the invention according to any one of claims 1 to 3 , wherein the estimation unit further estimates the roll angle from the traveling information, and the correction unit corrects the roll angle error. Further derive and correct the roll angle.

一方、上記目的を達成するために、請求項に記載のセンサ誤差補正プログラムは、移動体に搭載された、移動体の回転運動を検出する角速度センサ、移動体の並進運動を検出する加速度センサ、及び移動体の車輪の速度を検出する車輪速センサの各々から検出した値を含む走行情報を取得するステップと、走行情報の角速度センサから検出した値を用いて、移動体のピッチ角方向の回転量を示す第一の角度変化量を推定し、走行情報の加速度センサ及び車輪速センサから検出した値を用いて、移動体のピッチ角方向の回転量を示す第二の角度変化量を推定するステップと、第一の角度変化量及び第二の角度変化量の差分から走行情報の誤差を導出し、誤差に基づいて、取得部で取得した走行情報を補正するステップと、を含み、走行情報の誤差は、移動体の車幅方向に沿う左右方向の加速度センサの誤差であり、補正するステップは、走行情報の左右方向の加速度センサの値を補正する処理をコンピュータに実行させる。 On the other hand, in order to achieve the above object, a sensor error correction program according to claim 5 comprises an angular velocity sensor mounted on a mobile body for detecting rotational motion of the mobile body and an acceleration sensor for detecting translational motion of the mobile body. , and a step of acquiring traveling information including values detected from each of wheel speed sensors that detect the speed of the wheels of the moving body, and using the values detected from the angular velocity sensors of the traveling information to determine the pitch angle direction of the moving body. A first angle change amount indicating the amount of rotation is estimated, and a second angle change amount indicating the amount of rotation in the pitch angle direction of the moving object is estimated using the values detected by the acceleration sensor and wheel speed sensor of the travel information. and a step of deriving an error of the travel information from the difference between the first angle change amount and the second angle change amount, and correcting the travel information acquired by the acquisition unit based on the error. The error of the information is the error of the acceleration sensor in the lateral direction along the vehicle width direction of the moving body, and the step of correcting causes the computer to execute the process of correcting the value of the acceleration sensor in the lateral direction of the travel information.

本開示によれば、傾斜している斜面に対して逆方向に旋回する逆バンク又は遠心力等の影響で旋回方向とは逆向きに車両が傾いている場合でも、横方向のセンサの値を補正できる。 According to the present disclosure, even if the vehicle is tilted in the opposite direction to the turning direction due to the reverse bank turning in the opposite direction to the inclined slope or the influence of centrifugal force, etc., the sensor value in the lateral direction can be detected. can be corrected.

本実施形態に係るセンサを搭載した車両の一例を模式的に示す模式図である。It is a schematic diagram which shows typically an example of the vehicle which mounts the sensor which concerns on this embodiment. 本実施形態に係るセンサ誤差補正装置のハードウェア構成の一例を示すブロック図である。It is a block diagram showing an example of the hardware constitutions of the sensor error correction device concerning this embodiment. 本実施形態に係るセンサ誤差補正装置の機能的な構成の一例を示すブロック図である。It is a block diagram showing an example of a functional configuration of a sensor error correction device according to the present embodiment. 本実施形態に係るゼロ点補正値の導出の説明に供するセンサ誤差補正装置の入出力の一例を示すブロック図である。FIG. 3 is a block diagram showing an example of inputs and outputs of a sensor error correction device for explaining derivation of a zero point correction value according to the present embodiment; 本実施形態に係る新たなゼロ点補正値の導出の説明に供するセンサ誤差補正装置の入出力の一例を示すブロック図である。FIG. 3 is a block diagram showing an example of inputs and outputs of a sensor error correction device for explaining derivation of a new zero point correction value according to the present embodiment; 本実施形態に係る近似したピッチ角を用いたゼロ点補正値の導出の説明に供するセンサ誤差補正装置の入出力の一例を示すブロック図である。FIG. 4 is a block diagram showing an example of inputs and outputs of a sensor error correction device for explaining derivation of a zero point correction value using an approximated pitch angle according to the present embodiment; 本実施形態に係るヨーレートと時間の関係の一例を示すグラフである。5 is a graph showing an example of the relationship between yaw rate and time according to the embodiment; 本実施形態に係るセンサ誤差補正処理の一例を示すフローチャートである。6 is a flowchart showing an example of sensor error correction processing according to the embodiment;

以下、図面を参照して、本開示の技術を実施するための形態例を詳細に説明する。なお、本実施形態は、移動体が車両である場合を例示する。しかし、移動体は、車両に限定されない。移動体は、移動する物体であれば、飛行する物体であってもよい。以下、車両に搭載されたセンサ誤差補正装置について詳細に説明する。 Embodiments for implementing the technology of the present disclosure will be described in detail below with reference to the drawings. In addition, this embodiment illustrates the case where the moving object is a vehicle. However, mobile bodies are not limited to vehicles. The moving object may be a flying object as long as it is a moving object. The sensor error correction device mounted on the vehicle will be described in detail below.

図1は、本実施形態に係るセンサを搭載した車両10の一例を模式的に示す模式図である。車両10は、車両10の回転運動を検出する角速度センサ11、車両10の並進運動を検出する加速度センサ12、及び車両10の車輪の速度を検出する車輪速センサ13を搭載している。 FIG. 1 is a schematic diagram schematically showing an example of a vehicle 10 equipped with a sensor according to this embodiment. The vehicle 10 is equipped with an angular velocity sensor 11 that detects rotational motion of the vehicle 10 , an acceleration sensor 12 that detects translational motion of the vehicle 10 , and a wheel speed sensor 13 that detects the speed of the wheels of the vehicle 10 .

角速度センサ11は、単位時間当たりの車両10のピッチ角方向の回転の変化量を示すピッチレートQを検出する。また、角速度センサ11は、単位時間当たりの車両10のロール角の回転の変化量を示すロールレートPを検出する。また、角速度センサ11は、単位時間当たりの車両10の鉛直軸まわりの回転の変化量を示すヨーレートRを検出する。 The angular velocity sensor 11 detects a pitch rate Q that indicates the amount of change in rotation of the vehicle 10 in the pitch angle direction per unit time. The angular velocity sensor 11 also detects a roll rate P that indicates the amount of change in rotation of the roll angle of the vehicle 10 per unit time. The angular velocity sensor 11 also detects a yaw rate R that indicates the amount of change in rotation of the vehicle 10 about the vertical axis per unit time.

加速度センサ12は、車両10の進行方向に沿う前後方向の加速度G、車両10の車幅方向に沿う左右方向の加速度G、車両10の鉛直軸方向に沿う上下方向の加速度Gを検出する。また、車輪速センサ13は、車両10の車輪の速度Uを検出する。 The acceleration sensor 12 detects an acceleration G x in the longitudinal direction along the traveling direction of the vehicle 10 , an acceleration G y in the lateral direction along the width direction of the vehicle 10 , and an acceleration G z in the vertical direction along the vertical axis of the vehicle 10 . do. Moreover, the wheel speed sensor 13 detects the speed U of the wheels of the vehicle 10 .

図2は、本実施形態に係るセンサ誤差補正装置20のハードウェア構成の一例を示すブロック図である。図2に示すように、本実施形態に係るセンサ誤差補正装置20は、車両10に搭載され、CPU(Central Processing Unit)21、ROM(Read Only Memory)22、RAM(Random Access Memory)23、ストレージ24、表示部25、及びセンサ11、12、13を含んで構成されている。CPU21、ROM22、RAM23、ストレージ24、表示部25、及びセンサ11、12、13の各々はバス26により相互に接続されている。 FIG. 2 is a block diagram showing an example of the hardware configuration of the sensor error correction device 20 according to this embodiment. As shown in FIG. 2, a sensor error correction device 20 according to the present embodiment is mounted on a vehicle 10 and includes a CPU (Central Processing Unit) 21, a ROM (Read Only Memory) 22, a RAM (Random Access Memory) 23, a storage 24, a display unit 25, and sensors 11, 12, and 13. CPU 21 , ROM 22 , RAM 23 , storage 24 , display unit 25 , and sensors 11 , 12 , 13 are interconnected by bus 26 .

CPU21は、センサ誤差補正装置20の全体を統括し、制御する。ROM22は、本実施形態で用いるセンサ誤差補正プログラムを含む各種プログラム及びデータ等を記憶している。RAM23は、各種プログラムの実行時のワークエリアとして用いられるメモリである。CPU21は、ROM22に記憶されたプログラムをRAM23に展開して実行することにより、センサの値の検出及び補正を行う。ストレージ24は、一例としてHDD(Hard Disk Drive)、SSD(Solid State Drive)、又はフラッシュメモリ等である。なお、ストレージ24には、センサ誤差補正プログラム等を記憶してもよい。 The CPU 21 supervises and controls the sensor error correction device 20 as a whole. The ROM 22 stores various programs including a sensor error correction program used in this embodiment, data, and the like. The RAM 23 is a memory used as a work area when executing various programs. The CPU 21 develops a program stored in the ROM 22 in the RAM 23 and executes it, thereby detecting and correcting sensor values. The storage 24 is, for example, a HDD (Hard Disk Drive), SSD (Solid State Drive), flash memory, or the like. Note that the storage 24 may store a sensor error correction program or the like.

表示部25は、角速度センサ11、加速度センサ12、及び車輪速センサから検出したデータ等を表示する。表示部25は、例えば、液晶モニタ、CRT(Cathode Ray Tube)モニタ、FPD(Flat Panel Display)モニタ等である。 The display unit 25 displays data and the like detected by the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor. The display unit 25 is, for example, a liquid crystal monitor, a CRT (Cathode Ray Tube) monitor, an FPD (Flat Panel Display) monitor, or the like.

なお、本実施形態では角速度センサ11、加速度センサ12、及び車輪速センサ13で取得した走行情報を表示部25に表示する形態について説明した。しかし、これに限定されない。取得した走行情報を車両10に搭載されている別の装置に連携する形態としてもよいし、補正した後の走行情報を表示部25に表示する形態としてもよい。 In the present embodiment, the configuration in which the traveling information acquired by the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor 13 is displayed on the display unit 25 has been described. However, it is not limited to this. The acquired travel information may be linked to another device installed in the vehicle 10 , or the corrected travel information may be displayed on the display unit 25 .

センサ11、12、13は、車両10の走行中の姿勢及び走行状態等を検出する角速度センサ11、加速度センサ12、及び車輪速センサ13である。 The sensors 11 , 12 , 13 are an angular velocity sensor 11 , an acceleration sensor 12 , and a wheel speed sensor 13 that detect the posture and running state of the vehicle 10 while the vehicle 10 is running.

なお、本実施形態では、センサ誤差補正装置20が車両10に搭載されている形態について説明した。しかし、これに限定されない。センサ誤差補正装置20は、車両10の外部に設けられてもよい。この場合、センサ11、12、13によって検出されたデータは、通信回線等を通して別の場所に設置されているセンサ誤差補正装置20に送信され、センサ誤差補正装置20において走行情報の誤差が補正される処理を行ってもよい。 Note that, in the present embodiment, the configuration in which the sensor error correction device 20 is mounted on the vehicle 10 has been described. However, it is not limited to this. The sensor error correction device 20 may be provided outside the vehicle 10 . In this case, the data detected by the sensors 11, 12, and 13 are transmitted to the sensor error correction device 20 installed at another location through a communication line or the like, and the sensor error correction device 20 corrects the travel information error. processing may be performed.

次に、センサ誤差補正装置20の機能構成について説明する。図3は、本実施形態に係るセンサ誤差補正装置20の機能的な構成の一例を示すブロック図である。 Next, the functional configuration of the sensor error correction device 20 will be described. FIG. 3 is a block diagram showing an example of the functional configuration of the sensor error correction device 20 according to this embodiment.

図3に示すように、センサ誤差補正装置20は、取得部31、推定部32、及び補正部33を有する。CPU13がセンサ誤差補正プログラムを実行することで、取得部31、推定部32、及び補正部33として機能する。 As shown in FIG. 3 , the sensor error correction device 20 has an acquisition section 31 , an estimation section 32 and a correction section 33 . The CPU 13 functions as an acquisition unit 31 , an estimation unit 32 , and a correction unit 33 by executing the sensor error correction program.

取得部31は、車両10に搭載されている角速度センサ11、加速度センサ12、及び車輪速センサ13からデータを取得する。具体的には、取得部31は、車両10に搭載された角速度センサから車体の傾きの変化量を示すロールレートP、ピッチレートQ、及びヨーレートRを取得する。また、取得部31は、加速度センサ12から前後方向の加速度Gx、左右方向の加速度G、鉛直方向の加速度Gを取得し、車輪速センサ13から車輪の速度Uを取得する。つまり、取得部31は、車両10に搭載された、角速度センサ11、加速度センサ12、及び車輪速センサ13の各々から検出した値を含むデータ(以下、「走行情報」という。)を取得する。また、走行情報は、移動体が旋回を始めてから終了するまでの期間に取得部31によって取得される。 Acquisition unit 31 acquires data from angular velocity sensor 11 , acceleration sensor 12 , and wheel speed sensor 13 mounted on vehicle 10 . Specifically, the acquisition unit 31 acquires a roll rate P, a pitch rate Q, and a yaw rate R that indicate the amount of change in the tilt of the vehicle body from an angular velocity sensor mounted on the vehicle 10 . The acquisition unit 31 also acquires the longitudinal acceleration Gx, the lateral acceleration G y , and the vertical acceleration G z from the acceleration sensor 12 , and acquires the wheel speed U from the wheel speed sensor 13 . That is, the acquisition unit 31 acquires data (hereinafter referred to as “travel information”) including values detected by each of the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor 13 mounted on the vehicle 10. Further, the travel information is acquired by the acquisition unit 31 during the period from when the moving object starts to when it finishes turning.

なお、本実施形態に係る走行情報は、角速度センサ11、加速度センサ12、及び車輪速センサ13から取得したデータであると説明した。しかし、これに限定されない。走行情報は、車両10の位置を示す情報及びデータを取得した日時を含む形態としてもよいし、走行情報は、角速度センサ11、加速度センサ12、及び車輪速センサ13から取得したデータの少なくとも1つを含む形態としてもよい。 It has been described that the travel information according to the present embodiment is data obtained from the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor 13. FIG. However, it is not limited to this. The travel information may include information indicating the position of the vehicle 10 and the date and time when the data was acquired. It is good also as a form containing.

推定部32は、走行情報の角速度センサ11から検出した値を用いて、移動体のピッチ角方向の回転量を示す第一の角度変化量を推定する。また、推定部32は、走行情報の加速度センサ12及び車輪速センサ13から検出した値を用いて、移動体のピッチ角方向の回転量を示す第二の角度変化量を推定する。つまり、推定部32は、角速度センサ11から取得した走行情報を用いて第一のピッチ角の変化量を推定する。また、推定部32は、加速度センサ12、及び車輪速センサ13から取得した走行情報を用いて、ロール角φ、ピッチ角θ、及び第二のピッチ角の変化量を推定する。 The estimating unit 32 estimates a first angle change amount indicating the amount of rotation in the pitch angle direction of the moving object, using the value detected by the angular velocity sensor 11 of the travel information. The estimating unit 32 also estimates a second angle change amount indicating the amount of rotation in the pitch angle direction of the moving body using the values detected by the acceleration sensor 12 and the wheel speed sensor 13 of the travel information. That is, the estimation unit 32 estimates the amount of change in the first pitch angle using the travel information acquired from the angular velocity sensor 11 . The estimator 32 also uses the travel information acquired from the acceleration sensor 12 and the wheel speed sensor 13 to estimate the amount of change in the roll angle φ, the pitch angle θ, and the second pitch angle.

補正部33は、第一の角度変化量及び第二の角度変化量の差分から走行情報の誤差を導出し、誤差に基づいて、取得部31で取得した走行情報を補正する。つまり、補正部33は、第一のピッチ角の変化量、及び第二のピッチ角の変化量の差分から、推定されたロール角φに含まれる誤差と、加速度センサ12によって検出された加速度Gに含まれる誤差とを導出する。また、補正部33は、導出したロール角の誤差、及び加速度Gの誤差を用いて、推定したロール角φ、及び走行情報に含まれる加速度Gを補正する。 The correction unit 33 derives the error of the travel information from the difference between the first angle change amount and the second angle change amount, and corrects the travel information acquired by the acquisition unit 31 based on the error. That is, the correction unit 33 calculates the error included in the estimated roll angle φ from the difference between the first pitch angle change amount and the second pitch angle change amount, and the acceleration G and the error contained in y . Further, the correction unit 33 corrects the estimated roll angle φ and the acceleration Gy included in the travel information using the derived roll angle error and acceleration Gy error.

次に、センサ誤差補正装置20の作用について説明する前に、車両10に搭載されたセンサの誤差を推定するための手法を説明する。本実施形態では、6軸センサの運動方程式を用いた誤差の推定方法について説明する。 Next, before describing the action of the sensor error correction device 20, a method for estimating the error of the sensor mounted on the vehicle 10 will be described. In this embodiment, an error estimation method using the equation of motion of a 6-axis sensor will be described.

並進運動の状態を示す運動方程式では、6軸センサの運動方程式を用いて、ピッチ角を導出しても累積による誤差は発生しないのに対して、回転運動の状態を示す運動方程式では、計測時間で累積を行うため、導出するとピッチ角に累積誤差が発生する。本実施形態では、並進運動の状態を示す運動方程式及び回転運動の状態を示す運動方程式の性質を用いて、誤差を導出する。なお、本実施形態に係る誤差は、基準の位置からのずれを表す値(以下、「オフセット誤差」という。)であるとする。 In the equation of motion that indicates the state of translational motion, even if the pitch angle is derived using the equation of motion of a 6-axis sensor, no accumulated error occurs. Since accumulation is performed by , an accumulated error occurs in the pitch angle when derived. In this embodiment, the error is derived using the properties of the equation of motion indicating the state of translational motion and the equation of motion indicating the state of rotational motion. It should be noted that the error according to the present embodiment is a value representing deviation from the reference position (hereinafter referred to as "offset error").

本実施形態では、錯綜を避けるために、ロール角φは十分小さい(傾斜10度以下)ものとし、ヨーレートR、ピッチレートQ、前後方向の加速度Gは、補正済みであり、誤差が含まれないものとする。また、本実施形態では、車両10は、車両10の車幅方向に沿う左右方向の速度V、及び車両10の鉛直方向に沿う上下方向の速度Wは、無視できるほど小さい場合について説明する。 In this embodiment, in order to avoid complication, the roll angle φ is assumed to be sufficiently small (inclination of 10 degrees or less), and the yaw rate R, pitch rate Q, and longitudinal acceleration Gx have already been corrected, and errors are not included. Make it not exist. In the present embodiment, the vehicle 10 has a lateral velocity V along the width direction of the vehicle 10 and a vertical velocity W along the vertical direction of the vehicle 10, which are so small as to be negligible.

本実施形態に係る角速度センサ11及び加速度センサ12に適用される6軸センサの運動方程式を用いて、ピッチ角θ及びロール角φの推定を行う。6軸センサの運動方程式は、以下の数式によって表される。 The pitch angle θ and the roll angle φ are estimated using the equation of motion of the 6-axis sensor applied to the angular velocity sensor 11 and the acceleration sensor 12 according to this embodiment. The equation of motion of the 6-axis sensor is represented by the following formulas.

Figure 0007238490000001
Figure 0007238490000001

Figure 0007238490000002
Figure 0007238490000002

Figure 0007238490000003
Figure 0007238490000003

Figure 0007238490000004
Figure 0007238490000004

Figure 0007238490000005
Figure 0007238490000005

Figure 0007238490000006
Figure 0007238490000006

上述した式(1)、式(2)、及び式(3)は移動体の回転運動の状態を示す式である。また、上述した式(4)、式(5)、式(6)は、移動体の並進運動の状態を示す式である。 Equations (1), (2), and (3) described above are equations representing the state of rotational motion of the moving body. Also, the above equations (4), (5), and (6) are equations indicating the state of the translational motion of the moving body.

式(2)を本実施形態に係る車両10に搭載された角速度センサ11に、式(4)及び式(5)を本実施形態に係る車両10に搭載された加速度センサ12及び車輪速センサ13に適用し、ピッチ角θ、及びロール角φを導出する。導出するピッチ角θ、及びロール角φは、以下の数式のよって表される。 Formula (2) is applied to the angular velocity sensor 11 mounted on the vehicle 10 according to the present embodiment, and formulas (4) and (5) are applied to the acceleration sensor 12 and the wheel speed sensor 13 mounted on the vehicle 10 according to the present embodiment. to derive the pitch angle θ and roll angle φ. The derived pitch angle θ and roll angle φ are represented by the following formulas.

Figure 0007238490000007
Figure 0007238490000007

Figure 0007238490000008
Figure 0007238490000008

Figure 0007238490000009
Figure 0007238490000009

ここで、式(7)の左辺(θの上にドットが付された記号)は、微小時間当たりの角速度センサによって検出されたピッチ角θの変化量を表し、φは、加速度センサ12によって検出されたロール角φを表している。また、gは、重力加速度を表し、θは、加速度センサ12によって検出されたピッチ角を表している。また、式(8)の右辺の第1項(Uの上にドットが付された記号)は、微小時間当たりの車輪速センサ13によって検出された車輪速度Uの変化量を表している。 Here, the left side of equation (7) (the symbol with a dot above θ) represents the amount of change in the pitch angle θ detected by the angular velocity sensor per minute time, and φ A is the amount detected by the acceleration sensor 12. It represents the detected roll angle φ. Also, g represents the gravitational acceleration, and θ A represents the pitch angle detected by the acceleration sensor 12 . Also, the first term (dotted symbol above U) on the right side of equation (8) represents the amount of change in the wheel speed U detected by the wheel speed sensor 13 per minute.

式(7)及び式(8)を用いて、ロール角の誤差を導出する。式(7)の両辺を積算したものは、以下の数式によって表される。 Equations (7) and (8) are used to derive the roll angle error. The sum of both sides of Equation (7) is represented by the following formula.

Figure 0007238490000010
Figure 0007238490000010

ここで、Δθは、角速度センサが検出したピッチ角の変化量であり、Tは、角速度センサが検出を開始した時間であり、Tは角速度センサが検出を終了した時間である。 Here, ΔθG is the amount of change in the pitch angle detected by the angular velocity sensor, Ts is the time when the angular velocity sensor started detection, and Te is the time when the angular velocity sensor finished detection.

また、ピッチ角は時間に依存するものとすると、加速度センサが検出した時間で近似することができ、式(8)は、以下の数式によって表される。 Also, if the pitch angle depends on time, it can be approximated by the time detected by the acceleration sensor, and equation (8) is expressed by the following equation.

Figure 0007238490000011
Figure 0007238490000011

ここで、Δθは、加速度センサが検出したピッチ角の変化量であり、aは、ピッチ角θの傾きである。よって、式(10)、及び式(11)の差分は、以下の数式によって表される。 Here, Δθ A is the amount of change in the pitch angle detected by the acceleration sensor, and a is the inclination of the pitch angle θ A. Therefore, the difference between equations (10) and (11) is represented by the following equations.

Figure 0007238490000012
Figure 0007238490000012

本来であれば、式(12)の右辺の第1項と第2項は、角速度センサ11、及び加速度センサ12のそれぞれで検出したピッチ角θの変化量であるため、同値となり、打ち消しあう。しかし、ロール角φに、オフセット誤差が含まれる場合、角速度センサ11で検出したピッチ角の変化量Δθに誤差が生じる。 Originally, the first and second terms on the right side of equation (12) are the amount of change in the pitch angle θ detected by the angular velocity sensor 11 and the acceleration sensor 12, respectively, so they have the same value and cancel each other out. However, if the roll angle φ A includes an offset error, an error occurs in the amount of change Δθ G in the pitch angle detected by the angular velocity sensor 11 .

ここで、ロール角φは、十分小さいことを考慮すると、cosφは1、sinφはφに近似することができ、式(12)は以下の数式によって表される。 Considering that the roll angle φ is sufficiently small, cos φ can be approximated to 1 and sin φ can be approximated to φ, and Equation (12) is expressed by the following formula.

Figure 0007238490000013
Figure 0007238490000013

加速度センサ12によって検出されたロール角φにオフセット誤差を含んでいることを考慮すると、加速度センサ12によって検出されたロール角φは、以下の数式によって表される。 Considering that the roll angle φ A detected by the acceleration sensor 12 includes an offset error, the roll angle φ A detected by the acceleration sensor 12 is represented by the following formula.

Figure 0007238490000014
Figure 0007238490000014

ここで、φは誤差を含んでいない車両10の左右方向のロール角であり、εφは、ロール角φに含まれているロール角の誤差であり、ロール角の誤差は一定値であるとする。式(14)を式(13)に適用すると、角速度センサ11、及び加速度センサ12のそれぞれで検出したピッチ角の変化量Δθの差分は、以下の数式によって表される。 Here, φ is the lateral roll angle of the vehicle 10 that does not include an error, εφ is the roll angle error included in the roll angle φA , and the roll angle error is assumed to be a constant value. do. When formula (14) is applied to formula (13), the difference between the pitch angle variation Δθ detected by each of the angular velocity sensor 11 and the acceleration sensor 12 is represented by the following formula.

Figure 0007238490000015
Figure 0007238490000015

式(15)の右辺の第2項は、誤差を含んでいない車両10のピッチ角の変化量に対応し、加速度センサ12が検出したピッチ角の変化量である第1項と同値であるとみなすことができる。よって、式(15)は以下の数式によって表される。 The second term on the right side of equation (15) corresponds to the amount of change in the pitch angle of the vehicle 10 that does not contain an error, and is equivalent to the first term that is the amount of change in the pitch angle detected by the acceleration sensor 12. can be regarded as Therefore, the formula (15) is represented by the following formula.

Figure 0007238490000016
Figure 0007238490000016

式(16)を整理すると、加速度センサ12で検出したロール角φに含まれているロール角の誤差εφは、以下の数式によって表される。 By rearranging the equation (16), the roll angle error ε φ included in the roll angle φ A detected by the acceleration sensor 12 is represented by the following formula.

Figure 0007238490000017
Figure 0007238490000017

以上、式(17)によると、ロール角φに含まれているロール角の誤差εφは、加速度センサ12で検出したピッチ角の変化量Δθと、角速度センサ11で検出したピッチ角の変化量Δθとの差分から求めることができる。 As described above, according to the equation (17), the roll angle error ε φ included in the roll angle φ A is the difference between the change amount Δθ A of the pitch angle detected by the acceleration sensor 12 and the pitch angle detected by the angular velocity sensor 11. It can be obtained from the difference from the amount of change ΔθG .

次に、式(9)を用いて、加速度センサ12の左右方向の加速度Gの誤差を導出する。ここで、ロール角φは、十分小さいことを考慮すると、sinφはφに近似することができ、加速度センサ12によって検出されたロール角φは以下の数式によって表される。 Next, the error of the lateral acceleration Gy of the acceleration sensor 12 is derived using equation (9). Considering that the roll angle φ is sufficiently small, sin φ can be approximated to φ, and the roll angle φA detected by the acceleration sensor 12 is expressed by the following formula.

Figure 0007238490000018
Figure 0007238490000018

ここで、加速度センサ12によって検出されたロール角φの誤差は、左右方向の加速度Gによるものであり、左右方向の加速度Gに誤差が含まれていることを考慮すると、誤差を補正することで、車両10のロール角φを求めることができる。加速度センサ12によって検出された加速度Gの誤差を補正する値(以下、「ゼロ点補正値」という。)を用いて補正すると、車両10のロール角φは、以下の数式で表される。 Here, the error in the roll angle φA detected by the acceleration sensor 12 is due to the acceleration Gy in the horizontal direction, and considering that the error is included in the acceleration Gy in the horizontal direction, the error is corrected By doing so, the roll angle φ of the vehicle 10 can be obtained. The roll angle φ of the vehicle 10 is expressed by the following formula when corrected using a value for correcting the error of the acceleration Gy detected by the acceleration sensor 12 (hereinafter referred to as "zero point correction value").

Figure 0007238490000019
Figure 0007238490000019

Figure 0007238490000020
Figure 0007238490000020

ζは、ゼロ点補正値であり、式(19)は、加速度センサ12によって検出された加速度Gにゼロ点補正値ζを用いて補正したものである。式(20)は、ゼロ点補正値ζを用いて補正することで求まる車両10のロール角である。 ζ y is a zero point correction value, and Equation (19) is obtained by correcting the acceleration G y detected by the acceleration sensor 12 using the zero point correction value ζ y . Equation (20) is the roll angle of the vehicle 10 obtained by correcting using the zero point correction value ζy .

また、加速度センサ12によって検出されたロール角φにオフセット誤差を含んでいることを考慮すると、式(18)、及び式(20)を式(14)に代入すると、以下の数式によって表される。 Considering that the roll angle φA detected by the acceleration sensor 12 includes an offset error, substituting equations (18) and (20) into equation (14) gives the following equation: be.

Figure 0007238490000021
Figure 0007238490000021

式(21)を整理すると、ゼロ点補正値ζは、以下の数式によって表される。 By rearranging equation (21), the zero point correction value ζ y is represented by the following equation.

Figure 0007238490000022
Figure 0007238490000022

以上、式(22)によると、加速度センサ12よって検出された左右方向の加速度Gを補正するゼロ点補正値ζは、ロール角の誤差εφと、ピッチ角θを用いて求めることができる。 As described above, according to the equation (22), the zero point correction value ζ y for correcting the horizontal acceleration G y detected by the acceleration sensor 12 can be obtained using the roll angle error ε φ and the pitch angle θ. can.

求めたゼロ点補正値ζを加速度センサ12によって検出した走行情報に反映することで、新たにゼロ点補正値を求める際に、前回求めたゼロ点補正値を考慮して、新たなゼロ点補正値を導出できる。新たなゼロ点補正値は、以下の数式によって表される。 By reflecting the determined zero point correction value ζ y in the traveling information detected by the acceleration sensor 12, when determining a new zero point correction value, the previously determined zero point correction value is taken into account to obtain a new zero point A correction value can be derived. A new zero point correction value is represented by the following formula.

Figure 0007238490000023
Figure 0007238490000023

ここで、kは、ゼロ点補正値を導出した回数である。つまり、2回目以降にゼロ点補正値を導出する場合は、式(23)に示すように、前回導出したゼロ点補正値を用いて導出できる。 Here, k is the number of times the zero point correction value is derived. That is, when deriving the zero point correction value for the second and subsequent times, it can be derived using the previously derived zero point correction value, as shown in Equation (23).

なお、本実施形態では、旋回中のピッチ角θの変化の割合が一定である形態について説明した。しかし、これに限定されない。旋回中のピッチ角θの変化の割合が一定でない場合、ピッチ角の変化量Δθは、加速度センサが検出を開始した時点のピッチ角及び加速度センサが検出を終了した時点のピッチ角の差分を用いて導出してもよい。上述の内容を考慮すると、ピッチ角の変化量Δθは、以下の数式によって表される。 In this embodiment, a mode in which the rate of change in the pitch angle θA during turning is constant has been described. However, it is not limited to this. If the rate of change in the pitch angle θA during turning is not constant, the pitch angle change amount ΔθA is the difference between the pitch angle when the acceleration sensor starts detection and the pitch angle when the acceleration sensor finishes detection. may be derived using Considering the above contents, the pitch angle change amount Δθ A is represented by the following formula.

Figure 0007238490000024
Figure 0007238490000024

θ(T)は、旋回を開始した時点のピッチ角であり、θ(Te)は、旋回を終了した時点のピッチ角である。 θ A (T s ) is the pitch angle at the start of turning, and θ A (T e ) is the pitch angle at the end of turning.

また、本実施形態では、ピッチ角θは、十分大きい値である形態について説明した。しかし、これに限定されない。ピッチ角θが十分小さい場合、ピッチ角θは、cosθは1、sinθはθに近似してもよい。ピッチ角θが十分小さいことを考慮すると、式(22)は以下の数式によって表される。 Moreover, in the present embodiment, the pitch angle θ A has a sufficiently large value. However, it is not limited to this. When the pitch angle θ A is sufficiently small, the pitch angle θ A may approximate cos θ A to 1 and sin θ A to θ A . Considering that the pitch angle θ A is sufficiently small, Equation (22) is represented by the following equation.

Figure 0007238490000025
Figure 0007238490000025

次に、図4、5、6を参照して、本実施形態に係るデータの入出力について説明する。まず、図4を参照して、ゼロ点補正値を導出する場合のデータの入出力について説明する。図4は、本実施形態に係るゼロ点補正値の導出の説明に供するセンサ誤差補正装置の入出力の一例を示すブロック図である。 Next, input/output of data according to the present embodiment will be described with reference to FIGS. First, with reference to FIG. 4, data input/output when deriving a zero point correction value will be described. FIG. 4 is a block diagram showing an example of input/output of the sensor error correction device for explaining the derivation of the zero point correction value according to this embodiment.

車両10に搭載された角速度センサ11は、ロールレートP、及びピッチレートQ、ヨーレートRを検出し、加速度センサ12は、前後方向の加速度G、左右方向の加速度G、及び上下方向の加速度Gを検出し、車輪速センサ13は、車輪速度Uを検出する。 An angular velocity sensor 11 mounted on the vehicle 10 detects a roll rate P, a pitch rate Q, and a yaw rate R, and an acceleration sensor 12 detects longitudinal acceleration G x , lateral acceleration G y , and vertical acceleration. Gz is detected, and the wheel speed sensor 13 detects the wheel speed U.

取得部31は、角速度センサ11、加速度センサ12、及び車輪速センサ13から検出した値を走行情報として取得する。 The acquisition unit 31 acquires values detected by the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor 13 as travel information.

推定部32は、加速度センサ12から取得した前後方向の加速度G及び車輪速センサ13から取得した車輪速度Uを用いて、ピッチ角の変化量Δθ、及びピッチ角θを推定する。 The estimating unit 32 uses the longitudinal acceleration G x obtained from the acceleration sensor 12 and the wheel speed U obtained from the wheel speed sensor 13 to estimate the pitch angle change amount Δθ A and the pitch angle θ A .

また、推定部32は、角速度センサ11から取得したヨーレートR、加速度センサ12から取得した左右方向の加速度G、車輪速センサ13から取得した車輪速度U、及び加速度センサから推定されたピッチ角θを用いて、ロール角φを推定する。 The estimation unit 32 also calculates the yaw rate R obtained from the angular velocity sensor 11, the lateral acceleration G y obtained from the acceleration sensor 12, the wheel speed U obtained from the wheel speed sensor 13, and the pitch angle θ estimated from the acceleration sensor. A is used to estimate the roll angle φ A.

また、推定部32は、角速度センサ11から取得したピッチレートQ、ヨーレートR、及び推定したロール角φを用いて、ピッチ角の変化量Δθを推定する。 The estimator 32 also uses the pitch rate Q and the yaw rate R obtained from the angular velocity sensor 11 and the estimated roll angle φ A to estimate the amount of change Δθ G in the pitch angle.

補正部33は、角速度センサ11から取得したヨーレートR、加速度センサ12から推定したピッチ角θ、ピッチ角の変化量Δθ、及び角速度センサ11から推定したピッチ角の変化量Δθを用いて、ゼロ点補正値ζを導出する。 The correction unit 33 uses the yaw rate R obtained from the angular velocity sensor 11, the pitch angle θ A estimated from the acceleration sensor 12, the pitch angle variation Δθ A , and the pitch angle variation Δθ G estimated from the angular velocity sensor 11. , to derive the zero-point correction value ζ y .

次に、図5を参照して、新たなゼロ点補正値を導出する場合のデータの入出力について説明する。図5は、本実施形態に係る新たなゼロ点補正値の導出の説明に供するセンサ誤差補正装置の入出力の一例を示すブロック図である。図5は、図4とは異なり、前回導出したゼロ点補正値を考慮して、新たなゼロ点補正値を導出することができる。図5が示すゼロ点補正値を導出する過程は、上述の図4と同様のため、割愛する。 Next, input/output of data when deriving a new zero-point correction value will be described with reference to FIG. FIG. 5 is a block diagram showing an example of inputs and outputs of a sensor error correction device for explaining derivation of a new zero point correction value according to this embodiment. In FIG. 5, unlike FIG. 4, a new zero point correction value can be derived in consideration of the previously derived zero point correction value. The process of deriving the zero point correction value shown in FIG. 5 is the same as in FIG. 4 described above, and is therefore omitted.

補正部33は、導出したゼロ点補正値ζkを用いて、加速度センサ12から取得した左右方向の加速度Gを補正する。また、推定部32は、補正した左右方向の加速度G+ζkを用いて、ロール角φを推定する。補正部33は、ロール角φを推定するに用いたゼロ点補正値ζkを考慮して、新たなゼロ点補正値ζk+1を導出する。 The correction unit 33 corrects the lateral acceleration G y acquired from the acceleration sensor 12 using the derived zero point correction value ζ y k. The estimation unit 32 also estimates the roll angle φ A using the corrected left-right acceleration G yy k. The correction unit 33 derives a new zero point correction value ζ y k+1 in consideration of the zero point correction value ζ y k used for estimating the roll angle φA .

次に、図6を参照して、近似したピッチ角を用いてゼロ点補正値を導出する場合のデータの入出力について説明する。図6は、本実施形態に係る近似したピッチ角を用いたゼロ点補正値の導出の説明に供するセンサ誤差補正装置の入出力の一例を示すブロック図である。図6は、図4及び図5とは異なり、ピッチ角を近似してゼロ点補正値を導出することができる。図6が示すピッチ角の変化量Δθ、ピッチ角θ、ピッチ角の変化量Δθ、ロール角φを推定する過程は、上述の図4と同様のため、割愛する。 Next, with reference to FIG. 6, input/output of data when deriving a zero-point correction value using an approximated pitch angle will be described. FIG. 6 is a block diagram showing an example of inputs and outputs of a sensor error correction device for explaining derivation of a zero point correction value using an approximated pitch angle according to this embodiment. In FIG. 6, unlike FIGS. 4 and 5, the pitch angle can be approximated to derive the zero point correction value. The process of estimating the pitch angle change amount Δθ G , pitch angle θ A , pitch angle change amount Δθ A , and roll angle φ A shown in FIG. 6 is omitted because it is the same as in FIG.

上述したようにピッチ角θが十分小さい場合、cosθは1に近似することができる。補正部33は、cosθは1に近似できることを考慮して、角速度センサ11から取得したヨーレートR、加速度センサから推定したピッチ角の変化量Δθ、及び角速度センサから推定したピッチ角の変化量Δθを用いて、ゼロ点補正値ζを導出する。 As described above, cos θ A can be approximated to 1 when the pitch angle θ A is sufficiently small. Considering that cos θ A can be approximated to 1, the correction unit 33 calculates the yaw rate R obtained from the angular velocity sensor 11, the amount of change in pitch angle Δθ A estimated from the acceleration sensor, and the amount of change in pitch angle estimated from the angular velocity sensor. Δθ G is used to derive the zero point correction value ζ y .

なお、図6では、加速度センサ12から推定したピッチ角θを近似して、ゼロ点補正値ζを導出する形態について説明した。しかし、これに限定されない。近似したピッチ角θを用いて導出したゼロ点補正値ζkを考慮して、上述の図5と同様に新たなゼロ点補正値ζk+1を導出してもよい。 In addition, in FIG. 6, the form which approximates the pitch angle (theta )A estimated from the acceleration sensor 12, and derives|leads-out the zero point correction value (zeta) y was demonstrated. However, it is not limited to this. A new zero point correction value ζ y k+1 may be derived in the same manner as in FIG .

なお、本実施形態では、第一のピッチ角の変化量、及び第二のピッチ角の変化量を推定して、ロール角の誤差、及び左右方向の加速度Gを導出する形態について説明した。しかし、これに限定されない。第一のロール角の変化量、及び第二のロール角の変化量を推定して、ピッチ角の誤差、及び前後方向の加速度Gの誤差を導出して補正を行ってもよい。 In this embodiment, the variation in the first pitch angle and the variation in the second pitch angle are estimated to derive the roll angle error and the lateral acceleration Gy . However, it is not limited to this. By estimating the amount of change in the first roll angle and the amount of change in the second roll angle, the error in the pitch angle and the error in the longitudinal acceleration Gx may be derived and corrected.

なお、本実施形態に係るロール角の誤差εφ、及びゼロ点補正値ζを導出する対象期間は、車両10が旋回している期間である。 Note that the target period for deriving the roll angle error εφ and the zero point correction value ζy according to the present embodiment is the period during which the vehicle 10 is turning.

次に図を参照して、ロール角の誤差εφ、及びゼロ点補正値ζを導出する対象期間について説明する。図は、本実施形態に係るヨーレートと時間の関係の一例を示すグラフである。 Next, with reference to FIG. 7 , the target period for deriving the roll angle error ε φ and the zero point correction value ζ y will be described. FIG. 7 is a graph showing an example of the relationship between the yaw rate and time according to this embodiment.

に示すように、センサ誤差補正装置20は、角速度センサ11によって検出されたヨーレートと、閾値を比較し、閾値を超えた場合、走行情報の記録を開始し、ヨーレートが閾値を下回るまで走行情報を記憶する。センサ誤差補正装置20は、記憶した走行情報を用いて、誤差の推定を行う。また、再度、旋回運動を始めた場合、センサ誤差補正装置20は、ヨーレートが閾値を超えた期間から閾値を下回るまで走行情報を記憶し、取得した走行情報と、前回導出したゼロ点補正値ζとを用いて、誤差の推定を行う。 As shown in FIG. 7 , the sensor error correction device 20 compares the yaw rate detected by the angular velocity sensor 11 with a threshold, and if the threshold is exceeded, starts recording travel information, and travels until the yaw rate falls below the threshold. Store information. The sensor error correction device 20 uses the stored travel information to estimate the error. Further, when the turning motion is started again, the sensor error correction device 20 stores the traveling information from the period when the yaw rate exceeds the threshold until it falls below the threshold. y is used to estimate the error.

次に、図を参照して、本実施形態に係るセンサ誤差補正プログラムの作用について説明する。まず、図は、本実施形態に係るセンサ誤差補正処理の一例を示すフローチャートである。CPU13がセンサ誤差補正プログラムを実行することによって、図6に示すセンサ誤差補正処理が実行される。図に示すセンサ誤差補正処理は、例えば、ユーザが車両10を起動した場合、センサ誤差補正装置20にセンサ誤差補正処理の実行指示が入力され、実行される。また、センサ誤差補正処理は、センサ11~13のよる検出の度、又は、所定の定期的なタイミングにより実行されてもよい。 Next, the operation of the sensor error correction program according to this embodiment will be described with reference to FIG . First, FIG. 8 is a flowchart showing an example of sensor error correction processing according to this embodiment. The sensor error correction process shown in FIG. 6 is executed by the CPU 13 executing the sensor error correction program. For example, when the user starts the vehicle 10, the sensor error correction process shown in FIG . Further, the sensor error correction process may be executed each time the sensors 11 to 13 detect, or at predetermined regular timing.

ステップS101で、取得部31は、角速度センサ11から取得したヨーレートと、閾値とを比較して、車両10が旋回運動を開始したか否かの判定を行う。ヨーレートが閾値を超えた場合(ステップS101:YES)、ステップS102に移行する。一方、ヨーレートが閾値以下である場合(ステップS101:NO)、ヨーレートが閾値を超えるまで待機する。 In step S101, the acquiring unit 31 compares the yaw rate acquired from the angular velocity sensor 11 with a threshold value, and determines whether or not the vehicle 10 has started turning motion. If the yaw rate exceeds the threshold (step S101: YES), the process proceeds to step S102. On the other hand, if the yaw rate is equal to or less than the threshold (step S101: NO), the process waits until the yaw rate exceeds the threshold.

ステップS102で、取得部31は、角速度センサ11、加速度センサ12、及び車輪速センサ13から走行情報を取得し、記憶する。 In step S102, the acquisition unit 31 acquires the travel information from the angular velocity sensor 11, the acceleration sensor 12, and the wheel speed sensor 13, and stores it.

ステップS103で、推定部32は、角速度センサ11が検出した走行情報を用いて、ピッチ角θを推定し、加速度センサ12、及び車輪速センサ13が検出した走行情報を用いて、ロール角φ、及びピッチ角θを推定する。 In step S103, the estimating unit 32 uses the traveling information detected by the angular velocity sensor 11 to estimate the pitch angle θG , and uses the traveling information detected by the acceleration sensor 12 and the wheel speed sensor 13 to estimate the roll angle φ Estimate A , and the pitch angle θ A .

ステップS104で、推定部32は、角速度センサ11から取得したヨーレートと、閾値とを比較して、車両10が旋回運動を終了したか否かの判定を行う。ヨーレートが閾値を下回った場合(ステップS103:YES)、ステップS105に移行する。一方、ヨーレートが閾値を超えた場合(ステップS103:NO)、ステップS102に移行して、走行情報を繰り返し取得する。 In step S104, the estimator 32 compares the yaw rate acquired from the angular velocity sensor 11 with a threshold to determine whether the vehicle 10 has finished turning. If the yaw rate is below the threshold (step S103: YES), the process proceeds to step S105. On the other hand, when the yaw rate exceeds the threshold (step S103: NO), the process proceeds to step S102 to repeatedly acquire travel information.

ステップS105で、推定部32は、推定したθを用いて、ピッチ角の変化量Δθを推定する。ステップS106で、推定部32は、推定したθを用いて、ピッチ角の変化量Δθを推定する。 In step S105, the estimator 32 estimates the pitch angle change amount Δθ G using the estimated θ G . In step S106, the estimator 32 estimates the pitch angle change amount Δθ A using the estimated θ A .

ステップS107で、補正部33は、推定部32で推定したピッチ角の変化量Δθ、ピッチ角の変化量Δθ、ピッチ角θ、及び取得部31で取得したヨーレートを用いて、ロール角の誤差εφ、及びゼロ点補正値ζを導出する。また、補正部33は、導出したロール角の誤差εφ、及びゼロ点補正値ζを用いて、角速度センサ11から検出される値から推定されるロール角φと、加速度センサ12から検出される左右方向の加速度Gを補正する。 In step S107, the correction unit 33 uses the pitch angle change amount Δθ G , the pitch angle change amount Δθ A and the pitch angle θ A estimated by the estimation unit 32, and the yaw rate acquired by the acquisition unit 31 to obtain the roll angle and the zero- point correction value ζ y are derived. Further, the correction unit 33 uses the derived roll angle error ε φ and the zero point correction value ζ y to estimate the roll angle φ A from the value detected by the angular velocity sensor 11 and the roll angle φ A detected from the acceleration sensor 12 . Corrects the lateral acceleration Gy .

ステップS108で、取得部31は、車両10の走行が終了したか否かの判定を行う。車両10が目的地に到着し、走行を終了した場合(ステップS108:YES)、センサ誤差補正処理を終了する。車両10の走行が終了していない場合(ステップS108:NO)、ステップS101に移行する。 In step S108, the acquisition unit 31 determines whether or not the vehicle 10 has finished traveling. When the vehicle 10 reaches the destination and finishes traveling (step S108: YES), the sensor error correction process is finished. If the vehicle 10 has not finished running (step S108: NO), the process proceeds to step S101.

以上説明したように、本実施形態によれば、角速度センサ11によって検出された値から推定したピッチ角の変化量と、加速度センサ12及び車輪速センサ13によって検出された値から推定したピッチ角の変化量とを用いて、ロール角の誤差、及びゼロ点補正値を導出することができる。従って、逆バンク又は遠心力等の影響で、旋回方向と逆方向に車体が傾く場合であっても、ロール角方向の車両の傾き及び左右方向の加速度を精度よく補正できる。 As described above, according to the present embodiment, the amount of change in the pitch angle estimated from the values detected by the angular velocity sensor 11 and the pitch angle estimated from the values detected by the acceleration sensor 12 and the wheel speed sensor 13 The amount of change can be used to derive the roll angle error and the zero point correction value. Therefore, even if the vehicle body tilts in the direction opposite to the turning direction due to the influence of reverse bank or centrifugal force, the tilt of the vehicle in the roll angle direction and the acceleration in the lateral direction can be accurately corrected.

その他、上記実施形態で説明したセンサ誤差補正装置の構成は、一例であり、主旨を逸脱しない範囲内において状況に応じて変更してもよい。 In addition, the configuration of the sensor error correction device described in the above embodiment is merely an example, and may be changed according to circumstances without departing from the gist of the invention.

また、上記実施形態で説明したプログラムの処理の流れも、一例であり、主旨を逸脱しない範囲内において不要なステップを削除したり、新たなステップを追加したり、処理順序を入れ替えたりしてもよい。 Further, the flow of processing of the program described in the above embodiment is also an example, and unnecessary steps may be deleted, new steps added, or the processing order changed without departing from the scope of the invention. good.

なお、上記実施形態でCPUがソフトウェア(プログラム)を読み込んで実行したセンサ誤差補正処理を、CPU以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、FPGA(Field Programmable Gate Array)等の製造後に回路構成を変更可能なPLD(Programmable Logic Device)、及びASIC(Application Specific Integrated Circuit)等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、各センサから走行情報を取得する処理を、これらの各種のプロセッサのうちの1つで実行してもよいし、同種又は異種の2つ以上のプロセッサの組み合わせ(例えば、複数のFPGA、及びCPUとFPGAとの組み合わせ等)で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。 Note that the sensor error correction processing executed by the CPU by reading the software (program) in the above embodiment may be executed by various processors other than the CPU. In this case, the processor is a PLD (Programmable Logic Device) whose circuit configuration can be changed after manufacturing, such as an FPGA (Field Programmable Gate Array), and an ASIC (Application Specific Integrated Circuit), which is dedicated to executing specific processing. An example is a dedicated electric circuit, which is a processor having a circuit configuration designed for In addition, the process of acquiring travel information from each sensor may be executed by one of these various processors, or a combination of two or more processors of the same or different types (for example, multiple FPGAs and It may be executed by a combination of a CPU and an FPGA, etc.). More specifically, the hardware structure of these various processors is an electric circuit in which circuit elements such as semiconductor elements are combined.

また、上記実施形態では、センサ誤差補正処理のプログラムがストレージ24に予め記憶(インストール)されている態様を説明したが、これに限定されない。プログラムは、CD-ROM(Compact Disk Read Only Memory)、DVD-ROM(Digital Versatile Disk Read Only Memory)、及びUSB(Universal Serial Bus)メモリ等の記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。 Further, in the above-described embodiment, the program for the sensor error correction processing has been pre-stored (installed) in the storage 24, but the present invention is not limited to this. The program may be provided in a form recorded on a recording medium such as a CD-ROM (Compact Disk Read Only Memory), a DVD-ROM (Digital Versatile Disk Read Only Memory), and a USB (Universal Serial Bus) memory. Also, the program may be downloaded from an external device via a network.

10 車両
11 角速度センサ
12 加速度センサ
13 車輪速センサ
20 センサ誤差補正装置
21 CPU
22 ROM
23 RAM
24 ストレージ
25 表示部
26 入力部
27 バス
31 取得部
32 推定部
33 補正部
10 vehicle 11 angular velocity sensor 12 acceleration sensor 13 wheel speed sensor 20 sensor error correction device 21 CPU
22 ROMs
23 RAM
24 storage 25 display unit 26 input unit 27 bus 31 acquisition unit 32 estimation unit 33 correction unit

Claims (5)

移動体に搭載された、移動体の回転運動を検出する角速度センサ、移動体の並進運動を検出する加速度センサ、及び移動体の車輪の速度を検出する車輪速センサの各々から検出した値を含む走行情報を取得する取得部と、
前記走行情報の前記角速度センサから検出した値を用いて、前記移動体のピッチ角方向の回転量を示す第一の角度変化量を推定し、前記走行情報の前記加速度センサ及び前記車輪速センサから検出した値を用いて、前記移動体のピッチ角方向の回転量を示す第二の角度変化量を推定する推定部と、
前記第一の角度変化量及び前記第二の角度変化量の差分から前記走行情報の誤差を導出し、前記誤差に基づいて、前記取得部で取得した前記走行情報を補正する補正部と、
を備え
前記走行情報の誤差は、前記移動体の車幅方向に沿う左右方向の加速度センサの誤差であり、
前記補正部は、前記走行情報の前記左右方向の加速度センサの値を補正する、
センサ誤差補正装置。
Includes values detected from each of an angular velocity sensor mounted on the mobile body that detects the rotational motion of the mobile body, an acceleration sensor that detects the translational motion of the mobile body, and a wheel speed sensor that detects the speed of the wheels of the mobile body an acquisition unit that acquires travel information;
Using the value detected by the angular velocity sensor of the travel information, a first angle change amount indicating the amount of rotation in the pitch angle direction of the moving body is estimated, and the acceleration sensor and the wheel speed sensor of the travel information are used to estimate an estimating unit that uses the detected value to estimate a second angle change amount that indicates the amount of rotation of the moving body in the pitch angle direction;
a correction unit that derives an error in the travel information from the difference between the first angle change amount and the second angle change amount, and corrects the travel information acquired by the acquisition unit based on the error;
with
The error in the travel information is an error in an acceleration sensor in the left-right direction along the vehicle width direction of the moving object,
The correction unit corrects the value of the acceleration sensor in the horizontal direction of the travel information.
Sensor error correction device.
前記走行情報は、前記移動体が旋回を始めてから終了するまでの期間に前記取得部によって取得される、
請求項1記載のセンサ誤差補正装置。
The travel information is acquired by the acquisition unit during a period from when the moving object starts to when it finishes turning.
2. The sensor error correction device according to claim 1.
前記補正部は、前記導出した前記走行情報の誤差と、前回に前記補正部によって導出された前記走行情報の誤差とを用いて、前記走行情報をさらに補正する
請求項1又は請求項2記載のセンサ誤差補正装置。
3. The correcting unit further corrects the traveling information by using the error of the derived traveling information and the error of the traveling information previously derived by the correcting unit. Sensor error correction device.
前記推定部は、前記走行情報からロール角をさらに推定し、
前記補正部は、前記ロール角の誤差をさらに導出し、前記ロール角を補正する、
請求項1から請求項の何れか1項に記載のセンサ誤差補正装置。
The estimation unit further estimates a roll angle from the travel information,
The correction unit further derives an error of the roll angle and corrects the roll angle.
The sensor error correction device according to any one of claims 1 to 3 .
移動体に搭載された、移動体の回転運動を検出する角速度センサ、移動体の並進運動を検出する加速度センサ、及び移動体の車輪の速度を検出する車輪速センサの各々から検出した値を含む走行情報を取得するステップと、
前記走行情報の前記角速度センサから検出した値を用いて、前記移動体のピッチ角方向の回転量を示す第一の角度変化量を推定し、前記走行情報の前記加速度センサ及び前記車輪速センサから検出した値を用いて、前記移動体のピッチ角方向の回転量を示す第二の角度変化量を推定するステップと、
前記第一の角度変化量及び前記第二の角度変化量の差分から前記走行情報の誤差を導出し、前記誤差に基づいて、取得した前記走行情報を補正するステップと、
含み、
前記走行情報の誤差は、前記移動体の車幅方向に沿う左右方向の加速度センサの誤差であり、
前記補正するステップは、前記走行情報の前記左右方向の加速度センサの値を補正する、
処理をコンピュータに実行させるセンサ誤差補正プログラム。
Includes values detected from each of an angular velocity sensor mounted on the mobile body that detects the rotational motion of the mobile body, an acceleration sensor that detects the translational motion of the mobile body, and a wheel speed sensor that detects the speed of the wheels of the mobile body a step of obtaining travel information;
Using the value detected by the angular velocity sensor of the travel information, a first angle change amount indicating the amount of rotation in the pitch angle direction of the moving body is estimated, and the acceleration sensor and the wheel speed sensor of the travel information are used to estimate estimating a second angle change amount indicating the amount of rotation of the moving body in the pitch angle direction using the detected value;
a step of deriving an error of the travel information from the difference between the first angle change amount and the second angle change amount, and correcting the acquired travel information based on the error;
including
The error in the travel information is an error in an acceleration sensor in the left-right direction along the vehicle width direction of the moving object,
The correcting step corrects the value of the acceleration sensor in the horizontal direction of the travel information.
A sensor error correction program that causes a computer to execute processing .
JP2019043173A 2019-03-08 2019-03-08 Sensor error correction device and sensor error correction program Active JP7238490B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2019043173A JP7238490B2 (en) 2019-03-08 2019-03-08 Sensor error correction device and sensor error correction program

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2019043173A JP7238490B2 (en) 2019-03-08 2019-03-08 Sensor error correction device and sensor error correction program

Publications (2)

Publication Number Publication Date
JP2020144093A JP2020144093A (en) 2020-09-10
JP7238490B2 true JP7238490B2 (en) 2023-03-14

Family

ID=72354051

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2019043173A Active JP7238490B2 (en) 2019-03-08 2019-03-08 Sensor error correction device and sensor error correction program

Country Status (1)

Country Link
JP (1) JP7238490B2 (en)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150094980A1 (en) 2012-04-04 2015-04-02 Jc Inertial Oy Vehicle positioning
JP2017102030A (en) 2015-12-02 2017-06-08 株式会社Jvcケンウッド Pitch angular velocity correction value calculation device, pose angle calculation device, and pitch angular velocity correction value calculation method
JP2018112520A (en) 2017-01-13 2018-07-19 トヨタ自動車株式会社 Sensor error correction device and method

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150094980A1 (en) 2012-04-04 2015-04-02 Jc Inertial Oy Vehicle positioning
JP2017102030A (en) 2015-12-02 2017-06-08 株式会社Jvcケンウッド Pitch angular velocity correction value calculation device, pose angle calculation device, and pitch angular velocity correction value calculation method
JP2018112520A (en) 2017-01-13 2018-07-19 トヨタ自動車株式会社 Sensor error correction device and method

Also Published As

Publication number Publication date
JP2020144093A (en) 2020-09-10

Similar Documents

Publication Publication Date Title
US11167816B2 (en) Control of a two-wheeled self-balancing vehicle
KR102440358B1 (en) Inertial-based navigation device and Inertia-based navigation method based on relative preintegration
CN103270543B (en) Driving assist device
US8200452B2 (en) Attitude-angle detecting apparatus and attitude-angle detecting method
JP7036080B2 (en) Inertial navigation system
EP1760431B1 (en) Inertial navigation system with a plurality of Kalman filters and vehicle equipped with such a system
US20160299234A1 (en) Fail operational vehicle speed estimation through data fusion of 6-dof imu, gps, and radar
US9321498B2 (en) Method of estimating mounting angle error of gyroscopes by using a turning device, and corresponding turning device
US10703365B1 (en) Lane tracking method and lane tracking system for an autonomous vehicle
US10429207B2 (en) Pitch angular velocity correction value calculation device, attitude angle calculation device, and method for calculating pitch angular velocity correction value
JP6383907B2 (en) Vehicle position measuring apparatus and method
EP3183394A1 (en) Earthmoving machine comprising weighted state estimator
JP5402244B2 (en) Vehicle physical quantity estimation device
KR101160630B1 (en) Method for motion mode decision, navigation system using the method, and providing method thereof
JP7238490B2 (en) Sensor error correction device and sensor error correction program
US20210055738A1 (en) Self-propelled robot
JP2015004593A (en) Navigation device
JP5352922B2 (en) Angular velocity calculation device, navigation device
US20090062980A1 (en) Vehicular posture estimation device and vehicular posture estimation method
JP7360976B2 (en) Sensor error correction device
JP3381520B2 (en) Navigation device
US11796318B2 (en) Rotation measurement system using Coriolis and Euler forces
JP2012042285A (en) Yaw angle initialization abnormality determination device and method
US11780424B2 (en) Vehicle control device and vehicle control method
JP6981459B2 (en) Sensor error correction device

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20211228

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20220921

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20220927

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20221031

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20230131

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20230213

R150 Certificate of patent or registration of utility model

Ref document number: 7238490

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R150