6 Robot state estimation

According to different models, you only need to set the purchased model in [.bashrc], X1 (ordinary four-wheel drive) X3 (Mike wheel) X3plus (Mike wheel mechanical arm) R2 (Ackerman differential) and so on. Section takes X3 as an example

Open the [.bashrc] file

sudo vim .bashrc 

Find the [ROBOT_TYPE] parameter and modify the corresponding model

xxxxxxxxxx
export ROBOT_TYPE=X3    # ROBOT_TYPE: X1 X3 X3plus R2 X7 

6.1 Start

6.1.1 Code reference path

xxxxxxxxxx
~/yahboomcar_ws/src/yahboomcar_bringup/launch/bringup.launch 

6.1.1 Start

xxxxxxxxxx
roslaunch yahboomcar_bringup bringup.launch use_rviz:=true 

6.1.1 View tf tree and node graph

1. view the TF tree

xxxxxxxxxx
rosrun rqt_tf_tree rqt_tf_tree 

2. view the node graph

xxxxxxxxxx
rosrun rqt_graph rqt_graph 

6.1.2 launch file analysis

In the bringup.launch file, there are several important nodes

1. /drive_node

This node mainly publishes /imu/imu_raw and /vel_raw data, terminal input

xxxxxxxxxx
rosnode info /driver_node  

As can be seen,

• The /driver_node node publishes the /imu/imu_raw topic data to the /imu_filter_madgwick node, which filters and fuses the imu data;
• The /driver_node node publishes the /vel_raw topic data to the /odometry_publisher node. After the latter is integrated, the /odom_raw data is published;

2. /odometry_publisher

This node mainly receives the vel_raw topic data sent by the /driver_node node, publishes the /odom_raw topic data to /ekf_localization, and publishes the /tf topic data to other nodes, terminal input,

xxxxxxxxxx
rosnode info /odometry_publisher 

3. /imu_filter_madgwick

This node mainly filters and fuses imu data, and then publishes the processed /imu/imu_data topic data to the /ekf_localization node to receive and publish /tf data to other nodes, terminal input,

xxxxxxxxxx
rosnode info /imu_filter_madgwick 

4. /ekf_localization

This node mainly integrates imu data and odom data and publishes tf data, terminal input,

xxxxxxxxxx
rosnode info /ekf_localization 

Next, we describe these nodes in detail.

6.1.3 imu_filter_madgwick

1 Introduction

IMU refers to a six-axis sensor that includes a gyroscope and an accelerometer. MARG refers to a nine-axis sensor that adds a magnetometer to the IMU.

xxxxxxxxxx
IMU  =  gyroscope  +  accelerometer 
MARG (Magnetic, Angular  Rate, and  Gravity)  =  gyroscope  +  accelerometer  +  magnetometer 

Madgwick is an Orientation Filter that filters and fuses raw data from IMU devices. It fuses angular velocity, acceleration, and (optional) magnetometer readings from generic IMU devices into orientation quaternions, and publishes the fused data on the IMU topic, regardless of the overall IMU integration process.

2. topic

3. parameters

6.1.4 robot_localization

1 Introduction

robot_localizationis a collection of state estimation nodes, each of which is an implementation of a nonlinear state estimator for a robot moving in 3D space. It includes two state estimation nodes ekf_localization_nodeand ukf_localization_node. in addition, robot_localizationsupply navsat_transform_node, it helps to integrate GPS data.

ekf_localization_nodeis Extended Kalman Filter . It uses an omnidirectional motion model to predict states in time and uses sensed sensor data to correct the predicted estimates.

ukf_localization_nodeis unscented Kalman filter . It uses a carefully chosen set of sigma points to project the state through the same motion model used in the EKF, and then uses these projected sigma points to recover the state estimate and covariance. This eliminates the use of the Jacobian matrix and makes the filter more stable. However, with ekf_localization_nodeIt is also more computationally heavy in comparison.

2. topic

3. parameters

• frequency: The true frequency (in Hz) at which the filter produces state estimates. Note: The filter does not start computing until it has received at least one message from one of the inputs.

• [sensor] : For each sensor, the user needs to define this parameter according to the message type. Each parameter name is indexed from 0 (e.g. odom0, odom1, etc.) and must be defined in order (e.g. don't use pose0 and pose2 if pose1 is not already defined). The value of each parameter is the topic name for that sensor.

  xxxxxxxxxx
  odom0: /odom_raw 
  

  xxxxxxxxxx
  imu0:/imu/data 
  

• [sensor]_differential: For each sensor message defined above that contains pose information, the user can specify whether the pose variables should be integrated differentially. If the given value is set to true, then for the measurement taken at time t from the relevant sensor, we will first subtract the measurement at time t-1 and then convert the resulting value to velocity.

  xxxxxxxxxx
  ~odomN_differential 
  

  xxxxxxxxxx
  ~imuN_differential 
  

  xxxxxxxxxx
  ~poseN_differential 
  

• [sensor]_relative: If this parameter is set to true, any measurements from this sensor will be fused relative to the first measurement received from this sensor. This is useful, for example, if you want the state estimate to always start at (0,0,0) and the roll, pitch and yaw angle values to be (0,0,0).

  xxxxxxxxxx
  ~odomN_relative 
  

  xxxxxxxxxx
  ~imuN_relative 
  

  xxxxxxxxxx
  ~poseN_relative 
  

• two_d_mode: Set this to true if your robot operates in a flat environment and can ignore small changes in the ground (as reported by the IMU). It fuses all 3D variables (Z, roll, pitch and their respective velocity and acceleration) into a value of 0. This ensures that the covariances of these values don't explode, while ensuring that your robot's state estimate remains fixed on the XY plane.

• odom0_config ： [false, false, false, false, false, false, true, true, false, false, false, true, false, false, false]

  The order of Boolean values is: [[X],[Y],[Z],[roll],[pitch],[yaw],[X ' ],[Y ' ],[Z ' ],[roll '],[pitch '],[yaw '],[X ''],[Y ''],[Z '']]. The user must specify which variables of these messages should be fused into the final state estimate.

4. the published transformation

If the user's world_frameparameter is set to odom_framevalue, the conversion from odom_frameThe coordinate system given by the parameter is published to base_link_frameThe coordinate system given by the parameter. If the user's world_frameparameter is set to map_framevalue, the conversion from map_frameThe coordinate system given by the parameter is published to odom_frameThe coordinate system given by the parameter.

For example, we set to publish the transformation from the coordinate system given by the [odom_frame] parameter to the coordinate system given by the [base_link_frame] parameter.

xxxxxxxxxx
odom_frame: odom 
base_link_frame: base_footprint 
world_frame: odom