路面地図上での自己位置の表現

路面地図上での車両の運動を表現するため，それぞれの座標系を定義する．まず車 両運動を記述するのに便利な後輪車輪軸上の中心に，図3.1で示す車両座標系を設定す る．車載カメラは車両に取り付けられており，剛体として扱われる．

n

^{番目のカメラ座}

標系における3次元点

cn

p と，車両座標系における点

p

_vの関係は，車両に対するカメラ の取り付け位置を表す回転・並進行列

vcn

R ^，t_vcnによって定義され，次の式で対応付 けられる．これらは定数として扱い，事前のキャリブレーションによって求めておく．

n n n

v



c v c



c v

p R p t

(3.1)

地図座標系の点

p

_mと車両座標系の点は離散化された時刻

k

における 6 自由度の回 転・並進行列R^k_vw^，t^k_vwより次の式で対応付けられる．

k k

m  vw v vw

p R p t (3.2)

図3.1 座標系の定義と地図の平面近似 3.1.1.地図の道路勾配を使った車両姿勢の分解

市街地や都市高速道路では勾配の強い道路があるため，道路全体を平面と仮定した

40

平面上位置+Yaw角回転の 3自由度の車両姿勢の推定では対応できない．しかしながら 回転・並進の 6 自由度を全て推定する場合，推定変数が増えることから精度の低下が 予想される．そこで本研究では地図から情報を得ることで3自由度に制限する(図3.1)．

具体的には車両は地図を局所的に近似した平面上を走行しているものと仮定すること で路面の勾配を得る．路面の勾配は，車両の Roll/Pitch 軸の回転

R

^k_slopeに対応する．こ れにより車両の姿勢R^k_vw^{は，地図から得られる}

R

^k_slopeと，推定が必要な

R

^k_yawの二つに 分解できる．

また車両の位置

t

^k_vw

   x

_vw^k

, y

_vw^k

, z

_vw^k

 

^{Tについても同様に} 3 自由度があるが，車両が路 面上を走行していると仮定すると，地図から高さy_vw^k を得ることができる．これらをま とめると，本研究が時刻kにおいて推定する変数は，車両のYaw回転を表すと



_vw^kと，

平面上での並進を表すx_vw^k ,z_vw^{k の3}自由度となる．以降，表記を容易にするため添字を 省略し，

x

^k

 [ , , ] x z

^{k k}



^{k T}と書くものとする．

3.1.2.自車近傍の路面勾配の計算

前項で用いた自車近傍の路面勾配を表す回転行列

R

^k_slopeを求める方法について述べ る．自車近傍の地図の平面近似には，地図に含まれる線分の端点の３次元座標を利用 する．直線道路などは線分の端点が疎であることがあり，そのまま利用すると近傍の 点の数が極端に減るため，線分を1.0[m]の等間隔に分割することで点の密度を揃える．

また平面近似する近傍の範囲として，道路の幅員の規定[51]を参考にすると，自車両を 中心とした 3.5[m]の範囲が適切であると考えられる．しかしながら広い交差点などで

は，3.5[m]の範囲に平面近似に十分な点が無いことがあり，ランク落ちや精度低下を招

くことがある．このためその3倍に相当する 10.5[m]の範囲の点を平面近似に用い，推 定したい位置からの距離に応じて，標準偏差 3.5[m]の正規分布によって重みをつけた (図 3.2） ． 具 体 的 に は 求 め た い 位 置

[ , ] x z

^T か ら 高 さ y を 回 帰 す る 関 数

c x z

y a   a x a z 

^{を地図の３次元点}

[ , , ] m m m

ⁱ_x ⁱ_y ^{i T}_z から学習する．具体的には次のコスト Eを最小とする平面a[ , , ]a a a_{c x z} ^{Tを求めた．}

 

2 { | 3 }

2

2 2

( )

| ,

( ) ( )

0 3.5

i

i i i i

y c x x z z

i i d

i i

i i i

x z

E w m a a m a m

w N d

d m x m z



 





 

   



   







(3.4) ( )

k k k

vw slope yaw

k k

yaw Yaw



vw





R R R

R ^(3.3)

41

図3.2 地図の平面近似

近 似 し た 平 面 に 対 応 し た 回 転

R

^k_slopeを 計 算 す る ． バ イ ア ス 成 分 を 除 い て ， 平 面

x z

y a x a z  

を考え，平面上の単位ベクトル

γ

をx=0,z=1でとる．

2

1 0

1 1

z z

a a

  

     

γ (3.5)

平面の法線ベクトルは次になる．

2 2

1 1

1

x

x z

z

a

a a a

  

      

β (3.6)

これより回転行列は次で構成できる．

 ^{, ,} 

k

slope

 

R β γ β γ

(3.7)

42

また前項のy_vw^{k も}y_vw^k a_ca x_{x vw}^k a z_{z vw}^{k として求めた．}

なおこの処理は地図が与えられれば事前計算が可能である．このため実験に用いる

XZ 平面を 1.0x1.0[m]のグリッドに区切って事前計算を行い，自己位置に応じて参照す

るものとした．

ドキュメント内 車両の自動運転に向けた走行レーン地図と車載カメラ画像の照合による自己位置推定に関する研究(本文) (ページ 39-42)