From 00157d64106d140832a8a54553597a4bcc265488 Mon Sep 17 00:00:00 2001 From: HelixCopex Date: Thu, 26 Mar 2026 05:01:47 +0800 Subject: [PATCH] =?UTF-8?q?=E6=97=A0=E8=BF=B9=E5=8D=A1=E5=B0=94=E6=9B=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 + docs/IMU/packet/MSG_IMU.md | 35 +++ src/imu_receiver_node.cpp | 422 ++++++++++++++++++++++++++++--------- 3 files changed, 359 insertions(+), 100 deletions(-) create mode 100644 docs/IMU/packet/MSG_IMU.md diff --git a/CMakeLists.txt b/CMakeLists.txt index 92a135f..e063353 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(Eigen3 REQUIRED) # 添加 SDK 库路径 set(TRANSMITTER_SDK_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib/transmitter_sdk) @@ -41,6 +42,7 @@ ament_target_dependencies(imu_receiver_node std_msgs geometry_msgs sensor_msgs + Eigen3 ) # ==================== 安装目标 ==================== diff --git a/docs/IMU/packet/MSG_IMU.md b/docs/IMU/packet/MSG_IMU.md new file mode 100644 index 0000000..56af2a2 --- /dev/null +++ b/docs/IMU/packet/MSG_IMU.md @@ -0,0 +1,35 @@ +# MSG_IMU | FDISYSTEMS支持中心 + +1. 机体坐标系及坐标系方向如图所示。在FDI系列产品中，模块表面都会标注准确的机体坐标系方向。具体以实际产品为准。 + +2. 对于角速度，加速度的标定解释： + - (1) 转台标定：测量并补偿了陀螺仪和加速度计的零偏、尺度误差及三轴不垂直度。 + - (2) 温箱标定：在宽温范围（如-40℃~85℃）下，测量了传感器误差随温度的变化规律，并生成了补偿模型。对陀螺和加表的输出数据进行了补偿。 + +3. 对于加速度中未分离重力加速度解释： + 加速度计测量的是载体运动与地球重力的"混合"结果。简单来说，当模块静止时，加速度读数并不为零，而会显示出一个重力值（例如，平放时Z轴约为+9.8 m/s²）。 + +## 基本信息 + +| 属性 | 值 | +|------|-----| +| **Packet ID** | 0x40 | +| **Length** | 56 | +| **Read / Write** | Read | + +## 数据字段 + +| Offset | Size | Format | Field | Unit | Description | +|--------|------|--------|-------|------|-------------| +| 0 | 4 | float32_t | Gyroscope_X | rad/s | 机体系X轴角速度 | +| 4 | 4 | float32_t | Gyroscope_Y | rad/s | 机体系Y轴角速度 | +| 8 | 4 | float32_t | Gyroscope_Z | rad/s | 机体系Z轴角速度 | +| 12 | 4 | float32_t | Accelerometer_X | m/s² | 机体系X轴加速度(未分离重力加速度) | +| 16 | 4 | float32_t | Accelerometer_Y | m/s² | 机体系Y轴加速度(未分离重力加速度) | +| 20 | 4 | float32_t | Accelerometer_Z | m/s² | 机体系Z轴加速度(未分离重力加速度) | +| 24 | 4 | float32_t | Magnetometer_X | mG | 机体系X轴磁感应强度 | +| 28 | 4 | float32_t | Magnetometer_Y | mG | 机体系Y轴磁感应强度 | +| 32 | 4 | float32_t | Magnetometer_Z | mG | 机体系Z轴磁感应强度 | +| 36 | 4 | float32_t | IMU_Temperature | deg C | 如果IMU数据由多个传感器组成则该值为这些传感器的平均温度 | +| 40 | 4 | float32_t | Pressure | Pa | 气压值，如果没装气压计，默认为一个标准大气压101325Pa | +| 48 | 8 | int64_t | Timestamp | us | 数据的时间戳，从上电开始启动的微秒数。时钟源为MCU外部晶振。 | \ No newline at end of file diff --git a/src/imu_receiver_node.cpp b/src/imu_receiver_node.cpp index ca1bce4..e205c8e 100644 --- a/src/imu_receiver_node.cpp +++ b/src/imu_receiver_node.cpp @@ -1,21 +1,24 @@ /** * @file imu_receiver_node.cpp - * @brief IMU 数据接收节点 (FDI DETA10) - 环形缓冲区版本 + * @brief IMU 数据接收节点 (FDI DETA10) - 环形缓冲区版本 + UKF * * 通过串口读取 FDI DETA10 IMU 模块的数据 * 波特率：921600 * 协议：FDILink * * 转发数据包： - * - MSG_AHRS (0x41): 航姿参考系统数据 - * - MSG_BODY_VEL (0x60): 机体系速度数据 (IMU卡尔曼滤波后) + * - MSG_BODY_ACCELERATION (0x62): 滤波修正后的机体系加速度（不包括重力） * - * 特性：使用环形缓冲区避免数据堆积爆发 + * 特性： + * - 使用环形缓冲区避免数据堆积爆发 + * - 使用 UKF（无迹卡尔曼滤波器）对加速度数据进行滤波 */ #include +#include #include #include +#include #include #include #include @@ -38,9 +41,8 @@ constexpr uint8_t FRAME_START = 0xFC; constexpr uint8_t FRAME_END = 0xFD; -// 数据包 ID -constexpr uint8_t MSG_AHRS = 0x41; // 航姿参考系统数据 -constexpr uint8_t MSG_BODY_VEL = 0x60; // 机体系速度数据 (卡尔曼滤波后) +// 数据包 ID - 只使用 MSG_BODY_ACCELERATION +constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 滤波修正后的机体系加速度 // 默认串口设备 constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyIMU"; @@ -56,6 +58,11 @@ constexpr size_t MAX_PUBLISH_RATE_HZ = 100; // 最大发布频率 100Hz constexpr auto PUBLISH_PERIOD = std::chrono::microseconds(1000000 / MAX_PUBLISH_RATE_HZ); // 10ms +// UKF 参数 +constexpr int UKF_STATE_DIM = 2; // 状态维度 [位置, 速度] +constexpr int UKF_MEAS_DIM = 1; // 测量维度 [位置] +constexpr int UKF_SIGMA_POINTS = 2 * UKF_STATE_DIM + 1; // 2n+1 个 Sigma 点 + // CRC8 查找表 static const uint8_t CRC8Table[] = { 0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, @@ -109,16 +116,221 @@ static const uint16_t CRC16Table[256] = { 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0}; -// IMU 数据结构 -struct ImuDataFrame { - bool ahrs_valid = false; - float roll_speed = 0, pitch_speed = 0, heading_speed = 0; - float roll = 0, pitch = 0, heading = 0; - float q1 = 0, q2 = 0, q3 = 0, q4 = 0; - int64_t timestamp_us = 0; +/** + * @brief 无迹卡尔曼滤波器 (UKF) 用于加速度滤波 + * + * 状态向量: [位置, 速度]^T + * 测量: 位置 + * + * 使用 Sigma 点变换处理非线性 + */ +class UKF { +public: + UKF() + : x_(Eigen::Vector2d::Zero()), P_(Eigen::Matrix2d::Identity()), + Q_(Eigen::Matrix2d::Identity() * 0.001), + R_(Eigen::Matrix::Identity() * 0.03), dt_(0.01), + initialized_(false) { + // 初始化权重 + computeWeights(); + } - bool vel_valid = false; - float vel_x = 0, vel_y = 0, vel_z = 0; + /** + * @brief 设置过程噪声 + */ + void setProcessNoise(double q) { + Q_ = Eigen::Matrix2d::Identity() * q; + Q_(1, 1) = q * 10; // 速度噪声更大一些 + } + + /** + * @brief 设置测量噪声 + */ + void setMeasurementNoise(double r) { R_(0, 0) = r; } + + /** + * @brief 更新滤波器 - 简单的一维位置滤波 + * @param measurement 测量值 (加速度/位置) + * @return 滤波后的值 + */ + double update(double measurement) { + if (!initialized_) { + x_(0) = measurement; + x_(1) = 0; // 初始速度为0 + initialized_ = true; + return measurement; + } + + // === 预测步骤 === + // 生成 Sigma 点 + Eigen::Matrix sigma_points; + generateSigmaPoints(sigma_points); + + // 传播 Sigma 点通过状态转移方程 + Eigen::Matrix sigma_points_pred; + for (int i = 0; i < UKF_SIGMA_POINTS; ++i) { + // 状态转移: 恒定速度模型 + // x_k+1 = x_k + v_k * dt + // v_k+1 = v_k + sigma_points_pred(0, i) = sigma_points(0, i) + sigma_points(1, i) * dt_; + sigma_points_pred(1, i) = sigma_points(1, i); + } + + // 计算预测均值 + x_pred_.setZero(); + for (int i = 0; i < UKF_SIGMA_POINTS; ++i) { + x_pred_ += weights_m_[i] * sigma_points_pred.col(i); + } + + // 计算预测协方差 + P_pred_.setZero(); + for (int i = 0; i < UKF_SIGMA_POINTS; ++i) { + Eigen::Vector2d diff = sigma_points_pred.col(i) - x_pred_; + P_pred_ += weights_c_[i] * diff * diff.transpose(); + } + P_pred_ += Q_; + + // === 更新步骤 === + // 传播 Sigma 点到测量空间 + Eigen::Matrix Z_sigma; + for (int i = 0; i < UKF_SIGMA_POINTS; ++i) { + // 测量模型: 只测量位置 + Z_sigma(0, i) = sigma_points_pred(0, i); + } + + // 计算预测测量均值 + z_pred_.setZero(); + for (int i = 0; i < UKF_SIGMA_POINTS; ++i) { + z_pred_ += weights_m_[i] * Z_sigma.col(i); + } + + // 计算预测测量协方差 + Eigen::Matrix S; + S.setZero(); + for (int i = 0; i < UKF_SIGMA_POINTS; ++i) { + Eigen::Matrix diff = Z_sigma.col(i) - z_pred_; + S += weights_c_[i] * diff * diff.transpose(); + } + S += R_; + + // 计算互协方差 + Eigen::Matrix T; + T.setZero(); + for (int i = 0; i < UKF_SIGMA_POINTS; ++i) { + Eigen::Vector2d diff_x = sigma_points_pred.col(i) - x_pred_; + Eigen::Matrix diff_z = Z_sigma.col(i) - z_pred_; + T += weights_c_[i] * diff_x * diff_z.transpose(); + } + + // 计算卡尔曼增益 + Eigen::Matrix K = T * S.inverse(); + + // 更新状态 + Eigen::Matrix z; + z(0) = measurement; + x_ = x_pred_ + K * (z - z_pred_); + + // 更新协方差 + P_ = P_pred_ - K * S * K.transpose(); + + return x_(0); + } + + /** + * @brief 设置采样时间 + */ + void setDt(double dt) { dt_ = dt; } + + /** + * @brief 获取当前估计值 + */ + double getEstimate() const { return x_(0); } + + /** + * @brief 获取当前速度估计 + */ + double getVelocity() const { return x_(1); } + +private: + /** + * @brief 计算 Sigma 点权重 + */ + void computeWeights() { + // 使用标准 UKF 参数 + // alpha = 1e-3, beta = 2, kappa = 0 + double alpha = 1e-3; + double beta = 2.0; + double kappa = 0.0; + + lambda_ = alpha * alpha * (UKF_STATE_DIM + kappa) - UKF_STATE_DIM; + + weights_m_.resize(UKF_SIGMA_POINTS); + weights_c_.resize(UKF_SIGMA_POINTS); + + weights_m_[0] = lambda_ / (UKF_STATE_DIM + lambda_); + weights_c_[0] = weights_m_[0] + (1 - alpha * alpha + beta); + + for (int i = 1; i < UKF_SIGMA_POINTS; ++i) { + weights_m_[i] = 0.5 / (UKF_STATE_DIM + lambda_); + weights_c_[i] = weights_m_[i]; + } + } + + /** + * @brief 生成 Sigma 点 + */ + void generateSigmaPoints( + Eigen::Matrix &sigma_points) { + // 计算矩阵平方根 + Eigen::Matrix2d sqrt_P = ((UKF_STATE_DIM + lambda_) * P_).llt().matrixL(); + + sigma_points.col(0) = x_; + + for (int i = 0; i < UKF_STATE_DIM; ++i) { + sigma_points.col(i + 1) = x_ + sqrt_P.col(i); + sigma_points.col(i + 1 + UKF_STATE_DIM) = x_ - sqrt_P.col(i); + } + } + + // 状态向量 [位置, 速度] + Eigen::Vector2d x_; + Eigen::Vector2d x_pred_; + + // 协方差矩阵 + Eigen::Matrix2d P_; + Eigen::Matrix2d P_pred_; + + // 过程噪声和测量噪声 + Eigen::Matrix2d Q_; + Eigen::Matrix R_; + + // 预测测量 + Eigen::Matrix z_pred_; + + // UKF 参数 + double lambda_; + std::vector weights_m_; + std::vector weights_c_; + + // 采样时间 + double dt_; + + // 初始化标志 + bool initialized_; +}; + +// IMU 数据结构 - 使用 MSG_BODY_ACCELERATION 数据包 +struct ImuDataFrame { + bool valid = false; + + // 滤波修正后的机体系加速度 (m/s²) - 不包括重力 + float acc_x = 0, acc_y = 0, acc_z = 0; + + // 当地重力加速度 (m/s²) + float g_force = 0; + + // 时间戳 (从接收到数据的时间) + std::chrono::steady_clock::time_point receive_time; }; // 线程安全的环形缓冲区 @@ -191,23 +403,48 @@ public: this->declare_parameter("verbose", true); // 是否输出详细日志 this->declare_parameter("enable_crc", false); // 是否启用CRC校验（默认关闭） + // UKF 参数 + this->declare_parameter("ukf_process_noise", 0.001); // 过程噪声 + this->declare_parameter("ukf_measurement_noise", 0.03); // 测量噪声 + this->declare_parameter("ukf_dt", 0.01); // 采样时间 + // 获取参数 serial_port_ = this->get_parameter("serial_port").as_string(); baudrate_ = this->get_parameter("baudrate").as_int(); enable_crc_ = this->get_parameter("enable_crc").as_bool(); + // 获取 UKF 参数并设置 + double process_noise = this->get_parameter("ukf_process_noise").as_double(); + double measurement_noise = + this->get_parameter("ukf_measurement_noise").as_double(); + double dt = this->get_parameter("ukf_dt").as_double(); + + // 设置 UKF 参数 + ukf_x_.setProcessNoise(process_noise); + ukf_x_.setMeasurementNoise(measurement_noise); + ukf_x_.setDt(dt); + ukf_y_.setProcessNoise(process_noise); + ukf_y_.setMeasurementNoise(measurement_noise); + ukf_y_.setDt(dt); + ukf_z_.setProcessNoise(process_noise); + ukf_z_.setMeasurementNoise(measurement_noise); + ukf_z_.setDt(dt); + RCLCPP_INFO(this->get_logger(), "---------------------------------"); - RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点"); + RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点 (UKF 版本)"); RCLCPP_INFO(this->get_logger(), "串口: %s", serial_port_.c_str()); RCLCPP_INFO(this->get_logger(), "波特率: %d", baudrate_); RCLCPP_INFO(this->get_logger(), "CRC校验: %s", enable_crc_ ? "启用" : "禁用"); + RCLCPP_INFO(this->get_logger(), + "UKF 参数 - 过程噪声: %.4f, 测量噪声: %.4f, dt: %.4f", + process_noise, measurement_noise, dt); RCLCPP_INFO(this->get_logger(), "---------------------------------"); // 创建发布者 - ahrs_pub_ = this->create_publisher("imu/ahrs", 10); - body_vel_pub_ = this->create_publisher( - "imu/body_velocity", 10); + imu_pub_ = this->create_publisher("imu/data", 10); + accel_pub_ = this->create_publisher( + "imu/body_acceleration", 10); connection_status_pub_ = this->create_publisher( "imu/connection_status", 10); @@ -351,9 +588,6 @@ private: std::vector current_frame; current_frame.reserve(MAX_FRAME_SIZE); - // 当前正在积累的 IMU 数据帧 - ImuDataFrame current_data; - while (running_ && rclcpp::ok()) { ssize_t n = read(serial_fd_, read_buf, sizeof(read_buf)); @@ -396,8 +630,8 @@ private: rx_buffer.begin() + i + 5 + remaining); if (current_frame.back() == FRAME_END) { - // 解析帧到 current_data - parseFrameToData(current_frame, current_data); + // 解析帧 + parseFrame(current_frame); } in_frame = false; i += total_frame_len; @@ -414,7 +648,7 @@ private: } else { current_frame.push_back(byte); if (byte == FRAME_END) { - parseFrameToData(current_frame, current_data); + parseFrame(current_frame); in_frame = false; i++; } else if (current_frame.size() >= MAX_FRAME_SIZE) { @@ -437,8 +671,8 @@ private: } } - // 解析帧到数据结构 - 独立处理，不等待配对 - void parseFrameToData(const std::vector &frame, ImuDataFrame &data) { + // 解析帧 - 只处理 MSG_BODY_ACCELERATION + void parseFrame(const std::vector &frame) { if (frame.size() < 8) return; if (frame.back() != FRAME_END) @@ -458,42 +692,28 @@ private: } const uint8_t *payload = frame.data() + 7; - bool should_push = false; - switch (msg_id) { - case MSG_AHRS: - if (data_len >= 48) { - data.ahrs_valid = true; - data.roll_speed = *reinterpret_cast(payload); - data.pitch_speed = *reinterpret_cast(payload + 4); - data.heading_speed = *reinterpret_cast(payload + 8); - data.roll = *reinterpret_cast(payload + 12); - data.pitch = *reinterpret_cast(payload + 16); - data.heading = *reinterpret_cast(payload + 20); - data.q1 = *reinterpret_cast(payload + 24); - data.q2 = *reinterpret_cast(payload + 28); - data.q3 = *reinterpret_cast(payload + 32); - data.q4 = *reinterpret_cast(payload + 36); - data.timestamp_us = *reinterpret_cast(payload + 40); - should_push = true; + // 只处理 MSG_BODY_ACCELERATION 数据包 (0x62) + if (msg_id == MSG_BODY_ACCELERATION) { + if (data_len >= 16) { + ImuDataFrame data; + data.valid = true; + + // 滤波修正后的机体系加速度 (m/s²) - 不包括重力 + data.acc_x = *reinterpret_cast(payload); + data.acc_y = *reinterpret_cast(payload + 4); + data.acc_z = *reinterpret_cast(payload + 8); + + // 当地重力加速度 (m/s²) + data.g_force = *reinterpret_cast(payload + 12); + + data.receive_time = std::chrono::steady_clock::now(); + + // 推入环形缓冲区 + ring_buffer_.push(data); } - break; - - case MSG_BODY_VEL: - if (data_len >= 12) { - data.vel_valid = true; - data.vel_x = *reinterpret_cast(payload); - data.vel_y = *reinterpret_cast(payload + 4); - data.vel_z = *reinterpret_cast(payload + 8); - should_push = true; - } - break; - } - - // 只要有有效数据就推入缓冲区（不等待配对） - if (should_push) { - ring_buffer_.push(data); } + // 其他数据包类型被忽略 } // 处理循环 - 固定频率从环形缓冲区读取并发布 @@ -523,49 +743,51 @@ private: } } - // 发布数据 + // 发布数据 - 使用 UKF 对加速度进行滤波 void publishData(const ImuDataFrame &data) { - // 时间戳转换 rclcpp::Time current_time = this->now(); - rclcpp::Time imu_time; - if (!imu_time_initialized_) { - imu_base_time_ = - current_time - rclcpp::Duration(0, data.timestamp_us * 1000); - imu_time_initialized_ = true; - } - imu_time = imu_base_time_ + rclcpp::Duration(0, data.timestamp_us * 1000); - latest_imu_timestamp_ = imu_time; + // 使用 UKF 对加速度数据进行滤波 + float filtered_acc_x = ukf_x_.update(data.acc_x); + float filtered_acc_y = ukf_y_.update(data.acc_y); + float filtered_acc_z = ukf_z_.update(data.acc_z); + + // 发布加速度消息 (滤波后的数据) + geometry_msgs::msg::Vector3 accel_msg; + accel_msg.x = filtered_acc_x; + accel_msg.y = filtered_acc_y; + accel_msg.z = filtered_acc_z; + accel_pub_->publish(accel_msg); // 发布 IMU 消息 sensor_msgs::msg::Imu imu_msg; - imu_msg.header.stamp = imu_time; + imu_msg.header.stamp = current_time; imu_msg.header.frame_id = "imu_link"; - imu_msg.angular_velocity.x = data.roll_speed; - imu_msg.angular_velocity.y = data.pitch_speed; - imu_msg.angular_velocity.z = data.heading_speed; - imu_msg.orientation.x = data.q2; - imu_msg.orientation.y = data.q3; - imu_msg.orientation.z = data.q4; - imu_msg.orientation.w = data.q1; - imu_msg.linear_acceleration.x = 0; - imu_msg.linear_acceleration.y = 0; - imu_msg.linear_acceleration.z = 0; - ahrs_pub_->publish(imu_msg); - // 发布速度 - geometry_msgs::msg::Vector3 vel_msg; - vel_msg.x = data.vel_x; - vel_msg.y = data.vel_y; - vel_msg.z = data.vel_z; - body_vel_pub_->publish(vel_msg); + // 线性加速度 - 使用 UKF 滤波后的值 + imu_msg.linear_acceleration.x = filtered_acc_x; + imu_msg.linear_acceleration.y = filtered_acc_y; + imu_msg.linear_acceleration.z = filtered_acc_z; + + // 角速度和姿态暂时设为0（MSG_BODY_ACCELERATION 不包含这些数据） + imu_msg.angular_velocity.x = 0; + imu_msg.angular_velocity.y = 0; + imu_msg.angular_velocity.z = 0; + + imu_msg.orientation.x = 0; + imu_msg.orientation.y = 0; + imu_msg.orientation.z = 0; + imu_msg.orientation.w = 1; // 单位四元数 + + imu_pub_->publish(imu_msg); // 日志 - RCLCPP_INFO(this->get_logger(), - "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f | VEL - X: " - "%.3f, Y: %.3f, Z: %.3f", - data.roll, data.pitch, data.heading, data.vel_x, data.vel_y, - data.vel_z); + RCLCPP_INFO( + this->get_logger(), + "Body Accel(raw): [%.3f, %.3f, %.3f] | Accel(UKF): [%.3f, %.3f, " + "%.3f] m/s² | G-force: %.3f m/s²", + data.acc_x, data.acc_y, data.acc_z, filtered_acc_x, filtered_acc_y, + filtered_acc_z, data.g_force); } void publishStatus() { @@ -592,13 +814,13 @@ private: std::thread process_thread_; RingBuffer ring_buffer_; - // IMU 时间戳同步 - bool imu_time_initialized_ = false; - rclcpp::Time imu_base_time_; - rclcpp::Time latest_imu_timestamp_; + // UKF 滤波器（用于加速度滤波） + UKF ukf_x_; + UKF ukf_y_; + UKF ukf_z_; - rclcpp::Publisher::SharedPtr ahrs_pub_; - rclcpp::Publisher::SharedPtr body_vel_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr accel_pub_; rclcpp::Publisher::SharedPtr connection_status_pub_; rclcpp::TimerBase::SharedPtr status_timer_; };