若泉:
200526
6/9までにやること ・長谷川さんのを参考に，自己位置推定のアルゴリズムを考える
自己位置推定
透視変換 パターンマッチングで十字路や丁字路を検知 pyb
長谷川さんの方法
v_L：左タイヤの速度
v_R：右タイヤの速度
v：中心速度
d：中心からタイヤまでの距離
\omega：回転中心からの角速度
\theta：進行角度
v=\frac{(v_R+v_L)}{2}
\omega=\frac{v_R-v_L}{2d}
\dot{x}=v\cos{\theta}
\dot{y}=v\sin{\theta}
\dot{\theta}=\omega
タイヤの回転角度
\phi_R：右タイヤの回転角度
\phi_L：右タイヤの回転角度
$`r$：タイヤの半径
\Delta\theta=\frac{r\phi_R-r\phi_L}{2d}
マップマッチングのための下準備
地図の作成
-
HPで配布されている道路パーツから，地図データを作成
![]()
-
Pythonで白線データのみを抽出
![]()
-
画像編集ソフトとPythonで，絶対に横切れない部分と，通過可能なガイドライン部分に分ける．(この際，地図データをトリミングしたので，元データと高さが少し違う)
-
それぞれの画像に対し，白線部分を1,それ以外を0としたCSVを作成
長期的目標
- 真上から画像が入ってきたときに，地図のどこにいて，どういう向きを向いているかを推定する．
- タイヤの動かしたものと比較し，どのくらい誤差があるかを出して，位置を算出する．
短期的目標
- マップ情報を載せて，マップマッチング的なものをやってみる．
200615
自己位置補正
進んでいる最中に，チェックポイントが見えたら，それをもとに自分の位置を更新する．
問題のモデル化
P_n=(X_n,Y_n)：車両の現在位置(車両前面中央部)
\theta：車両の現在進行方向(北との差)
\phi：視野角(正面から片端までの角)
L_v：視野距離
P_c=(X_c,Y_c)：チェックポイントの座標
\psi：チェックポイントの見える角(正面からの角)
L_c：チェックポイントとの距離
視野角\phiと視野距離L_cで構成される視界内に，位置P_c=(X_c,Y_c)のチェックポイントが入ったとき，チェックポイントの見える角\psiとチェックポイントとの距離L_cから，現在位置P_n=(X_n,Y_n)と進行方向\thetaを推定する．
- チェックポイントが視界内に入る条件
L_c<L_vかつ|\psi|<|\phi| -
\psiの求め方\psi=\theta-\arctan(\frac{x_c-x_n}{y_c-y_n})実際に求めるときには座標によって場合分けが必用-180°<\psi<180°となるように設定し，機体の左側がマイナス，右側がプラスになるようにする．
目的地（近点）までの速度を決定する手法
現在位置を補正した時に，その後の速度も決め直す必要がある．
- q°回転させるのに必要な速度比を求める
\omega=\frac{v_R-v_L}{2d}
より
t秒で車体を回転させることを考えれば
\omega=\frac{1}{t}\times q\times\frac{\pi}{180}[rad/s]にすればよい
よってそれぞれv_R,v_Lは
v_R=v+\frac{qd\pi}{180t}
v_L=v-\frac{qd\pi}{180t}
となる．
ここでvは目標とする平均速度をあらかじめ定めておけばよいので，
右辺は一意に決めることができる．