JP2019052955A - 測定装置、測定方法、及び、プログラム - Google Patents

Abstract

【課題】外界センサを利用して移動体の姿勢角を検出する。
【解決手段】測定装置は、移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する。また、地物検知部による地物の検知結果に基づき、第２時刻における第１座標セットに対応する座標である第２座標セットを取得する。そして、測定装置は、第１座標セットと第２座標セットに基づいて、移動体の姿勢角の変化量を算出する。
【選択図】図１０

Description

本発明は、移動体の姿勢角を検出する技術に関する。

ジャイロセンサ、加速度センサなどの自律センサの出力に基づいて、移動体のロール角、ピッチ角などを検出する手法が知られている。特許文献１は、自律センサの出力に基づいて移動体の姿勢を示す四元数を算出し、ヨー軸成分をキャンセルする修正を行った後、移動体のロール角及びピッチ角を算出する手法を記載している。

特開２０１３−１０４６６５号公報

しかし、特許文献１は、外界センサを利用して移動体の姿勢角を検出、補正することについては記載していない。

本発明の解決しようとする課題としては、上記のものが一例として挙げられる。本発明は、外界センサを利用して移動体の姿勢角を検出することを目的とする。

請求項１に記載の発明は、測定装置であって、移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する第１取得部と、前記地物検知部による前記地物の検知結果に基づき、第２時刻における前記第１座標セットに対応する座標である第２座標セットを取得する第２取得部と、前記第１座標セットと前記第２座標セットに基づいて、前記移動体の姿勢角の変化量を算出する変化量算出部と、を備えることを特徴とする。

請求項１２に記載の発明は、測定装置によって実行される測定方法であって、移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する第１取得工程と、前記地物検知部による前記地物の検知結果に基づき、第２時刻における前記第１座標セットに対応する座標である第２座標セットを取得する第２取得工程と、前記第１座標セットと前記第２座標セットに基づいて、前記移動体の姿勢角の変化量を算出する変化量算出工程と、を備えることを特徴とする。

請求項１３に記載の発明は、コンピュータを備える測地装置によって実行されるプログラムであって、移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する第１取得部、前記地物検知部による前記地物の検知結果に基づき、第２時刻における前記第１座標セットに対応する座標である第２座標セットを取得する第２取得部、前記第１座標セットと前記第２座標セットに基づいて、前記移動体の姿勢角の変化量を算出する変化量算出部、として前記コンピュータを機能させることを特徴とする。

ライダによる計測値と車両の姿勢角の関係を説明する図である。 第１実施例により車両のロール角を算出する第１の方法を示す。 ライダによる計測座標をシフトする方法を説明する図である。 第１実施例により車両のロール角を算出する第２の方法を示す。 第１実施例に係る測定装置の構成を示す。 第１実施例に係る姿勢角補正処理のフローチャートである。 第１実施例の変形例を説明する図である。 第２実施例により車両のロール角を算出する方法を示す。 第２実施例に係る測定装置の構成を示す。 第２実施例に係る姿勢角補正処理のフローチャートである。 ライダの走査方法の例を示す。

本発明の１つの好適な実施形態では、測定装置は、移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する第１取得部と、前記地物検知部による前記地物の検知結果に基づき、第２時刻における前記第１座標セットに対応する座標である第２座標セットを取得する第２取得部と、前記第１座標セットと前記第２座標セットに基づいて、前記移動体の姿勢角の変化量を算出する変化量算出部と、を備える。

上記の測定装置は、移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する。また、地物検知部による地物の検知結果に基づき、第２時刻における第１座標セットに対応する座標である第２座標セットを取得する。そして、測定装置は、第１座標セットと第２座標セットに基づいて、移動体の姿勢角の変化量を算出する。こうして、地物検知部による地物の検知結果を利用して、姿勢角の変化量を算出することができる。

上記の測定装置の一態様では、前記第１座標セット及び前記第２座標セットは、前記地物における２点の同一座標系における座標である。この態様では、地物における２点の座標に基づいて姿勢角の変化量を算出する。

上記の測定装置の他の一態様では、前記地物検知部は、当該地物検知部の現在位置を基準とした前記地物の距離及び方向を検出する。これにより、現在位置に対する地物の距離及び方向が得られる。

上記の測定装置の他の一態様では、前記第１取得部及び前記第２取得部は、前記地物の距離及び方向に基づき、それぞれ前記第１座標セット及び第２座標セットを算出する。この態様では、地物の距離及び方向に基づいて、第１座標セット及び第２座標セットが算出される。

上記の測定装置の他の一態様では、前記変化量算出部は、前記第１座標セットが示す２点の座標と、前記第２座標セットが示す２点の座標とのずれ量に基づいて前記姿勢角の変化量を算出する。この態様では、第１座標セットと第２座標セットが示す座標のずれ量に基づいて姿勢角の変化量が算出される。

好適な例では、前記２点は、１つの地物における２つの点である。また、他の好適な例では、前記２点は、１つの地物における１点と他の地物における１点である。

上記の測定装置の他の一態様は、前記第１時刻又は前記第２時刻のいずれか一方において取得された前記移動体の姿勢角と、前記変化量とに基づいて、前記第１時刻又は前記第２時刻の他方における前記移動体の姿勢角を算出する姿勢角算出部を備える。この態様では、２つの時刻間の姿勢角の変化量と、一方の時刻における姿勢角とに基づいて、他方の時刻における姿勢角を算出することができる。

上記の測定装置の他の一態様では、前記変化量算出部が算出した変化量に基づいて、自律センサの出力に基づいて算出された前記移動体の姿勢角を補正する補正部を備える。この態様では、移動体の自律センサに基づいて算出された姿勢角を補正することができる。

好適な例では、前記姿勢角は、ロール角、ピッチ角及びヨー角の少なくとも１つを含む。また、好適な例では、前記地物検知部はライダである。

本発明の他の好適な実施形態では、測定装置によって実行される測定方法は、移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する第１取得工程と、前記地物検知部による前記地物の検知結果に基づき、第２時刻における前記第１座標セットに対応する座標である第２座標セットを取得する第２取得工程と、前記第１座標セットと前記第２座標セットに基づいて、前記移動体の姿勢角の変化量を算出する変化量算出工程と、を備える。この方法により、地物検知部による地物の検知結果を利用して、姿勢角を算出することができる。

本発明の他の好適な実施形態では、コンピュータを備える測地装置によって実行されるプログラムは、移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する第１取得部、前記地物検知部による前記地物の検知結果に基づき、第２時刻における前記第１座標セットに対応する座標である第２座標セットを取得する第２取得部、前記第１座標セットと前記第２座標セットに基づいて、前記移動体の姿勢角の変化量を算出する変化量算出部、として前記コンピュータを機能させる。このプログラムをコンピュータで実行することにより、上記の測定装置を実現することができる。このプログラムは、記憶媒体に記憶して取り扱うことができる。

以下、図面を参照して本発明の好適な実施例について説明する。
［概要説明］
従来のカーナビゲーション装置においては、ジャイロセンサや加速度センサなどの自律センサの出力に基づいてロール角、ピッチ角などの姿勢角を推定し、ＧＰＳ測位情報や地図情報を使用して姿勢角の補正を行っている。しかし、車両の姿勢は、道路形状と必ずしも一致せず、乗員の位置や動き、車両の加減速状態などにより変化するため、ＧＰＳ測位情報や地図情報のみでは、姿勢角を精度良く補正することができない場合が多い。

そこで、以下に述べる実施例では、ライダなどの外界センサによる地物の計測結果や地図情報に基づいて車両の姿勢角を求め、ジャイロセンサや加速度センサなどを利用して推定した姿勢角の補正を行う。これにより、車両の姿勢角の検出精度を向上させることができる。

［基本原理］
図１（Ａ）は、車両を中心としたボディ座標系を示す。原点に車両が存在するとし、車両の進行方向をｘ軸、車両の左右方向をｙ軸、車両の上下方向をｚ軸と規定する。ここで、ｘ軸回りの角度は車両の「ロール角」と呼ばれ、ｙ軸回りの角度は車両の「ピッチ角」と呼ばれ、ｚ軸回りの角度は車両の「ヨー角」と呼ばれる。以下、ロール角、ピッチ角、ヨー角をまとめて車両の「姿勢角」と呼ぶ。

図１（Ｂ）は、車両に搭載したライダ（ＬｉＤＡＲ：Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）により計測対象物を計測する様子を示す。車両の自車位置をボディ座標系の原点とする。この場合、ライダは計測対象物の計測値として、自車位置から計測対象物までの距離Ｌ、並びに、自車位置を基準とした計測対象物の水平角α及び垂直角βを出力する。出力された距離Ｌ、水平角α及び垂直角βを以下の式でデカルト座標系に変換すると、計測対象物の座標（Ｌｘ、Ｌｙ、Ｌｚ）が得られる。

ここで、ボディ座標系における自車位置と計測対象物との各角度は、車両の各姿勢角と対応付けることができる。即ち、図１（Ａ）に示すように、ｘｙ平面における自車位置と計測対象物との角度はヨー角に相当し、ｘｚ平面における自車位置と計測対象物との角度はピッチ角に相当し、ｙｚ平面における自車位置と計測対象物との角度はロール角に相当する。よって、各平面における自車位置と計測対象物との角度に基づいて、計測対象物を基準とした車両の姿勢角を得ることができる。

なお、以下の説明では、ワールド座標系（地図座標系）３軸を大文字の「Ｘ」、「Ｙ」、「Ｚ」で示し、ボディ座標系（車両座標系）の３軸を小文字の「ｘ」、「ｙ」、「ｚ」で示すものとする。

［第１実施例］
（姿勢角の算出方法）
次に、ライダによる計測値を用いて姿勢角を算出する具体的な方法について説明する。なお、以下ではワールド座標系のＹＺ平面においてロール角を算出する例を説明するが、同様の方法により、ＸＺ平面においてピッチ角を算出することができ、ＸＹ平面においてヨー角を算出することができる。第１実施例では、２つの方法により、姿勢角を算出することができる。

（１）第１の方法
第１の方法は、１つの地物における２つの基準点の地図座標を利用する。なお、「基準点」とは、姿勢角の算出に用いられる、地物における点である。図２は、第１の方法により、ライダによる計測値を用いて車両のロール角を算出する方法を示す。この方法では、車両の姿勢が水平ではない（ロール角≠０）場合に、地物Ａを、その実際の設置角度に加えて車両の姿勢角分だけさらに傾いているものとして計測する。

いま、地図データベース（ＤＢ）などに記憶されている地図情報中に、地物Ａにおける２つの基準点として地物Ａの下辺の２つの端点の座標が含まれているものとする。この場合、地物Ａを特定することにより、地物Ａの２つの基準点Ａ_ｌ、Ａ_ｒの座標（以下、「地図座標」とも呼ぶ。）Ａ_ｌ（ｙ_Ａｌ，ｚ_Ａｌ）及びＡ_ｒ（ｙ_Ａｒ，ｚ_Ａｒ）を地図情報から取得する。また、地図情報中に基準点Ａ_ｌと点Ａ_ｒとの間の距離Ｒが含まれており、これも地図情報から取得するものとする。

次に、ライダを用いて地物Ａの下辺の２つの端点を計測する。前述のように、ライダを用いた計測により計測対象物である地物Ａの２つの基準点までの距離Ｌ、水平角度α及び垂直角度βが得られるので、式（１）に基づいて２つの基準点の座標を算出する。こうして、ライダの計測により、地物Ａの２つの基準点の座標（以下、「計測座標」とも呼ぶ。）が取得できる。但し、こうして得られた計測座標は、図１（Ｂ）に示すようにボディ座標系における座標であるので、これをワールド座標系における座標に変換する。

ここで、図２に示すように、地図情報から取得した地図座標Ａ_ｌ（ｙ_Ａｌ，ｚ_Ａｌ）とライダの計測により取得した計測座標Ａ’_ｌ（ｙ_Ａ’ｌ，ｚ_Ａ’ｌ）とが一致していると仮定する。この場合、地図座標Ａ_ｒ（ｙ_Ａｒ，ｚ_Ａｒ）と計測座標Ａ’_ｒ（ｙ_Ａ’ｒ，ｚ_Ａ’ｒ）とは、距離ｒだけずれている。よって、地図情報に基づく地図座標Ａ_ｌ（ｙ_Ａｌ，ｚ_Ａｌ）とＡ_ｒ（ｙ_Ａｒ，ｚ_Ａｒ）が作る線分Ａ_ｌＡ_ｒに対して、ライダによる計測で得られた計測座標Ａ’_ｌ（ｙ_Ａ’ｌ，ｚ_Ａ’ｌ）とＡ’_ｒ（ｙ_Ａ’ｒ，ｚ_Ａ’ｒ）が作る線分Ａ’_ｌＡ’_ｒは、角度φだけ傾いていることになる。即ち、ライダを搭載している車両は、地物Ａに対して角度φだけ傾いていることになる。よって、この角度φを車両のロール角として算出する。

具体的には、地図座標Ａ_ｒ（ｙ_Ａｒ，ｚ_Ａｒ）と計測座標Ａ’_ｒ（ｙ_Ａ’ｒ，ｚ_Ａ’ｒ）の距離ｒは以下のように求められる。

上記のように車両の姿勢が傾いており、線分Ａ’_ｌＡ’_ｒが線分Ａ_ｌＡ_ｒに対して傾いていても、地物Ａの２つの基準点間の距離Ｒは不変であり、三角形Ａ_ｌＡ_ｒＡ’_ｒは二等辺三角形となる。よって、図２に示すように点Ａ_ｌから線分Ａ_ｒＡ’_ｒの中点Ｍに垂線を下すと、頂角がφ／２の直角三角形Ａ_ｌＭＡ_ｒができるので、以下の式によりロール角φを求めることができる。

なお、式（３−１）の代わりに、下記の弦の長さを求める公式を用いてもよい。

このように、１つの地物Ａにおける２つの基準点についての地図情報中の地図座標とライダにより計測された計測座標とのずれ量を利用すれば、地物Ａを基準とした車両のロール角を算出することができる。なお、上記の説明では、ＹＺ平面においてロール角を求めているが、同様に、地物Ａの２つの基準点の地図座標及び計測座標を利用して、ＸＹ平面においてヨー角を求めることができ、ＸＺ平面においてピッチ角を求めることができる。

なお、上記の説明においては、ライダによる計測で得られた計測座標Ａ’_ｌが地図座標Ａ_ｌと一致しているものと仮定していた。しかし、実際には計測座標が必ずしも地図座標と一致しないことが多い。そこで、ライダによる計測で得られた２つの計測座標のうちの１つが、対応する地図座標と一致しない場合には、２つの計測座標のうちの１つを、それに対応する地図座標と一致させるように計測座標をシフトする処理が必要となる。

図３は、ライダによる計測により得られた計測座標Ａ’_ｌ（ｙ_Ａ’ｌ，ｚ_Ａ’ｌ）を地図座標Ａ_ｌ（ｙ_Ａｌ，ｚ_Ａｌ）と一致するように、計測座標をシフトする方法を示す。図３において、地図情報から取得した地物Ａの下辺の端点の地図座標がＡ_ｌ（ｙ_Ａｌ，ｚ_Ａｌ）、Ａ_ｒ（ｙ_Ａｒ，ｚ_Ａｒ）であり、ライダによる計測で得られたそれら２点の計測座標がＡ’_ｌ（ｙ_Ａ’ｌ，ｚ_Ａ’ｌ）、Ａ’_ｒ（ｙ_Ａ’ｒ，ｚ_Ａ’ｒ）であるとする。この場合、破線矢印９１に示すように、計測座標Ａ’_ｌ（ｙ_Ａ’ｌ，ｚ_Ａ’ｌ）を地図座標Ａ_ｌ（ｙ_Ａｌ，ｚ_Ａｌ）と一致するようにＹ方向及びＺ方向にシフトする。そして、破線矢印９２に示すように、これと同じシフト量だけ計測座標Ａ’_ｒ（ｙ_Ａ’ｒ，ｚ_Ａ’ｒ）をＹ方向及びＺ方向にシフトすると、下記の式により、シフト後の計測座標Ａ’’_ｒ（ｙ_Ａ’’ｒ，ｚ_Ａ’’ｒ）が得られる。

こうして、２つの計測座標のうちの１つを、対応する地図座標と一致させることができる。この処理を行った後、上記のように、点Ａ_ｌ（ｙ_Ａｌ，ｚ_Ａｌ）、点Ａ_ｒ（ｙ_Ａｒ，ｚ_Ａｒ）、点Ａ’’_ｒ（ｙ_Ａ’’ｒ，ｚ_Ａ’’ｒ）の座標を用いてロール角φを算出すればよい。

上記の例では、地物Ａの下辺の２つの端点を基準点として利用しているが、地図情報に含まれる点の座標であれば、これ以外の点を基準点としても良い。即ち、地物Ａについて、地図情報中に座標が含まれる任意の２点の地図座標と、ライダにより計測されたそれら２点の計測座標とのずれ量を利用して、地物Ａを基準とした車両のロール角を算出することができる。

また、上記の説明では、地図座標Ａ_ｌ（ｙ_Ａｌ，ｚ_Ａｌ）とＡ_ｒ（ｙ_Ａｒ，ｚ_Ａｒ）との間の距離Ｒが地図情報中に含まれているものとしているが、地図情報中に含まれていない場合には、それらの地図座標から下記の式により距離Ｒを求めることができる。

（２）第２の方法
第２の方法は、１つの地物における１つの基準点の地図座標と、別の地物における１つの基準点の地図座標とを利用する。図４は、第２の方法により、ライダによる計測値を用いて車両のロール角を算出する方法を示す。この方法でも、車両の姿勢が水平ではない（ロール角≠０）場合に、地物Ａ、Ｂを、その実際の設置角度に加えて車両の姿勢角の分だけさらに傾いているものとして計測する。

いま、地図ＤＢなどに記憶されている地図情報中に地物Ａの基準点である中央点の地図座標と、別の地物Ｂの基準点である中央点の地図座標とが含まれているものとする。この場合、地物Ａを特定することにより地図情報から地物Ａの基準点の地図座標Ａ（ｙ_Ａ，ｚ_Ａ）を取得し、地物Ｂを特定することにより地図情報から地物Ｂの基準点の地図座標Ｂ（ｙ_Ｂ，ｚ_Ｂ）を取得する。また、地図情報中に基準点Ａと基準点Ｂとの間の距離Ｒが含まれており、これも地図情報から取得するものとする。

次に、ライダを用いて地物Ａの基準点Ａと地物Ｂの基準点Ｂをそれぞれ計測する。前述のように、ライダを用いた計測により計測対象物である地物Ａの基準点Ａ及び地物Ｂの基準点Ｂまでの距離Ｌ、水平角度α及び垂直角度βがそれぞれ得られるので、式（１）に基づいて２つの基準点の座標を算出することができる。こうして、ライダの計測により、地物Ａ、Ｂの基準点の計測座標が取得できる。但し、こうして得られた計測座標は、図１（Ｂ）に示すようにボディ座標系における座標であるので、これをワールド座標系における座標に変換する。こうして、図４に示すように、ワールド座標系における計測座標Ａ’（ｙ_Ａ’，ｚ_Ａ’）及びＢ’（ｙ_Ｂ’，ｚ_Ｂ’）を取得する。

ここで、図４に示すように、地図情報から取得した地図座標Ａ（ｙ_Ａ，ｚ_Ａ）とライダの計測により取得した計測座標Ａ’（ｙ_Ａ’，ｚ_Ａ’）とが一致していると仮定する。一方、地図座標Ｂ（ｙ_Ｂ，ｚ_Ｂ）と計測座標Ｂ’（ｙ_Ｂ’，ｚ_Ｂ’）とは、距離ｒだけずれている。この場合、地図情報に基づく地図座標Ａ（ｙ_Ａ，ｚ_Ａ）とＢ（ｙ_Ｂ，ｚ_Ｂ）が作る線分ＡＢに対して、ライダによる計測で得られた計測座標Ａ’（ｙ_Ａ’，ｚ_Ａ’）とＢ’（ｙ_Ｂ’，ｚ_Ｂ’）が作る線分Ａ’Ｂ’は、角度φだけ傾いていることになる。即ち、ライダを搭載している車両は、地物Ａ、Ｂに対して角度φだけ傾いていることになる。よって、この角度φを車両のロール角として算出することができる。

具体的には、地図座標Ｂ（ｙ_Ｂ，ｚ_Ｂ）と計測座標Ｂ’（ｙ_Ｂ’，ｚ_Ｂ’）の距離ｒは以下のように求められる。

上記のように車両の姿勢が傾いており、線分Ａ’Ｂ’が線分ＡＢに対して傾いていても、地物Ａの基準点Ａと地物Ｂの基準点Ｂとの間の距離Ｒは不変であり、三角形ＡＢＢ’は二等辺三角形となる。よって、図４に示すように点Ａから線分ＢＢ’の中点Ｍに垂線を下すと、頂角がφ／２の直角三角形ＡＭＢができるので、以下の式によりロール角φを求めることができる。

なお、式（８−１）の代わりに、前述の式（４）を用いてもよい。

上記の例では、基準点として、地物Ａと地物Ｂの中央点の座標を基準点として利用しているが、地図情報中に含まれる点の座標であれば、中央点以外の点を基準点としても良い。即ち、地物ＡとＢの各々について、地図情報中に座標が含まれる任意の１点の地図座標と、ライダにより計測されたその１点の計測座標とのずれ量を利用して、地物Ａ、Ｂを基準とした車両のロール角を算出することができる。

また、上記の説明では、地図座標Ａ（ｙ_Ａ，ｚ_Ａ）とＢ（ｙ_Ｂ，ｚ_Ｂ）との間の距離Ｒが地図情報中に含まれているものとしているが、地図情報中に含まれていない場合には、それら地図座標から前述の式（６）により求めることができる。

（測定装置の構成）
図５は、第１実施例に係る測定装置の構成を示す。測定装置１０は、ライダ１１と、自律センサ１２と、地図ＤＢ１３と、制御部１４と、補正部１５とを備える。

ライダ１１は、移動体としての車両に搭載され、周囲の計測対象物として地物を計測する。具体的には、ライダ１１は、図１（Ｂ）に示すように、自己の位置、即ち車両の位置を基準として地物の距離Ｌ、地物の水平角α及び垂直角βを計測する。ライダ１１の計測結果は制御部１４に送られる。自律センサ１２は、例えばジャイロセンサ、加速度センサなどであり、車両の姿勢角、即ち、ロール角、ピッチ角、ヨー角などを計測して補正部１５へ供給する。

地図ＤＢ１３は、地図情報を記憶する。前述のように、地図情報は、地物毎に記憶され、各地物における１点又は複数の点の地図座標を記憶している。また、１つの地物について２点以上の地図座標を記憶している場合、それらの点の間の距離を記憶していても良い。なお、地図座標は、ワールド座標系における座標として記憶されている。

制御部１４は、ＣＰＵ、ＲＯＭ、ＲＡＭなどを備え、予め用意されたプログラムを実行することにより、測定装置１０の全体を制御する。また、制御部１４は、前述のように、地図ＤＢ１３内の地図情報と、ライダ１１により計測された情報とに基づいて、ライダ１１の姿勢角、即ち、ライダ１１を搭載した車両の姿勢角を算出する。算出された姿勢角は補正部１５へ送られる。

補正部１５は、制御部１４により算出された姿勢角を利用して、自律センサ１２により計測された姿勢角を補正し、出力する。出力された姿勢角は、車両の運転制御などに利用される。

上記の構成において、ライダ１１は本発明の地物検知部の一例であり、制御部１４は本発明の第１取得部、第２取得部、変化量算出部及び姿勢角算出部の一例であり、補正部１５は本発明の補正部の一例である。また、時刻ｔにおける計測座標は本発明の第１座標セットに相当し、時刻ｔ＋１における計測座標は本発明の第２座標セットに相当する。

（姿勢角補正処理）
次に、第１実施例に係る姿勢角補正処理について説明する。図６は、姿勢角補正処理のフローチャートである。この処理は、測定装置１０により実行される。

まず、ライダ１１は、車両の周囲の地物を走査し（ステップＳ１１）、走査結果を基に、１つ又は複数の地物における２つの基準点に対する距離及び角度を取得する（ステップＳ１２）。これにより、第１の例では、地物Ａの下辺の２つの端点までの距離Ｌ及び角度α、βが得られる。また、第２の例では、地物Ａの中央点までの距離Ｌ及び角度α、βと、地物Ｂの中央点までの距離Ｌ及び角度α、βが得られる。

次に、制御部１４は、地図ＤＢ１３を参照し、ライダ１１で検出した地物における２つの基準点の地図座標を取得する（ステップＳ１３）。具体的には、制御部１４は、車両の自車位置と、ステップＳ１２で得られた地物までの距離Ｌと、地物に対する角度α、βに基づいて、地図情報に含まれる複数の地物のうちからライダ１１で検出した地物を特定し、その地物について記憶されている地図座標を取得する。これにより、第１の例では、地物Ａの下辺の２つの端点の地図座標Ａ_ｌ（ｙ_Ａｌ，ｚ_Ａｌ）、Ａ_ｒ（ｙ_Ａｒ，ｚ_Ａｒ）が取得される。また、第２の例では、地物Ａの中央点の地図座標Ａ（ｙ_Ａ，ｚ_Ａ）と、地物Ｂの中央点の地図座標Ｂ（ｙ_Ｂ，ｚ_Ｂ）が取得される。なお、制御部１４は、地図情報中に地物における２つの基準点間の距離Ｒが含まれている場合には、距離Ｒも取得する。

次に、制御部１４は、ステップＳ１２で取得した地物の距離Ｌ及び角度α、βに基づいて、図１（Ｂ）を参照して説明したように式（１）を用いて、地物における２つの基準点のボディ座標系における計測座標を取得する（ステップＳ１４）。具体的に、第１の例では、地物Ａの下辺の２つの端点の計測座標が取得される。また、第２の例では、地物Ａの中央点の計測座標と、地物Ｂの中央点の計測座標とが取得される。

次に、制御部１４は、ステップＳ１２で取得したボディ座標系における計測座標を、ワールド座標系における計測座標に変換する（ステップＳ１５）。これにより、第１の方法では、地物Ａの下辺の２つの端点の計測座標Ａ’_ｌ（ｙ_Ａ’ｌ，ｚ_Ａ’ｌ）、Ａ’_ｒ（ｙ_Ａ’ｒ，ｚ_Ａ’ｒ）が得られる。また、第２の方法では、地物Ａの中央点の計測座標Ａ’（ｙ_Ａ’，ｚ_Ａ’）と地物Ｂの中央点の計測座標Ｂ’（ｙ_Ｂ’，ｚ_Ｂ’）とが得られる。

こうして、２つの基準点の地図座標と、それらに対応する計測座標が得られると、制御部１４は、上記の第１の方法又は第２の方法により、車両の姿勢角、より詳しくは車両に搭載されたライダ１１の姿勢角を算出する（ステップＳ１６）。具体的には、制御部１４は、車両のロール角、ピッチ角及びヨー角の少なくとも１つを算出する。

次に、補正部１５は、算出した姿勢角に基づいて、ジャイロセンサなどの自律センサにより得られた姿勢角を補正し、補正後の姿勢角を出力する（ステップＳ１７）。具体的には、ステップＳ１６で算出されたロール角、ピッチ角、又は、ヨー角に基づいて、自律センサにより得られたロール角、ピッチ角、又は、ヨー角を補正し、出力する。

このように、第１実施例の姿勢角補正処理では、１つ又は複数の地物における２つの基準点の地図座標と計測座標とを用いて姿勢角を算出し、算出した姿勢角を用いて、自律センサにより得られた姿勢角を補正することができる。

（変形例）
上記の姿勢角補正処理では、ライダ１１により地物を検出し、その地物における２つの基準点の地図座標を地図ＤＢ１３から取得している。しかし、車両が既知の所定位置に停車したときなどには、そのときに検出される地物は既知であるので、ライダ１１により毎回計測をしなくても地物の基準点の地図座標を取得することができる。例えば、図７に示すように、車両３０が自宅の駐車場などの所定位置Ｐｘに停車したとき、車両の前方には壁や弊などの地物３２が存在すると仮定する。この場合には、ライダ１１により計測を行うことなく、車両の自車位置が駐車場の所定位置Ｐｘと一致したときに、地物３２の基準点Ｐ１、Ｐ２の地図座標を地図ＤＢ１３から読み出すようにすればよい。これにより、例えば運転手が毎日車両に乗車したときに、上記の姿勢角補正処理を実行し、自律センサにより得られる姿勢角を補正することができる。

また、上記の例では、ライダ１１の計測により得られた計測座標をボディ座標系からワールド座標系に変換し、ワールド座標系において姿勢角を算出しているが、その代わりに、地図座標をワールド座標系からボディ座標系に変換し、ボディ座標系において姿勢角を算出してもよい。

［第２実施例］
次に、第２実施例について説明する。第２実施例では、地図座標を利用せず、１つ又は複数の地物における２つの基準点の異なる時刻における計測座標を利用する。即ち、ある時刻における２つの基準点の計測座標と、別の時刻における同じ２つの基準点の計測座標とを用いて姿勢角の変化量を算出する。

図８は、第２実施例により、ライダによる計測値を用いて車両のロール角の変化量を算出する方法を示す。いま、基準点として、地物Ａの中央点と地物Ｂの中央点とを使用する。時刻ｔにおいて、ライダにより地物Ａの基準点Ａ_ｔの計測座標Ａ_ｔ（ｙ_Ａｔ，ｚ_Ａｔ）と、地物Ｂの基準点Ｂ_ｔの計測座標Ｂ_ｔ（ｙ_Ｂｔ，ｚ_Ｂｔ）を取得する。また、別の時刻ｔ＋１において、ライダにより地物Ａの基準点Ａ_ｔ＋１の計測座標Ａ_ｔ＋１（ｙ_Ａｔ＋１，ｚ_Ａｔ＋１）と、地物Ｂの基準点Ｂ_ｔ＋１の計測座標Ｂ_ｔ＋１（ｙ_Ｂｔ＋１，ｚ_Ｂｔ＋１）を取得する。なお、これらの計測座標はボディ座標系における座標である。

ここで、図８に示すように、時刻ｔにおける計測座標Ａ_ｔ（ｙ_Ａｔ，ｚ_Ａｔ）と、時刻ｔ＋１における計測座標Ａ_ｔ＋１（ｙ_Ａｔ＋１，ｚ_Ａｔ＋１）とが一致していると仮定する。一方、時刻ｔにおける計測座標Ｂ_ｔ（ｙ_Ｂｔ，ｚ_Ｂｔ）と、時刻ｔ＋１における計測座標Ｂ_ｔ＋１（ｙ_Ｂｔ＋１，ｚ_Ｂｔ＋１）とは、距離ｒだけずれている。この場合、計測座標Ａ_ｔ（ｙ_Ａｔ，ｚ_Ａｔ）と計測座標Ｂ_ｔ（ｙ_Ｂｔ，ｚ_Ｂｔ）が作る線分Ａ_ｔＢ_ｔに対して、計測座標Ａ_ｔ＋１（ｙ_Ａｔ＋１，ｚ_Ａｔ＋１）と計測座標Ｂ_ｔ＋１（ｙ_Ｂｔ＋１，ｚ_Ｂｔ＋１）が作る線分Ａ_ｔ＋１Ｂ_ｔ＋１は、角度φだけ傾いていることになる。即ち、ライダを搭載している車両は、時刻ｔから時刻ｔ＋１の間に、地物Ａ、Ｂに対して角度φだけ傾いたことになる。よって、この角度φを車両のロール角の変化量として算出する。

具体的に、基準点Ａ_ｔと基準点Ｂ_ｔの距離Ｒは、基準点Ａ_ｔと基準点Ｂ_ｔの計測座標から以下の式により算出することができる。

また、基準点Ｂ_ｔと基準点Ｂ_ｔ＋１の距離ｒは、基準点Ｂ_ｔと基準点Ｂ_ｔ＋１の計測座標から以下の式により算出することができる。

上記のように車両の姿勢が傾いており、線分Ａ_ｔ＋１Ｂ_ｔ＋１が線分Ａ_ｔＢ_ｔに対して傾いていても、２つの基準点間の距離Ｒは不変であり、三角形Ａ_ｔＢ_ｔＢ_ｔ＋１は二等辺三角形となる。よって、図８に示すように点Ａｔから線分Ｂ_ｔＢ_ｔ＋１の中点Ｍに垂線を下すと、頂角がφ／２の直角三角形Ａ_ｔＭＢ_ｔができるので、以下の式によりロール角の変化量Δφを求めることができる。

なお、上記の例では、地物Ａの１つの基準点Ａと地物Ｂの１つの基準点Ｂを用いているが、その代わりに、１つの地物における２つの基準点を用いてもよい。即ち、１つの地物における２つの基準点について、時刻ｔと時刻ｔ＋１における計測座標をそれぞれ求め、それらに基づいてロール角の変化量を求めても良い。

このように、第２実施例によれば、地図情報が無い場合や地図情報中に地物の基準点の地図座標が含まれていない場合でも、１つ又は複数の地物における２つの基準点について異なる時刻において計測された計測座標のずれ量を利用して、車両の相対的なロール角、即ちロール角の変化量を算出することができる。

さらには、地図情報に含まれる地図座標を利用して求めたロール角を基準とすることにより、地図情報に地図座標が含まれない区間においてもロール角を算出することができる。例えば、第１実施例の方法により時刻ｔにおいて任意の地物の地図座標を用いてロール角φ_ｔが算出でき、さらに第２実施例の方法により別の地物の計測座標を用いて時刻ｔと時刻ｔ＋１の間のロール角変化量Δφが算出できたとすると、時刻ｔ＋１におけるロール角φ_ｔ＋１は、
φ_ｔ＋１＝φ_ｔ＋Δφ
と求めることができる。

なお、上記の説明では、ＹＺ平面においてロール角を求めているが、同様に、ＸＹ平面においてヨー角の変化量を求めることができ、ＸＺ平面においてピッチ角の変化量を求めることができる。

（測定装置の構成）
図９は、第２実施例に係る測定装置の構成を示す。測定装置２０は、ライダ２１と、自律センサ２２と、制御部２４と、補正部２５とを備える。図示のように、第２実施例に係る測定装置２０は、第１実施例に係る測定装置１０と比較して、地図ＤＢ１３を有しない点以外は同一である。よって、各構成要素についての説明は省略する。

（姿勢角補正処理）
次に、第２実施例に係る姿勢角補正処理について説明する。図１０は、姿勢角補正処理のフローチャートである。この処理は、測定装置２０により実行される。

まず、ライダ２１は、車両の周囲の地物を走査し（ステップＳ２１）、走査結果を基に、１つ又は複数の地物における２つの基準点に対する距離及び角度を取得する（ステップＳ２２）。これにより、図８の例では、時刻ｔにおける地物Ａ、Ｂの基準点までの距離Ｌ及び角度α、βが得られる。

次に、制御部２４は、ステップＳ２２で取得した地物の距離Ｌ及び角度α、βに基づいて、式（１）を用いて、地物における２つの基準点のボディ座標系における計測座標を取得する（ステップＳ２３）。これにより、図８の例では、時刻ｔにおける地物Ａの基準点の計測座標Ａ_ｔ（ｙ_Ａｔ，ｚ_Ａｔ）と、地物Ｂの基準点Ｂ_ｔの計測座標Ｂ_ｔ（ｙ_Ｂｔ，ｚ_Ｂｔ）とが取得される。

次に、制御部２４は、同一の２つの基準点について、別の時刻の計測座標が取得できているか否かを判定する（ステップＳ２４）。別時刻の計測座標が取得できていない場合（ステップＳ２４：Ｎｏ）、処理はステップＳ２１へ戻り、ステップＳ２１〜Ｓ２３を繰り返す。これにより、別の時刻における計測座標が取得される。図８の例では、時刻ｔ＋１における地物Ａの基準点の計測座標Ａ_ｔ＋１（ｙ_Ａｔ＋１，ｚ_Ａｔ＋１）と、地物Ｂの基準点の計測座標Ｂ_ｔ＋１（ｙ_Ｂｔ＋１，ｚ_Ｂｔ＋１）が取得される。

そして、異なる時刻における２つの基準点の計測座標が取得されると（ステップＳ２４：Ｙｅｓ）、制御部２４は、図８を参照して説明した方法により、車両の姿勢角の変化量を算出する（ステップＳ２６）。具体的には、制御部２４は、車両のロール角の変化量、ピッチ角の変化量及びヨー角変化量の少なくとも１つを算出する。

次に、補正部２５は、算出した姿勢角の変化量に基づいて、ジャイロセンサなどの自律センサにより得られた姿勢角を補正し、補正後の姿勢角を出力する（ステップＳ２６）。具体的には、ステップＳ２５で算出されたロール角の変化量、ピッチ角の変化量、又は、ヨー角の変化量に基づいて、自律センサにより検出されたロール角、ピッチ角、又は、ヨー角を補正し、出力する。

こうして、第２実施例の姿勢角補正処理では、１つ又は複数の地物における２つの基準点についての、異なる時刻における計測座標を用いて姿勢角の変化量を算出し、算出した変化量を用いて、自律センサが検出した姿勢角を補正することができる。

［変形例］
以下、上記の実施例についての各種の変形例について説明する。以下の変形例は、適宜組み合わせて適用することができる。

（変形例１）
第１実施例の第１の方法では、地物の基準点として地物の下辺の２つの端点を用いている。この場合、地物の典型例としては道路標識看板などが挙げられ、その下辺の２つの端点を用いることができる。但し、三角形や四角形の道路標識看板の場合には、下辺に限らず他の辺の２つの端点を用いてもよい。また、地図座標が用意されていれば、地物の外周上に存在する点に限らず、地物の内側に存在する点を用いてもよい。第２実施例において１つの地物における２つの基準点を用いる場合も同様である。

また、第１実施例の第２の方法では、基準点として２つの地物それぞれの中央点を使用しているが、これに限らず、地物の上端、下端など、各平面において地物の外周上に存在する点を用いてもよい。また、地図座標が用意されていれば、中央点以外の地物の内側に存在する点を用いてもよい。第２実施例において２つの地物でそれぞれ１つの基準点を用いる場合も同様である。

（変形例２）
地物の計測に用いるライダは、典型的には図１１（Ａ）に示すように、車両３０の上下方向（Ｚ方向）に延びる円筒状のスキャンエリア３１ａを有するため、ＹＺ平面上でロール角を求める場合、及び、ＸＺ平面上でピッチ角を求める場合に適している。しかし、例えば、図１１（Ｂ）に示すように球状のスキャンエリア３１ｂを有するライダや、図１１（Ｃ）に示すように車両の進行方向（Ｘ方向）に延びる円筒状のスキャンエリア３１ｃを有するライダを用いて車両の上下方向に存在する地物の基準点、例えば高架の床や道路の白線を計測すれば、ＸＹ平面上でのヨー角の算出も適切に行うことができる。

（変形例３）
上記の実施例では、外界センサとしてライダを用いているが、本発明の適用はこれには限られず、カメラなどの他の外界センサを用いても良い。

（変形例４）
第１実施例においては地図ＤＢ中の地図情報から地物の基準点の地図座標を取得しているが、地図ＤＢ自体のデータ構造は特定のものに限定されるものではない。即ち、基準点の地図座標を取得可能であれば、各種のデータ構造の地図情報を利用することができる。

１０、２０ 測定装置
１１、２１ ライダ
１２、２２ 自律センサ
１３ 地図データベース
１４、２４ 制御部
１５、２５ 補正部

Claims (14)

1. 移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する第１取得部と、
   前記地物検知部による前記地物の検知結果に基づき、第２時刻における前記第１座標セットに対応する座標である第２座標セットを取得する第２取得部と、
   前記第１座標セットと前記第２座標セットに基づいて、前記移動体の姿勢角の変化量を算出する変化量算出部と、
   を備えることを特徴とする測定装置。
2. 前記第１座標セット及び前記第２座標セットは、前記地物における２点の同一座標系における座標であることを特徴とする請求項１に記載の測定装置。
3. 前記地物検知部は、当該地物検知部の現在位置を基準とした前記地物の距離及び方向を検出することを特徴とする請求項１又は２に記載の測定装置。
4. 前記第１取得部及び前記第２取得部は、前記地物の距離及び方向に基づき、それぞれ前記第１座標セット及び第２座標セットを算出することを特徴とする請求項３に記載の測定装置。
5. 前記変化量算出部は、前記第１座標セットが示す２点の座標と、前記第２座標セットが示す２点の座標とのずれ量に基づいて前記姿勢角の変化量を算出することを特徴とする請求項１乃至４のいずれか一項に記載の測定装置。
6. 前記２点は、１つの地物における２つの点であることを特徴とする請求項１乃至５のいずれか一項に記載の測定装置。
7. 前記２点は、１つの地物における１点と他の地物における１点であることを特徴とする請求項１乃至５のいずれか一項に記載の測定装置。
8. 前記第１時刻又は前記第２時刻のいずれか一方において取得された前記移動体の姿勢角と、前記変化量とに基づいて、前記第１時刻又は前記第２時刻の他方における前記移動体の姿勢角を算出する姿勢角算出部を備えることを特徴とする請求項１乃至７のいずれか一項に記載の測定装置。
9. 前記変化量算出部が算出した変化量に基づいて、自律センサの出力に基づいて算出された前記移動体の姿勢角を補正する補正部を備えることを特徴とする請求項１乃至８のいずれか一項に記載の測定装置。
10. 前記姿勢角は、ロール角、ピッチ角及びヨー角の少なくとも１つを含むことを特徴とする請求項１乃至９のいずれか一項に記載の測定装置。
11. 前記地物検知部はライダであることを特徴とする請求項１乃至１０のいずれか一項に記載の測定装置。
12. 測定装置によって実行される測定方法であって、
    移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する第１取得工程と、
    前記地物検知部による前記地物の検知結果に基づき、第２時刻における前記第１座標セットに対応する座標である第２座標セットを取得する第２取得工程と、
    前記第１座標セットと前記第２座標セットに基づいて、前記移動体の姿勢角の変化量を算出する変化量算出工程と、
    を備えることを特徴とする測定方法。
13. コンピュータを備える測地装置によって実行されるプログラムであって、
    移動体に搭載される地物検知部による地物の検知結果に基づき、第１時刻における座標である第１座標セットを取得する第１取得部、
    前記地物検知部による前記地物の検知結果に基づき、第２時刻における前記第１座標セットに対応する座標である第２座標セットを取得する第２取得部、
    前記第１座標セットと前記第２座標セットに基づいて、前記移動体の姿勢角の変化量を算出する変化量算出部、
    として前記コンピュータを機能させることを特徴とするプログラム。
14. 請求項１３に記載のプログラムを記憶した記憶媒体。