背景・目的
今の自分の仕事は車両の自律走行のための障害物検知システムを開発することです。その際のデータ解析やロジック検討用シミュレータ作成を効率化するために、最近Pythonの勉強をし始めました。
始めてから2週間くらい経ちある程度扱い方が分かってきたので、手始めに障害物検知用レーダが物体を検知する簡易シミュレータを作ってみたので紹介します。
概要
２次元平面のXY直交座標系において、ある物体が速度v[m/s]、角速度ω[deg/s]で移動するものとします。それを扇形の検知範囲を持つレーダが検知し、自分の検知範囲に存在する物体までの距離や角度、距離方向の速度と角速度を出力します。レーダといっても反射強度までは出力できるようにはしていません。今後はそういうのも作れたらいいな。
シンプルながらも出来るだけ汎用的なシミュレータにしたかったので、検知対象の物体の動きはシミュレーションfigureの下にある二つのスライダから速度v[m/s]と角速度ω[deg/s]の入力を与えることで動きをコントロールできるようにしました。
物体の状態量はX座標、Y座標、姿勢のヨー角であり、その状態ベクトルは以下のように定義しています。
またこれより、状態方程式は以下のようになります。
以上の状態方程式から求められた物体の位置座標がレーダ検知範囲に存在するかを内外判定し、範囲内に物体がいるときはガウスノイズを持った距離、角度、極座標系の速度を出力します。プロット図の青点が状態方程式から求めた真値、赤点はレーダが出力した距離と角度から計算した物体の位置です。
サンプルプログラム
今回作成したシミュレータのプログラムは以下のGitHubで公開しています。これをアレンジすることで、カルマンフィルタのような物体追跡技術やSLAMのような地図構築、位置計測技術のシミュレーションにまで今後は発展させていきたいです。 github.com
今回ハマったこと
シミュレーションのGIF画像を貼り付けるのに苦労した。アップロードしてもすぐに失敗して、しばらくしたらいつの間にかアップロードされているし。サイズが大きすぎるのかと思って圧縮もしたのに何でだろう？？
参考文献
扱う状態方程式が同じだったので、ソフトのクラス設計を参考にさせていただきました。ありがとうございました。 qiita.com
