JP2007064853A - 複合測位による移動体の測位制御装置、システムおよびプログラム - Google Patents

Abstract

【課題】衛星からのデータ受信が遮断される場所においても高い精度で連続的な測位を可能とし、その測位結果に基づき移動体のリアルタイム制御を実現する。
【解決手段】衛星航法と慣性航法による複合測位を用いて移動体（建機１０）の位置測位を行う測位制御装置１００を、複合測位であらかじめ定義された精度が得られるか否かを判定し、必要な精度が得られないと判定された場合、既知点に設置されるトータルステーション３０や移動体に設置される車速センサ等、支援センサから出力される測位結果を受信し、当該測位結果を用いて慣性航法による測位誤差を補正する構成とした。
【選択図】 図２

Description

本発明は、衛星からの電波を用いて（リアルタイムに）移動体の測位を行う高精度衛星測位と、慣性センサを用いて前記移動体の変位量を計測して測位を行う慣性航法とを組み合わせ、移動体の位置座標を連続的に算出する、複合測位による移動体の測位制御装置およびシステムならびにプログラムに関する。

車両を含む移動体の測位の分野において、ＲＴＫ−ＧＰＳ（Real Time Kinematics-Global Positioning System）方式を採用した場合、その性能は、ｃｍオーダの精度でリアルタイムに計測できることが実証されている。
しかしながら、ここで得られる測位結果は、秒間隔（例えば１秒間隔）で得られる離散値であるばかりか、例えば、土木工事に用いられる建設機械（以下、建機という）等の移動体に利用した場合、建機のアームや深い木々等、遮蔽物の存在によりＧＰＳ衛星からの電波の受信が中断されると途切れてしまう情報である。このため、ＩＮＳ（Inertial Navigation System）による慣性航法を用いて移動体の変位量を計測し、前記したＲＴＫ−ＧＰＳによる高精度衛星航法（高精度測位衛星測位）とを組み合わせて位置を連続的に算出する複合測位が提案されている（例えば、特許文献１参照）。
特開２００２−４２６１号公報（段落００１１、図１）

しかしながら前記した複合測位によれば、測位に必要なＧＰＳ衛星からの電波が遮断される場所では慣性センサのみで測位することになり、この慣性センサを用いた慣性航法は、測位誤差が時間経過とともに大きくなることから、測位結果を移動体の動作制御にリアルタイムに反映させるリアルタイム性が要求される建機の制御には不向きである。
また、特許文献１に開示された技術によれば、３軸ジャイロシステムといった高価な慣性センサを要し、廉価構成が要求される建機への適用は困難である。

本発明は前記した事情に基づいてなされたものであり、ＧＰＳ衛星からの電波が遮断される場所においても高い精度で連続的な測位を可能とし、例えばリアルタイム性、および廉価構成が要求される建機の制御にも適用可能な、複合測位による移動体の測位制御装置、システムおよびプログラムを提供することを目的とする。

前記した課題を解決するために本発明は、衛星航法と慣性航法による複合測位を用いて移動体の測位を行う測位制御装置を、前記複合測位であらかじめ定義された精度が得られるか否かを判定し、必要な精度が得られないと判定された場合、既知点に設置されるトータルステーション、あるいは移動体に設置される車速センサ等の支援センサから出力される測位結果を受信し、当該測位結果を用いて慣性航法による測位誤差を補正する構成としたものである。

本発明によれば、測位制御装置が、支援センサから出力される測位結果を受信し、当該測位結果を用いて慣性航法による測位誤差を補正することで、衛星からの電波が遮断される場所においても高い精度で連続的な測位を可能とし、このことにより、例えばリアルタイム性、および廉価構成が要求される建機の制御に適用することができる。

図１は、本発明実施形態に係る移動体の測位制御システムの作業環境をイメージした図である。なお、本実施形態では、特に断らない限り、位置情報は、「緯度・経度・高さ」からなり、航法情報は、「位置、姿勢、速度」からなる。
図１では、山岳地帯で周辺に深い木々が生い茂っており、ＧＰＳ衛星からの電波を受信して測位するには比較的電波事情の悪い土木工事現場を想定している。図１において、符号１０は移動体としての建機であり、ここでは、無人運転が可能なブルドーザが例示されている。建機１０には、上空約２万Ｋｍの６つの円軌道に４個ずつ配置されたＧＰＳ衛星２０からの電波を受信して、緯度、経度、高さ等の位置情報を割り出すことができるＧＰＳ受信機の他に、ジャイロや加速度計等の慣性センサ２が設置されている。このため、建機１０は、ＧＰＳ（ＲＴＫ−ＧＰＳ）による高精度衛星測位と慣性センサによる慣性航法の複合測位によって位置、姿勢、速度に関する航法情報の決定を行い、ここで得られる情報を用いて自身の運転、あるいは建機アームやブレードを、あらかじめ登録済みの作業計画に従い、無人運転あるいは自動制御することができる。ちなみに、ＲＴＫ−ＧＰＳにより得られる測位結果は、本実施形態では１秒間隔で得られる離散的なものであり、高精度測位が行われない１秒の間のいわば空白時間は、慣性センサ２により補間される。

但し、土木工事現場が、前記したように山岳地帯で周辺に深い木々が生い茂った比較的電波事情が悪い地点にあることから、ＧＰＳ衛星２０からの電波が受信できなくなった場合に慣性センサ２による測位のみとなり、このため、高い精度での連続した測位が不可能になる。従って、本発明実施形態によれば、時間経過と共に誤差が大きくなる慣性センサ２による測位を補正する手段として、ＲＴＫ−ＧＰＳでの測位が困難になる場所に支援センサとして、例えば、ＧＰＳ一体型自動追尾トータルステーション３０が設置される。このＧＰＳ一体型自動追尾トータルステーション３０が設置された場所が既知点になる。なお、既知点とは、三角点や水準点等の公共基準点をいう。

ＧＰＳ一体型自動追尾トータルステーション３０（以下「トータルステーション３０」という）により、基準となる観測点（既知点）と、求点となる観測点（建機１０の現在位置）で同時に４つ以上のＧＰＳ衛星２０からの電波を受信し、これにより既知点で取得した信号（観測データ）を無線で建機１０に転送することにより、建機１０が自身の観測データとトータルステーションから取得した既知点での観測データを用いてリアルタイムに基線解析を行って建機１０の現在位置を決定することができる（国土地理院技術資料「ＲＴＫ−ＧＰＳを利用する公共測量作業マニュアル」参照）。
つまり、ＲＴＫ−ＧＰＳにおける固定局がトータルステーション３０（図２の測位装置２００）に該当し、ＲＴＫ−ＧＰＳにおける移動局が建機１０（図２の測位制御装置１００）に該当する。また、トータルステーション３０は、電波事情が悪く、前記したＲＴＫ−ＧＰＳによる移動局（建機１０）・固定局（既知点）での４つ以上のＧＰＳ衛星２０からの電波の同時受信が困難な場合、トータルステーション３０が本来持つ光学測量を実行して建機１０の現在位置を求め、慣性センサ２のみで測位した場合の測位誤差を補正するためにも用いられる。詳細は後記する。

なお、ここでは、既知点にトータルステーション３０を設置することで、ＲＴＫ−ＧＰＳによる測位が行えない場合でも、連続的に高精度測位を行うこととしたがトータルステーション３０を写真測量機で代替してもよい（ＧＰＳと接続された写真測量機）。

図２は、本発明実施形態に係る移動体の測位制御システムの内部構成を示すブロック図であり、ここでは、測位に関係するブロックのみ抽出して示してある。
本発明実施形態に係る移動体の測位制御システムは、建機１０内部に実装される測位制御装置１００（移動局）と、既知点に設置される測位装置２００（固定局）で構成される。

建機１０の内部に実装される測位制御装置１００は、ＲＴＫ−ＧＰＳ受信機１と、慣性センサ２と、ＰＣ（Personal Computer）３と、無線受信機４とで構成される。また、測位装置２００は、ＲＴＫ−ＧＰＳ受信機１１と、支援センサとしてのトータルステーション３０と、無線送信機１４で構成される。

測位制御装置１００内のＲＴＫ−ＧＰＳ受信機１は、ＧＰＳ衛星２０から受信した電波を測位のための観測データとしてＰＣ３に供給する。また、慣性センサ２は、建機１０に設置されたジャイロや加速度計等であり、ここで計測された信号は建機１０の位置、姿勢、速度等の航法情報演算のためにＰＣ３に供給される。

ＰＣ３には、他に、無線受信機４を介し、既知点に設置された測位装置２００から既知点でのＧＰＳ衛星２０からの電波に基づく観測データとトータルステーション３０の測位機能による建機１０の位置情報が適宜供給されている。すなわち、通常、ＲＴＫ−ＧＰＳによる高精度測位が可能なとき、トータルステーション３０は、ＲＴＫ−ＧＰＳ受信機１１で受信される既知点でのＧＰＳ衛星２０からの電波に基づく観測データを、無線送信機１４を介して建機１０へ送信する。一方、ＧＰＳ衛星２０からの電波事情の悪い異常時（ＲＴＫ−ＧＰＳによる高精度測位が不可能なとき）、トータルステーション３０は、建機１０に設置されるターゲット（プリズム反射鏡）を自動追尾して前記ターゲットとの間の距離および角度を測量し、当該建機１０の位置情報を、無線送信機１４を介して建機１０に送信する。

ＰＣ３は、更に、慣性センサ２から受信した信号に基づき建機１０が移動中の加速度を測定し、積分計算によって移動距離を算出して現在位置を求める慣性航法と、前記したＲＴＫ−ＧＰＳによる衛星航法との組合せによる複合測位により、建機１０の位置、姿勢、速度から成る航法情報を生成する。
また、ＰＣ３は、ＲＴＫ−ＧＰＳによる高精度測位が不可能なときは、慣性センサ２からのみ受信した信号を演算することにより得られる建機１０の航法情報を、既知点に設置されたトータルステーション３０から送信される建機１０の位置情報（緯度・経度・高さ）により補正し、建機１０の位置、姿勢（、速度）から成る航法情報を生成する。
そして、前記により生成される航法情報（位置、姿勢、速度）とあらかじめ内蔵の記憶装置（図示せず）に登録済みの作業計画情報とを照らし合わせて建機１０を自動運転、もしくは建機１０のアームやブレード等の制御対象部品を自動制御する。

このため、ＰＣ３は、複合測位であらかじめ定義された精度が得られるか否かを判定し、必要な精度が得られないと判定された場合、既知点に設置されるトータルステーション３０から出力される測位結果を受信し、当該測位結果を用いて慣性航法による測位誤差を補正する高精度測位部３１と、複合測位により取得される航法情報のうちの、位置、姿勢、速度情報の少なくとも１つから、建機１０の運転、あるいは建機１０のアームやブレード等の制御対象をリアルタイムに動作させるために、あらかじめ登録された作業計画に従う制御量を演算し、当該計算された制御量に基づき建機１０の運転、あるいは建機１０のアームやブレード等の制御対象の動作を制御する建機制御部３２で構成される。

なお、測位制御装置１００と測位装置２００とで、４つ以上のＧＰＳ衛星から同時に電波を受信できないときは、擬似衛星を用いて数あわせをしてもよい。ここで、擬似衛星（pseudo-lite）とは、ＧＰＳ信号等と同様の信号を電波として発振する機器であり、ユーザが受信できるＧＰＳ衛星２０の数が不十分な場合にＧＰＳ衛星２０を利用した航法を補間する意味で使用される。例えば、ａ，ｂ，ｃ，ｄの４つのＧＰＳ衛星２０からの電波で測位を行っているとき、ｄの電波が途絶えると、代わりにｅという擬似衛星からの電波を用い、測位制御装置１００も測位装置２００もａ，ｂ，ｃというＧＰＳ衛星２０と、ｅという擬似衛星の電波を用いてのＲＴＫ−ＧＰＳによる高精度測位が可能になる。

図３は、本発明の他の実施形態に係る移動体の測位制御システムの内部構成を示すブロック図であり、ここでは、トータルステーション３０の代替として無線ＬＡＮ（Local Area Network）基準局４０が示されている。他は、図２に示す実施形態と同様である。
無線ＬＡＮを用いる場合は、既知点に最低３つの無線ＬＡＮ基準局４０を設置し、そして建機１０を無線ＬＡＮの移動局５０とする。
無線ＬＡＮ基準局４０は、ＲＴＫ−ＧＰＳによる高精度測位が可能なとき、ＲＴＫ−ＧＰＳ受信機１１で受信される既知点での観測データを、無線送信機１４を介して建機１０へ送信する。一方、電波事情の悪い異常時（ＲＴＫ−ＧＰＳによる高精度測位が不可能なとき）、無線ＬＡＮ基準局４０は三点測量の原理で建機１０の現在位置を無線ＬＡＮによる測位で精度よく算出し、その位置情報を建機１０に送信する。
なお、一例として、無線ＬＡＮによる測位は、位置が既知の少なくとも３つの図示しない無線ＬＡＮ固定局を用いて無線ＬＡＮ移動局５０の位置を測位するものであり、無線ＬＡＮ基準局４０からの無線ＬＡＮ移動局５０に対する呼びかけに応答する当該移動局５０の電波を前記した図示しない各無線ＬＡＮ固定局で受信し、無線ＬＡＮ基準局４０が、その受信の時間差（距離差）から移動局の位置をリアルタイムで測位し位置情報を得るというものである。

図４は、本発明実施形態に係る移動体の測位制御装置、具体的には、図２、図３に示す測位制御装置１００のうち、ＰＣ３の高精度測位部３１の内部構成を示すブロック図である。
高精度測位部３１は、ＲＴＫ−ＧＰＳ測位処理系３１１と、慣性（ＩＮＳ）センサ測位処理系３１２と、支援センサ測位処理系３１３と、タイミング制御部３１４と、複合測位精度判定部３１５と、複合測位誤差補正部３１６と、ＲＴＫ−ＧＰＳデータファイル３１７と、慣性センサデータファイル３１８と、支援センサデータファイル３１９と、閾値テーブル３２０とで構成される。前記した各構成ブロックのうち、慣性センサ測位処理系３１２と、支援センサ測位処理系３１３と、複合測位誤差補正部３１６で測位誤差補正制御部３００を構成する。

ＲＴＫ−ＧＰＳ測位処理系３１１は、ＲＴＫ−ＧＰＳ受信機１を介して受信される信号（ＧＰＳの観測データ）と、既知点に設置された測位装置２００から送信される既知点でのＧＰＳの観測データを、タイミング制御部３１４から出力されるタイミング信号に基づき取込む。そして、ＲＴＫ−ＧＰＳ受信機１により受信される観測データに基づき測位演算される建機１０の現在位置情報と、既知点に設置された測位制御装置２００から送信される観測データに基づく既知点の位置情報とで基線解析を行い、リアルタイムに建機１０の現在位置（緯度・経度・高さ）を決定し、ＲＴＫ−ＧＰＳデータファイル３１７に格納するとともに複合測位精度判定部３１５へ供給する。

また、測位誤差補正制御部３００を構成する慣性センサ測位処理系３１２は、慣性センサ２から到来する信号を、タイミング制御部３１４から出力されるタイミング信号によって取込み、建機１０が移動中の加速度を測定し、積分計算によって移動距離を算出して現在位置を求め、慣性センサデータファイル３１８に格納するとともに複合測位精度判定部３１５へ供給する。
複合測位精度判定部３１５は、ＲＴＫ−ＧＰＳ測位処理系３１１と慣性センサ測位処理系３１２とによる測位結果を組み合わせて複合測位を行い、これらの複合測位により、閾値テーブル３２０にあらかじめ登録された精度が得られるか否かを比較演算により判定し、その結果によって複合測位誤差補正部３１６を起動する。
なお、精度が得られないと判定される場合とは、例えば、通常あり得ないような速度で移動したという結果が得られた場合、実際は移動しているのに、停止しているというような結果が得られた場合等である。

一方、測位誤差補正制御部３００を構成する支援センサ測位処理系３１３は、既知点に設置されるトータルステーション等の測位装置２００から送信される位置、姿勢、速度情報から成る建機１０の位置情報をタイミング制御部３１４から出力されるタイミング信号に基づき取込んで複合測位誤差補正部３１６へ供給する。
複合測位誤差補正部３１６には、前記した支援センサ測位処理部３１３から送信される建機１０の位置情報（前記した「緯度・経度・高さ」）の他に、慣性センサ測位処理系３１２から出力される建機１０の航法情報（前記した「位置、姿勢、速度」）が供給されており、複合測位誤差補正部３１６は、複合測位精度判定部３１５で必要な精度が得られないと判定された場合、既知点に設置されたトータルステーション３０等の測位装置２００から送信される位置情報を用いて慣性センサ測位処理系３１２による測位誤差を補正し、慣性センサデータファイル３１８を更新する。前記した慣性センサ測位処理系３１２による測位誤差の補正についは後記する。

なお、複合測位精度判定部３１５は、複合測位による今回測位結果と前回側位結果との差分と、閾値テーブル３２０に登録された精度（閾値）を比較することにより必要な精度が得られるか否かを判定することとし、測位誤差補正制御部３００は、トータルステーション３０等の測位装置２００から出力される測位結果を基準値とし、慣性センサ２による測位結果を観測量とするカルマンフィルタにより、建機１０の現在時刻における位置を推定することで、慣性センサ２を介して得られる時間経過とともに大きくなる測位誤差を補正することとする。カルマンフィルタの詳細については後記する。

図５は、本発明実施形態に係る移動体の測位制御装置、具体的には、図２、図３に示す測位制御装置１００のうち、ＰＣ３の建機制御部３２の内部構成を示すブロック図である。
建機制御部３２は、作業計画ファイル３２１と、航法情報受信部３２２と、制御量計算部３２３と、対象制御部３２４と、作業計画登録部３２５とで構成される。

制御量計算部３２３は、航法情報受信部３２２を介して取得される、位置、姿勢、速度情報の少なくとも１つから、建機１０の運転を含む、建機１０のアームやブレード等の制御対象部品を動作させるために、あらかじめ作業計画ファイル３２１に登録された作業計画に従う、車軸等の回転速度や操蛇角等の制御量を演算して対象制御部３２４に供給する。
対象制御部３２４は、制御量計算部３２３で計算された制御量に基づき制御対象の動作を制御する。なお、作業計画登録部３２５は、あらかじめユーザによって定義される、建機１０の運転プログラム、あるいは建機１０のアームやブレードによる作業内容を時系列的に作業計画ファイル３２１に登録する。

図６〜図１０は、本発明実施形態に係る移動体の測位制御装置の動作を説明するために引用したフローチャートであり、基本動作（図６）、ＲＴＫ−ＧＰＳによる衛星航法（衛星測位）と慣性センサによる慣性航法との複合測位（図７）、慣性センサによる慣性航法と支援センサによる複合測位（図８）、カルマンフィルタ補正処理（図９）、建機制御（図１０）のそれぞれの動作の流れが示されている。
図６〜図１０にはまた、本発明のコンピュータプログラムの処理手順も併せて示されている。このコンピュータプログラムは、ＰＣ３が持つ記憶装置（図示せず）にあらかじめ格納され、あるいは外部からインストールされ、内蔵のＣＰＵが当該コンピュータプログラムを逐次読み出し、実行することで、以下に示す機能が実現されるものである。
以下、図６〜図１０に示すフローチャートを参照しながら本発明実施形態に係る移動体の測位制御装置の動作について詳細に説明する。

図６は、本発明実施形態に係る移動体の測位制御装置の動作を説明するために引用したフローチャートであり、具体的には、ＰＣ３の高精度測位部３１による処理の流れを示す。

ところで、建機１０は、通常、ＲＴＫ−ＧＰＳ受信機１と慣性センサ２によりそれぞれ受信される信号をもとに衛星航法と慣性航法による複合測位により、自身の現在位置を認識し、作業実行のための位置決めを行っている。図６に示すフローチャートにおいて、高精度測位部３１は、まず、複合測位精度判定部３１５において、前記した衛星航法と慣性航法による複合測位で充分な精度が得られるか否かを判定する（ステップＳ６１）。
なお、ＲＴＫ−ＧＰＳ測位処理系３１１では、ＲＴＫ−ＧＰＳ受信機１により受信されるＧＰＳの信号（観測データ）に基づき測位演算される建機１０の位置情報と、既知点に設置された測位制御装置２００から送信される観測データに基づき測位演算される既知点の位置情報とで基線解析を行ってリアルタイムに建機１０の位置（緯度・経度・高さ）を決定する処理を実行し、また、慣性センサ測位処理系３１２では、慣性センサ２から到来する信号に基づき建機１０が移動中の加速度を測定し、積分計算によって移動距離を算出して位置を求める処理をそれぞれ実行している。そして、複合測位精度判定部３１５において、ＲＴＫ−ＧＰＳ測位処理系３１１による測位結果と慣性センサ測位処理系３１２による測位結果とを組み合わせて現在時刻における建機１０の位置、姿勢を連続的に算出している。つまり、ＲＴＫ−ＧＰＳの１秒間隔での測位の補間を行う。

なお、複合測位により必要な精度が得られるか否かの判定（ステップＳ６１）は、複合測位による今回の測位結果と前回の側位結果の差分と、閾値テーブル３２０にあらかじめ登録されてある閾値とを比較することによりなされる。
前記したステップＳ６１の判定処理において、必要な精度が得られると判定された場合は（ステップＳ６１“Ｙｅｓ”）、前記した衛星航法と慣性航法による複合測位を継続する（ステップＳ６２）。

前記した衛星航法と慣性航法による複合測位については、具体的な手順が図７のフローチャートに示されている。すなわち、慣性センサ測位処理系３１２は、慣性センサ２から到来する、位置、姿勢、速度に関する情報を１／５０秒間隔で採取し（ステップＳ６２１）、それを５０回繰り返した後（ステップＳ６２２）、ＲＴＫ−ＧＰＳ受信機１を介して採取される信号（観測データ）との同期をとる（ステップＳ６２３）。

一方、図６に示すステップＳ６１において、複合測位精度判定部３１５で必要な精度が得られないと判定された場合（ステップＳ６１“Ｎｏ”）、慣性センサ２による慣性航法と、本発明により追加された支援センサ（ここではトータルステーション３０）による測量結果に従う複合測位を行う（ステップＳ６３）。

慣性センサ２による慣性航法と支援センサによる測量結果に従う複合測位については具体的な手順が図８のフローチャートに示されている。すなわち、慣性センサ測位処理系３１２は、慣性センサ２から到来する信号を１／５０秒間隔で採取し（ステップＳ６３１）、それを５０回繰り返した後（ステップＳ６３２）、トータルステーション３０の出力である、位置、姿勢に関する航法情報との同期をとる（ステップＳ６３３）。そして、図６のステップＳ６４に示す「ＩＮＳの誤差補正」処理、ここでは、支援センサ測位処理系３１３により出力される、位置、姿勢に関する航法情報から、慣性センサ測位処理系３１２により出力される位置、姿勢の誤差補正を行う処理へ移行する。また、ステップＳ６２→ステップＳ６４という処理の流れでの「ＩＮＳの誤差補正」を行う。

なお、前記したステップＳ６３１とステップＳ６３３の処理において、単に情報を採取する処理と並行して、支援センサ測位処理系３１３では、既知点に設置されたトータルステーション３０から送信される位置、姿勢から成る建機１０の航法情報をタイミング制御部３１４から出力されるタイミング信号に基づき取込んで複合測位誤差補正部３１６へ供給している。また、慣性センサ測位処理系３１２は、慣性センサ２から到来する信号に基づき建機１０が移動中の加速度を測定し、積分計算によって移動距離を算出して現在位置を求め、複合測位誤差補正部３１６へ都度供給している。
このことにより、複合測位誤差補正部３１６では後記するＩＮＳの誤差補正処理が可能になる。

説明を図６に示す基本動作のフローチャートに戻し、ＩＮＳの誤差補正処理（ステップＳ６４）について説明する。ここでは、支援センサ測位処理系３１３により出力される航法情報から、慣性センサ測位処理系３１２により出力される航法情報の誤差補正を行う処理について説明する。
複合測位誤差補正部３１６には、支援センサ測位処理部３１３から送信される建機１０の位置、姿勢に関する航法情報の他に、慣性センサ測位処理系３１２から出力される建機１０の位置、姿勢、速度に関する航法情報が供給されており、複合測位誤差補正部３１６は、複合測位精度判定部３１５で必要な精度が得られないと判定された場合、既知点に設置されたトータルステーション３０から送信される航法情報を用いて慣性センサ測位処理系３１２による測位誤差を補正し、慣性センサデータファイル３１８を更新する。前記した測位誤差の補正にあたり、ここでは、カルマンフィルタを用いることとする。

「カルマンフィルタ」とは、システムの状態推定を行うフィルタリング理論であり、状態方程式と観測方程式の２つの演算式で示される。これら方程式については、例えば、特開２０００−２８３８０号公報にその詳細が示されている。本発明実施形態では、慣性航法と衛星航法による複合測位にあって、慣性航法の精度を向上させる意味で用いられる。
具体的に、測位誤差補正制御部３００（複合測位誤差補正部３１６）は、トータルステーション３０を介して得られる、位置、姿勢、速度から成る航法情報と、慣性センサ２を介して得られる、位置、姿勢、速度から成る航法情報との差分を観測量としてカルマンフィルタに入力し、慣性センサ２を介して得られる航法情報及び慣性センサ（ジャイロ、加速度計）の出力（角速度、加速度）の誤差を状態量として推定し（カルマンフィルタ出力であって、建機１０の現在時刻における、位置、姿勢、速度）、当該推定された状態量と慣性センサ２を介して得られる航法情報とを演算することにより誤差を補正している。ここで支援センサから得られる航法情報としては位置、速度（姿勢を含まない）でもよい。
なお、ここでは、支援センサ測位処理系３１３により出力される航法情報から、慣性センサ測位処理系３１２により出力される航法情報の誤差補正を行う場合を例示したが、ＲＴＫ−ＧＰＳ測位処理系３１１により出力される測位結果から、慣性センサ測位処理系３１２により出力される航法情報の誤差補正を行う場合も同様である。但し、この場合、カルマンフィルタに入力される観測量は、ＲＴＫ−ＧＰＳを介して得られる、位置、速度から成る測位結果と、慣性センサ２を介して得られる、位置、姿勢、速度から成る航法情報との差分になる。

ここで、慣性センサ測位処理系３１２は、慣性センサ２を介して測位結果を取得している状態で、その観測時刻と測位結果と誤差共分散を出力している。また、支援センサ測位処理系３１３は、トータルステーション３０を介して測位結果を取得している状態で、その観測時刻と測位結果と誤差共分散を出力している。前記した両出力を入力として得た複合測位誤差補正部３１６は、支援センサ測位処理系３１３から出力される測位結果を取得してその観測時刻まで遡ってカルマンフィルタによる状態量の推定を行い、当該推定した状態量を初期値とし慣性センサ測位処理系３１２から出力される測位結果を用いて現在時刻までの状態量の伝播を行う。そして、慣性センサ測位処理系３１２における最新の観測時刻の状態量から現在時刻までの状態量を予測する。ここでいう状態量とは、建機１０の位置、姿勢、速度の航法情報をいう。

前記した状態量を予測するための処理の流れが図９にフローチャートで示されている。
具体的に、複合測位誤差補正部３１６は、まず、観測値Ｙの計算を行う（ステップＳ６４１）。観測値Ｙの計算は、支援センサ測位処理系３１３から出力される位置、姿勢、速度に関する観測情報（航法情報）に基づき所定の演算式を実行することにより求めることができる。次に、状態遷移行列Ｆの計算を行う（ステップＳ６４２）。この計算処理は、前回の状態遷移行列Ｆの計算時点からの移動距離、経過時刻および、先に求めた姿勢から状態遷移行列Ｆを求めるものである。

以下、前記したステップＳ６４１、Ｓ６４２で計算した観測値Ｙおよび状態遷移行列Ｆをもとに状態ベクトルの推定量Ｘを求める（ステップＳ６４３〜Ｓ６４６）。具体的には、まず、誤差共分散行列Σの予測計算を行い（ステップＳ６４３）、カルマンゲインＫの計算を行う（ステップＳ６４４）。その後、誤差共分散行列Σの計算を行い（ステップＳ６４５）、更に、カルマンゲインＫ、および観測値Ｙに基づいて状態ベクトルの推定量Ｘを求める（ステップＳ６４６）。
前記した状態方程式、観測方程式、および観測値Ｙ、状態遷移行列Ｆ、誤差共分散行列Σ、カルマンゲインＫ、状態ベクトルの推定量Ｘを求めるために使用される演算式は、前記した特開２０００−２８３８０号公報にその一例が示されている。
以上の処理をトータルステーション３０等、既知点に設置された測位装置２００から航法情報（位置、姿勢、速度）が得られる毎に繰り返し行うことでＩＮＳ誤差補正（ステップＳ６４）を行う。

説明を図６の基本動作を示すフローチャートに戻す。高精度測位部３１により前記したＩＮＳ誤差補正処理（ステップＳ６４）が実行され、その結果得られる航法情報は建機制御部３２に転送される。
建機制御部３２では、その航法情報をリアルタイムに航法情報受信部３２２で受信して制御量計算のために制御量計算部３２３へ供給する。制御量計算部３２３は、作業計画ファイル３２１にあらかじめ登録されてある作業計画に基づき、例えば、回転速度や操蛇角等キャタピラ（登録商標）の制御量を計算して対象制御部３２４へ供給する。対象制御部３２４は、制御量計算部３２３により供給される制御量に基づき、建機自身の運転、もしくはアームやブレード等の制御を行う。

図１０は、建機の制御例を具体的に説明するために引用したフローチャートであり、（ａ）に、キャタピラ（登録商標）を制御する例、（ｂ）に建機アームやブレードを制御する例が示されている。
以下、図１０（ａ）（ｂ）に示すフローチャートを参照しながら、図５に示す建機制御部３２の動作について詳細に説明する。

図１０（ａ）において、まず、ユーザは、作業計画を策定し、キーボード等を介してＰＣ３にその作業計画毎に纏められたスケジュールを入力する。ＰＣ３の建機制御部３２は、作業計画登録部３２５を起動し、入力された作業計画を取込んで作業計画ファイル３２１（記憶装置）に登録する（ステップＳ６６１）。
一方、建機制御部３２では、航法情報受信部３２２により高精度測位部３１から送信される航法情報をリアルタイムに受信しており、都度、制御量計算部３２３に供給している。このことにより、制御量計算部３２３では、取得した建機１０の位置、姿勢、速度から成る航法情報から作業計画ファイル３２１に登録された作業計画に沿う位置に移動させるように、車軸の回転速度や操蛇角等の制御量を計算して対象制御部３２４へ供給する（ステップＳ６６２）。続いて、対象制御部３２４では、その制御量に従って制御対象となるキャタピラ（登録商標）を運転制御する（ステップＳ６６３）。

図１０（ｂ）において、まず、ユーザは作業計画を策定し、その作業計画に沿って制御対象となるアームやブレードの動きについてキーボード等を介してＰＣ３に入力する。このことにより、ＰＣ３の建機制御部３２は作業計画登録部３２５を起動し、入力された作業計画を取込んで作業計画ファイル３２１（記憶装置）に登録する（ステップＳ６６４）。
一方、建機制御部３２では航法情報受信部３２２により高精度測位部３１から送信される航法情報をリアルタイムに受信しており、都度、制御量計算部３２３に供給している。このことにより、制御量計算部３２３では、取得した建機１０の位置、姿勢から成る航法情報からアームやブレード等の制御対象を、作業計画ファイル３２１に登録された作業計画に沿う、高さ（深さ）、方向に動作させるための制御量（角度等）を計算（ステップＳ６６５）して対象制御部３２４へ供給する。続いて、対象制御部３２４では、その計算結果に従って制御対象となるアームやブレードを制御する（ステップＳ６６６）。

図１１は、本発明実施形態に係る移動体の測位制御システムにおける他の作業環境をイメージした図である。
ここでは、移動体として転圧ローラを備えた建機１０が例示されており、慣性センサ２としてのジャイロや加速度計の他に、速度誤差を精度よく補正するために支援センサとしての車速センサ５が取り付けられている。
なお、車速センサ５は、本来車輪に取り付けられるものであるが、ここでは転圧ローラを軸支するフレームに取り付けられ、レーザ等によって建機１０の速度を計測し、慣性センサの速度誤差補正を行うための補正情報として利用する例が示されている。すなわち、車輪の有無に依存せずに車速を計測することができる。なお、建機１０は高架等の物陰にあって、図１同様、比較的電波事情の悪い作業環境をイメージしている。

図１２は、本発明の他の実施形態に係る移動体の測位制御システムの内部構成を示すブロック図であり、図２に示す実施形態同様、測位に関係するブロックのみ抽出して示してある。
図２に示す実施形態との差異は、ＰＣ３に実装される高精度測位部３１へ入力される情報として、既知点に設置されたトータルステーション３０等の測位装置２００による航法情報を、車速センサ５により計測される建機１０車速情報で代替したものである。このとき、ＰＣ３の高精度測位部３１は、入力される車速情報に基づき、慣性センサ２を用いた慣性航法との複合測位により慣性センサ測位の補正情報として用いるものである。この複合測位において、カルマンフィルタを用いる等、高精度測位の手順については、図６〜図９において支援センサを車速センサ５に置換した場合と同様であるため、重複を回避する意味で詳細説明を省略する。

以上説明のように本発明は、衛星航法と慣性航法による複合測位を用いて移動体の位置測位を行う測位制御装置を、複合測位であらかじめ定義された精度が得られるか否かを判定し、必要な精度が得られないと判定された場合、既知点に設置されるトータルステーションや移動体に設置される車速センサ等、支援センサから出力される測位結果を受信し、当該測位結果を用いて慣性航法による測位誤差を補正する構成としたものである。
このことにより、測位制御装置が、支援センサから出力される測位結果を受信し、当該測位結果を用いて慣性航法による測位誤差を補正することで、ＧＰＳ衛星からの電波が遮断される場所においても高い精度で連続的な位置測位を可能とし、従って、リアルタイム性、および廉価構成が要求される建機の制御に適用することができる。

ＲＴＫ−ＧＰＳは、携帯電話やインターネット等で、補正データ等を取得するネットワーク型のＲＴＫ−ＧＰＳでもよい。また、ＲＴＫ−ＧＰＳは一例であり、通常のＧＰＳの１０ｍ程度の精度（位置精度）に比較して高い数ｃｍオーダの高精度で高精度測位を行うが、「衛星からの電波を用いて（リアルタイムに）移動体の測位を行う高精度衛星測位」が、ＲＴＫ−ＧＰＳに限定されることはない。例えば、ＭＴＳＡＴや準天頂衛星等、本来のＧＰＳ衛星以外の衛星を用いる測位でもよいし、他の衛星測位でもよい。精度も、制御対象物の動きの速さに応じて、例えば数十ｃｍオーダの高精度でもよい。

本発明の実施形態に係る移動体の測位制御システムの作業環境をイメージした図である。 本発明の実施形態に係る移動体の測位制御システムの内部構成を示すブロック図である。 本発明の他の実施形態に係る移動体の測位制御システムの内部構成を示すブロック図である。 本発明の実施形態に係る移動体の測位制御装置のうち、高精度測位部の内部構成を示すブロック図である。 本発明の実施形態に係る移動体の測位制御装置のうち、建機制御部の内部構成を示すブロック図である。 本発明の実施形態に係る移動体の測位制御装置の動作を説明するために引用したフローチャートである。 本発明の実施形態に係る移動体の測位制御装置の動作を説明するために引用したフローチャートである。 本発明の実施形態に係る移動体の測位制御装置の動作を説明するために引用したフローチャートである。 本発明の実施形態に係る移動体の測位制御装置の動作を説明するために引用したフローチャートである。 本発明の実施形態に係る移動体の測位制御装置の動作を説明するために引用したフローチャートである。 本発明の他の実施形態に係る移動体の測位制御システムの作業環境をイメージした図である。 本発明の他の実施形態に係る移動体の測位制御システムの内部構成を示すブロック図である。

符号の説明

１，１１ ＲＴＫ−ＧＰＳ受信機
２ 慣性センサ
３ ＰＣ
４，１４ 無線送受信機
５ 車速センサ
３０ トータルステーション
３１ 高精度測位部
３２ 建機制御部
４０ 擬似衛星基準局または無線ＬＡＮ基準局
５０ 擬似衛星受信機または無線ＬＡＮ受信機
３００ 測位誤差補正制御部
３１１ ＲＴＫ−ＧＰＳ測位処理系
３１２ 慣性センサ測位処理系
３１３ 支援センサ測位処理系
３１４ タイミング制御部
３１５ 複合測位精度判定部
３１６ 複合測位誤差補正部
３２１ 作業計画ファイル（記憶装置）
３２２ 航法情報受信部
３２３ 制御量計算部
３２４ 対象制御部
３２５ 作業計画登録部

Claims (11)

1. 衛星からの電波を用いて移動体の測位を行う高精度衛星測位と、慣性センサを用いて前記移動体の変位量を計測して測位を行う慣性航法とを組み合わせ、前記移動体の位置座標を連続的に算出する、複合測位による移動体の測位制御装置であって、
   前記複合測位であらかじめ定義された精度が得られるか否かを判定する複合測位精度判定部と、
   前記複合測位精度判定部において前記定義された精度が得られないと判定された場合、既知点に設置された支援センサから出力される前記移動体の測位結果を受信し、当該測位結果を用いて前記慣性航法による測位誤差を補正する測位誤差補正制御部と、
   を備えたことを特徴とする複合測位による移動体の測位制御装置。
2. 前記支援センサは、
   前記既知点の位置情報を基に、前記移動体との距離と角度を観測して前記移動体の位置情報を求め、前記移動体に送信するものであることを特徴とする請求項１に記載の複合測位による移動体の測位制御装置。
3. 前記支援センサは、光学測量、写真測量および無線ＬＡＮ測量の少なくとも１つを用いて前記移動体の位置情報を求めるものであることを特徴とする請求項２に記載の移動体の測位制御装置。
4. 前記支援センサは、
   前記移動体に設置され、前記移動体の速度を観測する車速センサであることを特徴とする請求項１に記載の複合測位による移動体の測位制御装置。
5. 前記複合測位精度判定部は、
   前記複合測位による今回の測位結果と前回の側位結果の差分と、閾値とを比較することにより前記定義された精度が得られるか否かを判定することを特徴とする請求項１に記載の複合測位による移動体の測位制御装置。
6. 前記測位誤差補正制御部は、
   ＧＰＳを介して得られる測位結果、もしくは前記支援センサを介して得られる航法情報と前記慣性センサを介して得られる航法情報との差分を観測量としてカルマンフィルタに入力して前記慣性センサを介して得られる航法情報の誤差を状態量として推定し、前記推定された状態量に基づき前記慣性センサを介して得られる航法情報の誤差を補正することを特徴とする請求項１に記載の複合測位による移動体の測位制御装置。
7. 前記測位誤差補正制御部は、
   前記慣性センサを介して測位結果を取得している状態で、その観測時刻と測位結果と誤差共分散とを出力する慣性センサ測位処理系と、
   前記支援センサを介して測位結果を取得している状態で、その観測時刻と測位結果と誤差共分散とを出力する支援センサ測位処理系と、
   前記支援センサ測位処理系から出力される測位結果を取得してその観測時刻まで遡って前記カルマンフィルタによる状態量の推定を行い、当該推定した状態量を初期値とし前記慣性センサ測位処理系から出力される測位結果を用いて現在時刻までの状態量の伝播を行い、前記慣性センサ測位処理系における最新の観測時刻の状態量から現在時刻までの状態量を予測する複合測位誤差補正部と、
   を備えて成ることを特徴とする請求項６に記載の複合測位による移動体の測位制御装置。
8. 前記複合測位により取得される、位置、姿勢、速度情報の少なくとも１つから、前記移動体の制御対象を動作させるために、あらかじめ記憶装置に登録された作業計画に従う前記制御対象の制御量を計算する制御量計算部と、
   前記計算された制御量に基づき前記制御対象の動作を制御する対象制御部と、
   を更に備えたことを特徴とする請求項１〜７のいずれか１項に記載の複合測位による移動体の測位制御装置。
9. 衛星からの電波を用いて移動体の測位を行う高精度衛星測位と、慣性センサを用いて前記移動体の変位量を計測して測位を行う慣性航法とを組み合わせて前記移動体の位置座標を連続的に算出する、複合測位による移動体の測位制御システムであって、
   既知点、もしくは前記移動体に設置される支援センサと、
   前記移動体にあって、前記複合測位であらかじめ定義された精度が得られるか否かを判定する複合測位精度判定部と、前記定義された精度が得られないと判定された場合、前記支援センサから出力される測位結果を受信し、当該測位結果を用いて前記慣性航法による測位誤差を補正する測位誤差補正制御部とから成る測位制御装置と、
   を備えたことを特徴とする複合測位による移動体の測位制御システム。
10. 衛星からの電波を用いて移動体の測位を行う高精度衛星測位と、慣性センサを用いて前記移動体の変位量を計測して測位を行う慣性航法とを組み合わせ、演算装置により前記移動体の位置座標を連続的に算出する、複合測位による移動体の測位制御装置に用いられるコンピュータプログラムであって、
    前記複合測位であらかじめ定義された精度が得られるか否かを判定するステップと、
    前記定義された精度が得られないと判定された場合、既知点に設置される支援センサから出力される測位結果を受信し、当該測位結果を用いて前記慣性航法による測位誤差を補正するステップと、
    をコンピュータに実行させるコンピュータプログラム。
11. 前記複合測位により取得される、位置、姿勢、速度情報の少なくとも１つから、前記移動体の制御対象をリアルタイムに動作させるために、あらかじめ記憶装置に登録された作業計画に従う前記制御対象の制御量を計算するステップと、
    前記計算された制御量に基づき前記制御対象の動作を制御するステップと、
    を前記コンピュータに更に実行させることを特徴とする請求項１０に記載のコンピュータプログラム。

Images