JP2019179421A - Position calculation device and dump truck - Google Patents
Position calculation device and dump truck Download PDFInfo
- Publication number
- JP2019179421A JP2019179421A JP2018068450A JP2018068450A JP2019179421A JP 2019179421 A JP2019179421 A JP 2019179421A JP 2018068450 A JP2018068450 A JP 2018068450A JP 2018068450 A JP2018068450 A JP 2018068450A JP 2019179421 A JP2019179421 A JP 2019179421A
- Authority
- JP
- Japan
- Prior art keywords
- error
- time point
- dead reckoning
- adaptive
- calculated
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000004364 calculation method Methods 0.000 title claims description 136
- 230000003044 adaptive effect Effects 0.000 claims abstract description 128
- 238000007476 Maximum Likelihood Methods 0.000 claims abstract description 61
- 238000005259 measurement Methods 0.000 claims description 43
- 238000001514 detection method Methods 0.000 claims description 26
- 230000001133 acceleration Effects 0.000 claims description 20
- 238000006073 displacement reaction Methods 0.000 claims description 11
- 230000007704 transition Effects 0.000 claims description 9
- 238000000034 method Methods 0.000 description 44
- 230000008569 process Effects 0.000 description 33
- 238000012545 processing Methods 0.000 description 20
- 238000010586 diagram Methods 0.000 description 9
- 230000014509 gene expression Effects 0.000 description 7
- 239000011159 matrix material Substances 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 239000013598 vector Substances 0.000 description 6
- 230000008859 change Effects 0.000 description 3
- 238000012937 correction Methods 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 230000003111 delayed effect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000012423 maintenance Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000000546 chi-square test Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
本発明は、位置算出装置及びダンプトラックに係り、特に鉱山や建設現場で移動するダンプトラックに搭載された位置算出装置の位置測定誤差に関する。 The present invention relates to a position calculation device and a dump truck, and more particularly to a position measurement error of a position calculation device mounted on a dump truck that moves in a mine or a construction site.
自律的に走行するダンプトラックにおいてその位置を算出する機能は重要な機能の一つである。この位置算出の例として、慣性計測装置(IMU)を用い、車輪の回転数から速度を計測する車輪速度計、車輪の車軸に対する傾きを計測する操舵角計等の計測装置の結果を用いて、ダンプトラックの位置を算出する。GNSS等のように直接位置を算出する機器から出力される位置に対して、速度、姿勢、加速度、角速度などの関連する運動パラメータから位置を順次更新する方法があり、デッドレコニングと呼ばれる。 The function of calculating the position of an autonomously traveling dump truck is one of important functions. As an example of this position calculation, using an inertial measurement device (IMU), using the result of a measurement device such as a wheel speedometer that measures the speed from the rotation speed of the wheel, a steering angle meter that measures the inclination of the wheel with respect to the axle, Calculate the position of the dump truck. There is a method of sequentially updating the position from the related motion parameters such as velocity, posture, acceleration, angular velocity, etc., with respect to the position output from a device that directly calculates the position such as GNSS, which is called dead reckoning.
従来よりデッドレコニングを用いて算出した位置を、カルマンフィルタなどに代表される確率フィルタを用いて、全地球航法衛星システム(GNSS)にて算出された位置へ徐々に修正する手法がある。この確率フィルタを用いた位置修正方法は、デッドレコニング、GNSSのそれぞれから算出される誤差を考慮して位置を修正するため、確率的に最も存在する可能性の高い位置を算出できることが長所である。この際に、確率フィルタを用いて、自律走行ダンプトラックの位置の誤差を分散値として算出し、この分散値が存在する可能性のある範囲を誤差楕円として表わすことが可能である。 Conventionally, there is a method of gradually correcting a position calculated using dead reckoning to a position calculated by a global navigation satellite system (GNSS) using a stochastic filter represented by a Kalman filter or the like. Since the position correction method using the probability filter corrects the position in consideration of errors calculated from the dead reckoning and the GNSS, it is possible to calculate a position that is most likely to exist probabilistically. . At this time, by using a probability filter, it is possible to calculate an error in the position of the autonomous dump truck as a variance value, and to express a range in which this variance value may exist as an error ellipse.
特許文献1には、誤差楕円を用いたダンプトラックの走行制御装置として、「自車両の予測位置を算出する位置算出装置と、予測位置を中心とし、運搬車両が予め定められた期待確率で存在する位置範囲を算出する誤差見積もり範囲算出部と、運搬車両の目標経路と位置範囲内に含まれる各地点との乖離量のうちの最大値からなる最大乖離量を算出する最大乖離量算出部と、最大乖離量が相対的に大きい場合に、運搬車両の目標車速を相対的に小さく定める目標車速決定部と、目標車速に従って、目標経路に沿って運搬車両を走行させるための制御を行う目標経路追従部(要約抜粋)」を備えた構成が開示されている。
In
また特許文献2には、「ナビゲーション装置は、GPS位置算出部と、センサ情報取得部と、GPS位置とセンサ情報とに基づいて移動体の位置および向き、並びに、これらの誤差共分散行列を算出する位置誤差算出部と、各センサ情報を累積して得られた位置と、GPS位置との位置の差をオフセットとして算出するオフセット算出部と、算出されたオフセットに基づいて、位置誤差算出部によって算出された誤差共分散行列を補正する誤差共分散行列補正部と、位置誤差算出部によって算出された移動体の位置および向き、並びに、誤差共分散行列補正部によって補正された誤差共分散行列を用いて、地図データに含まれる道路上に移動体の位置を推定するマップマッチ処理部とを備える(要約抜粋)」構成が開示されている。
Further,
従来の確率フィルタの手法ではデッドレコニングでの見積もり誤差を信用して、誤差分散値を算出する仕組みになっている。そのため、デッドレコニングでの見積もり誤差が大きく誤っている、もしくはスリップなどの考慮されていない車両運動が発生した場合には、現実にそぐわない誤差楕円を出力することがあるという課題がある。このような予め設定された見積もり誤差であるデッドレコニングでの見積もり誤差自体に意図せず含まれる誤差を、モデル化誤差と呼ぶことにする。特許文献1では誤差楕円を基づいて走行制御を行うため、誤差楕円に含まれるモデル化誤差が大きいと実際の走行制御に支障をきたす可能性があり、このモデル化誤差を含めた誤差楕円を出力することにより、目標車速を制御したいという要望がある。また、モデル化誤差の発生要因の一つとなる車両運動は、積載の有無等の車両状態の変化によっても生じるが、この車両状態の変化は、特定の車両に固有に生じる。これに対し、特許文献2では、過去の他の車両の動作を基に算出したオフセットを用いて誤差楕円の補正を行うので、特定の車両に固有の車両運動が生じた場合にはオフセットに反映されず、新たに生じた誤差の検出が遅れるという課題が残る。
The conventional stochastic filter method has a mechanism for calculating an error variance value by trusting an estimation error in dead reckoning. For this reason, there is a problem in that an error ellipse that does not fit in reality may be output when an estimation error in dead reckoning is largely incorrect or when a vehicle motion such as slip is not considered. Such an error that is unintentionally included in the estimation error in dead reckoning, which is a preset estimation error, will be referred to as a modeling error. In
本発明は上記課題に鑑みてなされたものであり、特定の車両に生じる予期せざる誤差の補正を行って誤差楕円を生成することを目的とする。 The present invention has been made in view of the above problems, and an object thereof is to generate an error ellipse by correcting an unexpected error occurring in a specific vehicle.
上記課題を解決するために本発明は、グローバル座標系で表された移動体のGNSS出力位置を第1時点及びそれよりも遅い第2時点の各時点において逐次計測するGNSSセンサと、前記移動体の運動量及び姿勢を検出する移動体センサの検出値を用いて、初回は計測始点に当該計測始点からの変位量を、次回からは前回算出した相対位置に当該相対位置からの変位量を累積加算して、前記計測始点からの前記移動体の相対位置を逐次更新するデッドレコニング装置と、前記移動体の速度、重量、前記移動体が走行する路面の勾配の少なくとも一つにより定義された移動体状態を検出する移動体状態センサと、の其々に接続された、前記移動体の位置算出装置であって、前記位置算出装置は、一時記憶装置を含む位置算出コントローラを備え、前記位置算出コントローラは、前記第1時点におけるGNSS出力位置、及び前記第1時点において前記デッドレコニング装置が算出した相対位置を用いて前記第1時点における前記移動体の最尤位置を計算し、前記第1時点から前記第2時点までの間に前記デッドレコニング装置が算出した相対位置を前記第1時点における最尤位置に足し合わせて、前記第2時点における仮定デッドレコニング位置を算出し、前記第1時点から前記第2時点までの間に前記デッドレコニング装置が算出した相対位置を前記第1時点におけるGNSS出力位置に足し合わせて、前記第2時点における適応デッドレコニング位置を算出し、前記第2時点におけるGNSS出力位置と前記第2時点における適応デッドレコニング位置との差からなる適応デッドレコニング誤差を算出すると共に、前記第2時点における前記移動体状態センサからの検出値を基に移動体状態を決定し、予め一時記憶装置に記憶された移動体状態と適応デッドレコニング誤差の平均値及び分散値とを関係付けた誤差統計量データのうち、前記決定された移動体状態に応じた適応デッドレコニング誤差の平均値及び分散値は、新たに算出された適応デッドレコニング誤差を用いて再計算して更新し、前記第2時点における仮定デッドレコニング位置を中心とし仮定デッドレコニング誤差の分散値からなる誤差楕円に前記第2時点における仮定デッドレコニング位置から前記再計算された適応デッドレコニング誤差の平均値分ずれた位置を中心とし、前記再計算された適応デッドレコニング誤差の分散値からなる誤差楕円を加えて存在可能性範囲を算出し、前記存在可能性範囲内に前記第2時点におけるGNSS出力位置が含まれている場合には、当該GNSS出力位置を用いて前記第2時点における最尤位置及び当該最尤位置を中心とする最尤誤差楕円を算出し、前記存在可能性範囲の外に前記第2時点におけるGNSS出力位置が存在する場合には、前記第2時点における仮定デッドレコニング位置を中心とする最尤誤差楕円を算出し、前記最尤誤差楕円を前記移動体が存在する位置範囲として出力する、ことを特徴とする。 In order to solve the above-described problems, the present invention provides a GNSS sensor that sequentially measures a GNSS output position of a moving object represented by a global coordinate system at each of a first time point and a second time point later than the GNSS output position; Using the detection value of the moving body sensor that detects the momentum and posture of the camera, the displacement from the measurement start point is accumulated at the measurement start point for the first time, and the displacement amount from the relative position is added to the previously calculated relative position from the next time. A dead reckoning device that sequentially updates the relative position of the moving body from the measurement start point, and a moving body defined by at least one of the speed, weight, and gradient of the road surface on which the moving body travels A mobile body position sensor for detecting a state, each of which is connected to the mobile body position sensor, wherein the position calculator includes a position calculation controller including a temporary storage device. The position calculation controller calculates the maximum likelihood 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, Adding the relative position calculated by the dead reckoning device between the first time point and the second time point to the maximum likelihood position at the first time point to calculate an assumed dead reckoning position at the second time point; The adaptive dead reckoning position at the second time point is calculated by adding the relative position calculated by the dead reckoning device between the first time point and the second time point to the GNSS output position at the first time point, The adaptive deck comprising the difference between the GNSS output position at the second time point and the adaptive dead reckoning position at the second time point. A reckoning error is calculated, a moving body state is determined based on a detection value from the moving body state sensor at the second time point, and an average value of the moving body state and adaptive dead reckoning error stored in advance in a temporary storage device And the average value and variance value of the adaptive dead reckoning error according to the determined moving body state are reproduced using the newly calculated adaptive dead reckoning error. An error ellipse consisting of a variance value of the assumed dead reckoning error centered on the assumed dead reckoning position at the second time point is updated to the recalculated adaptive dead reckoning error from the assumed dead reckoning position at the second time point. An error ellipse consisting of variance values of the recalculated adaptive dead reckoning error is added, with the position shifted by the average value as the center. When 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 A maximum likelihood error ellipse centered on the maximum likelihood position is calculated, and when 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. The maximum likelihood error ellipse is calculated, and the maximum likelihood error ellipse is output as a position range where the moving object exists.
本発明によれば、特定の車両に生じる予期せざる誤差の補正を行って誤差楕円を生成することができる。上記した以外の目的、構成、効果については以下の説明で明らかにされる。 According to the present invention, an error ellipse can be generated by correcting an unexpected error occurring in a specific vehicle. Objects, configurations, and effects other than those described above will be clarified in the following description.
以下、図面を参照して本発明の実施形態について説明する。全図を通じて同一の構成には同一符号を付け、重複説明を省略する。 Hereinafter, embodiments of the present invention will be described with reference to the drawings. Throughout the drawings, the same components are denoted by the same reference numerals, and redundant description is omitted.
以下、位置算出の対象となる移動体として、鉱山内を管制局からの指示に従って自律走行する自律走行ダンプトラック(以下「ダンプトラック」と略記する)を用いた例について説明する。 Hereinafter, an example in which an autonomous traveling dump truck (hereinafter abbreviated as “dump truck”) that autonomously travels in the mine in accordance with an instruction from the control station will be described as a mobile object for position calculation.
図1は、ダンプトラック1の外観を示す概略図である。図1に示すように、ダンプトラック1は、車体フレーム(vehicle frame)2と、車体フレーム2上に起伏可能に設けられたベッセル3とを有する。更に車体フレーム2の前側上方に運転室4が設けられている。そして、車体フレーム2の下部には左右の前輪5及び後輪6が備えられる。
FIG. 1 is a schematic view showing the appearance of the
ダンプトラック1の前部にはGNSSアンテナ7を備える。図1ではGNSSアンテナ7を一つ図示しているが、実際には、ダンプトラック1の前部に車幅方向に間隔を空けて二つ以上のGNSSアンテナ7が設置されている。GNSSアンテナ7は、グローバル座標系で表されたダンプトラック1の絶対位置を逐次計測するGNSSセンサ10(図2参照)の構成要素である。
A
更にダンプトラック1の運動量を示す検出値及びダンプトラック1の姿勢を示す検出値を用いて、初回は計測始点に当該計測始点からの変位量を、次回からは前回算出した相対位置に当該相対位置からの変位量を累積加算して、計測始点からのダンプトラック1の相対位置を逐次更新するデッドレコニング(以下「デッドレコニング」を「DR」と略記する)装置210を備える。
Further, using the detection value indicating the momentum of the
ダンプトラック1は、GNSSセンサ10から出力される絶対座標系で表されたGNSS出力位置、及びDR装置210から出力される相対位置を基にダンプトラック1の自己位置を算出する位置算出装置100と、加速装置や制動装置を含む走行駆動装置400に対して位置算出装置100から取得した自己位置データを用いて自律走行制御を行う自律走行制御装置300と、を備える。
The
図2は、ダンプトラック1のハードウェア構成図である。位置算出装置100は、CPU101、ROM102、RAM103、HDD104、入力インターフェース(I/F)105、出力I/F106を含みこれらがバス107を介して互いに接続されたコンピュータ(位置算出コントローラ)を用いて構成される。RAM103又はHDD104は、一時記憶装置に相当する。
FIG. 2 is a hardware configuration diagram of the
DR装置210は、コンピュータ(DRコントローラ)を用いて構成される。DRコントローラの入力段には、ダンプトラック1の運動量や姿勢を示すパラメータを計測するセンサ、操舵角センサ30の其々が接続される。
The
運動量を検出するセンサは、具体的には、車輪回転数を基に速度を計測する速度センサ201、ダンプトラック1のヨー角、ロール角、ピッチ角などの各軸の角度(姿勢)を計測もしくは算出する姿勢センサ202と、ダンプトラック1の加速度を計測する加速度センサ203、ダンプトラック1の角速度を計測する角速度センサ204である。
Specifically, the sensor that detects the momentum measures a
積載状態センサ20(重量センサに相当する)は、ベッセル3の積載状態を検出するセンサである。操舵角センサ30は、ダンプトラック1の直進旋回状態を検出するセンサである。勾配演算器40は、車輪速及び駆動輪に加わるトルクを基に、ダンプトラック1の走行路の勾配を演算する演算器である。
The loading state sensor 20 (corresponding to a weight sensor) is a sensor that detects the loading state of the
位置算出コントローラの入力I/F105はDR装置210に接続され、出力I/F106は自律走行制御装置300に接続される。自律走行制御装置300は位置算出装置100の位置算出結果の出力先となる外部装置に相当する。更に自律走行制御装置300の出力段には、加速装置及び制動装置を含む走行駆動装置400が接続される。DR装置210及び自律走行制御装置300を構成するコンピュータの具体的なハードウェア構成は位置算出装置100と同じである。
The input I /
加速度センサ203に代えて速度センサ201からの出力に対して時間微分演算を行い、加速度を演算で求める加速度演算器を備えてもよい。
Instead of the
図3は、位置算出装置100、DR装置210、及び自律走行制御装置300の機能構成を示すブロック図である。
FIG. 3 is a block diagram illustrating functional configurations of the
DR装置210は、速度センサ201、姿勢センサ202、加速度センサ203、及び角速度センサ204、及び操舵角センサ30からの出力を基に、ダンプトラック1の各時点の次の時刻における位置を求めて更新するDR位置演算部211を含む。
The
GNSSセンサ10はGNSS出力位置及び位置計測ステータスを含むGNSSセンサデータを位置算出装置100へ出力し、DR装置210は、DR位置演算部211が算出した更新位置データを位置算出装置100へ出力する。また、本実施形態では速度センサ201、加速度センサ203、積載状態センサ20、操舵角センサ30の検出値をDR装置210が位置算出装置100にも出力するものとする。
The
位置計測ステータスは、その時刻においてGNSSセンサ10からどのような精度で位置が算出されているかを示すデータである。例えば「4」なら公称誤差0.02mのRTK測位、「3」なら公称誤差0.5m、「1」なら公称誤差5mの単独測位、「0」なら非測位(測位データ無し)などである。ただし、位置計測ステータスは、上記に限らず位置の計測有無が判断できるデータであれば、ユーザーが任意に定義してよい。
The position measurement status is data indicating how accurately the position is calculated from the
位置算出装置100は、速度センサ201、加速度センサ203、積載状態センサ20、操舵角センサ30、及び勾配演算器40の出力を基に車両状態を決定する車両状態決定部132、GNSSセンサ10の計測結果の安定性を判断する位置計測安定性判断部114、確率フィルタを用いて自車位置を推定する最尤位置演算部115、過去のある時点から最新のサンプリング時点までに算出された推定位置の誤差分散値を算出する仮定DR誤差算出部112、一時記憶部131、適応DR誤差算出部142、存在可能性範囲算出部113、仮定DR位置算出部111、及び適応DR誤差パラメータ算出部141を含む。
The
最尤位置演算部115は、GNSSセンサ10及びDR装置210の出力を用いて確率フィルタ処理等を実行することにより、ダンプトラック1の現在の確率的な位置の分布を推定する。
The maximum likelihood
最尤位置演算部115が推定する確率的な位置の分布は、最も存在可能性が高い位置の絶対座標を中心とし、GNSSセンサ10とDR装置210の演算結果に含まれる仮定DR誤差算出部112により計算される誤差分散である。
The probabilistic position distribution estimated by the maximum likelihood
適応DR誤差算出部142は、GNSSセンサ10の位置の第1時点と第2時点での位置の差分とその間のDR装置210からの出力を比較することによりDRによって生じるモデル化誤差を適応DR誤差の平均値と誤差分散値として計算する。平均値及び誤差分散値は統計量の一例である。
The adaptive DR
一時記憶部131は、車両状態や運動の検出値等を一時的に記憶する。 The temporary storage unit 131 temporarily stores the vehicle state, the detected motion value, and the like.
存在可能性範囲算出部113は、仮定DR誤差算出部112の出力するDRによる誤差分散値(仮定DR位置の誤差分散)および適応DR誤差算出部142が出力する適応DR誤差の誤差分散値からダンプトラック1の存在可能性範囲である、仮定DR位置での誤差楕円と適応DR誤差の誤差楕円を算出する。
The existence possibility
自律走行制御装置300は、予め定められた目標軌道と最尤位置演算部115から取得した誤差楕円の最外縁との距離に応じて、目標軌道への復帰速度を算出する目標速度算出部301と、走行駆動装置400に対して目標速度に従って目標軌道に復帰させるための制御を実行する速度制御部302と、を備える。
The autonomous
上記各機能ブロックは、図2で示すハードウェアと上記各機能ブロックの機能を実現するソフトウェアとが協働して構成してもよいし、各機能ブロックを実現する回路を用いて構成してもよい。 Each functional block may be configured by the hardware shown in FIG. 2 and software that implements the function of each functional block, or may be configured by using a circuit that implements each functional block. Good.
次に図4を参照してダンプトラック1における位置算出装置100の動作について説明する。図4は、ダンプトラック1の動作の概要を示すフローチャートである。以下の処理は、位置算出装置100が第2時点において各センサから出力された検出値を用いて第2時点におけるダンプトラック1の存在可能性範囲を算出し、その算出過程において第2時点よりも前の第1時点で算出した位置との比較を行い、DRでの見積もり誤差が適正な範囲内にあるかを評価する例を示す。
Next, the operation of the
位置算出装置100はGNSSセンサ10からX(t)時点(第1時点)の出力を取得する(S100)。
The
DR装置210と位置算出装置100は、X(t)時点以降から車体センサ、より具体的には、速度センサ201、姿勢センサ202、加速度センサ203、角速度センサ204及び操舵角センサ30から車体センサ1サンプリング周期後の検出値を取得する(S101)。
The
また速度センサ201、姿勢センサ202、加速度センサ203、角速度センサ204、及び操舵角センサ30は時刻同期がとれており、少なくともGNSSセンサ10の計測間隔と同じか、短い計測間隔で同期をとりながら各センサが検出結果を出力する。
The
DR位置演算部211は、ステップS101で取得した各センサの出力を用いて、第1時点での位置からの位置変位量Δxを算出する(S102)。
The DR
DR位置演算部211は、各センサの検出値に基づいて1サンプル時間(Δt)の更新量δxを算出し、第1時点からの位置変位量Δxを計算する。
The DR
更新量δxは車体座標系から絶対座標系への変換行列Rθが決まった場合は、DR位置演算部211は下式(1)により算出する。
また、加速度センサ203が存在しない場合には、DR位置演算部211は下式(2)により更新量δxを算出する。
第1時点からの位置変位量Δxは下式(3)のように算出する。
次に仮定DR位置算出部111が第1時点での絶対位置x(t)に、ステップ102で得た位置変位量Δx(t+1)を足し合わせて仮定DR位置x(t+1)tildeを下式(4)により算出する(S103)。
車両状態決定部132は、速度センサ201、加速度センサ203、積載状態センサ20、操舵角センサ30、及び勾配演算器40からの出力を基に車両状態を決定し、決定した車両状態に応じて、1サンプル時間当たりの適応DR誤差の平均値と分散値を一時記憶部131から取得する(S104)。
The vehicle
車両状態は、積荷もしくは空荷を示す積載状態と、加速中もしくは減速中もしくは一定速度走行中を示す速度状態と、上り勾配走行中もしくは下り勾配走行中もしくは平坦地走行中を示す勾配状態と、直進走行中もしくは右旋回走行中もしくは左旋回走行中を示す直進旋回状態と、車輪滑り有りもしくは車輪滑り無しを示す車輪すべり状態と、のうちの少なくとも一つ又は少なくとも二つ以上の任意の組み合わせを用いて定義される。 The vehicle state includes a loading state indicating a load or an empty load, a speed state indicating that the vehicle is accelerating or decelerating or traveling at a constant speed, a gradient state indicating that the vehicle is traveling on an uphill, traveling on a downhill, or traveling on a flat ground, Any combination of at least one of at least one of at least one of at least one of a straight traveling state indicating straight traveling, a right turning traveling, or a left turning traveling and a wheel slip state indicating wheel slipping or no wheel slipping. It is defined using
車両状態決定部132は、各センサからの検出値を変数に持つ関数から、車両状態Λi(i=1、2、・・・、k)である確率P(Λi)を、下式(5)を用いて決定する。
関数fi(*)は事前に決められているものとし、単一の式でなく複数の式で構成されていても良い。また、入力値と検出値から機械学習を用いて車両状態を学習してもよい。また、Mは自重であるが、積荷状態と空荷状態を区別するだけのステータス値でも良い。車両状態決定部132は、算出された確率が最も高くなる状態を現在の車両状態として決定する。車両状態決定部132は、決定した車両状態を仮定DR誤差算出部112、及び適応DR誤差パラメータ算出部141の両方に出力する。
The function f i (*) is assumed to be determined in advance, and may be composed of a plurality of expressions instead of a single expression. Moreover, you may learn a vehicle state using machine learning from an input value and a detected value. 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
次に仮定DR誤差算出部112では、ステップ103で算出した仮定DR位置における誤差分散値を算出する(S105)。仮定DR位置の誤差分散値Σ(t+1)は、1サンプル時間前の仮定DR位置の誤差分散値Σ(t)に、予め設定されている1サンプル時間での更新量の誤差分散値W、および車両状態決定部132が決定する車両状態に応じた1サンプリング時間当たりの適応DR誤差の分散値E[ε2(t)]を足した値となる。仮定DR誤差算出部112は仮定DR位置の誤差分散値を以下の式(6)を用いて算出する。仮定DR誤差算出部112は、算出した仮定DR位置の誤差分散値Σ(t+1)を存在可能性範囲算出部113に出力し、ステップS107へ進む。
一方、適応DR誤差算出部142は、S103で算出した仮定DR位置における適応DR誤差の平均値の蓄積量とその誤差分散値を算出する(S106)。この適応DR誤差の蓄積量や誤差分散値はS104で得られる適応DR誤差の平均値E[ε(t)]と分散値E[ε2(t)]から算出する。この処理を適応DR誤差算出処理という。この処理内容の詳細は後述する。適応DR誤差算出部142は、算出した適応DR誤差を存在可能性範囲算出部113に出力し、ステップS107へ進む。
On the other hand, the adaptive DR
存在可能性範囲算出部113は、仮定DR位置とその誤差分散値、および適応DR誤差の平均値と誤差分散値から、GNSSセンサ10の出力する位置が存在する可能性のある位置を算出する(S107)。本ステップの処理を存在可能性範囲算出処理という。この処理内容の詳細は後述する。存在可能性範囲算出部113は、上記処理の結果、次の位置計測安定性判断部114で用いる、仮定DRの誤差楕円と適応DRの誤差楕円を算出する。
The existence possibility
位置計測安定性判断部114は、GNSSセンサ10からの出力の安定性を判断する(S108)。本ステップの処理を位置計測安定性判断処理という。この処理内容の詳述は後述する。位置計測安定性判断部114は、上記処理の結果、GNSSセンサ10からの出力が、予め定めた安定性を満たすと判断した場合には、ステップS101においてGNSSセンサ10から取得したGNSSセンサデータに値「1」からなる安定性フラグを付加し、上記安定性を満たさないと判断した場合には、GNSSセンサ10から取得したGNSSセンサデータに値「0」からなる安定性フラグを付加する。その後、ステップS109へと続く。
The position measurement
最尤位置演算部115は、確率フィルタを用いてダンプトラック1の最尤位置及び誤差分散値を演算し、出力I/F106を通して、自律走行制御装置300へ自車位置データを出力する(S109)。
The maximum likelihood
確率フィルタの代表的な方法としてカルマンフィルタや、パーティクルフィルタなどを挙げることができる。本実施形態では、GNSSセンサ10及びDR装置210においてそれぞれ算出した誤差分散から、カルマンフィルタにより最尤位置及び誤差分散値を求める。最尤位置演算部115が実行する最尤位置及びその誤差分散値の算出処理は後述する。
As a representative method of the stochastic filter, a Kalman filter, a particle filter, or the like can be given. 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
次に、適応DR誤差パラメータ算出部141は、GNSSセンサデータに付加された安定性フラグを参照し、安定性フラグの値が「1」であれば(S110/Yes)、適応DR誤差更新処理を実行する(S111)。適応DR誤差更新処理の詳細は後述する。
Next, the adaptive DR error
一方、適応DR誤差算出部142は、GNSSセンサデータに付加された安定性フラグの値が「0」であれば(S110/No)、適応DR誤差更新処理を実行することなく、一時記憶部131に適応DR誤差の平均値及び誤差分散値を書きこむ、所謂データ保持処理だけを実行する(S112)。
On the other hand, if the value of the stability flag added to the GNSS sensor data is “0” (S110 / No), the adaptive DR
次に、適応DR誤差パラメータ算出部141は、1ステップ前の車両状態(第1移動体状態に相当する)と現在の車両状態(第2移動体状態に相当する)の遷移確率Pを更新し、一時記憶部131へ保存する(S113)。この際、複数の車両状態の遷移確率が一定以上の場合、それらの車両状態は同一の車両状態であるとみなし、適応DR誤差の平均値および誤差分散値を融合する。この処理を車両状態更新処理といい、詳細は後述する。
Next, the adaptive DR error
位置算出装置100がGNSSセンサ10からX(t+1)時点の出力を受け取らなければ(S114/No)、第1時点X(t)を計測始点として維持したまま、S101〜S114の処理をGNSSサンプリング1周期分ループして処理し続ける(S115)。
If the
一方、位置算出装置100がGNSSセンサからX(t+1)時点の出力を受け取ると(S114/Yes)と、計測始点をX(t+1)に更新してS101からS114までの処理を繰り返す(S116)。
On the other hand, when the
<適応DR誤差算出処理>
次に仮定DR位置における適応DR誤差の蓄積量とその誤差分散値の算出について説明する。
<Adaptive DR error calculation process>
Next, the accumulation amount of adaptive DR error at the assumed DR position and the calculation of the error variance value will be described.
適応DR誤差算出部142は、S104にて決定した現在の車両状態における適応DR誤差の平均値と分散値を取得する。
The adaptive DR
次に適応DR誤差算出部142は、第1時点からのDR維持時間を算出する。DR維持時間は仮定DR位置の更新サンプル数nとし、適応DR誤差算出部142が仮定DR位置算出部111が起動するたびに起動しているとすれば、単純に前回値n_に1を足せばよい。よって、更新サンプル数nは下式(7)で表せる。
また、更新サンプル数nは車両状態によって変わるものとする。つまり、前サンプル時刻における車両状態と今回の車両状態が違った場合、n_=0として上記の算出を実行するものとする。 Further, the update sample number n is assumed to change depending on the vehicle state. That is, when the vehicle state at the previous sample time is different from the current vehicle state, the above calculation is executed with n_ = 0.
適応DR誤差算出部142は、nサンプル時間後に仮定DR位置の更新毎にDRにより生じる誤差の平均値を下式(8)、分散値を下式(9)により逐次算出する。
これらの処理が終了すれば、適応DR誤差算出部142の適応DR誤差算出処理を終了する。
When these processes are finished, the adaptive DR error calculation process of the adaptive DR
<存在可能性範囲算出処理>
存在可能性範囲算出部113は、仮定DR誤差と適応DR誤差を考慮した車両位置の存在する可能性がある範囲を算出する(S107)。存在可能性範囲は仮定DR位置算出部111にて算出される仮定DR位置(暫定的な位置である)及びその誤差分散と、適応DR誤差算出部142にて算出する適応DR誤差の分散値及び平均値を用いて算出することができる。
<Existence possibility range calculation processing>
The existence possibility
具体的には、仮定DR位置算出部111にて算出される仮定DR位置を(xe、ye)t、仮定DR位置での誤差分散をΣ、事前に決定されているマハラノビス距離をdとすると、仮定DR位置の存在範囲は下式(10)に示す楕円方程式範囲内である。
同様に、適応DR誤差の平均値をE[ε(t)]=(xd、yd)t、分散値をE[ε2(t)]として、適応DR誤差の存在範囲は下式(11)の楕円方程式範囲内となる。
よって、存在可能性範囲算出部113は、式(10)から得られる仮定DR位置の存在範囲(誤差楕円)に、式(11)から得られる適応DR誤差の存在範囲(誤差楕円)を加えた範囲を存在可能範囲として算出する。すなわち、存在可能性範囲は、これら二つの楕円の範囲となる。
Therefore, the existence possibility
図5に存在可能性範囲の概念図を示す。点1001が仮定DR位置、誤差楕円1002が式(10)により求められる仮定DR誤差の誤差楕円である。誤差楕円1002は仮定DR位置1001を中心としている。この誤差楕円1002は楕円内部に、マハラノビス距離dで規定された確率で位置が存在することを示している。また、式(11)で表わされる適応DR誤差の誤差楕円は、適応DR誤差の平均値[ε(t)]をベクトル1003で表せば、仮定DR位置を始点として、適応DR誤差の平均値[ε(t)]=(xd、yd)tで表わされるベクトルの終点としたベクトルとなり、この終点が適応DR誤差範囲を示す誤差楕円1005の中心1004であることを示している。つまり、適応DR誤差の誤差楕円1005の中心1004は仮定DR位置から適応DR誤差の平均値分だけずらした点を意味する。また、適応DR誤差の誤差分散E[ε2(t)]とマハラノビス距離dにより、適応DR誤差の誤差楕円の中心1004からの適応DR誤差の広がりを持つ楕円が式(11)により求まり、これが適応DR誤差の誤差楕円1005で表わされる。ダンプトラック1の位置の存在範囲は、仮定DR誤差の誤差楕円1002及び適応DR誤差の誤差楕円1005のいずれかもしくは両方の内側にあると考えればよい。
FIG. 5 shows a conceptual diagram of the existence possibility range. A point 1001 is an assumed DR position, and an
<位置計測安定性判断処理>
次に、ステップS108の位置計測安定性判断処理について図6を参照して説明する。本実施形態では位置計測安定性の判断を存在可能性範囲算出処理によって求めた誤差楕円により実施する。この安定性判断は他にも測位ステータスのみで行ってもよく、またχ2乗検定による棄却判断でも良い。図6は誤差楕円を用いた位置計測安定性判断処理の流れを示すフローチャートである。
<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 determination of the position measurement stability is performed using the error ellipse obtained by the existence possibility range calculation process. In addition, this stability determination may be performed based on only the positioning status, or may be a rejection determination based on a chi-square test. FIG. 6 is a flowchart showing the flow of position measurement stability determination processing using an error ellipse.
位置計測安定性判断部114は、GNSSセンサ10から取得した位置計測ステータスをチェックし、GNSSセンサ10からの出力が存在するかを判断する。GNSSセンサ10からの出力がある場合は(S201/Yes)、ステップS202に移行し、無い場合は(S201/No)、安定性フラグを「0」に設定する(S207)。
The position measurement
位置計測安定性判断部114は、S107で求めた仮定DR誤差の誤差楕円を取得する(S202)。
The position measurement
次に位置計測安定性判断部114は、GNSSセンサ10から出力される検出位置が仮定DR誤差の誤差楕円の範囲内であるかを判断する(S203)。
Next, the position measurement
この楕円の範囲内であるかの判断は式(6)の右辺(x、y)にGNSSセンサ10から出力される検出位置(X、Y)を代入し計算した結果が、マハラノビス距離dの2乗以下の場合は、仮定DR誤差の誤差楕円の範囲内であると判断(S203/Yes)し、安定性フラグを「1」に設定する(S206)。マハラノビス距離dの2乗より大きい場合は仮定DR誤差の誤差楕円の範囲外であると判断(S203/No)し、ステップS204へ移行する。
The determination as to whether it is within the range of the ellipse is made by substituting the detection position (X, Y) output from the
位置計測安定性判断部114は、GNSSセンサ10の検出値が仮定DR誤差の誤差楕円範囲外であった場合、S107で求めた適応DR誤差の誤差楕円を取得する(S204)。
If the detected value of the
次に位置計測安定性判断部114は、GNSSセンサ10から出力される検出位置が適応DR誤差の誤差楕円の範囲内であるかを判断する(S205)。
Next, the position measurement
この楕円の範囲内であるかの判断は式(7)の右辺に(x、y)にGNSSセンサ10から出力されるGNSS出力位置(X、Y)を代入し計算した結果が、マハラノビス距離dの2乗以下の場合は、適応DR誤差の誤差楕円の範囲内であると判断(S205/Yes)し、安定性フラグを「1」に設定する(S206)。マハラノビス距離dの2乗より大きい場合は適応DR誤差の誤差楕円の範囲外であると判断(S205/No)し、安定性フラグを「0」に設定する(S207)。そして位置計測安定性判断部114の処理を終了する。
The determination as to whether it is within the range of the ellipse is obtained by substituting the GNSS output position (X, Y) output from the
<推定位置及び推定位置の誤差分散値の算出処理>
次に、ステップS109の最尤位置及び最尤位置の誤差分散値の算出処理について図7を参照して説明する。図7は最尤位置演算部115が実行する最尤位置及び最尤位置の誤差分散値の算出処理の流れを示すフローチャートである。
<Calculation process of estimated position and error variance value of estimated position>
Next, the maximum likelihood position in step S109 and the error variance value calculation process for the maximum likelihood position will be described with reference to FIG. FIG. 7 is a flowchart showing the flow of processing for calculating the maximum likelihood position and the error variance value of the maximum likelihood position executed by the maximum likelihood
まず最尤位置演算部115は、位置計測安定性判断部114が設定した安定性フラグの値が「1」かどうかを判断する。安定性フラグの値が「1」の場合は(S301/Yes)、GNSSセンサ10からの検出位置が存在し、かつ外れ値でもないため、検出位置の誤差分散値を設定する(S302)。
First, the maximum likelihood
一方、最尤位置演算部115は、安定性フラグの値が「0」の場合は(S301/No)、GNSSセンサ10からの検出値が存在しない、もしくは外れ値であったため、GNSSセンサ10の検出位置の誤差分散値に予め決められた極大値を設定する。また、GNSSセンサ10の検出位置を仮定DR位置算出部111が算出した仮定DR位置と同じ値に設定する(S303)。
On the other hand, when the value of the stability flag is “0” (S301 / No), the maximum likelihood
次に最尤位置演算部115は、カルマンフィルタにおけるイノベーションベクトルνを算出する(S304)。イノベーションベクトルνはGNSSセンサ10の検出位置X及び仮定DR位置xtildeから下式(12)のように求まる。
最尤位置演算部115は、カルマンゲインKを算出する(S305)。カルマンゲインKは、仮定DR位置の誤差分散値が適応DR誤差の誤差分散値を含むことで、モデル化誤差を考慮でき、下式(13)により求められる。
次に最尤位置演算部115は、最尤位置をダンプトラック1の推定位置として算出する(S306)。最尤位置は、下式(14)により算出できる。
次に最尤位置演算部115は、最尤位置における誤差分散値P(t)を算出する(S307)。誤差分散値は下式(15)のように算出できる。
最後に最尤位置演算部115は、算出した最尤位置x(t)、誤差分散値P(t)から誤差楕円(「最尤誤差楕円」という)を計算し、それらを自律走行制御装置300へ出力する(S308)。
Finally, the maximum likelihood
最尤位置x(t)と誤差分散値P(t)から算出される最尤誤差楕円は下式(16)のように求めることが出来る。
最尤誤差楕円は、最尤位置x(t)を中心として、誤差分散値P(t)で規定される確率分布においてマハラノビス距離がdとなる等確率線を表わしている。最尤誤差楕円内にマハラノビス距離dで規定される確率で正しい位置(ダンプトラック1が実際に存在する位置)が存在することを表わしている。
The maximum likelihood error ellipse represents an isoprobability line with the Mahalanobis distance being d in the probability distribution defined by the error variance value P (t) with the maximum likelihood position x (t) as the center. This indicates that a correct position (position where the
自律走行制御装置300の目標速度算出部301は、最尤誤差楕円の最外縁部と目標軌道との乖離に応じて目標軌道に復帰する際の目標速度を決定し、速度制御部302が走行駆動装置400に対して目標速度で走行するための制御信号を出力する。また、速度制御部302は必要に応じて制動装置に対する制動信号や操舵モータに対する操舵角信号も出力する。
The target
また、上記の方法以外に、適応DR誤差の平均値および誤差楕円も出力することで、自律走行制御装置300は、適応DR誤差の平均値が最尤誤差楕円の内側にある場合は、最尤誤差楕円を基に速度を算出し、適応DR誤差の平均値が最尤誤差楕円の外側にある場合は、最尤位置に適応DR誤差の平均値を足した点を中心とした適応DR誤差の誤差楕円を基に速度を算出してもよい。
In addition to the above-described method, the average value of the adaptive DR error and the error ellipse are also output, so that the autonomous
上記の方法以外に、「2つの存在範囲を内包する楕円を生成して目標速度算出部301へ出力する」、「2つの位置の存在範囲を表わす分散を重ね合わせて一つの正規分布にフィッティングして目標速度算出部301へ出力する」、「2つの位置の存在範囲を両方とも目標速度算出部301へ出力する」などの方法でも同じ効果が得られる。
In addition to the above method, “generate an ellipse containing two existence ranges and output it to the target
位置算出装置100に接続される外部装置は自律走行制御装置300に限らない。例えばダンプトラック1の管制制御を行う管制サーバに対して、最尤誤差楕円を通知してもよい。
The external device connected to the
<適応DR誤差更新処理>
ステップS111における適応DR誤差パラメータ算出部141による適応DR誤差ε(t)およびその平均値、分散値の算出処理について図9に従って説明する。図9は、適応DR誤差更新の流れを示すフローチャートである。
<Adaptive DR error update process>
Processing for calculating the adaptive DR error ε (t), its average value, and variance value by the adaptive DR error
まず適応DR誤差ε(t)を、GNSSセンサ10の検出値とDRによって積算した移動量による値とを比較することにより算出する(S401)。図8に適応DR誤差の概念図を示す。
First, the adaptive DR error ε (t) is calculated by comparing the detected value of the
DRはそれ単体では絶対的な位置を表わすことができない。そこで、適応DR誤差パラメータ算出部141は、計測始点となる第1時点でのGNSSセンサ10の検出値701(X(t−1))にDR装置210によってステップS102にて算出された第1時点からの位置変位量Δx(t)により表わされるベクトル702を足し合わせた適応DR位置と、次の計測タイミングでGNSSセンサ10にて計測された位置703(X(t))との差分704によって求めることができる。時刻tにおける差分704を適応DR誤差ε(t)で表す。適応DR誤差ε(t)は、下式(17)により算出する。
次に適応DR誤差パラメータ算出部141は、ステップS104にて決定した現在の車両状態における、適応DR誤差の平均値を更新する(S402)。適応DR誤差パラメータ算出部141は、適応DR誤差ε(t)の平均値を一時記憶部131から取得した現在の車両状態の平均値E[ε(t−1)]を基に下式(18)で求める。
ここで、一時記憶部131のデータ構成を図10に示す。一時記憶部131には、車両状態と、それぞれに対応する適応DR誤差の平均値、分散値、及びそれらを算出したサンプル数が記録されている。 Here, the data structure of the temporary storage unit 131 is shown in FIG. The temporary storage unit 131 stores the vehicle state, the average value of the adaptive DR error corresponding to each vehicle, the variance value, and the number of samples calculated from them.
車両状態は、例えば、「積荷or空荷」「加速or減速or一定速度」「上り勾配走行or下り勾配走行or平坦地走行」「直進走行or右旋回走行or左旋回走行」「グリップorスリップ」などの状態の組み合わせで表現される。それぞれは関数f(*)内で表わされる。例えば「空荷or積荷」であれば、「M<0.1トン」を空荷、「M>=0.1トン」を積荷とするなどである。 The vehicle state may be, for example, “load or empty load”, “acceleration or deceleration or constant speed”, “uphill traveling or downgraded traveling or flat land traveling”, “straight traveling or right turning traveling or left turning traveling”, “grip or slip”. It is expressed by a combination of states such as “”. Each is represented in a function f (*). For example, for “empty or loaded”, “M <0.1 tons” is empty, “M> = 0.1 tons” is loaded, and so on.
また、適応DR誤差パラメータ算出部141は、適応DR誤差ε(t)の分散値E[ε2(t)]も更新する。適応DR誤差パラメータ算出部141は適応DR誤差ε(t)の分散値E[ε2(t)]を下式(19)により逐次求める。
次に適応DR誤差パラメータ算出部141は、算出した適応DR誤差の平均値及び分散値を一時記憶部131の該当する部分へ上書き更新する(S403)。これらの処理が終了すれば、適応DR誤差更新処理を終了する。
Next, the adaptive DR error
<車両状態更新処理>
適応DR誤差パラメータ算出部141は、車両状態の遷移確率P(Λt|Λt−1)を更新する。ある時刻における車両状態Λm(t−1)から次のサンプル時における車両状態Λn(t)への各車両状態への遷移確率P(Λmn(t|t−1))を下式(20)により算出する。
The adaptive DR error
これらを集めた遷移確率P(Λt|Λt−1)は下記行列(21)で表わされる。
車両状態の遷移確率は、上式をそのまま用いた場合は、値が安定しない場合も多いので、確率勾配法などを用いて、徐々に収束させるような手法をとってもよい。 If the above equation is used as it is, the transition probability of the vehicle state often does not stabilize, so a method of gradually converging using a probability gradient method or the like may be used.
ステップS403において、ある車両状態Λsからある車両状態Λu(t)までの遷移確率が両方向とも、閾値よりも高い場合は、車両状態に安定性が無いため、一つの車両状態へと融合する。その場合は、各車両状態に設定された適応DR誤差平均値と適応DR誤差分散値とをそれぞれのサンプル数を用いて、下式(22)、(23)のように更新する。
これらの処理が終了すれば、適応DR誤差パラメータ算出部141による車両状態更新処理を終了する。
When these processes are finished, the vehicle state update process by the adaptive DR error
本実施形態では、仮定DR位置の誤差分散値が適応DR誤差の誤差分散値を含むことで、カルマンゲインKがモデル化誤差を考慮し、このカルマンゲインKを用いて最尤誤差楕円を求める。そして最尤誤差楕円の最外縁と目標軌道との差を基に走行制御を行うので、スリップなどの考慮されていなかった現象が生じた場合でも適切な走行制御が可能となる。 In the present embodiment, since the error variance value of the assumed DR position includes the error variance value of the adaptive DR error, the Kalman gain K considers the modeling error, and the Kalman gain K is used to obtain the maximum likelihood error ellipse. 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 can be performed even when a phenomenon such as slip is not considered.
また特許文献2に記載発明では、近々の過去の車両の動作しか考慮されていないため、有る特定の車両状態の場合にオフセットが生じるとその検出が遅れる。これに対して、本実施形態によれば、車両状態毎に適応DR誤差を記憶しているので、いち早く誤差の検出が可能となる。
In addition, in the invention described in
更に本実施形態によれば、適応DR誤差を算出する際に、適応DR誤差の平均分誤差楕円の中心をずらし、誤差楕円そのものの大きさ(面積)は不変とする。これにより、誤差楕円そのものの大きさを大きくする(長軸、短軸の長さを変える)特許文献2の手法の比べて、より適切に誤差楕円を出力することができる。
Furthermore, according to the present embodiment, when calculating the adaptive DR error, the center of the average error ellipse of the adaptive DR error is shifted, and the size (area) of the error ellipse itself is unchanged. As a result, the error ellipse can be output more appropriately as compared with the method of
上記各実施形態は本発明を限定する趣旨ではなく、本発明の要旨を変更しない範囲の様々な態様は、本発明に含まれる。例えば、移動体として自律走行するダンプトラック1を例に挙げたが、異なる種類の作業機械であってもよい。また、各処理の実行順序は上記実施形態に限定されず、後に続く処理を妨げない範囲で処理の前後を入れ替えてもよい。
The above embodiments are not intended to limit the present invention, and various aspects within the scope not changing the gist of the present invention are included in the present invention. For example, although the
1 :ダンプトラック
2 :車体フレーム
3 :ベッセル
4 :運転室
5 :前輪
6 :後輪
7 :GNSSアンテナ
10 :GNSSセンサ
20 :積載状態センサ
30 :操舵角センサ
40 :勾配演算器
100 :位置算出装置
210 :デッドレコニング装置
300 :自律走行制御装置
400 :走行駆動装置
1: Dump truck 2: Body frame 3: Vessel 4: Cab 5: Front wheel 6: Rear wheel 7: GNSS antenna 10: GNSS sensor 20: Loading state 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 (6)
前記移動体の運動量及び姿勢を検出する移動体センサの検出値を用いて、初回は計測始点に当該計測始点からの変位量を、次回からは前回算出した相対位置に当該相対位置からの変位量を累積加算して、前記計測始点からの前記移動体の相対位置を逐次更新するデッドレコニング装置と、
前記移動体の速度、重量、前記移動体が走行する路面の勾配の少なくとも一つにより定義された移動体状態を検出する移動体状態センサと、
の其々に接続された、前記移動体の位置算出装置であって、
前記位置算出装置は、一時記憶装置を含む位置算出コントローラを備え、
前記位置算出コントローラは、
前記第1時点におけるGNSS出力位置、及び前記第1時点において前記デッドレコニング装置が算出した相対位置を用いて前記第1時点における前記移動体の最尤位置を計算し、
前記第1時点から前記第2時点までの間に前記デッドレコニング装置が算出した相対位置を前記第1時点における最尤位置に足し合わせて、前記第2時点における仮定デッドレコニング位置を算出し、
前記第1時点から前記第2時点までの間に前記デッドレコニング装置が算出した相対位置を前記第1時点におけるGNSS出力位置に足し合わせて、前記第2時点における適応デッドレコニング位置を算出し、
前記第2時点におけるGNSS出力位置と前記第2時点における適応デッドレコニング位置との差からなる適応デッドレコニング誤差を算出すると共に、前記第2時点における前記移動体状態センサからの検出値を基に移動体状態を決定し、
予め一時記憶装置に記憶された移動体状態と適応デッドレコニング誤差の平均値及び分散値とを関係付けた誤差統計量データのうち、前記決定された移動体状態に応じた適応デッドレコニング誤差の平均値及び分散値は、新たに算出された適応デッドレコニング誤差を用いて再計算して更新し、
前記第2時点における仮定デッドレコニング位置を中心とし仮定デッドレコニング誤差の分散値からなる誤差楕円に前記第2時点における仮定デッドレコニング位置から前記再計算された適応デッドレコニング誤差の平均値分ずれた位置を中心とし、前記再計算された適応デッドレコニング誤差の分散値からなる誤差楕円を加えて存在可能性範囲を算出し、
前記存在可能性範囲内に前記第2時点におけるGNSS出力位置が含まれている場合には、当該GNSS出力位置を用いて前記第2時点における最尤位置及び当該最尤位置を中心とする最尤誤差楕円を算出し、前記存在可能性範囲の外に前記第2時点におけるGNSS出力位置が存在する場合には、前記第2時点における仮定デッドレコニング位置を中心とする最尤誤差楕円を算出し、
前記最尤誤差楕円を前記移動体が存在する位置範囲として出力する、
ことを特徴とする位置算出装置。 A GNSS sensor that sequentially measures a GNSS output position of a moving object represented in a global coordinate system at each of a first time point and a later second time point;
Using the detection value of the moving body sensor that detects the momentum and posture of the moving body, the displacement amount from the measurement start point is set as the measurement start point for the first time, and the displacement amount from the relative position is calculated as the previous relative position from the next time. A dead reckoning device that sequentially updates the relative position of the moving body from the measurement start point,
A moving body state sensor that detects a moving body state defined by at least one of a speed, a weight of the moving body, and a gradient of a road surface on which the moving body travels;
Each of which is connected to each of the mobile body position calculating device,
The position calculation device includes a position calculation controller including a temporary storage device,
The position calculation controller includes:
Calculating the maximum likelihood 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;
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 device 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,
Calculates an 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, and moves based on the detection value from the moving body state sensor at the second time point. Determine your physical condition,
The average of adaptive dead reckoning errors according to the determined mobile body state, out of error statistic data relating the mobile body state previously stored in the temporary storage device and the average value and variance value of adaptive dead reckoning error. Values and variance values are recalculated and updated using newly calculated adaptive dead reckoning errors,
A position shifted 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 having a variance value of the assumed dead reckoning error centered on the assumed dead reckoning position at the second time point , And the existence possibility range is calculated by adding an error ellipse consisting of variance values 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 centered on the maximum likelihood position using the GNSS output position. An error ellipse is calculated, and if the GNSS output position at the second time point is outside the existence possibility range, a maximum likelihood error ellipse centered on the assumed dead reckoning position at the second time point is calculated.
Outputting the maximum likelihood error ellipse as a position range where the moving object exists;
A position calculation apparatus characterized by the above.
前記位置算出コントローラは、前記移動体状態センサの前記第1時点における検出値を基に前記第1時点における第1移動体状態を算出し、前記移動体状態センサの前記第2時点における検出値を基に前記第2時点における第2移動体状態を算出し、前記第1移動体状態から前記第2移動体状態への遷移確率を算出し、当該遷移確率が予め定められた閾値よりも高い場合は、前記第1移動体状態において算出された適応デッドレコニング誤差の平均値及び分散値と、前記第2移動体状態において算出された適応デッドレコニング誤差の平均値及び分散値と、を前記第2時点で算出された適応デッドレコニング誤差を加えて更新する、
ことを特徴とする位置算出装置。 The position calculation apparatus according to claim 1,
The position calculation controller calculates a first moving body state at the first time point based on a detection value at the first time point of the moving body state sensor, and calculates a detection 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 the transition probability from the first mobile state to the second mobile state, and the transition probability is higher than a predetermined threshold The average value and variance value of the adaptive dead reckoning error calculated in the first mobile state and the average value and variance value of the adaptive dead reckoning error calculated in the second mobile state are the second value. Update with the adaptive dead reckoning error calculated at the time,
A position calculation apparatus characterized by the above.
前記位置算出コントローラは、前記第2時点における前記GNSS出力位置と前記第2時点における仮定デッドレコニングとの乖離、又は前記第2時点における前記GNSS出力位置と前記第2時点における適応デッドレコニングとの乖離に基づいて、前記第2時点における前記GNSS出力位置の位置計測安定性を判断し、位置計測安定性はあると判断した場合にのみ、前記第2時点に出力された適応デッドレコニング誤差を用いて前記誤差統計量データを更新する、
ことを特徴とする位置算出装置。 The position calculation apparatus according to claim 1,
The position calculation controller is configured such that a difference 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. Updating the error statistic data;
A position calculation apparatus characterized by the above.
前記位置算出装置が算出した位置を基に自律走行制御を行う自律走行制御装置と、前記自律走行制御装置からの制御信号に基づいて動作する走行駆動装置と、を備えたダンプトラックであって、
前記移動体は前記ダンプトラックであり、
前記位置算出装置は、前記最尤誤差楕円を前記位置範囲として前記自律走行制御装置に出力する、
ことを特徴とするダンプトラック。 A position calculating device according to claim 1;
A dump truck comprising an autonomous traveling control device that performs autonomous traveling control based on the position calculated by the position calculating device, and a traveling drive device that operates based on a control signal from the autonomous traveling 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 characterized by that.
前記ダンプトラックは、
速度センサと、
加速度センサ又は速度センサの出力を基に前記ダンプトラックの加速度を演算する加速度演算器と、
積荷の積載状態を検出する積載状態センサと、
角速度センサと、
前記ダンプトラックの直進旋回状態を検出する操舵角センサと、
前記ダンプトラックの走行路の勾配を演算する勾配演算器と、のうちの少なくとも一つ以上を備え、
前記位置算出コントローラは、前記ダンプトラックの速度、加速度、積載状態、角速度、操舵角、又は勾配の少なくとも一つ以上を用いて車両状態を決定し、前記ダンプトラックの車両状態毎に生じる適応デッドレコニング誤差の大きさを学習する、
ことを特徴とするダンプトラック。 The dump truck according to claim 4,
The dump truck is
A speed sensor;
An acceleration calculator for calculating the acceleration of the dump truck based on the output of the acceleration sensor or the speed sensor;
A loading state sensor for detecting the loading state of the load;
An angular velocity sensor;
A steering angle sensor for detecting a straight-turning state of the dump truck;
A gradient calculator for calculating the gradient of the traveling path of the dump truck, and at least one of them,
The position calculation controller determines a 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 characterized by that.
前記位置算出コントローラは、逐次変化する車両状態に応じて学習された適応デッドレコニング誤差の分散値を選択し、
前記選択された適応デッドレコニング誤差の分散値が相対的に大きければ相対的に大きな最尤誤差楕円を生成し、前記選択された適応デッドレコニング誤差の分散値が相対的に小さければ相対的に小さな最尤誤差楕円を生成する、
ことを特徴とするダンプトラック。
The dump truck according to claim 5,
The position calculation controller selects a variance value of adaptive dead reckoning error learned according to a vehicle state that changes sequentially,
A relatively large maximum likelihood error ellipse is generated if the variance value of the selected adaptive dead reckoning error is relatively large, and relatively small if the variance value of the selected adaptive dead reckoning error is relatively small. Generate a maximum likelihood error ellipse,
A dump truck characterized by that.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018068450A JP6900341B2 (en) | 2018-03-30 | 2018-03-30 | Position calculation device and dump truck |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2018068450A JP6900341B2 (en) | 2018-03-30 | 2018-03-30 | Position calculation device and dump truck |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2019179421A true JP2019179421A (en) | 2019-10-17 |
JP6900341B2 JP6900341B2 (en) | 2021-07-07 |
Family
ID=68278583
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2018068450A Active JP6900341B2 (en) | 2018-03-30 | 2018-03-30 | Position calculation device and dump truck |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6900341B2 (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2021196214A (en) * | 2020-06-11 | 2021-12-27 | トヨタ自動車株式会社 | Position estimation apparatus and computer program for position estimation |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2002328157A (en) * | 2001-04-27 | 2002-11-15 | Pioneer Electronic Corp | Positioning error area setting device, positioning error area setting method, positioning error area setting processing program and navigation device |
JP2002341011A (en) * | 2001-05-11 | 2002-11-27 | Pioneer Electronic Corp | Position measuring device, navigation system, position measuring method and navigation method |
JP2014142272A (en) * | 2013-01-24 | 2014-08-07 | Clarion Co Ltd | Position detection device and program |
JP2016053916A (en) * | 2014-09-04 | 2016-04-14 | 日立建機株式会社 | Transport vehicle and travel control device thereof |
WO2018138904A1 (en) * | 2017-01-30 | 2018-08-02 | 三菱電機株式会社 | Position measurement device and position measurement method |
-
2018
- 2018-03-30 JP JP2018068450A patent/JP6900341B2/en active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2002328157A (en) * | 2001-04-27 | 2002-11-15 | Pioneer Electronic Corp | Positioning error area setting device, positioning error area setting method, positioning error area setting processing program and navigation device |
JP2002341011A (en) * | 2001-05-11 | 2002-11-27 | Pioneer Electronic Corp | Position measuring device, navigation system, position measuring method and navigation method |
JP2014142272A (en) * | 2013-01-24 | 2014-08-07 | Clarion Co Ltd | Position detection device and program |
JP2016053916A (en) * | 2014-09-04 | 2016-04-14 | 日立建機株式会社 | Transport vehicle and travel control device thereof |
WO2018138904A1 (en) * | 2017-01-30 | 2018-08-02 | 三菱電機株式会社 | Position measurement device and position measurement method |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2021196214A (en) * | 2020-06-11 | 2021-12-27 | トヨタ自動車株式会社 | Position estimation apparatus and computer program for position estimation |
JP7287353B2 (en) | 2020-06-11 | 2023-06-06 | トヨタ自動車株式会社 | Position estimation device and position estimation computer program |
US11828602B2 (en) | 2020-06-11 | 2023-11-28 | Toyota Jidosha Kabushiki Kaisha | Location estimating device, storage medium storing computer program for location estimation and location estimating method |
Also Published As
Publication number | Publication date |
---|---|
JP6900341B2 (en) | 2021-07-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US9645250B2 (en) | Fail operational vehicle speed estimation through data fusion of 6-DOF IMU, GPS, and radar | |
US9753144B1 (en) | Bias and misalignment compensation for 6-DOF IMU using GNSS/INS data | |
US10370004B2 (en) | System and method for determining state of stiffness of tires of vehicle | |
EP2077432B1 (en) | Moving object with tilt angle estimating mechanism | |
US9630672B2 (en) | Roll angle estimation device and transport equipment | |
US8041472B2 (en) | Positioning device, and navigation system | |
JP7036080B2 (en) | Inertial navigation system | |
US10625746B2 (en) | Self-position estimation method and self-position estimation device | |
JP2018036067A (en) | Own vehicle position recognition device | |
AU2015305864B2 (en) | Earthmoving machine comprising weighted state estimator | |
CN108627154B (en) | Polar region operation gesture and course reference system | |
JP2019184566A (en) | Vehicle and vehicle position estimation device | |
CN109677415B (en) | Apparatus and method for estimating radius of curvature of vehicle | |
US10429207B2 (en) | Pitch angular velocity correction value calculation device, attitude angle calculation device, and method for calculating pitch angular velocity correction value | |
JP6539129B2 (en) | Vehicle position estimation device, steering control device using the same, and vehicle position estimation method | |
JP2016206976A (en) | Preceding vehicle track calculation device for driving support control of vehicle | |
JP7069624B2 (en) | Position calculation method, vehicle control method and position calculation device | |
WO2017141469A1 (en) | Position estimation device | |
JP6900341B2 (en) | Position calculation device and dump truck | |
JP2019194037A (en) | Compartment line recognition device | |
CN112697153A (en) | Positioning method of autonomous mobile device, electronic device and storage medium | |
JP2007155365A (en) | Unit and program for computing correction factor of direction sensor | |
JP7028223B2 (en) | Self-position estimator | |
JP4831441B2 (en) | Correction coefficient calculation device and calculation program for direction sensor | |
CN114543795B (en) | Installation error estimation method and adjustment method for dual-antenna course angle and related equipment |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20200731 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20210521 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20210608 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20210616 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6900341 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |