JP2019179421A - Position calculation device and dump truck - Google Patents

Position calculation device and dump truck Download PDF

Info

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
Application number
JP2018068450A
Other languages
Japanese (ja)
Other versions
JP6900341B2 (en
Inventor
幹雄 板東
Mikio Bando
幹雄 板東
航 田中
Ko Tanaka
航 田中
石本 英史
Hidefumi Ishimoto
英史 石本
信一 魚津
Shinichi Uozu
信一 魚津
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hitachi Construction Machinery Co Ltd
Original Assignee
Hitachi Construction Machinery Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hitachi Construction Machinery Co Ltd filed Critical Hitachi Construction Machinery Co Ltd
Priority to JP2018068450A priority Critical patent/JP6900341B2/en
Publication of JP2019179421A publication Critical patent/JP2019179421A/en
Application granted granted Critical
Publication of JP6900341B2 publication Critical patent/JP6900341B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

To compensate for an unexpected error that occurs in a particular vehicle.SOLUTION: A maximum likelihood position at a first time point is calculated using each error variance value of a GNSS output position and a dead reckoning (DR) position; an assumed DR position is calculated by adding a relative position calculated by a DR device from the first time point to a second time point to the maximum likelihood position; an adaptive DR position is calculated by adding the relative position to the GNSS output position at the first time point; and a difference between the GNSS output position and the adaptive DR position is obtained as an adaptive DR error, and a vehicle state at that time is also determined. While the adaptive DR error corresponding to the vehicle state is stored and a newly calculated adaptive DR error is taken into account, an average value and a variance value of the adaptive DR error are updated; and if there is a GNSS output position within an existence possibility range obtained by adding an error ellipse centered on a position obtained by shifting the assumed DR position by the average value of the adaptive DR error to the error ellipse based on the assumed DR position, the maximum likelihood error ellipse is obtained based on the same, otherwise, based on the assumed DR position.SELECTED DRAWING: Figure 5

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 Patent Document 1, as a travel control device for a dump truck using an error ellipse, “a position calculation device that calculates a predicted position of the host vehicle, and a transport vehicle exists with a predetermined expected probability centered on the predicted position. An error estimation range calculation unit that calculates a position range to be calculated, and a maximum deviation amount calculation unit that calculates a maximum deviation amount among the deviation amounts between the target route of the transport vehicle and each point included in the position range, and When the maximum deviation is relatively large, a target vehicle speed determination unit that sets the target vehicle speed of the transport vehicle to be relatively small, and a target route that performs control for causing the transport vehicle to travel along the target route according to the target vehicle speed A configuration including a “follower (summary excerpt)” is disclosed.

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

米国特許出願公開第2017/017235号明細書US Patent Application Publication No. 2017/017235 特開2014−142272号公報JP 2014-142272 A

従来の確率フィルタの手法ではデッドレコニングでの見積もり誤差を信用して、誤差分散値を算出する仕組みになっている。そのため、デッドレコニングでの見積もり誤差が大きく誤っている、もしくはスリップなどの考慮されていない車両運動が発生した場合には、現実にそぐわない誤差楕円を出力することがあるという課題がある。このような予め設定された見積もり誤差であるデッドレコニングでの見積もり誤差自体に意図せず含まれる誤差を、モデル化誤差と呼ぶことにする。特許文献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 Patent Document 1, since traveling control is performed based on an error ellipse, if the modeling error included in the error ellipse is large, there is a possibility that actual traveling control may be hindered, and an error ellipse including this modeling error is output. By doing so, there is a desire to control the target vehicle speed. In addition, the vehicle motion, which is one of the factors causing the modeling error, also occurs due to a change in the vehicle state such as loading / unloading, but this change in the vehicle state is inherent to a specific vehicle. On the other hand, in Patent Document 2, since the error ellipse is corrected using an offset calculated based on the operation of another vehicle in the past, if a vehicle motion specific to a specific vehicle occurs, it is reflected in the offset. However, there remains a problem that detection of a newly generated error is delayed.

本発明は上記課題に鑑みてなされたものであり、特定の車両に生じる予期せざる誤差の補正を行って誤差楕円を生成することを目的とする。   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.

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

以下、図面を参照して本発明の実施形態について説明する。全図を通じて同一の構成には同一符号を付け、重複説明を省略する。   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 dump truck 1. As shown in FIG. 1, the dump truck 1 includes a vehicle frame 2 and a vessel 3 provided on the vehicle frame 2 so as to be raised and lowered. Further, a driver's cab 4 is provided above the front side of the vehicle body frame 2. In the lower part of the body frame 2, left and right front wheels 5 and rear wheels 6 are provided.

ダンプトラック1の前部にはGNSSアンテナ7を備える。図1ではGNSSアンテナ7を一つ図示しているが、実際には、ダンプトラック1の前部に車幅方向に間隔を空けて二つ以上のGNSSアンテナ7が設置されている。GNSSアンテナ7は、グローバル座標系で表されたダンプトラック1の絶対位置を逐次計測するGNSSセンサ10(図2参照)の構成要素である。   A GNSS antenna 7 is provided at the front portion of the dump truck 1. Although one GNSS antenna 7 is illustrated in FIG. 1, in reality, two or more GNSS antennas 7 are installed at the front portion of the dump truck 1 with a space 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 expressed in the global coordinate system.

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

ダンプトラック1は、GNSSセンサ10から出力される絶対座標系で表されたGNSS出力位置、及びDR装置210から出力される相対位置を基にダンプトラック1の自己位置を算出する位置算出装置100と、加速装置や制動装置を含む走行駆動装置400に対して位置算出装置100から取得した自己位置データを用いて自律走行制御を行う自律走行制御装置300と、を備える。   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 expressed in the absolute coordinate system output from the GNSS sensor 10 and the relative position output from the DR device 210. And an autonomous traveling control device 300 that performs autonomous traveling control using self-position data acquired from the position calculating device 100 for the traveling drive device 400 including an acceleration device and a braking device.

図2は、ダンプトラック1のハードウェア構成図である。位置算出装置100は、CPU101、ROM102、RAM103、HDD104、入力インターフェース(I/F)105、出力I/F106を含みこれらがバス107を介して互いに接続されたコンピュータ(位置算出コントローラ)を用いて構成される。RAM103又はHDD104は、一時記憶装置に相当する。   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, which are connected to each other via a bus 107 (position calculation controller). Is done. The RAM 103 or the HDD 104 corresponds to a temporary storage device.

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

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

積載状態センサ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 vessel 3. The steering angle sensor 30 is a sensor that detects the straight-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.

位置算出コントローラの入力I/F105はDR装置210に接続され、出力I/F106は自律走行制御装置300に接続される。自律走行制御装置300は位置算出装置100の位置算出結果の出力先となる外部装置に相当する。更に自律走行制御装置300の出力段には、加速装置及び制動装置を含む走行駆動装置400が接続される。DR装置210及び自律走行制御装置300を構成するコンピュータの具体的なハードウェア構成は位置算出装置100と同じである。   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 traveling control device 300 corresponds to an external device that is an output destination of the position calculation result of the position calculation device 100. Furthermore, a traveling drive device 400 including an acceleration device and a braking device is connected to the output stage of the autonomous traveling 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.

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

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

DR装置210は、速度センサ201、姿勢センサ202、加速度センサ203、及び角速度センサ204、及び操舵角センサ30からの出力を基に、ダンプトラック1の各時点の次の時刻における位置を求めて更新するDR位置演算部211を含む。   The DR device 210 obtains and updates the position of each dump truck 1 at the next time based on 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. DR position calculation unit 211 is included.

GNSSセンサ10はGNSS出力位置及び位置計測ステータスを含むGNSSセンサデータを位置算出装置100へ出力し、DR装置210は、DR位置演算部211が算出した更新位置データを位置算出装置100へ出力する。また、本実施形態では速度センサ201、加速度センサ203、積載状態センサ20、操舵角センサ30の検出値をDR装置210が位置算出装置100にも出力するものとする。   The GNSS sensor 10 outputs 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 updated position data calculated by the DR position calculation unit 211 to the position calculation device 100. In this embodiment, the DR device 210 also outputs the detection values of the speed sensor 201, the acceleration sensor 203, the loading state sensor 20, and the steering angle sensor 30 to the position calculation device 100.

位置計測ステータスは、その時刻において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 GNSS sensor 10 at that time. For example, “4” means RTK positioning with a nominal error of 0.02 m, “3” means nominal error of 0.5 m, “1” means 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 it is data that can determine whether or not the position is measured.

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

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

最尤位置演算部115が推定する確率的な位置の分布は、最も存在可能性が高い位置の絶対座標を中心とし、GNSSセンサ10とDR装置210の演算結果に含まれる仮定DR誤差算出部112により計算される誤差分散である。   The probabilistic position distribution estimated by the maximum likelihood position calculation unit 115 is centered on the absolute coordinates of the position 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. Is the error variance calculated by

適応DR誤差算出部142は、GNSSセンサ10の位置の第1時点と第2時点での位置の差分とその間のDR装置210からの出力を比較することによりDRによって生じるモデル化誤差を適応DR誤差の平均値と誤差分散値として計算する。平均値及び誤差分散値は統計量の一例である。   The adaptive DR error calculation unit 142 compares the difference in position between the first time point and the second time point of the position of the GNSS sensor 10 with the output from the DR device 210 between them, and converts the modeling error caused by the DR to the adaptive DR error. Is calculated as the mean and error variance. The average value and the error variance value are examples of statistics.

一時記憶部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 range calculation unit 113 dumps the error variance value (error variance at the assumed DR position) by the DR output from the assumed DR error calculation unit 112 and the error variance value of the adaptive DR error output from 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 possible range of the track 1, are calculated.

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

上記各機能ブロックは、図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 position calculation device 100 in 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 values output from the respective sensors at the second time point. An example is shown in which a comparison with the position calculated at the previous first time point is performed to evaluate whether the estimation error in DR is within an appropriate range.

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

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

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

DR位置演算部211は、ステップS101で取得した各センサの出力を用いて、第1時点での位置からの位置変位量Δxを算出する(S102)。   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).

DR位置演算部211は、各センサの検出値に基づいて1サンプル時間(Δt)の更新量δxを算出し、第1時点からの位置変位量Δxを計算する。   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.

更新量δxは車体座標系から絶対座標系への変換行列Rθが決まった場合は、DR位置演算部211は下式(1)により算出する。

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

また、加速度センサ203が存在しない場合には、DR位置演算部211は下式(2)により更新量δxを算出する。

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

第1時点からの位置変位量Δxは下式(3)のように算出する。

Figure 2019179421
The positional displacement amount Δx from the first time point is calculated as in the following equation (3).
Figure 2019179421

次に仮定DR位置算出部111が第1時点での絶対位置x(t)に、ステップ102で得た位置変位量Δx(t+1)を足し合わせて仮定DR位置x(t+1)tildeを下式(4)により算出する(S103)。

Figure 2019179421
Next, the assumed DR position calculation unit 111 adds the positional displacement amount Δx (t + 1) obtained in step 102 to the absolute position x (t) at the first time point to lower the assumed DR position x (t + 1) tilde. Calculation is performed using equation (4) (S103).
Figure 2019179421

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

車両状態は、積荷もしくは空荷を示す積載状態と、加速中もしくは減速中もしくは一定速度走行中を示す速度状態と、上り勾配走行中もしくは下り勾配走行中もしくは平坦地走行中を示す勾配状態と、直進走行中もしくは右旋回走行中もしくは左旋回走行中を示す直進旋回状態と、車輪滑り有りもしくは車輪滑り無しを示す車輪すべり状態と、のうちの少なくとも一つ又は少なくとも二つ以上の任意の組み合わせを用いて定義される。   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)を用いて決定する。

Figure 2019179421
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 detection value from each sensor as a variable by the following equation (5). To determine.
Figure 2019179421

関数f(*)は事前に決められているものとし、単一の式でなく複数の式で構成されていても良い。また、入力値と検出値から機械学習を用いて車両状態を学習してもよい。また、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 state determination unit 132 determines the state having the highest calculated probability as the current vehicle state. The vehicle state determination unit 132 outputs the determined vehicle state to both the assumed DR error calculation unit 112 and the adaptive DR error parameter calculation unit 141.

次に仮定DR誤差算出部112では、ステップ103で算出した仮定DR位置における誤差分散値を算出する(S105)。仮定DR位置の誤差分散値Σ(t+1)は、1サンプル時間前の仮定DR位置の誤差分散値Σ(t)に、予め設定されている1サンプル時間での更新量の誤差分散値W、および車両状態決定部132が決定する車両状態に応じた1サンプリング時間当たりの適応DR誤差の分散値E[ε(t)]を足した値となる。仮定DR誤差算出部112は仮定DR位置の誤差分散値を以下の式(6)を用いて算出する。仮定DR誤差算出部112は、算出した仮定DR位置の誤差分散値Σ(t+1)を存在可能性範囲算出部113に出力し、ステップS107へ進む。

Figure 2019179421
Next, the assumed DR error calculation unit 112 calculates an error variance value at the assumed DR position calculated in step 103 (S105). The error variance value Σ (t + 1) at the assumed DR position is set to the error variance value Σ (t) at the assumed DR position one sample time before, the error variance value W of the update amount at one sample time set in advance, and This is a value obtained by adding the variance value E [ε 2 (t)] of the adaptive DR error per sampling time corresponding to the vehicle state determined by the vehicle state determination unit 132. The assumed DR error calculation unit 112 calculates an error variance value of the assumed DR position using the following equation (6). The assumed DR error calculation unit 112 outputs the calculated error variance value Σ (t + 1) of the assumed DR position to the existence possibility range calculation unit 113, and proceeds to step S107.
Figure 2019179421

一方、適応DR誤差算出部142は、S103で算出した仮定DR位置における適応DR誤差の平均値の蓄積量とその誤差分散値を算出する(S106)。この適応DR誤差の蓄積量や誤差分散値はS104で得られる適応DR誤差の平均値E[ε(t)]と分散値E[ε(t)]から算出する。この処理を適応DR誤差算出処理という。この処理内容の詳細は後述する。適応DR誤差算出部142は、算出した適応DR誤差を存在可能性範囲算出部113に出力し、ステップS107へ進む。 On the other hand, the adaptive DR error calculation unit 142 calculates the accumulation amount of the average value of the adaptive DR error at the assumed DR position calculated in S103 and its error variance value (S106). The accumulated amount and error variance value of the adaptive DR error are calculated from the average value E [ε (t)] and the variance value E [ε 2 (t)] of the adaptive DR error obtained in S104. This processing is called adaptive DR error calculation processing. Details of this processing 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 the process proceeds to step S107.

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

位置計測安定性判断部114は、GNSSセンサ10からの出力の安定性を判断する(S108)。本ステップの処理を位置計測安定性判断処理という。この処理内容の詳述は後述する。位置計測安定性判断部114は、上記処理の結果、GNSSセンサ10からの出力が、予め定めた安定性を満たすと判断した場合には、ステップS101においてGNSSセンサ10から取得したGNSSセンサデータに値「1」からなる安定性フラグを付加し、上記安定性を満たさないと判断した場合には、GNSSセンサ10から取得したGNSSセンサデータに値「0」からなる安定性フラグを付加する。その後、ステップS109へと続く。   The position measurement stability determination unit 114 determines the stability of the output from the GNSS sensor 10 (S108). This process is called position measurement stability determination process. The details of this processing 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 position measurement stability determination unit 114 sets the value to the GNSS sensor data acquired from the GNSS sensor 10 in step S101. When a stability flag consisting of “1” is added and it is determined that the above-mentioned stability is not satisfied, a stability flag consisting of a value “0” is added to the GNSS sensor data acquired from the GNSS sensor 10. Then, it continues to step S109.

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

確率フィルタの代表的な方法としてカルマンフィルタや、パーティクルフィルタなどを挙げることができる。本実施形態では、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 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.

次に、適応DR誤差パラメータ算出部141は、GNSSセンサデータに付加された安定性フラグを参照し、安定性フラグの値が「1」であれば(S110/Yes)、適応DR誤差更新処理を実行する(S111)。適応DR誤差更新処理の詳細は後述する。   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). Details of the adaptive DR error update process will be described later.

一方、適応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 error calculation unit 142 performs the adaptive DR error update process without executing the adaptive DR error update process. Only the so-called data holding process of writing the average value and error variance value of the adaptive DR error is executed (S112).

次に、適応DR誤差パラメータ算出部141は、1ステップ前の車両状態(第1移動体状態に相当する)と現在の車両状態(第2移動体状態に相当する)の遷移確率Pを更新し、一時記憶部131へ保存する(S113)。この際、複数の車両状態の遷移確率が一定以上の場合、それらの車両状態は同一の車両状態であるとみなし、適応DR誤差の平均値および誤差分散値を融合する。この処理を車両状態更新処理といい、詳細は後述する。   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). Then, it is stored in the temporary storage unit 131 (S113). At this time, if the transition probabilities of the plurality of vehicle states are equal to or higher than a certain level, the vehicle states are regarded as the same vehicle state, and the average value and the error variance value of the adaptive DR error are merged. This process is called a vehicle state update process, and details will be described later.

位置算出装置100がGNSSセンサ10からX(t+1)時点の出力を受け取らなければ(S114/No)、第1時点X(t)を計測始点として維持したまま、S101〜S114の処理をGNSSサンプリング1周期分ループして処理し続ける(S115)。   If the position calculation device 100 does not receive an output at the time point X (t + 1) from the GNSS sensor 10 (S114 / No), the processing from S101 to S114 is performed with the first time point X (t) maintained as the measurement start point. The process continues by looping for a period (S115).

一方、位置算出装置100がGNSSセンサからX(t+1)時点の出力を受け取ると(S114/Yes)と、計測始点をX(t+1)に更新してS101からS114までの処理を繰り返す(S116)。   On the other hand, when the position calculation device 100 receives the output at the time point X (t + 1) from the GNSS sensor (S114 / Yes), the measurement start point is updated to X (t + 1) and the processing from S101 to S114 is repeated (S116).

<適応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 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.

次に適応DR誤差算出部142は、第1時点からのDR維持時間を算出する。DR維持時間は仮定DR位置の更新サンプル数nとし、適応DR誤差算出部142が仮定DR位置算出部111が起動するたびに起動しているとすれば、単純に前回値n_に1を足せばよい。よって、更新サンプル数nは下式(7)で表せる。

Figure 2019179421
Next, the adaptive DR error calculation unit 142 calculates the DR maintenance time from the first time point. If the DR maintenance time is assumed to be the number n of updated samples 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 adding 1 to the previous value n_ Good. Therefore, the update sample number n can be expressed by the following equation (7).
Figure 2019179421

また、更新サンプル数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)により逐次算出する。

Figure 2019179421
Figure 2019179421
The adaptive DR error calculation unit 142 sequentially calculates an average value of errors caused by DR every time the assumed DR position is updated after n sample times, and the variance value by the following formula (9).
Figure 2019179421
Figure 2019179421

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

<存在可能性範囲算出処理>
存在可能性範囲算出部113は、仮定DR誤差と適応DR誤差を考慮した車両位置の存在する可能性がある範囲を算出する(S107)。存在可能性範囲は仮定DR位置算出部111にて算出される仮定DR位置(暫定的な位置である)及びその誤差分散と、適応DR誤差算出部142にて算出する適応DR誤差の分散値及び平均値を用いて算出することができる。
<Existence possibility range calculation processing>
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 an assumed DR position (a provisional position) calculated by the assumed DR position calculation unit 111 and its error variance, a variance value of the adaptive DR error calculated by the adaptive DR error calculation unit 142, and It can be calculated using the average value.

具体的には、仮定DR位置算出部111にて算出される仮定DR位置を(xe、ye)t、仮定DR位置での誤差分散をΣ、事前に決定されているマハラノビス距離をdとすると、仮定DR位置の存在範囲は下式(10)に示す楕円方程式範囲内である。

Figure 2019179421
Specifically, assuming that the assumed DR position calculated by the assumed DR position calculation unit 111 is (xe, ye) t, the error variance at the assumed DR position is Σ, and the previously determined Mahalanobis distance is d. The existence range of the assumed DR position is within the elliptic equation range shown in the following equation (10).
Figure 2019179421

同様に、適応DR誤差の平均値をE[ε(t)]=(xd、yd)t、分散値をE[ε(t)]として、適応DR誤差の存在範囲は下式(11)の楕円方程式範囲内となる。

Figure 2019179421
Similarly, the average value of the adaptive DR error E [ε (t)] = (xd, yd) t, the variance value as E [ε 2 (t)] , the existence range of the adaptive DR errors following formula (11) Within the elliptic equation range.
Figure 2019179421

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

図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[ε(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 error ellipse 1002 is an error ellipse of an assumed DR error obtained by Expression (10). The error ellipse 1002 is centered on the assumed DR position 1001. This error ellipse 1002 indicates that a position exists within the ellipse with a probability defined by the Mahalanobis distance d. Further, the error ellipse of the adaptive DR error represented by the equation (11) can be expressed by calculating the average value [ε (t)] of the adaptive DR error with the vector 1003 and the average value of the adaptive DR error starting from the assumed DR position [ [epsilon] (t)] = (xd, yd) is a vector that is an end point of the vector represented by t, and this end point indicates 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. Also, an ellipse having an adaptive DR error spread from the center 1004 of the error ellipse of the adaptive DR error is obtained by the equation (11) based on the error variance E [ε 2 (t)] of the adaptive DR error and the Mahalanobis distance d. It is represented by an 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.

<位置計測安定性判断処理>
次に、ステップ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 stability determination unit 114 checks the position measurement status acquired from the GNSS sensor 10 and determines whether there is an output from the GNSS sensor 10. When there is an output from the GNSS sensor 10 (S201 / Yes), the process proceeds to step S202, and when there is no output (S201 / No), the stability flag is set to “0” (S207).

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

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

この楕円の範囲内であるかの判断は式(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 GNSS sensor 10 into the right side (x, y) of Equation (6), and the result of calculation is 2 of the Mahalanobis distance d. If it is less than or equal to the power, it is determined that it is within the error ellipse range of the assumed DR error (S203 / Yes), and the stability flag is set to "1" (S206). If it is larger than the square of Mahalanobis distance d, it is determined that it is outside the range of the error ellipse of the assumed DR error (S203 / No), and the process proceeds to step S204.

位置計測安定性判断部114は、GNSSセンサ10の検出値が仮定DR誤差の誤差楕円範囲外であった場合、S107で求めた適応DR誤差の誤差楕円を取得する(S204)。   If 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).

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

この楕円の範囲内であるかの判断は式(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 GNSS sensor 10 for (x, y) on the right side of the equation (7), and calculating the Mahalanobis distance d. If it is less than or equal to the square of, it is determined that it is within the error ellipse range of the adaptive DR error (S205 / Yes), and the stability flag is set to "1" (S206). If it is greater than the square of Mahalanobis distance d, it is determined that it is outside the range of the error ellipse of the adaptive DR error (S205 / No), and the stability flag is set to “0” (S207). And the process of the position measurement stability judgment part 114 is complete | finished.

<推定位置及び推定位置の誤差分散値の算出処理>
次に、ステップ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 position calculation unit 115.

まず最尤位置演算部115は、位置計測安定性判断部114が設定した安定性フラグの値が「1」かどうかを判断する。安定性フラグの値が「1」の場合は(S301/Yes)、GNSSセンサ10からの検出位置が存在し、かつ外れ値でもないため、検出位置の誤差分散値を設定する(S302)。   First, the maximum likelihood position calculation unit 115 determines whether 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), since the detection position from the GNSS sensor 10 exists and is not an outlier, an error variance value of the detection position is set (S302).

一方、最尤位置演算部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 position calculation unit 115 does not have a detection value from the GNSS sensor 10 or is an outlier. A predetermined maximum value is set as the error variance value at the detection position. Further, the detection position of the GNSS sensor 10 is set to the same value as the assumed DR position calculated by the assumed DR position calculation unit 111 (S303).

次に最尤位置演算部115は、カルマンフィルタにおけるイノベーションベクトルνを算出する(S304)。イノベーションベクトルνはGNSSセンサ10の検出位置X及び仮定DR位置xtildeから下式(12)のように求まる。

Figure 2019179421
Next, the maximum likelihood position calculation unit 115 calculates an innovation vector ν in the Kalman filter (S304). The innovation vector ν is obtained from the detection position X of the GNSS sensor 10 and the assumed DR position xtilde as shown in the following expression (12).
Figure 2019179421

最尤位置演算部115は、カルマンゲインKを算出する(S305)。カルマンゲインKは、仮定DR位置の誤差分散値が適応DR誤差の誤差分散値を含むことで、モデル化誤差を考慮でき、下式(13)により求められる。

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

次に最尤位置演算部115は、最尤位置をダンプトラック1の推定位置として算出する(S306)。最尤位置は、下式(14)により算出できる。

Figure 2019179421
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).
Figure 2019179421

次に最尤位置演算部115は、最尤位置における誤差分散値P(t)を算出する(S307)。誤差分散値は下式(15)のように算出できる。

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

最後に最尤位置演算部115は、算出した最尤位置x(t)、誤差分散値P(t)から誤差楕円(「最尤誤差楕円」という)を計算し、それらを自律走行制御装置300へ出力する(S308)。   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 to calculate the autonomous traveling control device 300. (S308).

最尤位置x(t)と誤差分散値P(t)から算出される最尤誤差楕円は下式(16)のように求めることが出来る。

Figure 2019179421
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).
Figure 2019179421

最尤誤差楕円は、最尤位置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 dump truck 1 actually exists) exists with a probability defined by the Mahalanobis distance d within the maximum likelihood error ellipse.

自律走行制御装置300の目標速度算出部301は、最尤誤差楕円の最外縁部と目標軌道との乖離に応じて目標軌道に復帰する際の目標速度を決定し、速度制御部302が走行駆動装置400に対して目標速度で走行するための制御信号を出力する。また、速度制御部302は必要に応じて制動装置に対する制動信号や操舵モータに対する操舵角信号も出力する。   The target speed calculation unit 301 of the autonomous travel control device 300 determines a target speed when returning to the target trajectory according to the difference between the outermost edge of the maximum likelihood error ellipse and the target trajectory, and the speed control unit 302 drives the travel. A control signal for traveling at the 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 as necessary.

また、上記の方法以外に、適応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 traveling control apparatus 300 can obtain the maximum likelihood when the average value of the adaptive DR error is inside the maximum likelihood error ellipse. When the velocity is calculated based on the error ellipse and the average value of the adaptive DR error is outside the maximum likelihood error ellipse, the adaptive DR error centered on the point obtained by adding the average value of the adaptive DR error to the maximum likelihood position is calculated. The speed may be calculated based on the error ellipse.

上記の方法以外に、「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 speed calculation unit 301”, “superimpose the variances representing the existence ranges of two positions and fit them into one normal distribution. The same effect can be obtained by methods such as “output to target speed calculation unit 301” and “output both ranges of two positions to target speed calculation unit 301”.

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

<適応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 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.

まず適応DR誤差ε(t)を、GNSSセンサ10の検出値とDRによって積算した移動量による値とを比較することにより算出する(S401)。図8に適応DR誤差の概念図を示す。   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 accumulated by the DR (S401). FIG. 8 shows a conceptual diagram of adaptive DR error.

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)により算出する。

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

次に適応DR誤差パラメータ算出部141は、ステップS104にて決定した現在の車両状態における、適応DR誤差の平均値を更新する(S402)。適応DR誤差パラメータ算出部141は、適応DR誤差ε(t)の平均値を一時記憶部131から取得した現在の車両状態の平均値E[ε(t−1)]を基に下式(18)で求める。

Figure 2019179421
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 average value of the adaptive DR error ε (t) from the temporary storage unit 131. )
Figure 2019179421

ここで、一時記憶部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[ε(t)]も更新する。適応DR誤差パラメータ算出部141は適応DR誤差ε(t)の分散値E[ε(t)]を下式(19)により逐次求める。

Figure 2019179421
The adaptive DR error parameter calculation unit 141 also updates the variance value E [ε 2 (t)] of the adaptive DR error ε (t). Adaptive DR error parameter calculation unit 141 sequentially determines the variance E [ε 2 (t)] The following equation of the adaptive DR error ε (t) (19).
Figure 2019179421

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

<車両状態更新処理>
適応DR誤差パラメータ算出部141は、車両状態の遷移確率P(Λt|Λt−1)を更新する。ある時刻における車両状態Λm(t−1)から次のサンプル時における車両状態Λn(t)への各車両状態への遷移確率P(Λmn(t|t−1))を下式(20)により算出する。

Figure 2019179421
<Vehicle state update processing>
The adaptive DR error parameter calculation unit 141 updates the vehicle state transition probability P (Λt | Λt−1). 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 expressed by the following equation (20). calculate.
Figure 2019179421

これらを集めた遷移確率P(Λt|Λt−1)は下記行列(21)で表わされる。

Figure 2019179421
The collected transition probability P (Λt | Λt−1) is expressed by the following matrix (21).
Figure 2019179421

車両状態の遷移確率は、上式をそのまま用いた場合は、値が安定しない場合も多いので、確率勾配法などを用いて、徐々に収束させるような手法をとってもよい。   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)のように更新する。

Figure 2019179421
Figure 2019179421
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 therefore, it is merged 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 shown in the following equations (22) and (23) using the respective sample numbers.
Figure 2019179421
Figure 2019179421

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

本実施形態では、仮定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 Patent Document 2, only the operation of the vehicle in the past is taken into consideration, so that detection is delayed when an offset occurs in a specific vehicle state. On the other hand, according to this embodiment, since the adaptive DR error is stored for each vehicle state, the error can be detected quickly.

更に本実施形態によれば、適応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 Patent Document 2 in which the size of the error ellipse itself is increased (the major axis and the minor axis are changed in length).

上記各実施形態は本発明を限定する趣旨ではなく、本発明の要旨を変更しない範囲の様々な態様は、本発明に含まれる。例えば、移動体として自律走行するダンプトラック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 dump truck 1 that autonomously travels as a moving body has been described as an example, different types of work machines may be used. Moreover, the execution order of each process is not limited to the said embodiment, You may replace the front and back of a process in the range which does not prevent the subsequent process.

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)

グローバル座標系で表された移動体の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時点における仮定デッドレコニング位置を中心とする最尤誤差楕円を算出し、
前記最尤誤差楕円を前記移動体が存在する位置範囲として出力する、
ことを特徴とする位置算出装置。
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時点における第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.
請求項1に記載の位置算出装置において、
前記位置算出コントローラは、前記第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.
請求項1に記載の位置算出装置と、
前記位置算出装置が算出した位置を基に自律走行制御を行う自律走行制御装置と、前記自律走行制御装置からの制御信号に基づいて動作する走行駆動装置と、を備えたダンプトラックであって、
前記移動体は前記ダンプトラックであり、
前記位置算出装置は、前記最尤誤差楕円を前記位置範囲として前記自律走行制御装置に出力する、
ことを特徴とするダンプトラック。
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.
請求項4に記載のダンプトラックにおいて、
前記ダンプトラックは、
速度センサと、
加速度センサ又は速度センサの出力を基に前記ダンプトラックの加速度を演算する加速度演算器と、
積荷の積載状態を検出する積載状態センサと、
角速度センサと、
前記ダンプトラックの直進旋回状態を検出する操舵角センサと、
前記ダンプトラックの走行路の勾配を演算する勾配演算器と、のうちの少なくとも一つ以上を備え、
前記位置算出コントローラは、前記ダンプトラックの速度、加速度、積載状態、角速度、操舵角、又は勾配の少なくとも一つ以上を用いて車両状態を決定し、前記ダンプトラックの車両状態毎に生じる適応デッドレコニング誤差の大きさを学習する、
ことを特徴とするダンプトラック。
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.
請求項5に記載のダンプトラックにおいて、
前記位置算出コントローラは、逐次変化する車両状態に応じて学習された適応デッドレコニング誤差の分散値を選択し、
前記選択された適応デッドレコニング誤差の分散値が相対的に大きければ相対的に大きな最尤誤差楕円を生成し、前記選択された適応デッドレコニング誤差の分散値が相対的に小さければ相対的に小さな最尤誤差楕円を生成する、
ことを特徴とするダンプトラック。
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.
JP2018068450A 2018-03-30 2018-03-30 Position calculation device and dump truck Active JP6900341B2 (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (5)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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