零漂数据采集拟合

scripts/base_feedback_monitor.py

@@ -66,9 +66,9 @@ class BaseFeedbackMonitor(Node):
             "robotvel_x",
             "robotvel_y",
             "robotvel_z",
-            "gyro_raw_z_rad_s",
+            "gyro_z_filtered_pre_bias",
-            "gyro_bias_fit_z_rad_s",
+            "gyro_z_bias_model",
-            "gyro_corrected_z_rad_s",
+            "gyro_z_final_for_yaw",
         ])
 
         self.start_time = self.get_clock().now()
@@ -169,9 +169,9 @@ class BaseFeedbackMonitor(Node):
             "robotvel_x": self.robotvel_msg.x if self.robotvel_msg is not None else None,
             "robotvel_y": self.robotvel_msg.y if self.robotvel_msg is not None else None,
             "robotvel_z": self.robotvel_msg.z if self.robotvel_msg is not None else None,
-            "gyro_raw_z_rad_s": self.gyrodebug_msg.x if self.gyrodebug_msg is not None else None,
+            "gyro_z_filtered_pre_bias": self.gyrodebug_msg.x if self.gyrodebug_msg is not None else None,
-            "gyro_bias_fit_z_rad_s": self.gyrodebug_msg.y if self.gyrodebug_msg is not None else None,
+            "gyro_z_bias_model": self.gyrodebug_msg.y if self.gyrodebug_msg is not None else None,
-            "gyro_corrected_z_rad_s": self.gyrodebug_msg.z if self.gyrodebug_msg is not None else None,
+            "gyro_z_final_for_yaw": self.gyrodebug_msg.z if self.gyrodebug_msg is not None else None,
         }
 
     def log_snapshot(self) -> None:
@@ -270,9 +270,9 @@ class BaseFeedbackMonitor(Node):
             gyro = self.gyrodebug_msg
             sys.stdout.write(
                 "gyro debug:      "
-                f"raw_z={self._fmt_float(gyro.x, 6)} "
+                f"pre_bias={self._fmt_float(gyro.x, 6)} "
                 f"bias_z={self._fmt_float(gyro.y, 6)} "
-                f"corr_z={self._fmt_float(gyro.z, 6)} rad/s\n"
+                f"final_z={self._fmt_float(gyro.z, 6)} rad/s\n"
             )
         else:
             sys.stdout.write("gyro debug:      N/A\n")

scripts/fit_gyro_bias.py

@@ -13,12 +13,14 @@ def load_samples(csv_path: Path):
     with csv_path.open("r", encoding="utf-8", newline="") as f:
         reader = csv.DictReader(f)
         fieldnames = reader.fieldnames or []
-        if "gyro_raw_z_rad_s" in fieldnames:
+        if "gyro_z_filtered_pre_bias" in fieldnames:
+            source_name = "gyro_z_filtered_pre_bias"
+        elif "gyro_raw_z_rad_s" in fieldnames:
             source_name = "gyro_raw_z_rad_s"
         elif "imu_angular_velocity_z" in fieldnames:
             source_name = "imu_angular_velocity_z"
         else:
-            raise ValueError("CSV does not contain gyro_raw_z_rad_s or imu_angular_velocity_z")
+            raise ValueError("CSV does not contain gyro_z_filtered_pre_bias, gyro_raw_z_rad_s, or imu_angular_velocity_z")
         for row in reader:
             t = row.get("elapsed_s")
             z = row.get(source_name)

src/origincar_base/include/origincar_base/origincar_base.h

@@ -221,8 +221,15 @@ private:
 	Vel_Pos_Data Robot_Vel;
 	MPU6050_DATA Mpu6050_Data;
 	float Power_voltage;
-	float stationary_velocity_threshold_;
+	float gyro_z_bias_intercept_;
-	float stationary_yaw_rate_threshold_;
+	float gyro_z_bias_slope_;
+	float gyro_z_raw_from_mcu_;
+	float gyro_z_after_startup_bias_;
+	float gyro_z_after_median_;
+	float gyro_z_filtered_pre_bias_;
+	float gyro_z_bias_model_;
+	float gyro_z_final_for_yaw_;
+	double gyro_z_bias_elapsed_s_;
 	float gyro_z_low_pass_alpha_;
 	float gyro_z_low_pass_;
 	std::array<float, 3> gyro_z_median_window_;

src/origincar_base/launch/base_serial.launch.py

@@ -1,6 +1,7 @@
 from launch import LaunchDescription
 import launch_ros.actions
 
+
 def generate_launch_description():
     robot_parameters = [
         {'usart_port_name': '/dev/ttyACM0',
@@ -10,6 +11,9 @@ def generate_launch_description():
          'cmd_vel': 'cmd_vel',
          'akm_cmd_vel': 'none',
          'product_number': 0,
+         'gyro_z_bias_intercept': 0.0,
+         'gyro_z_bias_slope': 0.0,
+         'gyro_z_low_pass_alpha': 0.6,
          'odom_pose_cov_x': 0.01,
          'odom_pose_cov_y': 0.01,
          'odom_pose_cov_yaw': 0.0225}

src/origincar_base/src/origincar_base.cpp

@@ -16,8 +16,6 @@ int Init_imu_num = 0;
 float gyro_z_sum = 0;
 namespace
 {
-  constexpr float kDefaultStationaryVelocityThreshold = 0.01f;
-  constexpr float kDefaultStationaryYawRateThreshold = 0.003f;
   constexpr float kDefaultGyroZLowPassAlpha = 0.6f;
 
   float median3(float a, float b, float c)
@@ -276,6 +274,15 @@ void origincar_base::Publish_Voltage()
   }
 }
 
+void origincar_base::Publish_GyroDebug()
+{
+  origincar_msg::msg::Data gyro_debug;
+  gyro_debug.x = gyro_z_filtered_pre_bias_;
+  gyro_debug.y = gyro_z_bias_model_;
+  gyro_debug.z = gyro_z_final_for_yaw_;
+  gyrodebug_publisher->publish(gyro_debug);
+}
+
 unsigned char origincar_base::Check_Sum(unsigned char Count_Number, unsigned char mode)
 {
   unsigned char check_sum = 0, k;
@@ -360,24 +367,29 @@ bool origincar_base::Get_Sensor_Data()
 
         Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
         Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
+        gyro_z_raw_from_mcu_ = Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;
 
         if (Init_imu_num < 100)
         {
           Init_imu_num += 1;
           gyro_z_sum += Mpu6050_Data.gyros_z_data;
+          gyro_z_after_startup_bias_ = 0.0f;
+          gyro_z_after_median_ = 0.0f;
+          gyro_z_filtered_pre_bias_ = 0.0f;
           Mpu6050.angular_velocity.z = 0;
         }
         else
         {
-          Mpu6050.angular_velocity.z = (Mpu6050_Data.gyros_z_data - (gyro_z_sum / (float)Init_imu_num)) * GYROSCOPE_RATIO;
+          gyro_z_after_startup_bias_ =
-          gyro_z_median_window_[gyro_z_median_index_] = Mpu6050.angular_velocity.z;
+              (Mpu6050_Data.gyros_z_data - (gyro_z_sum / (float)Init_imu_num)) * GYROSCOPE_RATIO;
+          gyro_z_median_window_[gyro_z_median_index_] = gyro_z_after_startup_bias_;
           gyro_z_median_index_ = (gyro_z_median_index_ + 1) % gyro_z_median_window_.size();
           if (gyro_z_median_count_ < gyro_z_median_window_.size())
           {
             gyro_z_median_count_ += 1;
           }
 
-          float median_gyro_z = Mpu6050.angular_velocity.z;
+          float median_gyro_z = gyro_z_after_startup_bias_;
           if (gyro_z_median_count_ == gyro_z_median_window_.size())
           {
             median_gyro_z = median3(
@@ -385,6 +397,7 @@ bool origincar_base::Get_Sensor_Data()
                 gyro_z_median_window_[1],
                 gyro_z_median_window_[2]);
           }
+          gyro_z_after_median_ = median_gyro_z;
 
           if (!gyro_z_low_pass_initialized_)
           {
@@ -397,7 +410,8 @@ bool origincar_base::Get_Sensor_Data()
                 gyro_z_low_pass_alpha_ * gyro_z_low_pass_ +
                 (1.0f - gyro_z_low_pass_alpha_) * median_gyro_z;
           }
-          Mpu6050.angular_velocity.z = gyro_z_low_pass_;
+          gyro_z_filtered_pre_bias_ = gyro_z_low_pass_;
+          Mpu6050.angular_velocity.z = gyro_z_filtered_pre_bias_;
           // RCLCPP_INFO(this->get_logger(),"gyro_z_sum: %.2f, err: %.2f, gyroz: %.2f ", gyro_z_sum ,(gyro_z_sum / (float)Init_imu_num), Mpu6050.angular_velocity.z);
         }
         Robot_Vel.Z = Mpu6050.angular_velocity.z;
@@ -425,19 +439,12 @@ void origincar_base::Control()
     Sampling_Time = (current_time - last_time).seconds();
     if (true == Get_Sensor_Data())
     {
-      const bool is_stationary =
+      gyro_z_bias_elapsed_s_ += Sampling_Time;
-          std::fabs(Robot_Vel.X) < stationary_velocity_threshold_ &&
+      gyro_z_bias_model_ =
-          std::fabs(Robot_Vel.Y) < stationary_velocity_threshold_ &&
+          gyro_z_bias_intercept_ + gyro_z_bias_slope_ * static_cast<float>(gyro_z_bias_elapsed_s_);
-          std::fabs(Mpu6050.angular_velocity.z) < stationary_yaw_rate_threshold_;
+      gyro_z_final_for_yaw_ = gyro_z_filtered_pre_bias_ - gyro_z_bias_model_;
-      if (is_stationary)
+      Mpu6050.angular_velocity.z = gyro_z_final_for_yaw_;
-      {
+      Robot_Vel.Z = gyro_z_final_for_yaw_;
-        Mpu6050.angular_velocity.z = 0.0f;
-        Robot_Vel.Z = 0.0f;
-      }
-      else
-      {
-        Robot_Vel.Z = Mpu6050.angular_velocity.z;
-      }
       Robot_Pos.X += 1.03 * (Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
       Robot_Pos.Y += 1.01 * (Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time; // 1.125
       Robot_Pos.Z += Robot_Vel.Z * Sampling_Time;
@@ -445,6 +452,7 @@ void origincar_base::Control()
       Quaternion_Solution(Mpu6050.angular_velocity.x, Mpu6050.angular_velocity.y, Robot_Vel.Z,
                           Mpu6050.linear_acceleration.x, Mpu6050.linear_acceleration.y, Mpu6050.linear_acceleration.z);
       Publish_ImuSensor();
+      Publish_GyroDebug();
       Publish_Voltage();
       Publish_Odom();
       rclcpp::spin_some(this->get_node_base_interface());
@@ -461,6 +469,15 @@ origincar_base::origincar_base()
   memset(&Receive_Data, 0, sizeof(Receive_Data));
   memset(&Send_Data, 0, sizeof(Send_Data));
   memset(&Mpu6050_Data, 0, sizeof(Mpu6050_Data));
+  gyro_z_bias_intercept_ = 0.0f;
+  gyro_z_bias_slope_ = 0.0f;
+  gyro_z_raw_from_mcu_ = 0.0f;
+  gyro_z_after_startup_bias_ = 0.0f;
+  gyro_z_after_median_ = 0.0f;
+  gyro_z_filtered_pre_bias_ = 0.0f;
+  gyro_z_bias_model_ = 0.0f;
+  gyro_z_final_for_yaw_ = 0.0f;
+  gyro_z_bias_elapsed_s_ = 0.0;
   gyro_z_median_window_.fill(0.0f);
   gyro_z_median_index_ = 0;
   gyro_z_median_count_ = 0;
@@ -478,8 +495,8 @@ origincar_base::origincar_base()
   this->declare_parameter<std::string>("robot_frame_id", "base_link");
   this->declare_parameter<std::string>("gyro_frame_id", "gyro_link");
   this->declare_parameter<bool>("publish_tf", false);
-  this->declare_parameter<double>("stationary_velocity_threshold", kDefaultStationaryVelocityThreshold);
+  this->declare_parameter<double>("gyro_z_bias_intercept", 0.0);
-  this->declare_parameter<double>("stationary_yaw_rate_threshold", kDefaultStationaryYawRateThreshold);
+  this->declare_parameter<double>("gyro_z_bias_slope", 0.0);
   this->declare_parameter<double>("gyro_z_low_pass_alpha", kDefaultGyroZLowPassAlpha);
 
   // Odom covariance parameters (tunable via YAML)
@@ -495,10 +512,10 @@ origincar_base::origincar_base()
   this->get_parameter("robot_frame_id", robot_frame_id);
   this->get_parameter("gyro_frame_id", gyro_frame_id);
   this->get_parameter("publish_tf", publish_tf_);
-  stationary_velocity_threshold_ =
+  gyro_z_bias_intercept_ =
-      static_cast<float>(this->get_parameter("stationary_velocity_threshold").as_double());
+      static_cast<float>(this->get_parameter("gyro_z_bias_intercept").as_double());
-  stationary_yaw_rate_threshold_ =
+  gyro_z_bias_slope_ =
-      static_cast<float>(this->get_parameter("stationary_yaw_rate_threshold").as_double());
+      static_cast<float>(this->get_parameter("gyro_z_bias_slope").as_double());
   gyro_z_low_pass_alpha_ =
       static_cast<float>(this->get_parameter("gyro_z_low_pass_alpha").as_double());
   if (gyro_z_low_pass_alpha_ < 0.0f)
@@ -523,6 +540,8 @@ origincar_base::origincar_base()
 
   robotvel_publisher = create_publisher<origincar_msg::msg::Data>("robotvel", 10);
 
+  gyrodebug_publisher = create_publisher<origincar_msg::msg::Data>("gyro_debug", 10);
+
   pose_pub_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
       "/set_pose",
       rclcpp::SystemDefaultsQoS().reliable());