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 19:48:05 +08:00
parent 003ab44016
commit 7be3a7bdab
2 changed files with 523 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
CMakeLists.txt
View File

@@ -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}
)

511
src/navigation_node.cpp Normal file
View File

@@ -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;
}