先進技術調査グループのエンジニアの酒井 (@neko_suki)です。
過去に2回、Turtlebot3の遠隔制御について紹介をしました。
今回は、ネットワークの切断やほかの要因によって大幅な通信遅延が発生した際に、Turtlebot3を安全に遠隔制御技術する技術として以下の2点についてご紹介します。
①フェールセーフ機能
②メッセージの遅延への対応
Turtlebot3の遠隔制御は、PS3コントローラからのメッセージをクラウド経由でTurtlebot3に届けることで実現しています。
PS3コントローラはRaspberry Pi3にBluetoothで接続されています。そのRaspberry Pi3上で弊社製品のintdash Edgeが動作しています。intdash EdgeはTurtlebot3側のRaspberry Piにも搭載されています。この二つが弊社のクラウドを経由して通信を行っています。
Wi-FiやLTEなどの公衆無線では、通信環境が悪くなると、ネットワークが一時的に切断されたり、遅延が発生することがあります。
そのような状況で発生する課題とその対応策として、以下の2点について検討しました。
①フェールセーフ機能
②メッセージの遅延への対応
①フェールセーフ機能の検討
フェールセーフ機能とは何か
Turtlebot3のようにロボットを遠隔制御する場合、Wi-FiやLTEなどの公衆無線では、通信環境が悪くなるとネットワークが一時的に切断されたり遅延が発生することがあります。そのため、そのような状況を想定して安全に制御するためのフェールセーフ機能が必要になります。
以下の動画で、実際に実機を使い意図的にネットワークを切断した状態を発生させて何が起こるのかを確認しました。動画では、タイヤが制御されるようにPS3コントローラのスティックを固定しながら、Turtlebot3に接続している有線を抜きます(注: Wi-FiはOFFにしています)。有線を抜いた後もタイヤが動き続ける様子が確認できます。
動画では台の上にTurtlebot3を固定していますが、もし実際に地面の上を動いているときにネットワークの切断などが発生すると、そのまま動き続け障害物や壁などに衝突する可能性があります。
フェールセーフ機能対応前の処理フロー
Turtlebot3では、PS3コントローラ側のRaspberry Piから送信されたjoyメッセージ
をintdash Edgeを経由して rosbridge_tcp
で受け取ります。rosbridge_tcp
は受け取ったjoyメッセージ
をpublishします。publishされたjoyメッセージ
は、タイヤを制御するteleop_twist_joy
ノードと アームを制御するteleop_arm_node
ノードにsubscribeされます。
teleop_arm_node
は受け取ったjoyメッセージ
から、移動後のアーム座標を計算した joint_trajectory_point
というFloat64MultiArray型 のメッセージを生成してpublishします。
Turtlebot3はその座標情報をもとに、モーターを動かします。
一方、teleop_twist_joy
は受け取ったjoyメッセージ
から、速度を計算したcmd_vel
というTwist型 のメッセージを生成してpublishします。
速度と角度を基に、タイヤの制御が行われます。
アームの制御では、1つのjoy
メッセージから移動後の座標を計算するため、ネットワークの切断や遅延によってメッセージが到達しない場合には、アームは動作しません。
しかし、タイヤの制御はそうはなりません。ネットワークの切断や遅延によってメッセージが到達しない場合には、過去に設定された速度・角度の情報がそのまま使われます。
その結果、例えば障害物などがあると避けられずにぶつかってしまいます。
フェールセーフ機能対応後の処理フロー
フェールセーフ機能に対応するために、cmd_vel_stopper
というROSノードを追加しました。
cmd_vel_stopper
はネットワークの切断や遅延を検出するために、一定時間Joyメッセージ
が受信できなかった場合に、速度を0にしたcmd_vel
をpublishするようにします。速度が0のcmd_vel
をpublishすると、タイヤの制御が停止します。
ROSノードの構成は以下のようになります。
cmd_vel_stopper
も joy
メッセージをsubscribeします。cmd_vel_stopper
はJoyメッセージ
を受信したタイミングを記録します。
そして内部で、100msec毎に最後にJoyメッセージ
を受信したタイミングを監視します。もし現在時刻と最後にJoyメッセージ
を受信したタイミングが200msecよりも大きいなら、速度を0にしたcmd_vel
をpublishします。これによって、タイヤを停止します。
実際に実機を使って確認した動画が以下の動画になります。先ほどとは異なり、有線を抜くとタイヤが止まることが確認できました。
これによって、ネットワークの切断や遅延があってもTurtlebot3を安全に停止させるフェールセーフ機能が実現できました。
②メッセージの遅延への対応
メッセージ遅延による課題
次は、メッセージが遅延した場合の処理について考えます。
現在の実装では、コントローラ側から20ms間隔でJoyメッセージ
を発行します。このJoyメッセージ
はクラウドを経由するためネットワーク遅延分だけ遅れてTurtlebot3 に到達します。
Joyメッセージが
PS3コントローラ側のRaspberry PiからTurtlebot3に到達するまでに、一時的なネットワークの切断などによって大きな遅延が発生したとします。そうすると、ネットワークの状況が回復した際に、過去のJoyメッセージ
が含まれた複数のメッセージが到達します。
そのままJoyメッセージ
をpublishすると、遅延によるレスポンスの悪化と、メッセージ間隔が維持されないことによる再現性の悪化の2つの問題が発生します。
レスポンスが悪化すると、例えば10秒の切断が発生したら10秒後を予想して操作をしなければいけません。
また、操作の再現性が失われると、意図しない動作になってしまう可能性があります。
したがって、ここにも対応が必要になります。
メッセージ遅延への対策案
対策案1
対策案1では受信側で間隔をとってjoyメッセージ
をpublishします。
複数のメッセージを同時に受信した場合にそのままpublishすると、メッセージの間隔が変わってしまうため操作側の意図した通りに制御することが出来ません。なので、この案では1つ前のJoyメッセージ
をpublishした時刻から20ms経過していない場合は20msec経過してから次のJoyメッセージ
をpublishします。
この案のメリットは、遅延が発生しても正確な動作が行える点です。 一方で、一度遅延が発生するとその遅れは継続して引き継がれるというデメリットがあります。これにより、レスポンスが非常に悪くなります。
対策案2
対策案2では遅延が大きいjoyメッセージは使用しないで破棄します。今のところ受信側に届くまでにメッセージを捨てる仕組みは用意されていないため、受信側での判断が必要になります。
この案のメリットは、遅延の解消後はリアルタイム性のある制御が可能になる点です。
一方でデメリットは、途中で捨てた分のJoyメッセージ
が処理されなくなる点です。その結果、ネットワークが切断していた期間の動作は実行されなくなります。
Turtlebot3の遠隔制御はデモ用途で使用しています。したがって、所定の動作を確実に再現することよりもリアルタイム性の維持の方が重要になります。
そのため、今回は対策案2を採用しました。
具体的には、PS3コントローラ側のRaspberry Piで設定したタイムスタンプと、Turtlebot3側で受信した時点でのタイムスタンプを比較します。もしこの差が1秒以上離れていたら何等かの遅延が発生したと判断しそのJoyメッセージ
はpublishしません。
この機能を追加するために rosbridge_tcp
の使用をやめて、intdash EdgeからJoyメッセージ
を直接取得するようにしました。
この処理を実現するためにはPS3コントローラ側のRaspberry PiとTurtlebot3のRaspberry Piの時刻は同期が必要です。同じntpサーバーを使用して時刻同期をしているため、1秒以上の遅延が発生しているような状況を異常と判断するのはデモ用途においては妥当だと思います。
まとめ
今回は通信遅延発生時にTurtlebot3を安全に遠隔制御する技術についてご紹介しました。
先進技術調査グループではロボットの遠隔制御に限らず、プロトコルや機械学習に関連したテーマなど、様々な技術テーマの調査・検証を進めています。 今後も継続的に調査・検証の結果を記事として投稿できれば良いと思います。
最後までご覧いただきありがとうございました。