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/README.md b/README.md index b75c444..51b7d3d 100644 --- a/README.md +++ b/README.md @@ -21,10 +21,6 @@ - MVSDK (工业相机 SDK) - 待补充... -## 目标 - -感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26 。 - ## 软件架构 ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为例说明。 @@ -49,9 +45,9 @@ ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为 ### IMU 收发节点 - 功能：IMU 与主机通信的封装节点。物理上读取 IMU（DETA10）数据 -- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyUSB1` +- 连接：IMU_CP210x，协商 UART 通信速率 921600，`/dev/ttyIMU` - 接收：IMU_CP210x -> 六轴加速度、陀螺仪数据 -- 发布：IMU 数据（地磁角、六轴角加速度、加速度、陀螺仪） +- 发布：IMU 数据（加速度、陀螺仪） ### 中央节点 @@ -84,6 +80,14 @@ stateDiagram-v2 ### 导航节点 - 功能：根据 IMU 数据计算出当前位置，根据状态决定目标位置并发出移动到该位置所需的指令 -- 接收：收发节点 -> IMU.地磁角, IMU.速度, IMU.加速度, IMU.六轴陀螺仪 +- 接收：收发节点 -> IMU.加速度, IMU.六轴陀螺仪(AHRS) 中央节点 -> 状态 - 发布：导航运动指令 + +考虑到 IMU 的实际情况，导航建模思路如下： +- 地图范围：一个 $8 \times 12 m$ 的矩形，机器人运动的范围。以左下角为坐标原点 $(0,0)$，地图对角为 $(12,8)$，坐标单位为米。 +- 障碍区：不可跨越，不可侵入，由地图中两个矩形组成，第一个矩形的两个角点为 $(2.5,1.5) ~ (4.5,8)$，第二个为 $(7.5,0) ~ (9.5,6.5)$。 +- 基地：同样为场地中角点描述的矩形，我方基地为 $(0,6) ~ (1.5,8)$。 +- 增益点：同样为场地中角点描述的矩形，范围为 $(4.5,2.5) ~ (7.5,5.5)$. +- 机器人：视为一直径 $0.75 m$ 的圆，圆心位置为 IMU 安装位置。开始时，圆心位于 $(1.235,6.265)$。 +- 机器人的朝向和位置计算：IMU 安装于机器人圆心，开始时，IMU X 轴正方向（机器人朝向）与场地 Y 轴负方向相同，Y 轴正方向与场地 X 轴负方向相同，IMU Z 轴则朝向地面。当机器人朝向发生改变时，IMU 轴系的朝向也会相应发生改变，因此需要结合陀螺仪数据才能正确反向解算加速度计带来的加速度信息，从而完成惯性导航。IMU 安装并不完全平整，且机器人运动有颠簸，因此需要将 Z 轴数据结合解算出在二维平面上的加速度和位移。 \ No newline at end of file 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/launch/uart_transmitter.launch.py b/launch/uart_transmitter.launch.py deleted file mode 100644 index 1e52044..0000000 --- a/launch/uart_transmitter.launch.py +++ /dev/null @@ -1,102 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration - - -def generate_launch_description(): - # 声明启动参数 - serial_port_arg = DeclareLaunchArgument( - 'serial_port', - default_value='/dev/ttyCH340', - description='CH340 串口设备路径' - ) - - baudrate_arg = DeclareLaunchArgument( - 'baudrate', - default_value='115200', - description='串口波特率' - ) - - send_frequency_arg = DeclareLaunchArgument( - 'send_frequency', - default_value='10.0', - description='发送频率 (Hz)' - ) - - # 控制参数 - x_move_arg = DeclareLaunchArgument( - 'x_move', - default_value='0', - description='平动左右 [-660, 660]' - ) - - y_move_arg = DeclareLaunchArgument( - 'y_move', - default_value='0', - description='平动前后 [-660, 660]' - ) - - yaw_arg = DeclareLaunchArgument( - 'yaw', - default_value='0', - description='云台偏航 [-660, 660]' - ) - - pitch_arg = DeclareLaunchArgument( - 'pitch', - default_value='0', - description='云台俯仰 [-660, 660]' - ) - - feed_arg = DeclareLaunchArgument( - 'feed', - default_value='0', - description='拨弹轮 [-660, 660]' - ) - - left_switch_arg = DeclareLaunchArgument( - 'left_switch', - default_value='3', - description='左拨杆 [1, 3]' - ) - - right_switch_arg = DeclareLaunchArgument( - 'right_switch', - default_value='3', - description='右拨杆 [1, 3]' - ) - - # 创建节点 - uart_transmitter_node = Node( - package='amadeus_26', - executable='uart_transmitter_node', - name='uart_transmitter_node', - output='screen', - parameters=[{ - 'serial_port': LaunchConfiguration('serial_port'), - 'baudrate': LaunchConfiguration('baudrate'), - 'send_frequency': LaunchConfiguration('send_frequency'), - 'x_move': LaunchConfiguration('x_move'), - 'y_move': LaunchConfiguration('y_move'), - 'yaw': LaunchConfiguration('yaw'), - 'pitch': LaunchConfiguration('pitch'), - 'feed': LaunchConfiguration('feed'), - 'left_switch': LaunchConfiguration('left_switch'), - 'right_switch': LaunchConfiguration('right_switch'), - }], - ) - - return LaunchDescription([ - serial_port_arg, - baudrate_arg, - send_frequency_arg, - x_move_arg, - y_move_arg, - yaw_arg, - pitch_arg, - feed_arg, - left_switch_arg, - right_switch_arg, - uart_transmitter_node, - ]) \ No newline at end of file diff --git a/src/imu_receiver_node.cpp b/src/imu_receiver_node.cpp index 49c39d8..1f69d14 100644 --- a/src/imu_receiver_node.cpp +++ b/src/imu_receiver_node.cpp @@ -1,23 +1,34 @@ /** * @file imu_receiver_node.cpp - * @brief IMU 数据接收节点 (FDI DETA10) - 高实时性版本 + * @brief IMU 数据接收节点 (FDI DETA10) - 环形缓冲区版本 + UKF * * 通过串口读取 FDI DETA10 IMU 模块的数据 * 波特率：921600 * 协议：FDILink * * 转发数据包： - * - MSG_AHRS (0x41): 航姿参考系统数据 - * - MSG_BODY_ACCELERATION (0x62): 机体系加速度数据 + * - MSG_BODY_ACCELERATION (0x62): 滤波修正后的机体系加速度（不包括重力） + * - MSG_AHRS (0x41): 航姿参考系统数据（四元数、角速度） + * + * 特性： + * - 使用环形缓冲区避免数据堆积爆发 + * - 使用 UKF（无迹卡尔曼滤波器）对加速度数据进行滤波 + * - 融合 AHRS 姿态数据和 UKF 滤波后的加速度 */ #include +#include +#include #include +#include #include #include #include #include #include +#include +#include +#include #include #include #include @@ -33,8 +44,8 @@ constexpr uint8_t FRAME_START = 0xFC; constexpr uint8_t FRAME_END = 0xFD; // 数据包 ID +constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 滤波修正后的机体系加速度 constexpr uint8_t MSG_AHRS = 0x41; // 航姿参考系统数据 -constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 机体系加速度数据 // 默认串口设备 constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyIMU"; @@ -44,6 +55,20 @@ constexpr int DEFAULT_BAUDRATE = 921600; constexpr size_t SERIAL_READ_BUF_SIZE = 4096; constexpr size_t MAX_FRAME_SIZE = 256; +// 环形缓冲区配置 +constexpr size_t RING_BUFFER_SIZE = 16; // 环形缓冲区最大帧数 +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 点 + +// 数据同步超时 (ms) +constexpr int DATA_SYNC_TIMEOUT_MS = 10; + // CRC8 查找表 static const uint8_t CRC8Table[] = { 0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, @@ -97,34 +122,378 @@ static const uint16_t CRC16Table[256] = { 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0}; +/** + * @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(); + } + + /** + * @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_; +}; + +// AHRS 数据结构 +struct AhrsData { + bool valid = false; + + // 角速度 (rad/s) + float roll_speed = 0; + float pitch_speed = 0; + float heading_speed = 0; + + // 欧拉角 (rad) + float roll = 0; + float pitch = 0; + float heading = 0; + + // 四元数 + float q1 = 0, q2 = 0, q3 = 0, q4 = 0; + + // 时间戳 (us) + int64_t timestamp_us = 0; + + // 接收时间 + std::chrono::steady_clock::time_point receive_time; +}; + +// 加速度数据结构 +struct AccelData { + 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; +}; + +// 融合的 IMU 数据结构 +struct ImuDataFrame { + bool valid = false; + + // 加速度 (UKF 滤波后) + float acc_x = 0, acc_y = 0, acc_z = 0; + + // 角速度 (来自 AHRS) + float gyro_x = 0, gyro_y = 0, gyro_z = 0; + + // 四元数 (来自 AHRS) + float q1 = 0, q2 = 0, q3 = 0, q4 = 0; + + // 时间戳 + rclcpp::Time timestamp; +}; + +// 线程安全的环形缓冲区 +template class RingBuffer { +public: + explicit RingBuffer(size_t size) : max_size_(size) {} + + // 推入数据，如果满则丢弃最旧的数据 + void push(const T &item) { + std::lock_guard lock(mutex_); + if (buffer_.size() >= max_size_) { + buffer_.pop(); // 丢弃最旧的数据 + } + buffer_.push(item); + cond_var_.notify_one(); + } + + // 非阻塞弹出 + bool try_pop(T &item) { + std::lock_guard lock(mutex_); + if (buffer_.empty()) { + return false; + } + item = buffer_.front(); + buffer_.pop(); + return true; + } + + // 获取最新数据（不清除） + bool peek_latest(T &item) { + std::lock_guard lock(mutex_); + if (buffer_.empty()) { + return false; + } + item = buffer_.back(); // 返回最新的 + return true; + } + + // 清空缓冲区 + void clear() { + std::lock_guard lock(mutex_); + while (!buffer_.empty()) + buffer_.pop(); + } + + size_t size() { + std::lock_guard lock(mutex_); + return buffer_.size(); + } + + bool empty() { + std::lock_guard lock(mutex_); + return buffer_.empty(); + } + +private: + std::queue buffer_; + size_t max_size_; + std::mutex mutex_; + std::condition_variable cond_var_; +}; + class ImuReceiverNode : public rclcpp::Node { public: - ImuReceiverNode() : Node("imu_receiver_node") { + ImuReceiverNode() + : Node("imu_receiver_node"), accel_ring_buffer_(RING_BUFFER_SIZE), + ahrs_ring_buffer_(RING_BUFFER_SIZE), + output_ring_buffer_(RING_BUFFER_SIZE) { // 声明参数 this->declare_parameter("serial_port", DEFAULT_SERIAL_PORT); this->declare_parameter("baudrate", DEFAULT_BAUDRATE); 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(); - RCLCPP_INFO(this->get_logger(), "================================="); - RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点启动 (高实时性版本)"); + // 获取 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 数据接收节点 (UKF + AHRS 融合)"); 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(), "================================="); + 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_acc_pub_ = this->create_publisher( + imu_pub_ = this->create_publisher("imu/data", 10); + accel_pub_ = this->create_publisher( "imu/body_acceleration", 10); - g_force_pub_ = - this->create_publisher("imu/g_force", 10); connection_status_pub_ = this->create_publisher( "imu/connection_status", 10); @@ -138,12 +507,20 @@ public: // 创建接收线程 receive_thread_ = std::thread(&ImuReceiverNode::receiveLoop, this); + // 创建融合线程 + fusion_thread_ = std::thread(&ImuReceiverNode::fusionLoop, this); + + // 创建处理线程（固定频率处理数据） + process_thread_ = std::thread(&ImuReceiverNode::processLoop, this); + // 创建状态发布定时器 status_timer_ = this->create_wall_timer( std::chrono::seconds(1), std::bind(&ImuReceiverNode::publishStatus, this)); RCLCPP_INFO(this->get_logger(), "IMU 节点初始化完成"); + RCLCPP_INFO(this->get_logger(), "环形缓冲区大小: %zu, 最大发布频率: %zu Hz", + RING_BUFFER_SIZE, MAX_PUBLISH_RATE_HZ); } ~ImuReceiverNode() { @@ -151,6 +528,12 @@ public: if (receive_thread_.joinable()) { receive_thread_.join(); } + if (fusion_thread_.joinable()) { + fusion_thread_.join(); + } + if (process_thread_.joinable()) { + process_thread_.join(); + } if (serial_fd_ >= 0) { close(serial_fd_); } @@ -251,45 +634,33 @@ private: return crc16; } - // 高实时性接收循环 - 批量读取，无阻塞 + // 接收循环 - 只负责解析数据并存入环形缓冲区 void receiveLoop() { - // 使用双缓冲区策略：一个用于接收，一个用于处理 std::vector rx_buffer; rx_buffer.reserve(SERIAL_READ_BUF_SIZE * 2); - - // 临时读取缓冲区 uint8_t read_buf[SERIAL_READ_BUF_SIZE]; - - // 帧解析状态 bool in_frame = false; std::vector current_frame; current_frame.reserve(MAX_FRAME_SIZE); while (running_ && rclcpp::ok()) { - // 批量读取串口数据 - 非阻塞 ssize_t n = read(serial_fd_, read_buf, sizeof(read_buf)); if (n > 0) { - // 将新数据追加到缓冲区 rx_buffer.insert(rx_buffer.end(), read_buf, read_buf + n); - // 处理缓冲区中的数据 size_t i = 0; while (i < rx_buffer.size()) { uint8_t byte = rx_buffer[i]; if (!in_frame) { - // 查找帧头 if (byte == FRAME_START) { - // 检查是否有足够的字节来读取帧头信息 if (i + 5 <= rx_buffer.size()) { - // 读取帧头信息 uint8_t msg_id = rx_buffer[i + 1]; uint8_t data_len = rx_buffer[i + 2]; uint8_t seq = rx_buffer[i + 3]; uint8_t header_crc = rx_buffer[i + 4]; - // 可选的CRC校验 bool header_valid = true; if (enable_crc_) { uint8_t header[4] = {FRAME_START, msg_id, data_len, seq}; @@ -297,7 +668,6 @@ private: } if (header_valid) { - // 开始收集帧数据 in_frame = true; current_frame.clear(); current_frame.push_back(FRAME_START); @@ -306,32 +676,24 @@ private: current_frame.push_back(seq); current_frame.push_back(header_crc); - // 计算还需要读取多少字节 - size_t total_frame_len = - 5 + 2 + data_len + 1; // 头+CRC16+数据+尾 - size_t remaining = total_frame_len - 5; // 已经读取了5字节 + size_t total_frame_len = 5 + 2 + data_len + 1; + size_t remaining = total_frame_len - 5; - // 检查缓冲区中是否有足够的数据 if (i + 5 + remaining <= rx_buffer.size()) { - // 数据足够，直接复制剩余部分 current_frame.insert(current_frame.end(), rx_buffer.begin() + i + 5, rx_buffer.begin() + i + 5 + remaining); - // 检查帧尾 if (current_frame.back() == FRAME_END) { // 解析帧 - processFrame(current_frame); + parseFrame(current_frame); } - // 无论是否有帧尾，都结束当前帧处理 in_frame = false; i += total_frame_len; continue; } else { - // 数据不够，需要等待更多数据 - // 删除已处理的部分，保留当前帧的后续数据 rx_buffer.erase(rx_buffer.begin(), rx_buffer.begin() + i); - i = 5; // 保留当前帧头 + i = 5; break; } } @@ -339,17 +701,12 @@ private: } i++; } else { - // 已经在帧中，继续收集数据 current_frame.push_back(byte); - - // 检查是否到达帧尾 if (byte == FRAME_END) { - // 解析帧 - processFrame(current_frame); + parseFrame(current_frame); in_frame = false; i++; } else if (current_frame.size() >= MAX_FRAME_SIZE) { - // 帧太长，丢弃 in_frame = false; i++; } else { @@ -358,40 +715,30 @@ private: } } - // 如果不在帧中且缓冲区太大，清理旧数据 if (!in_frame && rx_buffer.size() > SERIAL_READ_BUF_SIZE) { - // 保留最后一部分数据，可能包含不完整的帧头 if (rx_buffer.size() > 100) { rx_buffer.erase(rx_buffer.begin(), rx_buffer.end() - 100); } } } else if (n < 0 && errno != EAGAIN && errno != EWOULDBLOCK) { - // 读取错误 RCLCPP_ERROR(this->get_logger(), "串口读取错误: %s", strerror(errno)); } - // n == 0 或 EAGAIN: 没有数据可读，继续循环（无sleep，保证实时性） } } - // 处理完整的一帧数据 - void processFrame(const std::vector &frame) { - // 最小帧长度检查：头(5) + CRC16(2) + 尾(1) = 8 + // 解析帧 - 处理 MSG_BODY_ACCELERATION 和 MSG_AHRS + void parseFrame(const std::vector &frame) { if (frame.size() < 8) return; - - // 检查帧尾 if (frame.back() != FRAME_END) return; uint8_t msg_id = frame[1]; uint8_t data_len = frame[2]; - - // 验证数据长度 size_t expected_len = 5 + 2 + data_len + 1; if (frame.size() != expected_len) return; - // 可选的数据CRC校验 if (enable_crc_) { uint16_t data_crc = (frame[5] << 8) | frame[6]; uint16_t calc_crc = calculateCRC16(frame.data() + 7, data_len); @@ -399,144 +746,177 @@ private: return; } - // 解析数据包 - parsePacket(msg_id, frame.data() + 7, data_len); - } + const uint8_t *payload = frame.data() + 7; - void parsePacket(uint8_t msg_id, const uint8_t *data, uint8_t len) { - switch (msg_id) { - case MSG_AHRS: - ahrs_count_++; - parseAHRS(data, len); - break; - case MSG_BODY_ACCELERATION: - body_acc_count_++; - parseBodyAcceleration(data, len); - break; - default: - break; + // 处理 MSG_BODY_ACCELERATION 数据包 (0x62) + if (msg_id == MSG_BODY_ACCELERATION) { + if (data_len >= 16) { + AccelData 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(); + + // 推入加速度环形缓冲区 + accel_ring_buffer_.push(data); + } + } + // 处理 MSG_AHRS 数据包 (0x41) + else if (msg_id == MSG_AHRS) { + if (data_len >= 48) { + AhrsData data; + data.valid = true; + + // 角速度 (rad/s) + data.roll_speed = *reinterpret_cast(payload); + data.pitch_speed = *reinterpret_cast(payload + 4); + data.heading_speed = *reinterpret_cast(payload + 8); + + // 欧拉角 (rad) + 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); + + // 时间戳 (us) + data.timestamp_us = *reinterpret_cast(payload + 40); + + data.receive_time = std::chrono::steady_clock::now(); + + // 推入 AHRS 环形缓冲区 + ahrs_ring_buffer_.push(data); + } } } - // 解析 AHRS 数据 (0x41) - void parseAHRS(const uint8_t *data, uint8_t len) { - if (len < 48) - return; + // 融合线程 - 同步加速度数据和 AHRS 数据 + void fusionLoop() { + while (running_ && rclcpp::ok()) { + // 尝试获取最新的加速度和 AHRS 数据 + AccelData accel_data; + AhrsData ahrs_data; - // 解析角速度 (rad/s) - float roll_speed = *reinterpret_cast(data); - float pitch_speed = *reinterpret_cast(data + 4); - float heading_speed = *reinterpret_cast(data + 8); + bool has_accel = accel_ring_buffer_.peek_latest(accel_data); + bool has_ahrs = ahrs_ring_buffer_.peek_latest(ahrs_data); - // 解析欧拉角 (rad) - float roll = *reinterpret_cast(data + 12); - float pitch = *reinterpret_cast(data + 16); - float heading = *reinterpret_cast(data + 20); + if (has_accel && has_ahrs) { + // 检查时间同步（简单策略：都取最新的） + // 使用 UKF 对加速度进行滤波 + float filtered_acc_x = ukf_x_.update(accel_data.acc_x); + float filtered_acc_y = ukf_y_.update(accel_data.acc_y); + float filtered_acc_z = ukf_z_.update(accel_data.acc_z); - // 解析四元数 - float q1 = *reinterpret_cast(data + 24); - float q2 = *reinterpret_cast(data + 28); - float q3 = *reinterpret_cast(data + 32); - float q4 = *reinterpret_cast(data + 36); + // 创建融合后的数据 + ImuDataFrame fused_data; + fused_data.valid = true; - // 解析时间戳 (int64_t, us) - int64_t timestamp_us = *reinterpret_cast(data + 40); + // UKF 滤波后的加速度 + fused_data.acc_x = filtered_acc_x; + fused_data.acc_y = filtered_acc_y; + fused_data.acc_z = filtered_acc_z; - // 将 IMU 时间戳转换为 ROS 时间 - // 使用当前 ROS 时间作为基准，计算相对时间 - rclcpp::Time current_time = this->now(); - rclcpp::Time imu_time = - imu_base_time_ + rclcpp::Duration(0, timestamp_us * 1000); + // AHRS 提供的角速度 (注意：AHRS中的RollSpeed对应X轴角速度) + fused_data.gyro_x = ahrs_data.roll_speed; + fused_data.gyro_y = ahrs_data.pitch_speed; + fused_data.gyro_z = ahrs_data.heading_speed; - // 如果是第一个数据包，初始化基准时间 - if (!imu_time_initialized_) { - imu_base_time_ = current_time - rclcpp::Duration(0, timestamp_us * 1000); - imu_time = current_time; - imu_time_initialized_ = true; + // AHRS 提供的四元数 (q1是实部，ROS使用q4作为实部，注意转换) + // FDI: [q1, q2, q3, q4] where q1 is scalar + // ROS: [x, y, z, w] where w is scalar + fused_data.q1 = ahrs_data.q2; // x + fused_data.q2 = ahrs_data.q3; // y + fused_data.q3 = ahrs_data.q4; // z + fused_data.q4 = ahrs_data.q1; // w (实部) + + fused_data.timestamp = this->now(); + + // 推入输出缓冲区 + output_ring_buffer_.push(fused_data); + } + + // 短暂休眠 + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } + } - // 保存最新的 IMU 时间戳，供 BodyAcceleration 使用 - latest_imu_timestamp_ = imu_time; + // 处理循环 - 固定频率从环形缓冲区读取并发布 + void processLoop() { + ImuDataFrame data; + auto next_publish_time = std::chrono::steady_clock::now(); + + while (running_ && rclcpp::ok()) { + // 等待到下一次发布时间 + std::this_thread::sleep_until(next_publish_time); + next_publish_time += PUBLISH_PERIOD; + + // 获取最新的数据（如果有） + // 策略：只取最新的一个数据，丢弃中间堆积的数据 + bool has_data = false; + ImuDataFrame latest_data; + + // 清空缓冲区，只保留最新的 + while (output_ring_buffer_.try_pop(data)) { + latest_data = data; + has_data = true; + } + + if (has_data) { + publishData(latest_data); + } + } + } + + // 发布数据 + void publishData(const ImuDataFrame &data) { + // 发布加速度消息 (UKF 滤波后的数据) + geometry_msgs::msg::Vector3 accel_msg; + accel_msg.x = data.acc_x; + accel_msg.y = data.acc_y; + accel_msg.z = data.acc_z; + accel_pub_->publish(accel_msg); // 发布 IMU 消息 sensor_msgs::msg::Imu imu_msg; - imu_msg.header.stamp = imu_time; + imu_msg.header.stamp = data.timestamp; imu_msg.header.frame_id = "imu_link"; - // 角速度 (转换为 ROS 坐标系: x=roll, y=pitch, z=heading) - imu_msg.angular_velocity.x = roll_speed; - imu_msg.angular_velocity.y = pitch_speed; - imu_msg.angular_velocity.z = heading_speed; + // 线性加速度 - 使用 UKF 滤波后的值 + imu_msg.linear_acceleration.x = data.acc_x; + imu_msg.linear_acceleration.y = data.acc_y; + imu_msg.linear_acceleration.z = data.acc_z; - // 四元数 (FDI: Q1=w, Q2=x, Q3=y, Q4=z -> ROS: x,y,z,w) - imu_msg.orientation.x = q2; - imu_msg.orientation.y = q3; - imu_msg.orientation.z = q4; - imu_msg.orientation.w = q1; + // 角速度 - 使用 AHRS 提供的角速度 + imu_msg.angular_velocity.x = data.gyro_x; + imu_msg.angular_velocity.y = data.gyro_y; + imu_msg.angular_velocity.z = data.gyro_z; - // 线性加速度暂时设为0（从 MSG_BODY_ACCELERATION 获取） - imu_msg.linear_acceleration.x = 0; - imu_msg.linear_acceleration.y = 0; - imu_msg.linear_acceleration.z = 0; + // 姿态 - 使用 AHRS 提供的四元数 + imu_msg.orientation.x = data.q1; + imu_msg.orientation.y = data.q2; + imu_msg.orientation.z = data.q3; + imu_msg.orientation.w = data.q4; - ahrs_pub_->publish(imu_msg); + imu_pub_->publish(imu_msg); - // 根据 verbose 参数输出日志 - bool verbose = this->get_parameter("verbose").as_bool(); - if (verbose) { - RCLCPP_INFO( - this->get_logger(), - "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad, Time: %ld us", - roll, pitch, heading, timestamp_us); - } else { - RCLCPP_DEBUG(this->get_logger(), - "AHRS - Roll: %.3f, Pitch: %.3f, Heading: %.3f rad", roll, - pitch, heading); - } - } - - // 解析机体系加速度数据 (0x62) - void parseBodyAcceleration(const uint8_t *data, uint8_t len) { - if (len < 16) - return; - - // 解析机体系加速度 (m/s²) - float acc_x = *reinterpret_cast(data); - float acc_y = *reinterpret_cast(data + 4); - float acc_z = *reinterpret_cast(data + 8); - - // 解析重力加速度 - float g_force = *reinterpret_cast(data + 12); - - // 使用与 AHRS 相同的时间戳（如果可用），否则使用当前时间 - rclcpp::Time msg_time = latest_imu_timestamp_; - if (msg_time.nanoseconds() == 0) { - msg_time = this->now(); - } - - // 发布机体系加速度 - geometry_msgs::msg::Vector3 acc_msg; - acc_msg.x = acc_x; - acc_msg.y = acc_y; - acc_msg.z = acc_z; - body_acc_pub_->publish(acc_msg); - - // 发布重力加速度 - std_msgs::msg::Float32 g_msg; - g_msg.data = g_force; - g_force_pub_->publish(g_msg); - - // 根据 verbose 参数输出日志 - bool verbose = this->get_parameter("verbose").as_bool(); - if (verbose) { - RCLCPP_INFO(this->get_logger(), - "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s2, G: %.3f", acc_x, - acc_y, acc_z, g_force); - } else { - RCLCPP_DEBUG(this->get_logger(), - "BodyAcc - X: %.3f, Y: %.3f, Z: %.3f m/s2, G: %.3f", acc_x, - acc_y, acc_z, g_force); - } + // 日志 + RCLCPP_INFO(this->get_logger(), + "Accel(UKF): [%.3f, %.3f, %.3f] m/s² | " + "Gyro: [%.3f, %.3f, %.3f] rad/s | " + "Quat: [%.3f, %.3f, %.3f, %.3f]", + data.acc_x, data.acc_y, data.acc_z, data.gyro_x, data.gyro_y, + data.gyro_z, data.q1, data.q2, data.q3, data.q4); } void publishStatus() { @@ -544,10 +924,8 @@ private: msg.data = is_connected_ && (serial_fd_ >= 0); connection_status_pub_->publish(msg); - if (is_connected_) { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000, - "IMU 接收正常 - AHRS: %zu, BodyAcc: %zu", - ahrs_count_.load(), body_acc_count_.load()); + if (!is_connected_) { + RCLCPP_WARN(this->get_logger(), "IMU 接收不正常"); } } @@ -560,19 +938,21 @@ private: bool is_connected_ = false; std::atomic running_{true}; std::thread receive_thread_; + std::thread fusion_thread_; - // 统计信息 - std::atomic ahrs_count_; - std::atomic body_acc_count_; + // 线程 + std::thread process_thread_; + RingBuffer accel_ring_buffer_; + RingBuffer ahrs_ring_buffer_; + RingBuffer output_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_acc_pub_; - rclcpp::Publisher::SharedPtr g_force_pub_; + rclcpp::Publisher::SharedPtr imu_pub_; + rclcpp::Publisher::SharedPtr accel_pub_; rclcpp::Publisher::SharedPtr connection_status_pub_; rclcpp::TimerBase::SharedPtr status_timer_; }; diff --git a/src/uart_transmitter_node.cpp b/src/uart_transmitter_node.cpp index 9413ffd..c2e8c7e 100644 --- a/src/uart_transmitter_node.cpp +++ b/src/uart_transmitter_node.cpp @@ -38,13 +38,16 @@ public: this->declare_parameter("send_frequency", 50.0); // Hz // 控制参数 - this->declare_parameter("x_move", 0); // 平动左右 [-660, 660] - this->declare_parameter("y_move", 0); // 平动前后 [-660, 660] - this->declare_parameter("yaw", 0); // 云台偏航 [-660, 660] - this->declare_parameter("pitch", 0); // 云台俯仰 [-660, 660] - this->declare_parameter("feed", 0); // 拨弹轮 [-660, 660] - this->declare_parameter("left_switch", 0); // 左拨杆 [1, 3] - this->declare_parameter("right_switch", 0); // 右拨杆 [1, 3] + this->declare_parameter("x_move", 0); // 平动左右 [-660, 660] 负为向左 + this->declare_parameter("y_move", 0); // 平动前后 [-660, 660] 负为向后 + this->declare_parameter("yaw", 0); // 云台偏航 [-660, 660] 负为向左 + this->declare_parameter("pitch", 0); // 云台俯仰 [-660, 660] 负为向下 + this->declare_parameter("feed", + 0); // 拨弹轮 [-660, 660]， 660 开火，0 无动作 + this->declare_parameter("left_switch", + 3); // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭 + this->declare_parameter("right_switch", + 3); // 右拨杆 [1, 3] 2 小陀螺 3 关闭 // 获取参数 serial_port_ = this->get_parameter("serial_port").as_string(); @@ -178,7 +181,7 @@ private: case 921600: return B921600; default: - RCLCPP_WARN(this->get_logger(), "不支持的波特率 %d，使用 115200", + RCLCPP_WARN(this->get_logger(), "不支持的波特率 %d, 使用 115200", baudrate); return B115200; } @@ -240,9 +243,10 @@ private: // CRC8 (除帧头和帧尾外的所有数据，不包括CRC本身) // 当前idx=13, 数据长度=11 (从frame[2]到frame[12]) // uint8_t crc_value = calculateCRC8(frame + 2, 11); - frame[idx++] = 0xCC; // crc_value + // 帧尾 1，接收没有写 CRC8，因此固定为 CC + frame[idx++] = 0xCC; - // 帧尾 + // 帧尾 2 frame[idx++] = FRAME_TAIL; // 调试输出：打印完整帧内容