この記事は筆者オンリーのAdvent Calendar 202318日目の記事です。
前回までのあらすじ
PythonRoboticsのpure pursuitを動かして（？）みました。
今回やること
PythonRoboticsのLQRの処理について調査してみたいと思います。
LQRの各処理について
PythonRoboticsのpursuitのソースコードはこちらになります。
また、アルゴリズムの概要はこちらで紹介されています。
LQRとは
こちらやこちらやこちらの記事がわかりやすいと思います。
すみません、私もよくわかっていません。
PythonRoboticsの構成
Stateというクラスがあり、宣言時に現在位置、角速度、速度を初期化しているようです。
update関数があります。
これで現在位置、角速度、速度をアップデートしているようです。
pi_2_pi関数があります。
これで角度（-pi〜+piへ）の補正を行っているようです。
solve_dare関数があります。
これでリカッチ方程式を解いているようです。
dlqr関数があります。
これでLRQのゲインを算出しているようです。
lqr_speed_steering_control関数があります。
この関数で速度、角速度を算出しているようです。
calc_nearest_index関数があります。
この関数でTrajectory内の最も近い位置を算出しているようです。
calc_speed_profile関数があります。
この関数でTrajectoryの各速度を算出しているようです。
まとめ
一気に制御理論が出てきてわけわからなくなりました。