本センサを用いた姿勢推定の手法について

[y-abe-c]

USB出力9軸IMUセンサモジュール・rt_usb_9axisimu_driverを用いてロボットの姿勢推定を行いたいと考えています。 rt_usb_9axisimu_driverの /imu/data_raw メッセージから imu_filter_madgwick を使用してorientationを求めたいのですが、 下図のように実際とは大きく異なる姿勢が出力されます。

右上に大きく表示されている座標軸が推定値、左下が実際の姿勢（基板に表示されている向きと同じ）になります。 これを修正する手法がありましたらご教示いただけますと幸いです。

環境

• OS : Ubuntu 20.04
• ROS2 version : galactic
• 使用パッケージ：rt_usb_9axisimu_driver・imu_tools/imu_filter_madgwick
  • rt_usb_9axisimu_driverはソースからインストール、imu_filter_madgwickはバイナリでインストール
• キャリブレーション実行済み

  • 44 の方法で実行

• ファームウェアバージョン：2.0
• 上図はRviz2を使用しています

launchファイル

import os
import launch
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch.actions import (DeclareLaunchArgument, EmitEvent, RegisterEventHandler)
from launch.event_handlers import OnProcessStart
from launch_ros.actions import Node
from launch.events import matches_action
from launch_ros.actions import LifecycleNode
from launch_ros.event_handlers import OnStateTransition
from launch_ros.events.lifecycle import ChangeState
from lifecycle_msgs.msg import Transition
import launch
import launch.actions
import launch.event_handlers.on_process_start
import launch_ros.actions
import launch_ros.events
import launch_ros.events.lifecycle
import lifecycle_msgs.msg

def generate_launch_description():

    driver = launch_ros.actions.LifecycleNode(
        name='rt_usb_9axisimu_driver',
        package='rt_usb_9axisimu_driver',
        executable='rt_usb_9axisimu_driver',
        output='screen',
        namespace=''
    )
    to_inactive = launch.actions.EmitEvent(
       event=launch_ros.events.lifecycle.ChangeState(
           lifecycle_node_matcher=launch.events.matches_action(driver),
           transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
       )
   )
    to_active = launch.actions.EmitEvent(
       event=launch_ros.events.lifecycle.ChangeState(
           lifecycle_node_matcher=launch.events.matches_action(driver),
           transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE,
       )
   )

    filter_imu = launch_ros.actions.Node(
                package='imu_filter_madgwick',
                executable='imu_filter_madgwick_node',
                name='imu_filter',
                output='screen',
                parameters=[
                    {'stateless': False},
                    {'use_mag': True},
                    {'publish_tf': False},
                    {'reverse_tf': False},
                    {'publish_debug_topics': True},
                    {'constant_dt': 0.0},
                    {'world_frame' : "enu"},
                    {'gain': 0.1},
                    {'zeta': 0.0},
                    {'mag_bias_x': 0.0},
                    {'mag_bias_y': 0.0},
                    {'mag_bias_z': 0.0},
                    {'orientation_stddev': 0.0},
                ],
            )

    return LaunchDescription([
        driver,
        to_inactive,
        to_active, 
        filter_imu,
    ])

[y-abe-c]

すみません、解決しました。 Z軸加速度キャリブレーション実行時に、センサを垂直に立てていなかったのが原因でした。 失礼いたしました。