0
$\begingroup$

I have an Ackermann steered robot with optical flow sensor mounted at an offset from base_link x = 0, y = 0.4 , z = 0.15 and robot_state_publisher publishes static transform between base_link as parent and optical_flow_link as child. which outputs odometry in x and y as distances and I am calculating the velocities from the distances. Both verified with manual measurements over known distances and reports reasonably good. Now for yaw, I don't have any other sensor and I am using steering feedback from the actuator (nearly linear relationship) and finding the angle and using to find the Angular velocity (Vyaw) and then delta_theta.

Vyaw = Vx*tan(current_angle)/wheelbase
delta_theta = Vyaw*delta_time
Absolute_theta +=delta_theta

Now, I visualised in rviz for a full 360 drive, it showed a circle so, I continued with this approach. Now,I have also read that this is not the direct approach and the error will be increasing. So, I tried to fuse the information of this odom message with twist fields Vx, Vy and Vyaw and added process_noise_covariance as well. As I said I do not believ fully rely on steering input so I added covariance value of 0.1 in Vyaw. I had good values for Vx abd Vy so added small covariance.

I believe it works according to ROS REP105 and REP103.(correct me if I am missing something here

Now I started slam_toolbox and in the map frame for straight motion the yaw from slam and from EKF starts differs by 2 degrees which sometimes increases and when I turn the robot, it sometimes stays same but some after reversing and turning bit sharper the robot the yaw differs more and NAV2 due to above enters into lethal zones.

In rviz2, I visualize the covariance circle keeps getting larger which I think means its growing without bounds.

Optical flow sensor output with angular velocity computed from steering feedback

  header:
   stamp:
   sec: 1711996544
   nanosec: 364230972
  frame_id: odom
 child_frame_id: base_link
pose:
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
twist:
  twist:
    linear:
      x: 0.1738306736874741
      y: 0.013906453894997927
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: -0.024044470724701637
  covariance:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0

Here is the EKF odometry message:

header:
  stamp:
    sec: 1711996556
    nanosec: 323442581
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: -1.6793282770947027
      y: 0.22047952088394343
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: -0.1492351199844369
      w: 0.9888017389564152
  covariance:
  - 23.880361414199236
  - 50.379086083150106
  - 0.0
  - 0.0
  - 0.0
  - 15.01899733839927
  - 50.3790860831498
  - 127.52964039306863
  - 0.0
  - 0.0
  - 0.0
  - 33.59412032164872
  - 0.0
  - 0.0
  - 2.0435926596699186e-07
  - -2.2435245497600878e-13
  - -4.900634829392862e-12
  - 0.0
  - 0.0
  - 0.0
  - -2.2435245497600872e-13
  - 9.991993723637299e-07
  - -1.3818676719951415e-18
  - 0.0
  - 0.0
  - 0.0
  - -4.900634829392863e-12
  - -1.3818676719950882e-18
  - 9.991993723336081e-07
  - 0.0
  - 15.01899733839927
  - 33.59412032164872
  - 0.0
  - 0.0
  - 0.0
  - 17.329188507970443
twist:
  twist:
    linear:
      x: 0.1546616554043215
      y: -0.021090222967886374
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.013265298939958113
  covariance:
  - 9.999995111518736e-10
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 9.999998026597455e-10
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 9.921069996240334e-07
  - -5.41735905038207e-19
  - -1.1682705572021648e-17
  - 0.0
  - 0.0
  - 0.0
  - -5.417359050382068e-19
  - 8.538481248084917e-07
  - -5.2559712026548233e-23
  - 0.0
  - 0.0
  - 0.0
  - -1.1682705572021646e-17
  - -5.255971202654596e-23
  - 8.538481248084905e-07
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 9.99999809068553e-10

Another thing, my sensor for slam is Intel D435 RGB-D and not lidar which might be not properly suitable for slam_toolbox but it should do basic moves and I tuned slam_toolbox a bit to reduce the jumping of the robot too much from previous runs as well.

I am currently looking to integrate the rtabmap rgb-d odomettry with this and tell the EKF that use rtab's odometry for yaw but when rtab's odometry is lost then use the steering angle odometry until the rgb-d odom is back in action. I tried this but then EKF just uses rtab's odom and when its lost there is sudden big jumps.

My current EKF params.

### ekf config file ###
ekf_filter_node:
    ros__parameters:
        
        frequency: 20.0
        sensor_timeout: 0.05       
        two_d_mode: true

        transform_time_offset: 0.2     
        transform_timeout: 0.1
        print_diagnostics: true

        debug: false
        permit_corrected_publication: false
        publish_acceleration: false
        publish_tf: true
        
        #map_frame: map              # Defaults to "map" if unspecified
        odom_frame: odom            # Defaults to "odom" if unspecified
        base_link_frame: base_link  # Defaults to "base_link" if unspecified
        world_frame: odom           # Defaults to the value of odom_frame if unspecified

        odom0: /optical_flow_odom

        odom0_config: [false,  false,  false,
                       false, false, false,
                       true, true, false,
                       false, false, true,
                       false, false, false]
        
        # odom1: /rtabmap_odom

        # odom1_config: [true,  true,  false,
        #                false, false, true,
        #                false, false, false,
        #                false, false, false,
        #                false, false, false]


        process_noise_covariance: [0.001, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.005, 0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.06, 0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.03, 0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.03, 0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.02, 0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.025, 0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.025, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.04, 0.0,    0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.01, 0.0,    0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.01, 0.0,    0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.1, 0.0,    0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.01, 0.0,    0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.01, 0.0,
                               0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.0,     0.0,     0.0,    0.0,    0.0,    0.0,    0.0,    0.0,    0.015]

# dynamic_process_noise_covariance: true

This is the link to the bag file with ekf odometry topic , slam's pose topic and a navigation goal. There is some need to tweak controller params of navigation but this is prior thing I think needs to resolve for a reasonable estimate.

https://github.com/Rak-r/ROS1-ROS2-contents/tree/main/nav2_run_1

Any guidance will be much appreaciated.@automatom

Expected

To run slam with ekf generating the odom to base_link transform. and setup the NAV2 stack

$\endgroup$
3
  • $\begingroup$ Update: I have fused the full odom message (x, y, yaw, vx, vy, vyaw) and checked the slam now outputs the same distance values but the yaw is still hanging a similar situation. What to tune in process noise covariance. Also, I try to add measurement covariance in odometry message with fixed value for twist filed vx, vy, vyaw $\endgroup$ Apr 8 at 11:44
  • $\begingroup$ I don't have the cycles to download your bags and analyse them, I'm afraid. Please add a sample message from every sensor input to the question itself. You should also back up a bit: what are you trying to do? Are you wanting to run SLAM and use the EKF to generate the odom->base_link transform? $\endgroup$
    – automatom
    Apr 16 at 8:28
  • $\begingroup$ I have added the sensor message which I am using. @automatom. Yes, I am trying to setup slam and NAV2 stack. I have been trying with slam_toolbox and rtabmap. But this is the only bit I am getting wrong and trying to figure out. $\endgroup$ 2 days ago

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.