Gauss Newton 法,Levenberg-Marquardt 法 in Python
ちょっと前回の投稿からアップが遅れてしまいましたが,さぼっていたわけではなく...
引き続き Teb Planner のパラメータ調整と解析をしてたんですが,もう少し詳しく理解したかったんでソースを読んでみました.
で,気づいたんですが,LSD SLAM にしても, Teb Planner にしても,バンドル調整にしても,最近よく出てくる(少なくとも自分の周りで)技術は,結局非線形方程式のパラメータ最適化をしているなと.
で,この非線形方程式の最適化なんですが,あまりちゃんと勉強したことが今までなかったのでちょっとハードルが高かったのですが,せっかくの機会なのでちょっとかじってみることにしました.
読んだ本は,有名な下記の本です.さすが有名なだけあって,わかりやすかったですね.
- 作者: 金谷健一
- 出版社/メーカー: 共立出版
- 発売日: 2005/09/01
- メディア: 単行本
- 購入: 29人 クリック: 424回
- この商品を含むブログ (41件) を見る
なかなかちゃんと理解するのに時間がかかったんですが,根本的なアイデアとしては一次元のニュートン法が出発点になるんですね.
でも,高次元になるとヘッセ行列とかがいっぱい出てきて,途端に難しくなります...下記,簡単な例ですが,Pythonで非線形方程式のパラメータ推定をやってみました.
1. Gauss Newton 法
import numpy as np import numpy.linalg as linalg import random # Function is # f(x, y) = u0*x^3 + u1*x^2*(y^2 + u2*y) def f(x, y, z, u): return u[0] * x**3 + u[1] * x**2 * (y**2 + u[2] * y) - z def df_du0(x, y, u): return x**3 def df_du1(x, y, u): return x**2 * (y**2 + u[2] * y) def df_du2(x, y, u): return u[1] * x**2 * y def calculateDeltaU(H_sum, J_sum): return linalg.lstsq(H_sum, J_sum) def jacobian(x, y, z, u): nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]]) return f(x, y, z, u) * nablaF def hessian(x, y, u): nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]]) return nablaF * nablaF.T def createDataSet(x, y, u): z = [] for i in range(len(x)): noise = random.random() * 0.01 z.append(f(x[i], y[i], 0, u) + noise) return z def gaussNewton(x, y, z, u0, eps): u = np.array(u0).reshape((3, 1)).astype(float) oldDeltaU = np.zeros((3, 1)) deltaU = np.zeros((3, 1)) while(True): J_sum = np.zeros((3, 1)) H_sum = np.zeros((3, 3)) for i in range(len(x)): H_sum += hessian(x[i], y[i], u) J_sum += jacobian(x[i], y[i], z[i], u) oldDeltaU = deltaU deltaU = calculateDeltaU(H_sum, J_sum)[0] u -= deltaU if (linalg.norm(deltaU[0] - oldDeltaU) < eps): break return u if __name__ == "__main__": trueU = [0.8, 0.8, 0.8] u0 = [1.5, 1.5, 15] x = [1 for i in range(10)] y = [i for i in range(10)] z = createDataSet(x, y, trueU) u = gaussNewton(x, y, z, u0, 0.00001) print("Estimated u : ", u)
2. Levenberg-Marquardt 法
# -*- coding: utf-8 -*- import numpy as np import numpy.linalg as linalg import random # Function is # f(x, y) = u0*x^3 + u1*x^2*(y^2 + u2*y) def f(x, y, z, u): return u[0] * x**3 + u[1] * x**2 * (y**2 + u[2] * y) - z def J(x, y, z, u): totalJ = 0 for i in range(len(x)): totalJ += (f(x[i], y[i], z[i], u))**2 return 0.5 * totalJ def df_du0(x, y, u): return x**3 def df_du1(x, y, u): return x**2 * (y**2 + u[2] * y) def df_du2(x, y, u): return u[1] * x**2 * y def calculateDeltaU(H_sum, J_sum): return linalg.lstsq(H_sum, J_sum) def jacobian(x, y, z, u): nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]]) return f(x, y, z, u) * nablaF def hessian(x, y, u): nablaF = np.array([[df_du0(x, y, u)], [df_du1(x, y, u)], [df_du2(x, y, u)]]) return nablaF * nablaF.T def createDataSet(x, y, u): z = [] for i in range(len(x)): noise = random.random() * 0.01 z.append(f(x[i], y[i], 0, u) + noise) #z.append(f(x[i], y[i], 0, u)) return z def levenbergMarquardt(x, y, z, u0, eps): c = 0.0001 u = np.array(u0).reshape((3, 1)).astype(float) tmpu = u oldDeltaU = np.zeros((3, 1)) deltaU = np.zeros((3, 1)) newJ = J(x, y, z, u0) oldJ = newJ while(True): J_sum = np.zeros((3, 1)) H_sum = np.zeros((3, 3)) for i in range(len(x)): H_sum += hessian(x[i], y[i], u) J_sum += jacobian(x[i], y[i], z[i], u) H_sum = H_sum + c * H_sum * np.identity(3) tmpDeltaU = calculateDeltaU(H_sum, J_sum)[0] tmpu -= tmpDeltaU oldJ = newJ newJ = J(x, y, z, u) if (newJ > oldJ): c = 10 * c tmpu = u tmpDeltaU = deltaU else: c = 0.1 * c u = tmpu oldDeltaU = deltaU deltaU = tmpDeltaU if (linalg.norm(deltaU[0] - oldDeltaU) < eps): break return u, newJ if __name__ == "__main__": trueU = [0.8, 0.8, 0.8] u0 = [10, 10, 20] x = [1 for i in range(10)] y = [0.5 * i for i in range(10)] z = createDataSet(x, y, trueU) u, Jval = levenbergMarquardt(x, y, z, u0, 0.000001) print("Estimated u : {0}, J : {1}".format(u, Jval))
今月のやること.ROS Mapping & Localization
早くも3月第三週目に突入してしまったのですが,今月のやることをリストアップしておきます.先月の大きな目標として,Odometryフレーム内でロボットの制御を動かすという目標を作ったのですが,teb_local_plannerを使うことである程度目処はたったかなと思っています.今日これから外で実験してきます.
で,今月というか直近2ヶ月・3ヶ月の目標として,とりあえず既存のものを組み合わせて動くレベルのものを作ろうと思います.自分のテーマは画像処理だけでつくばチャレンジ完走で,今もそこは変わらないのですが,一から暗中模索でやるとどうしても全体のフローが見えづらく一連のタスクを洗い出せないので...ということで,まずはショッピングです(笑)公開されているライブラリにハマるように,センサーを買いあさります.
1.買い物
1.レーザスキャナ
画像処理だけで動かすことを最終目的としつつ,レファレンス取得用・一発目の構成作成用に購入します.ちょうど安価なものが発売されようとしてるみたいなので,見積もりをとってみます.
Scanse | Affordable Scanning LiDAR for Everyone
RPLIDAR - Slamtec - Leading Service Robot Localization and Navigation Solution Provider
2.ステレオカメラ
2つカメラを購入して,自作でステレオカメラを作ったのですが,ハード同期できないカメラだったので撮影する際にロボットを停止しなければならず,,,これではちょっと成り立たないので,買います.前回と同じように,2つカメラを購入して同期の仕組みも含めて作るべきか悩みましたが,結局値段が高くなってしまうのと自分でやると限界があるので出来合いのものを購入します.
3.処理ボード
これは,ちょっと興味半分なところもあるのですが(笑),予算と時間が許せば買いたいなと.
2.今月のやること
1. Week of 2017/3/5
Odometryフレーム内でのロボット制御の実現.
2. Week of 2017/3/12
teb_local_plannerのパラメータ調整
ショッピングの見積もり取り,発注.
gmappingをつかってmapを作れるようにする.
3. Week of 2017/3/19
??
4. Week of 2017/3/26
??
ROS Navigation Stack ~ Teb Local Planner ~
座標指示でロボットを制御することを目標に,ROSのNavigation Stackを用いて軌道生成・制御を使用としていたのが前回のエントリだったのですが,実際にロボットを動かしてみて驚愕の事実が発覚...
車タイプのロボットの制御には対応していない!
車タイプのロボットだと,当然のことながら前に進みながらじゃないと横方向に移動できませんが,DWA Local Plannerで対象としているロボットはその場で回転ができるロボット対象でした...
何とかハックできないかと思ったのですが,アッカーマンとかいろいろ考えだすと簡単にできそうでなく..午前中は途方に暮れていたのですが,見つけました.「teb_local_planner」です!いやあ,オープンソース様様ですね.DWA Local Plannerのパラメータ設定からそれほど大きな変更をすることなくすんなりと動かすことができました.実際に長い距離を動かし始めると問題も出てくるかと思うのですが,とりあえず明日もう少しパラメータ調整を実施して問題なさそうなら,次のアイテムに進みたいと思います.下記ビデオはteb_local_plannerを用いたレーンチェンジ動作のイメージです.
ROS Navigation Stack ~ DWA Local Planner パラメータ調整1 ~
先日の投稿で,cmd_velをつかってロボットが動かせるようになった様子をアップしましたが,今日はついにmove_baseを使ってロボットを動かせるようにします.すでにちょっと試してみて,前進はできるようになったのですが曲がる動作がどうもうまくいかず...パラメータを一通り勉強して調整する必要があるかと思ったので,このエントリを作りました.
1. DWA Local Plannerとは
ROSではローカルプランニングようにいくつかアルゴリズムが選択できるようになってますが,今回はDWA Local Plannerを使うことにしました.DWA Local Plannerでは,下記のようなフローで実際に軌道が生成されます.
2. 各種パラメータ
変更必須のパラメータ
座標軸毎の最大・最小速度
自分のロボットは車両型でy方向には動けないので,速度設定は下記のようになるみたい.
あと,つくばチャレンジの速度制限から,x方向の最大速度は1m/s(=3.6kph)
DWAPlannerROS:
max_vel_x: 1.0
min_vel_x: 0.0
max_vel_y: 0.0
min_vel_y: 0.0
縦横方向の最大・最小速度
DWAPlannerROS:
max_trans_vel: 1.0
min_trans_vel: 0.0
trans_stopped_vel: 0.1
最大・最小角速度
DWAPlannerROS:
max_rot_vel: 0.5
min_rot_vel: 0.0
rot_stopped_vel: 0.0
最大加速度
DWAPlannerROS:
acc_lim_x: 0.5
acc_lim_theta: 0.2
acc_lim_y: 0.0
必要に応じて変更するパラメータ
目標位置との許容誤差パラメータ
yaw_goal_tolerance: 0.1
xy_goal_tolerance: 0.15
制御空間でサンプリングした値に対するシミュレーションパラメータ
sim_time: 4.0
vx_samples: 10
vy_samples: 1
vtheta_samples: 20
sim_granularity: 0.025
ひとまず上記設定でロボットを動かしてみて,うまくいけばアップします!
ROS Navigation Stack ~ Arduino Controller ~
Arduino, ROSの制御周りの実装・リファクタが終わりました.
制御にはPIDを用いましたが,等速直線運動,定常円旋回に関してはそこそこうまく動くようになりました.
ちょっとギクシャクしてしまってますが,これは制御器の設定が悪いせいではなく,ハードの作りこみが甘いことが原因のようで,制御パラメータをいくら変えても改善しませんでした...
ということで,想定より時間がかかってしまいましたが,ようやく cmd_vel を受けるインターフェースが整ったので,今週から Navigation Stack を使ってロボットを動かしていきます!
今月のやること.ROS Navigation Stack
おはようございます.なんと,はや3月...最近時間がたつのが早いです.
さて,今月のロボット目標です.2月中にROS Navigation Stackを動かしてみる予定でしたが,案の情遅れてしまいました.OdometryやArduino周りのコードをリファクタしてたら思いのほか時間がかかってしまったというのが理由ですが,まあ,理解は深まってコードも整理されたので良しとしましょう!
ということで,3月も引き続き ROS Navigation Stackを動かすことを目標にやっていきます.
1. Week of 2017/3/6
Navigation Stackを用いたロボットの前進・右折・左折
2. Week of 2017/3/13
Odometryフレーム内でのロボットの制御
3. Week of 2017/3/20
??
4. Week of 2017/3/27
??
ROS Navigation Stack ~ Arduino Controller ~
おはようございます.前回の投稿から一週間以上経過して,早くも当初の目標をデコミットしてしまいましたが,,,
なにもしていなかったわけではなく,久々にArduinoのコードを整理していたらわけがわからなくなってしまい,,,,整理ついでにリファクタリングしてました.
あと,つくるものの規模が比較的大きくなってくると,どうしても前に作業したことを覚えておくのが難しくなってしまい,忘れてしまうので,備忘もかねて簡単な設計書を残すことにしました.
(なんか仕事でもないのに,仕事みたいなことしてるな...)
www.slideshare.net
制御のパラメータ調整をしないといけないですが,それはまた後日のエントリで...