JP6704488B1

JP6704488B1 - Coordinate data correction device, moving body tracking device and moving body tracking method using the same

Info

Publication number: JP6704488B1
Application number: JP2019070368A
Authority: JP
Inventors: 哲治 ▲濱▼田; 吉川　篤志; 篤志吉川; 堀　保義; 保義堀; 昭暢杉山
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2019-04-02
Filing date: 2019-04-02
Publication date: 2020-06-03
Anticipated expiration: 2039-04-02
Also published as: JP2020168903A

Abstract

【課題】移動体の動きに対し、正確な追従を行うことができる座標データ補正装置を得る。【解決手段】座標データ入力部３に自車を基準にした移動体の相対位置が入力され、フィルタ処理部４により、平滑化処理される一方で、自車状態量検出部３により車速およびヨーレートを含む自車状態量が検出され、自車移動量演算部１４により、自車の移動量を演算して、これを基にして、自車座標系移動体履歴記憶部１５が、移動体の相対位置を、自車基準座標系に変換して、記憶し、次いで、自車座標系移動体履歴曲率演算部１６により、移動体履歴の曲率を演算して、この曲率に基づき、自車座標系移動体履歴補正部１７により、移動体履歴を、平滑化処理される前の曲率に近づくように補正して、自車の操舵を制御する制御部７へ出力する。【選択図】図１PROBLEM TO BE SOLVED: To obtain a coordinate data correction device capable of accurately following a movement of a moving body. SOLUTION: A relative position of a moving body with respect to the own vehicle is input to a coordinate data input section 3 and smoothed by a filter processing section 4, while a vehicle speed and a yaw rate are detected by an own vehicle state amount detection section 3. A vehicle state amount including the vehicle is detected, the vehicle movement amount calculation unit 14 calculates the movement amount of the vehicle, and based on this, the vehicle coordinate system moving body history storage unit 15 displays The relative position is converted into the own vehicle reference coordinate system and stored, then the moving body history curvature calculation unit 16 calculates the curvature of the moving body history, and the own vehicle coordinates are calculated based on this curvature. The system moving body history correction unit 17 corrects the moving body history so as to approach the curvature before the smoothing processing, and outputs the corrected moving body history to the control unit 7 that controls the steering of the own vehicle. [Selection diagram] Figure 1

Description

本願は、移動体の追従に用いられる座標データ補正装置およびこれを用いた移動体追従装置並びに移動体追従方法に関するものである。 The present application relates to a coordinate data correction device used for tracking a moving body, a moving body tracking device using the same, and a moving body tracking method.

従来の移動体追従装置は、例えば、自動車等の車両の運転支援制御技術に属する。従来の移動体追従装置では、車両に搭載されたレーダーセンサ、ビデオカメラ、ＧＰＳ（ＧｌｏｂａｌＰｏｓｉｔｉｏｎｉｎｇＳｙｓｔｅｍ）装置などが検出する先行車（車両の前方を走行する車両）の位置、速度または走行軌跡に基づいて、先行車に追従して車両の走行を制御する（先行車追従制御）。
従来の移動体追従装置としては、例えば、特許文献１がある。 A conventional moving body tracking device belongs to, for example, a driving support control technique for a vehicle such as an automobile. In a conventional moving body tracking device, based on the position, speed or traveling locus of a preceding vehicle (vehicle traveling in front of the vehicle) detected by a radar sensor, a video camera, a GPS (Global Positioning System) device, etc. mounted on the vehicle. Then, the traveling of the vehicle is controlled by following the preceding vehicle (preceding vehicle following control).
As a conventional moving body tracking device, there is, for example, Patent Document 1.

特許文献１に記載された従来の移動体追従装置では、センサによる検出信号に基づき自車両前方の追従対象の自車両に対する相対位置を対象位置として算出するとともに、算出した対象位置を時間軸上で複数回取得し、取得した対象位置をバッファメモリにバッファリングする。そして、バッファリングした複数の対象位置に基づき、追従対象の移動軌跡を推定する。
この移動軌跡の推定で、現在の自車両位置よりも後方に位置していた追従対象について算出された対象位置が、バッファリングされている状態で、移動軌跡が推定されるように、自車両と追従対象との離間距離と自車速とに基づいて、対象位置の取得周期を調整し、移動体の履歴を保存するようにしている。 In the conventional moving body tracking device described in Patent Document 1, the relative position of the tracking target ahead of the own vehicle with respect to the own vehicle is calculated as the target position based on the detection signal from the sensor, and the calculated target position on the time axis. Acquire multiple times and buffer the acquired target position in the buffer memory. Then, the movement trajectory of the tracking target is estimated based on the plurality of buffered target positions.
In this estimation of the movement locus, the movement position is estimated so that the movement locus is estimated while the target position calculated for the tracking target located behind the current own vehicle position is buffered. The acquisition cycle of the target position is adjusted based on the distance from the tracking target and the vehicle speed, and the history of the moving body is saved.

特開２０１７―６５４７３号公報（第４〜７頁、第１図）JP-A-2017-65473 (pages 4 to 7, FIG. 1)

特許文献１に記載された従来の移動体追従装置では、移動体の履歴を等間隔で取得するように取得周期を調整することのみが記載されている。
このため、取得された移動体履歴情報に対する一般的な平滑化処理（ローパスフィルタまたは移動平均処理）を行った履歴に基づいて、移動体に追従制御しようとする場合、カーブ走行時は、移動体が通過した履歴と異なるものとなってしまい、移動体が実際に通過した経路を通れない場面が出てくるという問題があった。 The conventional moving body tracking device described in Patent Document 1 only describes adjusting the acquisition period so as to acquire the history of the moving body at equal intervals.
Therefore, when trying to control the tracking of the moving body based on the history of general smoothing processing (low-pass filter or moving average processing) performed on the acquired moving body history information, when the moving body travels on a curve, However, there is a problem that the moving object becomes different from the history of passing, and there are cases where the moving body cannot pass the route actually passed.

本願は、上記のような課題を解決するための技術を開示するものであり、移動体の動きに対し、正確な追従を行うことができる座標データ補正装置およびこれを用いた移動体追従装置並びに移動体追従方法を提供することを目的とする。 The present application discloses a technique for solving the above problems, and a coordinate data correction device capable of accurately following a movement of a mobile body, a mobile body tracking device using the coordinate data correction device, and An object is to provide a moving body tracking method.

本願に開示される座標データ補正装置は、自車に搭載された座標データ補正装置であって、ある点を基点とする座標系における、追従対象の移動体の座標データが入力される座標データ入力部、この座標データ入力部により入力された座標データを平滑化処理するフィルタ処理部、このフィルタ処理部により平滑化処理された座標データを、移動体の履歴として３点以憶する座標データ記憶部、この座標データ記憶部に記憶された平滑化処理された３点以上の座標データから移動体の履歴の曲率を演算し、フィルタ処理部におけるフィルタ処理仕様から求まる曲率の補正量を用いて、実際の移動体の履歴の曲率に近づけるように、移動体の履歴を補正する補正部を備えたものである。

A coordinate data correction device disclosed in the present application is a coordinate data correction device mounted on a vehicle, and is a coordinate data input device for inputting coordinate data of a moving object to be followed in a coordinate system having a certain point as a base point. Section, a filter processing section that smoothes the coordinate data input by the coordinate data input section, and a coordinate data storage section that stores the coordinate data smoothed by the filter processing section as three or more points of history of the moving body. , The curvature of the history of the moving body is calculated from the smoothed coordinate data of three or more points stored in the coordinate data storage unit, and the curvature correction amount obtained from the filter processing specification in the filter processing unit is used to actually The correction unit for correcting the history of the moving body is provided so as to approach the curvature of the history of the moving body.

本願に開示される座標データ補正装置によれば、移動体の動きに対し、正確な追従を行うことができる。 According to the coordinate data correction device disclosed in the present application, it is possible to accurately follow the movement of the moving body.

実施の形態１による座標データ補正装置の概略構成を示すブロック図である。1 is a block diagram showing a schematic configuration of a coordinate data correction device according to a first embodiment. 実施の形態１による座標データ補正装置の動作を示すフローチャートである。4 is a flowchart showing the operation of the coordinate data correction device in the first embodiment. 実施の形態１による座標データ補正装置の自車移動量の演算処理を示すフローチャートである。3 is a flowchart showing a process of calculating a moving amount of a vehicle of the coordinate data correction device according to the first embodiment. 実施の形態１による座標データ補正装置の移動体の過去履歴の自車座標系への変換処理を示すフローチャートである。5 is a flowchart showing a process of converting a past history of a moving body into a vehicle coordinate system of the coordinate data correction device according to the first embodiment. 実施の形態１による座標データ補正装置の移動体履歴の生成過程を示す図である。FIG. 6 is a diagram showing a process of generating a moving object history of the coordinate data correction device according to the first embodiment. 実施の形態１による座標データ補正装置のフィルタ処理の効果を示す説明図である。FIG. 6 is an explanatory diagram showing an effect of filter processing of the coordinate data correction device according to the first embodiment. 実施の形態１による座標データ補正装置の移動体履歴の補正の効果を示す説明図である。FIG. 6 is an explanatory diagram showing an effect of correction of a moving object history of the coordinate data correction device according to the first embodiment. 実施の形態１による座標データ補正装置のハードウェア構成を示す図である。1 is a diagram showing a hardware configuration of a coordinate data correction device according to a first embodiment. 実施の形態２による座標データ補正装置の概略構成を示すブロック図である。7 is a block diagram showing a schematic configuration of a coordinate data correction device according to a second embodiment. FIG. 実施の形態２による座標データ補正装置の動作を示すフローチャートである。7 is a flowchart showing the operation of the coordinate data correction device according to the second embodiment.

実施の形態１．
以下、実施の形態１による座標データ補正装置について、図を用いて説明する。
図１は、実施の形態１による座標データ補正装置の概略構成を示すブロック図である。
図１において、座標データ補正装置１は、それぞれ後述する自車状態量検出部２と、座標データ入力部３と、フィルタ処理部４と、座標データ記憶部５と、補正部６とを有し、補正部６で補正された追従対象の移動体の座標データに基づく制御量を制御部７に出力する。
なお、以下の説明において、「自車」とは、座標データ補正装置を搭載した車両を指している。 Embodiment 1.
Hereinafter, the coordinate data correction device according to the first embodiment will be described with reference to the drawings.
FIG. 1 is a block diagram showing a schematic configuration of a coordinate data correction device according to the first embodiment.
In FIG. 1, the coordinate data correction device 1 includes a vehicle state quantity detection unit 2, a coordinate data input unit 3, a filter processing unit 4, a coordinate data storage unit 5, and a correction unit 6, which will be described later. The control amount based on the coordinate data of the tracking target moving body corrected by the correction unit 6 is output to the control unit 7.
In the following description, “own vehicle” refers to a vehicle equipped with the coordinate data correction device.

自車状態量検出部２は、自車の走行状態を示す各種の自車状態量を取得し、取得した自車状態量を座標データ記憶部５に伝送する。実施の形態１では、自車状態量検出部２は、後述する自車速検出部１１およびヨーレート検出部１２を有する。
自車速検出部１１は、自車の走行速度を検出し、検出した走行速度に応じた信号を座標データ記憶部５へ出力する。
ヨーレート検出部１２は、自車の旋回方向への回転角の変化速度であるヨーレートを検出し、検出したヨーレートに応じた信号を座標データ記憶部５へ出力する。
なお、座標データ記憶部５に入力される自車状態量は、自車の走行速度、ヨーレートがあれば、他の信号があってもかまわない。
また、本実施の形態１においては、座標データ記憶部５にて制御周期間で得られたヨーレートをヨー角変化量に演算するため、ヨーレートの変わりに、ＧＰＳなど、自車のヨー角変化量が取得できるものであればよい。 The vehicle state quantity detection unit 2 acquires various vehicle state quantities indicating the traveling state of the vehicle, and transmits the acquired vehicle state quantity to the coordinate data storage unit 5. In the first embodiment, the vehicle state quantity detection unit 2 has a vehicle speed detection unit 11 and a yaw rate detection unit 12, which will be described later.
The own vehicle speed detection unit 11 detects the traveling speed of the own vehicle and outputs a signal corresponding to the detected traveling speed to the coordinate data storage unit 5.
The yaw rate detection unit 12 detects the yaw rate that is the change speed of the rotation angle of the vehicle in the turning direction, and outputs a signal corresponding to the detected yaw rate to the coordinate data storage unit 5.
Note that the vehicle state quantity input to the coordinate data storage unit 5 may include other signals as long as the vehicle traveling speed and yaw rate are available.
Further, in the first embodiment, the yaw rate obtained during the control period in the coordinate data storage unit 5 is calculated as the yaw angle change amount. Therefore, instead of the yaw rate, the yaw angle change amount of the vehicle such as GPS is changed. Can be obtained.

座標データ入力部３は、移動体相対位置検出部１３を有し、この移動体相対位置検出部１３で検出された信号をフィルタ処理部４へ出力する機能を有している。
移動体相対位置検出部１３は、自車を基準とした場合の移動体の相対縦位置、相対横位置を検出し、その位置に応じた信号をフィルタ処理部４へ出力する。なお、フィルタ処理部４を必ず通過する必要はないので、直接、座標データ記憶部５に出力するようにしてもよい。 The coordinate data input unit 3 has a moving body relative position detection unit 13, and has a function of outputting a signal detected by the moving body relative position detection unit 13 to the filter processing unit 4.
The moving body relative position detection unit 13 detects a relative vertical position and a relative horizontal position of the moving body with respect to the own vehicle, and outputs a signal corresponding to the position to the filter processing unit 4. Since it does not necessarily pass through the filter processing unit 4, it may be directly output to the coordinate data storage unit 5.

移動体相対位置検出部１３は、例えば、自車に搭載されたカメラにより撮影された自車前方の画像から、自車を基準とした、対象とする移動体の相対位置を検出する。カメラの他にも、ミリ波レーダあるいはレーザレーダ等、自車から見た移動体の相対位置が取得できるものでもよいし、ＧＰＳなどを自車基準座標系に変換して相対位置として取得するものでもよい。
移動体相対位置検出部１３により検出された移動体の相対位置は、自車の現在位置を基準とする固定座標系（自車固定座標系）で表されるものとする。
ここで、自車の進行方向をＸ方向（前方を正、後方を負とする）、横方向をＹ方向（右方を正、左方を負とする）と定義する。また、ヨーレートなど、自車の旋回の向きは、時計回りを正、反時計回りを負と定義する。なお、自車固定座標系では、自車座標は常に原点（０，０）となる。 The moving body relative position detection unit 13 detects, for example, the relative position of the target moving body based on the own vehicle from an image in front of the own vehicle taken by a camera mounted on the own vehicle. In addition to the camera, a millimeter-wave radar, a laser radar, or the like that can acquire the relative position of the moving body as viewed from the own vehicle, or convert GPS or the like into the own-vehicle reference coordinate system and acquire the relative position But it's okay.
The relative position of the moving body detected by the moving body relative position detection unit 13 is represented by a fixed coordinate system (own vehicle fixed coordinate system) based on the current position of the own vehicle.
Here, the traveling direction of the host vehicle is defined as the X direction (the front is positive, the rear is negative), and the lateral direction is the Y direction (right is positive and left is negative). In addition, the turning direction of the vehicle such as the yaw rate is defined as positive in the clockwise direction and negative in the counterclockwise direction. In the fixed vehicle coordinate system, the vehicle coordinate is always the origin (0,0).

フィルタ処理部４は、座標データ入力部３から出力される信号に対し、フィルタ処理（平滑化処理）を行なう。フィルタ処理部４は、各種フィルタ処理を行う機能を持ち、座標データ記憶部５に対して、フィルタ処理した信号を出力する機能を持つ。
フィルタ機能は、必要に応じてバイパス可能であるため、フィルタ処理後の信号およびフィルタ未処理の信号を同時に出力することができる。
具体的なフィルタ機能として、一般的に用いられるハイパス・ローパスフィルタ、移動平均などのフィルタなどを使用することができる。本実施の形態１では、使用するフィルタの仕様は、既知である必要がある。 The filter processing unit 4 performs filter processing (smoothing processing) on the signal output from the coordinate data input unit 3. The filter processing unit 4 has a function of performing various filter processes, and has a function of outputting a filtered signal to the coordinate data storage unit 5.
Since the filter function can be bypassed as necessary, the filtered signal and the unfiltered signal can be output at the same time.
As a specific filter function, a commonly used high-pass/low-pass filter, a moving average filter, or the like can be used. In the first embodiment, the specifications of the filter used need to be known.

座標データ記憶部５は、後述する自車移動量演算部１４、自車座標系移動体履歴記憶部１５を有し、自車基準座標系の移動体履歴情報を補正部６に出力する機能を有している。座標データ記憶部５には、３点以上の自車基準座標系（自車を基準とする座標系）の移動体の座標データが、移動体履歴情報として記憶されている。 The coordinate data storage unit 5 has an own vehicle movement amount calculation unit 14 and an own vehicle coordinate system moving body history storage unit 15, which will be described later, and has a function of outputting moving body history information of the own vehicle reference coordinate system to the correction unit 6. Have The coordinate data storage unit 5 stores coordinate data of a moving body in three or more points of the own vehicle reference coordinate system (coordinate system with the own vehicle as a reference) as moving body history information.

自車移動量演算部１４は、前回演算周期から今回演算周期までの時間（以後、「演算周期」という。実施の形態１では、１００ｍｓｅｃ）における自車移動量を演算する。
自車座標系移動体履歴記憶部１５は、自車移動量演算部１４で演算された自車基準座標系での所定の範囲の移動体履歴データを保持し、次回制御周期（今回の例では、１００ｍｓｅｃ後）に引き渡す機能を持つ。自車座標系移動体履歴記憶部１５は、移動体履歴データ（座標データ）を、自車基準座標系での移動体履歴データに変換して保持する。 The own vehicle movement amount calculation unit 14 calculates the own vehicle movement amount in the time from the previous calculation cycle to the current calculation cycle (hereinafter referred to as “calculation cycle”. In the first embodiment, 100 msec).
The own vehicle coordinate system moving body history storage unit 15 holds moving body history data in a predetermined range in the own vehicle reference coordinate system calculated by the own vehicle movement amount calculation unit 14, and stores the moving body history data in the next control cycle (in this example, , 100 msec later). The own vehicle coordinate system moving body history storage unit 15 converts the moving body history data (coordinate data) into moving body history data in the own vehicle reference coordinate system and holds it.

補正部６は、後述する自車座標系移動体履歴曲率演算部１６および自車座標系移動体履歴補正部１７を有し、補正処理された移動体履歴データを制御部７に出力する機能を有している。
自車座標系移動体履歴曲率演算部１６は、移動体履歴データがどの程度の曲率になっているかを演算する機能を持つ。 The correction unit 6 has a vehicle-coordinate system moving body history curvature calculation unit 16 and an own-vehicle coordinate system moving body history correction unit 17, which will be described later, and has a function of outputting the corrected moving body history data to the control unit 7. Have
The moving body history curvature calculation unit 16 of the own vehicle coordinate system has a function of calculating how much curvature the moving body history data has.

自車座標系移動体履歴補正部１７は、フィルタ処理部４の処理が実行され、自車座標系移動体履歴記憶部１５で保持されている移動体履歴を補正する機能を持つ。また、バイパス機能も合わせて有しており、移動体履歴の曲率が閾値未満の場合、補正を実施しなくてもよい。 The own vehicle coordinate system moving body history correction unit 17 has a function of performing the processing of the filter processing unit 4 and correcting the moving body history held in the own vehicle coordinate system moving body history storage unit 15. Further, it also has a bypass function, and if the curvature of the history of the moving body is less than the threshold value, it is not necessary to perform the correction.

制御部７は、補正部６で演算された移動体履歴に基づく目標値を演算し、例えば操舵用の動力を発生させ、自車を目標値に追従するように操舵制御を行う。
操舵制御の手法として、例えば、電動式パワーステアリングの電動モータ（種類は特に限定されず、直流、交流を問わない）、または油圧式パワーステアリングの油圧ポンプを用いることができる。
本実施の形態１では、制御部７として、電動パワーステアリング装置をベースとしているが、操舵を制御できるものであればよく、例えば、ハンドルと転舵輪との間の機械的リンクを廃したステアリングバイワイヤと称される操舵装置であってもよい。 The control unit 7 calculates a target value based on the moving object history calculated by the correction unit 6, generates steering power, for example, and performs steering control so that the own vehicle follows the target value.
As a method of steering control, for example, an electric motor of an electric power steering (the type is not particularly limited, DC or AC is acceptable), or a hydraulic pump of a hydraulic power steering can be used.
In the first embodiment, the control unit 7 is based on an electric power steering device, but any device that can control steering can be used, for example, steering-by-wire that eliminates the mechanical link between the steering wheel and the steered wheels. It may be a steering device called.

図５は、実施の形態１による座標データ補正装置の移動体履歴の生成過程を示す図であり、自車固定座標系から見た、移動体の相対位置を記憶しながら、演算周期毎に自車基準座標系へ変換する様子を示している。順次、アフィン変換（座標軸の回転＋移動）を行いながら相対位置を記憶する。
図５において、ｘ_１，ｙ_１軸〜ｘ_ｎ，ｙ_ｎ軸は、演算周期毎の自車基準座標系を示している。記号○（ｘ’_１，ｙ’_１）〜（ｘ’_ｎ，ｙ’_ｎ）は、演算周期毎の自車基準座標系から見た移動体の相対位置を示している。
例えば、ｘ_ｎ，ｙ_ｎ軸における演算周期で検出された移動体相対位置は（ｘ’_ｎ，ｙ’_ｎ）となり、ｘ_ｎ−１，ｙ_ｎ−１軸における演算周期で検出された移動体相対位置は（ｘ’_ｎ−１，ｙ’_ｎ−１）となる。 FIG. 5 is a diagram showing a process of generating a moving body history of the coordinate data correction apparatus according to the first embodiment, which stores the relative position of the moving body as seen from the fixed coordinate system of the own vehicle and stores the moving body history every calculation cycle. It shows a state of conversion to the vehicle reference coordinate system. The relative position is stored while sequentially performing affine transformation (rotation + movement of coordinate axes).
In FIG. 5, the x ₁ and y ₁ axes to the x _n and y _n axes represent the own vehicle reference coordinate system for each calculation cycle. The symbols ◯ (x′ ₁ , y′ ₁ ) to (x′ _n , y′ _n ) indicate relative positions of the moving body viewed from the own vehicle reference coordinate system for each calculation cycle.
For example, the relative position of the moving body detected in the calculation cycle in the x _n and y _n axes is (x′ _n , y′ _n ), and the moving body detected in the calculation cycle in the x _n−1 and y _n−1 axes. The relative position is (x'n _-1 , y'n _-1 ).

ｘ_ｎ，ｙ_ｎ軸をｘ_ｎ−１，ｙ_ｎ−１軸に変換する際、後述する式（１）の座標変換を行うことで、ｘ_ｎ，ｙ_ｎ軸で検出された移動体履歴についても、ｘ_ｎ−１，ｙ_ｎ−１軸から見た場合の相対位置として記憶されることとなる。 When converting the x _n , y _n axes to the x _n-1 , y _n-1 axes, the moving object history detected on the x _n , y _n axes is obtained by performing coordinate conversion of Expression (1) described later. Is also stored as a relative position when viewed from the x _n-1 and y _n-1 axes.

この手順を繰り返すことで、最終的には今回演算周期の自車固定座標系であるｘ_１，ｙ_１軸から見た移動体の過去履歴（ｘ’_１，ｙ’_１）〜（ｘ’_ｎ，ｙ’_ｎ）として記憶することができる。メモリは有限であるため、記憶する履歴数も有限とし、その値を超えた場合は古い値を削除し、新しい値を入れ込むＦＩＦＯ（Ｆｉｒｓｔ−ＩｎＦｉｒｓｔ−Ｏｕｔ）などの考え方を採用すればよい。 By repeating this procedure, the past history (x′ ₁ , y′ ₁ ) to (x′ _{n of} the moving body as seen from the x ₁ and y ₁ axes, which is the fixed coordinate system of the vehicle in the current calculation cycle, is finally obtained. , Y′ _n ) can be stored. Since the memory is finite, the number of history records to be stored is finite, and when the value exceeds that value, an old value is deleted and a new value is inserted, and a concept such as FIFO (First-In First-Out) may be adopted. ..

図６は、実施の形態１による座標データ補正装置のフィルタ処理の効果を示す説明図である。
図６において、自車３０と、先行車である移動体４０が示されている。
図中の記号〇は、フィルタ処理無しの移動体履歴の各座標データを示し、移動体履歴は、センサ取得値の曲線半径Ｒ１を有する。記号□は、フィルタ処理にローパスフィルタを用いた場合の移動体履歴の各座標データを示し、移動体履歴は、ローパスフィルタ処理後の曲線半径Ｒ２を有する。記号△は、フィルタ処理に移動平均を用いた場合の移動体履歴の各座標データを示し、移動体履歴は、移動平均処理後の曲線半径Ｒ３を有する。 FIG. 6 is an explanatory diagram showing the effect of the filter processing of the coordinate data correction device according to the first embodiment.
In FIG. 6, the own vehicle 30 and the moving body 40 that is the preceding vehicle are shown.
The symbol ◯ in the drawing indicates each coordinate data of the moving body history without filter processing, and the moving body history has the curve radius R1 of the sensor acquisition value. The symbol □ indicates each coordinate data of the moving body history when the low-pass filter is used for the filter processing, and the moving body history has the curve radius R2 after the low-pass filter processing. The symbol Δ indicates each coordinate data of the moving body history when the moving average is used for the filter processing, and the moving body history has the curve radius R3 after the moving average processing.

図７は、実施の形態１による座標データ補正装置の移動体履歴の補正の効果を示す説明図である。
図７において、符号３０および記号〇、△並びにＲ１、Ｒ３は図６におけるものと同一のものである。図７中、補正量ΔＲが示されている。 FIG. 7 is an explanatory diagram showing the effect of the correction of the moving object history of the coordinate data correction device according to the first embodiment.
In FIG. 7, reference numeral 30 and symbols ◯, Δ and R1, R3 are the same as those in FIG. In FIG. 7, the correction amount ΔR is shown.

次に、動作について説明する。
まず、自車移動量演算部１４の動作について説明する。
自車移動量演算部１４の具体的な演算として、自車状態量検出部２で検出される、自車速に基づく自車移動量と、ヨーレートに基づく演算周期間の自車旋回角度の変化量（ヨー角変化量）に基づき、式（１）に記載された座標変換を行う。
式（１）において、ｓｘ_ｎ、ｓｙ_ｎは、演算周期間の縦方向、横方向の自車移動量をそれぞれ表す。
［ｘ_ｎｙ_ｎ］^Ｔは、前回制御周期の自車固定座標系から見た、移動体相対位置検出部１３で検出された移動体の相対位置であり、［ｘ_ｎ−１ｙ_ｎ−１］^Ｔは、今回制御周期の自車固定座標系から見た、［ｘ_ｎｙ_ｎ］^Ｔとなる。θは今回演算周期におけるヨーレートを積分した値であり、自車固定座標系から見た自車のヨー角を表す。 Next, the operation will be described.
First, the operation of the vehicle movement amount calculation unit 14 will be described.
As a specific calculation of the own vehicle movement amount calculation unit 14, the own vehicle movement amount based on the own vehicle speed detected by the own vehicle state amount detection unit 2 and the change amount of the own vehicle turning angle between the calculation cycles based on the yaw rate. The coordinate conversion described in the equation (1) is performed based on the (yaw angle change amount).
In the formula _{(1), sx n, sy} n represents the vertical direction between the calculation cycle, the lateral vehicle movement amount, respectively.
_[X n _y ^{n] T} is seen from the vehicle fixed coordinate system of the previous control cycle, the relative position of the detected moving body in the moving body relative position detection unit _{_{13, [x n-1 y}} n-1 ] ^T is, as seen from the vehicle fixed coordinate system of the current control _cycle, the _{[x n} y ^{_n] T.} θ is a value obtained by integrating the yaw rate in the current calculation cycle, and represents the yaw angle of the own vehicle as seen from the own vehicle fixed coordinate system.

演算周期毎にこの座標変換を行うことで、常に自車座標を（０，０）とした移動体の過去履歴を表現できる。 By performing this coordinate conversion for each calculation cycle, the past history of the moving body can always be expressed with the own vehicle coordinate set to (0, 0).

式（１）の、ｓｘ_ｎ、ｓｙ_ｎは、各演算周期において自車が停車している、または自車状態量検出部２が検知できないほど小さい変動だった場合、ともに０となる。ヨー角変化量θについても同様となる。 Formula _{(1), sx n, sy} n , when being subject vehicle stopped at each calculation cycle, or the vehicle state quantity detection unit 2 was small variations that can not be detected, both zero. The same applies to the yaw angle change amount θ.

次に、自車座標系移動体履歴記憶部１５が、自車移動量演算部１４により演算された自車基準座標系での所定の範囲の移動体履歴データを保持し、次回制御周期（今回の例では、１００ｍｓｅｃ後）に引き渡す。 Next, the own vehicle coordinate system moving body history storage unit 15 holds the moving body history data of a predetermined range in the own vehicle reference coordinate system calculated by the own vehicle movement amount calculation unit 14, and the next control cycle (this time). In the example of 100, 100 msec later).

ここで、自車座標系移動体履歴記憶部１５に保持されている所定範囲の移動体履歴データについて説明する。
自車座標系移動体履歴記憶部１５には、フィルタ処理部４でフィルタ処理された移動体履歴データと、フィルタ処理されていない移動体履歴データが保持されている。
フィルタ処理を行う移動体履歴データ側にフィルタ処理を行わなかった場合、２つの移動体履歴データは同一となる。 Here, the moving body history data of a predetermined range stored in the moving body history storage unit 15 of the own vehicle coordinate system will be described.
The own vehicle coordinate system moving body history storage unit 15 holds moving body history data that has been filtered by the filter processing unit 4 and moving body history data that has not been filtered.
If no filtering process is performed on the moving object history data side to be filtered, the two moving object history data are the same.

次に、フィルタ処理を行う場合と、行わない場合の違いについて、図６を用いて説明する。
まず、フィルタ処理無しの移動体履歴の状態について説明する。
移動体は、車速４０ｋｍ／ｈで、曲線半径Ｒ１＝１２５．９７［ｍ］を走行している状態（横加速度０．９８ｍ／ｓ^２相当）である。この状態で得られる、図６の記号〇が移動体の走行履歴を表している。 Next, the difference between the case where filter processing is performed and the case where filter processing is not performed will be described with reference to FIG.
First, the state of the moving object history without the filter processing will be described.
The moving body is in a state of traveling at a vehicle speed of 40 km/h and a curve radius R1 of 125.97 [m] (corresponding to a lateral acceleration of 0.98 m/s ² ). The symbol ◯ in FIG. 6 obtained in this state represents the traveling history of the moving body.

次に、記号△で表される移動平均は、１０点分の処理を行っている。この点数については限定するものではなく、必要に応じて変更してよい。
移動平均そのものについても、（移動体履歴の総和（縦方向、横方向））／（移動平均数）で表わされる一般的なものである。 Next, the moving average represented by the symbol Δ is processed for 10 points. This score is not limited and may be changed as necessary.
The moving average itself is also a general one represented by (sum of moving object history (vertical direction, horizontal direction))/(number of moving averages).

同様に、記号□で表されるローパスフィルタは、カットオフ周波数４Ｈｚ，サンプリング時間１秒として処理を行っている。移動平均と同様、特段に限定しているものではなく、ハイパスフィルタなど、仕様がわかるものであれば何でもよい。 Similarly, the low-pass filter represented by the symbol □ is processed with a cutoff frequency of 4 Hz and a sampling time of 1 second. Similar to the moving average, the moving average is not particularly limited, and may be a high-pass filter or the like as long as the specifications can be understood.

今回の事例について総合的な誤差を計算すると、フィルタ処理無しの経路に対し、移動平均処理を行った場合、約０．５５ｍの誤差（曲線半径としては約４ｃｍ内側の経路）が発生し、ローパスフィルタの処理を行った場合、約０．５０ｍの誤差（曲線半径としては約４ｃｍ外側の経路）となる。
このように、フィルタ処理の内容に応じて、本来移動体が通過した経路と異なる経路になることがわかる。 Calculating the total error for this case, an error of about 0.55 m (a path with a radius of about 4 cm inside the curve) occurs when the moving average process is performed on the route without filter processing, and the low pass When the filter process is performed, an error of about 0.50 m (a path outside the curve radius of about 4 cm) is obtained.
In this way, it can be seen that the route is different from the route that the mobile body originally passed, depending on the content of the filtering process.

本実施の形態１では、車速、横加速度の一例を示したが、もちろん、車速・横加速度・過去履歴データから得られた曲率に対して可変である。 In the first embodiment, an example of the vehicle speed and the lateral acceleration is shown, but of course, it is variable with respect to the curvature obtained from the vehicle speed, the lateral acceleration, and the past history data.

次に、移動体履歴に対する補正部６による補正処理について説明する。
ここまでの手順を行うことで、現在演算周期における自車基準座標系での移動体履歴を得られたので、ここから実際の移動体履歴に近づけるための、フィルタ処理後の移動体履歴の補正処理について説明する。 Next, the correction process performed by the correction unit 6 on the moving object history will be described.
By performing the procedure up to this point, the moving body history in the own vehicle reference coordinate system in the current calculation cycle can be obtained. Therefore, the moving body history after filtering is corrected to approximate the actual moving body history from here. The processing will be described.

補正部６の自車座標系移動体履歴曲率演算部１６は、移動体履歴データがどの程度の曲率になっているかを演算する。この曲率を求めるためには、曲率項を持つ二次または三次などの関数で近似する必要があるが、曲率が分かれば、必ずしも関数近似をする必要はない。
曲率項を求めるための関数近似に関しては、一般的な近似手法として、よく知られている最小二乗法などを用いればよい。最小二乗法により移動体履歴データを二次または三次式で表すことができる。
二次式で近似した場合は「ｙ（ｘ）＝ａｘ^２＋ｂｘ＋ｃ」として表され、ａが曲率項に該当する。また、三次式で近似した場合は「ｙ（ｘ）＝ａｘ^３＋ｂｘ^２＋ｃｘ＋ｄ」としてあらわされ、ｂが曲率項に該当する。 The moving body history curvature calculation unit 16 of the vehicle coordinate system of the correction unit 6 calculates how much curvature the moving body history data has. In order to obtain this curvature, it is necessary to perform approximation with a quadratic or cubic function having a curvature term, but if the curvature is known, it is not necessary to perform function approximation.
Regarding the function approximation for obtaining the curvature term, a well-known least square method or the like may be used as a general approximation method. The moving object history data can be expressed by a quadratic or cubic equation by the least square method.
When approximated by a quadratic expression, it is represented as “y(x)=ax ² +bx+c”, and a corresponds to the curvature term. When approximated by a cubic expression, it is expressed as “y(x)=ax ³ +bx ² +cx+d”, and b corresponds to the curvature term.

補正部６の自車座標系移動体履歴補正部１７は、フィルタ処理部４の処理が実行され、自車座標系移動体履歴記憶部１５で保持されている移動体履歴を補正する。自車座標系移動体履歴補正部１７は、バイパス機能も合わせて有しており、移動体履歴の曲率が閾値未満の場合、補正を実施しなくてもよい。 The own vehicle coordinate system moving body history correction unit 17 of the correction unit 6 performs the processing of the filter processing unit 4 and corrects the moving body history held in the own vehicle coordinate system moving body history storage unit 15. The own vehicle coordinate system moving body history correction unit 17 also has a bypass function, and if the curvature of the moving body history is less than the threshold value, the correction may not be performed.

次に、移動体履歴の補正を実施する手段として、自車座標系移動体履歴記憶部１５で保持されている移動体履歴を直接補正する方法と、移動体履歴の曲率に相当する値を補正する方法について説明する。 Next, as means for correcting the moving body history, a method of directly correcting the moving body history held in the moving body history storage unit 15 of the own vehicle coordinate system and a value corresponding to the curvature of the moving body history are corrected. The method for doing so will be described.

まず、移動体履歴を直接補正する場合について説明する。
自車座標系移動体履歴記憶部１５で保持されている移動体履歴には、フィルタ処理を行っている移動体履歴と、フィルタ処理を行っていない移動体履歴との２つが常に存在することとなる。（フィルタ処理部４の処理をバイパスした場合、２つのフィルタ処理を行っていない移動体履歴が、自車座標系移動体履歴記憶部１５で保持されている状態となる。） First, a case where the history of the moving body is directly corrected will be described.
In the moving body history stored in the moving body history storage unit 15 of the own vehicle coordinate system, there are always two kinds of moving body history, that is, a moving body history that has been filtered and a moving body history that is not filtered. Become. (When the process of the filter processing unit 4 is bypassed, the moving body history that has not been subjected to the two filtering processes is held in the moving body history storage unit 15 of the own vehicle coordinate system.)

自車座標系移動体履歴曲率演算部１６で、フィルタ処理を行った移動体履歴の曲率が演算され、フィルタ処理仕様および自車速情報から曲率項の補正量が求まる。
その結果から、フィルタ処理を行った移動体履歴の曲線半径と、その曲線半径から移動体履歴の各点が垂直になるような線を求め、その関係を維持したまま曲線半径の補正量分だけ移動させることで、フィルタ処理を行う前の移動体履歴と同じ曲率情報を持った、フィルタ処理後の移動体履歴を得ることができる。図７に示すような結果になる。
すなわち、移動体履歴の座標データの各点を直接補正する方法である。 The curvature of the moving body history subjected to the filter processing is calculated by the moving body history curvature calculation unit 16 of the own vehicle coordinate system, and the correction amount of the curvature term is obtained from the filter processing specification and the own vehicle speed information.
From the result, find the curve radius of the moving object history that has been filtered, and the line that makes each point of the moving object history vertical from the curve radius, and keep the relationship and only the correction amount of the curve radius. By moving, it is possible to obtain the moving object history after the filtering processing having the same curvature information as the moving object history before performing the filtering processing. The result is as shown in FIG.
That is, this is a method of directly correcting each point in the coordinate data of the history of the moving body.

次に、求められた曲率項に対して補正を行う方法について述べる。
移動体履歴そのものを補正する方法については、上述のとおりであるが、曲率項に対して補正を行う場合は、フィルタ処理を行った移動体履歴の曲率と、曲率の補正量はすでに求められているので、上述の法線の関係などを用いずに、直接、曲率に対して補正値を加算すればよい。
具体的には、図７に記載しているとおり、移動平均後の曲線半径Ｒ３が求まっており、移動平均の補正量も求まっているので、図７に示す補正量ΔＲを加算する。この方法により、本来得られる正確な曲率項を得ることができる。 Next, a method of correcting the calculated curvature term will be described.
The method for correcting the moving object history itself is as described above. However, when correcting the curvature term, the curvature of the filtered moving object history and the correction amount of the curvature have already been obtained. Therefore, the correction value may be directly added to the curvature without using the above-described relationship of the normal line.
Specifically, as shown in FIG. 7, since the curve radius R3 after the moving average is obtained and the correction amount of the moving average is also obtained, the correction amount ΔR shown in FIG. 7 is added. By this method, an accurate curvature term that is originally obtained can be obtained.

次に、図２のフローチャートに基づき、実施の形態１による座標データ補正装置の動作について説明する。 Next, the operation of the coordinate data correction apparatus according to the first embodiment will be described based on the flowchart of FIG.

座標データ補正装置は、まず、移動体相対位置検出部１３により、今回制御周期における、移動体相対位置を取得する（ステップＳ１１１、第一のステップ）。 In the coordinate data correction device, first, the moving body relative position detection unit 13 acquires the moving body relative position in the current control cycle (step S111, first step).

次に、取得した相対位置に対し、必要に応じて、フィルタ処理部４によるフィルタ処理を行う（ステップＳ１１２、第二のステップ）。 Next, the acquired relative position is filtered by the filtering unit 4 as necessary (step S112, second step).

次に、後述する図３のステップＳ１２２で演算された自車移動量に基づき、現在記憶されている移動体の過去履歴を今回制御周期における、自車基準座標系に変換して、記憶する（ステップＳ１１３、第三のステップ）。 Next, based on the moving amount of the own vehicle calculated in step S122 of FIG. 3 to be described later, the past history of the moving body currently stored is converted into the own vehicle reference coordinate system in the current control cycle and stored ( Step S113, third step).

次に、今回制御周期における、自車基準座標系に変換された移動体の過去履歴に基づき、自車座標系移動体履歴曲率演算部１６による曲率演算を行う（ステップＳ１１４）。
この演算手法としては、一般的によく知られている最小法二乗法などを実施すればよい。また、曲率を得るためには二次、または三次式による近似を行えばよい。 Next, based on the past history of the moving body converted to the own vehicle reference coordinate system in the current control cycle, the own vehicle coordinate system moving body history curvature calculation unit 16 performs curvature calculation (step S114).
As the calculation method, the generally known least squares method or the like may be implemented. Further, in order to obtain the curvature, approximation by a quadratic or cubic equation may be performed.

次に、ステップＳ１１４で演算された曲率に該当する項が閾値以上であれば、自車座標系移動体履歴補正部１７による移動体履歴の補正を行うステップＳ１１６へ進み、閾値未満であれば、そのまま今回制御周期における処理は終了となる（ステップＳ１１５）。 Next, if the term corresponding to the curvature calculated in step S114 is greater than or equal to the threshold value, the process proceeds to step S116 in which the moving object history correction unit 17 corrects the moving object history. The processing in the current control cycle is finished as it is (step S115).

次に、移動体履歴の補正が必要と判断された場合、自車座標系移動体履歴補正部１７により、後述する図４のステップＳ１３１で実施されたフィルタ処理が行われていない移動体履歴の曲率に近づくように、移動体履歴の各点を直接補正（座標データ補正）、または曲率項に該当する数値に対して、補正を行う処理（座標データ曲率補正）が実施される（ステップＳ１１６、第四のステップ）。 Next, when it is determined that the correction of the moving object history is necessary, the moving object history correction unit 17 of the vehicle coordinate system moves the moving object history that has not been subjected to the filtering process performed in step S131 of FIG. Each point of the history of the moving body is directly corrected (coordinate data correction) so as to approach the curvature, or a process of correcting the numerical value corresponding to the curvature term (coordinate data curvature correction) is performed (step S116, Fourth step).

ステップＳ１１６で最終的に演算された結果を制御部７に出力し、今回制御周期の処理を終了する。制御部７は、ステップＳ１１６で出力された演算結果に基づき、移動体を追従するための自車の操舵を制御する（第五のステップ）。 The result finally calculated in step S116 is output to the control unit 7, and the processing of the current control cycle is ended. The control unit 7 controls the steering of the own vehicle to follow the moving body based on the calculation result output in step S116 (fifth step).

次に、図３のフローチャートに基づき、自車移動量演算部１４による自車移動量の演算処理について説明する。
まず、自車状態量検出部２により、今回制御周期における、自車の状態量を取得する（ステップＳ１２１）。 Next, the calculation processing of the own vehicle movement amount by the own vehicle movement amount calculation unit 14 will be described based on the flowchart of FIG.
First, the vehicle state quantity detector 2 acquires the state quantity of the vehicle in the current control cycle (step S121).

次に、取得された自車状態量に基づく、前回制御周期から今回制御周期までの自車移動量を、自車移動量演算部１４により、演算する（ステップＳ１２２）。 Next, the own vehicle movement amount calculation unit 14 calculates the own vehicle movement amount from the previous control period to the current control period based on the obtained own vehicle state amount (step S122).

ステップＳ１２２で演算された結果を、図２のステップＳ１１３および後述する図４のステップＳ１３１に渡し、今回制御周期における処理を終了する。 The result calculated in step S122 is passed to step S113 of FIG. 2 and step S131 of FIG. 4 which will be described later, and the processing in the current control cycle ends.

最後に、図４のフローチャートに基づき、自車座標系移動体履歴記憶部１５による、移動体の過去履歴の自車基準座標系への変換処理について説明する。 Finally, the conversion processing of the past history of the moving body into the own vehicle reference coordinate system by the own vehicle coordinate system moving body history storage unit 15 will be described based on the flowchart of FIG.

まず、移動体相対位置検出部１３により、今回制御周期における、移動体相対位置を取得する（ステップＳ１１１）。 First, the moving body relative position detection unit 13 acquires the moving body relative position in the current control cycle (step S111).

次に、上述した図３のステップＳ１２２で演算された自車移動量に基づき、現在記憶されている、フィルタ処理が行われていない移動体の過去履歴を、今回制御周期における自車基準座標系に変換する（ステップＳ１３１）。
自車基準座標系に変換された今回制御周期における移動体の過去履歴を、図２のステップＳ１１６に渡し、今回制御周期における処理を終了する。 Next, based on the moving amount of the own vehicle calculated in step S122 of FIG. 3 described above, the past history of the moving body that is currently stored and has not been subjected to the filter processing is set to the own vehicle reference coordinate system in the current control cycle. (Step S131).
The past history of the moving body in the current control cycle converted into the own vehicle reference coordinate system is passed to step S116 of FIG. 2, and the processing in the current control cycle is ended.

次に、座標データ補正装置のハードウェア構成について説明する。
図８は、処理回路１０１がプロセッサを用いて構成されている場合における座標データ補正装置１のハードウェア構成を示している。この場合、座標データ補正装置１の各要素の機能は、ソフトウェア等（ソフトウェア、ファームウェア、またはソフトウェアとファームウェア）との組み合わせにより実現される。
ソフトウェア等はプログラムとして記述され、メモリ１０３に格納される。処理回路１０１としてのプロセッサ１０２は、メモリ１０３に記憶されたプログラムを読み出して実行することにより、各部の機能を実現する。
すなわち、座標データ補正装置１は、処理回路１０１により実行されるときに、座標データ補正装置１を構成する各要素の機能を実行するためのプログラムを格納するメモリ１０３を有している。
換言すれば、このプログラムは、座標データ補正装置１の各要素の動作の手順、方法をコンピュータに実行させるものであるともいえる。 Next, the hardware configuration of the coordinate data correction device will be described.
FIG. 8 shows a hardware configuration of the coordinate data correction device 1 when the processing circuit 101 is configured using a processor. In this case, the function of each element of the coordinate data correction device 1 is realized by a combination of software and the like (software, firmware, or software and firmware).
The software and the like are described as a program and stored in the memory 103. The processor 102 as the processing circuit 101 realizes the function of each unit by reading and executing the program stored in the memory 103.
That is, the coordinate data correction device 1 has a memory 103 that stores a program for executing the function of each element that configures the coordinate data correction device 1 when the coordinate data correction device 1 is executed by the processing circuit 101.
In other words, it can be said that this program causes a computer to execute the procedure and method of operation of each element of the coordinate data correction device 1.

ここで、メモリ１０３は、例えば、ＲＡＭ（ＲａｎｄｏｍＡｃｃｅｓｓＭｅｍｏｒｙ）、ＲＯＭ（ＲｅａｄＯｎｌｙＭｅｍｏｒｙ）、フラッシュメモリー、ＥＰＲＯＭ（ＥｒａｓａｂｌｅＰｒｏｇｒａｍｍａｂｌｅＲｅａｄＯｎｌｙＭｅｍｏｒｙ）、ＥＥＰＲＯＭ（ＥｌｅｃｔｒｉｃａｌｌｙＥｒａｓａｂｌｅＰｒｏｇｒａｍｍａｂｌｅＲｅａｄＯｎｌｙＭｅｍｏｒｙ）などの、不揮発性または揮発性の半導体メモリ、ＨＤＤ（ＨａｒｄＤｉｓｋＤｒｉｖｅ）、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（ＤｉｇｉｔａｌＶｅｒｓａｔｉｌｅＤｉｓｃ）およびそのドライブ装置等、または、今後使用されるあらゆる記憶媒体であってもよい。 Here, the memory 103 is, for example, a RAM (Random Access Memory), a ROM (Read Only Memory), a flash memory, an EPROM (Erasable Programmable Read-Only Memory), an EEPROM (Electrically Programmable Memory), an EEPROM (Electrically Programmable Erasable Memory), an EEPROM (Electrically Programmable Era), or an EEPROM (Electrically Programmable Era). It is a volatile semiconductor memory, a hard disk drive (HDD), a magnetic disk, a flexible disk, an optical disk, a compact disk, a mini disk, a DVD (Digital Versatile Disc) and its drive device, or any storage medium used in the future. May be.

以上、座標データ補正装置１の各要素の機能が、ハードウェアおよびソフトウェア等のいずれか一方で実現される構成について説明した。
しかし、これに限ったものではなく、座標データ補正装置１の一部の要素を専用のハードウェアで実現し、別の一部の要素をソフトウェア等で実現する構成であってもよい。
例えば、一部の要素については専用のハードウェアとしての処理回路１０１でその機能を実現し、他の一部の要素については、プロセッサ１０２としての処理回路がメモリ１０３に格納されたプログラムを読み出して実行することによって、その機能を実現することが可能である。 Heretofore, the configuration in which the function of each element of the coordinate data correction device 1 is realized by either hardware or software has been described.
However, the configuration is not limited to this, and a configuration in which some of the elements of the coordinate data correction device 1 are implemented by dedicated hardware and another of the elements is implemented by software or the like may be used.
For example, for some of the elements, the function is realized by the processing circuit 101 as dedicated hardware, and for some of the other elements, the processing circuit as the processor 102 reads the program stored in the memory 103. By executing the function, it is possible to realize the function.

以上のように、座標データ補正装置１は、ハードウェア、ソフトウェア等、またはこれらの組み合わせによって、上述の各機能を実現することができる。 As described above, the coordinate data correction device 1 can realize the above-described functions by hardware, software, etc., or a combination thereof.

実施の形態１によれば、従来の移動体追従装置と比べて、特に車両のカーブ走行時における移動体履歴誤差を低減することができ、縁石への衝突または路肩への乗揚げを防止し、移動体追従時の安全性を向上させることができる。
また、フィルタ処理による移動体履歴の補正を行い、移動体の動きに対し正確な追従を行うことができる。 According to the first embodiment, it is possible to reduce the history error of the moving body, especially when the vehicle is traveling in a curve, as compared with the conventional moving body following device, and prevent a collision with a curb or a landing on a road shoulder, The safety when following a moving body can be improved.
In addition, it is possible to correct the history of the moving body by the filter processing and accurately follow the movement of the moving body.

実施の形態２．
図９は、実施の形態２による座標データ補正装置の概略構成を示すブロック図である。
図９において、符号１、３〜７は図１におけるものと同一のものである。
図９では、座標データ入力部３は、移動体の緯度経度を検出する緯度経度情報検出部１８を有する。座標データ記憶部５は、緯度経度座標を記憶する緯度経度座標系緯度経度座標記憶部１９を有する。補正部６は、移動体の緯度経度の曲率を演算する緯度経度座標系緯度経度曲率演算部２０と、緯度経度座標系緯度経度曲率演算部２０により演算された移動体の緯度経度の曲率を補正する緯度経度座標系緯度経度曲率補正部２１とを有する。 Embodiment 2.
FIG. 9 is a block diagram showing a schematic configuration of the coordinate data correction device according to the second embodiment.
9, reference numerals 1, 3 to 7 are the same as those in FIG.
In FIG. 9, the coordinate data input unit 3 includes a latitude/longitude information detection unit 18 that detects the latitude/longitude of the moving body. The coordinate data storage unit 5 has a latitude/longitude coordinate system latitude/longitude coordinate storage unit 19 for storing latitude/longitude coordinates. The correction unit 6 corrects the latitude/longitude curvature calculation unit 20 for calculating the curvature of the latitude/longitude of the moving body and the latitude/longitude curvature of the moving body calculated by the latitude/longitude curvature calculation unit 20 of the latitude/longitude coordinate system. Latitude/longitude coordinate system latitude/longitude curvature correction unit 21.

実施の形態２は、実施の形態１で述べたような自車基準座標系ではなく、移動体の緯度経度を用いて、緯度経度座標の補正を行なうようにしたものである。すなわち、座標系を、緯度経度を基準とする座標系としている。
緯度経度情報検出部１８により、直接、移動体の緯度経度を検出し、その緯度経度の検出情報に基づいて、フィルタ処理を行い、フィルタ処理後の値を記憶する。
そして、記憶したフィルタ処理後の値に対して、曲率を演算し、曲率または緯度経度座標に対し、実施の形態１と同様の補正を行うようにした。 In the second embodiment, the latitude/longitude coordinates are corrected using the latitude/longitude of the moving body instead of the own vehicle reference coordinate system as described in the first embodiment. That is, the coordinate system is a coordinate system based on latitude and longitude.
The latitude/longitude information detection unit 18 directly detects the latitude/longitude of the moving body, performs the filtering process based on the detection information of the latitude/longitude, and stores the value after the filtering process.
Then, the curvature is calculated with respect to the stored value after the filter processing, and the same correction as that in the first embodiment is performed on the curvature or the latitude/longitude coordinates.

次に、図１０のフローチャートを用いて、実施の形態２の移動体の座標データの補正について説明する。
まず、緯度経度情報検出部１８により、今回制御周期における、移動体の緯度経度情報を取得する（ステップＳ２１１）。 Next, correction of the coordinate data of the moving body according to the second embodiment will be described with reference to the flowchart of FIG.
First, the latitude/longitude information detection unit 18 acquires the latitude/longitude information of the moving body in the current control cycle (step S211).

次に、取得した移動体の緯度経度情報に対し、必要に応じて、フィルタ処理部４によるフィルタ処理を行う。（行う場合、ステップＳ２１２を実行）。 Next, the acquired latitude/longitude information of the moving body is subjected to a filtering process by the filtering unit 4 as necessary. (If yes, execute step S212).

次に、緯度経度座標系緯度経度座標記憶部１９により、移動体の緯度経度情報を、記憶する（ステップＳ２１３）。 Next, the latitude/longitude coordinate system latitude/longitude coordinate storage unit 19 stores the latitude/longitude information of the mobile body (step S213).

次に、今回制御周期における、移動体の過去の緯度経度情報に基づき、緯度経度座標系緯度経度曲率演算部２０による曲率の演算を行う（ステップＳ２１４）。 Next, based on the past latitude/longitude information of the moving body in the current control cycle, the latitude/longitude coordinate system latitude/longitude curvature calculation unit 20 calculates the curvature (step S214).

次に、ステップＳ２１４で演算された曲率に該当する項が閾値以上であれば、緯度経度座標系緯度経度曲率補正部２１により補正を行なうステップＳ２１６へ進み、閾値未満であれば、そのまま今回制御周期における処理を終了する（ステップＳ２１５）。 Next, if the term corresponding to the curvature calculated in step S214 is greater than or equal to the threshold value, the process proceeds to step S216 in which the latitude/longitude coordinate system latitude/longitude curvature correction unit 21 makes a correction. The process in (3) is finished (step S215).

次に、曲率の補正が必要と判断された場合、緯度経度座標系緯度経度曲率補正部２１により、緯度経度情報（座標データ）に基づく補正、または曲率項に該当する数値に対して、直接、補正を行う処理が実施される（ステップＳ２１６）。 Next, when it is determined that the curvature needs to be corrected, the latitude/longitude coordinate system latitude/longitude curvature correction unit 21 makes a correction based on the latitude/longitude information (coordinate data) or directly with respect to a numerical value corresponding to the curvature term. A correction process is performed (step S216).

ステップＳ２１６で最終的に演算された結果を制御部７に出力し、今回制御周期の処理を終了する。 The result finally calculated in step S216 is output to the control unit 7, and the processing of the current control cycle is ended.

実施の形態２によれば、移動体について、適切な曲率の緯度経度情報を得ることができる。 According to the second embodiment, it is possible to obtain latitude/longitude information with an appropriate curvature for the moving body.

本開示は、様々な例示的な実施の形態及び実施例が記載されているが、１つ、または複数の実施の形態に記載された様々な特徴、態様、及び機能は特定の実施の形態の適用に限られるのではなく、単独で、または様々な組み合わせで実施の形態に適用可能である。
従って、例示されていない無数の変形例が、本願明細書に開示される技術の範囲内において想定される。例えば、少なくとも１つの構成要素を変形する場合、追加する場合または省略する場合、さらには、少なくとも１つの構成要素を抽出し、他の実施の形態の構成要素と組み合わせる場合が含まれるものとする。 Although the present disclosure describes various exemplary embodiments and examples, the various features, aspects, and functions described in one or more of the embodiments are not described in particular embodiments. The present invention is not limited to the application, and can be applied to the embodiments alone or in various combinations.
Therefore, innumerable variations not illustrated are envisioned within the scope of the technology disclosed herein. For example, it is assumed that at least one component is modified, added or omitted, and at least one component is extracted and combined with the components of other embodiments.

１座標データ補正装置、２自車状態量検出部、３座標データ入力部、
４フィルタ処理部、５座標データ記憶部、６補正部、７制御部、
１１自車速検出部、１２ヨーレート検出部、１３移動体相対位置検出部、
１４自車移動量演算部、１５自車座標系移動体履歴記憶部、
１６自車座標系移動体履歴曲率演算部、１７自車座標系移動体履歴補正部、
１８緯度経度情報検出部、１９緯度経度座標系緯度経度座標記憶部、
２０緯度経度座標系緯度経度曲率演算部、２１緯度経度座標系緯度経度曲率補正部、
３０自車、４０移動体、１０１処理回路、１０２プロセッサ、１０３メモリ 1 coordinate data correction device, 2 vehicle state quantity detection unit, 3 coordinate data input unit,
4 filter processing unit, 5 coordinate data storage unit, 6 correction unit, 7 control unit,
11 vehicle speed detection unit, 12 yaw rate detection unit, 13 moving body relative position detection unit,
14 own vehicle movement amount calculation unit, 15 own vehicle coordinate system moving body history storage unit,
16 own vehicle coordinate system moving body history curvature calculation unit, 17 own vehicle coordinate system moving body history correction unit,
18 latitude/longitude information detection unit, 19 latitude/longitude coordinate system latitude/longitude coordinate storage unit,
20 latitude/longitude coordinate system latitude/longitude curvature calculation unit, 21 latitude/longitude coordinate system latitude/longitude curvature correction unit,
30 own vehicle, 40 moving body, 101 processing circuit, 102 processor, 103 memory

Claims

自車に搭載された座標データ補正装置であって、
ある点を基点とする座標系における、追従対象の移動体の座標データが入力される座標データ入力部、
この座標データ入力部により入力された上記座標データを平滑化処理するフィルタ処理部、
このフィルタ処理部により平滑化処理された上記座標データを、上記移動体の履歴として３点以上記憶する座標データ記憶部、
この座標データ記憶部に記憶された上記平滑化処理された３点以上の上記座標データから上記移動体の履歴の曲率を演算し、上記フィルタ処理部におけるフィルタ処理仕様から求まる曲率の補正量を用いて、実際の上記移動体の履歴の曲率に近づけるように、上記移動体の履歴を補正する補正部を備えたことを特徴とする座標データ補正装置。 A coordinate data correction device mounted on the vehicle,
In a coordinate system with a certain point as a base point, a coordinate data input section for inputting coordinate data of a moving object to be followed,
A filter processing unit that smoothes the coordinate data input by the coordinate data input unit,
A coordinate data storage unit that stores the coordinate data smoothed by the filter processing unit as three or more points as a history of the moving body.
The curvature of the history of the moving body is calculated from the smoothed coordinate data of three or more points stored in the coordinate data storage unit, and the correction amount of the curvature obtained from the filter processing specification in the filter processing unit is used. A coordinate data correction apparatus comprising a correction unit that corrects the history of the moving body so as to approximate the actual curvature of the history of the moving body.

上記座標データ記憶部は、上記平滑化処理された上記座標データに加えて、上記平滑化処理される前の上記座標データを記憶することを特徴とする請求項１に記載の座標データ補正装置。 The coordinate data correction device according to claim 1, wherein the coordinate data storage unit stores the coordinate data before the smoothing process, in addition to the smoothed coordinate data.

上記補正部による上記移動体の履歴の補正は、上記座標データの各点の補正により行なうことを特徴とする請求項１に記載の座標データ補正装置。 The coordinate data correction apparatus according to claim 1, wherein the correction of the history of the moving body by the correction unit is performed by correcting each point of the coordinate data.

上記補正部による上記移動体の履歴の補正は、上記曲率の補正により行なうことを特徴とする請求項１に記載の座標データ補正装置。 The coordinate data correction apparatus according to claim 1, wherein the correction of the history of the moving body by the correction unit is performed by correction of the curvature.

上記補正部による上記移動体の履歴の補正は、当該移動体の履歴の曲率が、予め定めた閾値以上の場合に行なうことを特徴とする請求項１から請求項４のいずれか一項に記載の座標データ補正装置。 The correction of the history of the moving body by the correction unit is performed when the curvature of the history of the moving body is equal to or larger than a predetermined threshold value. Coordinate data correction device.

上記座標系は、上記座標データ補正装置が搭載された自車を基準とする座標系であることを特徴とする請求項１から請求項５のいずれか一項に記載の座標データ補正装置。 6. The coordinate data correction device according to claim 1, wherein the coordinate system is a coordinate system based on a vehicle equipped with the coordinate data correction device.

上記座標系に基づき、自車の車速およびヨーレートを含む自車状態量を検出する自車状態量検出部を備えたことを特徴とする請求項６に記載の座標データ補正装置。 7. The coordinate data correction device according to claim 6, further comprising a vehicle state quantity detection unit that detects a vehicle state quantity including a vehicle speed and a yaw rate of the vehicle based on the coordinate system.

上記座標データ記憶部は、上記自車状態量から演算された自車移動量に基づいて、上記移動体の座標データを、上記自車を基準とする座標系に変換して記憶することを特徴とする請求項７に記載の座標データ補正装置。 The coordinate data storage unit converts the coordinate data of the moving body into a coordinate system based on the own vehicle and stores the coordinate data based on the own vehicle movement amount calculated from the own vehicle state amount. The coordinate data correction device according to claim 7.

上記座標系は、緯度経度を基準とする座標系であることを特徴とする請求項１から請求項５のいずれか一項に記載の座標データ補正装置。 The coordinate data correction device according to any one of claims 1 to 5, wherein the coordinate system is a coordinate system based on latitude and longitude.

請求項１から請求項９のいずれか一項に記載の座標データ補正装置、
この座標データ補正装置によって補正された上記移動体の履歴に基づき、上記移動体を追従するように、上記座標データ補正装置が搭載された自車の操舵を制御する制御部を備えたことを特徴とする移動体追従装置。 The coordinate data correction device according to any one of claims 1 to 9,
Based on the history of the moving body corrected by the coordinate data correcting apparatus, a control unit for controlling steering of a vehicle equipped with the coordinate data correcting apparatus is provided so as to follow the moving body. A moving body tracking device.

座標データ入力部に、ある点を基点とする座標系における、追従対象の移動体の座標データが入力される第一のステップ、
この第一のステップにより入力された上記座標データを、フィルタ処理部が平滑化処理する第二のステップ、
この第二のステップにより平滑化処理された上記座標データを、座標データ記憶部が上記移動体の履歴として３点以上記憶する第三のステップ、
補正部が、上記第三のステップで記憶された上記平滑化処理された３点以上の上記座標データから上記移動体の履歴の曲率を演算し、上記フィルタ処理部におけるフィルタ処理仕様から求まる曲率の補正量を用いて、実際の上記移動体の履歴の曲率に近づけるように、上記移動体の履歴を補正する第四のステップ、
この第四のステップにより補正された上記移動体の履歴に基づき、制御部が、上記移動体を追従するように、自車の操舵を制御する第五のステップを含むことを特徴とする移動体追従方法。 In the coordinate data input section, a first step in which coordinate data of a moving object to be followed is input in a coordinate system having a certain point as a base point,
The second step in which the filter processing unit smoothes the coordinate data input in the first step,
A third step in which the coordinate data storage unit stores three or more points of the coordinate data smoothed by the second step as a history of the moving body,
The correction unit calculates the curvature of the history of the moving body from the smoothed coordinate data of three or more points stored in the third step, and calculates the curvature of the curvature obtained from the filter processing specification in the filter processing unit. A fourth step of correcting the history of the moving body so as to approach the actual curvature of the history of the moving body using a correction amount ,
The moving body, characterized in that the control unit includes a fifth step of controlling the steering of the own vehicle so as to follow the moving body on the basis of the history of the moving body corrected in the fourth step. How to follow.

上記第三のステップは、上記第二のステップで平滑化処理された上記座標データに加えて、上記平滑化処理される前の上記座標データを記憶することを特徴とする請求項１１に記載の移動体追従方法。 12. The third step stores the coordinate data before the smoothing process in addition to the coordinate data smoothed in the second step. Mobile tracking method.

上記第四のステップでの上記移動体の履歴の補正は、上記座標データの各点の補正により行なうことを特徴とする請求項１１に記載の移動体追従方法。 12. The moving body tracking method according to claim 11, wherein the correction of the history of the moving body in the fourth step is performed by correcting each point of the coordinate data.

上記第四のステップでの上記移動体の履歴の補正は、上記曲率の補正により行なうことを特徴とする請求項１１に記載の移動体追従方法。 The moving body tracking method according to claim 11, wherein the correction of the history of the moving body in the fourth step is performed by correcting the curvature.

上記第四のステップでの上記移動体の履歴の補正は、当該移動体の履歴の曲率が、予め定めた閾値以上の場合に行なうことを特徴とする請求項１１から請求項１４のいずれか一項に記載の移動体追従方法。 The correction of the history of the moving body in the fourth step is performed when the curvature of the history of the moving body is equal to or larger than a predetermined threshold value. The method for following a moving object according to item.

上記第一のステップにおける座標系は、自車を基準とする座標系であることを特徴とする請求項１１から請求項１５のいずれか一項に記載の移動体追従方法。 The moving body tracking method according to any one of claims 11 to 15, wherein the coordinate system in the first step is a coordinate system based on the host vehicle.

上記第一のステップにおける座標系は、緯度経度を基準とする座標系であることを特徴とする請求項１１から請求項１５のいずれか一項に記載の移動体追従方法。 The moving body tracking method according to claim 11, wherein the coordinate system in the first step is a coordinate system based on latitude and longitude.