JPS62273408A - 車両位置及び姿勢角の計測装置 - Google Patents
車両位置及び姿勢角の計測装置Info
- Publication number
- JPS62273408A JPS62273408A JP11685786A JP11685786A JPS62273408A JP S62273408 A JPS62273408 A JP S62273408A JP 11685786 A JP11685786 A JP 11685786A JP 11685786 A JP11685786 A JP 11685786A JP S62273408 A JPS62273408 A JP S62273408A
- Authority
- JP
- Japan
- Prior art keywords
- vehicle
- light receiving
- light
- angle
- inclination angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Navigation (AREA)
Abstract
(57)【要約】本公報は電子出願前の出願データであるた
め要約のデータは記録されません。
め要約のデータは記録されません。
Description
【発明の詳細な説明】
３、発明の詳細な説明
〔産業上の利用分野〕
本発ＢＡは車両位置及び姿勢角の計測装置に係り、特に
建設現場において稼動する作条車両への運転支援及び作
業の自動化に際して必要となる車両の現在位置及び現場
の地形等の情報を得る次めの車両位置及び姿勢角の計６
：ＩＩ装置に関する。
建設現場において稼動する作条車両への運転支援及び作
業の自動化に際して必要となる車両の現在位置及び現場
の地形等の情報を得る次めの車両位置及び姿勢角の計６
：ＩＩ装置に関する。
従来、建設現場における作業車両の運転支援および作条
を、自動化する次めに車両の３次元位置を計測して利用
するシステムとしては、特開５９−１８０４２２（走行
軌跡解析システム）が挙げられる。
を、自動化する次めに車両の３次元位置を計測して利用
するシステムとしては、特開５９−１８０４２２（走行
軌跡解析システム）が挙げられる。
この７ステムは、車両の走行速度と車両前後傾斜角と走
行方位をそれぞれ一定時間毎にサンプリングし、これら
のサンプリング値に基づいて車両の現在位置を一定時間
毎に算出するようにしている。
行方位をそれぞれ一定時間毎にサンプリングし、これら
のサンプリング値に基づいて車両の現在位置を一定時間
毎に算出するようにしている。
しかし、上記うＥ米のポ両位置計シ！１システムの場合
、車両の相対的移動量を累算して車両の現在位置ｔ？ｇ
出する定め、路面の凹凸や′ｊ＠、輪のスリップ、ある
いはサンプリング１直の誤差によって車両の現在位置に
累積ｇ４差が生じ、車両の位置計測で５度が悪いという
問題がある。
、車両の相対的移動量を累算して車両の現在位置ｔ？ｇ
出する定め、路面の凹凸や′ｊ＠、輪のスリップ、ある
いはサンプリング１直の誤差によって車両の現在位置に
累積ｇ４差が生じ、車両の位置計測で５度が悪いという
問題がある。
本発明は上記実情に鑑みてなされｔもので、作業路面形
状が頻繁に変化するような建設現場においても、実時間
で車両の三次元位置及び車両の姿勢角を高精度で計測す
ることができる車両位置及び姿勢角の計測装置ｔ−提供
することを目的とする。
状が頻繁に変化するような建設現場においても、実時間
で車両の三次元位置及び車両の姿勢角を高精度で計測す
ることができる車両位置及び姿勢角の計測装置ｔ−提供
することを目的とする。
本発明では上記目的を達成する念めに、予め設定し九２
定点に設置され、それぞれ一定周期で回転する回転レー
ザ光を投光する第１．第２の投光手段と、車両（配設さ
ｎ１前記回転レーザ光の受光タイミングおよび受光高さ
位置を検出する第１゜第２の受光手段と、車両の左右若
しくは前後のいずれか一方の第１の＃１＄Ｉｒ角を検出
する傾斜角検出手段と、前記第１の受光手段によって検
出した受光タイミングに基づいて該受光時における前記
第１、第２の投光手段からの回転レーザ光の第１゜第２
の回転角および前記第２の受光手段によつて検出した受
光タイミングに基づいて該受光時における前１ｒ［１，
菓２０投光手段からの回転レーザ光のＭ３．ｇ４の回転
角を求める回転角Ｗ出手段と、前記第１．第２の投光手
段の設置間隔と前記回転角筑出手段によって算出した第
１．第２の回転角および第３．第４の回転角に基づいて
前記第１および第２の受光手段における各受光位置の或
るｘｙＺ座標系上の第１のｘｙ座座標位置オニび第２の
ｘｙ座Ｊｄ位置を算出する第１の演算手段と、前記第１
．第２の受光手段の配設間隔、該受光手段に工ってそれ
ぞれ検出した受光高さ位置、前記傾斜色検出手段によっ
て検出した第１の傾斜角および前記第１の演算手段によ
つて算出した第１゜第２のｘｙ座標位置に基づいて車両
の前後若しくは左右の第２の傾斜角および万位角を算出
する第２の演１手段と、前記第１．第２の受光手段によ
って検出した少なくとも一方の受光高さ位置、前記第１
の演算手段によって算出した少なくとも一方のｘｙＦＥ
標位置、前記傾斜角検出手段および第２の演算手段によ
って検出若しくは算出した車両の前後傾斜角、左右傾斜
角および方位角に基づいて車両代表点のｘｙｆ％標位置
を算出する第３の演算手段と、前記回転レーザ光の２座
標位（媒、前記第１．第２の受光手段によって検出した
少なくとも一方の受光高さ位置、前記傾斜角検出手段お
よび第２の演算手段によって噴出若しくは算出した■両
の前後傾斜角および左右傾斜角に基づいて前記本両代表
点の２座標位置を屈出する第４のｔｉＩ算手段とを具え
たことを特徴としている。
定点に設置され、それぞれ一定周期で回転する回転レー
ザ光を投光する第１．第２の投光手段と、車両（配設さ
ｎ１前記回転レーザ光の受光タイミングおよび受光高さ
位置を検出する第１゜第２の受光手段と、車両の左右若
しくは前後のいずれか一方の第１の＃１＄Ｉｒ角を検出
する傾斜角検出手段と、前記第１の受光手段によって検
出した受光タイミングに基づいて該受光時における前記
第１、第２の投光手段からの回転レーザ光の第１゜第２
の回転角および前記第２の受光手段によつて検出した受
光タイミングに基づいて該受光時における前１ｒ［１，
菓２０投光手段からの回転レーザ光のＭ３．ｇ４の回転
角を求める回転角Ｗ出手段と、前記第１．第２の投光手
段の設置間隔と前記回転角筑出手段によって算出した第
１．第２の回転角および第３．第４の回転角に基づいて
前記第１および第２の受光手段における各受光位置の或
るｘｙＺ座標系上の第１のｘｙ座座標位置オニび第２の
ｘｙ座Ｊｄ位置を算出する第１の演算手段と、前記第１
．第２の受光手段の配設間隔、該受光手段に工ってそれ
ぞれ検出した受光高さ位置、前記傾斜色検出手段によっ
て検出した第１の傾斜角および前記第１の演算手段によ
つて算出した第１゜第２のｘｙ座標位置に基づいて車両
の前後若しくは左右の第２の傾斜角および万位角を算出
する第２の演１手段と、前記第１．第２の受光手段によ
って検出した少なくとも一方の受光高さ位置、前記第１
の演算手段によって算出した少なくとも一方のｘｙＦＥ
標位置、前記傾斜角検出手段および第２の演算手段によ
って検出若しくは算出した車両の前後傾斜角、左右傾斜
角および方位角に基づいて車両代表点のｘｙｆ％標位置
を算出する第３の演算手段と、前記回転レーザ光の２座
標位（媒、前記第１．第２の受光手段によって検出した
少なくとも一方の受光高さ位置、前記傾斜角検出手段お
よび第２の演算手段によって噴出若しくは算出した■両
の前後傾斜角および左右傾斜角に基づいて前記本両代表
点の２座標位置を屈出する第４のｔｉＩ算手段とを具え
たことを特徴としている。
まず、前記第１．第２の投光手段の設置間隔とｍｌの受
光手段が該第１．第２の投光手段からの各回転レーザ光
を受光したときの回転レーザ光の第１．第２の回転角に
基づいて三角測量の原理により第１の受光手段における
受光位置の或る！７２座標系上の第１のｘｙ座標位位置
求める。同様にして、第２の受光手段における受光位置
のた）２のｘｙ座梯位喧を求める。
光手段が該第１．第２の投光手段からの各回転レーザ光
を受光したときの回転レーザ光の第１．第２の回転角に
基づいて三角測量の原理により第１の受光手段における
受光位置の或る！７２座標系上の第１のｘｙ座標位位置
求める。同様にして、第２の受光手段における受光位置
のた）２のｘｙ座梯位喧を求める。
車両は常に水平状態にないため、上記ｘｙｙ標位置から
直ちに車両代表点のＸ’ｆ座標位置を求めることはでき
ない。そこで、車両の姿勢角（車両の前後傾斜角、左右
傾斜角、方位角）を求める。
直ちに車両代表点のＸ’ｆ座標位置を求めることはでき
ない。そこで、車両の姿勢角（車両の前後傾斜角、左右
傾斜角、方位角）を求める。
車両の前後傾斜角および左右傾斜角のいずれか一方は傾
斜角検出手段によって直接検出し、他方の傾斜角および
方位角は、第１．第２の受光手段の配設間隔、該受光手
段によってそれぞれ検出した受光高さ位な（、前記傾斜
角検出手段によって検出した傾斜角および前記第１．第
２のｘｙｙ標位置に基づいて求める。
斜角検出手段によって直接検出し、他方の傾斜角および
方位角は、第１．第２の受光手段の配設間隔、該受光手
段によってそれぞれ検出した受光高さ位な（、前記傾斜
角検出手段によって検出した傾斜角および前記第１．第
２のｘｙｙ標位置に基づいて求める。
そして、車両代表点のｘｙ服標位寮は前記第１゜第２の
χｙ座標位鷺の少なくとも一方のｘｙ！ＩＫ！樟位ｒｋ
’ｔと前記検出又ｉ−ｔ、ｘ出して得た車両の姿勢角に
基づいて求め、車両代表点の２座標位置は回転レーザ光
の２座標位看、第１．第２の受光手段によりで検出した
少なくとも一方の受光高さ位置および車両の姿勢角（こ
の助合、方位角は除く）に基づいて求める。
χｙ座標位鷺の少なくとも一方のｘｙ！ＩＫ！樟位ｒｋ
’ｔと前記検出又ｉ−ｔ、ｘ出して得た車両の姿勢角に
基づいて求め、車両代表点の２座標位置は回転レーザ光
の２座標位看、第１．第２の受光手段によりで検出した
少なくとも一方の受光高さ位置および車両の姿勢角（こ
の助合、方位角は除く）に基づいて求める。
以下、本発明を添付図面を参照して′＃−ｔ−細に説明
する。
する。
まず、本発明に係る車両の三次元位置等の計測方法を原
理的に説明する。
理的に説明する。
瀉２図ＩＣおいて、Ａ点とＢ点の距離をＬＸｘ軸と線分
ＡＣとのなす角をα、ＸＵと線分ＢＣとのなす角をαｂ
とすると、Ｃ点のｘｙ座標は、次式、によって表わすこ
とができる。そこで、本発明では、上記α８．αｂを求
めるに際し、Ａ点およびＢ点にそれぞれ回転レーザ光を
投光する投光装置燵ヲ設置し、車両（受光装置咬を搭載
し、この受光装置か上記回転レーザ５？、を受光したと
きの各々の回転レーザ光の回転角を検出することにエリ
行なう。
ＡＣとのなす角をα、ＸＵと線分ＢＣとのなす角をαｂ
とすると、Ｃ点のｘｙ座標は、次式、によって表わすこ
とができる。そこで、本発明では、上記α８．αｂを求
めるに際し、Ａ点およびＢ点にそれぞれ回転レーザ光を
投光する投光装置燵ヲ設置し、車両（受光装置咬を搭載
し、この受光装置か上記回転レーザ５？、を受光したと
きの各々の回転レーザ光の回転角を検出することにエリ
行なう。
この回転角α１．αｂは、ｗ、３図に示すように回転レ
ーザ光の周期をＴ（第３図（、） ） 、Ａ点およびＢ
点に設置した設光装置からの各々の回転レーザ光が基準
方位（Ｘ軸方向）に達してから単載受光装ｄＫ入射する
までの時間をそれぞれｔ、およびＪ。
ーザ光の周期をＴ（第３図（、） ） 、Ａ点およびＢ
点に設置した設光装置からの各々の回転レーザ光が基準
方位（Ｘ軸方向）に達してから単載受光装ｄＫ入射する
までの時間をそれぞれｔ、およびＪ。
（第３図（ｂ）および（Ｃ））とすると、次式、のよう
に算出される。
に算出される。
上記第（１）式によって算出されるｘｙ座標は、単載受
光装置のレーザ光受光点のが棒であやで、車両代表点の
座標でにない。そこで、車両代表点の座標に換算するに
際し、車両の姿勢角（車両の前後傾斜角、左右傾斜角、
方位角）を求める。
光装置のレーザ光受光点のが棒であやで、車両代表点の
座標でにない。そこで、車両代表点の座標に換算するに
際し、車両の姿勢角（車両の前後傾斜角、左右傾斜角、
方位角）を求める。
次に、上記姿勢角の求め方について説明する。
ｌず、第４図に示すように車体１と受光装置２゜３をモ
デル化して考える。ここで、Ｐ点ｔｘ’ｙ’ｚ’座標の
座標原点とし、車両の前後傾斜角θ、左右傾斜角δおよ
び方位角ψがそれぞれＯのときにおける上記第４図に示
した車体各部の点ｘ／ ｙ／ ｓ／座標における座標位
置を示すと第５図のようになる。
デル化して考える。ここで、Ｐ点ｔｘ’ｙ’ｚ’座標の
座標原点とし、車両の前後傾斜角θ、左右傾斜角δおよ
び方位角ψがそれぞれＯのときにおける上記第４図に示
した車体各部の点ｘ／ ｙ／ ｓ／座標における座標位
置を示すと第５図のようになる。
なお、Ｑ点およびＴ点はそれぞれ受光装置２および３に
おけるレーザ光受光点である。
おけるレーザ光受光点である。
さて、車体の姿勢角が変化した場合における上記Ｑ点お
よびＴ点のｘｔ ｙ／ ｚ／座標系における移動位置（
Ｘｑｅ Ｆｑ＋ Ｚｑ ）および（４ｔ＊ ３’ｔｅ
Ｚｔ）は１次式、 によつて表わされる。
よびＴ点のｘｔ ｙ／ ｚ／座標系における移動位置（
Ｘｑｅ Ｆｑ＋ Ｚｑ ）および（４ｔ＊ ３’ｔｅ
Ｚｔ）は１次式、 によつて表わされる。
したがって、第２図のＡＡを原点とするｘｙｚ座像系に
おけるＰ点の位ｆ＆ ’ｔ” Ｐ （Ｘｇ ＋ ）’６
ｍ ’ １０）とすると、前記Ｑ ＡＺ Ｎ Ｔ点の
ｘｙｚ座傾系におけを位置（）Ｃｑ＋３’ｑｓＺｑ×お
よび（Ｘ乞＊ ）’ｌ ＋ ”ｔ）は、次式 ここで、第（５）式、第（６）式における（ ｘｑ ＊
７４ ）および（ｘｔ＋ｙｔ）は、ナれぞれ前記第（
１）式に基づいてＪｌ、出できる。ま之、車両の前後傾
斜角θおよび左右傾斜角δのうちいずれか一方、本実施
例では左右傾斜角δは傾斜センサを用いて計測するもの
とする。
おけるＰ点の位ｆ＆ ’ｔ” Ｐ （Ｘｇ ＋ ）’６
ｍ ’ １０）とすると、前記Ｑ ＡＺ Ｎ Ｔ点の
ｘｙｚ座傾系におけを位置（）Ｃｑ＋３’ｑｓＺｑ×お
よび（Ｘ乞＊ ）’ｌ ＋ ”ｔ）は、次式 ここで、第（５）式、第（６）式における（ ｘｑ ＊
７４ ）および（ｘｔ＋ｙｔ）は、ナれぞれ前記第（
１）式に基づいてＪｌ、出できる。ま之、車両の前後傾
斜角θおよび左右傾斜角δのうちいずれか一方、本実施
例では左右傾斜角δは傾斜センサを用いて計測するもの
とする。
上記第（３）式から第（６）式より、
＝ （ｈ、 −ｈ２）部δ内０（２）ψ−ｈ１ｇＩｎ’
ｇｈｉψ−ｂａｉｓθｃａψ１０．（７）＝ （ｈ、−
ｈ２）（２）δ画θ廊ψ ＋ｈ１−δ■ψ−ｂ部θ―ψ ・・・（８）上記第
（７）式および第（８）式からθを消去すると、ｈ、Ｉ
ＩＩＩＩδ＝ （７ｑ −ｙ ｔ ””ψ−（ｘｑ−ｘ
ｔ）ｔｈψ＝Ｕ癲（ψ＋Ｕ） ・・・（
９）ｘｑ−ｘｔ となり、上紀鄭（９）式を変形すると、となる。また、
第（７）式をθについて解くと、友だし、ｕ ’ ＝
、／ （ｈ １−ｈ ２）２ａｓ２δａｓ２（ｐ＋ ｂ
２ｃｏｓ２９）となる。すなわち、計測した（ Ｘｑ
＋ ３’ｑ ） ＋ （Ｘｔ＊ｙｔ）およびδから、上
記四穴および第０９式によつて車両の方位角ψおよび前
後傾斜角θを求めることができる。
ｇｈｉψ−ｂａｉｓθｃａψ１０．（７）＝ （ｈ、−
ｈ２）（２）δ画θ廊ψ ＋ｈ１−δ■ψ−ｂ部θ―ψ ・・・（８）上記第
（７）式および第（８）式からθを消去すると、ｈ、Ｉ
ＩＩＩＩδ＝ （７ｑ −ｙ ｔ ””ψ−（ｘｑ−ｘ
ｔ）ｔｈψ＝Ｕ癲（ψ＋Ｕ） ・・・（
９）ｘｑ−ｘｔ となり、上紀鄭（９）式を変形すると、となる。また、
第（７）式をθについて解くと、友だし、ｕ ’ ＝
、／ （ｈ １−ｈ ２）２ａｓ２δａｓ２（ｐ＋ ｂ
２ｃｏｓ２９）となる。すなわち、計測した（ Ｘｑ
＋ ３’ｑ ） ＋ （Ｘｔ＊ｙｔ）およびδから、上
記四穴および第０９式によつて車両の方位角ψおよび前
後傾斜角θを求めることができる。
上記車両の姿勢角（θ、δ、ψ）が求まると、Ｐ点の位
ｔｔｉｅＰ（ｘ０＊ ｙ６ ＊ ｚ□ ）は、ｇ （３
）式および第（５）式から、次式 Ｘ６ ＝Ｘｑ−（ＪＱｌ’ｌδ虐θ房ψ−ｈ、Ｉ−δ虐
ψ）ｙ６ ＝ ７４− （ｈ、（２）ａ−θ龜ψ＋ｈ、
廁６（２）ψ）１６ ＝Ｘｑ−１１１ＱＪｓｌ−〇＝ｈ
、 −ｈｊｃｒ１５δ朝θ・・・（２） となる。友だし、ｈｏけ回転レーザ光のｘｙｚｒ標系に
おける高さである。
ｔｔｉｅＰ（ｘ０＊ ｙ６ ＊ ｚ□ ）は、ｇ （３
）式および第（５）式から、次式 Ｘ６ ＝Ｘｑ−（ＪＱｌ’ｌδ虐θ房ψ−ｈ、Ｉ−δ虐
ψ）ｙ６ ＝ ７４− （ｈ、（２）ａ−θ龜ψ＋ｈ、
廁６（２）ψ）１６ ＝Ｘｑ−１１１ＱＪｓｌ−〇＝ｈ
、 −ｈｊｃｒ１５δ朝θ・・・（２） となる。友だし、ｈｏけ回転レーザ光のｘｙｚｒ標系に
おける高さである。
したがって、第四式からＰ点の位ｆ？Ｊ、を求めること
ができ、ま７’ｔＰ点と一定の関係にある車両代表点の
位置（例えば東心）４求めることもできる。
ができ、ま７’ｔＰ点と一定の関係にある車両代表点の
位置（例えば東心）４求めることもできる。
第１図は本発明の一実施例を示すブロック図である。同
図において、地上局側は、地上基準局１０１２台の投光
装置１１，１２、地図表示器１３および地図ｒ−タ記憶
装置１４から構成されている。地上基準局１０は更に送
受信機１０ａ１処理装置１０ｂおよび発振器１０ｃから
構成され、投光！４置１１．１２はパルスモータ目」、
１２鳳、回転ヘッドｌｌｂ、１２ｂおよびレーザ発振器
１１ｅ、１２ｃから狩こ成されている。
図において、地上局側は、地上基準局１０１２台の投光
装置１１，１２、地図表示器１３および地図ｒ−タ記憶
装置１４から構成されている。地上基準局１０は更に送
受信機１０ａ１処理装置１０ｂおよび発振器１０ｃから
構成され、投光！４置１１．１２はパルスモータ目」、
１２鳳、回転ヘッドｌｌｂ、１２ｂおよびレーザ発振器
１１ｅ、１２ｃから狩こ成されている。
一方、車両には、２つの受光装置２．３、車両左右傾斜
センサ４、送受信機５ｉ？工びその他、棟槌演算、処理
、制御を行なう装置が設けられている。
センサ４、送受信機５ｉ？工びその他、棟槌演算、処理
、制御を行なう装置が設けられている。
投光装ｆｉｉ！：１１および１２は、第２図に示すよう
に一定の間隔りをもりてｘｙｚ座標系のＡ点およびＢ点
に設置される。ここで、投光装置１１゜１２からの回転
レーザ光平面は！ｙ平面と平行になるように、ま友各回
転し−デ光平面の高さは所定高さｈｏとなるように設置
される。
に一定の間隔りをもりてｘｙｚ座標系のＡ点およびＢ点
に設置される。ここで、投光装置１１゜１２からの回転
レーザ光平面は！ｙ平面と平行になるように、ま友各回
転し−デ光平面の高さは所定高さｈｏとなるように設置
される。
投光装置１１は、レーザ発振器１１ａから連続発損され
るレーザ光を回転ヘッド１１ｂＫ入射し、この、回転ヘ
ッド１ｌｂ（ｉ−）＃シスモー２１１ｍで回転させるこ
とにより、該回転ヘッドｆｌｂから回転レーザ光を投光
する。パルスそ一夕１１ｍは地上基準局１０の発嘔器１
０ａから加えられるクロックパルスに同期して回転する
。なお、投光装置１２も投光装置１１と同様にして回転
レーザ光を投光する。すなわち、回転ヘッドｌｌｂおよ
び１２ｂの回転同期および位相は一致するようになって
いる。
るレーザ光を回転ヘッド１１ｂＫ入射し、この、回転ヘ
ッド１ｌｂ（ｉ−）＃シスモー２１１ｍで回転させるこ
とにより、該回転ヘッドｆｌｂから回転レーザ光を投光
する。パルスそ一夕１１ｍは地上基準局１０の発嘔器１
０ａから加えられるクロックパルスに同期して回転する
。なお、投光装置１２も投光装置１１と同様にして回転
レーザ光を投光する。すなわち、回転ヘッドｌｌｂおよ
び１２ｂの回転同期および位相は一致するようになって
いる。
また、地上基準局１０の処理装置１０ｂは、発振器１０
ｃからのクロックパルスによって投光装置１１．１２
からのレーザ光が基準方位（Ｘ軸方向）を向く時点を検
知し、この時点に送受信機１０ｍより基準方位を示す基
準方位信号を発生させる。
ｃからのクロックパルスによって投光装置１１．１２
からのレーザ光が基準方位（Ｘ軸方向）を向く時点を検
知し、この時点に送受信機１０ｍより基準方位を示す基
準方位信号を発生させる。
３に誠送受信機５は上記基準方位信号ｆ、受信すると、
これを銀画位置演算装置２０に加える。また、車両位置
ヌ算装置２０には受光装置２および３から回転レーザ光
の受光タイミングを示す信号および受光高さ位置ｈ１お
よびｈ２ｔ？示すｊＢ号が加えられ、史に車両左右傾斜
センサ４から左右傾斜角δを示す信号が加えられる。な
お、車両側に地上局側の発振器１０ａと閤−の時刻を計
時する手段を設けるようにすれば、レーザ光が基準方位
を向く時点を車両側のみで検知することもできる。
これを銀画位置演算装置２０に加える。また、車両位置
ヌ算装置２０には受光装置２および３から回転レーザ光
の受光タイミングを示す信号および受光高さ位置ｈ１お
よびｈ２ｔ？示すｊＢ号が加えられ、史に車両左右傾斜
センサ４から左右傾斜角δを示す信号が加えられる。な
お、車両側に地上局側の発振器１０ａと閤−の時刻を計
時する手段を設けるようにすれば、レーザ光が基準方位
を向く時点を車両側のみで検知することもできる。
車両位置演算装［２０鉱、まず各受光装置の受光位［Ｑ
、Ｔの座標位ｔＲ（Ｘｑ＋ ７ｑｅ Ｚｑ ）およびＣ
Ｘｔ＊ １％ ｒ Ｚｔ）を第（１）式およびｇ （２
１式に基づいて求める。なお、ｘ、＝ Ｑ ＝ ｈｏで
ある。次に、このようにして求め九（”ｑ ＊ ７ｑ
）、（ｘｔ、 ｙｔ）、受光高さ位置ｈ、および傾斜セ
ンサ４からの傾柵角δｆ！：ｇ （１０式に代入するこ
とによシ方位角ψを求める。更に、上記各値と方位角ψ
および受光高さ位１１ｔｈ２を第０ｕ式に代入すること
により車両前後傾斜角θを求める。
、Ｔの座標位ｔＲ（Ｘｑ＋ ７ｑｅ Ｚｑ ）およびＣ
Ｘｔ＊ １％ ｒ Ｚｔ）を第（１）式およびｇ （２
１式に基づいて求める。なお、ｘ、＝ Ｑ ＝ ｈｏで
ある。次に、このようにして求め九（”ｑ ＊ ７ｑ
）、（ｘｔ、 ｙｔ）、受光高さ位置ｈ、および傾斜セ
ンサ４からの傾柵角δｆ！：ｇ （１０式に代入するこ
とによシ方位角ψを求める。更に、上記各値と方位角ψ
および受光高さ位１１ｔｈ２を第０ｕ式に代入すること
により車両前後傾斜角θを求める。
続いて、上記受光位ｆＱ（Ｘ４．ｙ、＃Ｚ、）およびＴ
（Ｘｔ＊ Ｙｔ＋ ｚｔ）と上記のようＫして求めた
姿勢角（θ、δ、ψ）に基づいて車両代表点の三次元位
置を求める。なお、受光装置２の車両取付部分のＰ点の
位＃Ｐ （Ｘ６ ｅ ７６ ＊ ”６ ）は、第αつ式
に（Ｘｑｓ ７ｑ＋ Ｚｑ ）および（θ、δ、ψ）を
代入することにより求めることができる。
（Ｘｔ＊ Ｙｔ＋ ｚｔ）と上記のようＫして求めた
姿勢角（θ、δ、ψ）に基づいて車両代表点の三次元位
置を求める。なお、受光装置２の車両取付部分のＰ点の
位＃Ｐ （Ｘ６ ｅ ７６ ＊ ”６ ）は、第αつ式
に（Ｘｑｓ ７ｑ＋ Ｚｑ ）および（θ、δ、ψ）を
代入することにより求めることができる。
このようにして求めた原画代表点の三次元位置。
を示す信号は地図データ処理装置２１に加えられる。地
図データ処理装置！ｌｔ、２１は地図データ記憶装置２
３からの旧地図データに基づいて車両走行軌跡や地形デ
ータ等を更新して新地図データを作成する。
図データ処理装置！ｌｔ、２１は地図データ記憶装置２
３からの旧地図データに基づいて車両走行軌跡や地形デ
ータ等を更新して新地図データを作成する。
例えば、車両の走行領域（作業領域）を第１表のように
Ｘおよびｙ方向に有限個（ｎＸｎ ）で区分し、各区分
位置をアドレスとする記憶部を設け、水閘代表点がその
有限個のある位置に達すると、その位置に対応する記憶
部に２座掠を薔き込むことにより三次元走行軌跡の地図
データを記憶させることができる。
Ｘおよびｙ方向に有限個（ｎＸｎ ）で区分し、各区分
位置をアドレスとする記憶部を設け、水閘代表点がその
有限個のある位置に達すると、その位置に対応する記憶
部に２座掠を薔き込むことにより三次元走行軌跡の地図
データを記憶させることができる。
第１表
なお、すでに２座像が弥き込まれた位置に遅し九場合に
は最新の２座標に傅き換える工すにする。
は最新の２座標に傅き換える工すにする。
ま九、車両代表点を地面との接地点とすると、上記のよ
うにして記憶し九三次元の走行軌跡データは地形データ
としてとえることができる。
うにして記憶し九三次元の走行軌跡データは地形データ
としてとえることができる。
！之、地図データ処理装！２１は、地図データ記す、Ｑ
装置２３に記憶されている地図ｆ−タに基づいて車両の
走行軌跡あるいは地形を地図表示器２２に表示させる。
装置２３に記憶されている地図ｆ−タに基づいて車両の
走行軌跡あるいは地形を地図表示器２２に表示させる。
第６図は地図表示の一例を示す。もち胸、原画の現在位
置をその地図上に表示させることもできる。
置をその地図上に表示させることもできる。
！載送受信機５Ｖｉ地図データ記憶装置ＩｔＫ記憶され
ている地図データを送信し、地上基準局１０の送受イぎ
機１０＆はこれを受信して処理装置１０ｂに加える。処
理装置１０ｂは入力した地図データを地図データ記憶装
置１４に記憶させるとともに、その記憶させ次地図デー
タに基づいて車両の走行軌跡あるいは地形を地図表示器
１３に表示させる。
ている地図データを送信し、地上基準局１０の送受イぎ
機１０＆はこれを受信して処理装置１０ｂに加える。処
理装置１０ｂは入力した地図データを地図データ記憶装
置１４に記憶させるとともに、その記憶させ次地図デー
タに基づいて車両の走行軌跡あるいは地形を地図表示器
１３に表示させる。
作業指令発生装置２４には予定の作業内容が設定されて
おシ、演算処理装置ｊ１２５は、作業指令発生装置から
加えられる作業内容を示す信号と、地区データ処理装置
２１から加えられるデータ（地図データ、爪側位置デー
タ、姿勢角データ）とて基づいてエンジン制（至）信号
、変速制御信号、ステアリング制御信号および作業機ｆ
ｌｉｌｌ ｍ信号を形成してエンジン制御装置２６、変
速制御装置２７、ステアリングＩｔＩ１１御装＃、２８
および咋桑機制御装責２９を制御する。
おシ、演算処理装置ｊ１２５は、作業指令発生装置から
加えられる作業内容を示す信号と、地区データ処理装置
２１から加えられるデータ（地図データ、爪側位置デー
タ、姿勢角データ）とて基づいてエンジン制（至）信号
、変速制御信号、ステアリング制御信号および作業機ｆ
ｌｉｌｌ ｍ信号を形成してエンジン制御装置２６、変
速制御装置２７、ステアリングＩｔＩ１１御装＃、２８
および咋桑機制御装責２９を制御する。
なお、上記システムは、実時間で得九車両位置データお
よび地形データや作業指令を地上基準局１０とデータ通
信することにより車両の遠隔！１ｊ御等を行なう趨設作
業の管制システムを構成することができる。
よび地形データや作業指令を地上基準局１０とデータ通
信することにより車両の遠隔！１ｊ御等を行なう趨設作
業の管制システムを構成することができる。
なお、本実施例では前後傾斜角δは傾斜センナによって
検出するようＫＬＩが、前述した第（５）式および第（
６）弐【、それぞれ＠ （３）式および第（４）式を代
入した式は、 ｘｑ＝ ｘ。＋ ｈ１ｃｏｓＪｆｎθｃｍ９＋ −ｈ１
ｍａｇｉｎ９＋７ｑ ＝ Ｙ０＋ｈｌａＢδｍｌ嘔ψ＋
ｈ１ｍδａｔｓｑＪｚｑ ＝ ｇｏ＋ ｈ１ｃｏｓＪｅ
ａｔθＸｔ＝Ｘ、 ＋ ｈ２可δ龜θ篩ψ＋ｂ（２）θ
［有］ψ７ｔ” ７ ｏ ＋ ｈ ｌ ＣＩＲδｇｆａ
＃＊（Ｐ ＋ １）ｃｏｓｏ癲ψｚｔ＝ ｚ０＋ｈ２ｃ
ｃｅ＋δ。θ−に＋ｔｔｎθの合１ｉ６式となり、これ
らの式における未知数は（１６ｍ ７Ｇ１１６ ）およ
び（δ、θ、ψ）の合計６個であるため、上記６式を解
くことにより全ての未知数を求めることができる。し九
がって、この場分には＃ｉｘセンサは不キとなる。
検出するようＫＬＩが、前述した第（５）式および第（
６）弐【、それぞれ＠ （３）式および第（４）式を代
入した式は、 ｘｑ＝ ｘ。＋ ｈ１ｃｏｓＪｆｎθｃｍ９＋ −ｈ１
ｍａｇｉｎ９＋７ｑ ＝ Ｙ０＋ｈｌａＢδｍｌ嘔ψ＋
ｈ１ｍδａｔｓｑＪｚｑ ＝ ｇｏ＋ ｈ１ｃｏｓＪｅ
ａｔθＸｔ＝Ｘ、 ＋ ｈ２可δ龜θ篩ψ＋ｂ（２）θ
［有］ψ７ｔ” ７ ｏ ＋ ｈ ｌ ＣＩＲδｇｆａ
＃＊（Ｐ ＋ １）ｃｏｓｏ癲ψｚｔ＝ ｚ０＋ｈ２ｃ
ｃｅ＋δ。θ−に＋ｔｔｎθの合１ｉ６式となり、これ
らの式における未知数は（１６ｍ ７Ｇ１１６ ）およ
び（δ、θ、ψ）の合計６個であるため、上記６式を解
くことにより全ての未知数を求めることができる。し九
がって、この場分には＃ｉｘセンサは不キとなる。
以上説明したように本発明によれば、建設現場のような
不聚地でも計測誤差が累積することなく車両の：Ｅ、次
元位考を精度よく、かつ実時間で求めることができる。
不聚地でも計測誤差が累積することなく車両の：Ｅ、次
元位考を精度よく、かつ実時間で求めることができる。
また、高梢度なＩｉ ｒ＃ｆｉ！ 曽ｆｌ！報および地
形情や）を作菓者に提供して作呟支援を行なうことかで
さる。
形情や）を作菓者に提供して作呟支援を行なうことかで
さる。
第１図は本発明の一実施例を示すブロック図、第２図乃
至第５図は本＠明に係る位＊ｉ測方法を説明するため【
用いた図、８６図はり１図における地図表示器の表示例
を示すメである。 １・・・本体、２．３・・・受光装置、４・・・車両左
右傾斜センサ、５・・・送受偵機、１０・・・地上基準
局、１１．１２・・・投光装置、１３．２２・・・地図
表示器、１４．２３・・・地図ｒ−タ記憶装眞、２０・
・・車両位１Ｊ！ｔ、演算装置、２１・・・地図データ
処理装置。 出Ｐへ人代理へ 木 村 筒 久 第２図
至第５図は本＠明に係る位＊ｉ測方法を説明するため【
用いた図、８６図はり１図における地図表示器の表示例
を示すメである。 １・・・本体、２．３・・・受光装置、４・・・車両左
右傾斜センサ、５・・・送受偵機、１０・・・地上基準
局、１１．１２・・・投光装置、１３．２２・・・地図
表示器、１４．２３・・・地図ｒ−タ記憶装眞、２０・
・・車両位１Ｊ！ｔ、演算装置、２１・・・地図データ
処理装置。 出Ｐへ人代理へ 木 村 筒 久 第２図
Claims (4)
- （１）予め設定した２定点に設置され、それぞれ一定周
期で回転する回転レーザ光を投光する第１，第２の投光
手段と、 車両に配設され、前記回転レーザ光の受光タイミングお
よび受光高さ位置を検出する第１，第２の受光手段と、 車両の左右若しくは前後のいずれか一方の第１の傾斜角
を検出する傾斜角検出手段と、 前記第１の受光手段によって検出した受光タイミングに
基づいて該受光時における前記第１，第２の投光手段か
らの回転レーザ光の第１，第２の回転角および前記第２
の受光手段によって検出した受光タイミングに基づいて
該受光時における前記第１，第２の投光手段からの回転
レーザ光の第３，第４の回転角を求める回転角算出手段
と、前記第１，第２の投光手段の設置間隔と前記回転角
算出手段によって算出した第１，第２の回転角および第
３，第４の回転角に基づいて前記第１および第２の受光
手段における各受光位置の或るｘｙｚ座標系上の第１の
ｘｙ座標位置および第２のｘｙ座標位置を算出する第１
の演算手段と、前記第１，第２の受光手段の配設間隔、
該受光手段によってそれぞれ検出した受光高さ位置、前
記傾斜角検出手段によって検出した第１の傾斜角および
前記第１の演算手段によって算出した第１第２のｘｙ座
標位置に基づいて車両の前後若しくは左右の第２の傾斜
角および方位角を算出する第２の演算手段と、 前記第１，第２の受光手段によって検出した少なくとも
一方の受光高さ位置、前記第１の演算手段によって算出
した少なくとも一方のｘｙ座標位置、前記傾斜角検出手
段および第２の演算手段によって検出若しくは算出した
車両の前後傾斜角、左右傾斜角および方位角に基づいて
車両代表点のｘｙ座標位置を算出する第３の演算手段と
、前記回転レーザ光のｚ座標位置、前記第１，第２の受
光手段によって検出した少なくとも一方の受光高さ位置
、前記傾斜角検出手段および第２の演算手段によって検
出若しくは算出した車両の前後傾斜角および左右傾斜角
に基づいて前記車両代表点のｚ座標位置を算出する第４
の演算手段と、を具えた車両位置及び姿勢角の計測装置
。 - （２）前記算出した車両の代表点のｘｙｚ座標位置を順
次記憶する記憶手段と、前記記憶手段に記憶したｘｙｚ
座標位置に基づいて地図を作成表示する地図出力手段と
を含む特許請求の範囲第（１）項記載の車両位置及び姿
勢角の計測装置。 - （３）前記記憶手段は、同一のｘｙ座標位置を有する三
次元位置の場合には最新の三次元位置のみを記憶する特
許請求の範囲第（２）項記載の車両位置及び姿勢角の計
測装置。 - （４）予め設定した２定点に設置され、それぞれ一定周
期で回転する回転レーザ光を投光する第１，第２の投光
手段と、 車両に配設され、前記回転レーザ光の受光タイミングお
よび受光高さ位置を検出する第１，第２の受光手段と、 前記第１の受光手段によって検出した受光タイミングに
基づいて該受光時における前記第１，第２の投光手段か
らの回転レーザ光の第１，第２の回転角および前記第２
の受光手段によって検出した受光タイミングに基づいて
該受光時における前記第１，第２の投光手段からの回転
レーザ光の第３，第４の回転角を求める回転角算出手段
と、前記第１，第２の投光手段の設置間隔と前記回転角
算出手段によって算出した第１，第２の回転角および第
３，第４の回転角に基づいて前記第１および第２の受光
手段における各受光位置の或るｘｙｚ座標系上の第１の
ｘｙ座標位置および第２のｘｙ座標位置を算出する第１
の演算手段と、前記第１，第２の受光手段の配設間隔、
該受光手段によってそれぞれ検出した受光高さ位置およ
び前記第１の演算手段によって算出した第１，第２のｘ
ｙ座標位置に基づいて車両の前後傾斜角、左右傾斜角お
よび方位角を算出する第２の演算手段と、 前記第１，第２の受光手段によって検出した少なくとも
一方の受光高さ位置、前記第１の演算手段によって算出
した少なくとも一方のｘｙ座標位置および第２の演算手
段によって算出した車両の前後傾斜角、左右傾斜角およ
び方位角に基づいて車両代表点のｘｙ座標位置を算出す
る第３の演算手段と、 前記回転レーザ光のｚ座標位置、前記第１，第２の受光
手段によって検出した少なくとも一方の受光高さ位置お
よび第２の演算手段によって算出した車両の前後傾斜角
および左右傾斜角に基づいて前記車両代表点のｚ座標位
置を算出する第４の演算手段と、 を具えた車両位置及び姿勢角の計測装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP11685786A JP2603068B2 (ja) | 1986-05-21 | 1986-05-21 | 車両位置及び姿勢角の計測装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP11685786A JP2603068B2 (ja) | 1986-05-21 | 1986-05-21 | 車両位置及び姿勢角の計測装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPS62273408A true JPS62273408A (ja) | 1987-11-27 |
| JP2603068B2 JP2603068B2 (ja) | 1997-04-23 |
Family
ID=14697345
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP11685786A Expired - Fee Related JP2603068B2 (ja) | 1986-05-21 | 1986-05-21 | 車両位置及び姿勢角の計測装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2603068B2 (ja) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH027516U (ja) * | 1988-06-30 | 1990-01-18 | ||
| JPH02140614A (ja) * | 1988-11-22 | 1990-05-30 | Sokkisha Co Ltd | レーザを用いた測量方法 |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101756555B1 (ko) * | 2015-12-14 | 2017-07-11 | 현대오트론 주식회사 | Ｔｏｆ 센서를 이용한 차량 피치각 검출 장치 및 그 방법 |
-
1986
- 1986-05-21 JP JP11685786A patent/JP2603068B2/ja not_active Expired - Fee Related
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH027516U (ja) * | 1988-06-30 | 1990-01-18 | ||
| JPH02140614A (ja) * | 1988-11-22 | 1990-05-30 | Sokkisha Co Ltd | レーザを用いた測量方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| JP2603068B2 (ja) | 1997-04-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US5983166A (en) | Structure measurement system | |
| US4818107A (en) | System for measuring the position of a moving body | |
| CN105573310B (zh) | 一种煤矿巷道机器人定位与环境建模方法 | |
| JP2866289B2 (ja) | 建設機械の位置及び姿勢表示方法 | |
| CN113970329A (zh) | 捷联惯导和激光感知复合的掘进机位姿检测系统与方法 | |
| CN102322857A (zh) | 机动设备位姿测量系统及测量方法 | |
| CN104776843A (zh) | 一种悬臂式掘进机机身与截割头位姿检测方法 | |
| JP2017009574A (ja) | レーダ軸ずれ量算出装置およびレーダ軸ずれ量算出方法 | |
| CN113075650A (zh) | 一种基于uwb与惯性单元的地下巷道掘进装备实时定位方法 | |
| CN103278132A (zh) | 自行走式地下掘进机器人姿态测量装置及其测量方法 | |
| CN111637889A (zh) | 基于惯导和激光雷达三点测距的掘进机定位方法及系统 | |
| CN114803861B (zh) | 一种煤矿井下单轨吊车高精度定位系统及定位方法 | |
| CN115950420A (zh) | 一种掘进机组合位姿检测系统及其检测方法 | |
| JP2000338865A (ja) | デジタル道路地図のデータ収集装置 | |
| JPS62273408A (ja) | 車両位置及び姿勢角の計測装置 | |
| CN114485614B (zh) | 基于双全站仪的采掘设备的导航定位系统及方法 | |
| CN110850407B (zh) | 一种基于雷达技术的地形测量方法 | |
| KR101417431B1 (ko) | Ｌｉｄａｒ센서를 이용한 3차원 공간정보 생성시스템 | |
| JPS62273409A (ja) | 車両位置及び姿勢角の計測装置 | |
| KR200303737Y1 (ko) | 이동식 gps 지형 측량 시스템 | |
| CN112857367B (zh) | 一种基于机器视觉和惯性导航的掘进机位姿检测方法 | |
| JP3247143B2 (ja) | 移動体の位置・姿勢測量装置 | |
| JP2000356684A (ja) | 探査データ表示方法及び装置 | |
| CN114758001B (zh) | 基于pnt的轮胎吊自动行走方法 | |
| Browne et al. | Localization of autonomous mobile ground vehicles in a sterile environment: a survey |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |