JPH10160487A - 移動機の位置計測装置 - Google Patents

Abstract

(57)【要約】 【課題】 地表面上に他の設備を設置せずに、また、高
価なジャイロを３つも用いることなく、移動機が地表面
上でロール，ピッチ，ヨーの全ての軸の回りをどのよう
に回転しても、移動機の姿勢および位置を常に正確に決
定できる移動機の位置計測装置を提供する。 【解決手段】 移動機の回転角速度のヨー角成分を計測
する光ファイバジャイロ１と、移動機のピッチ姿勢を計
測するピッチ姿勢センサ２と、移動機のロール姿勢を計
測するロール姿勢センサ３と、移動機の速度を計測する
速度センサ４と、移動機のヨー角回転角速度，ロール姿
勢，ピッチ姿勢から移動機の姿勢を求めさらにこの姿勢
と速度から移動機の位置を計算する位置計算機５とから
なる移動機の位置計測装置。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、移動機の位置計測
装置に係り、特に起伏がある地表面上の領域内で自律移
動機を無人走行させるための位置計測装置に関するもの
である。

【０００２】

【従来の技術】起伏がある地表面上で自律移動機を無人
走行させるには、起伏の状況に応じてヨー姿勢を修正
し、位置を求める必要がある。起伏がある領域で走行す
る自律移動機の従来の位置計測方法は、光を利用するも
のと、超音波を利用するものと、３つのジャイロを用い
るものとに大きく分類できる。

【０００３】光を利用するものには、特開平5−274039
号公報，特開平2−16607号公報に記載のように、地表面
上に投光器を設置し移動機側でこの光を観測して位置を
計測するもの、特開平4−102104号公報に記載のよう
に、地表面上に投受光器を設置し移動機が反射する光で
位置計測をするもの、特開平1−162909号公報，特開平2
−125308号公報に記載のように、移動機に投受光器を設
置し地表面上に固定された反射鏡からの反射光を観測し
て位置計測をするものなどがある。

【０００４】超音波を利用するものには、特開平1−207
802号公報，特開平1−206411号公報，特開平1−206412
号公報に記載のように、地表面上に発信器を設置し移動
機上の受信機で観測して位置を求めるものがある。

【０００５】３つのジャイロを利用するものには、特開
平7−13625号公報，特開平7−13626号公報に記載のよう
に、ロール，ピッチ，ヨーの角速度を観測できるように
３つのジャイロを設置し、これらの角速度をそれぞれ単
純に積分して姿勢を求めるものがある。

【０００６】

【発明が解決しようとする課題】上記光を用いる方式で
は、地表面上に投光器，投受光器，反射鏡のいずれかを
設置する。移動機の位置を計算するには、それらの正確
な設置位置の情報が必要となる。そこで、設置工事，測
量などの手間のかかる作業が必要となる。

【０００７】同様に、超音波を用いる方式でも、地表面
上に発信器を設置し、その正しい位置を測量する必要が
ある。また、超音波は到達距離が短く、超音波方式では
精度が悪いという問題もあった。

【０００８】３つのジャイロを用いる方式は、地表面上
の設備が不要であるというメリットであるが、上記特開
平7−13625号公報，特開平7−13626号公報に記載のよう
に、それぞれの角速度を単純に積分し移動機の姿勢が求
められるのは、回転がロール軸，ピッチ軸，ヨー軸のい
ずれか一つの軸回りの回転に限定される場合のみであ
る。一般に成立する３つのジャイロから姿勢を求める方
法（後述の数式８で表現する示す方法）は、航空機の慣
性航法で用いられている姿勢計測の方法である。この方
法では、３つの角速度とも高精度で計測する必要があ
る。地表面上で自律移動機を走行させるために、ジャイ
ロを３つも使うのは、コストの面で不利である。

【０００９】本発明の目的は、地表面上に他の設備を設
置せずに、また、高価なジャイロを３つも用いることな
く、移動機が地表面上でロール，ピッチ，ヨーの全ての
軸の回りをどのように回転しても、移動機の姿勢および
位置を常に正確に決定できる移動機の位置計測装置を提
供することである。

【００１０】本発明の他の目的は、地表面上でロール，
ピッチ，ヨーの全ての軸の回りをどのように回転して
も、姿勢および位置を常に正確に決定できる位置計測装
置を備えた自律移動機を提供することである。

【００１１】

【課題を解決するための手段】本発明は、上記目的を達
成するために、起伏がある地表面上の領域内を移動する
移動機の回転角速度のヨー角成分を計測する光ファイバ
ジャイロと、移動機のロール姿勢を計測するロール姿勢
センサと、移動機のピッチ姿勢を計測するピッチ姿勢セ
ンサと、移動機の速度を計測する速度センサと、移動機
のヨー角回転角速度とロール姿勢とピッチ姿勢と速度と
に基づいて移動機の位置を計算する位置計算機とからな
る移動機の位置計測装置を提案する。

【００１２】本発明は、また、起伏がある地表面上の領
域内を移動する移動機の回転角速度のヨー角成分を計測
する光ファイバジャイロと、移動機の速度を計測する速
度センサと、移動対象領域の起伏状況地図と、起伏状況
地図および移動機の位置から移動機のロール姿勢および
ピッチ姿勢を計算する姿勢計算機と、移動機のヨー角回
転角速度とロール姿勢とピッチ姿勢と速度とに基づいて
移動機の位置を計算する位置計算機とからなる移動機の
位置計測装置を提案する。

【００１３】本発明は、上記他の目的を達成するため
に、前記のように特徴的に構成されたいずれかの位置計
測装置と、目標経路記憶装置と、制御量計算機と、走行
部とからなる自律移動機を提案する。

【００１４】前記走行部に、さらに、芝刈り部を取り付
けると、自律移動芝刈り機になる。前記走行部に、さら
に、牽引用フックを取り付けると、自律移動トラクタに
することができる。

【００１５】《本発明の原理》水平な地表面上にｘ軸を
とり、ｘ軸を上から見て時計回りに９０度回転させた方
向をｙ軸とし、鉛直上向きにｚ軸をとり、座標系ｘ0−
ｙ0−ｚ0を考える。

【００１６】図１に示すように、座標系ｘ0−ｙ0−ｚ0
をｚ軸の周りに（ｚ軸の正方向から負方向をみて時計回
りに）αだけ回転させて、できた座標系をｘ1−ｙ1−ｚ
1とする。

【００１７】図２に示すように、座標系ｘ1−ｙ1−ｚ1
をｙ1軸の周りに（ｙ1軸正方向から負方向をみて反時計
回りに）βだけ回転させて、できた座標系をｘ2−ｙ2−
ｚ2とする。

【００１８】図３に示すように、座標系ｘ2−ｙ2−ｚ2
をｘ2軸の周りに（ｘ2軸正方向から負方向をみて反時計
回りに）γだけ回転させて、できた座標系をｘ3−ｙ3−
ｚ3とする。

【００１９】一方、移動機については、前向きにｘ軸を
とり、右向きにｙ軸をとり、上向きにｚ軸をとる。移動
機に固定した座標系がｘ3−ｙ3−ｚ3に一致するとすれ
ば、移動機の姿勢は、角度α，β，γによって一意に表
される。この角度は、一般にオイラー角と呼ばれてい
る。

【００２０】これら座標系ｘn−ｙn−ｚn(ｎ＝0，1，
2，3)の座標軸方向の単位ベクトルをｉn，ｊn，ｋn(ｎ
＝0，1，2，3)で表すと、数式１〜数式３のようにな
る。

【００２１】

【数１】

【００２２】

【数２】

【００２３】

【数３】

【００２４】移動機の回転角速度をωとおき、移動機に
固定した座標系で分解して表すと、数式４のようにな
る。また、移動後の姿勢の定義により、数式５の関係が
成り立つ。[ωx，ωy，ωz]と[dγ/dt，dβ/dt，dα/d
t]との関係を求めるため、[ｉ3，ｊ3，ｋ3]と[ｉ2，ｊ
1，ｋ0]との関係を求めると、数式６のようになる。こ
れらの数式４，５，６から、数式７が成り立ち、成分ご
とに比較すると、数式８のようになる。

【００２５】

【数４】

【００２６】

【数５】

【００２７】

【数６】

【００２８】

【数７】

【００２９】

【数８】

【００３０】３つの光ファイバジャイロを用いて角速度
ωｘ，ωｙ，ωｚを計測し、数式８を解いてα，β，γ
を求める方式が、航空機の慣性航法で一般に用いられて
いる姿勢計測の方法である。この方法では、ωx，ωy，
ωzの３つとも高精度で計測する必要がある。

【００３１】航空機の慣性航法で用いられている方法で
は、ロール，ピッチ，ヨーともに、角速度を積分して姿
勢を求めたが、ロール，ピッチに関しては、重力方向を
検知すると、姿勢を直接的に求めることができる。ま
た、地磁気を検知すると、ヨー姿勢を直接的に求めるセ
ンサもある。これらの姿勢センサは、あまり精度が良く
ないが、角速度を積分して姿勢を求めているわけではな
いので、誤差が累積しないという利点がある。

【００３２】しかし、一定時間の精度を考えれば、光フ
ァイバジャイロの方が、積分による累積誤差を考慮して
も格段に高精度なので、航空機の位置計測や平坦地の自
律移動機の位置計測では、姿勢を直接的に求めるセンサ
よりも光ファイバジャイロで計測された角速度を積分す
る方式の方が、広く用いられている。

【００３３】ところで、本発明が対象とする地表面上を
移動する移動機は、高さ方向の移動が拘束されるので、
ヨー姿勢に関しては高精度が要求されるが、ピッチ，ロ
ール姿勢に関しては、それほどの高精度は要求されな
い。

【００３４】そこで、光ファイバジャイロほど高精度で
はないが、姿勢を直接計測するセンサを用いてピッチ，
ロール姿勢を計測し、ヨー姿勢のみ高精度な光ファイバ
ジャイロを用いて計測し、角速度を積分して姿勢および
位置を求めることが、コストと性能との兼ね合いでは、
有利であると考えられる。

【００３５】移動機に固定された座標系のｚ軸周りの回
転ωzを光ファイバジャイロで計測し、ロール姿勢β，
ピッチ姿勢γを姿勢センサで直接計測すると、地表面に
固定された座標系でのｚ軸周りの回転角速度は、数式９
により与えられる。

【００３６】

【数９】

【００３７】数式９を積分すると、ヨー姿勢αが求めら
れる。ちなみに、数式９をよく観察すると、回転ωzを
単純に積分してαが求められるのは、β，γが０の場合
のみであり、発明が解決しようとする課題で引用した特
開平7−13625号公報，特開平7−13626号公報の発明の適
用範囲が、限定的であることがわかる。

【００３８】３つの姿勢α,β，γが求められ、また移
動機の速度ｖが求められたら、移動機の位置は、数式１
０により与えられる。

【００３９】

【数１０】

【００４０】

【発明の実施の形態】次に、図４〜図１０を参照して、
上記本発明の原理による位置計測装置およびそれを応用
した移動機の実施例を説明する。

【００４１】《実施例１》図４は、上記本発明の原理に
よる位置計測装置の実施例の構成を示す斜視図である。
この位置計測装置は、移動機に固定された座標における
ヨー角速度を計測する光ファイバジャイロ１と、移動機
のピッチ姿勢を計測するピッチ姿勢センサ２と、ロール
姿勢を計測するロール姿勢センサ３と、移動機の速度を
計測する速度センサ４とを搭載している。速度センサ４
の一例としては、車輪６に取り付けられて車輪６の回転
数を計測するエンコーダが挙げられる。

【００４２】図５は、図４の位置計測装置の動作を説明
するフローチャートである。 ステップ１：光ファイバジャイロ１が、ヨー角速度ωz
を計測する。 ステップ２：ピッチ姿勢２センサが、ピッチ姿勢βを計
測する。 ステップ３：ロール姿勢３センサが、ロール姿勢γを計
測する。 ステップ４：位置計算機５が、ヨー角速度ωz，ピッチ
姿勢β，ロール姿勢γを用いて、数式９に基づき、ヨー
姿勢αを求める。 なお、速度センサ４は、各ステップの間、速度ｖを計測
し続けている。 ステップ５：位置計算機５は、数式１０に基づき、ヨー
姿勢α，ピッチ姿勢β，ロール姿勢γ，速度ｖから、移
動機の位置を求める。

【００４３】数式９に示すように、ヨー角速度ωzをピ
ッチ姿勢β，ロール姿勢γで補正した後に積分すると、
移動機がどのような姿勢になっても、その姿勢を正しく
求めることができ、その結果、位置ｘ，ｙ，ｚも正しく
求めることが可能となる。

【００４４】《実施例２》実施例１では、３つの未知量
ヨー姿勢α，ピッチ姿勢β，ロール姿勢γを求めるため
に、３つの計測量を用いた。ところが、地面上を動く場
合は、運動がほぼ２次元に限定されているので、地面の
起伏状況が予め与えられていれば、ヨー姿勢がわかる
と、ピッチ姿勢βとロール姿勢γとは、計算により求め
ることができる。本実施例は、ヨー軸回りの回転角速度
（または回転角度）と予め与えられている起伏状況とか
ら、位置を計測する装置である。

【００４５】図６は、本発明によりヨー軸回りの回転角
速度と予め与えられている起伏状況とから位置を計測す
る位置計測装置の実施例の構成を示す斜視図である。本
実施例２の起伏状況地図９には、地面上の点の座標
(ｘ，ｙ，ｚ)の集合{(ｘ0，ｙ0，ｚ0)，…，(ｘN，ｙ
N，ｚN)}が起伏状況として与えられている。この点のと
りかたは任意であるが、点が密であるほど地図の精度は
良くなる。なお、スタート時点での移動機の位置とヨー
姿勢とは、位置計算機に与えられていると仮定する。

【００４６】姿勢計算機８は、この位置とヨー姿勢と起
伏状況とをもとに、移動機のピッチ姿勢βとロール姿勢
γとを計算する。ピッチ姿勢βおよびロール姿勢γがわ
かると、位置計算機５は、光ファイバジャイロ１が計測
するヨー姿勢αと速度センサ４が計測する速度ｖとをも
とに、数式９に基づいて次の計測時点でのヨー姿勢αを
求めて、さらに、数式１０に基づいて位置ｘ，ｙ，ｚを
求める。このピッチ姿勢β，ロール姿勢γの計算とヨー
姿勢α，位置ｘ，ｙ，ｚの計算とを次々に繰り返してい
けば、位置を常に正確に把握できる。

【００４７】ここで、起伏状況地図を用いて姿勢を求め
る方法をより具体的に説明する。移動機の位置のｘ，ｙ
座標とヨー姿勢αとがわかっているときに、起伏状況地
図９に予め記録されている起伏状況データを用いて、ピ
ッチ姿勢β，ロール姿勢γを求める方法を説明する。

【００４８】まず、起伏状況地図９から、移動機の位置
ｘ，ｙに最も近い３つの点(ｘK，ｙK，ｚK)，(ｘL，ｙ
L，ｚL)，(ｘM，ｙM，ｚM)を探し出す。移動機は、この
３点(ｘK，ｙK，ｚK)，(ｘL，ｙL，ｚL)，(ｘM，ｙM，
ｚM)を通る平面に載っていると近似して考える。移動機
に固定した座標系のｚ成分の方向ベクトルｋ3は、この
平面の法線ベクトルと平行であるので、数式１１により
与えられる。ただし、各パラメータの関係は、数式１２
のようになる。

【００４９】

【数１１】

【００５０】

【数１２】

【００５１】次に、移動機の姿勢をα，β，γとおけ
ば、ｋ3は、数式１〜数式３により、数式１３のように
表される。この数式１３と数式１１とを比較すると、数
式１４のようになり、ピッチ姿勢β，ロール姿勢γが求
められる。

【００５２】

【数１３】

【００５３】

【数１４】

【００５４】以下の実施例３〜５は、実施例１または２
の位置計測装置の応用例である。 《実施例３》図７は、実施例１または２の位置計測装置
１０を用いて位置を計測し、その結果に基づいて目標経
路通りに移動する自律移動機の実施例の構成を示す斜視
図である。本実施例の目標経路記憶装置１１には、自律
移動機が通るべき目標軌道と、軌道上の各位置での目標
速度とが書き込まれている。

【００５５】図８は、図７の自律移動機の動作を示すフ
ローチャートである。 ステップ11：制御量計算機１２が、目標経路記憶装置１
１から目標軌道を読み出す。 ステップ12：位置計測装置１０が、移動機の位置を計測
する。 ステップ13：制御量計算機１２は、移動機の位置が目標
経路記憶装置１１に記憶されている目標軌道からどれだ
け離れているかを計算し、この偏差に基づき移動機のス
テアリング角を計算して、走行部１３に伝える。 ステップ14：制御量計算機１２は、目標経路記憶装置１
１に記憶されている目標速度に基づいてアクセル量を計
算し、それを走行部１３に伝える。 ステップ15：走行部１３は、与えられたステアリング量
とアクセル量に基づいて走行する。

【００５６】《実施例４》図９は、実施例１または２の
位置計測装置１０を用いて位置を計測し、その結果に基
づいて目標経路通りに移動して芝を刈る自律移動芝刈り
機の実施例の構成を示す斜視図である。作業パターン記
憶装置１６には、移動機の通るべき目標軌道と、軌道上
の各位置での目標速度と、芝を刈る区間および刈らない
区間の区別とが書き込まれている。

【００５７】位置計測装置１０により移動機の位置が与
えられると、制御量計算機１２は、移動機の位置が作業
パターン記憶装置１６に記憶されている目標軌道からど
れだけ離れているかを計算し、その偏差に基づき移動機
のステアリング角を計算し、走行部１３に伝える。制御
量計算機１２は、また、作業パターン記憶装置１６に記
憶されている目標速度に基づいてアクセル量を計算し、
走行部１３に伝える。走行部１３は、与えられたステア
リング量とアクセル量とに基づいて走行する。制御量計
算機１２は、作業パターン記憶装置１６に書き込まれて
いる芝刈り区間の始点に達すると、芝刈部１４に開始命
令を送り、芝刈区間の終点に達すると、芝刈部１４に終
了命令を送る。芝刈部１４は、開始命令または終了命令
に基づき芝刈作業をする。

【００５８】《実施例５》図１０は、実施例１または２
の位置計測装置１０を用いて位置を計測し、その結果に
基づいて目標経路通りに移動する自律移動トラクタの実
施例の構成を示す斜視図である。目標経路記憶装置１１
には、移動機の通るべき目標軌道と、軌道上の各位置で
の目標速度とが書き込まれている。

【００５９】位置計測装置１０により移動機の位置が与
えられると、制御量計算機２２は、移動機の位置が目標
経路記憶装置１１に記憶されている目標軌道からどれだ
け離れているかを計算し、その偏差に基づき移動機のス
テアリング角を計算し、走行部１３に伝える。制御量計
算機１２は、また、目標経路記憶装置１１に記憶されて
いる目標速度に基づいてアクセル量を計算して、走行部
１３に伝える。走行部１３は、与えられたステアリング
量およびアクセル量に基づいて走行する。牽引用フック
１５で特定作業用装置を牽引すると、トラクタとしての
役割を果たす。

【００６０】

【発明の効果】本発明によれば、ヨー角速度またはヨー
角を計測する光ファイバジャイロと、ピッチ姿勢を計測
するピッチ姿勢センサと、ロール姿勢を計測するロール
姿勢センサと、速度を検出する速度センサと、これらの
情報をもとに移動機の位置を計算する位置計算装置とか
らなる位置計測装置が得られ、移動機の姿勢がどのよう
に変化しても、その位置を高精度に計測できる。また、
ジャイロを３つも用いるかわりに、ジャイロ１つと姿勢
センサ２つとを組合せることにより、実用上十分な精度
の位置計測装置が低コストで得られる。

【図面の簡単な説明】

【図１】本発明の原理を説明するための座標系ｘ1−ｙ1
−ｚ1を示す図である。

【図２】本発明の原理を説明するための座標系ｘ2−ｙ2
−ｚ2を示す図である。

【図３】本発明の原理を説明するための座標系ｘ3−ｙ3
−ｚ3を示す図である。

【図４】本発明による位置計測装置の実施例１の構成を
示す斜視図である。

【図５】図４の位置推定装置の動作を説明するフローチ
ャートである。

【図６】本発明によりヨー軸回りの回転角速度と予め与
えられている起伏状況とから位置を計測する位置計測装
置の実施例２の構成を示す斜視図である。

【図７】実施例１または２の位置計測装置を用いて位置
を計測し、その結果に基づいて目標経路通りに移動する
自律移動機の実施例の構成を示す斜視図である。

【図８】図７の自律移動機の動作を示すフローチャート
である。

【図９】実施例１または２の位置計測装置を用いて位置
を計測し、その結果に基づいて目標経路通りに移動して
芝を刈る自律移動芝刈り機の実施例の構成を示す斜視図
である。

【図１０】実施例１または２の位置計測装置を用いて位
置を計測し、その結果に基づいて目標経路通りに移動す
る自律移動トラクタの実施例の構成を示す斜視図であ
る。

【符号の説明】

１ 光ファイバジャイロ ２ ピッチ姿勢センサ ３ ロール姿勢センサ ４ 速度センサ ５ 位置計算機 ６ 車輪 ７ ロールピッチ姿勢センサ ８ 姿勢計算機 ９ 起伏状況地図 １０ 位置計測装置 １１ 目標経路記憶装置 １２ 制御量計算機 １３ 走行部 １４ 芝刈り機 １５ 牽引用フック １６ 作業パターン記憶装置

───────────────────────────────────────────────────── フロントページの続き (72)発明者 藤井 健二郎 千葉県習志野市東習志野７丁目１番１号 株式会社日立製作所産業機器事業部内 (72)発明者 初本 慎太郎 千葉県習志野市東習志野７丁目１番１号 株式会社日立製作所産業機器事業部内 (72)発明者 松田 有司 千葉県習志野市東習志野７丁目１番１号 株式会社日立製作所産業機器事業部内 (72)発明者 神谷 敬之 千葉県習志野市東習志野７丁目１番１号 株式会社日立製作所産業機器事業部内 (72)発明者 乙母 正美 千葉県習志野市東習志野七丁目１番１号 日立京葉エンジニアリング株式会社内 (72)発明者 清野 憲二 千葉県習志野市東習志野七丁目１番１号 日立京葉エンジニアリング株式会社内

Claims (5)

【特許請求の範囲】
1. 【請求項１】 起伏がある地表面上の領域内を移動する
   移動機の回転角速度のヨー角成分を計測する光ファイバ
   ジャイロと、 前記移動機のロール姿勢を計測するロール姿勢センサ
   と、 前記移動機のピッチ姿勢を計測するピッチ姿勢センサ
   と、 前記移動機の速度を計測する速度センサと、 前記移動機のヨー角回転角速度とロール姿勢とピッチ姿
   勢と速度とに基づいて前記移動機の位置を計算する位置
   計算機とからなる移動機の位置計測装置。
2. 【請求項２】 起伏がある地表面上の領域内を移動する
   移動機の回転角速度のヨー角成分を計測する光ファイバ
   ジャイロと、 前記移動機の速度を計測する速度センサと、 移動対象領域の起伏状況地図と、 前記起伏状況地図および前記移動機の位置から前記移動
   機のロール姿勢およびピッチ姿勢を計算する姿勢計算機
   と、 前記移動機のヨー角回転角速度とロール姿勢とピッチ姿
   勢と速度とに基づいて前記移動機の位置を計算する位置
   計算機とからなる移動機の位置計測装置。
3. 【請求項３】 請求項１または請求項２に記載の位置計
   測装置と、 目標経路記憶装置と、 制御量計算機と、 走行部とからなる自律移動機。
4. 【請求項４】 請求項１または請求項２に記載の位置計
   測装置と、 作業パターン記憶装置と、 制御量計算機と、 走行部と、 芝刈り部とからなる自律移動芝刈り機。
5. 【請求項５】 請求項１または請求項２に記載の位置計
   測装置と、 目標経路記憶装置と、 制御量計算機と、 走行部と、 牽引用フックとからなる自律移動トラクタ。