cyy/yiliao2026
1
0
Fork 0
forked from zbw/yiliao2026
Code Pull Requests Activity

零漂数据采集拟合

Browse Source
This commit is contained in:
Orange
2000-01-01 08:38:51 +08:00
parent b14befd1cf
commit 2f51de27e5
6 changed files with 1430 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/origincar_base/include/origincar_base/origincar_base.h
View File

@@ -221,8 +221,15 @@ private:
Vel_Pos_Data Robot_Vel;
MPU6050_DATA Mpu6050_Data;
float Power_voltage;
float stationary_velocity_threshold_;
float stationary_yaw_rate_threshold_;
float gyro_z_bias_intercept_;
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_;

4
src/origincar_base/launch/base_serial.launch.py
View File

@@ -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}

69
src/origincar_base/src/origincar_base.cpp
View File

@@ -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_median_window_[gyro_z_median_index_] = Mpu6050.angular_velocity.z;
gyro_z_after_startup_bias_ =
(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 =
std::fabs(Robot_Vel.X) < stationary_velocity_threshold_ &&
std::fabs(Robot_Vel.Y) < stationary_velocity_threshold_ &&
std::fabs(Mpu6050.angular_velocity.z) < stationary_yaw_rate_threshold_;
if (is_stationary)
{
Mpu6050.angular_velocity.z = 0.0f;
Robot_Vel.Z = 0.0f;
}
else
{
Robot_Vel.Z = Mpu6050.angular_velocity.z;
}
gyro_z_bias_elapsed_s_ += Sampling_Time;
gyro_z_bias_model_ =
gyro_z_bias_intercept_ + gyro_z_bias_slope_ * static_cast<float>(gyro_z_bias_elapsed_s_);
gyro_z_final_for_yaw_ = gyro_z_filtered_pre_bias_ - gyro_z_bias_model_;
Mpu6050.angular_velocity.z = gyro_z_final_for_yaw_;
Robot_Vel.Z = gyro_z_final_for_yaw_;
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>("stationary_yaw_rate_threshold", kDefaultStationaryYawRateThreshold);
this->declare_parameter<double>("gyro_z_bias_intercept", 0.0);
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_ =
static_cast<float>(this->get_parameter("stationary_velocity_threshold").as_double());
stationary_yaw_rate_threshold_ =
static_cast<float>(this->get_parameter("stationary_yaw_rate_threshold").as_double());
gyro_z_bias_intercept_ =
static_cast<float>(this->get_parameter("gyro_z_bias_intercept").as_double());
gyro_z_bias_slope_ =
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());