Arduino Due + Rotary Encoder
前回のエントリから２週間弱が経過してしまい，，，当初の目標をちょっとデコミットしてしまいました．．．がそんなときもあります．ただ，何もやっていなかったわけではなく，，，あらたな問題を解決しています（笑）
実は，TebLocalPlannerの軌道生成に使うにあたって，使っていたエンコーダの分解能が荒すぎることが発覚し，いまこの付け替えをしてます．なんだか素朴な感じで恥ずかしいのですが，もともと田宮のラジコンをベースに使っていたため，ラジコンのタイヤにネオジム磁石を張り付けてこれをホールセンサで読み取ることでオドメトリを計算してました．
タイヤとネオジム磁石
ホールセンサ
で，タイヤの直径が12cmで，一周37.68cmになります．一秒間に一周したとしたら大体40cm/sになるのですが，張り付けた磁石の数が20個なので50msに一回パルスが立つか立たないかという計算になります．このくらいの速度が出ていれば問題ないのですが，停止状態から滑らかに速度を立ち上げる場合，低速域でもある程度速度コントロールができないといけないのですが，ここがどうしても荒くなってしまって最終位置精度を高くできませんでした．
ということで，思い切ってエンコーダを付け替えることにしました．購入したエンコーダは結構いいやつで，OMRONの産業用のものです．
が，付け替えようと思った矢先，マイコンの割込みのピンが足りないことが発覚．．．結局マイコン周りも大幅に改修しないといけなくなってしまいました．．．マイコンは Arduino Due を聖地（笑）秋葉原まで行って購入してきました．UNOやLeonardと違ってAVRではなくARMを使っているとのことで，クロック数も上がっていたのでマイコンを一つに集約しました．
平日は朝仕事前に作業をしていたのですが，ハードをいじりだすと作業毎にいちいち部屋を片付けられないのでリビングが荒れ放題です（笑）．独身万歳ですね．
今週末でハードの取り付け・マイコン周りのＳＷの整理を終わらせたいのですが．．．頑張ります．制御のキメが細かくなったらまたアップします．
つくばチャレンジ2017への参加のお勧め
ついに来てしまいました．．．．そうですつくばチャレンジ２０１７の参加募集開始です．
昨年ＳＩ２０１６に参加し，つくばチャレンジ２０１６の取り組みを終えてから早くも半年．．．それなりに前もって動いては来ましたが，まあなかなか思うようには進まないもんですね．
今年は３回目の参加ということで，そろそろ結果出したいです．
とりあえず，自戒の意味も込めて，２月７日に書いた今年の目標を反芻します．
- １．月ごとに目標・プランニングをしてブログで公表する．
週ごとに達成内容をブログで残す．（備忘録・整理目的）
自分の知識不足もあり，年単位のプランニングがやっぱり難しいので，月ごとに取り組む内容を決めて，ブログで公表します．で，週ごとの成果をブログで公表します．
- ２．まず，できる限りありものを使って，走るところまで持っていく．
フレームワークとかライブラリとかの使い方を勉強するのがめんどくさくて，昨年はもっとシンプルなものを自分で作れないかと思っていたのですが，まずはありものをできる限りパクる・再利用するという方針で作り上げていきたいと思います．
２月・３月でROS Navigation Stackを理解して動かすところまでを目標にやってきました．ちょっと手直しが発生したりして（ここは後のエントリで書きます．）完全には終わらなかったのですが，それっぽく動くところまでは来ました．
本番（２０１７年１１月）に完走目指す！となると，かなりハードルが高いのは痛いほどわかっていますが，目標は高く！ということで，それを目標に考えるととりあえず一回目の説明会の時点で「確認走行区間の自律走行達成」を実現しないと苦しくなるかと思います．これを目標にざっくりスケジュールを組むと．．．
４月 足回りの完成とTebLocalPlannerを使ったローカルプランニング
後のエントリで書きますが，TebLocalPlannerを使うにあたってエンコーダの分解能が荒すぎて細かい制御ができなかったため高いロータリーエンコーダを買いました．．．取り付けにするにあたって，Arduinoのピンが足りないことが発覚し，，，，結局マイコンも新しくしました．今取り付けしてますが，今月はこの足回りをかっちりさせることを最低限の目標にしてます．あとは，Zed Stereo Camera が届いたので，こいつの使い方をマスターします．
５月 レーザスキャナ・ステレオカメラを用いたマッピング
SWEEPを発注したのですが，5月には届いているはず．．．ということで，５月はレーザースキャナ・ステレオカメラを用いてマッピングします．できればゴールデンウィークに大清水公園に行ってデータを取ってこられれば．
６月 限定区間・静的環境での自律走行トライ
５月に作成したマップに対してＡＭＣＬを用いてローカリゼーションを実施して，少なくとも静的な環境では自律走行できるようにします．
７月 試走会一回目
一回目試走会．今年はここでの確認走行完了を目指します！
ということで，皆様応援よろしくお願いします！！！
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
早くも３月第三週目に突入してしまったのですが，今月のやることをリストアップしておきます．先月の大きな目標として，Odometryフレーム内でロボットの制御を動かすという目標を作ったのですが，teb_local_plannerを使うことである程度目処はたったかなと思っています．今日これから外で実験してきます．
で，今月というか直近２ヶ月・３ヶ月の目標として，とりあえず既存のものを組み合わせて動くレベルのものを作ろうと思います．自分のテーマは画像処理だけでつくばチャレンジ完走で，今もそこは変わらないのですが，一から暗中模索でやるとどうしても全体のフローが見えづらく一連のタスクを洗い出せないので．．．ということで，まずはショッピングです（笑）公開されているライブラリにハマるように，センサーを買いあさります．
１．買い物
１．レーザスキャナ
画像処理だけで動かすことを最終目的としつつ，レファレンス取得用・一発目の構成作成用に購入します．ちょうど安価なものが発売されようとしてるみたいなので，見積もりをとってみます．
Scanse | Affordable Scanning LiDAR for Everyone
RPLIDAR - Slamtec - Leading Service Robot Localization and Navigation Solution Provider
２．ステレオカメラ
２つカメラを購入して，自作でステレオカメラを作ったのですが，ハード同期できないカメラだったので撮影する際にロボットを停止しなければならず，，，これではちょっと成り立たないので，買います．前回と同じように，２つカメラを購入して同期の仕組みも含めて作るべきか悩みましたが，結局値段が高くなってしまうのと自分でやると限界があるので出来合いのものを購入します．
３．処理ボード
これは，ちょっと興味半分なところもあるのですが（笑），予算と時間が許せば買いたいなと．
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 パラメータ調整１ ~
先日の投稿で，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 を使ってロボットを動かしていきます！
