惯导初步实现

CMakeLists.txt

@@ -45,10 +45,22 @@ ament_target_dependencies(imu_receiver_node
   Eigen3
 )
 
+# ==================== 导航节点 ====================
+add_executable(navigation_node src/navigation_node.cpp)
+
+ament_target_dependencies(navigation_node 
+  rclcpp 
+  std_msgs
+  geometry_msgs
+  sensor_msgs
+  Eigen3
+)
+
 # ==================== 安装目标 ====================
 install(TARGETS 
   uart_transmitter_node
   imu_receiver_node
+  navigation_node
   DESTINATION lib/${PROJECT_NAME}
 )
 

README.md

@@ -90,4 +90,5 @@ stateDiagram-v2
 - 基地：同样为场地中角点描述的矩形，我方基地为 $ (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 轴数据结合解算出在二维平面上的加速度和位移。
+- 机器人的朝向和位置计算：IMU 安装于机器人圆心，开始时，IMU X 轴正方向（机器人朝向）与场地 Y 轴负方向相同，Y 轴正方向与场地 X 轴负方向相同，IMU Z 轴则朝向地面。当机器人朝向发生改变时，IMU 轴系的朝向也会相应发生改变，因此需要结合陀螺仪数据才能正确反向解算加速度计带来的加速度信息，从而完成惯性导航。
+- 根据已测量到的 AHRS 返回的数据，yaw 轴在 360° 范围内值为 $ (-3,3) $（左转为增，增到 3 再左转则为 -3）。根据最开始静止测量到的 yaw 值可以确定 **场地朝向**（因为开始时机器人始终朝向赛场坐标系 Y 轴负方向）。

src/navigation_node.cpp

@@ -0,0 +1,511 @@
+/**
+ * @file navigation_node.cpp
+ * @brief 导航节点 - 根据IMU数据进行惯性导航
+ *
+ * 功能：
+ * - 根据IMU加速度数据计算机器人位置（双重积分）
+ * - 根据AHRS的heading计算机器人在场地坐标系中的朝向
+ * - 订阅机器人状态，根据状态决定导航策略
+ *
+ * 坐标系说明：
+ * - 场地坐标系：左下角为原点(0,0)，X轴向右(0-12m)，Y轴向上(0-8m)
+ * - IMU坐标系：开始时X轴朝场地Y轴负方向，Y轴朝场地X轴负方向，Z轴朝地面
+ */
+
+#include <cmath>
+#include <deque>
+#include <eigen3/Eigen/Dense>
+#include <geometry_msgs/msg/point.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/int32.hpp>
+
+// 地图配置
+constexpr double MAP_WIDTH = 12.0;     // 场地宽度 (X方向) 单位：米
+constexpr double MAP_HEIGHT = 8.0;     // 场地高度 (Y方向) 单位：米
+constexpr double ROBOT_RADIUS = 0.375; // 机器人半径 0.75m直径
+
+// 初始位置
+constexpr double INITIAL_X = 1.235;
+constexpr double INITIAL_Y = 6.265;
+
+// 障碍区定义
+struct Rectangle {
+  double x1, y1, x2, y2;
+};
+
+constexpr Rectangle OBSTACLE1 = {2.5, 1.5, 4.5, 8.0};
+constexpr Rectangle OBSTACLE2 = {7.5, 0.0, 9.5, 6.5};
+
+// 我方基地
+constexpr Rectangle HOME_BASE = {0.0, 6.0, 1.5, 8.0};
+
+// 增益点
+constexpr Rectangle BUFF_ZONE = {4.5, 2.5, 7.5, 5.5};
+
+// 机器人状态
+enum class RobotState {
+  WAITING = 0,     // 等待
+  MOVING_TO_POINT, // 进点
+  SEARCHING,       // 索敌
+  ATTACKING,       // 攻击
+  RETURNING_HOME   // 回家
+};
+
+// 简单低通滤波器
+class LowPassFilter {
+public:
+  LowPassFilter(double alpha = 0.1) : alpha_(alpha), initialized_(false) {}
+
+  double filter(double value) {
+    if (!initialized_) {
+      last_value_ = value;
+      initialized_ = true;
+      return value;
+    }
+    last_value_ = alpha_ * value + (1.0 - alpha_) * last_value_;
+    return last_value_;
+  }
+
+  void reset() { initialized_ = false; }
+
+private:
+  double alpha_;
+  double last_value_;
+  bool initialized_;
+};
+
+// 滑动窗口平均滤波器
+class MovingAverageFilter {
+public:
+  explicit MovingAverageFilter(size_t window_size = 10)
+      : window_size_(window_size) {}
+
+  double filter(double value) {
+    buffer_.push_back(value);
+    if (buffer_.size() > window_size_) {
+      buffer_.pop_front();
+    }
+
+    if (buffer_.empty())
+      return value;
+
+    double sum = 0.0;
+    for (double v : buffer_) {
+      sum += v;
+    }
+    return sum / buffer_.size();
+  }
+
+  void reset() { buffer_.clear(); }
+
+private:
+  size_t window_size_;
+  std::deque<double> buffer_;
+};
+
+class NavigationNode : public rclcpp::Node {
+public:
+  NavigationNode() : Node("navigation_node") {
+    // 声明参数
+    this->declare_parameter("initial_x", INITIAL_X);
+    this->declare_parameter("initial_y", INITIAL_Y);
+    this->declare_parameter("velocity_decay", 0.95); // 速度衰减系数（零速校正）
+    this->declare_parameter("position_correction", 0.001); // 位置漂移校正
+    this->declare_parameter("static_threshold", 0.05); // 静止检测阈值 (m/s²)
+    this->declare_parameter("calibration_samples", 100); // 初始校准采样数
+    this->declare_parameter("acc_lpf_alpha", 0.1); // 加速度低通滤波系数
+    this->declare_parameter("vel_lpf_alpha", 0.2); // 速度低通滤波系数
+
+    // 获取参数
+    pos_x_ = this->get_parameter("initial_x").as_double();
+    pos_y_ = this->get_parameter("initial_y").as_double();
+    velocity_decay_ = this->get_parameter("velocity_decay").as_double();
+    position_correction_ =
+        this->get_parameter("position_correction").as_double();
+    static_threshold_ = this->get_parameter("static_threshold").as_double();
+    calibration_samples_ = this->get_parameter("calibration_samples").as_int();
+    acc_lpf_alpha_ = this->get_parameter("acc_lpf_alpha").as_double();
+    vel_lpf_alpha_ = this->get_parameter("vel_lpf_alpha").as_double();
+
+    // 初始化滤波器
+    acc_filter_x_.reset(new LowPassFilter(acc_lpf_alpha_));
+    acc_filter_y_.reset(new LowPassFilter(acc_lpf_alpha_));
+    vel_filter_x_.reset(new LowPassFilter(vel_lpf_alpha_));
+    vel_filter_y_.reset(new LowPassFilter(vel_lpf_alpha_));
+
+    RCLCPP_INFO(this->get_logger(), "========================================");
+    RCLCPP_INFO(this->get_logger(), "导航节点初始化");
+    RCLCPP_INFO(this->get_logger(), "初始位置: (%.3f, %.3f)", pos_x_, pos_y_);
+    RCLCPP_INFO(this->get_logger(), "速度衰减系数: %.3f", velocity_decay_);
+    RCLCPP_INFO(this->get_logger(), "静止检测阈值: %.3f m/s²",
+                static_threshold_);
+    RCLCPP_INFO(this->get_logger(), "校准采样数: %d", calibration_samples_);
+    RCLCPP_INFO(this->get_logger(), "========================================");
+
+    // 创建订阅者
+    imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
+        "imu/data", 10,
+        std::bind(&NavigationNode::imuCallback, this, std::placeholders::_1));
+
+    state_sub_ = this->create_subscription<std_msgs::msg::Int32>(
+        "robot_state", 10,
+        std::bind(&NavigationNode::stateCallback, this, std::placeholders::_1));
+
+    // 创建发布者
+    pose_pub_ =
+        this->create_publisher<geometry_msgs::msg::Point>("robot_pose", 10);
+    velocity_pub_ = this->create_publisher<geometry_msgs::msg::Vector3>(
+        "robot_velocity", 10);
+    heading_pub_ =
+        this->create_publisher<std_msgs::msg::Float64>("robot_heading", 10);
+    nav_cmd_pub_ =
+        this->create_publisher<geometry_msgs::msg::Vector3>("nav_cmd", 10);
+
+    // 创建定时器，用于定期发布导航信息
+    timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(20), // 50Hz
+        std::bind(&NavigationNode::navigationLoop, this));
+
+    RCLCPP_INFO(this->get_logger(), "导航节点初始化完成");
+  }
+
+private:
+  // IMU数据回调
+  void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) {
+    // 提取四元数 (ROS格式: x, y, z, w)
+    double qx = msg->orientation.x;
+    double qy = msg->orientation.y;
+    double qz = msg->orientation.z;
+    double qw = msg->orientation.w;
+
+    // 将四元数转换为欧拉角 (roll, pitch, yaw)
+    // yaw: 绕Z轴旋转 (heading)
+    double siny_cosp = 2.0 * (qw * qz + qx * qy);
+    double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
+    double yaw = std::atan2(siny_cosp, cosy_cosp);
+
+    // roll: 绕X轴旋转
+    double sinr_cosp = 2.0 * (qw * qx + qy * qz);
+    double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy);
+    double roll = std::atan2(sinr_cosp, cosr_cosp);
+
+    // pitch: 绕Y轴旋转
+    double sinp = 2.0 * (qw * qy - qz * qx);
+    double pitch = std::asin(std::clamp(sinp, -1.0, 1.0));
+
+    // 保存姿态数据
+    current_roll_ = roll;
+    current_pitch_ = pitch;
+    current_heading_ = yaw; // 这是IMU坐标系下的heading
+
+    // 初始校准阶段：收集初始heading样本
+    if (is_calibrating_) {
+      heading_samples_.push_back(yaw);
+      if (heading_samples_.size() >=
+          static_cast<size_t>(calibration_samples_)) {
+        // 计算平均初始heading
+        double sum = 0.0;
+        for (double h : heading_samples_) {
+          sum += h;
+        }
+        initial_heading_ = sum / heading_samples_.size();
+        is_calibrating_ = false;
+        RCLCPP_INFO(this->get_logger(),
+                    "初始校准完成! 初始heading: %.4f rad (%.2f°)",
+                    initial_heading_, initial_heading_ * 180.0 / M_PI);
+      }
+      return;
+    }
+
+    // 计算场地坐标系下的朝向
+    // 初始时IMU X轴朝场地Y轴负方向，即heading = -PI/2 对应场地朝向0
+    // 需要建立IMU heading与场地朝向的映射关系
+
+    // 计算相对于初始朝向的变化
+    double heading_delta = normalizeAngle(yaw - initial_heading_);
+
+    // 场地朝向：初始时机器人朝场地Y轴负方向（朝向下方）
+    // 当IMU顺时针旋转（yaw增大），场地朝向顺时针变化
+    field_heading_ = normalizeAngle(-M_PI_2 + heading_delta);
+
+    // 提取加速度 (IMU坐标系)
+    double acc_x_imu = msg->linear_acceleration.x;
+    double acc_y_imu = msg->linear_acceleration.y;
+
+    // 应用低通滤波
+    acc_x_imu = acc_filter_x_->filter(acc_x_imu);
+    acc_y_imu = acc_filter_y_->filter(acc_y_imu);
+
+    // 静止检测：加速度小于阈值时认为是静止状态
+    double acc_magnitude =
+        std::sqrt(acc_x_imu * acc_x_imu + acc_y_imu * acc_y_imu);
+    is_static_ = acc_magnitude < static_threshold_;
+
+    // 将加速度从IMU坐标系转换到场地坐标系
+    // IMU坐标系到场地坐标系的旋转：
+    // - 初始时IMU X轴朝场地-Y方向，IMU Y轴朝场地-X方向
+    // - 需要先将加速度旋转到与场地坐标系对齐
+
+    // 旋转矩阵：从IMU坐标系到场地坐标系
+    // 机器人朝向field_heading_时，需要将IMU加速度旋转
+    double cos_h = std::cos(field_heading_);
+    double sin_h = std::sin(field_heading_);
+
+    // IMU X轴朝机器人前方，Y轴朝机器人左方
+    // 场地坐标系：X向右，Y向上
+    // 机器人前方对应场地坐标系的(cos(field_heading_), sin(field_heading_))
+    // 机器人左方对应场地坐标系的(-sin(field_heading_), cos(field_heading_))
+
+    // 注意：IMU的X轴是机器人前方，Y轴是左方
+    // 场地坐标系转换：前方分量映射到场地朝向，左方分量垂直于场地朝向
+    acc_field_x_ = acc_x_imu * cos_h - acc_y_imu * sin_h;
+    acc_field_y_ = acc_x_imu * sin_h + acc_y_imu * cos_h;
+
+    // 获取当前时间
+    rclcpp::Time current_time = msg->header.stamp;
+    if (!last_time_.nanoseconds()) {
+      last_time_ = current_time;
+      return;
+    }
+
+    // 计算时间差
+    double dt = (current_time - last_time_).seconds();
+    last_time_ = current_time;
+
+    if (dt <= 0 || dt > 0.1) { // 时间差异常，跳过本次计算
+      return;
+    }
+
+    // 速度积分（使用梯形积分）
+    vel_x_ += (acc_field_x_ + last_acc_field_x_) * 0.5 * dt;
+    vel_y_ += (acc_field_y_ + last_acc_field_y_) * 0.5 * dt;
+
+    // 保存当前加速度用于下次积分
+    last_acc_field_x_ = acc_field_x_;
+    last_acc_field_y_ = acc_field_y_;
+
+    // 零速校正：如果检测到静止，逐渐减小速度
+    if (is_static_) {
+      vel_x_ *= velocity_decay_;
+      vel_y_ *= velocity_decay_;
+
+      // 如果速度很小，直接置零
+      if (std::abs(vel_x_) < 0.01)
+        vel_x_ = 0.0;
+      if (std::abs(vel_y_) < 0.01)
+        vel_y_ = 0.0;
+    }
+
+    // 应用速度滤波
+    vel_x_ = vel_filter_x_->filter(vel_x_);
+    vel_y_ = vel_filter_y_->filter(vel_y_);
+
+    // 位置积分（使用梯形积分）
+    pos_x_ += vel_x_ * dt;
+    pos_y_ += vel_y_ * dt;
+
+    // 边界检查：确保机器人在场地范围内
+    pos_x_ = std::clamp(pos_x_, ROBOT_RADIUS, MAP_WIDTH - ROBOT_RADIUS);
+    pos_y_ = std::clamp(pos_y_, ROBOT_RADIUS, MAP_HEIGHT - ROBOT_RADIUS);
+
+    has_imu_data_ = true;
+  }
+
+  // 状态回调
+  void stateCallback(const std_msgs::msg::Int32::SharedPtr msg) {
+    current_state_ = static_cast<RobotState>(msg->data);
+
+    // 根据状态更新目标位置
+    switch (current_state_) {
+    case RobotState::WAITING:
+      // 等待状态，不需要移动
+      break;
+    case RobotState::MOVING_TO_POINT:
+      // 进点：目标为增益点中心
+      target_x_ = (BUFF_ZONE.x1 + BUFF_ZONE.x2) / 2.0;
+      target_y_ = (BUFF_ZONE.y1 + BUFF_ZONE.y2) / 2.0;
+      break;
+    case RobotState::SEARCHING:
+      // 索敌：在增益点附近巡逻
+      target_x_ = (BUFF_ZONE.x1 + BUFF_ZONE.x2) / 2.0;
+      target_y_ = (BUFF_ZONE.y1 + BUFF_ZONE.y2) / 2.0;
+      break;
+    case RobotState::ATTACKING:
+      // 攻击：保持在当前位置或轻微调整
+      break;
+    case RobotState::RETURNING_HOME:
+      // 回家：目标为我方基地中心
+      target_x_ = (HOME_BASE.x1 + HOME_BASE.x2) / 2.0;
+      target_y_ = (HOME_BASE.y1 + HOME_BASE.y2) / 2.0;
+      break;
+    }
+  }
+
+  // 导航主循环
+  void navigationLoop() {
+    if (!has_imu_data_) {
+      return;
+    }
+
+    // 发布机器人位置 (使用Point，z表示heading)
+    auto pose_msg = geometry_msgs::msg::Point();
+    pose_msg.x = pos_x_;
+    pose_msg.y = pos_y_;
+    pose_msg.z = field_heading_; // 借用z字段存储heading
+    pose_pub_->publish(pose_msg);
+
+    // 发布速度
+    auto vel_msg = geometry_msgs::msg::Vector3();
+    vel_msg.x = vel_x_;
+    vel_msg.y = vel_y_;
+    vel_msg.z = 0.0; // 平面运动，Z向速度为0
+    velocity_pub_->publish(vel_msg);
+
+    // 发布朝向
+    auto heading_msg = std_msgs::msg::Float64();
+    heading_msg.data = field_heading_;
+    heading_pub_->publish(heading_msg);
+
+    // 计算导航指令（简单的位置控制）
+    geometry_msgs::msg::Vector3 nav_cmd;
+
+    if (current_state_ == RobotState::WAITING) {
+      // 等待状态：不移动
+      nav_cmd.x = 0.0; // 前进速度
+      nav_cmd.y = 0.0; // 横向速度
+      nav_cmd.z = 0.0; // 旋转速度
+    } else {
+      // 计算到目标点的向量
+      double dx = target_x_ - pos_x_;
+      double dy = target_y_ - pos_y_;
+      double distance = std::sqrt(dx * dx + dy * dy);
+
+      // 计算目标方向在场地坐标系中的角度
+      double target_angle = std::atan2(dy, dx);
+
+      // 计算需要旋转的角度（目标方向与当前朝向的差）
+      double angle_diff = normalizeAngle(target_angle - field_heading_);
+
+      // 简单的导航控制
+      // 如果距离目标较远，先朝向目标再前进
+      if (distance > 0.3) { // 30cm阈值
+        // 旋转控制：将角度差转换为旋转速度
+        double max_rot_speed = 1.0; // 最大旋转速度 rad/s
+        nav_cmd.z = std::clamp(angle_diff * 2.0, -max_rot_speed, max_rot_speed);
+
+        // 前进控制：当朝向接近目标方向时前进
+        if (std::abs(angle_diff) < 0.3) { // 约17度以内
+          double max_speed = 1.0;         // 最大前进速度 m/s
+          nav_cmd.x = std::min(distance * 2.0, max_speed);
+        } else {
+          nav_cmd.x = 0.0; // 先转向
+        }
+        nav_cmd.y = 0.0; // 暂时不使用横向移动
+      } else {
+        // 到达目标附近，停止
+        nav_cmd.x = 0.0;
+        nav_cmd.y = 0.0;
+        nav_cmd.z = 0.0;
+      }
+    }
+
+    nav_cmd_pub_->publish(nav_cmd);
+
+    // 日志输出（降低频率）
+    static int log_counter = 0;
+    if (++log_counter >= 10) {
+      log_counter = 0;
+      RCLCPP_INFO(this->get_logger(),
+                  "Pos: (%.3f, %.3f) | Heading: %.2f° | Vel: (%.3f, %.3f) | "
+                  "State: %d | Static: %s",
+                  pos_x_, pos_y_, field_heading_ * 180.0 / M_PI, vel_x_, vel_y_,
+                  static_cast<int>(current_state_), is_static_ ? "Yes" : "No");
+    }
+  }
+
+  // 角度归一化到 [-PI, PI]
+  double normalizeAngle(double angle) {
+    while (angle > M_PI)
+      angle -= 2.0 * M_PI;
+    while (angle < -M_PI)
+      angle += 2.0 * M_PI;
+    return angle;
+  }
+
+  // 订阅者
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr state_sub_;
+
+  // 发布者
+  rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr pose_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr velocity_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr heading_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::Vector3>::SharedPtr nav_cmd_pub_;
+
+  // 定时器
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  // 状态
+  RobotState current_state_ = RobotState::WAITING;
+
+  // 位置 (场地坐标系)
+  double pos_x_ = INITIAL_X;
+  double pos_y_ = INITIAL_Y;
+
+  // 速度 (场地坐标系)
+  double vel_x_ = 0.0;
+  double vel_y_ = 0.0;
+
+  // 加速度 (场地坐标系)
+  double acc_field_x_ = 0.0;
+  double acc_field_y_ = 0.0;
+  double last_acc_field_x_ = 0.0;
+  double last_acc_field_y_ = 0.0;
+
+  // 姿态
+  double current_roll_ = 0.0;
+  double current_pitch_ = 0.0;
+  double current_heading_ = 0.0; // IMU坐标系下的heading
+  double field_heading_ = -M_PI_2; // 场地坐标系下的朝向（初始朝Y轴负方向）
+  double initial_heading_ = 0.0; // 初始heading（用于校准）
+
+  // 目标位置
+  double target_x_ = 0.0;
+  double target_y_ = 0.0;
+
+  // 滤波器
+  std::unique_ptr<LowPassFilter> acc_filter_x_;
+  std::unique_ptr<LowPassFilter> acc_filter_y_;
+  std::unique_ptr<LowPassFilter> vel_filter_x_;
+  std::unique_ptr<LowPassFilter> vel_filter_y_;
+
+  // 校准
+  std::vector<double> heading_samples_;
+  bool is_calibrating_ = true;
+  int calibration_samples_ = 100;
+
+  // 参数
+  double velocity_decay_ = 0.95;
+  double position_correction_ = 0.001;
+  double static_threshold_ = 0.05;
+  double acc_lpf_alpha_ = 0.1;
+  double vel_lpf_alpha_ = 0.2;
+
+  // 时间
+  rclcpp::Time last_time_;
+
+  // 标志
+  bool has_imu_data_ = false;
+  bool is_static_ = false;
+};
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<NavigationNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}