diff --git a/CMakeLists.txt b/CMakeLists.txt
index e063353..a80963b 100644
--- a/CMakeLists.txt
+++ b/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}
 )
 
diff --git a/src/navigation_node.cpp b/src/navigation_node.cpp
new file mode 100644
index 0000000..807c942
--- /dev/null
+++ b/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;
+}
\ No newline at end of file