コンテンツにスキップ

駐車する(park_to_point)

要点

  • GUI の LiDAR の画面でクリックとドラッグで指定した位置と向きへ、事前の地図なしで、障害物を避けながら自分で入って止まるモードです。車庫入れも縦列駐車も同じ仕組みで行います。
  • 自分の位置は、クリックした瞬間の点群を基準にしたスキャンアンカで保ち、周りは局所の占有格子に溜め、経路は Hybrid A* で引きます。
  • 地図を使う走行のミッションの最後に、停止点から引き継いで駐車することもできます。

【シム/ホスト検証】

対象: park_to_point.py、raspi/nav/ の scan_anchor.py・local_map.py・hybrid_astar.py・reeds_shepp.py。確認した版: 上位 2d0a44e。 数値はシミュレータ(sim.park_bench)の値です。Hybrid A* と局所地図を C で書き直してからは、実機ではまだ走らせていません(開発記録)。

しくみ

flowchart LR
  A["目標をクリック<br/>(位置+向き)"] --> B["スキャンアンカ<br/>自分の位置を保つ"]
  B --> C["局所の占有格子<br/>+ 障害物までの距離場"]
  C --> D["Hybrid A*<br/>障害物を避ける経路"]
  D --> E["閉ループで追従<br/>前進・後退の切り返し"]
  E --> F["掃引で安全確認<br/>塞がれたら引き直す"]

駐車の目標と経路

LiDAR の画面で目標(橙の矢印)をドラッグで置くと、後退してから前進で入る経路(桃色)を引く。この例では残り 2.4 cm・向きの誤差 3.6° で到達した(シミュレータで撮影)。

設計の詳細

自分の位置 — 地図ではなくスキャンアンカ

駐車では SLAM の地図を使いません。代わりに、クリックした瞬間の点群を基準にして、その後の点群との重ね合わせ(点対線 ICP)で姿勢を保ちます。

壁際をクリックした人の意図は「あの壁に対してこの位置」です。推定がドリフトすると、駐車枠そのものが実際の壁に対して静かに動き、 間違った場所で「完了」と言ってしまいます。誤差が誤差として現れず、成功として現れるのが怖いところです。

自己位置の方法(シム、センサ誤差あり) 成功 最終位置の誤差
推測航法だけ 2/12 16.3 cm
スキャンアンカ 7/12 6.1 cm

精度を一番左右したのは、対応点として認める距離でした。「1 周期ぶんの移動」を想定した 0.30 m では別の壁と誤って組んでしまい、0.08 m にすると推定誤差が 3.65 cm から 0.96 cm になりました。

周り — 一度見た壁を忘れない局所地図

瞬間の点群をそのまま使うのをやめ、1 cm の格子に溜めます。

  • 点群は物陰と角度の粗さで、駐車枠の内側が見えていないことがありました。車庫入れの配置では、3 割の地点で実際より 3 cm 以上「余裕がある」と誤認していました(最大 21 cm)。
  • 格子に溜めれば、後退中に前方が視野から外れても、一度見た壁を覚えています。
  • 「見えていない所」を「空いている所」と混ぜません。 壁は通れない拘束、見えていない所はコストとして扱います。
  • 長方形の車体は、障害物までの距離場(ESDF)に対して 5 × 3 個の円で覆って判定します(前後 1.1 cm・左右 1.8 cm だけ安全側にはみ出す)。

経路 — Hybrid A*

「Reeds-Shepp の候補を短い順に試して、ぶつからない最初のものを選ぶ」方式では、回避できる形が候補の範囲に限られます。 実測では、車庫入れの 43 %・縦列の 78 % でしか回避経路が見つかりませんでした。Hybrid A* では 32/32 で見つかりました。

計画には時間の上限(既定 120 ms)があり、間に合わなければ Reeds-Shepp の候補に落とします。「動かない」より「少し進んで別の姿勢から引き直す」方が、次の機会に解けるからです。 曲率は車の限界の 0.95 倍で計画し、追従の誤差を舵で詰める余地を残します。

追従 — 前進も後退も閉ループで

舵を経路の曲率から直接出すだけだと、誤差を直すには区間を全部走り終えてから引き直すしかなく、位置 2〜6 cm・向き 4〜6° の残差で切り返しがもう一巡増えていました。 そこで経路からの横ずれと向きのずれを、曲率にフィードバックします。後退では向きの項の符号が反転することに注意が要ります。

前進と後退を切り替えるときは必ず止まり、止まっている間に次の区間の舵を作っておきます。切り替えた直後に舵が追いつかず、違う曲率で走り出すのを防ぐためです。

安全 — 車体が掃く範囲で確かめる

進行方向の扇の中の最近点だけを見る方式では、旋回中に車体の前後が振り出す範囲を見られず、経路にぶつかる障害物の 16 % が衝突まで扇に入りませんでした。 今は、制動距離ぶん車体が掃く範囲を、今の点群と溜めた地図の両方に当てて確かめます。塞がれていたら止まるだけでなく、その場で経路を引き直します。

計画と安全確認の基準が食い違うと、30 秒以上止まったり動いたりを繰り返した

計画が受け入れた経路を、安全確認が別の余裕の値で拒否すると、制動と引き直しが交互に続きました。判定の余裕は、計画と安全確認で同じ値を共有するようにしています。

自動停止は切って使う

STM32 の自動停止は「進む方向への接近そのもの」を止める仕組みで、「目的地に意図的に近づいて止まる」駐車とは原理的にぶつかります。 このモードだけは自動停止を切って使い、代わりに上の掃引による安全確認で守ります。