JP4041623B2

JP4041623B2 - Position recognition method for moving objects

Info

Publication number: JP4041623B2
Application number: JP20275899A
Authority: JP
Inventors: 雄司山口
Original assignee: Yanmar Co Ltd
Current assignee: Yanmar Co Ltd
Priority date: 1999-07-16
Filing date: 1999-07-16
Publication date: 2008-01-30
Anticipated expiration: 2019-07-16
Also published as: JP2001033549A

Description

【０００１】
【発明の属する技術分野】
農作業の省力化を実現する技術の一つに、トラクタやコンバイン、田植機等の各種車両式作業機の無人化がある。また、車両式作業機の無人化を実現する課題の一つとして移動体自身の作業フィールド内における位置認識技術の確立がある。本発明は、トラクタ等の農作業機の無人化を目的とした汎用的な移動体の位置認識技術に関する発明であり、特に、通信タイムラグによって生じる、位置認識のずれを最小限に補正して高い精度を維持することを可能とした位置認識技術に関する。
【０００２】
【従来の技術】
従来より移動体の位置認識技術としては、移動体自身以外に設置された管制塔のようなもの、例えばレーザ灯台や人工衛星を利用したＧＰＳによって移動体の位置認識を可能としていた。
【０００３】
【発明が解決しようとする課題】
上記従来技術においては、例えばレーザ灯台を管制塔として位置認識を行う場合、移動体との間での通信が行われることとなる。つまり、レーザ灯台は移動体の取付けられた反射鏡等から移動体の位置を認識するが、この認識結果を通信により移動体に送信することとなる。このため、通信タイムラグ及び座標解析処理等に処理時間を要するため、移動走行中の移動体がその位置情報を取得した場合には、既に取得した位置座標からは別の座標に移動していることとなる。このことは、将来的に移動体自身がフィールド内の位置を認識して、ステアリングの自動操作を行うことを考慮すると、現在位置の認識ずれが自動走行に支障を生じさせることが考えられ、無人化を目的とした本技術においては、位置認識のさらなる精度の向上が望まれているのである。
【０００４】
【課題を解決するための手段】
以上が本発明の解決する課題であり、次に課題を解決するための手段を説明する。
請求項１においては、外部管制塔からの測定結果による移動体（１）の位置座標が（Ａ ₀ ）である場合に、少なくとも通信時間および計測演算の処理時間の遅延時間分だけ、時間補正手段（２４）により移動体自身の位置計測値にて補正して、現在の移動体（１）の位置座標（Ａ ₁ ）を予測し、外部管制塔からの測定結果により位置座標が更新された場合、順次位置座標（Ａ ₁ 、Ａ ₂ 、Ａ ₃ ・・・）を取得し、灯台計測軌跡（Ａ）を得て、また、初期状態のみ外部管制塔の計測結果による移動体（１）の位置座標（Ｂ ₁ ）を取得し、その後は移動量算出手段（２５）の算出結果を加算して、順次位置座標（Ｂ ₁ 、Ｂ ₂ 、Ｂ ₃ ・・・）を取得し、移動体計測軌跡（Ｂ）を得て、移動体（１）の位置認識の座標として、前記外部管制塔により計測した位置座標（Ａ ₁ 、Ａ ₂ 、Ａ ₃ ・・・）と、移動体自身の計測した位置座標（Ｂ ₁ 、Ｂ ₂ 、Ｂ ₃ ・・・）の、内分点（Ｃ ₁ 、Ｃ ₂ 、Ｃ ₃ ・・・）であるマッチング軌跡（Ｃ）を得る制御であり、該内分点（Ｃ ₁ 、Ｃ ₂ 、Ｃ ₃ ・・・）は前記外部管制塔により計測した位置座標（Ａ ₁ 、Ａ ₂ 、Ａ ₃ ・・・）と、移動体自身の計測した位置座標（Ｂ ₁ 、Ｂ ₂ 、Ｂ ₃ ・・・）を、０から１の間で時間経過につれて１に近付いて最終値を１とする遷移係数（λ）による『λ×Ａ＋（１−λ）×Ｂ』で算出する内分点とした移動体の位置認識方法である。
【０００５】
請求項２においては、前記外部管制塔からの位置計測値が設定回数以上連続して同じ値である場合には、該外部管制塔による位置計測値の遷移係数（λ）を０とし、内分比（λ：１−λ）を０：１として、移動体自身の位置計測値のみで、位置認識する移動体の位置認識方法である。
【０００６】
請求項３においては、システム復帰後に、外部管制塔からの位置計測値が、前回値と異なる値となった場合には、再び、外部管制塔からの位置計測値と移動体自身の位置計測値との内分値、即ちマッチング軌跡（Ｃ）により位置認識を行うよう制御した移動体の位置認識方法である。
【０００７】
【発明の実施の形態】
次に本発明の実施の形態を添付の図面を用いて説明する。図１は本発明に係る位置認識システムを搭載した移動体の側面図、図２は同じく平面図、図３は本発明に係る位置認識システムの構成図、図４は自動追尾レーザ灯台による座標解析の模式図、図５はフィールド内を走行する移動体及び自動追尾レーザ灯台を上方から見た図、図６は従来の位置認識による精度を示すグラフ、図７は本発明の位置認識システムにおける信号処理図、図８は内分点による座標解析を示す模式図、図９は本発明の位置認識による精度を示すグラフである。
【０００８】
図１及び図２は本発明に係る位置認識システム１０を塔載した車両式作業機等の移動体１であり、車両前後に前輪２・２及び後輪３・３を配し、車両後部には追尾用反射鏡１１を配設している。追尾用反射鏡１１は後述する自動追尾レーザ灯台４から照射される走査光を反射することにより、該移動体１の位置認識を可能とするものであり、本発明においては、該追尾用反射鏡１１を移動体１の後輪軸３ａ上で、後輪３・３の略中央位置に配設するようにしている。つまり、追尾用反射鏡１１を駆動輪である後輪３・３の中央位置に配設することで、正確な位置認識を可能としている。
【０００９】
図３は、本発明に係る位置認識システム１０の構成を示す。位置認識システム１０は移動体１に搭載された前記追尾用反射鏡１１、地磁気センサ１２、車輪回転センサ１３、制御部２０、及び、フィールド内の適宜位置に配設される自動追尾レーザ灯台４に搭載された測量器４ａ、制御部３０等とから構成されている。そして、地磁気センサ１２により移動体１は方位角を計測可能とし、前後左右の車輪２・２・３・３に配設された車輪回転センサ１３により移動距離を計測可能としている。
【００１０】
外部管制塔である自動追尾レーザ灯台４には、上下及び水平方向に回動自在に支持された測量器４ａを配し、該測量器４ａが追尾用反射鏡１１に対して走査光を照射し、前記追尾用反射鏡１１により反射された走査光の検出結果に基づく演算により移動体１の位置認識を行うとともに、移動体１の走行に伴い移動する追尾用反射鏡１１を自動追尾するものである。
【００１１】
ここで、自動追尾レーザ灯台４による移動体１（正確には追尾用反射鏡１１）の座標演算方法について説明する。図４の如く、自動追尾レーザ灯台４の測量器４ａ上に測定点Ｓ₀をとり、追尾用反射鏡１１を点Ｐとする。測量器４ａは追尾用反射鏡１１から反射される走査光の検出結果に基づいて、測定点Ｓ₀に対する点Ｐ（追尾用反射鏡１１）の斜距離ＳＤ、垂直角Ｖ、水平角Ｈを算出する。そして、点Ｓ₀を含むｘｙ平面への点Ｐの投影点を点Ｈ₀とすれば、点Ｓ₀から点Ｈ₀までの距離ＬはＬ＝ＳＤ×ｃｏｓ（π／２−Ｖ）となる。そして、測量器４ａの直下の地面に基準点Ｂ₀（ｘ０、ｙ０、ｚ０）をとり、測定点Ｓ₀の高さをｈとすれば、追尾用反射鏡１１の座標Ｐ（ｘ、ｙ、ｚ）は、ｘ＝Ｌ×ｃｏｓ（Ｈ）＋ｘ０、ｙ＝Ｌ×ｓｉｎ（Ｈ）＋ｙ０、ｚ＝ＳＤ×ｃｏｓ（Ｖ）＋ｚ０＋ｈで求まる。
【００１２】
そして、自動追尾レーザ灯台４の制御部３０において上記算出方法により測定された移動体１の座標位置は、図３及び図５で示すように通信によって移動体１の制御部２０に送信される。もしくは、上述した測定値（斜距離ＳＤ、垂直角Ｖ、水平角Ｈ）を通信により移動体１の制御部２０に送信し、制御部２０において上記座標演算を行う構成としてもよい。なお、本実施例で示した座標算出方法は一実施例であり、算出方法はこれに限定されるものではない。
【００１３】
これら移動体１及び自動追尾レーザ灯台４間における処理時間は、測定器４ａと追尾用反射鏡１１間における走査光の照射及び検出時間、移動体１と自動追尾レーザ灯台４間の通信の確立、及びデータ転送時間、そして座標演算処理時間の積算時間となる。このため、正確な位置認識を行うためには、連続的な座標位置の取得が望ましいが、移動体１のある時点での座標位置を算出してから、次の座標位置を検出するまでにはタイムラグが生じることとなる。
【００１４】
図６は、自動追尾レーザ灯台４による位置認識結果を表すグラフである。これは移動体１がＸ軸方向に直進走行を行った場合の試験結果であり、グラフで表すように移動体１は実際には略等速走行を行っており、そのグラフが直線で表されている。これに対し、自動追尾レーザ灯台４による位置認識結果は、処理時間のタイムラグにより階段状のグラフとなっており、検出結果が離散的であることを示している。つまり、ある時点の位置認識から、次の位置認識の検出が行われるまでの間に、実際の座標からのずれが生じるのである。そして、実際の移動体１の位置と、自動追尾レーザ灯台４の位置認識結果の誤差が大きくなっている。（例えば、本結果によれば誤差は０．２ｍから０．６ｍの間を推移し、将来的に自動走行を行うことが困難である。）
【００１５】
次に、本発明に係る位置認識システム１０による位置認識方法について説明する。図７は制御部２０における信号処理図である。まず、制御部２０は前述の如く自動追尾レーザ灯台４による測定結果から追尾用反射１１位置（座標）を取得可能としている。本実施例においては、自動追尾レーザ灯台４より通信で取得した前記測定結果より制御部２０の反射鏡位置計測手段２１によって追尾用反射鏡１１の位置座標を算出する。
【００１６】
一方、方位角計測手段２２は移動体１に配設された地磁気センサ１２の検出結果より方位角情報を出力し、移動距離計測手段２３は回転センサ１３の検出結果より移動距離情報を出力する。そして、該方位角情報及び移動距離情報をもとに、移動量演算手段２５が移動体１の移動量を算出するのである。
【００１７】
また、移動体１は移動走行を行いながら自動追尾レーザ灯台４の測定結果から移動体自身の位置座標を取得するため、前述の如く位置座標の取得にはタイムラグが生じる。このため、ある時点での位置座標を取得した場合には、既に移動体１は移動していることとなる。そこで反射鏡位置計測手段２１により出力される移動体１の位置座標を、遅れ時間補正手段２４において、前記移動量演算手段２５の算出結果をもとに補正するのである。つまり、図８で示すように、ある時点での自動追尾レーザ灯台４の測定結果による移動体１の位置座標がＡ₀である場合には、遅れ時間補正手段２４により補正を行い、現在の移動体１の位置座標をＡ₁と予測するのである。
【００１８】
そして、図７で示すようにスイッチング２７は自動追尾レーザ灯台４からの測定結果により位置座標が更新された場合（以下、灯台更新時とする）、処理回路２７ｂによって遅れ時間補正された位置座標Ａ₁を取得し、次の灯台更新時までの間は、移動量算出手段２５の算出結果を加算してループ回路２７ａより順次位置座標Ａ₁、Ａ₂、Ａ₃・・・を取得し、灯台計測軌跡Ａ (図８に示す）を得るのである。
【００１９】
また、スイッチング２８は初期状態のみ処理回路２８ｃより自動追尾レーザ灯台４の計測結果による移動体１の位置座標Ｂ₁を取得し、その後は移動量算出手段２５の算出結果を加算してループ回路２８ａにより順次位置座標Ｂ₁、Ｂ₂、Ｂ₃・・・を取得し、移動体計測軌跡Ｂを得るのである。なお、計測を開始する初期状態においては、移動体１は停止しているので位置座標Ａ₀（Ａ₁）と位置座標Ｂ₁とは同一座標となる。
【００２０】
そして、図８に示すように、本発明における移動体１の位置座標は、位置座標Ａ₁、Ａ₂、Ａ₃・・・と位置座標Ｂ₁、Ｂ₂、Ｂ₃・・・の内分点Ｃ₁、Ｃ₂、Ｃ₃・・・としている。内分点Ｃ_kは位置座標Ｂ_k、Ａ_kをλ：１−λに内分する点であり（λを遷移係数とする）、図７で示すように、スイッチング２７・２８より出力されるそれぞれの位置座標には移動量演算手段２５により移動量の加算が行われた後、自動追尾レーザ灯台４による位置計測値には遷移係数である内分比λを乗算し、移動体自身の位置計測値には内分比１−λを乗算することにより、内分点を演算してマッチング座標として出力し、マッチング軌跡Ｃを得るのである。
【００２１】
遷移係数λは時間ｔを用いて、λ＝ｍｉｎ（λｔ・ｔ、１）より表され（つまり、λｔ・ｔ＜１においてλ＝λｔ・ｔ、λｔ・ｔ≧１においてλ＝１とする）、λｔは遷移パラメータであり、内分点Ｃk の灯台計測軌跡Ａへの近づき度合いを示す。つまり、λｔが大きければ内分点Ｃ_kは灯台計測軌跡Ａに早く近づく軌跡を描き、λｔが小さい場合にはその逆である。そして、ある時間ｔ_mが経過してλｔ・ｔ_m≧１となった後は、次の灯台更新時までの間は内分点Ｃ_k（ｋ≧ｍ）は灯台計測軌跡Ａと一致することとなる。
【００２２】
このように、本発明の位置認識システムは自動追尾レーザ灯台４の計測結果による灯台計測軌跡Ａと、移動体自身による計測結果である移動体計測軌跡Ｂとをマッチングさせている。つまり、外部管制塔である自動追尾レーザ灯台４からの位置計測値である灯台計測軌跡Ａと、移動体自身の位置計測値である移動体計測軌跡Ｂとの内分値、即ちマッチング軌跡Ｃによって位置認識を行うよう制御しているので、自動追尾レーザ灯台４との通信等によるタイムラグによる位置認識誤差を補正して、より正確な位置認識を可能としているのである。図９は、本発明による位置認識の試験結果を表す。試験は同様に移動体１がＸ軸方向に直進した場合のものであり、本発明による灯台計測軌跡Ａと移動体計測軌跡Ｂのマッチングによる位置認識結果が実際のグラフに略近い曲線を描いていることがわかる。そして、本結果によれば実際値との誤差が数ｃｍ以内に納まり、将来的に自動走行を行うことが可能となった。
【００２３】
また、前述の如く時間ｔ_m経過後、灯台計測軌跡Ａ上を位置座標として、制御部２０のスイッチング２７は順次ループ回路２７ａにより移動量を加算して移動体１の位置認識を行う。そして、次の灯台更新時には、再び前述した処理を行い灯台計測軌跡Ａと移動体計測軌跡Ｂのマッチングを行うこととなるが、図７で示すように灯台更新時においては、移動体計測軌跡Ｂの初期座標Ｂ₁は処理回路２８ｂよりマッチング座標を用いることとしている。つまり、図８で示すように、灯台計測軌跡Ａ上において灯台更新を行う前の最後の座標位置（矢視Ａ_e）を次処理の初期座標Ｂ₁としているのである。このような処理を行うことにより、移動体１の位置認識による軌跡がスムーズな軌跡を描くようになり、実際の軌跡により近い連続的な変化を移動体の位置座標とすることができるのである。
【００２４】
次に、自動追尾レーザ灯台４及びその周辺システムに異常が生じた場合の制御方法について説明する。自動追尾レーザ灯台４の測量器４ａに何らかの障害が生じた場合、また、移動体１と自動追尾レーザ灯台４間における通信に障害が生じた場合などにおいては、移動体１は自動追尾レーザ灯台４による位置認識が行えなくなる。そこで、本発明においては、自動追尾レーザ灯台４からの位置計測値が設定された回数以上連続して同じ値である場合、つまり自動追尾レーザ灯台４の計測値による位置情報が更新されない場合には、前記自動追尾レーザ灯台４による位置計測値の内分比（遷移係数）を０とし、内分比（λ：１−λ）を０：１とするのである。このような制御を行うことにより、内分点Ｃ_k、つまりマッチング座標は図８において移動体計測軌跡Ｂと一致することとなり、灯台更新が行われない場合においても、位置認識のずれを最小限に押さえるよう制御しているのである。
【００２５】
そして、制御部２０は自動追尾レーザ灯台４周辺のシステムに異常が生じた場合にも、灯台更新の検出処理は続行して行う。そして、再び自動追尾レーザ灯台４からの計測値による位置情報が更新された場合には、前述した内分比（λ：１−λ）をもとに、灯台計測軌跡Ａと移動体計測軌跡Ｂのマッチングを行い内分点Ｃ_kを移動体１の位置座標として認識するのである。つまり、外部管制塔である自動追尾レーザ灯台４からの位置計測値が、前回値と異なる値となった場合には、再び、内分によって位置認識を行うよう制御しているのである。このような制御を行うことにより、灯台更新のシステム異常中には、自動追尾レーザ灯台４の位置計測値の内分比を０として、移動***置座標を移動体計測軌跡Ｂと一致させることにより位置認識のずれを最小限に押さえるとともに、システム復帰後は即座に内分点によるマッチング処理を行い位置認識の高い精度を確保し、システムの信頼性の向上を図っているのである。
【００２６】
【発明の効果】
本発明の移動体の位置認識方法は、以上の如く制御したので以下のような効果を奏するものである。
請求項１に記載の如く、外部管制塔からの測定結果による移動体（１）の位置座標が（Ａ ₀ ）である場合に、少なくとも通信時間および計測演算の処理時間の遅延時間分だけ、時間補正手段（２４）により移動体自身の位置計測値にて補正して、現在の移動体（１）の位置座標（Ａ ₁ ）を予測し、外部管制塔からの測定結果により位置座標が更新された場合、順次位置座標（Ａ ₁ 、Ａ ₂ 、Ａ ₃ ・・・）を取得し、灯台計測軌跡（Ａ）を得て、また、初期状態のみ外部管制塔の計測結果による移動体（１）の位置座標（Ｂ ₁ ）を取得し、その後は移動量算出手段（２５）の算出結果を加算して、順次位置座標（Ｂ ₁ 、Ｂ ₂ 、Ｂ ₃ ・・・）を取得し、移動体計測軌跡（Ｂ）を得て、移動体（１）の位置認識の座標として、前記外部管制塔により計測した位置座標（Ａ ₁ 、Ａ ₂ 、Ａ ₃ ・・・）と、移動体自身の計測した位置座標（Ｂ ₁ 、Ｂ ₂ 、Ｂ ₃ ・・・）の、内分点（Ｃ ₁ 、Ｃ ₂ 、Ｃ ₃ ・・・）であるマッチング軌跡（Ｃ）を得る制御であり、該内分点（Ｃ ₁ 、Ｃ ₂ 、Ｃ ₃ ・・・）は前記外部管制塔により計測した位置座標（Ａ ₁ 、Ａ ₂ 、Ａ ₃ ・・・）と、移動体自身の計測した位置座標（Ｂ ₁ 、Ｂ ₂ 、Ｂ ₃ ・・・）を、０から１の間で時間経過につれて１に近付いて最終値を１とする遷移係数（λ）による『λ×Ａ＋（１−λ）×Ｂ』で算出する内分点としたので、外部管制塔からの位置計測値が更新されるまでのタイムラグによっても位置認識のずれを生じさせず、位置認識の精度が向上した。
また、位置認識情報を連続的な変化として取得することが可能となり、移動体の実際の軌跡に近い位置情報を取得可能となった。
【００２７】
請求項２に記載の如く、前記外部管制塔からの位置計測値が設定回数以上連続して同じ値である場合には、該外部管制塔による位置計測値の遷移係数（λ）を０とし、内分比（λ：１−λ）を０：１として、移動体自身の位置計測値のみで、位置認識するので、外部管制塔及びその周辺システムに障害が発生した場合においても、位置認識情報に大幅なずれを生じさせることなく、より正確な位置情報を取得可能となった。
さらに、たとえ移動体が一時停止している場合でも、移動体自身の位置計測値も変化しないので、外部管制塔からの位置計測値の内分比を０としても正確に位置情報を取得できるのである。
【００２８】
請求項３に記載の如く、システム復帰後に、外部管制塔からの位置計測値が、前回値と異なる値となった場合には、再び、外部管制塔からの位置計測値と移動体自身の位置計測値との内分値、即ちマッチング軌跡（Ｃ）により位置認識を行うよう制御したので、外部管制塔及びその周辺システムの障害復旧時には、即座に内分による正確な位置情報を取得可能となり、位置認識システムとして信頼性が向上した。
【図面の簡単な説明】
【図１】本発明に係る位置認識システムを搭載した移動体の側面図である。
【図２】同じく平面図である。
【図３】本発明に係る位置認識システムの構成図である。
【図４】自動追尾レーザ灯台による座標解析の模式図である。
【図５】フィールド内を走行する移動体及び自動追尾レーザ灯台を上方から見た図である。
【図６】従来の位置認識による精度を示すグラフである。
【図７】本発明の位置認識システムにおける信号処理図である。
【図８】内分点による座標解析を示す模式図である。
【図９】本発明の位置認識による精度を示すグラフである。
【符号の説明】
１移動体
４自動追尾レーザ灯台
４ａ測量器
１０位置認識システム
１１追尾用反射鏡
１２地磁気センサ
１３回転センサ
２０制御部
３０制御部[0001]
BACKGROUND OF THE INVENTION
One of the technologies that realizes labor saving in farm work is the unmanned operation of various vehicle working machines such as tractors, combines, and rice transplanters. In addition, as one of the problems for realizing unmanned vehicle work machines, there is establishment of position recognition technology in the work field of the moving body itself. The present invention relates to a general-purpose moving body position recognition technique for the purpose of unmanning agricultural machines such as tractors, and in particular, highly accurate by correcting a position recognition shift caused by a communication time lag to a minimum. The present invention relates to a position recognition technology that can maintain the above.
[0002]
[Prior art]
Conventionally, as a position recognition technique of a moving body, the position of the moving body can be recognized by a control tower installed other than the moving body itself, for example, a GPS using a laser lighthouse or an artificial satellite.
[0003]
[Problems to be solved by the invention]
In the prior art, for example, when position recognition is performed using a laser lighthouse as a control tower, communication with a moving body is performed. That is, the laser lighthouse recognizes the position of the moving body from a reflecting mirror or the like to which the moving body is attached, and transmits the recognition result to the moving body by communication. For this reason, since processing time is required for the communication time lag and coordinate analysis processing, etc., when a moving object that is traveling is acquiring its position information, it must be moved from the already acquired position coordinates to another coordinate. It becomes. In consideration of the fact that the mobile body itself will recognize the position in the field and automatically operate the steering in the future, it is considered that the misrecognition of the current position may hinder automatic driving. In the present technology for the purpose of achieving the above, further improvement in the accuracy of position recognition is desired.
[0004]
[Means for Solving the Problems]
The above is the problem to be solved by the present invention. Next, means for solving the problem will be described.
In claim 1, when the position coordinate of the moving body (1) based on the measurement result from the external control tower is (A ₀ ), the time correction means is at least the delay time of the communication time and the processing time of the measurement calculation. When the position coordinate (A ₁ ) of the current moving body ( ₁ ) is predicted by correcting with the position measurement value of the moving body itself according to (24), and the position coordinates are updated by the measurement result from the external control tower The position coordinates (A ₁ , A ₂ , A ₃ ...) Are obtained sequentially , the lighthouse measurement trajectory (A) is obtained, and the position of the moving body (1) based on the measurement result of the external control tower only in the initial state. The coordinates (B ₁ ) are acquired, and then the calculation results of the movement amount calculation means (25) are added to sequentially acquire the position coordinates (B ₁ , B ₂ , B ₃ ...) (B) is obtained and measured by the external control tower as the position recognition coordinates of the moving body (1). The internal dividing point (C ₁ , C ₂ ) between the position coordinates (A ₁ , A ₂ , A ₃ ...) And the position coordinates (B ₁ , B ₂ , B ₃ ...) Measured by the moving body itself. , C ₃ ...) To obtain a matching locus (C), and the internal dividing points (C ₁ , C ₂ , C ₃ ...) Are position coordinates (A ₁ ) measured by the external control tower. , a _2, a ₃ and..), the measured position coordinates of the moving body itself _{_{(B 1, B 2, B}} 3 ···) to the final value closer to 1 as the time between 0 and 1 This is a method for recognizing the position of a moving object as an internal dividing point calculated by “λ × A + (1−λ) × B” using a transition coefficient (λ) where 1 is 1.
[0005]
In claim 2, when the position measurement value from the external control tower is the same value continuously for a set number of times or more, the transition coefficient (λ) of the position measurement value by the external control tower is set to 0, and the internal distribution This is a method for recognizing the position of a moving body that recognizes the position only with the position measurement value of the moving body itself, where the ratio (λ: 1−λ) is 0: 1.
[0006]
In claim 3, when the position measurement value from the external control tower becomes a value different from the previous value after the system is restored, the position measurement value from the external control tower and the position measurement value of the moving body itself again. Is a position recognition method for a moving body that is controlled to perform position recognition based on the internal division value, i.e., the matching trajectory (C).
[0007]
DETAILED DESCRIPTION OF THE INVENTION
Next, embodiments of the present invention will be described with reference to the accompanying drawings. 1 is a side view of a mobile body equipped with a position recognition system according to the present invention, FIG. 2 is a plan view of the same, FIG. 3 is a block diagram of the position recognition system according to the present invention, and FIG. 4 is a coordinate analysis by an automatic tracking laser lighthouse. FIG. 5 is a view of a moving body and an automatic tracking laser lighthouse traveling in the field as seen from above, FIG. 6 is a graph showing the accuracy of conventional position recognition, and FIG. 7 is a signal in the position recognition system of the present invention. FIG. 8 is a schematic diagram showing coordinate analysis by internal dividing points, and FIG. 9 is a graph showing accuracy by position recognition of the present invention.
[0008]
1 and 2 show a moving body 1 such as a vehicle-type work machine equipped with a position recognition system 10 according to the present invention. Front wheels 2 and 2 and rear wheels 3 and 3 are arranged on the front and rear sides of the vehicle, and are arranged at the rear of the vehicle. Is provided with a tracking reflecting mirror 11. The tracking reflector 11 enables the position of the moving body 1 to be recognized by reflecting scanning light emitted from an automatic tracking laser lighthouse 4 described later. In the present invention, the tracking reflector 11 11 is arranged on the rear wheel shaft 3a of the moving body 1 at a substantially central position of the rear wheels 3. That is, the tracking reflector 11 is disposed at the center position of the rear wheels 3 and 3 as drive wheels, thereby enabling accurate position recognition.
[0009]
FIG. 3 shows a configuration of the position recognition system 10 according to the present invention. The position recognition system 10 includes the tracking reflector 11 mounted on the moving body 1, the geomagnetic sensor 12, the wheel rotation sensor 13, the control unit 20, and the automatic tracking laser lighthouse 4 disposed at an appropriate position in the field. It is comprised from the mounted surveying instrument 4a, the control part 30, etc. The geomagnetic sensor 12 allows the moving body 1 to measure the azimuth angle, and allows the moving distance to be measured by the wheel rotation sensors 13 disposed on the front, rear, left, and right wheels 2, 2, 3, and 3.
[0010]
The automatic tracking laser lighthouse 4 which is an external control tower is provided with a surveying instrument 4a supported so as to be rotatable in the vertical and horizontal directions, and the surveying instrument 4a irradiates the tracking reflecting mirror 11 with scanning light. The position of the moving body 1 is recognized by calculation based on the detection result of the scanning light reflected by the tracking reflecting mirror 11, and the tracking reflecting mirror 11 that moves as the moving body 1 travels is automatically tracked. is there.
[0011]
Here, a coordinate calculation method of the moving body 1 (more precisely, the tracking reflecting mirror 11) by the automatic tracking laser lighthouse 4 will be described. As shown in FIG. 4, a measurement point S ₀ is taken on the surveying instrument 4 a of the automatic tracking laser lighthouse 4, and the tracking reflector 11 is set as a point P. The surveying instrument 4a calculates the oblique distance SD, the vertical angle V, and the horizontal angle H of the point P (tracking reflecting mirror 11) with respect to the measuring point S ₀ based on the detection result of the scanning light reflected from the tracking reflecting mirror 11. To do. Then, when the projected point P points to the xy plane including the point S ₀ and the point H _0, the distance L from the point S ₀ to the point H ₀ becomes L = SD × cos (π / 2-V) . Then, if the reference point B ₀ (x0, y0, z0) is taken on the ground directly below the surveying instrument 4a and the height of the measurement point S ₀ is h, the coordinates P (x, y, z) is obtained by x = L × cos (H) + x0, y = L × sin (H) + y0, and z = SD × cos (V) + z0 + h.
[0012]
And the coordinate position of the moving body 1 measured by the said calculation method in the control part 30 of the automatic tracking laser lighthouse 4 is transmitted to the control part 20 of the moving body 1 by communication as shown in FIG.3 and FIG.5. Alternatively, the above-described measurement values (slope distance SD, vertical angle V, horizontal angle H) may be transmitted to the control unit 20 of the mobile body 1 by communication, and the control unit 20 may perform the coordinate calculation. Note that the coordinate calculation method shown in the present embodiment is one embodiment, and the calculation method is not limited to this.
[0013]
The processing time between the mobile body 1 and the automatic tracking laser lighthouse 4 includes the irradiation and detection time of the scanning light between the measuring instrument 4a and the tracking reflector 11, establishment of communication between the mobile body 1 and the automatic tracking laser lighthouse 4, And the data transfer time and the coordinate calculation processing time. For this reason, in order to perform accurate position recognition, it is desirable to obtain a continuous coordinate position. However, after calculating the coordinate position at a certain point in time of the moving body 1, until the next coordinate position is detected. A time lag will occur.
[0014]
FIG. 6 is a graph showing the result of position recognition by the automatic tracking laser lighthouse 4. This is a test result when the mobile body 1 travels straight in the X-axis direction. As shown in the graph, the mobile body 1 actually travels at a substantially constant speed, and the graph is represented by a straight line. ing. On the other hand, the position recognition result by the automatic tracking laser lighthouse 4 is a stepped graph due to the time lag of the processing time, indicating that the detection result is discrete. That is, there is a deviation from the actual coordinates between the position recognition at a certain time and the detection of the next position recognition. And the error of the position recognition result of the actual position of the moving body 1 and the position of the automatic tracking laser lighthouse 4 is large. (For example, according to this result, the error changes between 0.2 m and 0.6 m, and it is difficult to perform automatic traveling in the future.)
[0015]
Next, a position recognition method by the position recognition system 10 according to the present invention will be described. FIG. 7 is a signal processing diagram in the control unit 20. First, the control unit 20 can acquire the tracking reflection 11 position (coordinates) from the measurement result of the automatic tracking laser lighthouse 4 as described above. In the present embodiment, the position coordinate of the tracking reflector 11 is calculated by the reflector position measuring means 21 of the control unit 20 from the measurement result obtained by communication from the automatic tracking laser lighthouse 4.
[0016]
On the other hand, the azimuth angle measuring means 22 outputs azimuth angle information from the detection result of the geomagnetic sensor 12 disposed on the moving body 1, and the movement distance measuring means 23 outputs movement distance information from the detection result of the rotation sensor 13. Then, based on the azimuth angle information and the movement distance information, the movement amount calculation means 25 calculates the movement amount of the moving body 1.
[0017]
Further, since the moving body 1 acquires the position coordinates of the moving body itself from the measurement result of the automatic tracking laser lighthouse 4 while moving, there is a time lag in acquiring the position coordinates as described above. For this reason, when the position coordinates at a certain time point are acquired, the moving body 1 has already moved. Therefore, the position coordinates of the moving body 1 output by the reflecting mirror position measuring means 21 are corrected by the delay time correcting means 24 based on the calculation result of the moving amount calculating means 25. That is, as shown in FIG. 8, when the position coordinate of the moving body 1 based on the measurement result of the automatic tracking laser lighthouse 4 at a certain time point is A ₀ , the delay time correction means 24 corrects the current movement. The position coordinate of the body 1 is predicted as A ₁ .
[0018]
As shown in FIG. 7, when the position coordinate is updated by the measurement result from the automatic tracking laser lighthouse 4 (hereinafter referred to as the lighthouse update time), the switching 27 is corrected by the processing circuit 27b. ₁ until the next lighthouse update, the calculation result of the movement amount calculation means 25 is added and the position coordinates A ₁ , A ₂ , A ₃ ... Are obtained sequentially from the loop circuit 27a. The measurement trajectory A (shown in FIG. 8) is obtained.
[0019]
Further, the switching 28 obtains the position coordinate B ₁ of the moving body 1 based on the measurement result of the automatic tracking laser lighthouse 4 from the processing circuit 28c only in the initial state, and thereafter adds the calculation result of the movement amount calculating means 25 to add the loop circuit 28a. Sequentially acquire the position coordinates B ₁ , B ₂ , B ₃ ... To obtain the moving body measurement trajectory B. In the initial state where measurement is started, since the moving body 1 is stopped, the position coordinates A ₀ (A ₁ ) and the position coordinates B ₁ are the same coordinates.
[0020]
As shown in FIG. 8, the position coordinates of the moving body 1 according to the present invention are the internal coordinates of position coordinates A ₁ , A ₂ , A ₃ ... And position coordinates B ₁ , B ₂ , B _3. Points C ₁ , C ₂ , C ₃ ... The internal dividing point C _k is a point that internally divides the position coordinates B _k and A _k into λ: 1−λ (λ is a transition coefficient), and is output from the switching 27 and 28 as shown in FIG. After the movement amount is added to the respective position coordinates by the movement amount calculation means 25, the position measurement value by the automatic tracking laser lighthouse 4 is multiplied by the internal ratio λ which is a transition coefficient, and the position of the moving body itself is obtained. By multiplying the measured value by the internal division ratio 1-λ, the internal division point is calculated and output as matching coordinates, and a matching locus C is obtained.
[0021]
The transition coefficient λ is expressed by λ = min (λt · t, 1) using time t (that is, λ = λt · t when λt · t <1, and λ = 1 when λt · t ≧ 1). , Λt are transition parameters and indicate the degree of approach of the inner dividing point Ck to the lighthouse measurement trajectory A. That is, if λt is large, the internal dividing point C _k draws a locus that quickly approaches the lighthouse measurement locus A, and vice versa when λt is small. After a certain time t _m elapses and λt · t _m ≧ 1, the internal dividing point C _k (k ≧ m) must match the lighthouse measurement trajectory A until the next lighthouse update. It becomes.
[0022]
As described above, the position recognition system of the present invention matches the lighthouse measurement trajectory A based on the measurement result of the automatic tracking laser lighthouse 4 with the moving body measurement trajectory B which is the measurement result of the mobile body itself. That is, the internal value of the lighthouse measurement trajectory A which is a position measurement value from the automatic tracking laser lighthouse 4 which is an external control tower and the mobile object measurement trajectory B which is the position measurement value of the mobile object itself, that is, the matching trajectory C Since control is performed so as to perform position recognition, a position recognition error due to a time lag due to communication with the automatic tracking laser lighthouse 4 or the like is corrected, thereby enabling more accurate position recognition. FIG. 9 shows a test result of position recognition according to the present invention. Similarly, the test is performed when the moving body 1 goes straight in the X-axis direction, and the position recognition result by matching the lighthouse measurement trajectory A and the mobile object measurement trajectory B according to the present invention draws a curve that is substantially similar to an actual graph. I understand that. According to this result, the error from the actual value is within a few centimeters, and it is possible to perform automatic traveling in the future.
[0023]
Further, as described above, after the time t _{m has} elapsed, the switching 27 of the control unit 20 recognizes the position of the moving body 1 by sequentially adding the amount of movement by the loop circuit 27a with the position on the lighthouse measurement locus A as the position coordinates. Then, at the next lighthouse update, the above-described processing is performed again to match the lighthouse measurement trajectory A and the mobile object measurement trajectory B. As shown in FIG. For the initial coordinate B _{1, a} matching coordinate is used by the processing circuit 28b. That is, as shown in Figure 8, is the last coordinate position before performing the lighthouse updated on lighthouse measuring path A (arrow A _e) are the initial coordinates B ₁ of the following process. By performing such processing, the trajectory based on the position recognition of the moving body 1 draws a smooth trajectory, and continuous changes closer to the actual trajectory can be used as the position coordinates of the moving body.
[0024]
Next, a control method when an abnormality occurs in the automatic tracking laser lighthouse 4 and its peripheral system will be described. When any trouble occurs in the surveying instrument 4a of the automatic tracking laser lighthouse 4, or when communication between the mobile body 1 and the automatic tracking laser lighthouse 4 fails, the mobile body 1 is set to the automatic tracking laser lighthouse 4 Position recognition by cannot be performed. Therefore, in the present invention, when the position measurement value from the automatic tracking laser lighthouse 4 is the same value continuously for the set number of times, that is, when the position information based on the measurement value of the automatic tracking laser lighthouse 4 is not updated. The internal ratio (transition coefficient) of the position measurement values obtained by the automatic tracking laser lighthouse 4 is set to 0, and the internal ratio (λ: 1−λ) is set to 0: 1. By performing such control, the internal dividing point C _k , that is, the matching coordinates coincide with the moving object measurement trajectory B in FIG. 8, and even when the lighthouse is not updated, the position recognition shift is minimized. It is controlled to hold down.
[0025]
The control unit 20 continues the lighthouse update detection process even when an abnormality occurs in the system around the automatic tracking laser lighthouse 4. When the position information based on the measurement values from the automatic tracking laser lighthouse 4 is updated again, the lighthouse measurement trajectory A and the moving object measurement trajectory B are based on the internal ratio (λ: 1−λ) described above. Thus, the internal dividing point C _k is recognized as the position coordinate of the moving body 1. That is, when the position measurement value from the automatic tracking laser lighthouse 4 which is an external control tower becomes a value different from the previous value, the position is recognized again by the internal division. By performing such control, during the lighthouse update system abnormality, the internal ratio of the position measurement value of the automatic tracking laser lighthouse 4 is set to 0, and the moving body position coordinates are made to coincide with the moving body measurement trajectory B. In addition to minimizing misalignment of position recognition, matching processing using internal dividing points is performed immediately after the system is restored to ensure high accuracy of position recognition and improve system reliability.
[0026]
【The invention's effect】
Since the moving body position recognition method of the present invention is controlled as described above, the following effects can be obtained.
As described in claim 1, when the position coordinate of the moving body (1) according to the measurement result from the external control tower is (A ₀ ), the time is at least the delay time of the communication time and the processing time of the measurement calculation. The correction means (24) corrects the position measurement value of the moving body itself, predicts the current position coordinates (A ₁ ) of the moving body (1), and the position coordinates are updated by the measurement result from the external control tower. In this case, the position coordinates (A ₁ , A ₂ , A ₃ ...) Are obtained sequentially , the lighthouse measurement trajectory (A) is obtained, and the moving body (1) based on the measurement result of the external control tower only in the initial state. The position coordinates (B ₁ ) are acquired, and then the calculation results of the movement amount calculation means (25) are added to sequentially acquire the position coordinates (B ₁ , B ₂ , B ₃ ...) to give a measurement path (B), and a position recognition of the coordinates of the moving object (1), measured by the external control tower Between the position coordinates (A ₁ , A ₂ , A ₃ ...) And the position coordinates (B ₁ , B ₂ , B ₃ ...) Measured by the moving body itself (C ₁ , C _2). , C ₃ ...) To obtain a matching locus (C), and the internal dividing points (C ₁ , C ₂ , C ₃ ...) Are position coordinates (A ₁ ) measured by the external control tower. , a _2, a ₃ and..), the measured position coordinates of the moving body itself _{_{(B 1, B 2, B}} 3 ···) to the final value closer to 1 as the time between 0 and 1 Since the internal dividing point calculated by “λ × A + (1−λ) × B” with a transition coefficient (λ) of 1 is used, the position is also determined by the time lag until the position measurement value from the external control tower is updated. The accuracy of position recognition is improved without causing recognition shift.
In addition, position recognition information can be acquired as a continuous change, and position information close to the actual trajectory of the moving object can be acquired.
[0027]
When the position measurement value from the external control tower is the same value continuously over the set number of times as described in claim 2, the transition coefficient (λ) of the position measurement value by the external control tower is set to 0, Since the internal ratio (λ: 1-λ) is set to 0: 1, the position is recognized only by the position measurement value of the moving body itself . Therefore, even if a failure occurs in the external control tower and its peripheral system, the position recognition information This makes it possible to acquire more accurate position information without causing a significant shift.
Furthermore, even if the moving body is temporarily stopped, the position measurement value of the moving body itself does not change, and therefore the position information can be obtained accurately even if the internal ratio of the position measurement value from the external control tower is zero. is there.
[0028]
If the position measurement value from the external control tower is different from the previous value after the system is restored as described in claim 3, the position measurement value from the external control tower and the position of the moving body itself are again displayed. Since control was performed so as to recognize the position based on the internal value of the measured value, that is, the matching trajectory (C), accurate position information based on the internal content can be acquired immediately upon failure recovery of the external control tower and its surrounding system. Reliability improved as a position recognition system.
[Brief description of the drawings]
FIG. 1 is a side view of a mobile body equipped with a position recognition system according to the present invention.
FIG. 2 is also a plan view.
FIG. 3 is a configuration diagram of a position recognition system according to the present invention.
FIG. 4 is a schematic diagram of coordinate analysis by an automatic tracking laser lighthouse.
FIG. 5 is a view of a moving body and an automatic tracking laser lighthouse traveling in a field as viewed from above.
FIG. 6 is a graph showing accuracy by conventional position recognition.
FIG. 7 is a signal processing diagram in the position recognition system of the present invention.
FIG. 8 is a schematic diagram showing coordinate analysis by internal dividing points.
FIG. 9 is a graph showing accuracy by position recognition of the present invention.
[Explanation of symbols]
DESCRIPTION OF SYMBOLS 1 Mobile body 4 Automatic tracking laser lighthouse 4a Surveyor 10 Position recognition system 11 Reflecting mirror 12 Tracking magnetic sensor 13 Rotation sensor 20 Control part 30 Control part

Claims

外部管制塔からの測定結果による移動体（１）の位置座標が（Ａ ₀ ）である場合に、少なくとも通信時間および計測演算の処理時間の遅延時間分だけ、時間補正手段（２４）により移動体自身の位置計測値にて補正して、現在の移動体（１）の位置座標（Ａ ₁ ）を予測し、外部管制塔からの測定結果により位置座標が更新された場合、順次位置座標（Ａ ₁ 、Ａ ₂ 、Ａ ₃ ・・・）を取得し、灯台計測軌跡（Ａ）を得て、また、初期状態のみ外部管制塔の計測結果による移動体（１）の位置座標（Ｂ ₁ ）を取得し、その後は移動量算出手段（２５）の算出結果を加算して、順次位置座標（Ｂ ₁ 、Ｂ ₂ 、Ｂ ₃ ・・・）を取得し、移動体計測軌跡（Ｂ）を得て、移動体（１）の位置認識の座標として、前記外部管制塔により計測した位置座標（Ａ ₁ 、Ａ ₂ 、Ａ ₃ ・・・）と、移動体自身の計測した位置座標（Ｂ ₁ 、Ｂ ₂ 、Ｂ ₃ ・・・）の、内分点（Ｃ ₁ 、Ｃ ₂ 、Ｃ ₃ ・・・）であるマッチング軌跡（Ｃ）を得る制御であり、該内分点（Ｃ ₁ 、Ｃ ₂ 、Ｃ ₃ ・・・）は前記外部管制塔により計測した位置座標（Ａ ₁ 、Ａ ₂ 、Ａ ₃ ・・・）と、移動体自身の計測した位置座標（Ｂ ₁ 、Ｂ ₂ 、Ｂ ₃ ・・・）を、０から１の間で時間経過につれて１に近付いて最終値を１とする遷移係数（λ）による『λ×Ａ＋（１−λ）×Ｂ』で算出する内分点としたことを特徴とする移動体の位置認識方法。 When the position coordinate of the mobile body (1) based on the measurement result from the external control tower is (A ₀ ), the mobile body is corrected by the time correction means (24) by at least the delay time of the communication time and the processing time of the measurement calculation. When the position coordinate (A ₁ ) of the current moving body (1 ) is predicted by correcting with its own position measurement value, and the position coordinate is updated by the measurement result from the external control tower, the position coordinate (A ₁ , A ₂ , A ₃ ...), Lighthouse measurement trajectory (A) is obtained, and the position coordinate (B ₁ ) of the moving body (1) based on the measurement result of the external control tower is obtained only in the initial state After that, the calculation result of the movement amount calculation means (25) is added to sequentially acquire the position coordinates (B ₁ , B ₂ , B ₃ ...) To obtain the moving body measurement trajectory (B). The position coordinates (A ₁ , A) measured by the external control tower are used as the position recognition coordinates of the moving body (1). ₂ , A ₃ ...) And internal dividing points (C ₁ , C ₂ , C ₃ ...) Of the position coordinates (B ₁ , B ₂ , B ₃ ...) Measured by the moving body itself. This is control for obtaining a matching locus (C), and the internal dividing points (C ₁ , C ₂ , C ₃ ...) Are position coordinates (A ₁ , A ₂ , A ₃ ... ) Measured by the external control tower. )), And the transition coordinates (λ) in which the position coordinates (B ₁ , B ₂ , B ₃ ...) Measured by the moving body itself approach 1 with the passage of time between 0 and 1, and the final value is 1. ) Is used as an internal dividing point calculated by “λ × A + (1−λ) × B” .

前記外部管制塔からの位置計測値が設定回数以上連続して同じ値である場合には、該外部管制塔による位置計測値の遷移係数（λ）を０とし、内分比（λ：１−λ）を０：１として、移動体自身の位置計測値のみで、位置認識することを特徴とする請求項１記載の移動体の位置認識方法。 When the position measurement value from the external control tower is the same value continuously for the set number of times, the transition coefficient (λ) of the position measurement value by the external control tower is set to 0, and the internal division ratio (λ: 1− 2. The position recognition method for a moving body according to claim 1 , wherein the position is recognized only with the position measurement value of the moving body itself, where [lambda] is 0: 1 .

システム復帰後に、外部管制塔からの位置計測値が、前回値と異なる値となった場合には、再び、外部管制塔からの位置計測値と移動体自身の位置計測値との内分値、即ちマッチング軌跡（Ｃ）により位置認識を行うよう制御したことを特徴とする請求項２記載の移動体の位置認識方法。 If the position measurement value from the external control tower is different from the previous value after the system is restored, again the internal value of the position measurement value from the external control tower and the position measurement value of the moving body itself, That is, the moving body position recognition method according to claim 2, wherein the position recognition is performed based on the matching locus (C) .