JP2021156678A - 作業車両 - Google Patents

Abstract

【課題】従来の装置よりも柔軟にアンテナの設置位置の誤差を検出することが可能な作業車両を提供する。
【解決手段】作業車両は制御装置１５０を備える。制御装置１５０は、検出機能Ｆ１０６と、算出機能Ｆ１０４と、算出機能Ｆ１０５と、算出機能Ｆ１０７と、推定機能Ｆ１１０と、を有する。検出機能Ｆ１０６は、車両の速度、加速度、および角速度に基づいて定常走行を検出する。算出機能Ｆ１０４は、車両に対する第１アンテナおよび第２アンテナの設置情報に基づいて第１車両方向を算出する。算出機能Ｆ１０５は、定常走行が検出された場合に、第１アンテナの位置情報の時間変化に基づいて第２車両方向を算出する。算出機能Ｆ１０７は、第２車両方向に基づいて第１車両方向を修正するための方向修正パラメータを算出する。推定機能Ｆ１１０は、方向修正パラメータと第１車両方向に基づいて車両の位置および姿勢を推定する。
【選択図】図２

Description

本開示は、作業車両に関する。

従来からＧＰＳ衛星等の測位用信号を送信する送信機からの電波を受信してキャリアディファレンシャル測位を行う際のキャリア位相整数値バイアスを決定する方法、物体や移動体の方位・姿勢を計測する方法および装置に関する発明が知られている（下記特許文献１を参照）。

特許文献１に記載された方位・姿勢計測装置は、キャリア位相整数値バイアス決定装置と、方位または姿勢を求める手段と、から成る（請求項５等）。前記キャリア位相整数値バイアス決定装置は、第１・第２のアンテナと、アンテナ方位観測手段と、算出、修正、および決定を行う手段と、を備える（請求項４等）。上記アンテナ方位観測手段は、複数の測位用送信機からの測位用信号を、第１・第２のアンテナでそれぞれ受信し、測位用信号のキャリア位相変化量に基づいて第１のアンテナに対する第２のアンテナの方位を求める。

上記算出、修正、および決定を行う手段は、次のように動作する。まず、第１のアンテナに対する第２のアンテナの方位、第１・第２のアンテナ間の距離、および測位用送信機の位置から求まる計算上のキャリア位相差である計算位相差を算出する。また、第１・第２のアンテナで受信した前記測位用信号のキャリア位相差の小数部である観測位相差を求め、前記計算位相差の小数部が±０．５サイクルの範囲で前記観測位相差と一致するように前記計算位相差を修正する。そして、該修正後の計算位相差を前記観測位相差の整数値バイアスとして決定する（請求項４等）。

上記方位または姿勢を求める手段は、上記キャリア位相整数値バイアス決定装置で求められた整数値バイアスと前記観測位相差とから第１のアンテナに対する第２のアンテナの相対位置を求め、該相対位置から第１・第２のアンテナ間の方位または姿勢を求める（請求項５等）。

また、上記方位・姿勢計測装置は、前記第１・第２のアンテナを、略水平面を前後方向に進行する移動体に設け、前記アンテナ方位観測手段が、前記移動体の移動にともなう第１・第２のアンテナの移動による前記測位用信号のキャリア位相変化量に基づいて前記移動体の進行方位を観測する。さらに、この装置は、該進行方位を移動体の前方方位と見なして、該移動体の前方方位と移動体に対する第１・第２のアンテナの取り付け位置関係から第１のアンテナに対する第２のアンテナの方位を観測する（請求項７等）。

特開２００５‐０４３２１２号公報

たとえば、ダンプトラックなどの作業車両は、車両の方向を計測するために、上記従来の方位・姿勢計測装置と同様に、全球測位衛星システム（ＧＮＳＳ）などの衛星測位システムの電波を受信する第１のアンテナと第２のアンテナを備える。このようなアンテナは、たとえば、車体によって電波が遮蔽されるのを回避するために、車体から上方へ延びるポールの先端に設置される。

また、作業車両は、たとえば、鉱石や土砂などを積載したり、その積載物を降ろしたり、凹凸の多い地面の上を走行したりすることで、アンテナに振動や衝撃が作用することがある。そのため、時間の経過にともなって、アンテナの実際の設置位置と、あらかじめ設定された設置位置との間に誤差が生じ、その設定された設置位置に基づいて算出される車両の方向と、実際の車両の方向との間に誤差が生じることがある。

このような場合、前記従来の方位・姿勢計測装置は、略水平面を前後方向に進行する移動体の移動にともなう第１・第２のアンテナの移動に基づいて、移動体の進行方位を観測する。しかし、作業車両が走行する鉱石の採掘現場や建設現場では、作業車両が略水平面を前後方向に進行する機会は限定されている。そのため、作業車両においては、このような限定された条件ではなく、より柔軟にアンテナの設置位置の誤差を検出することが求められている。

本開示は、従来の装置よりも柔軟にアンテナの設置位置の誤差を検出することが可能な作業車両を提供する。

本開示の一態様は、車両と、前記車両に取り付けられて衛星測位システムの電波を受信する第１アンテナおよび第２アンテナと、前記電波に基づく前記第１アンテナの位置情報および前記第１アンテナと前記第２アンテナとを結ぶ基線方向を出力する受信機と、前記車両の速度、加速度、および角速度を計測するセンサと、前記車両の位置および姿勢を推定する制御装置と、を備えた作業車両であって、前記制御装置は、前記速度、前記加速度、および前記角速度に基づいて定常走行を検出する検出機能と、前記車両に対する前記第１アンテナおよび前記第２アンテナの設置情報に基づいて第１車両方向を算出する機能と、前記定常走行が検出された場合に、前記第１アンテナの前記位置情報の時間変化に基づいて第２車両方向を算出する機能と、前記第２車両方向に基づいて前記第１車両方向を修正するための方向修正パラメータを算出する機能と、前記方向修正パラメータと前記第１車両方向に基づいて前記車両の前記位置および前記姿勢を推定する推定機能と、を有する作業車両である。

本開示の上記一態様によれば、従来の装置よりも柔軟にアンテナの設置位置の誤差を検出することが可能な作業車両を提供することができる。

本開示に係る作業車両の実施形態１を示す側面図。 図１の作業車両の制御装置の機能ブロック図。 図１の作業車両の制御装置による処理の一例を示すフロー図。 図３のセンサ情報を記憶する処理を説明する表。 図３の位置および方向を記憶する処理を説明する表。 図３の定常走行を検出する処理のフロー図。 第１および第２アンテナ間の基線方向と車両方向との関係を示す模式図。 図３の第１車両方向を算出する処理のフロー図。 図８の処理で取得される方向修正パラメータを説明する模式図。 図３の第２車両方向を算出する処理のフロー図。 定常走行に含まれる旋回の一例を示す模式図。 車両の回転中心を説明する模式図。 図３の方向修正パラメータを算出する処理のフロー図。 方向修正パラメータを説明する模式図。 本開示に係る作業車両の実施形態２における制御装置の機能ブロック図。 図１５の制御装置の定常走行検出機能による処理の一例を示すフロー図。 図１５の制御装置の走行制御機能による処理の一例を示すフロー図。

以下、図面を参照して本開示に係る作業車両の実施形態を説明する。

［実施形態１］
図１は、本開示に係る作業車両の実施形態１を示す側面図である。図２は、図１の作業車両１００の制御装置１５０の機能ブロック図である。本実施形態の作業車両１００は、たとえば、鉱石の採掘現場や建設現場で使用されるダンプトラックである。作業車両１００は、たとえば、車両１１０と、測位装置１２０と、センサ１３０と、ベッセル１４０と、制御装置１５０とを備えている。

車両１１０は、たとえば、車体フレーム１１１と、車輪１１２と、キャビン１１３とを備えている。車体フレーム１１１は、たとえば、ラダー状の構造体である。車体フレーム１１１は、たとえば、車軸に取り付けられた左右の車輪１１２を、サスペンションを介して支持している。また、車体フレーム１１１は、たとえば、図示を省略するエンジン、発電機、モータ、動力伝達機構、操舵機構、油圧装置、および車両制御用のアクチュエータなどを支持している。

車輪１１２は、たとえば、動力伝達機構を介してモータに接続され、モータによって駆動されて車両１１０を走行させる。キャビン１１３は、作業車両１００のオペレータが搭乗するための車室である。キャビン１１３の内部には、たとえば、図示を省略するステアリングホイール、操作ペダル、操作レバー、情報装置、スピーカ、計器、表示ランプなどが設置されている。

測位装置１２０は、たとえば、全球測位衛星システム（ＧＮＳＳ）などの衛星測位システムによって構成することができる。測位装置１２０は、たとえば、第１アンテナ１２１と、第２アンテナ１２２と、受信機１２３とを備えている。第１アンテナ１２１および第２アンテナ１２２は、たとえば、車両１１０に取り付けられ、ＧＮＳＳなどの衛星測位システムの電波を受信する。

第１アンテナ１２１と第２アンテナ１２２は、たとえば車両１１０の前後方向および高さ方向に直交する車両１１０の幅方向に離隔して設置される。第１アンテナ１２１および第２アンテナ１２２は、たとえば、車両１１０に固定されたポールの先端に取り付けられ、ベッセル本体１４１の前端部１４１ａの定常位置よりも高い位置に設置される。

受信機１２３は、たとえば、第１アンテナ１２１および第２アンテナ１２２に対して信号ケーブルを介して接続されている。受信機１２３は、第１アンテナ１２１および第２アンテナ１２２が受信した電波に基づいて、第１アンテナ１２１の位置情報、および、第１アンテナ１２１と第２アンテナ１２２との間の基線方向を出力する。ここで、基線方向とは、たとえば、第１アンテナ１２１の設置位置と、第２アンテナ１２２の設置位置とを結ぶ直線の方向である。

センサ１３０は、たとえば、速度センサ１３１と、加速度センサ１３２と、角速度センサ１３３と、を含む。また、図１および図２に示す例において、センサ１３０は、たとえば、昇降センサ１３４を含んでいる。これらのセンサ１３０は、たとえば、コントロールエリアネットワーク（ＣＡＮ）を介して制御装置１５０と通信可能に接続されている。なお、測位装置１２０によって測定された位置に基づいて、制御装置１５０によって車両１１０の速度を算出する場合は、測位装置１２０を速度センサとして用い、速度センサ１３１を省略してもよい。

速度センサ１３１は、たとえば、車輪１１２の回転速度に基づいて車両１１０の速度を検知し、その速度を制御装置１５０へ出力する。加速度センサ１３２は、たとえば、重力加速度を除いた車両１１０の加速度を検知し、その加速度を制御装置１５０へ出力する。角速度センサ１３３は、たとえば、車両１１０の角速度を検知し、その角速度を制御装置１５０へ出力する。昇降センサ１３４は、たとえば、ベッセル本体１４１を昇降させる昇降シリンダ１４２のストロークを含むベッセル本体１４１の昇降状態を検知し、その昇降状態を制御装置１５０へ出力する。

ベッセル１４０は、たとえば、ベッセル本体１４１と、昇降シリンダ１４２と、回転軸１４３とを有している。ベッセル本体１４１は、たとえば、底部の後方側に設けられた回転軸１４３を中心に回転可能に車体フレーム１１１の上に支持されている。ベッセル本体１４１は、たとえば、作業車両１００において、鉱石、岩石、砂利、土砂などの積載物を積載して運搬するための部分である。

昇降シリンダ１４２は、たとえば、車両１１０の幅方向の両側に設けられた一対の油圧シリンダである。昇降シリンダ１４２は、ピストンロッドの先端がベッセル本体１４１の底部の回転軸１４３よりも前方に連結され、ピストンロッドと反対側のシリンダチューブの端部が車体フレーム１１１の下部に連結されている。昇降シリンダ１４２は、制御装置１５０によって制御される油圧装置の一部を構成する。昇降シリンダ１４２は、ピストンロッドを伸縮させることで、回転軸１４３を中心としてベッセル本体１４１を回動させ、ベッセル本体１４１の前端部１４１ａを昇降させる。

制御装置１５０は、たとえば、マイクロコントローラまたはファームウェアなどのコンピュータシステムである。制御装置１５０は、たとえば、中央処理装置（ＣＰＵ）、ＲＡＭやＲＯＭなどの記憶装置、その記憶装置に記憶されたプログラムおよびデータ、タイマーならびに信号の入出力を行う入出力部などによって構成されている。制御装置１５０とセンサ１３０は、たとえば、それぞれ時刻同期されて一定の周期で動作しているため、時刻のずれによる信号の取り零しが防止されている。また、制御装置１５０の駆動周期は、たとえば、センサ１３０の最も短い出力周期と等しくすることができる。

制御装置１５０は、たとえば、図２に示すように作業車両１００の位置および姿勢を算出する機能Ｆ１００と、作業車両１００の状態を監視する機能Ｆ２００とを有している。制御装置１５０のこれらの機能は、たとえば、制御装置１５０の記憶装置に記憶されたプログラムを、ＣＰＵによって実行することによって実現することができる。以下の説明では、作業車両１００の位置および姿勢を算出する機能Ｆ１００を、算出機能Ｆ１００と略称し、作業車両１００の状態を監視する機能Ｆ２００を、状態監視機能Ｆ２００と略称する場合がある。

図２に示す例において、制御装置１５０の算出機能Ｆ１００は、たとえば、アンテナ設置情報を記憶する機能Ｆ１０１と、位置および方向の情報を記憶する機能Ｆ１０２と、センサ情報を記憶する機能Ｆ１０３と、方向修正パラメータを記憶する機能Ｆ１０８とを有している。以下の説明では、制御装置１５０のこれらの機能を、それぞれ、記憶機能Ｆ１０１，Ｆ１０２，Ｆ１０３，Ｆ１０８と略称する場合がある。

また、図２に示す例において、制御装置１５０の算出機能Ｆ１００は、さらに、第１車両方向を算出する機能Ｆ１０４と、第２車両方向を算出する機能Ｆ１０５と、定常走行を検出する機能Ｆ１０６と、方向修正パラメータを算出する機能Ｆ１０７とを有している。以下の説明では、制御装置１５０のこれらの機能を、それぞれ、算出機能Ｆ１０４，Ｆ１０５、検出機能Ｆ１０６、算出機能Ｆ１０７と略称する場合がある。

また、図２に示す例において、制御装置１５０の算出機能Ｆ１００は、さらに、方向修正パラメータの使用可否を判定する機能Ｆ１０９と、作業車両１００の位置および姿勢を推定する機能Ｆ１１０とを有している。以下の説明では、制御装置１５０のこれらの機能を、それぞれ、判定機能Ｆ１０９、推定機能Ｆ１１０と略称する場合がある。

図３は、図１の作業車両１００の制御装置１５０による処理の一例を示すフロー図である。制御装置１５０は、たとえば、算出機能Ｆ１００によって、図３に示す処理を実行する。具体的には、制御装置１５０は、図３に示す処理を開始すると、たとえば、記憶機能Ｆ１０３によって、センサ情報を記憶する処理Ｐ１を実行する。この処理Ｐ１において、記憶機能Ｆ１０３は、制御装置１５０の入出力部を介してセンサ１３０から入力されたセンサ情報を、制御装置１５０を構成する記憶装置に記憶させる。

図４は、処理Ｐ１において記憶機能Ｆ１０３によって記憶されるセンサ情報の一例を示す表Ｔ１である。センサ情報は、たとえば、センサ１３０に含まれる速度センサ１３１、加速度センサ１３２、角速度センサ１３３、および昇降センサ１３４のそれぞれからの出力値である速度ｖ、加速度ａ、角速度ｗ、および昇降状態ｈを含む。また、センサ情報は、たとえば、各出力値が出力された時刻ｔ，ｔ−１，…，ｔ−ｍ，…を含む。制御装置１５０の記憶装置は、たとえば、現在時刻ｔのセンサ情報と、現在時刻ｔよりも前の時刻ｔ−１，…，ｔ−ｍ，…のセンサ情報とを含む、一定の期間のセンサ情報を記憶している。

制御装置１５０の記憶装置にセンサ情報が記憶される一定の期間は、少なくとも、第１アンテナ１２１の位置情報と、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向とが、受信機１２３から共に出力される周期以上の期間である。また、センサ情報に含まれる速度、加速度、および角速度は、速度センサ１３１、加速度センサ１３２、および角速度センサ１３３の出力値であってもよく、各センサの出力値を車両座標で三次元表記に変換した値であってもよい。

次に、制御装置１５０は、たとえば、第１アンテナ１２１の位置情報と、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向とが、共に受信機１２３から取得されているか否かを判定する処理Ｐ２を実行する。なお、第１アンテナ１２１の位置情報は、ＧＮＳＳの電波を受信した第１アンテナ１２１から受信機１２３へ信号が入力され、その結果、受信機１２３から出力される第１アンテナ１２１の位置情報である。また、基線方向は、たとえば、ＧＮＳＳの電波を受信した第１アンテナ１２１と第１アンテナ１２１から受信機１２３へ信号が入力され、その結果、受信機１２３から出力される第１アンテナ１２１と第２アンテナ１２２とを結ぶ直線の方向である。

制御装置１５０の記憶機能Ｆ１０２は、たとえば、受信機１２３から、それぞれ一定の周期で、第１アンテナ１２１の位置情報と、基線方向とを取得して、制御装置１５０の記憶装置に記憶させている。したがって、処理Ｐ２において、制御装置１５０は、たとえば、第１アンテナ１２１の位置情報と、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向とが、共に受信機１２３から取得されているか否かを判定する。

具体的には、たとえば、受信機１２３から、第１アンテナ１２１の位置情報が１［Ｈｚ］の周期で出力され、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向が１０［Ｈｚ］の周期で出力されているとする。この場合、制御装置１５０は、処理Ｐ２において、受信機１２３から基線方向のみが取得された時刻においては、第１アンテナ１２１の位置情報が取得されなかった（ＮＯ）と判定し、後述する処理Ｐ１３を実行する。一方、制御装置１５０は、処理Ｐ２において、第１アンテナ１２１の位置情報と基線方向とが共に取得された（ＹＥＳ）と判定すると、次の処理Ｐ３を実行する。

図５は、処理Ｐ３において制御装置１５０の記憶装置に記憶される第１アンテナ１２１の位置情報ｐと基線方向ｄの一例を示す表Ｔ２である。処理Ｐ３において、制御装置１５０は、たとえば、記憶機能Ｆ１０２により、受信機１２３から取得した第１アンテナ１２１の位置情報ｐと基線方向ｄとを、取得した時刻ｔ，ｔ−ｍ，…，ｔ−６ｍ，…とともに記憶装置に記憶させる。

図５は、第１アンテナ１２１の位置情報ｐと、基線方向ｄとが、周期ｍごとに共に出力される例を示している。第１アンテナ１２１の位置情報ｐは、たとえば、地図上の点を示す情報であり、２次元または３次元の位置情報である。また、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向ｄは、たとえば、２次元もしくは３次元のベクトル、または方位である。図３に示すように、処理Ｐ３の終了後、制御装置１５０は、次の処理Ｐ４を実行する。

図６は、図３の定常走行を検出する処理Ｐ４のフロー図である。なお、図６に示す各処理は、たとえば、制御装置１５０の検出機能Ｆ１０６によって実行することができる。制御装置１５０は、処理Ｐ４を開始すると、まず、平均速度を算出する処理Ｐ４０１を実行する。具体的には、この処理Ｐ４０１において、制御装置１５０は、たとえば、記憶装置を参照し、測位装置１２０から一周期前の第１アンテナ１２１の位置情報ｐが取得された時刻を参照する。さらに、制御装置１５０は、たとえば、一周期前の第１アンテナ１２１の位置情報ｐが取得された時刻から現在までの車両１１０の速度ｖを用いて、車両１１０の平均速度を算出する。

次に、制御装置１５０は、算出した車両１１０の平均速度が、所定の速度閾値Ｖｔｈよりも高いか否かを判定する処理Ｐ４０２を実行する。この処理Ｐ４０２において、制御装置１５０は、たとえば、算出した車両１１０の平均速度が所定の速度閾値Ｖｔｈ以下（ＮＯ）と判定すると、次の処理Ｐ４０８を実行する。

処理Ｐ４０８において、制御装置１５０は、記憶装置に記憶された定常走行フラグを０に設定し、図６に示す処理を終了する。この定常走行フラグが０の状態は、作業車両１００が定常走行をしていないことを示している。すなわち、定常走行は、車両１１０の速度の平均値が、所定の速度閾値Ｖｔｈよりも高いことを含む。これにより、車両１１０の停車中や速度閾値Ｖｔｈ以下の低速走行が定常走行から除外される。

一方、処理Ｐ４０２において、制御装置１５０は、たとえば、算出した車両１１０の平均速度が所定の速度閾値Ｖｔｈよりも高い（ＹＥＳ）と判定すると、次の処理Ｐ４０３を実行する。処理Ｐ４０３において、制御装置１５０は、たとえば、ベッセル１４０の高さを算出する。具体的には、制御装置１５０は、たとえば、記憶装置を参照し、現在時刻と、一周期前の測位装置１２０の出力が取得された時刻のベッセル１４０の昇降状態に基づいて、ベッセル１４０の高さを算出する。ここで、ベッセル１４０の高さは、たとえば、ベッセル本体１４１の前端部１４１ａの高さである。

たとえば、ベッセル１４０の昇降状態が、制御装置１５０の記憶装置にベッセル本体１４１の角度として記憶されているとする。この場合、制御装置１５０は、たとえば、ベッセル本体１４１の角度の正弦を用いて、ベッセル本体１４１の前端部１４１ａの高さを求めることができる。なお、ベッセル本体１４１の寸法データは、たとえば、あらかじめ制御装置１５０の記憶装置に記憶されている。処理Ｐ４０３の終了後、制御装置１５０は、次の処理Ｐ４０４を実行する。

処理Ｐ４０４において、制御装置１５０は、たとえば、処理Ｐ４０３で算出したベッセル１４０の高さが、所定の高さ閾値Ｈｔｈよりも低いか否かを判定する。より詳細には、制御装置１５０は、たとえば、現在時刻と、前回の測位装置１２０から位置情報ｐが取得された時刻とのそれぞれのベッセル１４０の高さが、高さ閾値Ｈｔｈよりも低いか否かを判定する。ここで、高さ閾値Ｈｔｈは、たとえば、第１アンテナ１２１もしくは第２アンテナ１２２の高さと等しい高さ、または、第１アンテナ１２１と第２アンテナ１２２のうち、より低い方の高さに設定することができる。

処理Ｐ４０４において、制御装置１５０が、たとえば、現在時刻と、前回の測位装置１２０から位置情報ｐが取得された時刻との両方または一方で、ベッセル１４０の高さが高さ閾値Ｈｔｈ以上である（ＮＯ）と判定したとする。この場合、制御装置１５０は、前述の定常走行フラグを０に設定する処理Ｐ４０８を実行し、図６に示す処理Ｐ４を終了する。

また、処理Ｐ４０４において、制御装置１５０が、たとえば、現在時刻と、前回の測位装置１２０から位置情報ｐが取得された時刻との両方または一方で、ベッセル１４０の高さが高さ閾値Ｈｔｈより低い（ＹＥＳ）と判定したとする。この場合、制御装置１５０は、たとえば、作業車両１００の走行状態を算出する処理Ｐ４０５を実行する。より具体的には、制御装置１５０は、処理Ｐ４０５において、作業車両１００の角速度、角加速度、および加速度のそれぞれの平均値と、車両１１０の方向の変化を算出する。

ここで、作業車両１００の角速度、角加速度、および加速度のそれぞれの平均値は、たとえば、前回の測位装置１２０から位置情報ｐが取得された時刻から現在時刻までの平均値である。作業車両１００の角加速度は、たとえば、ある時刻における作業車両１００の角速度と、その一周期前の時刻における作業車両１００の角速度との差分から算出することができる。車両１１０の方向、すなわち車両方向の変化は、たとえば、以下のように算出することができる。

図７は、第１アンテナ１２１と第１アンテナ１２１とを結ぶ基線方向ｄと、車両方向Ｄｖとの関係を示す模式図である。図７に示す例において、作業車両１００は、前回の測位装置１２０から位置情報ｐが取得された時刻から現在時刻までの間に、車両１１０が中心Ｃｔを中心とする円周に沿って角度θだけ旋回して移動している。この場合、車両方向Ｄｖは、車両１１０が旋回した角度θと等しい角度で変化する。同様に、基線方向ｄすなわち基線ベクトルも、車両１１０が旋回した角度θと等しい角度で変化する。

このように、車両方向Ｄｖが変化する角度θと、基線方向ｄが変化する角度θは等しい。そのため、現在時刻と、前回の測位装置１２０から位置情報ｐが取得された時刻との間の基線方向ｄの角度変化を算出することで、車両方向Ｄｖの角度変化を算出することができる。すなわち、制御装置１５０は、処理Ｐ４０５において、たとえば、基線方向ｄの角度変化を算出することで、車両方向Ｄｖの角度変化を算出する。以上のように、処理Ｐ４０５において、制御装置１５０は、たとえば作業車両１００の角速度、角加速度、および加速度のそれぞれの平均値と、車両方向Ｄｖの角度変化とを算出する。

次に、制御装置１５０は、たとえば、作業車両１００の走行状態が、定常走行であるか否かを判定する処理Ｐ４０６を実行する。ここで、定常走行とは、たとえば、作業車両１００が一定の運動状態で走行している状態である。より具体的には、作業車両１００の定常走行は、たとえば、中心および半径が同一の円周に沿って作業車両１００が走行する旋回と、作業車両１００が一直線に走行する直進とを含む。なお、定常走行は、旋回および直進に限定されず、他の走行状態を含んでもよい。

処理Ｐ４０６において、制御装置１５０は、たとえば、前の処理Ｐ４０５で算出した車両方向Ｄｖの変化と、車両１１０の角速度の時間分積分との差が閾値以下である場合に、作業車両１００が中心および半径が同一の円周に沿って走行する旋回中と判定する。この場合、作業車両１００の旋回が定常走行に含まれることから、制御装置１５０は、処理Ｐ４０６において、作業車両１００の走行状態が定常走行である（ＹＥＳ）と判定し、次の処理Ｐ４０７を実行する。

また、処理Ｐ４０６において、制御装置１５０は、たとえば、前の処理Ｐ４０５で算出した車両１１０の角加速度および加速度の平均値が閾値以下である場合に、作業車両１００が一直線に走行する直進中であると判定する。この場合、作業車両１００の直進が定常走行に含まれることから、制御装置１５０は、処理Ｐ４０６において、作業車両１００の走行状態が定常走行である（ＹＥＳ）と判定し、次の処理Ｐ４０７を実行する。

一方、前の処理Ｐ４０５で算出した車両方向Ｄｖの変化と、車両１１０の角速度の時間分積分との差が閾値より大であるか、または、車両１１０の角加速度および加速度の平均値が閾値より大であったとする。この場合、処理Ｐ４０６において、制御装置１５０は、作業車両１００の走行状態が定常走行ではない（ＮＯ）と判定し、前述の定常走行フラグを０に設定する処理Ｐ４０８を実行して、図６に示す処理Ｐ４を終了する。

処理Ｐ４０７において、制御装置１５０は、前述の処理Ｐ４０５で算出した車両１１０の角速度の平均値の絶対値｜αａｖｅ｜が、所定の角速度閾値αｔｈより小であるか否かを判定する。この処理Ｐ４０７において、制御装置１５０は、絶対値｜αａｖｅ｜が角速度閾値αｔｈより小である（ＹＥＳ）と判定すると、記憶装置に記憶された定常走行フラグを１に設定する処理Ｐ４０９を実行して、図６に示す処理Ｐ４を終了する。この定常走行フラグが１の状態は、作業車両１００の走行状態が直進であることを示している。

一方、処理Ｐ４０７において、制御装置１５０は、絶対値｜αａｖｅ｜が角速度閾値αｔｈ以上である（ＮＯ）と判定すると、記憶装置に記憶された定常走行フラグを２に設定する処理Ｐ４１０を実行して、図６に示す処理Ｐ４を終了する。この定常走行フラグが２の状態は、作業車両１００の走行状態が、中心および半径が同一の円周に沿って走行する旋回であることを示している。処理Ｐ４の終了後、制御装置１５０は、図３に示す処理Ｐ５を実行する。

処理Ｐ５において、制御装置１５０は、たとえば、検出機能Ｆ１０６により、作業車両１００の走行状態が、定常走行であるか否かを判定する。具体的には、制御装置１５０は、検出機能Ｆ１０６により、前の処理Ｐ４で設定されて記憶装置に記憶された定常走行フラグを参照する。参照した定常走行フラグが０である場合、作業車両１００が定常走行をしていないことを示しているので、制御装置１５０は、検出機能Ｆ１０６により、定常走行ではない（ＮＯ）と判定し、位置および姿勢を推定する処理Ｐ１３を実行する。

作業車両１００が特定の旋回や直進を含む定常走行をしていない場合、制御装置１５０は、処理Ｐ１３において、前回までの処理Ｐ６から処理Ｐ１２までを経て記憶装置に記憶された方向修正パラメータを用いて、作業車両１００の位置および姿勢を推定する。この作業車両１００の位置および姿勢を推定する処理Ｐ１３については、以下で処理Ｐ６から処理Ｐ１２までを説明した後に詳述する。

処理Ｐ５において、参照した定常走行フラグが１または２である場合、作業車両１００が定常走行をしていることを示しているので、制御装置１５０は、検出機能Ｆ１０６により、定常走行である（ＹＥＳ）と判定し、第１車両方向を算出する処理Ｐ６を実行する。ここで、第１車両方向とは、第１アンテナ１２１および第２アンテナ１２２の設置情報に基づいて算出される車両１１０の方向である。この設置情報は、たとえば、第１アンテナ１２１および第２アンテナ１２２が設置された位置の座標を含み、図２に示す制御装置１５０の記憶機能Ｆ１０１により、制御装置１５０の記憶装置に記憶されている。

図８は、第１車両方向を算出する処理Ｐ６のフロー図である。制御装置１５０は、たとえば、第１車両方向を算出する処理Ｐ６を開始すると、まず、算出機能Ｆ１０４により、方向修正パラメータを取得する処理Ｐ６０１を実行する。この処理Ｐ６０１において、制御装置１５０は、たとえば、記憶機能Ｆ１０１によって記憶装置に記憶された最新の方向修正パラメータを、算出機能Ｆ１０４によって取得する。

図９は、図８に示す処理Ｐ６０１において取得される方向修正パラメータを説明する模式図である。第１アンテナ１２１および第２アンテナ１２２は、車両１１０に設置されて位置が固定される。この車両１１０に対する第１アンテナ１２１の設置位置と第２アンテナ１２２の設置位置に基づいて、第１アンテナ１２１から第２アンテナ１２２へ向かうベクトルＡｖを求めることができる。また、車両１１０の前後方向に平行な車両１１０の中心線の方向を車両方向Ｄｖとする。

この場合、第１アンテナ１２１および第２アンテナ１２２の設置情報は、車両方向ＤｖとベクトルＡｖとのなす角であるアンテナ設置角δを含む。そして、方向修正パラメータは、何らかの原因によりアンテナ設置角δに変化が生じた場合に、その変化を補正するためのパラメータである。アンテナ設置角δおよび方向修正パラメータは、第１アンテナ１２１および第２アンテナ１２２の設置位置を車両１１０に固定された座標系の座標で表しても、たとえば座標変換を行うことで、同様に算出することが可能である。

方向修正パラメータは、後述する処理Ｐ８において、制御装置１５０の算出機能Ｆ１０７により算出されて、記憶機能Ｆ１０８により記憶装置に記憶される。なお、制御装置１５０の記憶装置には、たとえば、一定期間分の方向修正パラメータが、それぞれ、時刻とともに記憶されている。また、処理Ｐ８が未実施で方向修正パラメータが算出されていない場合には、制御装置１５０の記憶装置には、初期値としての０が記憶されている。方向修正パラメータが０の場合は、アンテナ設置角δの補正は行われない。

次に、制御装置１５０は、たとえば、算出機能Ｆ１０４により、第１車両方向を算出する処理Ｐ６０２を実行する。具体的には、たとえば、制御装置１５０は、算出機能Ｆ１０４により、図９に示すアンテナ設置角δから方向修正パラメータを差し引いた値と、図９に示すベクトルＡｖとを用いて、車両方向Ｄｖを算出し、図８に示す処理Ｐ６を終了する。この処理Ｐ６で算出される車両方向Ｄｖが、第１アンテナ１２１および第２アンテナ１２２の設置位置に由来する、第１車両方向である。次に、制御装置１５０は、たとえば算出機能Ｆ１０５により、図３に示すように、第２車両方向を算出する処理Ｐ７を実行する。

図１０は、第２車両方向を算出する処理Ｐ７のフロー図である。図１０に示す各処理は、たとえば、制御装置１５０の算出機能Ｆ１０５によって実行される。制御装置１５０は、処理Ｐ７を開始すると、まず、アンテナ位置の差分を算出する処理Ｐ７０１を実行する。処理Ｐ７０１において、制御装置１５０は、測位装置１２０の受信機１２３から制御装置１５０へ入力され、制御装置１５０の記憶機能Ｆ１０２によって記憶装置に記憶された第１アンテナ１２１の位置情報ｐを取得する。ここで、制御装置１５０は、現在の第１アンテナ１２１の位置情報ｐと、一周期前の第１アンテナ１２１の位置情報ｐとを取得する。

さらに、この処理Ｐ７０１において、制御装置１５０は、現在の第１アンテナ１２１の位置情報ｐと、一周期前の第１アンテナ１２１の位置情報ｐとの差分に基づいて、車両１１０の移動方向を算出する。より詳細には、制御装置１５０は、現在の第１アンテナ１２１の位置情報ｐと、一周期前の第１アンテナ１２１の位置情報ｐとの差分に基づいて、車両１１０の移動ベクトルを算出する。

次に、制御装置１５０は、定常走行フラグが２であるか否かを判定する処理Ｐ７０２を実行する。処理Ｐ７０２において、制御装置１５０が、定常走行フラグは２ではない（ＮＯ）、すなわち、定常走行フラグが１であると判定したとする。この場合、車両１１０の走行状態は一直線に走行する直進であり、車両方向Ｄｖと車両１１０の進行方向とが一致する。そのため、制御装置１５０は、前の処理Ｐ７０１で算出した車両１１０の移動ベクトルを第２車両方向として設定する処理Ｐ７０６を実行し、図１０に示す処理Ｐ７を終了する。

一方、処理Ｐ７０２において、制御装置１５０が、定常走行フラグは２である（ＹＥＳ）と判定したとする。この場合、車両１１０の走行状態は、中心および半径が同一の円に沿って走行する旋回である。そのため、制御装置１５０は、車両１１０の旋回の中心の位置を算出する処理Ｐ７０３を実行する。

図１１は、作業車両１００の定常走行に含まれる旋回の一例を示す模式図である。図１１では、中心Ｃｔを左回り（反時計回り）に旋回する車両１１０の現在時刻における位置と、一周期前の時刻における位置とを示している。制御装置１５０は、処理Ｐ７０３において、まず、一周期前の時刻の第１アンテナ１２１の位置情報ｐと、現在時刻の第１アンテナ１２１の位置情報ｐとに基づいて、一周期前の時刻における車両１１０の車両方向Ｄｖを算出する。

ここで、車両１１０は、中心Ｃｔおよび半径が同一の円に沿って旋回している。そのため、一周期前の時刻から現在時刻までの間の中心Ｃｔの周りの車両１１０の回転角度は、一周期前の時刻の車両方向Ｄｖと現在時刻の車両方向Ｄｖとの間の角度θに等しい。車両１１０の旋回の中心Ｃｔは、前述の処理Ｐ７０１で算出した移動ベクトルＭｖの二等分線上にある。また、移動ベクトルＭｖから旋回の中心Ｃｔまでの距離Ｄは、移動ベクトルＭｖの長さをＬとして、次の式（１）によって求めることができる。

Ｄ＝Ｌ／｛２×ｔａｎ（θ／２）｝ ・・・（１）

したがって、処理Ｐ７０３において、制御装置１５０は、たとえば、車両１１０の角速度の平均から移動ベクトルＭｖに対する法線を算出することで、旋回の中心Ｃｔを算出することができる。次に、制御装置１５０は、車両１１０の回転中心を算出する処理Ｐ７０４を実行する。

この処理Ｐ７０４において、制御装置１５０は、まず、第１アンテナ１２１の位置から車両１１０の回転中心へ向かうベクトルを算出する。そのために、制御装置１５０は、第１アンテナ１２１の設置位置から加速度センサ１３２の設置位置へ向かうベクトルと、加速度センサ１３２の設置位置から車両１１０の回転中心へ向かうベクトルとを算出する。

図１２は、車両１１０の回転中心Ｃｔｖを説明する模式図である。車両１１０の回転中心Ｃｔｖは、車両１１０を一つの剛体と考え、車両１１０の速度ベクトルＶｖの方向と車両方向Ｄｖとが等しくなる点である。なお、車両１１０の速度ベクトルＶｖの方向は、車両１１０の旋回の中心Ｃｔと車両１１０の回転中心Ｃｔｖとを結ぶ線分の法線方向である。加速度センサ１３２の設置位置から車両１１０の回転中心Ｃｔｖへ向かうベクトルは、次のように算出することができる。

前述の処理Ｐ４で算出された車両１１０の加速度の平均値の中から車両１１０の前後方向の成分であるａｘを抽出する。さらに、ヨー方向の角速度の平均値ｗｚと、車両方向Ｄｖからのずれｇを用いる。これにより、加速度センサ１３２の設置位置から車両１１０の回転中心Ｃｔｖへ向かうベクトルを（ａｘ／（ｗｚ×ｗｚ），ｇ）のように求めることができる。

以上により、第１アンテナ１２１から加速度センサ１３２へ向かうベクトルと、加速度センサ１３２から車両１１０の回転中心Ｃｔｖへ向かうベクトルとが求められる。さらに、これらのベクトルを加算することで、第１アンテナ１２１から車両１１０の回転中心Ｃｔｖへ向かうベクトルを求めることができる。

また、車両１１０が低速で走行している場合には、アッカーマンジオメトリに従って後輪軸中心Ｃｒａを車両１１０の回転中心に規定することもできる。この場合は、車両１１０の仕様に基づいて、第１アンテナ１２１から後輪軸中心Ｃｒａまでのベクトルを予め算出して、制御装置１５０の記憶装置に記憶させておくことができる。

さらに、処理Ｐ７０４において、制御装置１５０は、第１アンテナ１２１から車両１１０の回転中心Ｃｔｖへ向かうベクトルと、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向のベクトルとを用いて、車両１１０の回転中心Ｃｔｖを算出する。具体的には、記憶機能Ｆ１０２によって制御装置１５０の記憶装置に記憶された一周期前の第１アンテナ１２１の位置を中心として、第１アンテナ１２１から車体フレーム１１１の回転中心Ｃｔｖへ向かうベクトルを回転させる。

このときのベクトルの回転量は、たとえば、第１車両方向を算出することで決定される。ここで、第１車両方向は、たとえば、前述の処理Ｐ６０２と同様に、図９に示すアンテナ設置角δから方向修正パラメータを差し引いた値と、一周期前の第１アンテナ１２１から第２アンテナ１２２へ向かうベクトルＡｖとを用いて求めることができる。以上により、制御装置１５０は、処理Ｐ７０４において、車両１１０の回転中心Ｃｔｖを算出することができる。

次に、制御装置１５０は、第２車両方向を算出する処理Ｐ７０５を実行する。この処理Ｐ７０５において、制御装置１５０は、前述の処理Ｐ７０３で算出した車両１１０の旋回の中心Ｃｔと、前述の処理Ｐ７０４で算出した車両１１０の回転中心Ｃｔｖとに基づいて、第２車両方向を算出して、図１０に示す処理Ｐ７を終了する。

この処理Ｐ７で算出される第２車両方向は、測位装置１２０受信機１２３から出力された情報に由来する車両方向である。第２車両方向は、車両１１０の旋回の中心Ｃｔから車両１１０の回転中心Ｃｔｖへ向かうベクトルに対して右手系で求めた法線方向として、算出することができる。次に、制御装置１５０は、図３の方向修正パラメータを算出する処理Ｐ８を実行する。

図１３は、方向修正パラメータを算出する処理Ｐ８のフロー図である。図１４は、処理Ｐ８を説明する模式図である。制御装置１５０は、処理Ｐ８を開始すると、まず、修正角φを算出する処理Ｐ８０１を実行する。修正角φは、たとえば、第１車両方向Ｄｖ１と第２車両方向Ｄｖ２とのなす角である。ここで、第１車両方向Ｄｖ１は、第１アンテナ１２１と第２アンテナ１２２の設置位置に由来する車両方向であり、第２車両方向Ｄｖ２は、測位装置１２０の受信機１２３から出力された第１アンテナ１２１の位置情報ｐに由来する車両方向である。

すなわち、第１車両方向Ｄｖ１は、第１アンテナ１２１および第２アンテナ１２２の設置位置の誤差の影響を受けるのに対し、第２車両方向Ｄｖ２は、第１アンテナ１２１および第２アンテナ１２２の設置位置の誤差の影響を受けない。したがって、第２車両方向Ｄｖ２が実際の車両方向であると考え、制御装置１５０は、算出機能Ｆ１０７により、第１車両方向Ｄｖ１と第２車両方向Ｄｖ２との差分を算出し、第１車両方向Ｄｖ１を第２車両方向Ｄｖ２と等しくするための修正角φを算出する。

次に、制御装置１５０は、方向修正パラメータを更新する処理Ｐ８０２を実行する。この処理Ｐ８０２において、制御装置１５０は、たとえば、算出機能Ｆ１０７により、記憶装置に記憶された方向修正パラメータに、前の処理Ｐ８０１で算出された修正角φを加算することで、方向修正パラメータを更新する。次に、制御装置１５０は、方向修正パラメータを記憶する処理Ｐ８０３を実行する。この処理Ｐ８０３において、制御装置１５０は、たとえば、記憶機能Ｆ１０８により、前の処理Ｐ８０２で更新された方向修正パラメータを記憶装置に記憶させ、図１３に示す処理Ｐ８を終了する。

次に、制御装置１５０は、たとえば、判定機能Ｆ１０９により、図３に示すように、方向修正パラメータが収束したか否かを判定する処理Ｐ９を実行する。第２車両方向Ｄｖ２は、図１０に示す車両１１０の回転中心Ｃｔｖを算出する処理Ｐ７０４においても用いられている。そのため、第２車両方向Ｄｖ２にも、前回の方向修正パラメータでは除去しきれない誤差が生じている。この誤差を減少させるため、制御装置１５０は、処理Ｐ９において、前の処理Ｐ８で算出した最新の方向修正パラメータを用いて再度計算を行うことで、収束演算を実施する。

処理Ｐ９において、制御装置１５０は、たとえば、図１３に示す処理Ｐ８０１で算出された修正角φの絶対値が、あらかじめ設定した閾値よりも小さい場合に、方向修正パラメータが収束した（ＹＥＳ）と判定し、次の処理Ｐ１０を実行する。一方、処理Ｐ９において、制御装置１５０は、たとえば、修正角φの絶対値が、あらかじめ設定した閾値以上である場合に、方向修正パラメータが収束していない（ＮＯ）と判定し、処理Ｐ６から処理Ｐ９までを繰り返し実行する。

次に、制御装置１５０は、たとえば、判定機能Ｆ１０９により、方向修正パラメータの平均値を算出する処理Ｐ１０を実行する。方向修正パラメータは、サンプリング周期ごとに測位装置１２０の受信機１２３から制御装置１５０に入力される第１アンテナ１２１の位置情報ｐに基づいて算出される。そのため、第１アンテナ１２１が受信したＧＮＳＳの電波に基づいて受信機１２３が測位計算を行う際に発生する位置誤差によって、方向修正パラメータにも誤差が生じる。

この方向修正パラメータの誤差を除去するために、制御装置１５０は、処理Ｐ１０において、方向修正パラメータの逐次平均を実行する。方向修正パラメータの逐次平均においては、たとえば、積算回数の上限を設定したり、忘却係数を導入したりすることで、積算回数の過度の増加を抑制してもよい。これにより、方向修正パラメータの変化が反映されなくなるのを防止できる。

次に、制御装置１５０は、たとえば、判定機能Ｆ１０９により、方向修正パラメータの使用可否を判定する処理Ｐ１１を実行する。前述のように、各時刻において算出された方向修正パラメータは誤差を含むため、所定の期間に算出された方向修正パラメータの平均値を算出する。そのため、一定数以上の方向修正パラメータが算出されていることと、周期毎の方向修正パラメータの変動が過大にならないことが必要である。

したがって、処理Ｐ１１において、制御装置１５０は、方向修正パラメータの算出数が所定の閾値以上であり、かつ、方向修正パラメータの最新の平均と一周期前の平均との差が所定の閾値以下である場合に、方向修正パラメータを使用可（ＹＥＳ）と判定する。この場合、制御装置１５０は、方向修正パラメータを用いて車両方向を補正する処理Ｐ１２を実行し、補正した車両方向に基づいて、推定機能Ｆ１１０により、車両１１０の位置および姿勢を推定する処理Ｐ１３を実行する。

一方、処理Ｐ１１において、制御装置１５０は、方向修正パラメータを使用不可（ＮＯ）と判定すると、方向修正パラメータを使用せずに、推定機能Ｆ１１０により車両１１０の位置および姿勢を推定する処理Ｐ１３を実行する。最後に、制御装置１５０は、処理Ｐ１０で算出された方向修正パラメータの平均値と、処理Ｐ１３で推定された車両１１０の位置および姿勢を、状態監視機能Ｆ２００へＣＡＮを介して出力する処理Ｐ１４を実行する。以上により、制御装置１５０の算出機能Ｆ１００による図３に示す各処理が終了する。

次に、制御装置１５０の状態監視機能Ｆ２００は、第１アンテナ１２１および第２アンテナ１２２の設置情報と、方向修正パラメータとに基づいて、第１アンテナ１２１または第２アンテナ１２２の設置位置に誤差が生じていることを検出する。また、制御装置１５０の状態監視機能Ｆ２００は、たとえば、方向修正パラメータが所定の閾値を超えた場合に、第１アンテナ１２１および第２アンテナ１２２の設置位置の異常を判定する。

この場合、作業車両１００は、オペレータまたはユーザへ情報を通知するための情報通知装置を備えていてもよい。情報通知装置としては、たとえば、液晶表示装置、表示ランプ、スピーカ、ブザーなどを使用することができる。これにより、制御装置１５０の状態監視機能Ｆ２００は、方向修正パラメータが所定の閾値を超えた場合に、第１アンテナ１２１および第２アンテナ１２２の設置情報に対する誤差情報を情報通知装置へ出力して、作業車両１００のオペレータまたはユーザに異常を通知することができる。

以上のように、本実施形態の作業車両１００は、車両１１０と、その車両１１０に取り付けられて衛星測位システムの電波を受信する第１アンテナ１２１および第２アンテナ１２２とを備えている。また、作業車両１００は、衛星測位システムの電波に基づく第１アンテナ１２１の位置情報ｐ、および、第１アンテナ１２１と第２アンテナ１２２との間の基線方向ｄを出力する受信機１２３を備えている。また、作業車両１００は、車両１１０の速度、加速度、および角速度を計測するセンサ１３０と、車両１１０の位置および姿勢を推定する制御装置１５０と、を備えている。そして、制御装置１５０は、検出機能Ｆ１０６と、算出機能Ｆ１０４と、算出機能Ｆ１０５と、算出機能Ｆ１０７と、推定機能Ｆ１１０と、を有する。検出機能Ｆ１０６は、車両１１０の速度、加速度、および角速度に基づいて定常走行を検出する機能である。算出機能Ｆ１０４は、車両１１０に対する第１アンテナ１２１および第２アンテナ１２２の設置情報に基づいて第１車両方向Ｄｖ１を算出する機能である。算出機能Ｆ１０５は、定常走行が検出された場合に、第１アンテナ１２１の位置情報ｐの時間変化に基づいて第２車両方向Ｄｖ２を算出する機能である。算出機能Ｆ１０７は、第２車両方向Ｄｖ２に基づいて第１車両方向Ｄｖ１を修正するための方向修正パラメータを算出する機能である。推定機能Ｆ１１０は、方向修正パラメータと第１車両方向Ｄｖ１に基づいて車両１１０の位置および姿勢を推定する機能である。

このような構成により、本実施形態の作業車両１００によれば、従来の装置よりも柔軟に第１アンテナ１２１および第２アンテナ１２２の設置位置の誤差を検出することが可能になる。より詳細には、第１アンテナ１２１および第２アンテナ１２２は、衛星測位システムからの電波が車両１１０やベッセル１４０などによって遮蔽されないように、たとえば、車体フレーム１１１から上方に延びるポールの先端に取り付けられる。また、作業車両１００は、たとえば、ベッセル１４０に鉱石や土砂などを積載したり、ベッセル１４０から積載物を降ろしたり、鉱山や建設現場などの凹凸の多い地面の上を走行したりする。そのため、第１アンテナ１２１および第２アンテナ１２２には、振動や衝撃が作用することがある。すると、時間の経過にともなって、第１アンテナ１２１および第２アンテナ１２２のあらかじめ設定された設置位置と、実際の設置位置との間に誤差が生じることがある。このような場合、あらかじめ設定された設置位置に基づいて算出される第１車両方向Ｄｖ１と、実際の車両方向との間に誤差が生じることがある。

そこで、本実施形態の作業車両１００は、前述のように、制御装置１５０の検出機能Ｆ１０６によって定常走行が検出された場合に、第１アンテナ１２１が受信した衛星測位システムの電波に基づく第１アンテナ１２１の位置情報ｐの時間変化を用いて第２車両方向Ｄｖ２を算出する。そして、第２車両方向Ｄｖ２に基づいて第１車両方向Ｄｖ１を修正するための方向修正パラメータを算出し、その方向修正パラメータと第１車両方向Ｄｖ１に基づいて車両１１０の位置および姿勢を推定する。すなわち、本実施形態の作業車両１００は、従来の装置のような極めて限定された走行状態だけではなく、より多様な走行状態を含む定常走行を行うことで、第１車両方向Ｄｖ１と実際の車両方向Ｄｖとの間の誤差をより柔軟に補正することができる。また、第１アンテナ１２１および第２アンテナ１２２の設置位置に生じた誤差を検出するための作業が不要となる。

また、本実施形態の作業車両１００において、制御装置１５０は、方向修正パラメータの使用可否を判定する判定機能Ｆ１０９をさらに有している。そして、制御装置１５０の推定機能Ｆ１１０は、方向修正パラメータが使用可能である場合に、方向修正パラメータと第１車両方向Ｄｖ１に基づいて、車両１１０の位置および姿勢を推定する。この構成により、たとえば、大きな誤差を含む方向修正パラメータに基づいて第１車両方向Ｄｖ１が補正されることが防止され、第１車両方向Ｄｖ１をより正確に補正することが可能になる。

また、本実施形態の作業車両１００において、定常走行は、中心および半径が同一の円周に沿って走行する旋回と、一直線に走行する直進とを含む。この構成により、従来の装置のような作業車両１００が略水平面を前後方向に進行する極めて限定された走行状態だけではなく、旋回と直進を含む定常走行を行うことで、第１車両方向Ｄｖ１と実際の車両方向との間の誤差をより柔軟に補正することができる。

また、本実施形態の作業車両１００において、定常走行は、車両１１０の速度の平均値が所定の速度閾値Ｖｔｈよりも高いことを含む。この構成により、車両１１０が停止しているような場合や、車両１１０が速度閾値Ｖｔｈ以下の速度で徐行しているような場合を、定常走行から除外して、車両１１０の位置および姿勢をより正確に推定することが可能になる。

また、本実施形態の作業車両１００は、車両１１０に取り付けられたベッセル１４０と、ベッセル１４０を昇降させる昇降機構である昇降シリンダ１４２と、ベッセル１４０の高さを検知する昇降センサ１３４と、を備えている。そして、本実施形態の作業車両１００において、定常走行は、ベッセル１４０の高さが、所定の高さ閾値Ｈｔｈよりも低いことを含む。

この構成により、本実施形態の作業車両１００は、たとえばベッセル本体１４１の前端部１４１ａが、第１アンテナ１２１および第２アンテナ１２２の高さに基づく高さ閾値Ｈｔｈ以上である場合に、第２車両方向Ｄｖ２が算出されない。そのため、衛星測位システムから第１アンテナ１２１または第２アンテナ１２２への電波がベッセル１４０によって遮られた状態で第２車両方向Ｄｖ２を算出することが防止され、第２車両方向Ｄｖ２の誤差が減少する。したがって、本実施形態の作業車両１００によれば、車両１１０の位置および姿勢をより正確に推定することが可能になる。

また、本実施形態の作業車両１００は、オペレータまたはユーザへ情報を通知するための情報通知装置を備えることができる。この場合、制御装置１５０は、方向修正パラメータが所定の閾値を超えた場合に、第１アンテナ１２１および第２アンテナ１２２の設置情報に対する誤差情報を、情報通知装置へ出力することができる。この構成により、本実施形態の作業車両１００は、オペレータまたはユーザへ第１アンテナ１２１または第２アンテナ１２２の設置位置の誤差が生じていることを通知できる。

以上説明したように、本実施形態によれば、従来の装置よりも柔軟に第１アンテナ１２１または第２アンテナ１２２の設置位置の誤差を検出することが可能な作業車両１００を提供することができる。

［実施形態２］
以下、実施形態１で用いた一部の図を援用し、さらに図１５から図１７を参照して、本開示に係る作業車両の実施形態２を説明する。図１５は、本開示に係る作業車両の実施形態２を示す制御装置１５０の機能ブロック図である。本実施形態の作業車両１００は、制御装置１５０が備える機能が、前述の実施形態１の作業車両１００と異なっている。本実施形態の作業車両１００のその他の点は、前述の実施形態１の作業車両１００と同様であるため、同様の部分には同一の符号を付して説明を省略する。

本実施形態の作業車両１００において、制御装置１５０は、前述の実施形態１の算出機能Ｆ１００と同様の位置および姿勢の算出機能Ｆ１００’を有している。また、本実施形態の作業車両１００において、制御装置１５０は、状態監視機能Ｆ２００に代えて、走行制御機能Ｆ３００を有している。本実施形態の制御装置１５０において、算出機能Ｆ１００’は、推定機能Ｆ１１０による車両１１０の位置および姿勢の推定結果と、検出機能Ｆ１０６による定常走行の検知結果を、走行制御機能Ｆ３００へ出力する。

走行制御機能Ｆ３００は、算出機能Ｆ１００’から入力された推定結果と検知結果に基づいて、車両１１０の走行制御を実行する。より具体的には、走行制御機能Ｆ３００は、車両１１０の各種のアクチュエータを制御して、車両１１０のアクセルペダル、ブレーキペダル、変速機、ステアリングホイールなどを自動的に操作して、車両１１０を走行させる。また、本実施形態の作業車両１００において、制御装置１５０は、たとえば、検出機能Ｆ１０６により、図３および図４に示す定常走行を検出する処理Ｐ４に代えて、図１６に示す定常走行を検出する処理Ｐ４’を実行する。

図１６は、本実施形態の制御装置１５０の検出機能Ｆ１０６による処理Ｐ４’の一例を示すフロー図である。図１６に示す処理Ｐ４’において、図６に示す実施形態１の処理Ｐ４と同様の処理には、同一の符号を付して説明を省略する。

図１６に示す処理Ｐ４’の処理Ｐ４０２において、制御装置１５０は、車両１１０の平均速度が所定の速度閾値Ｖｔｈ以下（ＮＯ）と判定すると、たとえば検出機能Ｆ１０６により、記憶装置に記憶された速度不足フラグを１に設定する処理Ｐ４１１を実行する。この速度不足フラグが１に設定された状態は、車両１１０の速度が不足しているために方向修正パラメータの算出ができない状態であることを示している。処理Ｐ４１１の終了後、制御装置１５０は、前述の実施形態１と同様に、定常走行フラグを０に設定する処理Ｐ４０８を実行する。

また、図１６に示す処理Ｐ４’の処理Ｐ４０６において、制御装置１５０は、車両１１０の走行状態が定常走行ではない（ＮＯ）と判定すると、たとえば検出機能Ｆ１０６により、記憶装置に記憶された非定常フラグを１に設定する処理Ｐ４１２を実行する。この非定常フラグが１に設定された状態は、車両１１０の走行状態が定常走行ではなく、方向修正パラメータの算出ができない状態であることを示している。処理Ｐ４１２の終了後、制御装置１５０は、前述の実施形態１と同様に、定常走行フラグを０に設定する処理Ｐ４０８を実行する。

なお、速度不足フラグおよび非定常フラグの初期値は０であり、制御装置１５０のサンプリング周期毎の起動と同時に初期化される。速度不足フラグまたは非定常フラグが１に設定されると、方向修正パラメータは算出されず、作業車両１００の姿勢の推定誤差が大きくなる。そこで、本実施形態の制御装置１５０は、走行制御機能Ｆ３００により、車両１１０の走行状態が定常走行となるように、車両１１０の走行制御を実行する。

図１７は、本実施形態の作業車両１００における制御装置１５０の走行制御機能Ｆ３００による処理Ｐ１５の一例を示すフロー図である。制御装置１５０は、図１７に示す処理Ｐ１５を開始すると、まず、記憶装置に記憶された速度不足フラグおよび非定常フラグを取得する処理Ｐ１５０１を実行する。

次に、制御装置１５０は、速度不足フラグが１であるか否かを判定する処理Ｐ１５０２を実行する。この処理Ｐ１５０２において、制御装置１５０は、速度不足フラグが１である（ＹＥＳ）と判定すると、車両１１０の速度が不足して方向修正パラメータが算出できない状態であることが判断できる。この場合、制御装置１５０は、車両１１０を速度閾値Ｖｔｈを超える所定の速度まで加速させる走行制御のパラメータを設定する処理Ｐ１５０３を実行する。この処理Ｐ１５０３の終了後、制御装置１５０は、後述する割り込みの有無を判定する処理Ｐ１５０９を実行する。

一方、処理Ｐ１５０２において、制御装置１５０は、速度不足フラグが１ではない（ＮＯ）と判定すると、非定常フラグが１であるか否かを判定する処理Ｐ１５０４を実行する。この処理Ｐ１５０４において、制御装置１５０は、非定常フラグが１ではない（ＮＯ）と判定すると、車両１１０の走行状態は方向修正パラメータを算出可能な定常走行であると判断できることから、後述する割り込みの有無を判定する処理Ｐ１５０９を実行する。

一方、処理Ｐ１５０４において、制御装置１５０は、非定常フラグが１である（ＹＥＳ）と判定すると、車両１１０の速度が所定の速度閾値Ｖｔｈ’よりも高いか否かを判定する処理Ｐ１５０５を実行する。この処理Ｐ１５０５において、制御装置１５０は、車両１１０の速度が速度閾値Ｖｔｈ’よりも高い（ＹＥＳ）と判定すると、車両１１０の速度が定常走行の速度範囲に入るように、車両１１０を減速させる走行制御のパラメータを設定する処理Ｐ１５０６を実行する。この処理Ｐ１５０６の終了後、制御装置１５０は、後述する割り込みの有無を判定する処理Ｐ１５０９を実行する。

一方、処理Ｐ１５０５において、制御装置１５０は、車両１１０の速度が所定の速度閾値Ｖｔｈ’以下である（ＮＯ）と判定すると、車両１１０の速度に調整の余裕がないと判断できる。この場合、制御装置１５０は、車両１１０の角速度の絶対値｜α｜が所定の角速度閾値αｔｈよりも低いか否かを判定する処理Ｐ１５０７を実行する。この処理Ｐ１５０７において、制御装置１５０は、車両１１０の角速度の絶対値｜α｜が所定の角速度閾値αｔｈよりも低い（ＹＥＳ）と判定すると、車両１１０が直進していると判断できる。この場合、制御装置１５０は、ステアリングホイールの操舵角を維持する走行制御のパラメータを設定する処理Ｐ１５０８を実行する。この処理Ｐ１５０８の終了後、制御装置１５０は、後述する割り込みの有無を判定する処理Ｐ１５０９を実行する。

一方、処理Ｐ１５０７において、制御装置１５０は、車両１１０の角速度の絶対値｜α｜が所定の角速度閾値αｔｈ以上である（ＮＯ）と判定すると、割り込みの有無を判定する処理Ｐ１５０９を実行する。この処理Ｐ１５０９において、制御装置１５０は、他の制御装置から制御を停止させる割り込み信号が入力されたか否かと、車両１１０のオペレータによる車両１１０の操作の割り込みがあるか否かを判定する。

この処理Ｐ１５０９において、制御装置１５０は、割り込みがない（ＮＯ）と判定すると、前述の処理Ｐ１５０３、Ｐ１５０６、またはＰ１５０８において設定されたパラメータに基づいて車両１１０の走行制御を行う処理Ｐ１５１０を実行し、図１７に示す処理Ｐ１５を終了する。一方、処理Ｐ１５０９において、制御装置１５０は、割り込みがある（ＹＥＳ）と判定すると、前述の処理Ｐ１５０３、Ｐ１５０６、またはＰ１５０８において設定されたパラメータに基づく車両１１０の走行制御を中止する処理Ｐ１５１１を実行し、図１７に示す処理Ｐ１５を終了する。

以上のように、本実施形態の作業車両１００において、制御装置１５０は、車両１１０の走行を制御する走行制御機能Ｆ３００を有している。この走行制御機能Ｆ３００は、検出機能Ｆ１０６によって車両１１０の定常走行が検出されなかった場合に、定常走行の条件を満たすように車両１１０を制御する。このような構成により、本実施形態の作業車両１００によれば、実施形態１の作業車両１００と同様の効果を奏することができるだけでなく、制御装置１５０によって車両１１０の走行状態を定常走行に制御して、より確実に方向修正パラメータを算出することが可能になる。

以上、図面を用いて本開示に係る作業車両の実施形態を詳述してきたが、具体的な構成はこの実施形態に限定されるものではなく、本開示の要旨を逸脱しない範囲における設計変更等があっても、それらは本開示に含まれるものである。

１００ 作業車両
１１０ 車両
１２１ 第１アンテナ
１２２ 第２アンテナ
１２３ 受信機
１３０ センサ
１３４ 昇降センサ
１４０ ベッセル
１４２ 昇降シリンダ（昇降機構）
１５０ 制御装置
Ｃｔ 中心
ｄ 基線方向
Ｄｖ１ 第１車両方向
Ｄｖ２ 第２車両方向
Ｆ１０４ 第１車両方向を算出する機能
Ｆ１０５ 第２車両方向を算出する機能
Ｆ１０６ 検出機能
Ｆ１０７ 方向修正パラメータを算出する機能
Ｆ１０９ 方向修正パラメータの使用可否を判定する機能
Ｆ１１０ 推定機能
Ｆ３００ 走行制御機能
Ｈｔｈ 高さ閾値
ｐ 位置情報

本開示の一態様は、車両と、前記車両に取り付けられて衛星測位システムの電波を受信する第１アンテナおよび第２アンテナと、前記電波に基づく前記第１アンテナの位置情報および前記第１アンテナと前記第２アンテナとを結ぶ基線方向を出力する受信機と、前記車両の速度、加速度、および角速度を計測するセンサと、前記車両の位置および姿勢を推定する制御装置と、を備えた作業車両であって、前記制御装置は、前記速度、前記加速度、および前記角速度に基づいて、中心および半径が同一の円周に沿って走行する旋回走行と、一直線に走行する直進走行とを含み、前記速度の平均値が所定の速度閾値よりも高い定常走行を検出する検出機能と、前記車両に対する前記第１アンテナおよび前記第２アンテナの設置情報に基づいて第１車両方向を算出する機能と、前記定常走行として前記直進走行が検出された場合に、現在の前記第１アンテナの前記位置情報と一周期前の前記第１アンテナの前記位置情報との差分に基づいて算出される移動ベクトルを第２車両方向として設定し、前記定常走行として前記旋回走行が検出された場合に、前記車両の旋回中心と回転中心とに基づいて第２車両方向を算出する機能と、前記第２車両方向に基づいて前記第１車両方向を修正するための方向修正パラメータを算出する機能と、前記方向修正パラメータと前記第１車両方向に基づいて前記車両の前記位置および前記姿勢を推定する推定機能と、を有する作業車両である。

図７は、第１アンテナ１２１と第２アンテナ１２２とを結ぶ基線方向ｄと、車両方向Ｄｖとの関係を示す模式図である。図７に示す例において、作業車両１００は、前回の測位装置１２０から位置情報ｐが取得された時刻から現在時刻までの間に、車両１１０が中心Ｃｔを中心とする円周に沿って角度θだけ旋回して移動している。この場合、車両方向Ｄｖは、車両１１０が旋回した角度θと等しい角度で変化する。同様に、基線方向ｄすなわち基線ベクトルも、車両１１０が旋回した角度θと等しい角度で変化する。

処理Ｐ４０６において、制御装置１５０は、たとえば、前の処理Ｐ４０５で算出した車両方向Ｄｖの変化と、車両１１０の角速度の時間積分との差が閾値以下である場合に、作業車両１００が中心および半径が同一の円周に沿って走行する旋回中と判定する。この場合、作業車両１００の旋回が定常走行に含まれることから、制御装置１５０は、処理Ｐ４０６において、作業車両１００の走行状態が定常走行である（ＹＥＳ）と判定し、次の処理Ｐ４０７を実行する。

一方、前の処理Ｐ４０５で算出した車両方向Ｄｖの変化と、車両１１０の角速度の時間積分との差が閾値より大であるか、または、車両１１０の角加速度および加速度の平均値が閾値より大であったとする。この場合、処理Ｐ４０６において、制御装置１５０は、作業車両１００の走行状態が定常走行ではない（ＮＯ）と判定し、前述の定常走行フラグを０に設定する処理Ｐ４０８を実行して、図６に示す処理Ｐ４を終了する。

Claims (7)

1. 車両と、前記車両に取り付けられて衛星測位システムの電波を受信する第１アンテナおよび第２アンテナと、前記電波に基づく前記第１アンテナの位置情報および前記第１アンテナと前記第２アンテナとの間の基線方向を出力する受信機と、前記車両の速度、加速度、および角速度を計測するセンサと、前記車両の位置および姿勢を推定する制御装置と、を備えた作業車両であって、
   前記制御装置は、
   前記速度、前記加速度、および前記角速度に基づいて定常走行を検出する検出機能と、
   前記車両に対する前記第１アンテナおよび前記第２アンテナの設置情報に基づいて第１車両方向を算出する機能と、
   前記定常走行が検出された場合に、前記第１アンテナの前記位置情報の時間変化に基づいて第２車両方向を算出する機能と、
   前記第２車両方向に基づいて前記第１車両方向を修正するための方向修正パラメータを算出する機能と、
   前記方向修正パラメータと前記第１車両方向に基づいて前記車両の前記位置および前記姿勢を推定する推定機能と、を有する作業車両。
2. 前記制御装置は、前記方向修正パラメータの使用可否を判定する機能をさらに有し、
   前記推定機能は、前記方向修正パラメータが使用可能である場合に、前記方向修正パラメータと前記第１車両方向に基づいて前記車両の前記位置および前記姿勢を推定する、請求項１に記載の作業車両。
3. 前記定常走行は、中心および半径が同一の円周に沿って走行する旋回と、一直線に走行する直進とを含む、請求項１に記載の作業車両。
4. 前記定常走行は、前記速度の平均値が所定の速度閾値よりも高いことを含む、請求項１に記載の作業車両。
5. 前記車両に取り付けられたベッセルと、前記ベッセルを昇降させる昇降機構と、前記ベッセルの高さを検知する昇降センサと、を備え、
   前記定常走行は、前記ベッセルの高さが、所定の高さ閾値よりも低いことを含む、請求項１に記載の作業車両。
6. オペレータへ情報を通知するための情報通知装置を備え、
   前記制御装置は、前記方向修正パラメータが所定の閾値を超えた場合に、前記第１アンテナおよび前記第２アンテナの前記設置情報に対する誤差情報を、前記情報通知装置へ出力する、請求項１に記載の作業車両。
7. 前記制御装置は、前記車両の走行を制御する走行制御機能を有し、
   前記走行制御機能は、前記検出機能によって前記定常走行が検出されなかった場合に、前記定常走行の条件を満たすように前記車両を制御する、請求項１に記載の作業車両。

Images