通信遅延発生時にTurtlebot3を安全に遠隔制御する技術

f:id:aptpod-tetsu:20200605094629j:plain

先進技術調査グループのエンジニアの酒井 (@neko_suki)です。

過去に2回、Turtlebot3の遠隔制御について紹介をしました。

tech.aptpod.co.jp

tech.aptpod.co.jp

今回は、ネットワークの切断やほかの要因によって大幅な通信遅延が発生した際に、Turtlebot3を安全に遠隔制御技術する技術として以下の2点についてご紹介します。

①フェールセーフ機能

②メッセージの遅延への対応

Turtlebot3の遠隔制御は、PS3コントローラからのメッセージをクラウド経由でTurtlebot3に届けることで実現しています。

PS3コントローラはRaspberry Pi3にBluetoothで接続されています。そのRaspberry Pi3上で弊社製品のintdash Edgeが動作しています。intdash EdgeはTurtlebot3側のRaspberry Piにも搭載されています。この二つが弊社のクラウドを経由して通信を行っています。

f:id:aptpod_tech-writer:20200601165248p:plain
全体の構成

Wi-FiやLTEなどの公衆無線では、通信環境が悪くなると、ネットワークが一時的に切断されたり、遅延が発生することがあります。

f:id:aptpod_tech-writer:20200601170153p:plain

そのような状況で発生する課題とその対応策として、以下の2点について検討しました。

①フェールセーフ機能

②メッセージの遅延への対応

①フェールセーフ機能の検討

フェールセーフ機能とは何か

Turtlebot3のようにロボットを遠隔制御する場合、Wi-FiやLTEなどの公衆無線では、通信環境が悪くなるとネットワークが一時的に切断されたり遅延が発生することがあります。そのため、そのような状況を想定して安全に制御するためのフェールセーフ機能が必要になります。

以下の動画で、実際に実機を使い意図的にネットワークを切断した状態を発生させて何が起こるのかを確認しました。動画では、タイヤが制御されるようにPS3コントローラのスティックを固定しながら、Turtlebot3に接続している有線を抜きます(注: Wi-FiはOFFにしています)。有線を抜いた後もタイヤが動き続ける様子が確認できます。

www.youtube.com

動画では台の上にTurtlebot3を固定していますが、もし実際に地面の上を動いているときにネットワークの切断などが発生すると、そのまま動き続け障害物や壁などに衝突する可能性があります。

f:id:aptpod_tech-writer:20200601181113p:plain

フェールセーフ機能対応前の処理フロー

Turtlebot3では、PS3コントローラ側のRaspberry Piから送信されたjoyメッセージをintdash Edgeを経由して rosbridge_tcpで受け取ります。rosbridge_tcpは受け取ったjoyメッセージをpublishします。publishされたjoyメッセージは、タイヤを制御するteleop_twist_joyノードと アームを制御するteleop_arm_node ノードにsubscribeされます。

f:id:aptpod_tech-writer:20200601172530p:plain
ノード構成

teleop_arm_node は受け取ったjoyメッセージから、移動後のアーム座標を計算した joint_trajectory_point というFloat64MultiArray型 のメッセージを生成してpublishします。

f:id:aptpod_tech-writer:20200602154013p:plain
アーム制御(ROSノード)

Turtlebot3はその座標情報をもとに、モーターを動かします。

f:id:aptpod_tech-writer:20200601180241p:plain
アーム制御の結果

一方、teleop_twist_joy は受け取ったjoyメッセージから、速度を計算したcmd_velというTwist型 のメッセージを生成してpublishします。

f:id:aptpod_tech-writer:20200602160132p:plain
タイヤ制御(ROSノード)

速度と角度を基に、タイヤの制御が行われます。

f:id:aptpod_tech-writer:20200601180658p:plain
タイヤ制御の結果

アームの制御では、1つのjoyメッセージから移動後の座標を計算するため、ネットワークの切断や遅延によってメッセージが到達しない場合には、アームは動作しません。

f:id:aptpod_tech-writer:20200602154058p:plain
通信切断時のアーム制御(ROSノード)

f:id:aptpod_tech-writer:20200601180906p:plain
通信切断時のアーム制御

しかし、タイヤの制御はそうはなりません。ネットワークの切断や遅延によってメッセージが到達しない場合には、過去に設定された速度・角度の情報がそのまま使われます。

f:id:aptpod_tech-writer:20200601181043p:plain
通信切断時のタイヤ制御(ROSノード)

その結果、例えば障害物などがあると避けられずにぶつかってしまいます。

f:id:aptpod_tech-writer:20200601181113p:plain
通信切断時のタイヤ制御の結果

フェールセーフ機能対応後の処理フロー

フェールセーフ機能に対応するために、cmd_vel_stopper というROSノードを追加しました。

cmd_vel_stopperはネットワークの切断や遅延を検出するために、一定時間Joyメッセージが受信できなかった場合に、速度を0にしたcmd_vel をpublishするようにします。速度が0のcmd_vel をpublishすると、タイヤの制御が停止します。

ROSノードの構成は以下のようになります。

cmd_vel_stopperjoy メッセージをsubscribeします。cmd_vel_stopperJoyメッセージを受信したタイミングを記録します。 そして内部で、100msec毎に最後にJoyメッセージ を受信したタイミングを監視します。もし現在時刻と最後にJoyメッセージ を受信したタイミングが200msecよりも大きいなら、速度を0にしたcmd_vel をpublishします。これによって、タイヤを停止します。

f:id:aptpod_tech-writer:20200601182516p:plain
フェールセーフ機能を実装したROSノード構成

f:id:aptpod_tech-writer:20200601183105p:plain
フェールセーフ機能を入れた結果

実際に実機を使って確認した動画が以下の動画になります。先ほどとは異なり、有線を抜くとタイヤが止まることが確認できました。

www.youtube.com

これによって、ネットワークの切断や遅延があってもTurtlebot3を安全に停止させるフェールセーフ機能が実現できました。

②メッセージの遅延への対応

メッセージ遅延による課題

次は、メッセージが遅延した場合の処理について考えます。

現在の実装では、コントローラ側から20ms間隔でJoyメッセージを発行します。このJoyメッセージはクラウドを経由するためネットワーク遅延分だけ遅れてTurtlebot3 に到達します。

f:id:aptpod_tech-writer:20200602152037p:plain
ネットワーク遅延

JoyメッセージがPS3コントローラ側のRaspberry PiからTurtlebot3に到達するまでに、一時的なネットワークの切断などによって大きな遅延が発生したとします。そうすると、ネットワークの状況が回復した際に、過去のJoyメッセージが含まれた複数のメッセージが到達します。

そのままJoyメッセージをpublishすると、遅延によるレスポンスの悪化と、メッセージ間隔が維持されないことによる再現性の悪化の2つの問題が発生します。

レスポンスが悪化すると、例えば10秒の切断が発生したら10秒後を予想して操作をしなければいけません。

また、操作の再現性が失われると、意図しない動作になってしまう可能性があります。

したがって、ここにも対応が必要になります。

f:id:aptpod_tech-writer:20200602101915p:plain
課題

メッセージ遅延への対策案

対策案1

対策案1では受信側で間隔をとってjoyメッセージをpublishします。

複数のメッセージを同時に受信した場合にそのままpublishすると、メッセージの間隔が変わってしまうため操作側の意図した通りに制御することが出来ません。なので、この案では1つ前のJoyメッセージをpublishした時刻から20ms経過していない場合は20msec経過してから次のJoyメッセージをpublishします。

f:id:aptpod_tech-writer:20200601184705p:plain
対策案1

この案のメリットは、遅延が発生しても正確な動作が行える点です。 一方で、一度遅延が発生するとその遅れは継続して引き継がれるというデメリットがあります。これにより、レスポンスが非常に悪くなります。

対策案2

対策案2では遅延が大きいjoyメッセージは使用しないで破棄します。今のところ受信側に届くまでにメッセージを捨てる仕組みは用意されていないため、受信側での判断が必要になります。

f:id:aptpod_tech-writer:20200601184841p:plain
対策案2

この案のメリットは、遅延の解消後はリアルタイム性のある制御が可能になる点です。 一方でデメリットは、途中で捨てた分のJoyメッセージ が処理されなくなる点です。その結果、ネットワークが切断していた期間の動作は実行されなくなります。

Turtlebot3の遠隔制御はデモ用途で使用しています。したがって、所定の動作を確実に再現することよりもリアルタイム性の維持の方が重要になります。

そのため、今回は対策案2を採用しました。

具体的には、PS3コントローラ側のRaspberry Piで設定したタイムスタンプと、Turtlebot3側で受信した時点でのタイムスタンプを比較します。もしこの差が1秒以上離れていたら何等かの遅延が発生したと判断しそのJoyメッセージ はpublishしません。

この機能を追加するために rosbridge_tcp の使用をやめて、intdash EdgeからJoyメッセージ を直接取得するようにしました。

f:id:aptpod_tech-writer:20200601185441p:plain
対策案2 ノード構成

この処理を実現するためにはPS3コントローラ側のRaspberry PiとTurtlebot3のRaspberry Piの時刻は同期が必要です。同じntpサーバーを使用して時刻同期をしているため、1秒以上の遅延が発生しているような状況を異常と判断するのはデモ用途においては妥当だと思います。

まとめ

今回は通信遅延発生時にTurtlebot3を安全に遠隔制御する技術についてご紹介しました。

先進技術調査グループではロボットの遠隔制御に限らず、プロトコルや機械学習に関連したテーマなど、様々な技術テーマの調査・検証を進めています。 今後も継続的に調査・検証の結果を記事として投稿できれば良いと思います。

最後までご覧いただきありがとうございました。