Asterx-SBI 3 pro IMU orientation/lever arms leading to flyaway in position estimation

[federicobenna]

I have mobile robot setup with INS receiver + dual antenna setup.

My robot model is as follow:

<?xml version="1.0" ?>
<robot name="robot">
  <link name="/base_link"/>
  <link name="/imu_septentrio"/>
  <joint name="/base_link_to_imu_septentrio_joint" type="fixed">
    <parent link="/base_link"/>
    <child link="/imu_septentrio"/>
    <origin rpy="0 0 1.5708" xyz="-0.0973 -0.0296 0.23457"/>
  </joint>
  <link name="/ant_septentrio_aux"/>
  <joint name="/imu_septentrio_to_ant_septentrio_main_joint" type="fixed">
    <parent link="/imu_septentrio"/>
    <child link="/ant_septentrio_main"/>
    <origin rpy="0 0 0" xyz="-0.129 -0.069 -0.02"/>
  </joint>
  <link name="/ant_septentrio_main"/>
  <joint name="/ant_septentrio_main_to_ant_septentrio_aux_joint" type="fixed">
    <parent link="/ant_septentrio_main"/>
    <child link="/ant_septentrio_aux"/>
    <origin rpy="0 0 0" xyz="0.38 0.004 0.0"/>
  </joint>
  <link name="/vsm"/>
  <joint name="/base_link_to_vsm_joint" type="fixed">
    <parent link="/base_link"/>
    <child link="/vsm"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
</robot>

which results in this robot model (top view):

Receiver IMU has Z axis pointing up and Y opposite to robot's forward direction. Here's also a sketch of the robot:

The configuration yaml file does use both use_ros_axis_orientation: true, get_spatial_config_from_tf: true and configure_rx: true, so I expect the ros driver node to reconfigure the receiver once I launch the node using the information from my robot model. However, if I then check the web UI for INS settings, I see the receiver/imu orientation is set correctly, but lever arms (IMU<>MainARP and IMU<>base link) do not match what I would expect correctly set? What am i missing here?

This has clearly serious implications as the imu information is then poorly in the receiver and leads to flyaway estimates (red dots) when compared with another location information provider (blue dots)

[thomasemter]

Hi @federicobenna,

I am sorry for the inconvenience. I must admit, that the tf feature has not been tested extensively and there might be an error in such a way that the lever arms are defined in a different orientation via tf (IMU vs. vehicle). I have to confirm this.

[thomasemter]

I have a fix here. Can you please try it?

On a side note, your robot model top view does not match with what I get. I.e., there is no rotation between imu and ant_septentrio_main in my view:

[federicobenna]

Hi @thomasemter, Thanks for the quick reply!

On a side note, your robot model top view does not match with what I get. I.e., there is no rotation between imu and ant_septentrio_main in my view:

Indeed, somehow that model is missing a 90 deg angle rotation between imu and main antenna :/ for reference, here's an updated version that match the one I have been using:

<?xml version="1.0" ?>
<robot name="robot">
  <link name="/base_link"/>
  <link name="/imu_septentrio"/>
  <joint name="/base_link_to_imu_septentrio_joint" type="fixed">
    <parent link="/base_link"/>
    <child link="/imu_septentrio"/>
    <origin rpy="0 0 1.5708" xyz="-0.0973 -0.0296 0.23457"/>
  </joint>
  <link name="/ant_septentrio_aux"/>
  <joint name="/imu_septentrio_to_ant_septentrio_main_joint" type="fixed">
    <parent link="/imu_septentrio"/>
    <child link="/ant_septentrio_main"/>
    <origin rpy="0 0 -1.5708" xyz="-0.129 -0.069 -0.02"/>
  </joint>
  <link name="/ant_septentrio_main"/>
  <joint name="/ant_septentrio_main_to_ant_septentrio_aux_joint" type="fixed">
    <parent link="/ant_septentrio_main"/>
    <child link="/ant_septentrio_aux"/>
    <origin rpy="0 0 0" xyz="0.38 0.004 0.0"/>
  </joint>
  <link name="/vsm"/>
  <joint name="/base_link_to_vsm_joint" type="fixed">
    <parent link="/base_link"/>
    <child link="/vsm"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
</robot>

I have a fix here. Can you please try it?

That does the trick! WebUI now matches urdf and reality, see:

I also took the robot for a quick spin and Septentrio and the other localization results now agree!

Would it be possible to officially release this patch?

[thomasemter]

Hi @federicobenna,

great, thank you very much for testing. Yes, we will release the fix with the next update.