2019年10月31日木曜日

カルマンフィルタ積むまでもない時のローパスフィルタ

最近HX711をつんだ重量計を買ってM5stackでつかっていたのですよ。
アマゾンとか売ってる安いやつね。Arduinoライブラリでは引数に(10)とか入れて10回積算値を入れてたりするのだけれども、それだと応答性が悪い。応答性は計り取り(製薬やお料理)に重要で、速度が求められる。ということで、普通はこの積算回数と応答性のトレードオフになるんだけど、少しだけ賢い方法を紹介。

具体的な処理は以下の通り

  float w=ここで重量だとかを入力;
  static float oldV=0;  //平均値となる出力を入れる変数
  const float minRate=0.01; //重さの変動がないときはIIR係数0.01にする
               (小さい程積算回数が増えたのと等価になる)
  const float maxRate=0.999;//重さの変動が大きい時はIIR係数を1未満で大きな数にする(あまり弄る必要はなし)
  const float  threshWeight=5; //この変動量ぐらいが「大きい」「小さい」の閾値
  //////////実質的な処理は以下2行/////////////
  float rate = minRate+  min( fabs(w-oldV)/threshWeight ,1.0f)*(maxRate-minRate);
  oldV=w*rate+oldV*(1-rate);


重量計だとかは雑に考えると角がかけた階段状に出力値が変化すると考えられる、
そんなときはカルマンフィルタが王道だけど、とりあえずやっつけで実装したいときは上の実装で十分な応答性と精度が得られる。

rateがカルマンゲインに相当してるんだけど、この解説サイトの説明で言えば「予測不確かさ」と「観測不確かさ」がこの系には存在して、そのうち「予測」不確かさの割合をrateに入れるのが良いとされています。

過去の平均値と現在の入力値の差が予測不確かさに比例すると考えると上のの(w-oldV)は予測不確かさに比例した何かです。本来予測不確かさに対してカルマンゲインは定数である観測不確かさaをつかって、rate=x/(a+x)のような形になるんですが、上ではy=ax+bの形に近似してる事になります。

ここで、真面目に観測誤差とは何かとか考えてもいいんだけど、ぶっちゃけ必要な精度にあわせてminRateを小さくしていって、あとは変動が「大きい」時に高速化するためにthreshWeightを調整すると考えて勘で調整しても十分気持ちよく動いてくれる。



2019年10月4日金曜日

中華デジタルタイマーリレーの設定方法

先日買った、中華デジタルリレー設定方法の日本語訳が解りにくかったので、翻訳。

https://ja.aliexpress.com/item/33014830389.html?spm=a2g0s.9042311.0.0.463e4c4dOuik6D

モード設定をしてしまえば、電源復帰後そのモードで動きます。

トリガー入力はフォトカプラで絶縁されてるので、Gnd_tとSignalにそれなりの電圧をいれないとトリガー入力されません。3~24V とかいてありますが、5Vぐらいはいるっぽい。


SETを長押しして、設定モードに入る。UP/Downでモード切り替えができます。
モード切替後、SETを押してON/OFF(LED上ではCL/OPで表示)の時間を設定します。(SET=決定キー)
秒数入力モードの時以外に、再びSET長押しで、設定モードからぬけて通常動作します


モード一覧
P1.1: トリガーの立ち上がり/立ち下がりエッジ両方で[On時間]だけリレーをOn
P1.2:ONトリガになるたびに「On時間」だけ、リレーをON。トリガーONのままでも指定時間になったらOFFになる、立ち上がりトリガーが途中で立ったら指定秒数を再びセットする。(連打すればずっとON)
P1.3: トリガがONになるたびに「On時間」だけ、リレーをON 。P1.2と違いもう一回立ち上がりエッジがくると、中断してOFFになる
P1.4:トリガは無視、電源ON後一定時間のみONになる。 (車載装置などの遅延電源ON回路用途)
P2.1:立ち上がりエッジの後[Off時間]まってから[On時間]の間だけ、リレーOn。カウント中の入力は無視。(トリガーHiのままだと、Offのまま。一度LowにしてからHiにしないとカウントしない)
P2.2: P2.1と同じだけど、トリガーが入力されるたび秒数リセット
P3.1:トリガーでOn/OffループをLoop回まわす。 も一回トリガー入力があると、Loopを中断
P3.2:トリガーなしでOn/OffループをLoop回す(電源いれたら自動点滅する時に便利)
P-4: トリガーHiの間はリレーOn,トリガーLowになっても、[On時間]の間はOnを保持


時間入力モード:
設定で、OnやOff表示の後に自動で時間入力モードになる。stopボタンで小数点位置を決定できる。0.1秒単位と1秒単位がある。小数点が三つ表示されるのは「分」入力モード。


Stopモード:
Stopを一回おすと、タイマーは動くけど、リレーは動かさないコールドRUNモードになる。もう一回おすと復帰する

Stop長押しで、C-p,O-dモードが切り替えできるC-pは5分後にLED消灯, O-dモードは常にLED表示ON   (使うのか?この機能?)

2019年3月31日日曜日

robot_localization全然わからん。

robot_localizationを使って、roombaのwheel odometryと、realsense T265(VSLAMカメラ)によるVisual odometryを統合しようとしたら見事にハマったのでメモ。

http://docs.ros.org/melodic/api/robot_localization/html/index.html


T265のカメラがはき出すodometry座標系を利用して、その上にmapを置く構成の絶対座標を使っています。T265はroombaの端っこに載っているので、camera_pose_frameの下にbase_linkがずれた位置に存在しているというstatic TFを書いています。

後述のにように、このズレがたぶん色々な元凶な気ももします。
ということでTFとか楽勝なツヨツヨな方にチェックしていただければなぁ~。

暗黙のbase_link(本命)

Odometryのchild_frame情報をここで捨てている。
すなわち、Odometryのchild_framが暗黙のうちにbase_linkであることを仮定しているが、
VSLAMカメラは機体の端っこについているのでその分ずれる。
一応、座標変換ノードを入れれば対処可能?
https://github.com/cra-ros-pkg/robot_localization/blob/melodic-devel/src/ros_filter.cpp#L1690-L1701


差分とったら、元がなんであれframe_idはbase_linkだよね??(ついで)

differentialモードの時の話。
https://github.com/cra-ros-pkg/robot_localization/blob/melodic-devel/src/ros_filter.cpp#L2763

child_fram==base_linkだとして話すると、前回のodometoryPoseからの差分( poseTmp.setData(prevMeasurement.inverseTimes(poseTmp)) )の
frame_idはbase_linkだよね。そして、「twistPtr->header.frame_id = baseLinkFrameId_;」という風にたしかにbase_linkを設定している。
しかし、なぜか追加の回転「poseTmp.mult(targetFrameTrans, poseTmp)」をかけている
このへんが謎。

2019年2月21日木曜日

続ROS#と戦ってみた

とりあえずの戦況報告。
前回、BSON化してLaserScanを送るという所までできました。
その後、細々とした修正をに加え、PointCloudを扱うコードを追加したりしてました。

とりあえずの移動ロボット可視化には十分つかえそうな感じです。
もし要望があるようならば、そのうち解説をかこうかなと思っています。

2019年1月26日土曜日

ROS#と戦ってみた

期待から始まるが・・・

UnityからROSを使いたくて、ROS#に手を出してみた。独シーメンス社のオープンソースということで、勤め先内部的にインパクトがあるっていう下心はもちろんあるんだけど。
ARCore/Hololens上のUnityからロボットにアクセスしたかったので、UWPとかにも対応させたかった。ROS.NETのほうがTFも使えて多機能なんだけど、その辺の移植が死にそうなので諦めて、同様にROSSharpも諦めた。(うろ覚えだけどXML周り)
TFが使えないので同期がとれてるフレームを切りだすとかできないけど、ROS側でTFを解決してPoseStampで投げつけるかぁ。という感じで割り切りました。

ROS#のしくみ

ROS#がUWPに移植出来たりするのは中身が単純だからなのです。ROSデータの中身は単純なTCPで垂れ流し状態なのですが、それが何処にあるのか?を問い合わせるのにROS_MASTER(roscore)にお伺いを立てる必要があります。その時に使うXML-RPCのライブラリが.NETFrameworkにゴリゴリに依存してるとUWP(.NET Core)に移植しづらいという事になります。ここでrosbridge_serverの登場です。rosbridge_serverは全ての通信のwebsocketで受けとめてくれる。proxyサーバです。通信すべき相手の場所がどこか?という事は気にする必要はありません。また通信内容はJSON(オプションでBSON)なのでUWPだろうがUnityだろうが実績がある方法が転がっています。

デメリットもあって、JSONなのでデータがデカくなる上に、折角分散通信できるようなROSの設計ですが、rosbridgeというproxyに一旦データが集中して集まる事になります。平たくいって余り通信速度を求めてはいけません。

第一の壁NTP on Android in 化石社内ネットワーク

ロボットは複数のセンサの結果の整合性を保つため、センサ値や座標情報にはミリ秒精度のタイムスタンプが打たれます。この時間を同期するシステムは実はROSにはなくOSの時間をNTP等でミリ秒単位で合わせろという無茶振りをしてくるのがROSなのです。(ROS2はどうだか知らない)
さて、化石のような頭の硬い会社でROSを使いましょう。社内ネットワークにNTPは立っているが何故か平気で秒単位でNTP時刻からずれています。社外のNTPは使えません。ここまでは問題はないです。そこにAndroid端末がやってきます、Andoroidは社内の怪しいNTPサーバで時刻合わせなんてしてくれません。つまり、タイムスタンプが合わないのです。古すぎるor未来の時刻のデータは捨てられてしまい、通信不能に陥ってしまいます。
 そこでOSの時間を合わせるのを諦めて、アプリケーション(Unity)内でNTPをつかってUNIX時間を作ります。なお、NTPサーバはpythonのサンプルがあったので流用しました。(myntp.py
メッセセージヘッダにセットするのはROS#がよく考えられていて、StandardHeaderExtensions.cs にExtensionとして時刻をセットする関数を作るだけです。なお、NTPに時刻同期はNtpTime.csのように10秒に一回程度の頻度で時差を取り寄せています。

第二の壁 座標合わせ

壁というよりは、どうしようもなく出てくる仕様なのですが、何らかの方法で座標を合わせる必要があります。その座標計算に使うTransform行列がUnityがMatrix*Vectorな順番なのに対して、ROSのTF2ライブラリでつかえるPyKDLがVector*Matrixな順番で頭が混乱するといった程度なのですが、その辺の行列を生で触るのは初めてだったので半日潰してます。

第三の壁 マイペースなgazebo

実機を持ち出すとぬくぬくしたパソコンの前じゃなくて、クソ寒い工場に追い出されます。なのでギリギリめんえgazeboシミュレータでの物理シミュレーションを使った開発をします。当面gazeboで物理シミュレーション、ROSでロボット制御、Unityで可視化という流れです。折角NTPに対応したので、use_sim_timeパラメータをfalseにして、リアルタイムのシミュレーション結果にROSの時間をあてがうようにします。普通に動いたので安心してLIDAR(LaserScan)の可視化をしようとすると、データが見当たりません。gazeboからの位置情報などは正しい時間をさしているものの、センサだけはシミュレーション時間をさしています。設定で逃げれるものではなく、ソースをかきかえなければいけない状況だったので、こんな感じにセンサーの時刻を現在時刻に書き換えるROSノードをでっちあげました。

第四の壁 JSON、NaN食べない。

これでようやく、LIDAR可視化できるぞ!とおもったらUnity 上ではウンともスンとも言いません。Subscribeしてるのに、一つもメッセージがきません。と思ったらWebsocketを流れるJSON に観測値に混ざって「null」という見慣れない文字。JSONの数値はNaN,Infが扱えないので、rosbridge側で勝手にnull値をいれていました。このnull問題、LIDARの性質上10cm以下や10m以上は欠測なのでInfやNaNが結構入ります。ros-sharpのissues読んでるとこれもproxyノードで適当な値をいれて逃げてますが、LIDARはともかく他のセンサまで考えると、欠測は欠測だろ。数字を入れるのは美しくない・・・。という気もします。そこで馬鹿正直な方法としては「float」型を「float?」型にするという手があります。するとJSON.NETではnullable(null値を取る事ができる)floatとして処理されます(ただし処理オーバーヘッドがある)。確かにこれで解決はできるのですが、ソースコード中のfloatを片っ端からfloat?に書き換えて、nullチェックを全てに足すのは流石に面倒。
ということで、rosbridgeの実装を眺めてるとBSONという文字をソースコードに発見。rosbridgeにbson_only_modeという機能があるのに気が付いたのでした。BSONの場合数値をdouble型にするので、NaNもInfも表現できます。ただ、rosbridgeがBSONの時も無駄にnull置換をしていたので、BSONモードの時にかぎってnull置換をしないようにしました。ROS#側はJSON.NETがそもそもBSON使えるというのがあって、Serialize/Deserialize部分をちょっと書き換えるだけでした。

 第五の壁 まともにテストされてない・・・

これで晴れてLIDARが表示されました!でもなんかへん?LIDARの表示は球体・線・エリア塗りつぶしの3種類あるんだけど、球体だけがまともで残り二つは明後日の向きになる。と思ったら、残り二つはUnityのtransformの使い方、つまりlocalPositionとPositionの使い分けがきちんとできてない感じでした。そして二つの間違え方が違うので、不慣れなメンバーが混じってたのでしょう・・・。映ったからOK!でいろんな角度でのテストまではしなかったのかな?

 まとめ

 色々はまり所はありましたが、ROS-sharpは単機能な分トラブルシュートは簡単なのかなという感じです。ちゃんとARCore実機でカクつくこともなく動いたし。Buggyなのを除けば十分選択肢にはいるのかなという感じです。今回作ったのをプルリクするのか細々とforkしていくのかどっちが良いかは悩み所です。(BSONはオプションで切り替えとかじゃないとだめなかなぁ?)ご意見があれば嬉しいです。