OSOYOO PI CARキット ROS2化 #10 – ORB_SLAM3 姿勢推定の改良 その1

OSOYOO PI CAR

前回で、ORB_SLAM3をROS2ノードとしての搭載は大部分完了したのですが、実際に動かしてみると、たまに姿勢を間違ってしまっているように見えます。
ピッチ15度くらいあるはずなのに、0度になってしまったりすることが度々あります。
今回は、姿勢推定精度向上のため、IMU情報をORB_SLAM3に入力しようと思います。
元々、ORB_SLAM3には、IMUの加速度、ジャイロ情報を入力するためのIFが用意されていますので、そこに計測したカメラ姿勢を入力していきます。
ついでですが、今までカメラはを単に超音波センサーの上に乗っけていましたが、これを機にカメラマウントを作ることにしました。
今回は、カメラマウントとそれを制御するROS2ノードを作成します。

■ カメラマウント

カメラマウントは、IMU、カメラの水平設置等など面倒なので、簡易的に常に水平を維持するようなマウントを廃材で作成しました。マウント側で水平に保ちつつ、カメラ姿勢を150HzでUSB Serialで送信するようにしています。姿勢維持に関してはラズパイでリアルタイム制御は、まず無理かと思います。
カメラマウントコントローラーは、IMUの情報取得、水平維持は手元にあったArduino Nano、MPU-6050、SG-90で構成しました。
カメラ自体が軽いことと、IMUを使用するので、重力式ではなくSG90で水平を維持することにしました。
部品点数は知れているので、ＰＣＢを作らず、ユニバーサル基板上にlinuxで通信可能なように回路を組んであります。（今回は簡単なので回路図書いてません。）
プログラムについては、本題とは違うMCU制御の説明となる上、内容が難しくなりますので、ざっと動きだけ、お伝えしょうと思います。
ここでは、こういうユニットを作ったのだと流していただけると助かります。
機会がありましたら、説明させていただこうと思います。

部品名個数補足
Arduino Nano1カメラマウントコントローラ。中華互換品。
MPU-6050 6DOF GY-52116軸IMU。ORB_SLAMには6軸で足りるので、手元にあった部品を使用。
0.1uF セラミックコンデンサ1リセット防止用。linux USB シリアル接続用。
GNDとReset間に挟むことで、open()時のMCUリセットを防止するためのコンデンサ。
16V 100uF電解コンデンサ1SG90２個動作対策用に、少し大きめパスコンを入れてある。プログラムも水平、垂直用SG90の動作タイミングを調整してArduino Nano自体がリブートしないように調整済み。
SG902手元にあった再利用部品を流用。
部品リスト

安定的に装着できるちょうどいい位置がなかったので、下記のようにバッテリーボックスに装着できるよう、3Dプリント部品をつくりました。

〇 動作確認

制作した装置をRaspberry PI Carに装着して動きを確認しました。
下記のような感じで水平を保ちます。
急激な姿勢の変化には耐えられないかもしれませんが、微小動作程度であればいけそうな感じです。

水平テスト

■ ROS2 カメラマウントノード

〇 設計方針

ROS2の作法に従って、カメラの姿勢情報をUSB Serial経由で受信し、ROS2標準のsensor_msgs/msg/Imuメッセージに変換し、publishするノードを設計します（下図参照）。
姿勢情報を直接/picar_map_mono(ORB_SLAM3を使用したマップ、位置情報生成ノード）でリードしない理由は、複数コントローラによる分散処理を可能にしたいからです。
このようにしておくと、Raspberry PI3で実行不可能なプログラムであっても、ＰＣ上では快適に動作します。
ROS2では、デバイスから取得した情報を、一旦ROS2メッセージに変換するノードを設けると、分散処理などの、ROS2の恩恵を受けることができます。
アルゴとデバイスノードは分けておいた方がＲＯＳ２設計ではお得だと思いますので、今回は、このような形態とします。

枠で囲ったノードが今回の設計対象

〇 処理内容

ROS2の基本的な制御方式のみ説明させていただこうと思います。
linux上でのSerial制御については、今回は本題ではないのと本題以上に長くなりそうですので、説明を省こうと思います。
主要箇所に赤、黄、青でマーカーを入れていますので、その箇所毎に説明します。

赤：/picar_cam/imu publisherの生成

カメラのIMU情報をROS2標準「sensor_msgs/msg/Imu」形式でメッセージ送信するためのpublisherを生成します。生成したpublisherは、青：「カメラIMU情報取得スレッド」で情報配信で使用します。

黄：カメラ IMU情報(USB serial)取得スレッドの生成

カメラ IMU情報取得するスレッド：「taskImuReader()」を生成します。
USB Serialからデータ取得する関係上、非同期でデータ受信する必要がありますので、スレッドで対処します。

青：カメラIMU情報取得スレッド

このスレッドは、下記のように動作します。

① スレッド起動時に、imu_initialize()でUSB serialを初期化します。
② 初期化後は、for(;;)文でUSB Serial経由でカメラIMU情報(緑字の構造体）を取得しつづけます。
③ 取得に成功すると、ROS2標準「sensor_msgs/msg/Imu」形式にデータ変換して、情報配信します。

この際にORB_SLAMへの入力情報として重要となるのが受信時間。
IMUの取得時間と画像を同期する術がないので、「受信開始時間ーUSB Serial通信時間」で、大体の時間を算出してます。
もっと確実にカメラとIMUの計測タイミングを知る仕組みがあれば、より正確になるかと思いますが、残念ながら、現状はそのような仕組みは設けていませんので、かなりの誤差がのることが前提となります。おそらく、Raspberry PIの負荷状況によって変化するはずですし、ORB_SLAMの推定精度にも関わると思います。

/*
 *  this contains the of camera.
 */
#include "rclcpp/rclcpp.hpp"

/*
 *  here's the definition of messages.
 */
#include "sensor_msgs/msg/imu.hpp"
#include "picar_interfaces/srv/pose_camera.hpp"

:
: 省略(Seral制御用コード）
:

/*
 *  here's the configuration of this file.
 */
#define CNF_MSG_DELAY   (int64_t)86800  /* is the time(ns) of delay.    */

:
: 省略(Seral制御用コード）
:

/*
 *  here's the message of IMU device.
 */
struct ImuMessage {
    float   acl_x;      /* is the x of acceleration(g). */
    float   acl_y;      /* is the y of acceleration(g). */
    float   acl_z;      /* is the z of acceleration(g). */

    float   gyro_x;     /* is the x of gyro(degree/sec).*/
    float   gyro_y;     /* is the y of gyro(degree/sec).*/
    float   gyro_z;     /* is the z of gyro(degree/sec).*/

    float   roll;       /* is the roll(degree).         */
    float   pitch;      /* is the pitch(degree).        */
};
typedef struct ImuMessage imumsg_t;

:
: 省略(Seral制御用コード）
:

/*
 *  here's the name space.
 */
using std::placeholders::_1;
using std::placeholders::_2;

class PicarCamNode : public rclcpp::Node
{
public:
    PicarCamNode() : Node("picar_cam")
    {
:
: 省略(Seral制御用コード）
:
        /*
         *  this creates publisher.
         *  No.1
         */
        imu_publisher_ =
            this->create_publisher<sensor_msgs::msg::Imu>("picar_cam/imu", 10);

        /*
         *  this starts imu reader.
         *  No.2
         */
        th_imu_ =
            std::thread(std::bind(&PicarCamNode::taskImuReader, this));

        RCLCPP_INFO(this->get_logger(), "camera controller has been started.");
    }

private:
:
: 省略(Seral制御用コード）
:
    /* here's the imu publisher.*/
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;

    /*
     *  purpose :   entry to read imu.
     *  No.3
     */
    void taskImuReader()
    {
        auto msg = sensor_msgs::msg::Imu();
        imumsg_t    rawmsg;
        std_msgs::msg::Header::_stamp_type  stamp;

        if (imu_initialize() == -1) {
            return;
        }

        RCLCPP_INFO(this->get_logger(), "the camera imu has been started");

        /*
         *  this read imu.
         *  this generate ros2 sensor message from raw imu message.
         */
        for (;;) {
            if (imu_read(&rawmsg, &msg.header.stamp) == 0) {
                msg.header.frame_id = "camera_imu";
                msg.angular_velocity.x = rawmsg.gyro_x;
                msg.angular_velocity.y = rawmsg.gyro_y;
                msg.angular_velocity.z = rawmsg.gyro_z;
                msg.linear_acceleration.x = rawmsg.acl_x;
                msg.linear_acceleration.y = rawmsg.acl_y;
                msg.linear_acceleration.z = rawmsg.acl_z;
                msg.orientation.x =
                msg.orientation.y =
                msg.orientation.z = 0.0;
                msg.orientation.w = 1.0;
                imu_publisher_->publish(msg);
            }
        }
    }

:
: 省略(Seral制御用コード）
:
int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<PicarCamNode>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

■ 次回

今回は、まずはカメラ姿勢（IMU情報）を取得できるようにしました。
次回は、ORB_SLAM3にIMU情報をインプットしてみようと思います。
また、よろしければ見ていただけるとありがたいです。

タイトルとURLをコピーしました