JP3555003B2

JP3555003B2 - Mobile device position measurement device

Info

Publication number: JP3555003B2
Application number: JP23450997A
Authority: JP
Inventors: 俊宏青野; 健二郎藤井; 慎太郎初本; 敬之神谷
Original assignee: Hitachi Ltd
Current assignee: Hitachi Ltd
Priority date: 1997-08-29
Filing date: 1997-08-29
Publication date: 2004-08-18
Anticipated expiration: 2017-08-29
Also published as: JPH1172336A

Description

【０００１】
【産業上の利用分野】
本発明は移動機の位置計測装置に係り、特に、自律移動機などの制御のため、起伏のある場所でも位置を高精度に計測するのに好適な移動機の位置計測装置に関する。
【０００２】
【従来の技術】
従来の位置を計測する方法としては、特開平８−６８６５４号公報や特開平７−３０１５４１号公報などが出願されている。これらは自動車のナビゲーション装置の位置センサとして使われることを想定したもので、ＧＰＳ（ＧｌｏｂａｌＰｏｓｉｔｉｏｎｉｎｇＳｙｓｔｅｍ；全地球測位システム）、ジャイロ、車速センサなどを用い、カルマンフィルタにより位置を計算するものである。ＧＰＳだけでも、あるいはジャイロと車速センサだけでも位置を計測できるが、これらを組み合わせて使うことにより、位置計測精度の向上を狙ったものである。
【０００３】
【発明が解決しようとする課題】
例えば、芝刈り機を自動運転することを考える。グラウンド・ゴルフ場等では、芝を刈った跡の見た目の良さが要求されるため、刈残しは許されない。そこで、隣り合う刈跡をある程度オーバーラップさせることで刈残しを防いでいるが、オーバーラップ幅が大きくなるほど作業効率は下がる。通常許容されるオーバーラップ幅は１０〜３０ｃｍ程度であることを考えると、１０ｃｍ程度の精度を目標に制御することが望ましい。また、田や畑で農耕機械を自動運転することを考えると、農耕機械の作業部分の幅は、多くの場合１．５〜３ｍ程度であるので、効率よく作業するには、この１割程度の精度、すなわち１５〜３０ｃｍ程度の精度で走行する必要がある。高速道路での自動車の自動運転を考えても、車線からはみ出さずに走行するには１０ｃｍの精度が要求される。
【０００４】
しかし、上記従来例においては、ＧＰＳの精度が１００ｍ程度であまりよくなく、ジャイロと車速センサからのデータを用いてカルマンフィルタ等の方法で補正しても、せいぜい十数ｍ程度の精度しか実現できず、とうてい１０ｃｍの精度には達しない。１０ｃｍ程度の精度で位置を計測するには、このような精度が１００ｍ程度のＧＰＳを用いてもあまり意味がなく、もっと精度のいい計測手段を用いる必要がある。
【０００５】
また、平坦地をまっすぐに進むときには、ジャイロによって車体の垂直軸周りの回転角速度を計測して、この角速度が“０”を保つように走行すればよいが、起伏地で走行する場合には、車体垂直軸周りの回転角速度を“０”に保って走行しても、走行軌跡を真上から見おろしたらまっすぐにはなっていない。これは、起伏によるロール・ピッチが影響するためである。起伏地において位置を計測するには、上記従来例のような構成では不十分で、ロール・ピッチを検出して補正することが必要である。上記従来例ではロール・ピッチの影響は考慮されていなかった。
【０００６】
また、ＧＰＳを用いるのなら、ＧＰＳのアンテナが車体のどこに取り付けられているのかということも問題になる。これは精度が１００ｍ程度のＧＰＳを使うのであれば、車体のどこに取り付けようが、それほど問題にはならないが、もっと精度のいい計測手段を用いて、１０ｃｍの精度を目標に計測するとなると、アンテナの位置が問題となる。アンテナの位置の影響は、車体の傾斜・進行方向によって変わってくる。例えば、ゴルフ場の中には傾斜が１０度程度のところもあるが、この場合、２ｍの高さにアンテナがあると仮定すると、水平位置のずれは３４ｃｍになる。この数値だけで許容される誤差の範囲を超えてしまっている。上記従来例ではこのことは考慮されていない。
【０００７】
また、移動体の位置を計測するには、計測のリアルタイム性が要求されるが、ＧＰＳにおいては、アンテナが衛星からの電波を受信してから、位置を出力するのに遅れ時間が発生する。さらに、すべての計測周期にＧＰＳデータが得られるわけではない。つまり、５０ｍｓ毎に位置を計測したいとしても、ＧＰＳは１秒に１回しか位置を出力しないという機種もある。このようなＧＰＳデータの不連続性も考慮する必要がある。
【０００８】
本発明の目的は、自律移動機の制御において、従来よりもさらに高精度な位置の計算が、例えば起伏地でも可能となり、しかも、比較的安価なセンサ類の組み合わせで実現できる移動機の位置計測装置を提供することである。
【０００９】
【課題を解決するための手段】
本発明の移動機の位置計測装置は、車体の垂直軸周りの角速度を計測するジャイロと、前記車体の地面の起伏による傾斜を計測する傾斜センサと、前記車体の速度を計測する車速センサと、前記車体の位置を、ＧＰＳアンテナによって得た衛星からの信号および補正データに基づいて計算するディファレンシャルＧＰＳ受信機と、前記ＧＰＳに補正データを提供する通信機と、前記角速度および傾斜に基づいて車体の進行角を計算する進行角計算機と、前記車速、前記進行角、前記傾斜、および前記ディファレンシャルＧＰＳによる位置に基づいて、前記車体の位置を計算する位置計算機であり、前記ディファレンシャルＧＰＳ受信機が位置を出力しないときは、前記ジャイロからのデータ、前記傾斜センサからのデータ、および前記車速センサからのデータに基づいて計算した位置を累積する内界計測により前記車体の位置を計算し、前記ディファレンシャルＧＰＳ受信機が位置を出力するときは、前記ディファレンシャルＧＰＳ受信機が出力する位置が観測されているときに前記内界計測により計算した位置と、前記ディファレンシャルＧＰＳ受信機が出力する位置を前記ＧＰＳアンテナの位置で補正した位置とをそれぞれの信用度に応じて重み付けすることで、前記ディファレンシャルＧＰＳ受信機が出力する位置が観測されているときの前記車体の位置を求め、該ディファレンシャルＧＰＳ受信機が出力する位置が観測されているときの前記車体の位置を、前記内界計測によって、前記ジャイロからのデータ、前記傾斜センサからのデータ、および前記車速センサからのデータに基づいて計算した位置を累積することで補正し、現在の前記車体の位置を計測する構成とすることにより上記課題を解決する。ジャイロや各センサによる計測は、経時的な誤差があるが精密である。ＧＰＳによる測定は、精度はラフであるが経時的な影響がない。これらの特性を合わせ、さらにＧＰＳの遅延時間などを補正することにより、例えば起伏地でも、より正確な位置計算が可能になる。
【００１０】
【発明の実施の形態】
以下、本発明の実施の形態を、図面を参照して説明する。
「実施形態１：位置計測装置の実施形態」
本実施形態の構成と動作の一例を、まず各センサデータの取り込み方の観点から、〈車速の計測〉〈車体垂直軸回りの回転速度の計測〉〈車体の傾斜の計測〉〈ディファレンシャルＧＰＳによる位置の計測〉の順で説明し、次にこれらのセンサデータを処理する方法を、〈進行角の計算〉〈冗長性を利用した位置の計算〉の順で説明する。
【００１１】
〈車速の計測〉
車速の計測の仕方としては、車輪にエンコーダを取り付けて車輪の回転量に比例したパルスを計測して、このパルスの数をカウンタで数えるという方法がある。このような速度の計測方法をとる場合、本実施形態における車速センサ１０、１１は、エンコーダに相当し、コンバータｃ８、９は、パルス数を数えてディジタル信号として出力するカウンタに相当する。
【００１２】
また、車速を計測する別の方法としては、ドップラー速度計を車体前方に取り付けて、地面から反射する波のドップラー効果により速度を計測する方法がある。この方法では、ドップラー速度計が出力するアナログ信号をＡ／Ｄ変換して速度を取り込む。このような方法をとる場合は、本実施形態における車速センサ１０、１１はドップラー速度計に相当し、コンバータｃ８、９はＡ／Ｄ変換器に相当する。
【００１３】
〈車体垂直軸周りの回転角速度の計測〉
車体垂直軸周りの回転角速度は、光ファイバジャイロ１によって計測する。光ファイバジャイロ１は、中心軸が車体の垂直軸に一致するように取り付けられなくてはならない。これによって、車体の垂直軸周りの回転が検出できるようになる。光ファイバジャイロ１は、回転角速度に比例した電圧を出力することもあるし、回転角速度をシリアル通信データとして出力することもある。この電圧またはシリアル通信データは、Ａ／Ｄ変換器またはシリアル通信データデコーダによってディジタルデータに変換される。本実施形態におけるコンバータａ２は、このＡ／Ｄ変換器またはシリアル通信デコーダに相当する。
【００１４】
〈車体の傾斜の計測〉
平坦地を走行するのであれば、車速と車体垂直軸周りの回転角速度を計測すれば車体の位置は計測できるが、起伏地を走行する場合は、車体垂直軸周りの回転角速度を、地球の鉛直軸周りの回転角速度に投影した成分を求めなくてはならない。そこで、車体の傾斜を求める必要がある。車体の傾斜は傾斜センサ４を用いて求める。車両の加速度が十分小さいのなら、重力を検知することで車体の傾斜は求まり、車両の加速度が無視できない場合は、３軸の加速度センサと３軸のジャイロを組み合わせることで車体の傾斜は求まる。この重力センサあるいは３軸加速度センサと、３軸ジャイロの複合センサが、本実施形態における傾斜センサ４に相当する。この傾斜センサの出力としては、ロール・ピッチ姿勢をそれぞれアナログ出力するか、あるいはシリアル通信データとして出力する場合が考えられる。このときコンバータｂ５は、Ａ／Ｄ変換器あるいはシリアルデコーダに相当する。
【００１５】
〈ディファレンシャルＧＰＳによる位置の計測〉
ＧＰＳは人工衛星からの電波の伝搬時間を用いて位置を計測するセンサである。カーナビゲーションに使われるようなものは、人為的に加えられたノイズや、伝達するまでの電離層などの影響により精度が１００ｍ程度であるが、ディファレンシャルＧＰＳといって、位置が既知の固定された場所に、ＧＰＳアンテナ２０とＧＰＳ受信機１９をおいて位置計測誤差を計算し、この誤差を移動するシステムのＧＰＳ受信機１３に送信し、移動システム側のＧＰＳ受信機１３は、この誤差を補正して位置を計算することにより、高精度の位置を計測できるシステムもある。この精度は最近では１ｍ程度とも言われている。移動体の制御には１０ｃｍ程度の精度で位置が計測できることが望ましいので、本システムではこのディファレンシャルＧＰＳを用いる。多くの場合このディファレンシャルＧＰＳはシリアル通信の形で出力する。
【００１６】
コンバータｄ１２は、このシリアル通信の形の位置データをディジタルに変換する。また、１０ｃｍ程度の精度の計測が要求される際には、ＧＰＳアンテナ１４の傾きによる影響も考慮されなくてはいけない。ここでは傾斜センサ４で車体の傾斜をはかり、光ファイバジャイロ１と傾斜センサ４のデータを基に進行角計算機３で進行角を計算することにより、ＧＰＳアンテナ１４の位置を位置計算機１６にて補正する。詳しい補正の計算方法は後ほど説明される。
以上、各センサデータの取り込み方について説明した。次にこれらのセンサデータを処理する方法を、〈進行角の計算〉〈冗長性を利用した位置の計算〉の順で説明する。
【００１７】
〈進行角の計算〉
起伏地における進行方向を計算するには、車体垂直軸周りの回転角速度ωを、地球の鉛直軸周りの回転角速度に投影した成分を積分することで、進行方向αを求めなくてはならない。そのためには、車体垂直軸周りの回転角速度と、車体の傾斜（ピッチβ、ロールγ）を計測して、下記の数式１によって車体の進行角を求めなくてはならない。なお、この計算は、光ファイバジャイロ１と傾斜センサ４からのデータをもとに進行角計算機３で行われ、その結果は位置計算機１６に送られる。
【００１８】
【数１】

【００１９】
〈冗長性を利用した位置の計算〉
移動体の位置（ｘ、ｙ、ｚ）は、光ファイバジャイロ１、傾斜センサ４から求める進行角α、車体傾き角β、γと、車速センサ１０、１１により計測される車速ｒから以下の数式２に基づき計算できる。
【００２０】
【数２】

【００２１】
この位置計算の仕方を内界計測という。また、ＧＰＳ１３によっても位置（ξ、η、ζ）は計測できる。したがって、この計測系は冗長であるといえる。内界計測は、短時間であれば高精度であるが、時間が経つにつれて誤差が累積する。一方、ＧＰＳ１３による位置計測の精度は時間には依存しないが、移動機を制御するには不十分である。そこで、計測の冗長性を活かして、時間に関係なく移動体の制御に十分な精度で位置を計測したい。そのためには、各センサの誤差特性に応じて、内界計測データとＧＰＳデータに、各々の信用度に応じて最適な重み付けをして位置を計算する必要がある。ここでは信用度に応じた最適な重み付けの方法を説明する。なお、以下の説明では信用度のかわりに分散という統計学用語を用いるが、分散が大きいほど信用度が低いということである。
【００２２】
車速の誤差をρ、進行角、ピッチ、ロールの誤差をθ、φ、ψ、ＧＰＳの誤差を（δ、ε、κ）とおき、内界計測の中心点から見たＧＰＳのアンテナの位置を（−Ｓ、０、Ｈ）とし、ＧＰＳがｉ番目のデータを出力する時間をｃ（ｉ）、このデータが出力されるまでに要する時間をＤとおくと、観測のモデルは、数式３となる。
【００２３】
【数３】

【００２４】
ここで、数式４とおくと、数式３は数式５となる。
【００２５】
【数４】

【００２６】
【数５】

【００２７】
この数式５において、下記の数式６は内界計測による位置の変化を表している。また、数式７はＧＰＳアンテナ１４の傾きによる影響を表す項である。
【００２８】
【数６】

【００２９】
【数７】

【００３０】
ところで、ｉ番目までのＧＰＳデータが観測されているときの、下記数式８の分散行列と推定量を下記数式９とすると、
【００３１】
【数８】

【００３２】
【数９】

【００３３】
ｉ番目までのＧＰＳデータが観測されているときの、下記数式１０の分散行列と推定量は、数式１１、１２となる。
【００３４】
【数１０】

【００３５】
【数１１】

【００３６】
【数１２】

【００３７】
ここで、さらにｉ＋１番目のＧＰＳデータが観測されると、数式８の分散は、下記数式１３となる。
【００３８】
【数１３】

【００３９】
ただし、下記数式１４は、ｉ＋１番目のＧＰＳデータの分散行列である。
【００４０】
【数１４】

【００４１】
このとき、前記数式１２に示される、ｉ番目のＧＰＳデータを観測してからは、内界計測のみで計算してきた位置と、ｉ＋１番目のＧＰＳによる位置（数式１５）の最適な重みづけをした位置の推定量は、下記数式１６で与えられる。
【００４２】
【数１５】

【００４３】
【数１６】

【００４４】
ここで求められた推定量（上記数式１６）は過去のものなので、内界計測のデータによって現在の位置を求めるために、下記数式１７を計算する。
【００４５】
【数１７】

【００４６】
以上より、位置の計算の仕方は、ＧＰＳデータが出力されたときは、数式１１、１２、１３、１６、１７に基づいて位置を計算し、ＧＰＳデータが出力されていないときは、数式１７に基づいて計算すればよい。この計算は位置計算機１６にて行われる。
【００４７】
これは、内界計測とディファレンシャルＧＰＳのそれぞれの誤差の分散を計算して、この分散によって内界計測とディファレンシャルＧＰＳへのウェイトを最適化することで、計算によって求められた位置の誤差の分散を最小にしようというものである。
【００４８】
図１１に、この計算過程のブロック図を示す。ＧＰＳデータが観測されないときは、図１１で示されるように、内界データをもとに、進行角を計算して、計算された進行角と計測された速度をもとに位置を更新していく。ＧＰＳが観測されると、内界計測の分散と、内界計測とＧＰＳを両方用いたときの分散を計算し、この２つの分散をもとにして、ＧＰＳデータにアンテナ位置補正を行ったものと内界計測から計算した位置の加重平均をとる。ここで求められる位置はＧＰＳの遅延時間の分だけ遅れたデータなので、この遅れをこの間の進行方向と速度を用いて補正する。
【００４９】
〈本実施形態における動作〉
以下、本実施形態における位置計測装置の動作を、図２に示すフローチャートと図１１に示す計算プロセスのブロック図を用いて説明する。
コンバータａ２は光ファイバジャイロ１からの角速度信号を、コンバータｂ５は傾斜センサ４からの傾斜角信号を、コンバータｃ８、９は車速センサ１０、１１からの車速信号を、それぞれディジタル信号に変換する（ステップ１、２、３、図１１の「内界データの収得」）。
【００５０】
このディジタル化された角速度信号と傾斜角信号とを用いて、進行角計算機３は進行角を計算する（ステップ４、図１１の「進行角計算」）。
ここで、ＧＰＳ受信機１３がＧＰＳアンテナ１４で捕らえた衛星信号と、通信機１７で受信した位置補正信号とに基づき、位置を計算し出力したならば（ステップ５、図１１の「ＧＰＳデータの収得」）、コンバータｄ１２はＧＰＳ受信機１３からの位置信号をｄｉｇｉｔａｌ信号に変換し（ステップ９、図１１の「ＧＰＳデータの収得」）、位置計算機１６は数式１１（図１１の「内界計測の分散計算」）、数式１２（図１１の「内界計測による位置計算」）、数式１３（図１１の「ＧＰＳ補正分散計算」）、数式１６（図１１の「内界計測とＧＰＳによる位置計算」）、数式１７（図１１の「遅延補正」）に基づき、進行角と傾斜角と車速とＧＰＳデータから位置を計算する。
【００５１】
ＧＰＳ受信機１３が位置を出力しないなら（ステップ５）、位置計算機１６は数式１７に基づき進行角と傾斜角と車速から位置を計算する（ステップ６、図１１の「遅延補正」に相当）。
この計算された位置はコンバータｅ７により変換されて、アプリケーションシステム６（例えば自律移動機のコントローラ）に出力される（ステップ７）。
計測の周期を一定にするために、タイマ２２から割り込みが入るのをまち（ステップ８）、ステップ１に戻る。
【００５２】
〈本実施形態の効果〉
本実施形態においては、傾斜によるアンテナの位置も考慮した上で、冗長なセンサを用いて、それぞれのセンサの誤差特性に応じて最適な重み付けをして位置を計算するので、各センサ情報の長所が生かされ、ＧＰＳ単独あるいは内界計測単独で計測したときよりも精度の良い位置が得られる。
【００５３】
「実施形態２：本発明の自律移動機への適用」
位置計測装置を起伏地を対象とした自律移動機に搭載した場合の実施形態を、図３に示すブロック図を用いて説明する。
〈目標軌道の与え方〉
自律移動機が通るべき経路は予め入力しておく。地上を移動する場合は、水平座標を与えておけば垂直座標は一意に決定できるので、目標軌道は水平座標で記述する。すなわち、真上から見おろしたときに見える経路を与えるのである。目標軌道は、図５に示すように、直線（ＬＩＮＥ）か曲線（ＡＲＣ）かの種類と、始点座標、終点座標、回転中心座標、最低速度、最高速度、加減速距離からなる。制御用コントローラ５１はこれを順番に実行していく。
【００５４】
〈目標方向の決め方〉
この自律移動機は、位置計測装置５０で計測された位置と目標軌道の位置関係から目標方向を決め、目標方向に車体の進行方向が一致するようにステアリング制御機構５３を制御する。
目標方向は、図６に示すように、車体の位置から目標軌道へおろした垂線ベクトルに、ある係数をかけたものと、目標軌道の接線ベクトルの和で決められる。このように目標方向を決めると、車体の位置が目標軌道の上であれば目標方向は目標軌道上をなぞるように決まり、車体の位置が目標軌道からずれると目標方向は目標軌道に戻るように決まる。
【００５５】
〈ステアリングとアクセルの制御〉
車体方向が目標方向に追従するようにステアリング制御機構５３への指令量を制御コントローラ５１はＰＩＤ制御する。
目標速度は、目標軌道で与えられる最低速度、最高速度、加減速距離と観測された位置によって、図７に示すように、制御コントローラ５１にて決められる。車体速度が目標速度に追従するようにアクセル機構５２への指令量を制御コントローラ５１はＰＩＤ制御する。
自律移動機に本位置計測装置５０を搭載した場合の動作の流れを、図４のフローチャートにまとめる。
【００５６】
〈本実施形態の効果〉
従来技術では、起伏地における自律移動は十分な精度が得られなかったが、傾斜センサを用いて起伏に対応できるようにした位置計測装置で位置を計ることにより、起伏でも十分な精度で自律移動できるようになる。
【００５７】
「実施形態３：本発明の自律移動作業機械機への適用」
前述の実施形態２で述べた自律移動機は、作業機構を取り付けて、これを制御するようにすれば自律移動作業機械となる。本実施形態では位置計測装置を自律移動作業機械に搭載した場合を説明する。
【００５８】
図８に、本自律移動作業機械の構成を示す。これは、上記実施形態２の自律移動機に、作業機構５５を取り付けて制御できるようにしたものである。この場合の目標軌道のデータ構造を図１０に示す。これは図５に示した目標軌道に作業情報をつけ加えたものである。作業情報としては、たとえば作業機械が芝刈り機の場合は、刃の上げ下げ、回転停止であるし、耕うん機の場合は耕耘部の高さ、回転速さが相当する。また、農薬散布機の場合は散布する農薬の圧力が作業情報に相当する。作業機構５５はこれらの芝刈り刃駆動装置や、耕耘装置や、農薬散布機構に相当する。
【００５９】
図９に、本自律移動作業機械の動作のフローチャートを示す。これは図５に示す自律移動機のフローチャートに、ステップ８として制御用コントローラ５１が目標軌道で与えられている作業情報を作業機構に送るようにしたものである。
【００６０】
〈本実施形態の効果〉
従来技術では、起伏地における自律移動しながらの作業は十分な精度が得られなかったが、傾斜センサを用いて起伏に対応できるようにした位置計測装置で位置を計ることにより、起伏でも十分な精度で自律移動しながら作業できるようになる。
【００６１】
【発明の効果】
傾斜センサをつけて起伏地での進行角を正しく求められるようにし、さらに２つのＧＰＳを用意して固定側から移動側に補正データを送ることで精度の良くなったＧＰＳを用いて、それぞれの長所を生かすように各誤差の分散に応じて比重を変えて位置を計算することで、自律移動機を制御するのに十分な精度で位置が計算できる。最近では人工衛星からの電波の位相を用いて位置計測するＧＰＳもあり、この精度は数ｃｍに達しているが、非常に高価である。本発明では、このような非常に高価なＧＰＳを使わなくても、比較的安価なセンサを組み合わせることで自律移動に十分な高精度な位置計測ができるようになった。
【図面の簡単な説明】
【図１】本発明の位置推定装置の一実施形態の構成を示すブロック図である。
【図２】本発明の位置推定装置の一実施形態のフローチャートである。
【図３】本発明の一実施形態である自律移動機の構成を示すブロック図である。
【図４】本発明の一実施形態である自律移動機のフローチャートである。
【図５】本発明の一実施形態である自律移動機で用いられる目標軌道のデータ構造の一例である。
【図６】本発明の一実施形態である自律移動機での目標方向計算方法の一例である。
【図７】本発明の一実施形態である自律移動機での目標速度計算方法の一例である。
【図８】本発明の一実施形態である自律移動作業機械の構成を示すブロック図である。
【図９】本発明の一実施形態である自律移動作業機械のフローチャートである。
【図１０】本発明の一実施形態である自律移動作業機械で用いられる目標軌道のデータ構造の一例である。
【図１１】本発明の位置推定装置の一実施形態の処理のブロック図である。
【符号の説明】
１光ファイバジャイロ
２コンバータａ
３進行角計算機
４傾斜センサ
５コンバータｂ
６アプリケーションシステム
７コンバータｅ
８コンバータｃ
９コンバータｃ
１０車速センサ
１１車速センサ
１２コンバータｄ
１３ＧＰＳ受信機
１４ＧＰＳアンテナ
１５通信アンテナ
１６位置計算機
１７通信機
１８通信機
１９ＧＰＳ受信機
２０ＧＰＳアンテナ
２１通信アンテナ
２２タイマ
５０位置計測装置
５１制御コントローラ
５２アクセル制御機構
５３ステアリング制御機構
５４車輪
５５作業機構[0001]
[Industrial applications]
The present invention relates to a position measuring device for a mobile device, and more particularly, to a position measuring device for a mobile device suitable for measuring a position with high accuracy even in an uneven place for controlling an autonomous mobile device.
[0002]
[Prior art]
As a conventional method for measuring a position, Japanese Patent Application Laid-Open Nos. 8-68654 and 7-301541 have been filed. These are supposed to be used as position sensors of a navigation device of a car, and use a GPS (Global Positioning System), a gyro, a vehicle speed sensor, and the like to calculate a position by a Kalman filter. Although the position can be measured only by the GPS or only by the gyro and the vehicle speed sensor, the position measurement accuracy is improved by using these in combination.
[0003]
[Problems to be solved by the invention]
For example, consider automatic operation of a lawnmower. In a ground golf course or the like, the appearance of the mowing marks is required, so that no mowing is allowed. Therefore, the remaining mowing is prevented by overlapping adjacent mowing traces to some extent, but the working efficiency decreases as the overlapping width increases. Considering that the normally allowable overlap width is about 10 to 30 cm, it is desirable to control the accuracy to about 10 cm. Also, considering that the agricultural machine is automatically operated in a field or a field, the width of the working part of the agricultural machine is about 1.5 to 3 m in many cases. , That is, with a precision of about 15 to 30 cm. Even when considering the automatic driving of an automobile on a highway, an accuracy of 10 cm is required for traveling without protruding from the lane.
[0004]
However, in the above conventional example, the accuracy of the GPS is about 100 m, which is not very good. Even if correction is performed by a method such as a Kalman filter using data from a gyro and a vehicle speed sensor, only an accuracy of about a few tens of meters can be realized at best. In most cases, the accuracy of 10 cm is not reached. In order to measure the position with an accuracy of about 10 cm, it is meaningless to use a GPS having such an accuracy of about 100 m, and it is necessary to use a measuring means with higher accuracy.
[0005]
In addition, when traveling straight on a flat ground, the gyro measures the rotational angular velocity around the vertical axis of the vehicle body, and the vehicle may run so that this angular speed keeps “0”. Even when the vehicle travels while maintaining the rotational angular velocity around the vertical axis of the vehicle at “0”, the vehicle is not straight if the traveling trajectory is viewed from directly above. This is because the roll pitch due to the undulations affects. In order to measure the position on an uneven land, the configuration as in the above-described conventional example is insufficient, and it is necessary to detect and correct the roll pitch. In the above conventional example, the influence of the roll pitch was not considered.
[0006]
Also, if GPS is used, there is also a problem of where the GPS antenna is mounted on the vehicle body. This does not matter much if you use a GPS with an accuracy of about 100 m, no matter where you attach it to the car body, but if you use a more accurate measuring means to measure with an accuracy of 10 cm as a target, the antenna Position matters. The effect of the position of the antenna depends on the inclination and traveling direction of the vehicle body. For example, some golf courses have an inclination of about 10 degrees. In this case, assuming that the antenna is at a height of 2 m, the horizontal position shift is 34 cm. This value alone exceeds the allowable error range. This is not considered in the above conventional example.
[0007]
To measure the position of a moving object, real-time measurement is required. However, in GPS, a delay occurs after the antenna receives a radio wave from a satellite and then the position is output. Further, GPS data is not obtained in every measurement cycle. That is, there is a model in which the GPS outputs the position only once a second even if the position is to be measured every 50 ms. It is necessary to consider such discontinuity of GPS data.
[0008]
An object of the present invention is to control the position of a mobile device in controlling an autonomous mobile device, which can calculate a position with higher accuracy than before, for example, on an uneven land, and can be realized by a combination of relatively inexpensive sensors. It is to provide a device.
[0009]
[Means for Solving the Problems]
The position measuring device of the mobile device of the present invention includes a gyro for measuring an angular velocity of the vehicle body around a vertical axis, an inclination sensor for measuring an inclination of the vehicle body due to undulation of the ground, and a vehicle speed sensor for measuring the speed of the vehicle body. A differential GPS receiver that calculates the position of the vehicle body based on a signal from a satellite obtained by a GPS antenna and correction data, a communication device that provides correction data to the GPS, and a vehicle body based on the angular velocity and inclination. and movement angle calculator for calculating a movement angle, the vehicle speed, the movement angle, the inclined, and based on the position of the differential GPS, a position calculator for calculating the vehicle body position, the differential GPS receiver is located Is not output, the data from the gyro, the data from the inclination sensor, and the vehicle speed When the differential GPS receiver outputs a position, the position output by the differential GPS receiver is observed when the position of the vehicle body is calculated by an internal measurement that accumulates the position calculated based on the data from the satellite GPS. The differential GPS reception by weighting the position calculated by the internal measurement and the position output by the differential GPS receiver with the position of the GPS antenna in accordance with the respective degrees of credit. The position of the vehicle body when the position output by the aircraft is observed is obtained, and the position of the vehicle body when the position output by the differential GPS receiver is observed is obtained from the gyro by the internal measurement. Data from the tilt sensor and data from the vehicle speed sensor. The calculated position based on the data corrected by accumulating, to solve the above problems by adopting a configuration for measuring the current position of the vehicle body. The measurement by the gyro and each sensor has a time-dependent error, but is precise. The measurement by GPS has rough accuracy but no influence over time. Together, these properties, by further Rukoto be corrected and GPS time delay, for example in undulating areas, thereby enabling more accurate position calculations.
[0010]
BEST MODE FOR CARRYING OUT THE INVENTION
Hereinafter, embodiments of the present invention will be described with reference to the drawings.
"Embodiment 1: Embodiment of position measuring device"
An example of the configuration and operation of the present embodiment will be described first, from the viewpoint of how to capture each sensor data, <measurement of vehicle speed><measurement of rotation speed around the vertical axis of the vehicle><measurement of inclination of the vehicle><position by differential GPS> Next, a method for processing these sensor data will be described in the order of <calculation of advancing angle><calculation of position using redundancy>.
[0011]
<Measurement of vehicle speed>
As a method of measuring the vehicle speed, there is a method in which an encoder is attached to a wheel to measure a pulse proportional to the rotation amount of the wheel, and the number of the pulse is counted by a counter. When such a speed measurement method is adopted, the vehicle speed sensors 10 and 11 in the present embodiment correspond to encoders, and the converters c8 and 9 correspond to counters that count the number of pulses and output the digital signals.
[0012]
As another method of measuring the vehicle speed, there is a method of mounting a Doppler speedometer in front of the vehicle body and measuring the speed by the Doppler effect of waves reflected from the ground. In this method, the analog signal output from the Doppler velocimeter is A / D-converted to capture the velocity. When such a method is employed, the vehicle speed sensors 10 and 11 in the present embodiment correspond to Doppler velocimeters, and the converters c8 and 9 correspond to A / D converters.
[0013]
<Measurement of angular velocity around the vertical axis of the vehicle>
The rotational angular velocity about the vertical axis of the vehicle body is measured by the optical fiber gyro 1. The optical fiber gyro 1 must be mounted such that the central axis coincides with the vertical axis of the vehicle body. As a result, the rotation of the vehicle body about the vertical axis can be detected. The optical fiber gyro 1 may output a voltage proportional to the rotational angular velocity, or may output the rotational angular velocity as serial communication data. This voltage or serial communication data is converted into digital data by an A / D converter or a serial communication data decoder. The converter a2 in the present embodiment corresponds to the A / D converter or the serial communication decoder.
[0014]
<Measurement of body inclination>
When traveling on a flat ground, the position of the vehicle body can be measured by measuring the vehicle speed and the rotational angular velocity around the vertical axis of the vehicle, but when traveling on an uneven surface, the rotational angular speed around the vertical axis of the vehicle is The component projected onto the angular velocity around the axis must be determined. Therefore, it is necessary to determine the inclination of the vehicle body. The inclination of the vehicle body is obtained using the inclination sensor 4. If the acceleration of the vehicle is sufficiently small, the inclination of the vehicle body can be obtained by detecting gravity. If the acceleration of the vehicle cannot be ignored, the inclination of the vehicle body can be obtained by combining a three-axis acceleration sensor and a three-axis gyro. This composite sensor of the gravity sensor or the three-axis acceleration sensor and the three-axis gyro corresponds to the tilt sensor 4 in the present embodiment. As the output of the tilt sensor, it is conceivable that the roll / pitch attitude is output in analog form or output as serial communication data. At this time, the converter b5 corresponds to an A / D converter or a serial decoder.
[0015]
<Position measurement by differential GPS>
GPS is a sensor that measures the position using the propagation time of radio waves from artificial satellites. The one used for car navigation has an accuracy of about 100 m due to the influence of artificially added noise and the ionosphere before transmission, but it is called a differential GPS, and a fixed place with a known position Then, the GPS antenna 20 and the GPS receiver 19 are used to calculate a position measurement error, transmit the error to the GPS receiver 13 of the moving system, and the GPS receiver 13 of the mobile system corrects the error. Some systems can measure the position with high precision by calculating the position by using the system. This accuracy is said to be about 1 m recently. Since it is desirable that the position of the moving object can be measured with an accuracy of about 10 cm, this system uses this differential GPS. In many cases, this differential GPS outputs in the form of serial communication.
[0016]
The converter d12 converts the position data in the form of serial communication into digital data. Further, when measurement with an accuracy of about 10 cm is required, the influence of the inclination of the GPS antenna 14 must be considered. Here, the inclination of the vehicle body is measured by the inclination sensor 4, and the traveling angle is calculated by the traveling angle calculator 3 based on the data of the optical fiber gyro 1 and the inclination sensor 4, so that the position of the GPS antenna 14 is corrected by the position calculator 16. I do. The detailed correction calculation method will be described later.
The method of capturing each sensor data has been described above. Next, a method of processing these sensor data will be described in the order of <calculation of advancing angle><calculation of position using redundancy>.
[0017]
<Calculation of travel angle>
In order to calculate the traveling direction on the undulating terrain, the traveling direction α must be obtained by integrating a component obtained by projecting the rotational angular velocity ω around the vertical axis of the vehicle body into the rotational angular velocity around the vertical axis of the earth. For this purpose, it is necessary to measure the rotational angular velocity about the vertical axis of the vehicle body and the inclination (pitch β, roll γ) of the vehicle body and obtain the advance angle of the vehicle body by the following equation 1. This calculation is performed by the traveling angle calculator 3 based on data from the optical fiber gyro 1 and the tilt sensor 4, and the result is sent to the position calculator 16.
[0018]
(Equation 1)

[0019]
<Calculation of position using redundancy>
The position (x, y, z) of the moving object is calculated from the following formula based on the advancing angle α, the body inclination angles β and γ obtained from the optical fiber gyro 1 and the inclination sensor 4 and the vehicle speed r measured by the vehicle speed sensors 10 and 11. 2 can be calculated.
[0020]
(Equation 2)

[0021]
This method of calculating the position is called inner world measurement. The position (ξ, η, ζ) can also be measured by the GPS 13. Therefore, it can be said that this measurement system is redundant. The inner world measurement is highly accurate in a short time, but errors accumulate over time. On the other hand, the accuracy of the position measurement by the GPS 13 does not depend on time, but is insufficient for controlling the mobile device. Therefore, we want to take advantage of the measurement redundancy to measure the position with sufficient accuracy for controlling the moving object regardless of time. For that purpose, it is necessary to calculate the position by weighing the inner world measurement data and the GPS data optimally in accordance with the respective credibility in accordance with the error characteristics of each sensor. Here, an optimal weighting method according to the credit level will be described. In the following description, a statistical term of variance is used instead of credit, but the larger the variance, the lower the credit.
[0022]
The error of the vehicle speed is ρ, the error of the advancing angle, pitch, and roll is θ, φ, ψ, and the error of the GPS is (δ, ε, κ), and the position of the GPS antenna viewed from the center point of the internal measurement is (-S, 0, H), the time when the GPS outputs the i-th data is c (i), and the time required until this data is output is D, and the observation model is expressed by the following equation (3). Become.
[0023]
(Equation 3)

[0024]
Here, if Equation 4 is set, Equation 3 becomes Equation 5.
[0025]
(Equation 4)

[0026]
(Equation 5)

[0027]
In this equation 5, the following equation 6 represents a change in position due to the inner world measurement. Equation 7 is a term representing the effect of the inclination of the GPS antenna 14.
[0028]
(Equation 6)

[0029]
(Equation 7)

[0030]
By the way, when the variance matrix and the estimated amount of the following equation 8 when the i-th GPS data are observed are represented by the following equation 9,
[0031]
(Equation 8)

[0032]
(Equation 9)

[0033]
When the GPS data up to the i-th GPS data is observed, the variance matrix and the estimated amount of Expression 10 below are Expressions 11 and 12.
[0034]
(Equation 10)

[0035]
(Equation 11)

[0036]
(Equation 12)

[0037]
Here, when the (i + 1) th GPS data is further observed, the variance of Expression 8 becomes Expression 13 below.
[0038]
(Equation 13)

[0039]
However, Equation 14 below is a variance matrix of the (i + 1) th GPS data.
[0040]
[Equation 14]

[0041]
At this time, after observing the i-th GPS data shown in the above equation 12, optimal weighting was applied to the position calculated only by the inner field measurement and the position (equation 15) by the (i + 1) th GPS. The position estimation amount is given by Expression 16 below.
[0042]
(Equation 15)

[0043]
(Equation 16)

[0044]
Since the estimated amount (formula 16) obtained here is a past value, the following formula 17 is calculated in order to determine the current position from the data of the internal measurement.
[0045]
[Equation 17]

[0046]
As described above, the method of calculating the position is as follows. When the GPS data is output, the position is calculated based on Equations 11, 12, 13, 16, and 17, and when the GPS data is not output, Equation 17 is used. What is necessary is just to calculate based on it. This calculation is performed by the position calculator 16.
[0047]
This is to calculate the variance of each error of the inner field measurement and the differential GPS, and optimize the weight to the inner field measurement and the differential GPS by this variance, thereby obtaining the variance of the error of the position obtained by the calculation. Try to minimize it.
[0048]
FIG. 11 shows a block diagram of this calculation process. When the GPS data is not observed, as shown in FIG. 11, the traveling angle is calculated based on the inner world data, and the position is updated based on the calculated traveling angle and the measured speed. Go. When GPS is observed, the variance of the inner field measurement and the variance when both the inner field measurement and the GPS are used are calculated, and the GPS data is subjected to antenna position correction based on the two variances. And the weighted average of the positions calculated from the inner world measurements. Since the position obtained here is data delayed by the GPS delay time, this delay is corrected using the traveling direction and speed during this time.
[0049]
<Operation in the present embodiment>
Hereinafter, the operation of the position measuring device according to the present embodiment will be described with reference to the flowchart shown in FIG. 2 and the block diagram of the calculation process shown in FIG.
The converter a2 converts the angular velocity signal from the optical fiber gyro 1, the converter b5 converts the inclination angle signal from the inclination sensor 4, and the converters c8 and 9 convert the vehicle speed signals from the vehicle speed sensors 10 and 11 into digital signals (step). 1, 2, 3, "Acquisition of inner world data" in FIG. 11).
[0050]
Using the digitized angular velocity signal and the tilt angle signal, the advancing angle calculator 3 calculates the advancing angle (step 4, "calculation of the advancing angle" in FIG. 11).
Here, if the GPS receiver 13 calculates and outputs the position based on the satellite signal captured by the GPS antenna 14 and the position correction signal received by the communication device 17 (step 5, "GPS data of FIG. 11") Acquisition ”), the converter d12 converts the position signal from the GPS receiver 13 into a digital signal (Step 9,“ Acquisition of GPS data ”in FIG. 11), and the position calculator 16 calculates the equation 11 (“ Inner measurement in FIG. 11 ”). ), Equation 12 (“Position calculation by inner field measurement” in FIG. 11), Equation 13 (“GPS correction variance calculation” in FIG. 11), and Equation 16 (“Position by inner field measurement and GPS in FIG. 11”). Calculation)), and the position is calculated from the advancing angle, the inclination angle, the vehicle speed, and the GPS data based on Expression 17 (“delay correction” in FIG. 11).
[0051]
If the GPS receiver 13 does not output the position (Step 5), the position calculator 16 calculates the position from the advancing angle, the inclination angle, and the vehicle speed based on Expression 17 (Step 6, corresponding to "delay correction" in FIG. 11).
The calculated position is converted by the converter e7 and output to the application system 6 (for example, a controller of the autonomous mobile device) (Step 7).
In order to keep the measurement cycle constant, wait for an interrupt from the timer 22 (step 8), and return to step 1.
[0052]
<Effect of this embodiment>
In the present embodiment, the position of the antenna due to the inclination is also taken into consideration, and the redundant sensor is used to calculate the position with the optimum weight according to the error characteristics of each sensor. Is utilized, and a position with higher accuracy than that obtained by measurement using only GPS or internal measurement alone can be obtained.
[0053]
“Embodiment 2: Application of the present invention to an autonomous mobile device”
An embodiment in which the position measurement device is mounted on an autonomous mobile device intended for an uneven land will be described with reference to a block diagram shown in FIG.
<How to give the target trajectory>
The route that the autonomous mobile device should pass is input in advance. When moving on the ground, if the horizontal coordinates are given, the vertical coordinates can be uniquely determined, so the target trajectory is described by the horizontal coordinates. In other words, it gives a path that can be seen when looking down from directly above. As shown in FIG. 5, the target trajectory includes a type of a straight line (LINE) or a curve (ARC), a start point coordinate, an end point coordinate, a rotation center coordinate, a minimum speed, a maximum speed, and an acceleration / deceleration distance. The control controller 51 executes this in order.
[0054]
<How to determine the target direction>
This autonomous mobile device determines a target direction from the positional relationship between the position measured by the position measuring device 50 and the target trajectory, and controls the steering control mechanism 53 so that the traveling direction of the vehicle body coincides with the target direction.
As shown in FIG. 6, the target direction is determined by the sum of a value obtained by multiplying a perpendicular vector drawn from the position of the vehicle body to the target track by a certain coefficient and a tangent vector of the target track. When the target direction is determined in this way, if the position of the vehicle body is on the target trajectory, the target direction is determined to follow the target trajectory, and if the position of the vehicle body deviates from the target trajectory, the target direction returns to the target trajectory. Decided.
[0055]
<Control of steering and accelerator>
The controller 51 performs PID control of a command amount to the steering control mechanism 53 so that the vehicle body direction follows the target direction.
The target speed is determined by the controller 51 based on the minimum speed, the maximum speed, the acceleration / deceleration distance, and the observed position given in the target trajectory, as shown in FIG. The controller 51 performs PID control of the command amount to the accelerator mechanism 52 so that the vehicle speed follows the target speed.
The operation flow when the position measuring device 50 is mounted on the autonomous mobile device is summarized in the flowchart of FIG.
[0056]
<Effect of this embodiment>
In the prior art, autonomous movement on uneven terrain could not achieve sufficient accuracy.However, by measuring the position with a position measurement device that can respond to undulation using an inclination sensor, autonomous movement with sufficient accuracy even on undulation become able to.
[0057]
"Embodiment 3: Application of the present invention to an autonomous mobile work machine"
The autonomous mobile machine described in the second embodiment is an autonomous mobile work machine if a work mechanism is attached and controlled. In the present embodiment, a case where the position measuring device is mounted on an autonomous mobile work machine will be described.
[0058]
FIG. 8 shows a configuration of the autonomous mobile work machine. This is such that the work mechanism 55 is attached to the autonomous mobile device of the second embodiment so that it can be controlled. FIG. 10 shows the data structure of the target trajectory in this case. This is obtained by adding work information to the target trajectory shown in FIG. As the work information, for example, when the work machine is a lawn mower, the blade is raised and lowered and rotation is stopped, and when the work machine is a tiller, the height and the rotation speed of a tilling unit correspond to the work machine. In the case of a pesticide sprayer, the pressure of the pesticide to be sprayed corresponds to the work information. The working mechanism 55 corresponds to these lawn mowing blade driving devices, tillage devices, and agricultural chemical spraying mechanisms.
[0059]
FIG. 9 shows a flowchart of the operation of the autonomous mobile work machine. This is the same as the flowchart of the autonomous mobile device shown in FIG. 5 except that the control controller 51 sends the work information given in the target trajectory to the work mechanism in step 8.
[0060]
<Effect of this embodiment>
In the prior art, the work while moving autonomously on an ups and downs did not have sufficient accuracy.However, by measuring the position with a position measurement device that was able to respond to the ups and downs by using an inclination sensor, You can work while autonomously moving with precision.
[0061]
【The invention's effect】
Attach the tilt sensor so that the advancing angle on the ups and downs can be obtained correctly, and prepare two GPSs and send the correction data from the fixed side to the moving side. By calculating the position by changing the specific gravity according to the variance of each error so as to take advantage of the advantages, the position can be calculated with sufficient accuracy to control the autonomous mobile device. Recently, there is also a GPS that measures the position using the phase of a radio wave from an artificial satellite. This accuracy has reached several centimeters, but is very expensive. In the present invention, even without using such an extremely expensive GPS, a highly accurate position measurement sufficient for autonomous movement can be performed by combining relatively inexpensive sensors.
[Brief description of the drawings]
FIG. 1 is a block diagram illustrating a configuration of an embodiment of a position estimation device of the present invention.
FIG. 2 is a flowchart of an embodiment of the position estimating device of the present invention.
FIG. 3 is a block diagram illustrating a configuration of an autonomous mobile device according to an embodiment of the present invention.
FIG. 4 is a flowchart of an autonomous mobile device according to an embodiment of the present invention.
FIG. 5 is an example of a data structure of a target trajectory used in the autonomous mobile device according to the embodiment of the present invention.
FIG. 6 is an example of a target direction calculation method in an autonomous mobile device according to an embodiment of the present invention.
FIG. 7 is an example of a target speed calculation method in an autonomous mobile device according to an embodiment of the present invention.
FIG. 8 is a block diagram illustrating a configuration of an autonomous mobile work machine according to an embodiment of the present invention.
FIG. 9 is a flowchart of the autonomous mobile work machine according to the embodiment of the present invention.
FIG. 10 is an example of a data structure of a target trajectory used in the autonomous mobile work machine according to the embodiment of the present invention.
FIG. 11 is a block diagram of a process according to an embodiment of the position estimation device of the present invention.
[Explanation of symbols]
1 Optical fiber gyro 2 Converter a
3 Travel angle calculator 4 Tilt sensor 5 Converter b
6 Application system 7 Converter e
8 Converter c
9 Converter c
10 Vehicle speed sensor 11 Vehicle speed sensor 12 Converter d
Reference Signs List 13 GPS receiver 14 GPS antenna 15 Communication antenna 16 Position calculator 17 Communication device 18 Communication device 19 GPS receiver 20 GPS antenna 21 Communication antenna 22 Timer 50 Position measurement device 51 Controller 52 Accel control mechanism 53 Steering control mechanism 54 Wheel 55 Work mechanism

Claims

A gyro for measuring an angular velocity around a vertical axis of the vehicle body, an inclination sensor for measuring an inclination of the vehicle body due to undulation of the ground, a vehicle speed sensor for measuring the speed of the vehicle body, and a position of the vehicle body were obtained by a GPS antenna. A differential GPS receiver that calculates based on a signal from a satellite and correction data, a communication device that provides correction data to the GPS, a travel angle calculator that calculates a travel angle of a vehicle body based on the angular velocity and the inclination, vehicle speed, the traveling angle, the tilt, and on the basis of the position due differential GPS, the position measuring apparatus of the mobile unit and a position calculator for calculating the vehicle body position,
When the differential GPS receiver does not output a position, the position of the vehicle body is obtained by an internal measurement that accumulates a position calculated based on data from the gyro, data from the inclination sensor, and data from the vehicle speed sensor. And calculate
When the differential GPS receiver outputs a position, the position calculated by the internal measurement when the position output by the differential GPS receiver is observed, and the position output by the differential GPS receiver are the By weighting the position corrected by the position of the GPS antenna in accordance with the degree of trust, the position of the vehicle body when the position output by the differential GPS receiver is observed is determined, and the differential GPS receiver The position of the vehicle body when the output position is observed is accumulated by the internal measurement, based on the data from the gyro, the data from the tilt sensor, and the position calculated based on the data from the vehicle speed sensor. To measure the current position of the vehicle body. Position measuring device of the mobile device according to claim.