VSAG/amadeus_26
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

滤波参数，四元数

Browse Source
This commit is contained in:
Huang Haoyu
2026-03-26 18:54:59 +08:00
parent 00157d6410
commit 892a3aec4d
1 changed files with 176 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

220
src/imu_receiver_node.cpp
View File

@@ -8,10 +8,12 @@
*
* 转发数据包：
* - MSG_BODY_ACCELERATION (0x62): 滤波修正后的机体系加速度（不包括重力）
* - MSG_AHRS (0x41): 航姿参考系统数据（四元数、角速度）
*
* 特性：
* - 使用环形缓冲区避免数据堆积爆发
* - 使用 UKF（无迹卡尔曼滤波器）对加速度数据进行滤波
* - 融合 AHRS 姿态数据和 UKF 滤波后的加速度
*/
#include <atomic>
@@ -41,8 +43,9 @@
constexpr uint8_t FRAME_START = 0xFC;
constexpr uint8_t FRAME_END = 0xFD;
// 数据包 ID - 只使用 MSG_BODY_ACCELERATION
// 数据包 ID
constexpr uint8_t MSG_BODY_ACCELERATION = 0x62; // 滤波修正后的机体系加速度
constexpr uint8_t MSG_AHRS = 0x41; // 航姿参考系统数据
// 默认串口设备
constexpr const char *DEFAULT_SERIAL_PORT = "/dev/ttyIMU";
@@ -63,6 +66,9 @@ 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,
@@ -319,8 +325,32 @@ private:
bool initialized_;
};
// IMU 数据结构 - 使用 MSG_BODY_ACCELERATION 数据包
struct ImuDataFrame {
// 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²) - 不包括重力
@@ -329,10 +359,27 @@ struct ImuDataFrame {
// 当地重力加速度 (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 <typename T> class RingBuffer {
public:
@@ -396,7 +443,9 @@ private:
class ImuReceiverNode : public rclcpp::Node {
public:
ImuReceiverNode()
: Node("imu_receiver_node"), ring_buffer_(RING_BUFFER_SIZE) {
: 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);
@@ -431,7 +480,7 @@ public:
ukf_z_.setDt(dt);
RCLCPP_INFO(this->get_logger(), "---------------------------------");
RCLCPP_INFO(this->get_logger(), "IMU 数据接收节点 (UKF 版本)");
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",
@@ -458,6 +507,9 @@ public:
// 创建接收线程
receive_thread_ = std::thread(&ImuReceiverNode::receiveLoop, this);
// 创建融合线程
fusion_thread_ = std::thread(&ImuReceiverNode::fusionLoop, this);
// 创建处理线程（固定频率处理数据）
process_thread_ = std::thread(&ImuReceiverNode::processLoop, this);
@@ -476,6 +528,9 @@ public:
if (receive_thread_.joinable()) {
receive_thread_.join();
}
if (fusion_thread_.joinable()) {
fusion_thread_.join();
}
if (process_thread_.joinable()) {
process_thread_.join();
}
@@ -671,7 +726,7 @@ private:
}
}
// 解析帧 - 只处理 MSG_BODY_ACCELERATION
// 解析帧 - 处理 MSG_BODY_ACCELERATION 和 MSG_AHRS
void parseFrame(const std::vector<uint8_t> &frame) {
if (frame.size() < 8)
return;
@@ -693,10 +748,10 @@ private:
const uint8_t *payload = frame.data() + 7;
// 只处理 MSG_BODY_ACCELERATION 数据包 (0x62)
// 处理 MSG_BODY_ACCELERATION 数据包 (0x62)
if (msg_id == MSG_BODY_ACCELERATION) {
if (data_len >= 16) {
ImuDataFrame data;
AccelData data;
data.valid = true;
// 滤波修正后的机体系加速度 (m/s²) - 不包括重力
@@ -709,11 +764,91 @@ private:
data.receive_time = std::chrono::steady_clock::now();
// 推入环形缓冲区
ring_buffer_.push(data);
// 推入加速度环形缓冲区
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<const float *>(payload);
data.pitch_speed = *reinterpret_cast<const float *>(payload + 4);
data.heading_speed = *reinterpret_cast<const float *>(payload + 8);
// 欧拉角 (rad)
data.roll = *reinterpret_cast<const float *>(payload + 12);
data.pitch = *reinterpret_cast<const float *>(payload + 16);
data.heading = *reinterpret_cast<const float *>(payload + 20);
// 四元数
data.q1 = *reinterpret_cast<const float *>(payload + 24);
data.q2 = *reinterpret_cast<const float *>(payload + 28);
data.q3 = *reinterpret_cast<const float *>(payload + 32);
data.q4 = *reinterpret_cast<const float *>(payload + 36);
// 时间戳 (us)
data.timestamp_us = *reinterpret_cast<const int64_t *>(payload + 40);
data.receive_time = std::chrono::steady_clock::now();
// 推入 AHRS 环形缓冲区
ahrs_ring_buffer_.push(data);
}
}
}
// 融合线程 - 同步加速度数据和 AHRS 数据
void fusionLoop() {
while (running_ && rclcpp::ok()) {
// 尝试获取最新的加速度和 AHRS 数据
AccelData accel_data;
AhrsData ahrs_data;
bool has_accel = accel_ring_buffer_.peek_latest(accel_data);
bool has_ahrs = ahrs_ring_buffer_.peek_latest(ahrs_data);
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);
// 创建融合后的数据
ImuDataFrame fused_data;
fused_data.valid = true;
// UKF 滤波后的加速度
fused_data.acc_x = filtered_acc_x;
fused_data.acc_y = filtered_acc_y;
fused_data.acc_z = filtered_acc_z;
// 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;
// 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));
}
}
// 处理循环 - 固定频率从环形缓冲区读取并发布
@@ -732,7 +867,7 @@ private:
ImuDataFrame latest_data;
// 清空缓冲区，只保留最新的
while (ring_buffer_.try_pop(data)) {
while (output_ring_buffer_.try_pop(data)) {
latest_data = data;
has_data = true;
}
@@ -743,51 +878,45 @@ private:
}
}
// 发布数据 - 使用 UKF 对加速度进行滤波
// 发布数据
void publishData(const ImuDataFrame &data) {
rclcpp::Time current_time = this->now();
// 使用 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);
// 发布加速度消息 (滤波后的数据)
// 发布加速度消息 (UKF 滤波后的数据)
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_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 = current_time;
imu_msg.header.stamp = data.timestamp;
imu_msg.header.frame_id = "imu_link";
// 线性加速度 - 使用 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;
imu_msg.linear_acceleration.x = data.acc_x;
imu_msg.linear_acceleration.y = data.acc_y;
imu_msg.linear_acceleration.z = data.acc_z;
// 角速度和姿态暂时设为0（MSG_BODY_ACCELERATION 不包含这些数据）
imu_msg.angular_velocity.x = 0;
imu_msg.angular_velocity.y = 0;
imu_msg.angular_velocity.z = 0;
// 角速度 - 使用 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;
imu_msg.orientation.x = 0;
imu_msg.orientation.y = 0;
imu_msg.orientation.z = 0;
imu_msg.orientation.w = 1; // 单位四元数
// 姿态 - 使用 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;
imu_pub_->publish(imu_msg);
// 日志
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);
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() {
@@ -809,10 +938,13 @@ private:
bool is_connected_ = false;
std::atomic<bool> running_{true};
std::thread receive_thread_;
std::thread fusion_thread_;
// 线程
std::thread process_thread_;
RingBuffer<ImuDataFrame> ring_buffer_;
RingBuffer<AccelData> accel_ring_buffer_;
RingBuffer<AhrsData> ahrs_ring_buffer_;
RingBuffer<ImuDataFrame> output_ring_buffer_;
// UKF 滤波器（用于加速度滤波）
UKF ukf_x_;