特開 2023-117251 姿勢推定システム、制御システム、及び姿勢推定方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2023117251

(43)【公開日】2023-08-23

(54)【発明の名称】姿勢推定システム、制御システム、及び姿勢推定方法

(51)【国際特許分類】

   G01B 21/22 20060101AFI20230816BHJP

   G01S 17/931 20200101ALI20230816BHJP

   G01B 11/26 20060101ALI20230816BHJP

【ＦＩ】

G01B21/22

G01S17/931

G01B11/26 H

【審査請求】有

【請求項の数】12

【出願形態】ＯＬ

(21)【出願番号】P 2022019861

(22)【出願日】2022-02-10

(71)【出願人】

【識別番号】314012076

【氏名又は名称】パナソニックＩＰマネジメント株式会社

(74)【代理人】

【識別番号】100109210

【弁理士】

【氏名又は名称】新居 広守

(74)【代理人】

【識別番号】100137235

【弁理士】

【氏名又は名称】寺谷 英作

(74)【代理人】

【識別番号】100131417

【弁理士】

【氏名又は名称】道坂 伸一

(72)【発明者】

【氏名】浦添 和哉

(72)【発明者】

【氏名】向井 裕人

(72)【発明者】

【氏名】今村 隼

【テーマコード（参考）】

2F065

2F069

5J084

【Ｆターム（参考）】

2F065AA31

2F065AA37

2F065BB05

2F065BB15

2F065CC11

2F065DD03

2F065FF04

2F065FF05

2F065FF11

2F065JJ03

2F065JJ05

2F065JJ26

2F065MM06

2F065QQ31

2F069AA71

2F069AA93

2F069BB21

2F069BB24

2F069DD16

2F069DD19

2F069GG04

2F069GG07

2F069GG09

2F069HH09

5J084AA10

5J084AB16

5J084AB20

5J084AC02

5J084BA03

5J084CA17

5J084CA25

5J084DA07

5J084DA08

5J084DA09

5J084EA11

5J084EA22

(57)【要約】

【課題】積載物等による車両の定常的な姿勢のずれを推定すること。
【解決手段】姿勢推定システム１０は、取得部１１と、推定部１２と、出力部１３と、を備える。取得部１１は、車両に取り付けられた第１センサ３１の検知結果を取得する。推定部１２は、取得部１１が取得した第１センサ３１の検知結果に基づいて、第１センサ３１を基準とした車両が移動する路面の相対的な角度である相対角度を算出し、車両の基準姿勢に対する定常的な姿勢のずれを推定する。出力部１３は、推定部１２が推定した上記ずれを示す姿勢情報を出力する。
【選択図】図１

【特許請求の範囲】

【請求項1】

車両に取り付けられた第１センサの検知結果を取得する取得部と、
前記取得部が取得した前記第１センサの検知結果に基づいて、前記第１センサを基準とした前記車両が移動する路面の相対的な角度である相対角度を算出し、前記車両の基準姿勢に対する定常的な姿勢のずれを推定する推定部と、
前記推定部が推定した前記ずれを示す姿勢情報を出力する出力部と、を備える、
姿勢推定システム。

【請求項2】

前記基準姿勢は、前記車両の前記路面に対する相対的な角度に基づく姿勢である、
請求項１に記載の姿勢推定システム。

【請求項3】

前記第１センサは、前記車両の周辺の対象物を検知するセンサであって、
前記推定部は、前記第１センサで検知された前記対象物としての前記路面を示す点群に基づいて、前記相対角度を算出する、
請求項１又は２に記載の姿勢推定システム。

【請求項4】

前記第１センサは複数であって、複数の前記第１センサは、それぞれ互いに異なる方向を向くように前記車両に取り付けられており、
前記推定部は、複数の前記第１センサのそれぞれで検知された複数の前記点群に基づいて、前記相対角度を算出する、
請求項３に記載の姿勢推定システム。

【請求項5】

前記推定部は、前記相対角度の一定時間における分散が所定値を上回る場合、前記ずれを推定しない、又は、前記一定時間における前記相対角度を平均した角度を用いて前記ずれを推定する、
請求項１～４のいずれか１項に記載の姿勢推定システム。

【請求項6】

前記第１センサは少なくとも一対であって、一方の第１センサが前記車両の前方に取り付けられ、他方の第１センサが前記車両の後方に取り付けられており、
前記推定部は、前記一方の第１センサの検知結果に基づく前記相対角度と、前記他方の第１センサの検知結果に基づく前記相対角度と、に基づいて前記車両が勾配に進入しているか否かを判定し、前記車両が前記勾配に進入していると判定した場合、前記ずれを推定しない、
請求項１～４のいずれか１項に記載の姿勢推定システム。

【請求項7】

前記推定部は、前記相対角度に瞬時的な立ち上がり又は立下りが存在する場合、前記ずれを推定しない、
請求項１～４のいずれか１項に記載の姿勢推定システム。

【請求項8】

前記取得部は、前記車両の速度を検知する速度センサの検知結果を更に取得し、
前記推定部は、前記取得部が取得した前記車両の速度の変化が所定の範囲を超える場合、前記ずれを推定しない、
請求項１～４のいずれか１項に記載の姿勢推定システム。

【請求項9】

前記第１センサは、前記車両の周辺の対象物を検知するセンサであって、
前記推定部は、前記第１センサで検知された前記対象物としての前記路面を示す点群に含まれる路面点の数が所定数を下回る場合、前記ずれを推定しない、
請求項１～４のいずれか１項に記載の姿勢推定システム。

【請求項10】

前記車両の周辺における障害物の有無を検知する第２センサに対して、前記第２センサが前記障害物の有無を判定する閾値を指示する制御信号を出力する制御部を更に備え、
前記制御部は、前記姿勢情報が示す前記ずれの大きさに応じて、前記閾値を変化させる、
請求項１～９のいずれか１項に記載の姿勢推定システム。

【請求項11】

車両の基準姿勢に対する定常的な姿勢のずれを示す姿勢情報を取得する入力部と、
前記入力部で取得した前記姿勢情報に基づいて、前記車両の周辺における障害物の有無を検知する第２センサに対して、前記第２センサが前記障害物の有無を判定する閾値を指示する制御信号を出力する制御部と、を備え、
前記制御部は、前記姿勢情報が示す前記ずれの大きさに応じて、前記閾値を変化させる、
制御システム。

【請求項12】

車両に取り付けられた第１センサの検知結果を取得し、
取得した前記第１センサの検知結果に基づいて、前記第１センサを基準とした前記車両が移動する路面の相対的な角度である相対角度を算出し、前記車両の基準姿勢に対する定常的な姿勢のずれを推定する、
姿勢推定方法。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、一般に車両の姿勢を推定する姿勢推定システム、制御システム、及び姿勢推定方法に関する。

【背景技術】

【0002】

特許文献１には、自動車に搭載され、走行中の自動車が走行時に車両周辺に存在する障害物の有無を検知する車両周辺監視システムが開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２０１５－１３５３０１号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

本開示は、積載物等による車両の定常的な姿勢のずれを推定することができる姿勢推定システム等を提供する。

【課題を解決するための手段】

【0005】

本開示の一態様に係る姿勢推定システムは、取得部と、推定部と、出力部と、を備える。前記取得部は、車両に取り付けられた第１センサの検知結果を取得する。前記推定部は、前記取得部が取得した前記第１センサの検知結果に基づいて、前記第１センサを基準とした前記車両が移動する路面の相対的な角度である相対角度を算出し、前記車両の基準姿勢に対する定常的な姿勢のずれを推定する。前記出力部は、前記推定部が推定した前記ずれを示す姿勢情報を出力する。

【0006】

本開示の一態様に係る制御システムは、入力部と、制御部と、を備える。前記入力部は、車両の基準姿勢に対する定常的な姿勢のずれを示す姿勢情報を取得する。前記制御部は、前記入力部で取得した前記姿勢情報に基づいて、前記車両の周辺における障害物の有無を検知する第２センサに対して、前記第２センサが前記障害物の有無を判定する閾値を指示する制御信号を出力する。前記制御部は、前記姿勢情報が示す前記ずれの大きさに応じて、前記閾値を変化させる。

【0007】

本開示の一態様に係る姿勢推定方法では、車両に取り付けられた第１センサの検知結果を取得する。前記姿勢推定方法では、取得した前記第１センサの検知結果に基づいて、前記第１センサを基準とした前記車両が移動する路面の相対的な角度である相対角度を算出し、前記車両の基準姿勢に対する定常的な姿勢のずれを推定する。

【発明の効果】

【0008】

本開示の姿勢推定システム等では、積載物等による車両の定常的な姿勢のずれを推定することができる。

【図面の簡単な説明】

【0009】

【図1】図１は、実施の形態における姿勢推定システムの概要を示すブロック図である。

【図2】図２は、実施の形態における姿勢推定システムの推定部の動作例の説明図である。

【図3】図３は、実施の形態における姿勢推定システムの推定部で生成された３次元画像の一例を示す図である。

【図4】図４は、実施の形態における姿勢推定システムの推定部で生成された、積載物が存在する場合の点群を車両の幅方向から見た平面に投影した図である。

【図5】図５は、実施の形態における車両の路面に対する定常的な姿勢のずれと、相対角度との相関の説明図である。

【図6】図６は、実施の形態における姿勢推定システムの動作例を示すフローチャートである。

【図7】図７は、車両に積載物が存在しない場合の第２センサの動作の説明図である。

【図8】図８は、車両に積載物が存在する場合の第２センサの動作の説明図である。

【図9】図９は、車両に積載物が存在する場合の課題の説明図である。

【図10】図１０は、実施の形態の第１変形例における姿勢推定システムの説明図である。

【図11】図１１は、実施の形態の第２変形例における姿勢推定システムの説明図である。

【図12】図１２は、実施の形態の第３変形例における姿勢推定システムの説明図である。

【図13】図１３は、実施の形態の第４変形例における姿勢推定システムの説明図である。

【図14】図１４は、実施の形態の第５変形例における姿勢推定システムの説明図である。

【図15】図１５は、実施の形態の第６変形例における姿勢推定システムの説明図である。

【発明を実施するための形態】

【0010】

本開示の一態様に係る姿勢推定システムは、取得部と、推定部と、出力部と、を備える。前記取得部は、車両に取り付けられた第１センサの検知結果を取得する。前記推定部は、前記取得部が取得した前記第１センサの検知結果に基づいて、前記第１センサを基準とした前記車両が移動する路面の相対的な角度である相対角度を算出し、前記車両の基準姿勢に対する定常的な姿勢のずれを推定する。前記出力部は、前記推定部が推定した前記ずれを示す姿勢情報を出力する。

【0011】

これによれば、積載物等により車両に定常的な姿勢のずれが生じることに伴って、車両に取り付けられた第１センサも同様に既定位置からずれることから、第１センサの既定位置からのずれに相当する相対角度を算出することで、積載物等による車両の基準姿勢に対する定常的な姿勢のずれを推定することができる、という利点がある。

【0012】

本開示の他の態様に係る姿勢推定システムでは、基準姿勢は、前記車両の前記路面に対する相対的な角度に基づく姿勢である。

【0013】

これによれば、例えば上り勾配又は下り勾配等の路面の種類を問わず、車両の基準姿勢に対する定常的な姿勢のずれを推定することができる、という利点がある。

【0014】

本開示の他の態様に係る姿勢推定システムでは、前記第１センサは、前記車両の周辺の対象物を検知するセンサである。前記推定部は、前記第１センサで検知された前記対象物としての前記路面を示す点群に基づいて、前記相対角度を算出する。

【0015】

これによれば、車両の周辺の状況を把握するために使用される既設のセンサを用いて相対角度を算出することができるので、別途第１センサを用意しなくて済む、という利点がある。

【0016】

本開示の他の態様に係る姿勢推定システムでは、前記第１センサは複数である。複数の前記第１センサは、それぞれ互いに異なる方向を向くように前記車両に取り付けられている。前記推定部は、複数の前記第１センサのそれぞれで検知された複数の前記点群に基づいて、前記相対角度を算出する。

【0017】

これによれば、例えば第１センサの取付時に第１センサが既定位置からずれて取り付けられた場合等、第１センサの意図しない取付位置のずれが生じた場合でも、複数の第１センサの検知結果を用いて総合的に相対角度を算出することができるので、ロバスト性を向上しやすい、という利点がある。

【0018】

本開示の他の態様に係る姿勢推定システムでは、前記推定部は、前記相対角度の一定時間における分散が所定値を上回る場合、前記ずれを推定しない、又は、前記一定時間における前記相対角度を平均した角度を用いて前記ずれを推定する。

【0019】

これによれば、例えば車両が凹凸を有する路面を移動する等して車両の姿勢が瞬時的に変化した場合に、車両の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化するのを防ぎやすい、という利点がある。

【0020】

本開示の他の態様に係る姿勢推定システムでは、前記第１センサは少なくとも一対である。一方の第１センサが前記車両の前方に取り付けられ、他方の第１センサが前記車両の後方に取り付けられている。前記推定部は、前記一方の第１センサの検知結果に基づく前記相対角度と、前記他方の第１センサの検知結果に基づく前記相対角度と、に基づいて前記車両が勾配に進入しているか否かを判定し、前記車両が前記勾配に進入していると判定した場合、前記ずれを推定しない。

【0021】

これによれば、例えば車両が上り勾配又は下り勾配に進入する場合に、車両の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化するのを防ぎやすい、という利点がある。

【0022】

本開示の他の態様に係る姿勢推定システムでは、前記推定部は、前記相対角度に瞬時的な立ち上がり又は立下りが存在する場合、前記ずれを推定しない。

【0023】

これによれば、例えば車両が上り勾配若しくは下り勾配に進入する場合、又は車両が急激に加速若しくは減速する場合に、車両の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化するのを防ぎやすい、という利点がある。

【0024】

本開示の他の態様に係る姿勢推定システムでは、前記取得部は、前記車両の速度を検知する速度センサの検知結果を更に取得する。前記推定部は、前記取得部が取得した前記車両の速度の変化が所定の範囲を超える場合、前記ずれを推定しない。

【0025】

これによれば、例えば車両が急激に減速したり、急激に加速したり等した場合に、車両の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化するのを防ぎやすい、という利点がある。

【0026】

本開示の他の態様に係る姿勢推定システムでは、前記第１センサは、前記車両の周辺の対象物を検知するセンサである。前記推定部は、前記第１センサで検知された前記対象物としての前記路面を示す点群に含まれる路面点の数が所定数を下回る場合、前記ずれを推定しない。

【0027】

これによれば、例えば車両が上り勾配から平坦路に進入する場合に、車両の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化するのを防ぎやすい、という利点がある。

【0028】

本開示の他の態様に係る姿勢推定システムは、前記車両の周辺における障害物の有無を検知する第２センサに対して、前記第２センサが前記障害物の有無を判定する閾値を指示する制御信号を出力する制御部を更に備える。前記制御部は、前記姿勢情報が示す前記ずれの大きさに応じて、前記閾値を変化させる。

【0029】

これによれば、車両の基準姿勢に対する定常的な姿勢のずれの大きさに応じて第２センサの閾値を変化させるので、車両に積載物が存在しない場合の閾値を小さくすることができ、比較的小型の障害物等、反射波の受信電力が小さくなりがちな障害物を検知しやすくなる、という利点がある。

【0030】

本開示の一態様に係る制御システムは、入力部と、制御部と、を備える。前記入力部は、車両の基準姿勢に対する定常的な姿勢のずれを示す姿勢情報を取得する。前記制御部は、前記入力部で取得した前記姿勢情報に基づいて、前記車両の周辺における障害物の有無を検知する第２センサに対して、前記第２センサが前記障害物の有無を判定する閾値を指示する制御信号を出力する。前記制御部は、前記姿勢情報が示す前記ずれの大きさに応じて、前記閾値を変化させる。

【0031】

これによれば、車両の基準姿勢に対する定常的な姿勢のずれの大きさに応じて第２センサの閾値を変化させることで、車両に積載物が存在しない場合の閾値を小さくすることができ、比較的小型の障害物等、反射波の受信電力が小さくなりがちな障害物を検知しやすくなる、という利点がある。

【0032】

本開示の一態様に係る姿勢推定方法では、車両に取り付けられた第１センサの検知結果を取得する。前記姿勢推定方法では、取得した前記第１センサの検知結果に基づいて、前記第１センサを基準とした前記車両が移動する路面の相対的な角度である相対角度を算出し、前記車両の基準姿勢に対する定常的な姿勢のずれを推定する。

【0033】

これによれば、積載物等により車両に定常的な姿勢のずれが生じることに伴って、車両に取り付けられた第１センサも同様に既定位置からずれることから、第１センサの既定位置からのずれに相当する相対角度を算出することで、積載物等による車両の基準姿勢に対する定常的な姿勢のずれを推定することができる、という利点がある。

【0034】

なお、これらの包括的又は具体的な態様は、システム、方法、集積回路、コンピュータプログラム又はコンピュータで読み取り可能なＣＤ－ＲＯＭ等の記録媒体で実現されてもよく、システム、方法、集積回路、コンピュータプログラム又は記録媒体の任意な組み合わせで実現されてもよい。

【0035】

以下、実施の形態について、図面を参照しながら具体的に説明する。なお、以下で説明する実施の形態は、包括的又は具体的な一例を示すものである。以下の実施の形態で示される数値、形状、構成要素、構成要素の配置、ステップ、ステップの順序等は、一例であって本開示を限定するものではない。また、以下の実施の形態における構成要素のうち、独立請求項に記載されていない構成要素については、任意に付加可能な構成要素である。

【0036】

また、各図は、模式図であり、必ずしも厳密に図示されたものではない。また、各図において、同じ構成については同じ符号を付している。

【0037】

＜構成＞
図１は、実施の形態における姿勢推定システム１０の概要を示すブロック図である。図２は、実施の形態における姿勢推定システム１０の動作例の説明図である。

【0038】

実施の形態における姿勢推定システム１０は、自動車等の車両４の姿勢（ピッチ角、ヨー角、及びロール角）を推定するためのシステムである。特に、実施の形態における姿勢推定システム１０は、例えば車両４が凹凸を有する路面５を移動する場合、又は平坦路から上り勾配若しくは下り勾配に進入する場合等における、車両４の基準姿勢に対する瞬時的な姿勢のずれを推定するのではなく、車両４の基準姿勢に対する定常的な姿勢のずれを推定するためのシステムである。実施の形態では、車両４は自動車であることとして説明するが、車両４を自動車に限定する趣旨ではない。車両４は、例えば農業機械又は工業機械等、サスペンション等のばねを有する移動体であってもよい。

【0039】

ここで、基準姿勢とは、車両４が路面５を移動している場合の基準となる姿勢であって、具体的には、車両４の長手方向が路面５と平行である、言い換えればピッチ角が０度である状態をいう。なお、ここでいう「平行」は、完全に平行であることの他、略平行を含む。同様に、ここでいう「０度」は、完全の０度であることの他、略０度を含む。

【0040】

実施の形態では、基準姿勢は、平坦路を基準面とした車両４の基準面に対する絶対的な角度に基づく姿勢ではなく、車両４の路面５に対する相対的な角度に基づく姿勢である。つまり、実施の形態では、基準姿勢は、路面５の勾配に依らない車両４の姿勢となる。したがって、実施の形態では、姿勢推定システム１０は、車両４の路面５に対する相対的かつ定常的な姿勢のずれを推定する。

【0041】

また、ここで、車両４の基準姿勢に対する定常的な姿勢のずれとは、車両４が外的要因により基準姿勢に対して傾いており、この外的要因が取り除かれない間においては、車両４が路面５に対して傾いた状態が恒久的に維持されることをいう。このような車両４の基準姿勢に対する定常的な姿勢のずれは、例えば車両４の一部（例えば、前部、後部、左部、又右部）に積載物７が集中して存在する場合に生じ得る。積載物７は、例えば車両４に搭乗する乗員、又は車両４に積載される荷物等である。なお、図２、及び以降の説明で登場する図面においては、積載物７は、車両４の上に載せ置かれている形で表現されているが、積載物７の態様を限定する趣旨ではない。

【0042】

以下では、車両４の路面５に対する定常的な姿勢のずれとして、車両４の路面５に対するピッチ角を例に挙げて説明する。なお、上記ずれは、ピッチ角に限らず、ロール角、又はヨー角で表されてもよい。例えば、車両４の幅方向における左部又は右部に積載物７が集中して存在する場合、上記ずれは、車両４の路面５に対するロール角で表され得る。

【0043】

姿勢推定システム１０は、例えばメモリ及びプロセッサ（マイクロプロセッサ）を含むコンピュータを備え、メモリに格納された制御プログラムをプロセッサが実行することにより、姿勢推定システム１０の各部を制御して各種機能を実現する。なお、メモリは、プログラム及びデータを予め保持しているＲＯＭ（Read Only Memory）、プログラムの実行に際してデータ等の記憶に利用するためのＲＡＭ（Random Access Memory）等であり、例えば不揮発性メモリを含んでいてもよい。

【0044】

姿勢推定システム１０は、例えば車両４に搭載されている第１センサ３１（後述する）を制御するＥＣＵ（Electronic Control Unit）により実現される。なお、姿勢推定システム１０は、第１センサ３１を制御するＥＣＵに限らず、他のＥＣＵにより実現されてもよい。また、姿勢推定システム１０は、ＥＣＵとは別の独立した信号処理回路で実現されてもよい。

【0045】

図１に示すように、姿勢推定システム１０は、取得部１１と、推定部１２と、出力部１３と、を備えている。

【0046】

取得部１１は、車両４に取り付けられた第１センサ３１の検知結果を取得する。第１センサ３１は、車両４の周辺の対象物を検知するセンサである。第１センサ３１は、例えば先進運転支援システム（ADAS：Advanced driver-assistance systems）用のセンサであって、車両４の内部又は外部に搭載され、逐次（例えば、１／６０秒等の所定時間ごとに）、車両４の周辺の対象物を検知する。第１センサ３１は、例えば車両４の周辺の光等を検知するイメージセンサ（カメラ）である。カメラは、ステレオカメラであってもよいし、単眼カメラであってもよい。また、第１センサ３１は、例えば電磁波の反射を検知するレーダ、又はレーザ光の反射を検知するＬｉＤＡＲ（Light Detection and Ranging）等であってもよい。

【0047】

推定部１２は、取得部１１が取得した第１センサ３１の検知結果に基づいて、第１センサ３１を基準とした車両４が移動する路面５の相対的な角度である相対角度θ１（図５参照）を算出し、車両４の基準姿勢に対する定常的な姿勢のずれを推定する。実施の形態では、推定部１２は、第１センサ３１で検知された対象物としての路面５を示す点群に基づいて、相対角度θ１を算出する。

【0048】

以下、推定部１２で実行する処理について具体的に説明する。図２は、実施の形態における姿勢推定システム１０の推定部１２の動作例の説明図である。図２においては、車両４は、前部に積載物７が存在することにより、常時、前部が路面５に近づく向きに傾いた姿勢となっている。このため、図２においては、第１センサ３１の位置は、車両４が基準姿勢である場合の既定位置３１０から高さ方向及びピッチ方向に定常的にずれている。

【0049】

第１センサ３１は、単眼カメラ又はステレオカメラであって、車両４の前方を検知領域としていることとする。そして、推定部１２は、第１センサ３１で撮像した画像に対してＳｆＭ（Structure From Motion）及びＳＬＡＭ（Simultaneous Localization and Mapping）等の画像処理を実行することにより、車両４の周辺（ここでは、車両４の前方）の３次元画像及び点群データを生成することとする。以下では、点群データのうちの路面５上の任意の１点である路面点６１に焦点を当てて説明する。

【0050】

以下では、第１センサ３１が単眼カメラであって、時系列でのステレオマッチングを行うことにより３次元画像上に投影される路面点６１０の位置を算出する例について説明する。なお、ここでは説明を省略するが、第１センサ３１がステレオカメラである場合も、非時系列でのステレオマッチングを行うことにより、３次元画像上に投影される路面点６１０の位置を算出することが可能である。

【0051】

まず、推定部１２は、時刻ｔで第１センサ３１により撮像された路面点６１と、時刻ｔ＋１で第１センサ３１により撮像された路面点６１と、をマッチングする。ここで、時刻ｔと時刻ｔ＋１との間隔は、第１センサ３１のフレームレートに依存する。例えば、時刻ｔと時刻ｔ＋１との間隔は、１／６０秒である。

【0052】

次に、推定部１２は、時刻ｔでの第１センサ３１の位置と、時刻ｔ＋１での第１センサ３１の位置と、マッチングした路面点６１と、の３点で三角測量を実行することにより、これら３点の相対的な位置関係を算出する。そして、推定部１２は、算出した位置関係と、時刻ｔでの第１センサ３１の既定位置３１０と、時刻ｔ＋１での第１センサ３１の既定位置３１０と、に基づいて、３次元画像上に投影される路面点６１０の位置を算出する。

【0053】

図３は、実施の形態における姿勢推定システム１０の推定部１２で生成された３次元画像の一例を示す図である。図３の（ａ）は、車両４に積載物７が存在しない場合の３次元画像を示しており、図３の（ｂ）は、車両４に積載物７が存在する場合の３次元画像を示している。また、図３の（ａ）、（ｂ）の各々においては、点群６０は、多数の路面点６１０の集合を表している。さらに、図３の（ａ）、（ｂ）においては、３次元画像には路面５上に存在する棒状の物体８が投影されている。

【0054】

図３の（ｂ）に示すように、車両４に積載物７が存在することによる第１センサ３１の既定位置３１０からのずれは、３次元画像において修正されずに反映されている。すなわち、上記ずれが修正されていれば、図３の（ａ）に示すような３次元画像になるところ、上記ずれが修正されていないため、図３の（ｂ）に示すような３次元画像となる。上記ずれは、図３の（ａ）における物体８の先端の位置と、図３の（ｂ）における物体８の先端の位置との差分に相当する。

【0055】

ここで、第１センサ３１の既定位置３１０からのずれは、３次元画像に投影された物体８のみならず、３次元画像に投影された点群６０にも反映される。具体的には、図３の（ｂ）に示す３次元画像における点群６０は、車両４の幅方向から見ると、図４に示すような画像で表される。図４は、実施の形態における姿勢推定システム１０の推定部１２で生成された、積載物７が存在する場合の点群６０を車両４の幅方向から見た平面に投影した図である。図４において、横軸は路面５に沿った水平方向であり、縦軸は路面５と垂直な方向である。また、図４において、点群６０のうちの点群６０Ａは、３次元画像において路面５を示す点群であり、点群６０Ｂは、３次元画像において物体８を示す点群である。

【0056】

図４に示すように、３次元画像において路面５を示す点群６０Ａは、車両４の幅方向から見ると、前方に向かうにつれて横軸から離れるように平面上に投影される。この点群６０Ａは、車両４の前方へ向かうにつれて路面５から離れる向きに傾斜する直線Ｌ１（又は平面）に近似される。

【0057】

図５は、実施の形態における車両４の路面５に対する定常的な姿勢のずれと、相対角度θ１との相関の説明図である。図５に示すように、直線Ｌ１は、第１センサ３１が既定位置３１０にあると仮定した場合に３次元画像に投影される路面５の傾きを表している。この直線Ｌ１の傾きから、直線Ｌ１と実際の路面５とのなす角度θ１、つまり第１センサ３１を基準とした路面５に対する相対的な角度である相対角度θ１が算出される。

【0058】

そして、第１センサ３１の既定位置３１０からのずれ（相対角度θ１）は、車両４に積載物７が存在することによる車両４の路面５に対する相対的かつ定常的な姿勢のずれ（車両４の路面５に対するピッチ角）に相当する。

【0059】

したがって、推定部１２は、第１センサ３１の検知結果に基づいて相対角度θ１を算出することにより、車両４の基準姿勢に対する定常的な姿勢のずれを推定することが可能である。特に、実施の形態では、推定部１２は、上記ずれとして、車両４の路面５に対する相対的かつ定常的な姿勢のずれを推定することになる。

【0060】

出力部１３は、推定部１２が推定した上記ずれ、すなわち車両４の基準姿勢に対する定常的な姿勢のずれを示す姿勢情報を出力する。実施の形態では、出力部１３は、姿勢情報として上記の相対角度θ１を出力する。また、実施の形態では、出力部１３は、図１に示すように、制御システム２０に対して姿勢情報を出力する。

【0061】

制御システム２０は、例えばメモリ及びプロセッサ（マイクロプロセッサ）を含むコンピュータを備え、メモリに格納された制御プログラムをプロセッサが実行することにより、制御システム２０の各部を制御して各種機能を実現する。なお、メモリは、プログラム及びデータを予め保持しているＲＯＭ、プログラムの実行に際してデータ等の記憶に利用するためのＲＡＭ等であり、例えば不揮発性メモリを含んでいてもよい。

【0062】

制御システム２０は、例えば車両４に搭載されている第２センサ３２を制御するＥＣＵにより実現される。なお、制御システム２０は、第２センサ３２を制御するＥＣＵに限らず、他のＥＣＵにより実現されてもよい。また、制御システム２０は、ＥＣＵとは別の独立した信号処理回路で実現されてもよい。

【0063】

第２センサ３２は、車両４の周辺における障害物の有無を検知するセンサである。第２センサ３２は、例えば車両４の内部又は外部に搭載され、逐次（例えば、１／６０秒等の所定時間ごとに）、車両４の周辺の障害物の有無を検知する。第２センサ３２は、例えばＴｏＦ（Time of Flight）センサであって、検知領域に対して送信波を放射し、かつ、検知領域に存在する物体からの反射波を受信することで、障害物の有無を検知する。実施の形態では、第２センサ３２は、超音波を送信波とするソナーである。また、第２センサ３２は、例えば電磁波を送信波とするレーダ、又はレーザ光を送信波とする２次元ＬｉＤＡＲ等であってもよい。

【0064】

制御システム２０は、図１に示すように、入力部２１と、制御部２２と、を備えている。

【0065】

入力部２１は、車両４の基準姿勢に対する定常的な姿勢のずれを示す姿勢情報を取得する。実施の形態では、入力部２１は、姿勢推定システム１０の出力部１３から出力される姿勢情報を取得する。

【0066】

制御部２２は、入力部２１で取得した姿勢情報に基づいて、第２センサ３２に対して、第２センサ３２が障害物の有無を判定する閾値を指示する制御信号を出力する。

【0067】

ここで、第２センサ３２の閾値について説明する。既に述べたように、第２センサ３２は、検知領域に対して送信波を放射し、かつ、検知領域に存在する物体からの反射波を受信することで、障害物の有無を検知するが、物体には、障害物の他に、路面５も含まれる。つまり、第２センサ３２は、障害物で反射した反射波の他に、路面５で反射した反射波も受信する。このため、第２センサ３２は、反射波の受信電力と閾値とを比較することにより、障害物と路面５とを区別して検知するように構成されている。すなわち、基本的に、障害物からの反射波の受信電力は、路面５からの反射波の受信電力よりも大きくなるため、受信電力に対して適宜閾値を設定することで、障害物と路面５とを区別することが可能である。

【0068】

制御部２２は、姿勢情報が示す上記ずれの大きさに応じて、閾値を変化させる。実施の形態では、制御部２２は、相対角度θ１が０度である場合の閾値を基準閾値として、上記ずれ、つまり相対角度θ１が大きくなればなる程、閾値を基準閾値よりも大きくする。このような制御部２２の処理については、後述する＜利点＞にて詳細に説明する。

【0069】

＜動作＞
以下、姿勢推定システム１０の動作例（言い換えれば、姿勢推定方法の例）について、図６を参照して説明する。図６は、実施の形態における姿勢推定システム１０の動作例を示すフローチャートである。まず、取得部１１は、第１センサ３１から定期的に検知結果を取得する（Ｓ１）。

【0070】

次に、推定部１２は、取得部１１が取得した第１センサ３１の検知結果に基づいて、相対角度θ１を算出する（Ｓ２）。具体的には、推定部１２は、第１センサ３１で検知された対象物としての路面５を示す点群６０に基づいて、第１センサ３１が既定位置３１０にあると仮定した場合に３次元画像に投影される路面５の傾きを示す直線Ｌ１を算出する。そして、推定部１２は、算出した直線Ｌ１の傾きから、直線Ｌ１と実際の路面５とのなす角度、つまり相対角度θ１を算出する。

【0071】

そして、推定部１２は、算出した相対角度θ１から、車両４の基準姿勢に対する定常的な姿勢のずれを推定する（Ｓ３）。具体的には、推定部１２は、算出した相対角度θ１を車両４の路面５に対する相対的かつ定常的な姿勢のずれ（ピッチ角）とする。その後、出力部１３は、推定部１２が推定した上記ずれ、すなわち車両４の基準姿勢に対する定常的な姿勢のずれを示す姿勢情報を出力する（Ｓ４）。具体的には、出力部１３は、推定部１２で算出された相対角度θ１を、姿勢情報として制御システム２０に対して出力する。

【0072】

＜利点＞
以下、実施の形態における姿勢推定システム１０の利点について説明する。まず、姿勢推定システム１０により、車両４の基準姿勢に対する定常的な姿勢のずれを推定するに至った背景について、図７～図９を用いて説明する。図７は、車両４に積載物７が存在しない場合の第２センサ３２の動作の説明図である。図８は、車両４に積載物７が存在する場合の第２センサ３２の動作の説明図である。図９は、車両４に積載物７が存在する場合の課題の説明図である。図９において、横軸は時間、縦軸は第２センサ３２で受信した反射波の受信電力を表している。

【0073】

図７及び図８の各々に示すように、第２センサ３２は、送信波を前方に放射し、路面点６１からの反射波を受信していることとして説明する。ここで、図７に示すように、第２センサ３２の中心を通り、かつ、第２センサ３２の前方において他の方向と比較して送信強度及び受信強度が高くなる中心軸（同図の破線参照）と、第２センサ３２の中心及び路面点６１を結ぶ直線とのなす角度を「θ２」とする。同様に、図８に示すように、第２センサ３２の中心を通り、かつ、第２センサ３２の前方において他の方向と比較して送信強度及び受信強度が高くなる中心軸（同図の破線参照）と、第２センサ３２の中心及び路面点６１を結ぶ直線とのなす角度を「θ３」とする。

【0074】

図８に示すように車両４に積載物７が存在することで車両４の姿勢が定常的にずれている場合、図７に示すように車両４に積載物７が存在せずに車両４が基準姿勢を維持している場合と比較して、角度θ３は角度θ２よりも小さくなる。このため、同じ路面点６１からの反射波であっても、車両４の姿勢が定常的にずれている場合に第２センサ３２が受信する反射波の受信電力（図９の破線の波線参照）は、車両４が基準姿勢を維持している場合の受信電力（図９の実線の波線参照）よりも大きくなる。

【0075】

既に述べたように、第２センサ３２は、反射波の受信電力と閾値とを比較することにより、障害物と路面５とを区別して検知するように構成されている。この閾値が、例えば図９に示すように第１閾値Ｔｈ１である、と仮定する。図７に示すように車両４が基準姿勢を維持している場合であれば、路面５の路面点６１からの反射波の受信電力は第１閾値Ｔｈ１よりも十分に小さいため、第２センサ３２は、障害物と路面５とを明確に区別して検知することが可能である。

【0076】

しかしながら、図８に示すように車両４の姿勢が定常的にずれている場合であれば、路面５の路面点６１からの反射波の受信電力は閾値Ｔｈ１と殆ど同等、又は閾値Ｔｈ１を上回り得る。このため、第２センサ３２は、路面５を誤って障害物であると検知する可能性がある。

【0077】

上記の課題を解消するために、例えば車両４の姿勢が定常的にずれる場合を想定して、閾値を大きくすることが考えられる。具体的には、図９に示すように、閾値を第１閾値Ｔｈ１よりも大きい第２閾値Ｔｈ２にすることが考えられる。この場合、車両４の姿勢が定常的にずれている場合においても、路面５の路面点６１からの反射波の受信電力は第２閾値Ｔｈ２よりも十分に小さいため、第２センサ３２は、障害物と路面５とを明確に区別して検知することが可能である。

【0078】

しかしながら、上述のように単に閾値を大きくした場合、以下に示す新たな課題が生じる。すなわち、比較的小型の障害物等、反射波の受信電力が小さくなりがちな障害物が路面５上に存在する場合、障害物からの反射波の受信電力が第２閾値Ｔｈ２を上回らず、第２センサ３２が障害物を検知することができなくなる、という課題が生じる。

【0079】

これに対して、実施の形態における姿勢推定システム１０では、積載物７等により車両４に定常的な姿勢のずれが生じることに伴って、車両４に取り付けられた第１センサ３１も同様に既定位置３１０からずれることから、第１センサ３１の既定位置３１０からのずれに相当する相対角度θ１を算出することで、積載物７等による車両４の基準姿勢に対する定常的な姿勢のずれを推定することができる、という利点がある。

【0080】

そして、制御システム２０は、姿勢推定システム１０で推定された上記ずれを参照すれば、上記の課題を解消することが可能である。すなわち、上記ずれの大きさに応じて第２センサ３２の閾値を変化させることで、車両４に積載物７が存在せずに車両４が基準姿勢を維持している場合の閾値を小さくすることができ、比較的小型の障害物等、反射波の受信電力が小さくなりがちな障害物を検知しやすくなる、という利点がある。

【0081】

例えば、図９に示す例であれば、姿勢推定システム１０で推定された上記ずれが小さい、又は上記ずれが無い場合に第２センサ３２の閾値を第１閾値Ｔｈ１とし、姿勢推定システム１０で推定された上記ずれが大きい場合に第２センサ３２の閾値を第２閾値Ｔｈ２とする。このように上記ずれの大きさに応じて第２センサ３２の閾値を動的に変化させることで、車両４の姿勢が定常的にずれている場合においても、第２センサ３２は障害物と路面５とを明確に区別して検知することができる。また、車両４が基準姿勢を維持している場合においても、第２センサ３２は反射波の受信電力が小さくなりがちな障害物を検知しやすくなる。

【0082】

＜変形例＞
以下、本開示に係る姿勢推定システムについて、上記実施の形態に基づいて説明したが、本開示は、上記実施の形態に限定されるものではない。本開示の趣旨を逸脱しない限り、当業者が思い付く各種変形を上記実施の形態に施したものも、本開示の範囲内に含まれてもよい。

【0083】

＜第１変形例＞
図１０は、実施の形態の第１変形例における姿勢推定システム１０の説明図である。図１０の（ａ）は、積載物７が存在する車両４が、凹凸を有する路面５を移動している様子を表している。図１０の（ｂ）は、上記車両４が凹凸を有する路面５を移動する際の相対角度θ１の時系列変化を表している。図１０の（ｂ）に示すように、車両４が凹凸を有する路面５を移動する際に、相対角度θ１には、瞬時的な変化が表れる（同図の丸印を参照）。このような相対角度θ１の瞬時的な変化、つまり車両４の姿勢の瞬時的な変化を含めた姿勢情報を制御システム２０に出力した場合、車両４の姿勢の瞬時的な変化に伴って第２センサ３２の閾値が変化してしまい、障害物の検知性能に影響を及ぼし得る。

【0084】

そこで、第１変形例における姿勢推定システム１０では、推定部１２は、相対角度θ１の一定時間における分散が所定値を上回るか否かを判定することにより、車両４の姿勢に瞬時的な変化が表れているか否かを判定する。そして、推定部１２は、相対角度θ１の一定時間における分散が所定値を上回る場合、相対角度θ１を推定しない、つまり車両４の基準姿勢に対する定常的な姿勢のずれを推定しない。または、推定部１２は、上記の場合、上記一定時間における相対角度θ１を平均した角度を用いて、上記ずれを推定する。ここで、「ずれを推定しない」とは、そもそも上記ずれを推定する処理を実行しないことを含む他、上記ずれを推定するが推定結果を棄却することを含み得る。

【0085】

第１変形例によれば、例えば車両４が凹凸を有する路面５を移動する等して車両４の姿勢が瞬時的に変化した場合に、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化するのを防ぎやすい、という利点がある。特に、第１変形例によれば、相対角度θ１の瞬時的な変化を除いた姿勢情報を制御システム２０に出力するため、車両４の姿勢の瞬時的な変化に伴って第２センサ３２の閾値が変化せず、障害物の検知性能に影響を及ぼしにくい。

【0086】

＜第２変形例＞
図１１は、実施の形態の第２変形例における姿勢推定システム１０の説明図である。図１１は、積載物７が存在する車両４が、路面５の上り勾配に進入する様子を表している。このように車両４が路面５の上り勾配（又は下り勾配）に進入する際に、相対角度θ１には、車両４が凹凸を有する路面５を移動する場合と比較すると緩やかではあるが、瞬時的な変化が表れる。このような相対角度θ１の瞬時的な変化、つまり車両４の姿勢の瞬時的な変化を含めた姿勢情報を制御システム２０に出力した場合、車両４の姿勢の瞬時的な変化に伴って第２センサ３２の閾値が変化してしまい、障害物の検知性能に影響を及ぼし得る。

【0087】

そこで、第２変形例における姿勢推定システム１０では、図１１に示すように、車両４に少なくとも一対の第１センサ３１が取り付けられており、一方の第１センサ３１が車両４の前方に取り付けられ、他方の第１センサ３１が車両４の後方に取り付けられていることを前提として、上記の課題を解消する。すなわち、推定部１２は、一方の第１センサ３１の検知結果に基づく相対角度θ１と、他方の第１センサ３１の検知結果に基づく相対角度θ１とに基づいて、車両４が勾配に進入しているか否かを判定する。

【0088】

ここで、車両４が勾配に進入すると、車両４が勾配に進入する前と比較して、一方の第１センサ３１の検知結果に基づく相対角度θ１と、他方の第１センサ３１の検知結果に基づく相対角度θ１との差分に変化が生じる。推定部１２は、上記差分が所定値を上回るか否かにより、車両４が勾配に進入しているか否かを判定する。そして、推定部１２は、上記差分が所定値を上回る場合、つまり車両４が勾配に進入していると判定した場合、相対角度θ１を推定しない、つまり車両４の基準姿勢に対する定常的な姿勢のずれを推定しない。

【0089】

第２変形例によれば、例えば車両４が上り勾配又は下り勾配に進入する場合に、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化するのを防ぎやすい、という利点がある。特に、第２変形例によれば、第１変形例と同様に、車両４の姿勢の瞬時的な変化に伴って第２センサ３２の閾値が変化せず、障害物の検知性能に影響を及ぼしにくい。

【0090】

＜第３変形例＞
図１２は、実施の形態の第３変形例における姿勢推定システム１０の説明図である。図１２の（ａ）は、積載物７が存在する車両４が勾配（ここでは、上り勾配）に進入する際の相対角度θ１の時系列変化を表している。図１２の（ｂ）は、上記車両４が急激に加速し、その後、急激に減速する際の相対角度θ１の時系列変化を表している。図１２の（ａ）、（ｂ）に示すように、車両４が上り勾配に進入する場合、又は車両４が急激に加速若しくは減速する場合、相対角度θ１には、瞬時的な立ち上がり又は立ち下がりが表れる（同図の丸印を参照）。このような相対角度θ１の瞬時的な変化、つまり車両４の姿勢の瞬時的な変化を含めた姿勢情報を制御システム２０に出力した場合、車両４の姿勢の瞬時的な変化に伴って第２センサ３２の閾値が変化してしまい、障害物の検知性能に影響を及ぼし得る。

【0091】

そこで、第３変形例における姿勢推定システム１０では、推定部１２は、相対角度θ１に瞬時的な立ち上がり又は立下りが存在するか否かを判定することにより、車両４が勾配に進入しているか否か、又は車両４が急激に加速又は減速をしているか否かを判定する。例えば、推定部１２は、相対角度θ１が第１所定値を上回る状況が比較的短い所定時間において生じた場合、相対角度θ１に瞬時的な立ち上がりが存在すると判定する。また、例えば、推定部１２は、相対角度θ１が第２所定値（＜第１所定値）を下回る状況が比較的短い所定時間において生じた場合、相対角度θ１に瞬時的な立ち下がりが存在すると判定する。そして、推定部１２は、相対角度θ１に瞬時的な立ち上がり又は立ち下がりが存在する場合、相対角度θ１を推定しない、つまり車両４の基準姿勢に対する定常的な姿勢のずれを推定しない。

【0092】

第３変形例によれば、例えば車両４が上り勾配若しくは下り勾配に進入する場合、又は車両４が急激に加速若しくは減速する場合に、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化するのを防ぎやすい、という利点がある。特に、第３変形例によれば、第１変形例又は第２変形例と同様に、車両４の姿勢の瞬時的な変化に伴って第２センサ３２の閾値が変化せず、障害物の検知性能に影響を及ぼしにくい。

【0093】

＜第４変形例＞
図１３は、実施の形態の第４変形例における姿勢推定システム１０の説明図である。図１３の（ａ）は、積載物７が存在する車両４が、急ブレーキにより急激に減速する様子を表している。図１３の（ｂ）は、上記車両４が急激に加速し、その後、急激に減速する際の相対角度θ１の時系列変化を表している。図１３の（ｂ）に示すように、車両４が急激に減速したり、急激に加速したりする際に、相対角度θ１には、瞬時的な変化が表れる（同図の丸印参照）。このような相対角度θ１の瞬時的な変化、つまり車両４の姿勢の瞬時的な変化を含めた姿勢情報を制御システム２０に出力した場合、車両４の姿勢の瞬時的な変化に伴って第２センサ３２の閾値が変化してしまい、障害物の検知性能に影響を及ぼし得る。

【0094】

そこで、第４変形例における姿勢推定システム１０では、車両４の速度を検知する速度センサ（例えば、加速度センサ）が取り付けられていることを前提として、上記の課題を解消する。すなわち、取得部１１は、車両４の速度を検知する速度センサの検知結果を更に取得する。また、推定部１２は、取得部１１が取得した車両４の速度の変化が所定の範囲を超えるか否かを判定することにより、車両４が急激に加速又は減速をしているか否かを判定する。そして、推定部１２は、車両４の速度の変化が所定の範囲を超える場合、つまり車両４が急激に加速又は減速していると判定した場合、相対角度θ１を推定しない、つまり車両４の基準姿勢に対する定常的な姿勢のずれを推定しない。

【0095】

第４変形例によれば、例えば車両４が急激に減速したり、急激に加速したり等した場合に、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化するのを防ぎやすい、という利点がある。特に、第４変形例によれば、第１変形例～第３変形例と同様に、車両４の姿勢の瞬時的な変化に伴って第２センサ３２の閾値が変化せず、障害物の検知性能に影響を及ぼしにくい。

【0096】

＜第５変形例＞
図１４は、実施の形態の第５変形例における姿勢推定システム１０の説明図である。図１４の（ａ）は、第１センサ３１が、取付時において既定位置３１０からずれて車両４に取り付けられている様子を表している。図１４の（ｂ）は、車両４を上方から見た平面図であって、車両４の前後左右のそれぞれに第１センサ３１が取り付けられている様子を表している。

【0097】

図１４の（ａ）に示すように、第１センサ３１は、車両４に取り付けられる際に、既定位置３１０からずれて取り付けられる場合がある。このような場合、推定部１２が第１センサ３１の既定位置３１０を参照しても、第１センサ３１の実際の取付位置とずれているため、相対角度θ１の算出精度が劣化し、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化する可能性がある。

【0098】

そこで、第５変形例における姿勢推定システム１０では、図１４の（ｂ）に示すように、車両４の前後左右のそれぞれに第１センサ３１が取り付けられている、言い換えれば複数の第１センサ３１がそれぞれ互いに異なる方向を向くように車両４に取り付けられていることを前提として、上記の問題を解消する。すなわち、推定部１２は、複数の第１センサ３１のそれぞれで検知された複数の点群６０に基づいて、相対角度θ１を算出する。

【0099】

具体的には、推定部１２は、１つの第１センサ３１で検知された点群６０を車両４の幅方向から見た平面に投影するのではなく、全ての第１センサ３１でそれぞれ検知された全ての点群６０を車両４の幅方向から見た平面に投影し、これらの点群６０から直線Ｌ１を算出する。そして、推定部１２は、算出した直線Ｌ１の傾きから、相対角度θ１を算出する。

【0100】

第５変形例によれば、複数の第１センサ３１のそれぞれで検知された点群６０を用いて相対角度θ１を算出するため、いずれかの第１センサ３１が既定位置３１０からずれて取り付けられた場合であっても、相対角度θ１の算出結果に影響を及ぼしにくい。つまり、第５変形例によれば、例えば第１センサ３１の取付時に第１センサ３１が既定位置３１０からずれて取り付けられた場合等、第１センサ３１の意図しない取付位置のずれが生じた場合でも、複数の第１センサ３１の検知結果を用いて総合的に相対角度θ１を算出することができるので、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化しにくく、ロバスト性を向上しやすい、という利点がある。

【0101】

＜第６変形例＞
図１５は、実施の形態の第６変形例における姿勢推定システム１０の説明図である。図１５は、積載物７が存在する車両４が、上り勾配から平坦路へと進入する際の様子を表している。図１５に示すように、車両４が上り勾配から平坦路へと進入する際には、第１センサ３１が路面５よりも上方の空間を検知領域とするため、第１センサ３１が検知する路面５の領域が狭くなる。このため、第１センサ３１で検知される路面５を示す点群６０に含まれる路面点６１の数が少なくなり、相対角度θ１の算出精度が劣化し、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化する可能性がある。

【0102】

そこで、第６変形例における姿勢推定システム１０では、推定部１２は、第１センサ３１で検知された対象物としての路面５を示す点群６０に含まれる路面点６１の数が所定数を下回るか否かを判定する。そして、推定部１２は、路面点６１の数が所定数を下回る場合、相対角度θ１を推定しない、つまり車両４の基準姿勢に対する定常的な姿勢のずれを推定しない。

【0103】

第６変形例によれば、路面５を示す点群６０に含まれる路面点６１の数が、相対角度θ１を算出するために十分な数に達していない場合に上記ずれを推定せず、相対角度θ１を算出するために十分な数に達している場合のみ上記ずれを推定する。このため、第６変形例によれば、例えば車両４が上り勾配から平坦路に進入する場合に、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化するのを防ぎやすい、という利点がある。

【0104】

＜その他の変形例＞
実施の形態では、第１センサ３１及び第２センサ３２は、車両４の前方を検知領域とするように車両４に取り付けられているが、これに限られない。例えば、第１センサ３１及び第２センサ３２は、車両４の後方を検知領域とするように車両４に取り付けられてもよいし、車両４の左方又は右方を検知領域とするように車両４に取り付けられてもよい。第１センサ３１及び第２センサ３２が車両４の左方又は右方を検知領域とするように車両４に取り付けられた場合、積載物７による車両４のロール方向における定常的な姿勢のずれを推定することが可能である。つまり、第１センサ３１及び第２センサ３２は、車両４に対する前後左右の四方向のうちの少なくとも１つの方向を検知領域とするように車両４に取り付けられていればよい。もちろん、複数の方向を検知領域とするように複数の第１センサ３１及び複数の第２センサ３２が車両４に取り付けられていてもよい。

【0105】

実施の形態において、推定部１２は、車両４の角速度を検知することにより、車両４の姿勢を検知するジャイロセンサ等の姿勢センサの検知結果を更に参照してもよい。この場合、姿勢センサの検知結果は、平坦路を基準面とした車両４の基準面に対する絶対的な角度となる。そして、推定部１２は、車両４の基準面に対する絶対的な角度を更に参照することにより、第１センサ３１で検知された点群６０を、路面５上の点群か、路面５以外の点群かに分類する機能を動的に制御又は調整してもよい。この態様では、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度を向上することが期待できる。

【0106】

また、推定部１２は、姿勢センサの検知結果に基づいて、車両４の瞬時的な姿勢のずれが発生する状況を特定し、特定した状況においては、車両４の基準姿勢に対する定常的な姿勢のずれを推定しなくてもよい。この態様では、車両４の瞬時的な姿勢のずれによる影響を排除することで、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度を向上することが期待できる。

【0107】

実施の形態において、第１センサ３１で検知された点群６０のうち、車両４から比較的遠方に位置する点群を推定部１２が参照した場合、車両４の基準姿勢に対する定常的な姿勢のずれの推定精度が劣化する可能性がある。そこで、推定部１２は、第１センサ３１で検知された点群６０のうち、例えば車両４から所定距離の範囲内にある点群のみを参照し、所定距離よりも離れた点群を参照しなくてもよい。この態様では、車両４の基準姿勢に対する定常的な姿勢のずれの推定に悪影響を及ぼし得る点群を排除することができるので、推定精度を向上することが期待できる。

【0108】

実施の形態では、推定部１２は、車両４の路面５に対する相対的かつ定常的な姿勢のずれを推定しているが、これに限られない。例えば、推定部１２は、ＧＰＳ（Global Positioning System）等の測位システムからの地図情報を用いて路面５の勾配を取得することができれば、当該勾配を相対角度θ１に加算することにより、車両４の基準面に対する絶対的かつ定常的な姿勢のずれも推定することが可能である。

【0109】

実施の形態では、第１センサ３１は、カメラ又はＬｉＤＡＲ等の車両４の周辺の対象物を検知するセンサであるが、これに限られない。例えば、第１センサ３１は、車両４の角速度を検知することにより、車両４の姿勢を検知するジャイロセンサ等の姿勢センサであってもよい。この場合、第１センサ３１の検知結果は、平坦路を基準面とした車両４の基準面に対する絶対的な角度となる。このため、推定部１２は、例えば第１センサ３１の検知結果から路面５の勾配を減算することにより、相対角度θ１を算出することが可能である。路面５の勾配については、例えばＧＰＳ等の測位システムによる測位情報及び地図データから取得することが可能である。

【0110】

実施の形態において、姿勢推定システム１０は、単一のＥＣＵで実現してもよいし、姿勢推定システム１０が備える各機能を複数のＥＣＵに分散して実現してもよい。同様に、制御システム２０は、単一のＥＣＵで実現してもよいし、制御システム２０が備える各機能を複数のＥＣＵに分散して実現してもよい。

【0111】

実施の形態では、姿勢推定システム１０と制御システム２０とは、互いに独立したシステムであるが、これに限られない。例えば、姿勢推定システム１０は、制御システム２０を備えた構成であってもよい。この場合、姿勢推定システム１０は、制御部２２を更に備えればよい。また、この場合、出力部１３は、入力部２１に姿勢情報を出力する代わりに、制御部２２に姿勢情報を出力すればよい。

【0112】

また、上述の姿勢推定システム１０における処理の手順の実行順序は、必ずしも、上述した通りの順序に制限されるものではなく、発明の要旨を逸脱しない範囲で、実行順序を入れ替えたりその一部を省略したりすることができるものである。また、上記処理の手順の全部又は一部は、ハードウェアにより実現されても、ソフトウェアを用いて実現されてもよい。なお、ソフトウェアによる上記処理は、コンピュータ等に含まれるプロセッサがメモリに記憶された上記処理用の制御プログラムを実行することにより実現されるものである。また、その制御プログラムを記録媒体に記録して頒布や流通させてもよい。例えば、頒布された制御プログラムを、プロセッサを有する装置にインストールして、その装置のプロセッサに実行させることで、その装置に上述の処理の全部又は一部を行わせることが可能となる。

【0113】

また、上述した姿勢推定システム１０におけるコンピュータは、必須ではないが、タッチパッド等の入力装置、ディスプレイ若しくはスピーカ等の出力装置、ハードディスク装置若しくはＳＳＤ（Solid State Drive）等の記憶装置、ＤＶＤ－ＲＯＭ（Digital Versatile Disk Read Only Memory）若しくはＵＳＢ（Universal Serial Bus）メモリ等の記録媒体から情報を読み取る読取装置、又はネットワークを介して通信を行う送受信装置を有してもよい。例えば上述した制御プログラムがＵＳＢメモリ等の記録媒体に記録されていれば、読取装置は、その制御プログラムを読み取り、メモリ又はその他の記憶装置に記憶させる。また、送受信装置が、制御プログラムを格納している外部のサーバ装置とネットワークを介して通信を行い、サーバ装置から制御プログラムをダウンロードしてメモリ又はその他の記憶装置に記憶させてもよい。なお、姿勢推定システム１０は、集積回路として構成されてもよい。

【0114】

また、上述した各構成要素及び機能を任意に組み合わせることで実現される形態も本開示の範囲に含まれる。

【産業上の利用可能性】

【0115】

本開示は、車両の姿勢を推定するシステム等に適用可能である。

【符号の説明】

【0116】

１０ 姿勢推定システム
１１ 取得部
１２ 推定部
１３ 出力部
２０ 制御システム
２１ 入力部
２２ 制御部
３１ 第１センサ
３２ 第２センサ
３１０ 既定位置
４ 車両
５ 路面
６０、６０Ａ、６０Ｂ 点群
６１、６１０ 路面点
７ 積載物
８ 物体
Ｌ１ 直線
θ１ 相対角度
θ２、θ３ 角度

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】