JP6900341B2

JP6900341B2 - Position calculation device and dump truck

Info

Publication number: JP6900341B2
Application number: JP2018068450A
Authority: JP
Inventors: 幹雄板東; 航田中; 石本　英史; 英史石本; 信一魚津
Original assignee: Hitachi Construction Machinery Co Ltd
Current assignee: Hitachi Construction Machinery Co Ltd
Priority date: 2018-03-30
Filing date: 2018-03-30
Publication date: 2021-07-07
Anticipated expiration: 2038-03-30
Also published as: JP2019179421A

Description

本発明は、位置算出装置及びダンプトラックに係り、特に鉱山や建設現場で移動するダンプトラックに搭載された位置算出装置の位置測定誤差に関する。 The present invention relates to a position calculation device and a dump truck, and particularly relates to a position measurement error of the position calculation device mounted on a dump truck moving at a mine or a construction site.

自律的に走行するダンプトラックにおいてその位置を算出する機能は重要な機能の一つである。この位置算出の例として、慣性計測装置（ＩＭＵ）を用い、車輪の回転数から速度を計測する車輪速度計、車輪の車軸に対する傾きを計測する操舵角計等の計測装置の結果を用いて、ダンプトラックの位置を算出する。ＧＮＳＳ等のように直接位置を算出する機器から出力される位置に対して、速度、姿勢、加速度、角速度などの関連する運動パラメータから位置を順次更新する方法があり、デッドレコニングと呼ばれる。 The function of calculating the position of a dump truck that travels autonomously is one of the important functions. As an example of this position calculation, an inertial measurement unit (IMU) is used, and the results of a measuring device such as a wheel speedometer that measures the speed from the number of rotations of the wheel and a steering angle meter that measures the inclination of the wheel with respect to the axle are used. Calculate the position of the dump track. There is a method of sequentially updating the position from related motion parameters such as velocity, posture, acceleration, and angular velocity with respect to the position output from a device that directly calculates the position such as GNSS, which is called dead reckoning.

従来よりデッドレコニングを用いて算出した位置を、カルマンフィルタなどに代表される確率フィルタを用いて、全地球航法衛星システム（ＧＮＳＳ）にて算出された位置へ徐々に修正する手法がある。この確率フィルタを用いた位置修正方法は、デッドレコニング、ＧＮＳＳのそれぞれから算出される誤差を考慮して位置を修正するため、確率的に最も存在する可能性の高い位置を算出できることが長所である。この際に、確率フィルタを用いて、自律走行ダンプトラックの位置の誤差を分散値として算出し、この分散値が存在する可能性のある範囲を誤差楕円として表わすことが可能である。 Conventionally, there is a method of gradually correcting a position calculated by using dead reckoning to a position calculated by a global navigation satellite system (GNSS) using a probability filter represented by a Kalman filter or the like. The position correction method using this probability filter has an advantage that the position most likely to exist stochastically can be calculated because the position is corrected in consideration of the error calculated from each of dead reckoning and GNSS. .. At this time, it is possible to calculate the error of the position of the autonomous traveling dump truck as a variance value by using a probability filter, and express the range in which this variance value may exist as an error ellipse.

特許文献１には、誤差楕円を用いたダンプトラックの走行制御装置として、「自車両の予測位置を算出する位置算出装置と、予測位置を中心とし、運搬車両が予め定められた期待確率で存在する位置範囲を算出する誤差見積もり範囲算出部と、運搬車両の目標経路と位置範囲内に含まれる各地点との乖離量のうちの最大値からなる最大乖離量を算出する最大乖離量算出部と、最大乖離量が相対的に大きい場合に、運搬車両の目標車速を相対的に小さく定める目標車速決定部と、目標車速に従って、目標経路に沿って運搬車両を走行させるための制御を行う目標経路追従部（要約抜粋）」を備えた構成が開示されている。 In Patent Document 1, as a travel control device for a dump truck using an error ellipse, "a position calculation device for calculating the predicted position of the own vehicle and a transport vehicle centered on the predicted position exist with a predetermined expected probability. An error estimation range calculation unit that calculates the position range to be used, and a maximum deviation amount calculation unit that calculates the maximum deviation amount consisting of the maximum value of the deviation amount between the target route of the material handling vehicle and each point included in the position range. , The target vehicle speed determination unit that sets the target vehicle speed of the transport vehicle relatively small when the maximum deviation amount is relatively large, and the target route that controls the transport vehicle to travel along the target route according to the target vehicle speed. A configuration including a "following unit (summary excerpt)" is disclosed.

また特許文献２には、「ナビゲーション装置は、ＧＰＳ位置算出部と、センサ情報取得部と、ＧＰＳ位置とセンサ情報とに基づいて移動体の位置および向き、並びに、これらの誤差共分散行列を算出する位置誤差算出部と、各センサ情報を累積して得られた位置と、ＧＰＳ位置との位置の差をオフセットとして算出するオフセット算出部と、算出されたオフセットに基づいて、位置誤差算出部によって算出された誤差共分散行列を補正する誤差共分散行列補正部と、位置誤差算出部によって算出された移動体の位置および向き、並びに、誤差共分散行列補正部によって補正された誤差共分散行列を用いて、地図データに含まれる道路上に移動体の位置を推定するマップマッチ処理部とを備える（要約抜粋）」構成が開示されている。 Further, in Patent Document 2, "The navigation device calculates the position and orientation of the moving body based on the GPS position calculation unit, the sensor information acquisition unit, the GPS position and the sensor information, and their error covariance matrix. The position error calculation unit, the offset calculation unit that calculates the difference between the position obtained by accumulating each sensor information and the GPS position as an offset, and the position error calculation unit based on the calculated offset. The error co-dispersion matrix correction unit that corrects the calculated error co-distribution matrix, the position and orientation of the moving object calculated by the position error calculation unit, and the error co-dispersion matrix corrected by the error co-dispersion matrix correction unit. It is provided with a map match processing unit that estimates the position of a moving object on the road included in the map data (summary excerpt) ”.

米国特許出願公開第２０１７／０１７２３５号明細書U.S. Patent Application Publication No. 2017/017235 特開２０１４−１４２２７２号公報Japanese Unexamined Patent Publication No. 2014-142272

従来の確率フィルタの手法ではデッドレコニングでの見積もり誤差を信用して、誤差分散値を算出する仕組みになっている。そのため、デッドレコニングでの見積もり誤差が大きく誤っている、もしくはスリップなどの考慮されていない車両運動が発生した場合には、現実にそぐわない誤差楕円を出力することがあるという課題がある。このような予め設定された見積もり誤差であるデッドレコニングでの見積もり誤差自体に意図せず含まれる誤差を、モデル化誤差と呼ぶことにする。特許文献１では誤差楕円を基づいて走行制御を行うため、誤差楕円に含まれるモデル化誤差が大きいと実際の走行制御に支障をきたす可能性があり、このモデル化誤差を含めた誤差楕円を出力することにより、目標車速を制御したいという要望がある。また、モデル化誤差の発生要因の一つとなる車両運動は、積載の有無等の車両状態の変化によっても生じるが、この車両状態の変化は、特定の車両に固有に生じる。これに対し、特許文献２では、過去の他の車両の動作を基に算出したオフセットを用いて誤差楕円の補正を行うので、特定の車両に固有の車両運動が生じた場合にはオフセットに反映されず、新たに生じた誤差の検出が遅れるという課題が残る。 In the conventional probability filter method, the error variance value is calculated by trusting the estimation error in dead reckoning. Therefore, there is a problem that an error ellipse that does not match the reality may be output when the estimation error in dead reckoning is greatly incorrect or when a vehicle motion that is not considered such as slip occurs. An error that is unintentionally included in the dead reckoning estimation error itself, which is such a preset estimation error, is referred to as a modeling error. In Patent Document 1, since the running control is performed based on the error ellipse, if the modeling error included in the error ellipse is large, the actual running control may be hindered, and the error ellipse including this modeling error is output. There is a desire to control the target vehicle speed by doing so. In addition, vehicle motion, which is one of the causes of modeling error, is also caused by a change in the vehicle state such as the presence or absence of loading, and this change in the vehicle state is unique to a specific vehicle. On the other hand, in Patent Document 2, since the error ellipse is corrected by using the offset calculated based on the operation of other vehicles in the past, when the vehicle motion peculiar to a specific vehicle occurs, it is reflected in the offset. However, there remains the problem that the detection of the newly generated error is delayed.

本発明は上記課題に鑑みてなされたものであり、特定の車両に生じる予期せざる誤差の補正を行って誤差楕円を生成することを目的とする。 The present invention has been made in view of the above problems, and an object of the present invention is to correct an unexpected error occurring in a specific vehicle to generate an error ellipse.

上記課題を解決するために本発明は、グローバル座標系で表された移動体のＧＮＳＳ出力位置を第１時点及びそれよりも遅い第２時点の各時点において逐次計測するＧＮＳＳセンサと、前記移動体の運動量及び姿勢を検出する移動体センサの検出値を用いて、初回は計測始点に当該計測始点からの変位量を、次回からは前回算出した相対位置に当該相対位置からの変位量を累積加算して、前記計測始点からの前記移動体の相対位置を逐次更新するデッドレコニング装置と、前記移動体の速度、重量、前記移動体が走行する路面の勾配の少なくとも一つにより定義された移動体状態を検出する移動体状態センサと、の其々に接続された、前記移動体の位置算出装置であって、前記位置算出装置は、一時記憶装置を含む位置算出コントローラを備え、前記位置算出コントローラは、前記第１時点におけるＧＮＳＳ出力位置、及び前記第１時点において前記デッドレコニング装置が算出した相対位置を用いて前記第１時点における前記移動体の最尤位置を計算し、前記第１時点から前記第２時点までの間に前記デッドレコニング装置が算出した相対位置を前記第１時点における最尤位置に足し合わせて、前記第２時点における仮定デッドレコニング位置を算出し、前記第１時点から前記第２時点までの間に前記デッドレコニング装置が算出した相対位置を前記第１時点におけるＧＮＳＳ出力位置に足し合わせて、前記第２時点における適応デッドレコニング位置を算出し、前記第２時点におけるＧＮＳＳ出力位置と前記第２時点における適応デッドレコニング位置との差からなる適応デッドレコニング誤差を算出すると共に、前記第２時点における前記移動体状態センサからの検出値を基に移動体状態を決定し、予め一時記憶装置に記憶された移動体状態と適応デッドレコニング誤差の平均値及び分散値とを関係付けた誤差統計量データのうち、前記決定された移動体状態に応じた適応デッドレコニング誤差の平均値及び分散値は、新たに算出された適応デッドレコニング誤差を用いて再計算して更新し、前記第２時点における仮定デッドレコニング位置を中心とし仮定デッドレコニング誤差の分散値からなる誤差楕円に前記第２時点における仮定デッドレコニング位置から前記再計算された適応デッドレコニング誤差の平均値分ずれた位置を中心とし、前記再計算された適応デッドレコニング誤差の分散値からなる誤差楕円を加えて存在可能性範囲を算出し、前記存在可能性範囲内に前記第２時点におけるＧＮＳＳ出力位置が含まれている場合には、当該ＧＮＳＳ出力位置を用いて前記第２時点における最尤位置及び当該最尤位置を中心とする最尤誤差楕円を算出し、前記存在可能性範囲の外に前記第２時点におけるＧＮＳＳ出力位置が存在する場合には、前記第２時点における仮定デッドレコニング位置を中心とする最尤誤差楕円を算出し、前記最尤誤差楕円を前記移動体が存在する位置範囲として出力する、ことを特徴とする。 In order to solve the above problems, the present invention comprises a GNSS sensor that sequentially measures the GNSS output position of a moving object represented by a global coordinate system at each time point of a first time point and a second time point later than that, and the moving body. The displacement amount from the measurement start point is cumulatively added to the measurement start point at the first time, and the displacement amount from the relative position is cumulatively added to the relative position calculated last time from the next time using the detection value of the moving body sensor that detects the momentum and posture of. A moving body defined by at least one of a dead reckoning device that sequentially updates the relative position of the moving body from the measurement start point, the speed and weight of the moving body, and the slope of the road surface on which the moving body travels. A position calculation device for the moving body, which is connected to a moving body state sensor that detects a state, and the position calculation device includes a position calculation controller including a temporary storage device, and the position calculation controller. Calculates the most probable position of the moving body at the first time point using the GNSS output position at the first time point and the relative position calculated by the dead reckoning device at the first time point, and starts from the first time point. The relative position calculated by the dead reckoning device up to the second time point is added to the most probable position at the first time point to calculate the assumed dead reckoning position at the second time point, and from the first time point to the above. The relative position calculated by the dead reckoning device up to the second time point is added to the GNSS output position at the first time point to calculate the adaptive dead reckoning position at the second time point, and the GNSS output at the second time point is calculated. The adaptive dead reckoning error consisting of the difference between the position and the adaptive dead reckoning position at the second time point is calculated, and the moving body state is determined in advance based on the value detected from the moving body state sensor at the second time point. Of the error statistics data relating the moving body state stored in the temporary storage device and the average value and the dispersion value of the adaptive dead reckoning error, the average value of the adaptive dead reckoning error according to the determined moving body state. And the dispersion value is recalculated and updated using the newly calculated adaptive dead reckoning error, and the above-mentioned first is an error ellipse composed of the dispersion value of the assumed dead reckoning error centered on the assumed dead reckoning position at the second time point. An error ellipse consisting of the dispersion value of the recalculated adaptive dead reckoning error is added centering on the position deviated by the average value of the recalculated adaptive dead reckoning error from the assumed dead reckoning position at two time points. If the existence possibility range is calculated and the GNSS output position at the second time point is included in the existence possibility range, the maximum likelihood position at the second time point and the maximum likelihood position at the second time point are used. The maximum likelihood error error centered on the maximum likelihood position is calculated, and if the GNSS output position at the second time point exists outside the existence possibility range, the assumed dead reckoning position at the second time point is set as the center. It is characterized in that the maximum likelihood error ellipse to be calculated is calculated and the maximum likelihood error ellipse is output as a position range in which the moving body exists.

本発明によれば、特定の車両に生じる予期せざる誤差の補正を行って誤差楕円を生成することができる。上記した以外の目的、構成、効果については以下の説明で明らかにされる。 According to the present invention, it is possible to generate an error ellipse by correcting an unexpected error that occurs in a specific vehicle. Purposes, configurations, and effects other than those mentioned above will be clarified in the following explanations.

ダンプトラックの外観を示す概略図Schematic diagram showing the appearance of a dump truck ダンプトラックのハードウェア構成図Dump truck hardware configuration diagram 位置算出装置、デッドレコニング装置、及び自律走行制御装置の機能構成を示すブロック図A block diagram showing the functional configurations of a position calculation device, a dead reckoning device, and an autonomous driving control device. 位置算出装置の動作の概要を示すフローチャートFlowchart showing the outline of the operation of the position calculation device 存在可能性範囲の概念図Conceptual diagram of the range of existence 誤差楕円を用いた位置計測安定性判断処理の流れを示すフローチャートFlowchart showing the flow of position measurement stability judgment processing using an error ellipse 最尤位置及び最尤位置の誤差分散値の算出処理の流れを示すフローチャートFlowchart showing the flow of calculation processing of the maximum likelihood position and the error variance value of the maximum likelihood position 適応デッドレコニング誤差の概念図Conceptual diagram of adaptive dead reckoning error 適応デッドレコニング誤差更新処理の流れを示すフローチャートFlowchart showing the flow of adaptive dead reckoning error update processing 一時記憶部のデータ構成例を示す図The figure which shows the data structure example of a temporary storage part

以下、図面を参照して本発明の実施形態について説明する。全図を通じて同一の構成には同一符号を付け、重複説明を省略する。 Hereinafter, embodiments of the present invention will be described with reference to the drawings. The same components are designated by the same reference numerals throughout the drawings, and duplicate description is omitted.

以下、位置算出の対象となる移動体として、鉱山内を管制局からの指示に従って自律走行する自律走行ダンプトラック（以下「ダンプトラック」と略記する）を用いた例について説明する。 Hereinafter, an example will be described in which an autonomously traveling dump truck (hereinafter abbreviated as “dump truck”) that autonomously travels in the mine according to an instruction from the control station is used as a moving object for position calculation.

図１は、ダンプトラック１の外観を示す概略図である。図１に示すように、ダンプトラック１は、車体フレーム（ｖｅｈｉｃｌｅｆｒａｍｅ）２と、車体フレーム２上に起伏可能に設けられたベッセル３とを有する。更に車体フレーム２の前側上方に運転室４が設けられている。そして、車体フレーム２の下部には左右の前輪５及び後輪６が備えられる。 FIG. 1 is a schematic view showing the appearance of the dump truck 1. As shown in FIG. 1, the dump truck 1 has a vehicle frame 2 and a vessel 3 undulatingly provided on the vehicle frame 2. Further, a driver's cab 4 is provided above the front side of the vehicle body frame 2. Left and right front wheels 5 and rear wheels 6 are provided in the lower portion of the vehicle body frame 2.

ダンプトラック１の前部にはＧＮＳＳアンテナ７を備える。図１ではＧＮＳＳアンテナ７を一つ図示しているが、実際には、ダンプトラック１の前部に車幅方向に間隔を空けて二つ以上のＧＮＳＳアンテナ７が設置されている。ＧＮＳＳアンテナ７は、グローバル座標系で表されたダンプトラック１の絶対位置を逐次計測するＧＮＳＳセンサ１０（図２参照）の構成要素である。 A GNSS antenna 7 is provided at the front of the dump truck 1. Although one GNSS antenna 7 is shown in FIG. 1, two or more GNSS antennas 7 are actually installed at the front portion of the dump truck 1 at intervals in the vehicle width direction. The GNSS antenna 7 is a component of the GNSS sensor 10 (see FIG. 2) that sequentially measures the absolute position of the dump truck 1 represented by the global coordinate system.

更にダンプトラック１の運動量を示す検出値及びダンプトラック１の姿勢を示す検出値を用いて、初回は計測始点に当該計測始点からの変位量を、次回からは前回算出した相対位置に当該相対位置からの変位量を累積加算して、計測始点からのダンプトラック１の相対位置を逐次更新するデッドレコニング（以下「デッドレコニング」を「ＤＲ」と略記する）装置２１０を備える。 Furthermore, using the detected value indicating the momentum of the dump truck 1 and the detected value indicating the posture of the dump truck 1, the displacement amount from the measurement start point is set at the measurement start point for the first time, and the relative position is set at the relative position calculated last time from the next time. A dead reckoning device 210 (hereinafter, “dead reckoning” is abbreviated as “DR”) that sequentially updates the relative position of the dump truck 1 from the measurement start point by cumulatively adding the displacement amounts from the above.

ダンプトラック１は、ＧＮＳＳセンサ１０から出力される絶対座標系で表されたＧＮＳＳ出力位置、及びＤＲ装置２１０から出力される相対位置を基にダンプトラック１の自己位置を算出する位置算出装置１００と、加速装置や制動装置を含む走行駆動装置４００に対して位置算出装置１００から取得した自己位置データを用いて自律走行制御を行う自律走行制御装置３００と、を備える。 The dump truck 1 includes a position calculation device 100 that calculates the self-position of the dump truck 1 based on the GNSS output position represented by the absolute coordinate system output from the GNSS sensor 10 and the relative position output from the DR device 210. The vehicle includes an autonomous travel control device 300 that performs autonomous travel control using self-position data acquired from the position calculation device 100 with respect to the travel drive device 400 including an accelerator and a braking device.

図２は、ダンプトラック１のハードウェア構成図である。位置算出装置１００は、ＣＰＵ１０１、ＲＯＭ１０２、ＲＡＭ１０３、ＨＤＤ１０４、入力インターフェース（Ｉ／Ｆ）１０５、出力Ｉ／Ｆ１０６を含みこれらがバス１０７を介して互いに接続されたコンピュータ（位置算出コントローラ）を用いて構成される。ＲＡＭ１０３又はＨＤＤ１０４は、一時記憶装置に相当する。 FIG. 2 is a hardware configuration diagram of the dump truck 1. The position calculation device 100 includes a CPU 101, a ROM 102, a RAM 103, an HDD 104, an input interface (I / F) 105, and an output I / F 106, and is configured by using a computer (position calculation controller) in which these are connected to each other via a bus 107. Will be done. The RAM 103 or HDD 104 corresponds to a temporary storage device.

ＤＲ装置２１０は、コンピュータ（ＤＲコントローラ）を用いて構成される。ＤＲコントローラの入力段には、ダンプトラック１の運動量や姿勢を示すパラメータを計測するセンサ、操舵角センサ３０の其々が接続される。 The DR device 210 is configured by using a computer (DR controller). A sensor that measures parameters indicating the momentum and posture of the dump truck 1 and a steering angle sensor 30 are connected to the input stage of the DR controller.

運動量を検出するセンサは、具体的には、車輪回転数を基に速度を計測する速度センサ２０１、ダンプトラック１のヨー角、ロール角、ピッチ角などの各軸の角度（姿勢）を計測もしくは算出する姿勢センサ２０２と、ダンプトラック１の加速度を計測する加速度センサ２０３、ダンプトラック１の角速度を計測する角速度センサ２０４である。 Specifically, the sensor that detects the amount of momentum measures or measures the angle (attitude) of each axis such as the speed sensor 201 that measures the speed based on the wheel rotation speed, the yaw angle, the roll angle, and the pitch angle of the dump track 1. The attitude sensor 202 to be calculated, the acceleration sensor 203 for measuring the acceleration of the dump track 1, and the angular velocity sensor 204 for measuring the angular velocity of the dump track 1.

積載状態センサ２０（重量センサに相当する）は、ベッセル３の積載状態を検出するセンサである。操舵角センサ３０は、ダンプトラック１の直進旋回状態を検出するセンサである。勾配演算器４０は、車輪速及び駆動輪に加わるトルクを基に、ダンプトラック１の走行路の勾配を演算する演算器である。 The load state sensor 20 (corresponding to a weight sensor) is a sensor that detects the load state of the vessel 3. The steering angle sensor 30 is a sensor that detects the straight-ahead turning state of the dump truck 1. The gradient calculator 40 is a calculator that calculates the gradient of the traveling path of the dump truck 1 based on the wheel speed and the torque applied to the drive wheels.

位置算出コントローラの入力Ｉ／Ｆ１０５はＤＲ装置２１０に接続され、出力Ｉ／Ｆ１０６は自律走行制御装置３００に接続される。自律走行制御装置３００は位置算出装置１００の位置算出結果の出力先となる外部装置に相当する。更に自律走行制御装置３００の出力段には、加速装置及び制動装置を含む走行駆動装置４００が接続される。ＤＲ装置２１０及び自律走行制御装置３００を構成するコンピュータの具体的なハードウェア構成は位置算出装置１００と同じである。 The input I / F 105 of the position calculation controller is connected to the DR device 210, and the output I / F 106 is connected to the autonomous travel control device 300. The autonomous travel control device 300 corresponds to an external device that is an output destination of the position calculation result of the position calculation device 100. Further, a travel drive device 400 including an accelerator and a braking device is connected to the output stage of the autonomous travel control device 300. The specific hardware configuration of the computer constituting the DR device 210 and the autonomous travel control device 300 is the same as that of the position calculation device 100.

加速度センサ２０３に代えて速度センサ２０１からの出力に対して時間微分演算を行い、加速度を演算で求める加速度演算器を備えてもよい。 Instead of the acceleration sensor 203, an acceleration calculator may be provided which performs a time derivative calculation on the output from the speed sensor 201 and obtains the acceleration by the calculation.

図３は、位置算出装置１００、ＤＲ装置２１０、及び自律走行制御装置３００の機能構成を示すブロック図である。 FIG. 3 is a block diagram showing the functional configurations of the position calculation device 100, the DR device 210, and the autonomous travel control device 300.

ＤＲ装置２１０は、速度センサ２０１、姿勢センサ２０２、加速度センサ２０３、及び角速度センサ２０４、及び操舵角センサ３０からの出力を基に、ダンプトラック１の各時点の次の時刻における位置を求めて更新するＤＲ位置演算部２１１を含む。 The DR device 210 obtains and updates the position of the dump track 1 at the next time at each time point based on the outputs from the speed sensor 201, the attitude sensor 202, the acceleration sensor 203, the angular velocity sensor 204, and the steering angle sensor 30. The DR position calculation unit 211 is included.

ＧＮＳＳセンサ１０はＧＮＳＳ出力位置及び位置計測ステータスを含むＧＮＳＳセンサデータを位置算出装置１００へ出力し、ＤＲ装置２１０は、ＤＲ位置演算部２１１が算出した更新位置データを位置算出装置１００へ出力する。また、本実施形態では速度センサ２０１、加速度センサ２０３、積載状態センサ２０、操舵角センサ３０の検出値をＤＲ装置２１０が位置算出装置１００にも出力するものとする。 The GNSS sensor 10 outputs the GNSS sensor data including the GNSS output position and the position measurement status to the position calculation device 100, and the DR device 210 outputs the update position data calculated by the DR position calculation unit 211 to the position calculation device 100. Further, in the present embodiment, it is assumed that the DR device 210 also outputs the detection values of the speed sensor 201, the acceleration sensor 203, the load state sensor 20, and the steering angle sensor 30 to the position calculation device 100.

位置計測ステータスは、その時刻においてＧＮＳＳセンサ１０からどのような精度で位置が算出されているかを示すデータである。例えば「４」なら公称誤差０．０２ｍのＲＴＫ測位、「３」なら公称誤差０．５ｍ、「１」なら公称誤差５ｍの単独測位、「０」なら非測位（測位データ無し）などである。ただし、位置計測ステータスは、上記に限らず位置の計測有無が判断できるデータであれば、ユーザーが任意に定義してよい。 The position measurement status is data indicating with what accuracy the position is calculated from the GNSS sensor 10 at that time. For example, "4" means RTK positioning with a nominal error of 0.02 m, "3" means a nominal error of 0.5 m, "1" means a single positioning with a nominal error of 5 m, and "0" means non-positioning (no positioning data). However, the position measurement status is not limited to the above, and may be arbitrarily defined by the user as long as the data can determine whether or not the position is measured.

位置算出装置１００は、速度センサ２０１、加速度センサ２０３、積載状態センサ２０、操舵角センサ３０、及び勾配演算器４０の出力を基に車両状態を決定する車両状態決定部１３２、ＧＮＳＳセンサ１０の計測結果の安定性を判断する位置計測安定性判断部１１４、確率フィルタを用いて自車位置を推定する最尤位置演算部１１５、過去のある時点から最新のサンプリング時点までに算出された推定位置の誤差分散値を算出する仮定ＤＲ誤差算出部１１２、一時記憶部１３１、適応ＤＲ誤差算出部１４２、存在可能性範囲算出部１１３、仮定ＤＲ位置算出部１１１、及び適応ＤＲ誤差パラメータ算出部１４１を含む。 The position calculation device 100 measures the vehicle state determination unit 132 and the GNSS sensor 10 that determine the vehicle state based on the outputs of the speed sensor 201, the acceleration sensor 203, the load state sensor 20, the steering angle sensor 30, and the gradient calculator 40. Position measurement stability judgment unit 114 that judges the stability of the result, maximum likelihood position calculation unit 115 that estimates the vehicle position using a probability filter, estimated position calculated from a certain point in the past to the latest sampling time Includes a hypothetical DR error calculation unit 112 for calculating an error dispersion value, a temporary storage unit 131, an adaptive DR error calculation unit 142, an existence possibility range calculation unit 113, a hypothetical DR position calculation unit 111, and an adaptive DR error parameter calculation unit 141. ..

最尤位置演算部１１５は、ＧＮＳＳセンサ１０及びＤＲ装置２１０の出力を用いて確率フィルタ処理等を実行することにより、ダンプトラック１の現在の確率的な位置の分布を推定する。 The maximum likelihood position calculation unit 115 estimates the distribution of the current stochastic position of the dump truck 1 by executing a stochastic filter process or the like using the outputs of the GNSS sensor 10 and the DR device 210.

最尤位置演算部１１５が推定する確率的な位置の分布は、最も存在可能性が高い位置の絶対座標を中心とし、ＧＮＳＳセンサ１０とＤＲ装置２１０の演算結果に含まれる仮定ＤＲ誤差算出部１１２により計算される誤差分散である。 The distribution of stochastic positions estimated by the maximum likelihood position calculation unit 115 is centered on the absolute coordinates of the positions most likely to exist, and the assumed DR error calculation unit 112 included in the calculation results of the GNSS sensor 10 and the DR device 210. The error variance calculated by.

適応ＤＲ誤差算出部１４２は、ＧＮＳＳセンサ１０の位置の第１時点と第２時点での位置の差分とその間のＤＲ装置２１０からの出力を比較することによりＤＲによって生じるモデル化誤差を適応ＤＲ誤差の平均値と誤差分散値として計算する。平均値及び誤差分散値は統計量の一例である。 The adaptive DR error calculation unit 142 adjusts the modeling error caused by DR by comparing the difference between the positions of the GNSS sensor 10 at the first and second time points and the output from the DR device 210 between them. It is calculated as the average value and the error variance value of. The mean and error variance values are examples of statistics.

一時記憶部１３１は、車両状態や運動の検出値等を一時的に記憶する。 The temporary storage unit 131 temporarily stores the vehicle state, the detected value of motion, and the like.

存在可能性範囲算出部１１３は、仮定ＤＲ誤差算出部１１２の出力するＤＲによる誤差分散値（仮定ＤＲ位置の誤差分散）および適応ＤＲ誤差算出部１４２が出力する適応ＤＲ誤差の誤差分散値からダンプトラック１の存在可能性範囲である、仮定ＤＲ位置での誤差楕円と適応ＤＲ誤差の誤差楕円を算出する。 The existence possibility range calculation unit 113 dumps from the error dispersion value (error variance of the hypothetical DR position) by DR output by the hypothetical DR error calculation unit 112 and the error variance value of the adaptive DR error output by the adaptive DR error calculation unit 142. The error ellipse at the assumed DR position and the error ellipse of the adaptive DR error, which are the range of existence of track 1, are calculated.

自律走行制御装置３００は、予め定められた目標軌道と最尤位置演算部１１５から取得した誤差楕円の最外縁との距離に応じて、目標軌道への復帰速度を算出する目標速度算出部３０１と、走行駆動装置４００に対して目標速度に従って目標軌道に復帰させるための制御を実行する速度制御部３０２と、を備える。 The autonomous travel control device 300 includes a target speed calculation unit 301 that calculates the return speed to the target trajectory according to the distance between the predetermined target trajectory and the outermost edge of the error ellipse acquired from the most likely position calculation unit 115. The traveling drive device 400 is provided with a speed control unit 302 that executes control for returning to the target trajectory according to the target speed.

上記各機能ブロックは、図２で示すハードウェアと上記各機能ブロックの機能を実現するソフトウェアとが協働して構成してもよいし、各機能ブロックを実現する回路を用いて構成してもよい。 Each of the above functional blocks may be configured by the hardware shown in FIG. 2 and the software that realizes the functions of each of the above functional blocks in cooperation with each other, or may be configured by using a circuit that realizes each functional block. Good.

次に図４を参照してダンプトラック１における位置算出装置１００の動作について説明する。図４は、ダンプトラック１の動作の概要を示すフローチャートである。以下の処理は、位置算出装置１００が第２時点において各センサから出力された検出値を用いて第２時点におけるダンプトラック１の存在可能性範囲を算出し、その算出過程において第２時点よりも前の第１時点で算出した位置との比較を行い、ＤＲでの見積もり誤差が適正な範囲内にあるかを評価する例を示す。 Next, the operation of the position calculation device 100 on the dump truck 1 will be described with reference to FIG. FIG. 4 is a flowchart showing an outline of the operation of the dump truck 1. In the following processing, the position calculation device 100 calculates the existence possibility range of the dump truck 1 at the second time point using the detection value output from each sensor at the second time point, and in the calculation process, it is more than the second time point. An example of evaluating whether or not the estimation error in DR is within an appropriate range by comparing with the position calculated at the first time point before is shown.

位置算出装置１００はＧＮＳＳセンサ１０からＸ（ｔ）時点（第１時点）の出力を取得する（Ｓ１００）。 The position calculation device 100 acquires the output at the X (t) time point (first time point) from the GNSS sensor 10 (S100).

ＤＲ装置２１０と位置算出装置１００は、Ｘ（ｔ）時点以降から車体センサ、より具体的には、速度センサ２０１、姿勢センサ２０２、加速度センサ２０３、角速度センサ２０４及び操舵角センサ３０から車体センサ１サンプリング周期後の検出値を取得する（Ｓ１０１）。 The DR device 210 and the position calculation device 100 are a vehicle body sensor from the time of X (t), more specifically, a vehicle body sensor 1 from the speed sensor 201, the attitude sensor 202, the acceleration sensor 203, the angular speed sensor 204, and the steering angle sensor 30. The detected value after the sampling cycle is acquired (S101).

また速度センサ２０１、姿勢センサ２０２、加速度センサ２０３、角速度センサ２０４、及び操舵角センサ３０は時刻同期がとれており、少なくともＧＮＳＳセンサ１０の計測間隔と同じか、短い計測間隔で同期をとりながら各センサが検出結果を出力する。 Further, the speed sensor 201, the attitude sensor 202, the acceleration sensor 203, the angular velocity sensor 204, and the steering angle sensor 30 are time-synchronized, and are synchronized with each other at least at the same measurement interval as the GNSS sensor 10 or at a short measurement interval. The sensor outputs the detection result.

ＤＲ位置演算部２１１は、ステップＳ１０１で取得した各センサの出力を用いて、第１時点での位置からの位置変位量Δｘを算出する（Ｓ１０２）。 The DR position calculation unit 211 calculates the position displacement amount Δx from the position at the first time point using the output of each sensor acquired in step S101 (S102).

ＤＲ位置演算部２１１は、各センサの検出値に基づいて１サンプル時間（Δｔ）の更新量δｘを算出し、第１時点からの位置変位量Δｘを計算する。 The DR position calculation unit 211 calculates the update amount δx for one sample time (Δt) based on the detection value of each sensor, and calculates the position displacement amount Δx from the first time point.

更新量δｘは車体座標系から絶対座標系への変換行列Ｒθが決まった場合は、ＤＲ位置演算部２１１は下式（１）により算出する。

When the conversion matrix Rθ from the vehicle body coordinate system to the absolute coordinate system is determined, the update amount δx is calculated by the DR position calculation unit 211 by the following equation (1).

また、加速度センサ２０３が存在しない場合には、ＤＲ位置演算部２１１は下式（２）により更新量δｘを算出する。

If the acceleration sensor 203 does not exist, the DR position calculation unit 211 calculates the update amount δx by the following equation (2).

第１時点からの位置変位量Δｘは下式（３）のように算出する。

The position displacement amount Δx from the first time point is calculated by the following equation (3).

次に仮定ＤＲ位置算出部１１１が第１時点での絶対位置ｘ（ｔ）に、ステップ１０２で得た位置変位量Δｘ（ｔ＋１）を足し合わせて仮定ＤＲ位置ｘ（ｔ+１）ｔｉｌｄｅを下式（４）により算出する（Ｓ１０３）。

Next, the hypothetical DR position calculation unit 111 adds the position displacement amount Δx (t + 1) obtained in step 102 to the absolute position x (t) at the first time point to lower the hypothetical DR position x (t + 1) tilde. It is calculated by the formula (4) (S103).

車両状態決定部１３２は、速度センサ２０１、加速度センサ２０３、積載状態センサ２０、操舵角センサ３０、及び勾配演算器４０からの出力を基に車両状態を決定し、決定した車両状態に応じて、１サンプル時間当たりの適応ＤＲ誤差の平均値と分散値を一時記憶部１３１から取得する（Ｓ１０４）。 The vehicle state determination unit 132 determines the vehicle state based on the outputs from the speed sensor 201, the acceleration sensor 203, the load state sensor 20, the steering angle sensor 30, and the gradient calculator 40, and determines the vehicle state according to the determined vehicle state. The average value and the variance value of the adaptive DR error per sample time are acquired from the temporary storage unit 131 (S104).

車両状態は、積荷もしくは空荷を示す積載状態と、加速中もしくは減速中もしくは一定速度走行中を示す速度状態と、上り勾配走行中もしくは下り勾配走行中もしくは平坦地走行中を示す勾配状態と、直進走行中もしくは右旋回走行中もしくは左旋回走行中を示す直進旋回状態と、車輪滑り有りもしくは車輪滑り無しを示す車輪すべり状態と、のうちの少なくとも一つ又は少なくとも二つ以上の任意の組み合わせを用いて定義される。 The vehicle state includes a loaded state indicating a loaded or empty load, a speed state indicating accelerating, decelerating, or running at a constant speed, and a gradient state indicating uphill running, downhill running, or flat ground running. Any combination of at least one or at least two or more of a straight-ahead turning state indicating a straight-ahead running, a right-turning running, or a left-turning running, and a wheel-sliding state indicating with or without wheel slippage. Is defined using.

車両状態決定部１３２は、各センサからの検出値を変数に持つ関数から、車両状態Λｉ（ｉ＝１、２、・・・、ｋ）である確率Ｐ（Λｉ）を、下式（５）を用いて決定する。

The vehicle state determination unit 132 calculates the probability P (Λi) of the vehicle state Λi (i = 1, 2, ..., K) from the function having the value detected from each sensor as a variable in the following equation (5). To determine using.

関数ｆ_ｉ（＊）は事前に決められているものとし、単一の式でなく複数の式で構成されていても良い。また、入力値と検出値から機械学習を用いて車両状態を学習してもよい。また、Ｍは自重であるが、積荷状態と空荷状態を区別するだけのステータス値でも良い。車両状態決定部１３２は、算出された確率が最も高くなる状態を現在の車両状態として決定する。車両状態決定部１３２は、決定した車両状態を仮定ＤＲ誤差算出部１１２、及び適応ＤＲ誤差パラメータ算出部１４１の両方に出力する。 Function f _{i (*)} is assumed to be determined in advance, or may be composed of a plurality of expression instead of a single formula. Further, the vehicle state may be learned from the input value and the detected value by using machine learning. Further, although M is its own weight, it may be a status value that only distinguishes between a loaded state and an empty state. The vehicle state determination unit 132 determines the state with the highest calculated probability as the current vehicle state. The vehicle state determination unit 132 outputs the determined vehicle state to both the hypothetical DR error calculation unit 112 and the adaptive DR error parameter calculation unit 141.

次に仮定ＤＲ誤差算出部１１２では、ステップ１０３で算出した仮定ＤＲ位置における誤差分散値を算出する（Ｓ１０５）。仮定ＤＲ位置の誤差分散値Σ（ｔ＋１）は、１サンプル時間前の仮定ＤＲ位置の誤差分散値Σ（ｔ）に、予め設定されている１サンプル時間での更新量の誤差分散値Ｗ、および車両状態決定部１３２が決定する車両状態に応じた１サンプリング時間当たりの適応ＤＲ誤差の分散値Ｅ［ε^２（ｔ）］を足した値となる。仮定ＤＲ誤差算出部１１２は仮定ＤＲ位置の誤差分散値を以下の式（６）を用いて算出する。仮定ＤＲ誤差算出部１１２は、算出した仮定ＤＲ位置の誤差分散値Σ（ｔ＋１）を存在可能性範囲算出部１１３に出力し、ステップＳ１０７へ進む。

Next, the hypothetical DR error calculation unit 112 calculates the error variance value at the hypothetical DR position calculated in step 103 (S105). The error variance value Σ (t + 1) of the assumed DR position is the error variance value W of the update amount in one sample time preset to the error variance value Σ (t) of the assumed DR position one sample time ago, and ^{It is the sum of the variance value E [ε 2} (t)] of the adaptive DR error per sampling time according to the vehicle condition determined by the vehicle condition determination unit 132. The hypothetical DR error calculation unit 112 calculates the error variance value of the hypothetical DR position using the following equation (6). The hypothetical DR error calculation unit 112 outputs the calculated error variance value Σ (t + 1) of the hypothetical DR position to the existence possibility range calculation unit 113, and proceeds to step S107.

一方、適応ＤＲ誤差算出部１４２は、Ｓ１０３で算出した仮定ＤＲ位置における適応ＤＲ誤差の平均値の蓄積量とその誤差分散値を算出する（Ｓ１０６）。この適応ＤＲ誤差の蓄積量や誤差分散値はＳ１０４で得られる適応ＤＲ誤差の平均値Ｅ［ε（ｔ）］と分散値Ｅ［ε^２（ｔ）］から算出する。この処理を適応ＤＲ誤差算出処理という。この処理内容の詳細は後述する。適応ＤＲ誤差算出部１４２は、算出した適応ＤＲ誤差を存在可能性範囲算出部１１３に出力し、ステップＳ１０７へ進む。 On the other hand, the adaptive DR error calculation unit 142 calculates the accumulated amount of the average value of the adaptive DR error at the hypothetical DR position calculated in S103 and the error variance value thereof (S106). The accumulated amount of the adaptive DR error and the error variance value are calculated from the average value E [ε (t)] and the variance value E [ε ² (t)] of the adaptive DR error obtained in S104. This process is called an adaptive DR error calculation process. The details of this processing content will be described later. The adaptive DR error calculation unit 142 outputs the calculated adaptive DR error to the existence possibility range calculation unit 113, and proceeds to step S107.

存在可能性範囲算出部１１３は、仮定ＤＲ位置とその誤差分散値、および適応ＤＲ誤差の平均値と誤差分散値から、ＧＮＳＳセンサ１０の出力する位置が存在する可能性のある位置を算出する（Ｓ１０７）。本ステップの処理を存在可能性範囲算出処理という。この処理内容の詳細は後述する。存在可能性範囲算出部１１３は、上記処理の結果、次の位置計測安定性判断部１１４で用いる、仮定ＤＲの誤差楕円と適応ＤＲの誤差楕円を算出する。 The existence possibility range calculation unit 113 calculates the position where the output position of the GNSS sensor 10 may exist from the assumed DR position and its error variance value, and the average value and the error variance value of the adaptive DR error ( S107). The process of this step is called the existence possibility range calculation process. The details of this processing content will be described later. As a result of the above processing, the existence possibility range calculation unit 113 calculates the error ellipse of the hypothetical DR and the error ellipse of the adaptive DR to be used in the next position measurement stability determination unit 114.

位置計測安定性判断部１１４は、ＧＮＳＳセンサ１０からの出力の安定性を判断する（Ｓ１０８）。本ステップの処理を位置計測安定性判断処理という。この処理内容の詳述は後述する。位置計測安定性判断部１１４は、上記処理の結果、ＧＮＳＳセンサ１０からの出力が、予め定めた安定性を満たすと判断した場合には、ステップＳ１０１においてＧＮＳＳセンサ１０から取得したＧＮＳＳセンサデータに値「１」からなる安定性フラグを付加し、上記安定性を満たさないと判断した場合には、ＧＮＳＳセンサ１０から取得したＧＮＳＳセンサデータに値「０」からなる安定性フラグを付加する。その後、ステップＳ１０９へと続く。 The position measurement stability determination unit 114 determines the stability of the output from the GNSS sensor 10 (S108). The process of this step is called position measurement stability judgment process. Details of this processing content will be described later. When the position measurement stability determination unit 114 determines that the output from the GNSS sensor 10 satisfies the predetermined stability as a result of the above processing, the value is added to the GNSS sensor data acquired from the GNSS sensor 10 in step S101. When the stability flag consisting of "1" is added and it is determined that the above stability is not satisfied, the stability flag consisting of the value "0" is added to the GNSS sensor data acquired from the GNSS sensor 10. After that, the process continues to step S109.

最尤位置演算部１１５は、確率フィルタを用いてダンプトラック１の最尤位置及び誤差分散値を演算し、出力Ｉ／Ｆ１０６を通して、自律走行制御装置３００へ自車位置データを出力する（Ｓ１０９）。 The maximum likelihood position calculation unit 115 calculates the maximum likelihood position and the error variance value of the dump truck 1 using the probability filter, and outputs the vehicle position data to the autonomous travel control device 300 through the output I / F 106 (S109). ..

確率フィルタの代表的な方法としてカルマンフィルタや、パーティクルフィルタなどを挙げることができる。本実施形態では、ＧＮＳＳセンサ１０及びＤＲ装置２１０においてそれぞれ算出した誤差分散から、カルマンフィルタにより最尤位置及び誤差分散値を求める。最尤位置演算部１１５が実行する最尤位置及びその誤差分散値の算出処理は後述する。 A Kalman filter, a particle filter, and the like can be mentioned as typical methods of the probability filter. In the present embodiment, the maximum likelihood position and the error variance value are obtained by the Kalman filter from the error variances calculated by the GNSS sensor 10 and the DR device 210, respectively. The calculation process of the maximum likelihood position and its error variance value executed by the maximum likelihood position calculation unit 115 will be described later.

次に、適応ＤＲ誤差パラメータ算出部１４１は、ＧＮＳＳセンサデータに付加された安定性フラグを参照し、安定性フラグの値が「１」であれば（Ｓ１１０／Ｙｅｓ）、適応ＤＲ誤差更新処理を実行する（Ｓ１１１）。適応ＤＲ誤差更新処理の詳細は後述する。 Next, the adaptive DR error parameter calculation unit 141 refers to the stability flag added to the GNSS sensor data, and if the value of the stability flag is "1" (S110 / Yes), the adaptive DR error update process is performed. Execute (S111). The details of the adaptive DR error update process will be described later.

一方、適応ＤＲ誤差算出部１４２は、ＧＮＳＳセンサデータに付加された安定性フラグの値が「０」であれば（Ｓ１１０／Ｎｏ）、適応ＤＲ誤差更新処理を実行することなく、一時記憶部１３１に適応ＤＲ誤差の平均値及び誤差分散値を書きこむ、所謂データ保持処理だけを実行する（Ｓ１１２）。 On the other hand, if the value of the stability flag added to the GNSS sensor data is "0" (S110 / No), the adaptive DR error calculation unit 142 does not execute the adaptive DR error update process, and the temporary storage unit 131. Only the so-called data holding process, in which the average value and the error variance value of the adaptive DR error are written in, is executed (S112).

次に、適応ＤＲ誤差パラメータ算出部１４１は、１ステップ前の車両状態（第１移動体状態に相当する）と現在の車両状態（第２移動体状態に相当する）の遷移確率Ｐを更新し、一時記憶部１３１へ保存する（Ｓ１１３）。この際、複数の車両状態の遷移確率が一定以上の場合、それらの車両状態は同一の車両状態であるとみなし、適応ＤＲ誤差の平均値および誤差分散値を融合する。この処理を車両状態更新処理といい、詳細は後述する。 Next, the adaptive DR error parameter calculation unit 141 updates the transition probability P between the vehicle state one step before (corresponding to the first moving body state) and the current vehicle state (corresponding to the second moving body state). , Saved in the temporary storage unit 131 (S113). At this time, when the transition probabilities of a plurality of vehicle states are equal to or higher than a certain value, those vehicle states are regarded as the same vehicle state, and the average value and the error variance value of the adaptive DR error are fused. This process is called a vehicle state update process, and details will be described later.

位置算出装置１００がＧＮＳＳセンサ１０からＸ（ｔ＋１）時点の出力を受け取らなければ（Ｓ１１４／Ｎｏ）、第１時点Ｘ（ｔ）を計測始点として維持したまま、Ｓ１０１〜Ｓ１１４の処理をＧＮＳＳサンプリング１周期分ループして処理し続ける（Ｓ１１５）。 If the position calculation device 100 does not receive the output at the time of X (t + 1) from the GNSS sensor 10 (S114 / No), the processing of S101 to S114 is performed by GNSS sampling 1 while maintaining the first time point X (t) as the measurement start point. The process is continued by looping for a cycle (S115).

一方、位置算出装置１００がＧＮＳＳセンサからＸ（ｔ＋１）時点の出力を受け取ると（Ｓ１１４／Ｙｅｓ）と、計測始点をＸ（ｔ＋１）に更新してＳ１０１からＳ１１４までの処理を繰り返す（Ｓ１１６）。 On the other hand, when the position calculation device 100 receives the output at the time of X (t + 1) from the GNSS sensor (S114 / Yes), the measurement start point is updated to X (t + 1) and the processes from S101 to S114 are repeated (S116).

＜適応ＤＲ誤差算出処理＞
次に仮定ＤＲ位置における適応ＤＲ誤差の蓄積量とその誤差分散値の算出について説明する。 <Adaptive DR error calculation process>
Next, the accumulated amount of the adaptive DR error at the assumed DR position and the calculation of the error variance value will be described.

適応ＤＲ誤差算出部１４２は、Ｓ１０４にて決定した現在の車両状態における適応ＤＲ誤差の平均値と分散値を取得する。 The adaptive DR error calculation unit 142 acquires the average value and the variance value of the adaptive DR error in the current vehicle state determined in S104.

次に適応ＤＲ誤差算出部１４２は、第１時点からのＤＲ維持時間を算出する。ＤＲ維持時間は仮定ＤＲ位置の更新サンプル数ｎとし、適応ＤＲ誤差算出部１４２が仮定ＤＲ位置算出部１１１が起動するたびに起動しているとすれば、単純に前回値ｎ＿に１を足せばよい。よって、更新サンプル数ｎは下式（７）で表せる。

Next, the adaptive DR error calculation unit 142 calculates the DR maintenance time from the first time point. If the DR maintenance time is the number of updated samples n of the assumed DR position and the adaptive DR error calculation unit 142 is activated every time the assumed DR position calculation unit 111 is activated, simply add 1 to the previous value n_. Good. Therefore, the number of updated samples n can be expressed by the following equation (7).

また、更新サンプル数ｎは車両状態によって変わるものとする。つまり、前サンプル時刻における車両状態と今回の車両状態が違った場合、ｎ＿＝０として上記の算出を実行するものとする。 Further, it is assumed that the number of updated samples n changes depending on the vehicle condition. That is, when the vehicle state at the previous sample time and the vehicle state at this time are different, the above calculation is executed with n_ = 0.

適応ＤＲ誤差算出部１４２は、ｎサンプル時間後に仮定ＤＲ位置の更新毎にＤＲにより生じる誤差の平均値を下式（８）、分散値を下式（９）により逐次算出する。

The adaptive DR error calculation unit 142 sequentially calculates the average value of the error caused by DR each time the assumed DR position is updated after n sample times by the following equation (8) and the variance value by the following equation (9).

これらの処理が終了すれば、適応ＤＲ誤差算出部１４２の適応ＤＲ誤差算出処理を終了する。 When these processes are completed, the adaptive DR error calculation process of the adaptive DR error calculation unit 142 is completed.

＜存在可能性範囲算出処理＞
存在可能性範囲算出部１１３は、仮定ＤＲ誤差と適応ＤＲ誤差を考慮した車両位置の存在する可能性がある範囲を算出する（Ｓ１０７）。存在可能性範囲は仮定ＤＲ位置算出部１１１にて算出される仮定ＤＲ位置（暫定的な位置である）及びその誤差分散と、適応ＤＲ誤差算出部１４２にて算出する適応ＤＲ誤差の分散値及び平均値を用いて算出することができる。 <Existence possibility range calculation process>
The existence possibility range calculation unit 113 calculates a range in which the vehicle position may exist in consideration of the assumed DR error and the adaptive DR error (S107). The existence possibility range includes the hypothetical DR position (provisional position) calculated by the hypothetical DR position calculation unit 111 and its error variance, and the variance value of the adaptive DR error calculated by the adaptive DR error calculation unit 142. It can be calculated using the average value.

具体的には、仮定ＤＲ位置算出部１１１にて算出される仮定ＤＲ位置を（ｘｅ、ｙｅ）ｔ、仮定ＤＲ位置での誤差分散をΣ、事前に決定されているマハラノビス距離をｄとすると、仮定ＤＲ位置の存在範囲は下式（１０）に示す楕円方程式範囲内である。

Specifically, assuming that the hypothetical DR position calculated by the hypothetical DR position calculation unit 111 is (xe, yes) t, the error variance at the hypothetical DR position is Σ, and the predetermined Mahalanobis distance is d. The existing range of the hypothetical DR position is within the range of the elliptical equation shown in the following equation (10).

同様に、適応ＤＲ誤差の平均値をＥ［ε（ｔ）］＝（ｘｄ、ｙｄ）ｔ、分散値をＥ［ε^２（ｔ）］として、適応ＤＲ誤差の存在範囲は下式（１１）の楕円方程式範囲内となる。

Similarly, assuming that the average value of the adaptive DR error is E [ε (t)] = (xd, yd) t and the variance value is E [ε ² (t)], the range of existence of the adaptive DR error is the following equation (11). Is within the range of the elliptical equation of.

よって、存在可能性範囲算出部１１３は、式（１０）から得られる仮定ＤＲ位置の存在範囲（誤差楕円）に、式（１１）から得られる適応ＤＲ誤差の存在範囲（誤差楕円）を加えた範囲を存在可能範囲として算出する。すなわち、存在可能性範囲は、これら二つの楕円の範囲となる。 Therefore, the existence possibility range calculation unit 113 adds the existence range (error ellipse) of the adaptive DR error obtained from the equation (11) to the existence range (error ellipse) of the assumed DR position obtained from the equation (10). Calculate the range as the possible range. That is, the existence range is the range of these two ellipses.

図５に存在可能性範囲の概念図を示す。点１００１が仮定ＤＲ位置、誤差楕円１００２が式（１０）により求められる仮定ＤＲ誤差の誤差楕円である。誤差楕円１００２は仮定ＤＲ位置１００１を中心としている。この誤差楕円１００２は楕円内部に、マハラノビス距離ｄで規定された確率で位置が存在することを示している。また、式（１１）で表わされる適応ＤＲ誤差の誤差楕円は、適応ＤＲ誤差の平均値［ε（ｔ）］をベクトル１００３で表せば、仮定ＤＲ位置を始点として、適応ＤＲ誤差の平均値［ε（ｔ）］＝（ｘｄ、ｙｄ）ｔで表わされるベクトルの終点としたベクトルとなり、この終点が適応ＤＲ誤差範囲を示す誤差楕円１００５の中心１００４であることを示している。つまり、適応ＤＲ誤差の誤差楕円１００５の中心１００４は仮定ＤＲ位置から適応ＤＲ誤差の平均値分だけずらした点を意味する。また、適応ＤＲ誤差の誤差分散Ｅ［ε^２（ｔ）］とマハラノビス距離ｄにより、適応ＤＲ誤差の誤差楕円の中心１００４からの適応ＤＲ誤差の広がりを持つ楕円が式（１１）により求まり、これが適応ＤＲ誤差の誤差楕円１００５で表わされる。ダンプトラック１の位置の存在範囲は、仮定ＤＲ誤差の誤差楕円１００２及び適応ＤＲ誤差の誤差楕円１００５のいずれかもしくは両方の内側にあると考えればよい。 FIG. 5 shows a conceptual diagram of the existence possibility range. The point 1001 is the assumed DR position, and the error ellipse 1002 is the error ellipse of the assumed DR error obtained by the equation (10). The error ellipse 1002 is centered on the hypothetical DR position 1001. The error ellipse 1002 indicates that a position exists inside the ellipse with a probability defined by the Mahalanobis distance d. Further, for the error ellipse of the adaptive DR error represented by the equation (11), if the average value [ε (t)] of the adaptive DR error is represented by the vector 1003, the average value of the adaptive DR error [ ε (t)] = (xd, yd) t is the end point of the vector, which indicates that this end point is the center 1004 of the error ellipse 1005 indicating the adaptive DR error range. That is, the center 1004 of the error ellipse 1005 of the adaptive DR error means a point shifted from the assumed DR position by the average value of the adaptive DR error. Further, from the error variance E [ε ² (t)] of the adaptive DR error and the Mahalanobis distance d, an ellipse having a spread of the adaptive DR error from the center 1004 of the error ellipse of the adaptive DR error is obtained by the equation (11). It is represented by the error ellipse 1005 of the adaptive DR error. The existence range of the position of the dump truck 1 may be considered to be inside either or both of the error ellipse 1002 of the assumed DR error and the error ellipse 1005 of the adaptive DR error.

＜位置計測安定性判断処理＞
次に、ステップＳ１０８の位置計測安定性判断処理について図６を参照して説明する。本実施形態では位置計測安定性の判断を存在可能性範囲算出処理によって求めた誤差楕円により実施する。この安定性判断は他にも測位ステータスのみで行ってもよく、またχ２乗検定による棄却判断でも良い。図６は誤差楕円を用いた位置計測安定性判断処理の流れを示すフローチャートである。 <Position measurement stability judgment processing>
Next, the position measurement stability determination process in step S108 will be described with reference to FIG. In the present embodiment, the judgment of the position measurement stability is performed by the error ellipse obtained by the existence possibility range calculation process. In addition, this stability judgment may be made only by the positioning status, or may be a rejection judgment by the χ-square test. FIG. 6 is a flowchart showing the flow of the position measurement stability determination process using the error ellipse.

位置計測安定性判断部１１４は、ＧＮＳＳセンサ１０から取得した位置計測ステータスをチェックし、ＧＮＳＳセンサ１０からの出力が存在するかを判断する。ＧＮＳＳセンサ１０からの出力がある場合は（Ｓ２０１／Ｙｅｓ）、ステップＳ２０２に移行し、無い場合は（Ｓ２０１／Ｎｏ）、安定性フラグを「０」に設定する（Ｓ２０７）。 The position measurement stability determination unit 114 checks the position measurement status acquired from the GNSS sensor 10 and determines whether or not there is an output from the GNSS sensor 10. If there is an output from the GNSS sensor 10 (S201 / Yes), the process proceeds to step S202, and if there is no output (S201 / No), the stability flag is set to "0" (S207).

位置計測安定性判断部１１４は、Ｓ１０７で求めた仮定ＤＲ誤差の誤差楕円を取得する（Ｓ２０２）。 The position measurement stability determination unit 114 acquires an error ellipse of the assumed DR error obtained in S107 (S202).

次に位置計測安定性判断部１１４は、ＧＮＳＳセンサ１０から出力される検出位置が仮定ＤＲ誤差の誤差楕円の範囲内であるかを判断する（Ｓ２０３）。 Next, the position measurement stability determination unit 114 determines whether the detection position output from the GNSS sensor 10 is within the range of the error ellipse of the assumed DR error (S203).

この楕円の範囲内であるかの判断は式（６）の右辺（ｘ、ｙ）にＧＮＳＳセンサ１０から出力される検出位置（Ｘ、Ｙ）を代入し計算した結果が、マハラノビス距離ｄの２乗以下の場合は、仮定ＤＲ誤差の誤差楕円の範囲内であると判断（Ｓ２０３／Ｙｅｓ）し、安定性フラグを「１」に設定する（Ｓ２０６）。マハラノビス距離ｄの２乗より大きい場合は仮定ＤＲ誤差の誤差楕円の範囲外であると判断（Ｓ２０３／Ｎｏ）し、ステップＳ２０４へ移行する。 The determination of whether or not it is within the range of this ellipse is calculated by substituting the detection position (X, Y) output from the GNSS sensor 10 into the right side (x, y) of the equation (6), and the result is 2 of the Mahalanobis distance d. If it is less than or equal to the square, it is determined that the error is within the error ellipse of the assumed DR error (S203 / Yes), and the stability flag is set to "1" (S206). If it is larger than the square of the Mahalanobis distance d, it is determined that the error is out of the range of the error ellipse of the assumed DR error (S203 / No), and the process proceeds to step S204.

位置計測安定性判断部１１４は、ＧＮＳＳセンサ１０の検出値が仮定ＤＲ誤差の誤差楕円範囲外であった場合、Ｓ１０７で求めた適応ＤＲ誤差の誤差楕円を取得する（Ｓ２０４）。 When the detected value of the GNSS sensor 10 is outside the error ellipse range of the assumed DR error, the position measurement stability determination unit 114 acquires the error ellipse of the adaptive DR error obtained in S107 (S204).

次に位置計測安定性判断部１１４は、ＧＮＳＳセンサ１０から出力される検出位置が適応ＤＲ誤差の誤差楕円の範囲内であるかを判断する（Ｓ２０５）。 Next, the position measurement stability determination unit 114 determines whether the detection position output from the GNSS sensor 10 is within the error ellipse of the adaptive DR error (S205).

この楕円の範囲内であるかの判断は式（７）の右辺に（ｘ、ｙ）にＧＮＳＳセンサ１０から出力されるＧＮＳＳ出力位置（Ｘ、Ｙ）を代入し計算した結果が、マハラノビス距離ｄの２乗以下の場合は、適応ＤＲ誤差の誤差楕円の範囲内であると判断（Ｓ２０５／Ｙｅｓ）し、安定性フラグを「１」に設定する（Ｓ２０６）。マハラノビス距離ｄの２乗より大きい場合は適応ＤＲ誤差の誤差楕円の範囲外であると判断（Ｓ２０５／Ｎｏ）し、安定性フラグを「０」に設定する（Ｓ２０７）。そして位置計測安定性判断部１１４の処理を終了する。 The Mahalanobis distance d is determined by substituting the GNSS output position (X, Y) output from the GNSS sensor 10 into (x, y) on the right side of the equation (7) to determine whether it is within the range of this ellipse. If it is less than the square of, it is determined that it is within the range of the error ellipse of the adaptive DR error (S205 / Yes), and the stability flag is set to "1" (S206). If it is larger than the square of the Mahalanobis distance d, it is determined that it is out of the range of the error ellipse of the adaptive DR error (S205 / No), and the stability flag is set to "0" (S207). Then, the processing of the position measurement stability determination unit 114 is completed.

＜推定位置及び推定位置の誤差分散値の算出処理＞
次に、ステップＳ１０９の最尤位置及び最尤位置の誤差分散値の算出処理について図７を参照して説明する。図７は最尤位置演算部１１５が実行する最尤位置及び最尤位置の誤差分散値の算出処理の流れを示すフローチャートである。 <Calculation processing of estimated position and error variance value of estimated position>
Next, the calculation process of the maximum likelihood position and the error variance value of the maximum likelihood position in step S109 will be described with reference to FIG. 7. FIG. 7 is a flowchart showing a flow of calculation processing of the maximum likelihood position and the error variance value of the maximum likelihood position executed by the maximum likelihood position calculation unit 115.

まず最尤位置演算部１１５は、位置計測安定性判断部１１４が設定した安定性フラグの値が「１」かどうかを判断する。安定性フラグの値が「１」の場合は（Ｓ３０１／Ｙｅｓ）、ＧＮＳＳセンサ１０からの検出位置が存在し、かつ外れ値でもないため、検出位置の誤差分散値を設定する（Ｓ３０２）。 First, the maximum likelihood position calculation unit 115 determines whether or not the value of the stability flag set by the position measurement stability determination unit 114 is “1”. When the value of the stability flag is "1" (S301 / Yes), the detection position from the GNSS sensor 10 exists and is not an outlier, so the error variance value of the detection position is set (S302).

一方、最尤位置演算部１１５は、安定性フラグの値が「０」の場合は（Ｓ３０１／Ｎｏ）、ＧＮＳＳセンサ１０からの検出値が存在しない、もしくは外れ値であったため、ＧＮＳＳセンサ１０の検出位置の誤差分散値に予め決められた極大値を設定する。また、ＧＮＳＳセンサ１０の検出位置を仮定ＤＲ位置算出部１１１が算出した仮定ＤＲ位置と同じ値に設定する（Ｓ３０３）。 On the other hand, when the value of the stability flag of the maximum likelihood position calculation unit 115 is "0" (S301 / No), the value detected from the GNSS sensor 10 does not exist or is an outlier, so that the GNSS sensor 10 has an error value. A predetermined maximum value is set for the error variance value of the detection position. Further, the detection position of the GNSS sensor 10 is set to the same value as the hypothetical DR position calculated by the hypothetical DR position calculation unit 111 (S303).

次に最尤位置演算部１１５は、カルマンフィルタにおけるイノベーションベクトルνを算出する（Ｓ３０４）。イノベーションベクトルνはＧＮＳＳセンサ１０の検出位置Ｘ及び仮定ＤＲ位置ｘｔｉｌｄｅから下式（１２）のように求まる。

Next, the maximum likelihood position calculation unit 115 calculates the innovation vector ν in the Kalman filter (S304). The innovation vector ν can be obtained from the detection position X and the hypothetical DR position xtilde of the GNSS sensor 10 as shown in the following equation (12).

最尤位置演算部１１５は、カルマンゲインＫを算出する（Ｓ３０５）。カルマンゲインＫは、仮定ＤＲ位置の誤差分散値が適応ＤＲ誤差の誤差分散値を含むことで、モデル化誤差を考慮でき、下式（１３）により求められる。

The maximum likelihood position calculation unit 115 calculates the Kalman gain K (S305). The Kalman gain K can be obtained by the following equation (13) because the modeling error can be taken into consideration by including the error variance value of the adaptive DR error in the error variance value of the assumed DR position.

次に最尤位置演算部１１５は、最尤位置をダンプトラック１の推定位置として算出する（Ｓ３０６）。最尤位置は、下式（１４）により算出できる。

Next, the maximum likelihood position calculation unit 115 calculates the maximum likelihood position as the estimated position of the dump truck 1 (S306). The maximum likelihood position can be calculated by the following equation (14).

次に最尤位置演算部１１５は、最尤位置における誤差分散値Ｐ（ｔ）を算出する（Ｓ３０７）。誤差分散値は下式（１５）のように算出できる。

Next, the maximum likelihood position calculation unit 115 calculates the error variance value P (t) at the maximum likelihood position (S307). The error variance value can be calculated by the following equation (15).

最後に最尤位置演算部１１５は、算出した最尤位置ｘ（ｔ）、誤差分散値Ｐ（ｔ）から誤差楕円（「最尤誤差楕円」という）を計算し、それらを自律走行制御装置３００へ出力する（Ｓ３０８）。 Finally, the maximum likelihood position calculation unit 115 calculates an error ellipse (referred to as “maximum likelihood error ellipse”) from the calculated maximum likelihood position x (t) and the error variance value P (t), and uses them as the autonomous travel control device 300. Output to (S308).

最尤位置ｘ（ｔ）と誤差分散値Ｐ（ｔ）から算出される最尤誤差楕円は下式（１６）のように求めることが出来る。

The maximum likelihood error ellipse calculated from the maximum likelihood position x (t) and the error variance value P (t) can be obtained by the following equation (16).

最尤誤差楕円は、最尤位置ｘ（ｔ）を中心として、誤差分散値Ｐ（ｔ）で規定される確率分布においてマハラノビス距離がｄとなる等確率線を表わしている。最尤誤差楕円内にマハラノビス距離ｄで規定される確率で正しい位置（ダンプトラック１が実際に存在する位置）が存在することを表わしている。 The maximum likelihood error ellipse represents an equal probability line in which the Mahalanobis distance is d in the probability distribution defined by the error variance value P (t) centered on the maximum likelihood position x (t). Maximum likelihood error Indicates that the correct position (the position where the dump truck 1 actually exists) exists in the ellipse with the probability defined by the Mahalanobis distance d.

自律走行制御装置３００の目標速度算出部３０１は、最尤誤差楕円の最外縁部と目標軌道との乖離に応じて目標軌道に復帰する際の目標速度を決定し、速度制御部３０２が走行駆動装置４００に対して目標速度で走行するための制御信号を出力する。また、速度制御部３０２は必要に応じて制動装置に対する制動信号や操舵モータに対する操舵角信号も出力する。 The target speed calculation unit 301 of the autonomous travel control device 300 determines the target speed at the time of returning to the target trajectory according to the deviation between the outermost edge portion of the most probable error ellipse and the target trajectory, and the speed control unit 302 drives the travel. A control signal for traveling at a target speed is output to the device 400. The speed control unit 302 also outputs a braking signal for the braking device and a steering angle signal for the steering motor, if necessary.

また、上記の方法以外に、適応ＤＲ誤差の平均値および誤差楕円も出力することで、自律走行制御装置３００は、適応ＤＲ誤差の平均値が最尤誤差楕円の内側にある場合は、最尤誤差楕円を基に速度を算出し、適応ＤＲ誤差の平均値が最尤誤差楕円の外側にある場合は、最尤位置に適応ＤＲ誤差の平均値を足した点を中心とした適応ＤＲ誤差の誤差楕円を基に速度を算出してもよい。 In addition to the above method, by outputting the average value of the adaptive DR error and the error ellipse, the autonomous driving control device 300 is the most probable when the average value of the adaptive DR error is inside the most probable error ellipse. The velocity is calculated based on the error ellipse, and if the average value of the adaptive DR error is outside the most probable error ellipse, the adaptive DR error centered on the point where the average value of the adaptive DR error is added to the most probable position. The speed may be calculated based on the error ellipse.

上記の方法以外に、「２つの存在範囲を内包する楕円を生成して目標速度算出部３０１へ出力する」、「２つの位置の存在範囲を表わす分散を重ね合わせて一つの正規分布にフィッティングして目標速度算出部３０１へ出力する」、「２つの位置の存在範囲を両方とも目標速度算出部３０１へ出力する」などの方法でも同じ効果が得られる。 In addition to the above methods, "generate an ellipse containing two existing ranges and output it to the target velocity calculation unit 301" and "superimpose the variances representing the existing ranges of the two positions and fit them into one normal distribution. The same effect can be obtained by methods such as "outputting to the target speed calculation unit 301" and "outputting both existing ranges of the two positions to the target speed calculation unit 301".

位置算出装置１００に接続される外部装置は自律走行制御装置３００に限らない。例えばダンプトラック１の管制制御を行う管制サーバに対して、最尤誤差楕円を通知してもよい。 The external device connected to the position calculation device 100 is not limited to the autonomous travel control device 300. For example, the maximum likelihood error ellipse may be notified to the control server that controls the control of the dump truck 1.

＜適応ＤＲ誤差更新処理＞
ステップＳ１１１における適応ＤＲ誤差パラメータ算出部１４１による適応ＤＲ誤差ε（ｔ）およびその平均値、分散値の算出処理について図９に従って説明する。図９は、適応ＤＲ誤差更新の流れを示すフローチャートである。 <Adaptive DR error update processing>
The adaptive DR error ε (t) and the calculation processing of the average value and the variance value thereof by the adaptive DR error parameter calculation unit 141 in step S111 will be described with reference to FIG. FIG. 9 is a flowchart showing the flow of adaptive DR error update.

まず適応ＤＲ誤差ε（ｔ）を、ＧＮＳＳセンサ１０の検出値とＤＲによって積算した移動量による値とを比較することにより算出する（Ｓ４０１）。図８に適応ＤＲ誤差の概念図を示す。 First, the adaptive DR error ε (t) is calculated by comparing the detected value of the GNSS sensor 10 with the value based on the movement amount integrated by DR (S401). FIG. 8 shows a conceptual diagram of the adaptive DR error.

ＤＲはそれ単体では絶対的な位置を表わすことができない。そこで、適応ＤＲ誤差パラメータ算出部１４１は、計測始点となる第１時点でのＧＮＳＳセンサ１０の検出値７０１（Ｘ（ｔ−１））にＤＲ装置２１０によってステップＳ１０２にて算出された第１時点からの位置変位量Δｘ（ｔ）により表わされるベクトル７０２を足し合わせた適応ＤＲ位置と、次の計測タイミングでＧＮＳＳセンサ１０にて計測された位置７０３（Ｘ（ｔ））との差分７０４によって求めることができる。時刻ｔにおける差分７０４を適応ＤＲ誤差ε（ｔ）で表す。適応ＤＲ誤差ε（ｔ）は、下式（１７）により算出する。

DR alone cannot represent an absolute position. Therefore, the adaptive DR error parameter calculation unit 141 calculates the detection value 701 (X (t-1)) of the GNSS sensor 10 at the first time point, which is the measurement start point, by the DR device 210 in step S102. Obtained by the difference 704 between the adaptive DR position obtained by adding the vector 702 represented by the position displacement amount Δx (t) from the above and the position 703 (X (t)) measured by the GNSS sensor 10 at the next measurement timing. be able to. The difference 704 at time t is represented by the adaptive DR error ε (t). The adaptive DR error ε (t) is calculated by the following equation (17).

次に適応ＤＲ誤差パラメータ算出部１４１は、ステップＳ１０４にて決定した現在の車両状態における、適応ＤＲ誤差の平均値を更新する（Ｓ４０２）。適応ＤＲ誤差パラメータ算出部１４１は、適応ＤＲ誤差ε（ｔ）の平均値を一時記憶部１３１から取得した現在の車両状態の平均値Ｅ［ε（ｔ−１）］を基に下式（１８）で求める。

Next, the adaptive DR error parameter calculation unit 141 updates the average value of the adaptive DR error in the current vehicle state determined in step S104 (S402). The adaptive DR error parameter calculation unit 141 uses the following equation (18) based on the average value E [ε (t-1)] of the current vehicle state obtained from the temporary storage unit 131 for the average value of the adaptive DR error ε (t). ).

ここで、一時記憶部１３１のデータ構成を図１０に示す。一時記憶部１３１には、車両状態と、それぞれに対応する適応ＤＲ誤差の平均値、分散値、及びそれらを算出したサンプル数が記録されている。 Here, the data structure of the temporary storage unit 131 is shown in FIG. The temporary storage unit 131 records the vehicle state, the average value and the variance value of the adaptive DR errors corresponding to each, and the number of samples obtained by calculating them.

車両状態は、例えば、「積荷ｏｒ空荷」「加速ｏｒ減速ｏｒ一定速度」「上り勾配走行ｏｒ下り勾配走行ｏｒ平坦地走行」「直進走行ｏｒ右旋回走行ｏｒ左旋回走行」「グリップｏｒスリップ」などの状態の組み合わせで表現される。それぞれは関数ｆ（＊）内で表わされる。例えば「空荷ｏｒ積荷」であれば、「Ｍ＜０．１トン」を空荷、「Ｍ＞＝０．１トン」を積荷とするなどである。 The vehicle state is, for example, "load or empty load", "acceleration or deceleration or constant speed", "uphill running or downhill running or flat ground running", "straight running or right turning running or left turning running", "grip or slip". It is expressed by a combination of states such as ". Each is represented in the function f (*). For example, in the case of "empty cargo or cargo", "M <0.1 ton" is an empty cargo, "M> = 0.1 ton" is a cargo, and so on.

また、適応ＤＲ誤差パラメータ算出部１４１は、適応ＤＲ誤差ε（ｔ）の分散値Ｅ［ε^２（ｔ）］も更新する。適応ＤＲ誤差パラメータ算出部１４１は適応ＤＲ誤差ε（ｔ）の分散値Ｅ［ε^２（ｔ）］を下式（１９）により逐次求める。

^{The adaptive DR error parameter calculation unit 141 also updates the variance value E [ε 2} (t)] of the adaptive DR error ε (t). ^{The adaptive DR error parameter calculation unit 141 sequentially obtains the variance value E [ε 2} (t)] of the adaptive DR error ε (t) by the following equation (19).

次に適応ＤＲ誤差パラメータ算出部１４１は、算出した適応ＤＲ誤差の平均値及び分散値を一時記憶部１３１の該当する部分へ上書き更新する（Ｓ４０３）。これらの処理が終了すれば、適応ＤＲ誤差更新処理を終了する。 Next, the adaptive DR error parameter calculation unit 141 overwrites and updates the calculated average value and variance value of the adaptive DR error to the corresponding portion of the temporary storage unit 131 (S403). When these processes are completed, the adaptive DR error update process is completed.

＜車両状態更新処理＞
適応ＤＲ誤差パラメータ算出部１４１は、車両状態の遷移確率Ｐ（Λｔ｜Λｔ−１）を更新する。ある時刻における車両状態Λｍ（ｔ−１）から次のサンプル時における車両状態Λｎ（ｔ）への各車両状態への遷移確率Ｐ（Λｍｎ（ｔ｜ｔ−１））を下式（２０）により算出する。

<Vehicle status update process>
The adaptive DR error parameter calculation unit 141 updates the transition probability P (Λt | Λt-1) of the vehicle state. The transition probability P (Λmn (t | t-1)) from the vehicle state Λm (t-1) at a certain time to the vehicle state Λn (t) at the next sample is calculated by the following equation (20). calculate.

これらを集めた遷移確率Ｐ（Λｔ｜Λｔ−１）は下記行列（２１）で表わされる。

The transition probability P (Λt | Λt-1) obtained by collecting these is represented by the following matrix (21).

車両状態の遷移確率は、上式をそのまま用いた場合は、値が安定しない場合も多いので、確率勾配法などを用いて、徐々に収束させるような手法をとってもよい。 When the above equation is used as it is, the value of the transition probability of the vehicle state is often unstable. Therefore, a method of gradually converging by using a stochastic gradient descent method or the like may be adopted.

ステップＳ４０３において、ある車両状態Λｓからある車両状態Λｕ（ｔ）までの遷移確率が両方向とも、閾値よりも高い場合は、車両状態に安定性が無いため、一つの車両状態へと融合する。その場合は、各車両状態に設定された適応ＤＲ誤差平均値と適応ＤＲ誤差分散値とをそれぞれのサンプル数を用いて、下式（２２）、（２３）のように更新する。

In step S403, when the transition probability from a certain vehicle state Λs to a certain vehicle state Λu (t) is higher than the threshold value in both directions, the vehicle state is not stable and is fused into one vehicle state. In that case, the adaptive DR error average value and the adaptive DR error variance value set for each vehicle state are updated as in the following equations (22) and (23) using the respective sample numbers.

これらの処理が終了すれば、適応ＤＲ誤差パラメータ算出部１４１による車両状態更新処理を終了する。 When these processes are completed, the vehicle state update process by the adaptive DR error parameter calculation unit 141 is completed.

本実施形態では、仮定ＤＲ位置の誤差分散値が適応ＤＲ誤差の誤差分散値を含むことで、カルマンゲインＫがモデル化誤差を考慮し、このカルマンゲインＫを用いて最尤誤差楕円を求める。そして最尤誤差楕円の最外縁と目標軌道との差を基に走行制御を行うので、スリップなどの考慮されていなかった現象が生じた場合でも適切な走行制御が可能となる。 In the present embodiment, the error variance value of the assumed DR position includes the error variance value of the adaptive DR error, so that the Kalman gain K considers the modeling error and the maximum likelihood error ellipse is obtained using this Kalman gain K. Then, since the travel control is performed based on the difference between the outermost edge of the maximum likelihood error ellipse and the target trajectory, appropriate travel control is possible even when an unconsidered phenomenon such as slip occurs.

また特許文献２に記載発明では、近々の過去の車両の動作しか考慮されていないため、有る特定の車両状態の場合にオフセットが生じるとその検出が遅れる。これに対して、本実施形態によれば、車両状態毎に適応ＤＲ誤差を記憶しているので、いち早く誤差の検出が可能となる。 Further, in the invention described in Patent Document 2, since only the operation of the vehicle in the near past is taken into consideration, the detection is delayed when an offset occurs in a certain specific vehicle state. On the other hand, according to the present embodiment, since the adaptive DR error is stored for each vehicle state, the error can be detected quickly.

更に本実施形態によれば、適応ＤＲ誤差を算出する際に、適応ＤＲ誤差の平均分誤差楕円の中心をずらし、誤差楕円そのものの大きさ（面積）は不変とする。これにより、誤差楕円そのものの大きさを大きくする（長軸、短軸の長さを変える）特許文献２の手法の比べて、より適切に誤差楕円を出力することができる。 Further, according to the present embodiment, when calculating the adaptive DR error, the center of the error ellipse is shifted by the average of the adaptive DR error, and the size (area) of the error ellipse itself is unchanged. As a result, the error ellipse can be output more appropriately than the method of Patent Document 2 in which the size of the error ellipse itself is increased (the lengths of the major axis and the minor axis are changed).

上記各実施形態は本発明を限定する趣旨ではなく、本発明の要旨を変更しない範囲の様々な態様は、本発明に含まれる。例えば、移動体として自律走行するダンプトラック１を例に挙げたが、異なる種類の作業機械であってもよい。また、各処理の実行順序は上記実施形態に限定されず、後に続く処理を妨げない範囲で処理の前後を入れ替えてもよい。 Each of the above embodiments is not intended to limit the present invention, and various aspects within the scope of not changing the gist of the present invention are included in the present invention. For example, the dump truck 1 that autonomously travels as a moving body has been given as an example, but different types of work machines may be used. Further, the execution order of each process is not limited to the above embodiment, and the process may be interchanged before and after the process as long as the subsequent process is not hindered.

１：ダンプトラック
２：車体フレーム
３：ベッセル
４：運転室
５：前輪
６：後輪
７：ＧＮＳＳアンテナ
１０：ＧＮＳＳセンサ
２０：積載状態センサ
３０：操舵角センサ
４０：勾配演算器
１００：位置算出装置
２１０：デッドレコニング装置
３００：自律走行制御装置
４００：走行駆動装置
1: Dump truck 2: Body frame 3: Vessel 4: Driver's cab 5: Front wheels 6: Rear wheels 7: GNSS antenna 10: GNSS sensor 20: Load status sensor 30: Steering angle sensor 40: Gradient calculator 100: Position calculation device 210: Dead reckoning device 300: Autonomous travel control device 400: Travel drive device

Claims

A GNSS sensor that sequentially measures the GNSS output position of the moving object represented by the global coordinate system at each time point of the first time point and the second time point later than that.
Using the detection values of the moving body sensor that detects the momentum and posture of the moving body, the first time the measurement start point is the displacement amount from the measurement start point, and from the next time, the displacement amount from the relative position is the previously calculated relative position. , And a dead reckoning device that sequentially updates the relative position of the moving body from the measurement start point.
A mobile state sensor that detects a moving body state defined by at least one of the speed, weight, and slope of the road surface on which the moving body travels.
It is a position calculation device of the moving body connected to each of the above.
The position calculation device includes a position calculation controller including a temporary storage device.
The position calculation controller is
Using the GNSS output position at the first time point and the relative position calculated by the dead reckoning device at the first time point, the maximum likelihood position of the moving body at the first time point is calculated.
The relative position calculated by the dead reckoning device between the first time point and the second time point is added to the maximum likelihood position at the first time point to calculate the assumed dead reckoning position at the second time point.
The relative position calculated by the dead reckoning apparatus between the first time point and the second time point is added to the GNSS output position at the first time point to calculate the adaptive dead reckoning position at the second time point.
The adaptive dead reckoning error consisting of the difference between the GNSS output position at the second time point and the adaptive dead reckoning position at the second time point is calculated and moved based on the value detected from the moving body state sensor at the second time point. Determine your physical condition,
Of the error statistic data in which the moving body state stored in the temporary storage device in advance is associated with the average value and the variance value of the adaptive dead reckoning error, the average of the adaptive dead reckoning error according to the determined moving body state. Values and variances are recalculated and updated using the newly calculated adaptive dead reckoning error.
A position deviated from the assumed dead reckoning position at the second time point by the average value of the recalculated adaptive dead reckoning error to an error ellipse consisting of the variance values of the assumed dead reckoning error centered on the assumed dead reckoning position at the second time point. The existence possibility range is calculated by adding an error ellipse consisting of the variance value of the recalculated adaptive dead reckoning error.
When the GNSS output position at the second time point is included in the existence possibility range, the maximum likelihood position at the second time point and the maximum likelihood position centered on the maximum likelihood position using the GNSS output position are used. The error ellipse is calculated, and if the GNSS output position at the second time point exists outside the existence possibility range, the maximum likelihood error ellipse centered on the assumed dead reckoning position at the second time point is calculated.
The maximum likelihood error ellipse is output as a position range in which the moving body exists.
A position calculation device characterized in that.

In the position calculation device according to claim 1,
The position calculation controller calculates the first moving body state at the first time point based on the detected value at the first time point of the moving body state sensor, and calculates the detected value at the second time point of the moving body state sensor. When the second mobile state at the second time point is calculated based on this, the transition probability from the first mobile state to the second mobile state is calculated, and the transition probability is higher than a predetermined threshold value. The second is the average value and dispersion value of the adaptive dead reckoning error calculated in the first mobile state and the average value and dispersion value of the adaptive dead reckoning error calculated in the second mobile state. Update by adding the adaptive dead reckoning error calculated at the time.
A position calculation device characterized in that.

In the position calculation device according to claim 1,
The position calculation controller has a deviation between the GNSS output position at the second time point and the assumed dead reckoning at the second time point, or a difference between the GNSS output position at the second time point and the adaptive dead reckoning at the second time point. Based on the above, the position measurement stability of the GNSS output position at the second time point is determined, and only when it is determined that the position measurement stability is present, the adaptive dead reckoning error output at the second time point is used. Update the error statistic data,
A position calculation device characterized in that.

The position calculation device according to claim 1 and
A dump truck including an autonomous travel control device that performs autonomous travel control based on a position calculated by the position calculation device, and a travel drive device that operates based on a control signal from the autonomous travel control device.
The moving body is the dump truck.
The position calculation device outputs the maximum likelihood error ellipse as the position range to the autonomous travel control device.
A dump truck that features that.

In the dump truck according to claim 4,
The dump truck
With the speed sensor
An acceleration calculator that calculates the acceleration of the dump truck based on the output of the acceleration sensor or speed sensor,
A load status sensor that detects the load status of the load, and a load status sensor
Angular velocity sensor and
A steering angle sensor that detects the straight-ahead turning state of the dump truck, and
A gradient calculator for calculating the gradient of the traveling path of the dump truck, and at least one of the gradient calculators are provided.
The position calculation controller determines the vehicle state using at least one of the speed, acceleration, loading state, angular velocity, steering angle, or gradient of the dump truck, and adaptive dead reckoning that occurs for each vehicle state of the dump truck. Learn the magnitude of the error,
A dump truck that features that.

In the dump truck according to claim 5,
The position calculation controller selects the variance value of the adaptive dead reckoning error learned according to the gradually changing vehicle condition.
If the variance value of the selected adaptive dead reckoning error is relatively large, a relatively large maximum likelihood error ellipse is generated, and if the variance value of the selected adaptive dead reckoning error is relatively small, it is relatively small. Generate maximum likelihood error ellipse,
A dump truck that features that.