ROS 2自主移动机器人（AMR）导航与路径规划(3)
·
行为决策与运动控制
本章以 ROS 2 的 Navigation2 (Nav2) 框架为核心，深入展开全局规划、局部规划、控制器与行为树的实现细节。将从零构建一组自定义规划器与控制器插件，并将其集成到完整的自主导航行为中。
1.1 导航系统总体架构
Nav2 采用 规划器‑控制器‑行为树 三层结构，同时依赖 代价地图 提供环境建模。下面这幅文字流程图展示了数据流与控制流。
┌──────────────────── 全局规划层 (Global Planner) ───────────────────┐ │ │ │ [全局规划器插件] │ │ 输入: /map, 起始位姿, 目标位姿 │ │ 输出: nav_msgs::Path (全局路径) │ │ 典型算法: A*, NavFn, Theta*, Smac Planner │ │ │ └────────────────────────────┬────────────────────────────────────────┘ │ /plan ┌────────────────────────────▼────────────────────────────────────────┐ │ 行为树引擎 (Behavior Tree) │ │ │ │ [NavigateToPose] 动作节点 │ │ ├── [ComputePathToPose] (调用全局规划器) │ │ ├── [FollowPath] (调用局部控制器) │ │ ├── [IsStuck / Recovery] (恢复行为) │ │ └── [GoalReached] (条件节点) │ │ │ └────────────────────────────┬────────────────────────────────────────┘ │ 调用 ┌────────────────────────────▼────────────────────────────────────────┐ │ 局部规划层 (Controller) │ │ │ │ [控制器插件] │ │ 输入: /plan (全局路径), /odom, /scan, /tf │ │ 输出: /cmd_vel │ │ 典型算法: DWB, MPPI, Regulated Pure Pursuit, TEB │ │ │ └────────────────────────────┬────────────────────────────────────────┘ │ /cmd_vel ┌────────────────────────────▼────────────────────────────────────────┐ │ 硬件驱动 (Motor Driver) │ │ │ │ 将速度指令转换为电机控制，反馈里程计 │ └────────────────────────────────────────────────────────────────────┘
1.2 文件结构树
建立一个功能包 amr_navigation，包含自定义全局规划器、自定义控制器以及对应的行为树配置。
amr_navigation/ ├── CMakeLists.txt ├── package.xml ├── include/ │ └── amr_navigation/ │ ├── simple_global_planner.hpp # A* 全局规划器插件 │ ├── pid_controller.hpp # PID 控制器插件 │ └── common.hpp # 公共定义与工具 ├── src/ │ ├── simple_global_planner.cpp │ ├── pid_controller.cpp │ └── common.cpp ├── plugins/ │ └── plugins.xml # 插件描述文件 ├── behavior_trees/ │ ├── navigate_to_pose.xml # 自定义行为树 │ └── recovery.xml # 恢复行为子树 ├── config/ │ ├── nav2_params.yaml # 主参数文件 │ ├── global_costmap_params.yaml │ ├── local_costmap_params.yaml │ └── controller_params.yaml ├── launch/ │ └── navigation_launch.py └── test/ └── test_planner.cpp
1.3 主要函数树分析
全局规划器 (SimpleGlobalPlanner)
SimpleGlobalPlanner::configure() ├── 声明参数 (heuristic_weight, diagonal_movement 等) ├── 获取 costmap 指针 └── 初始化节点日志 SimpleGlobalPlanner::createPlan(start, goal) ├── 参数校验 (goal 是否在自由空间) ├── 调用 A* 搜索 │ ├── initialize_open_set(start) │ ├── while (open_set not empty) │ │ ├── current = pop_min_f_cost() │ │ ├── if current == goal → reconstruct_path() │ │ ├── for each neighbor of current │ │ │ ├── cost = current.g + distance(current, neighbor) * weight │ │ │ ├── if neighbor is unvisited or lower g │ │ │ │ └── update g, f, parent │ │ │ └── add to open_set │ │ └── if open_set too large → return failure │ └── return path └── 发布规划调试信息 (可选的 marker) SimpleGlobalPlanner::reconstruct_path(end_node) └── 从目标节点回溯至起点，生成 nav_msgs::Path
控制器 (PidController)
PidController::configure() ├── 声明 PID 增益、角度误差阈值、线速度限制等 ├── 获取 costmap 指针 (用于避障检测) └── 初始化平滑器 PidController::computeVelocityCommands(pose, velocity, goal_pose) ├── 计算位置误差 (dx, dy) ├── 计算偏航角误差 (dyaw) ├── if 角度误差 > 阈值 → 原地旋转 │ └── cmd_vel.angular.z = pid_angular(0, dyaw) ├── else → 前进 + 校正 │ ├── cmd_vel.linear.x = pid_linear(0, dist_error) │ ├── cmd_vel.angular.z = pid_angular(0, dyaw) (同时旋转) │ ├── 速度限制 (加速度、最大速度) │ └── 前方避障检查 (若激光数据异常则减速或急停) ├── 调用速度平滑器 └── 返回 cmd_vel
1.4 全局规划器插件实现
1.4.1 头文件
/**
* @file simple_global_planner.hpp
* @brief 基于 A* 的全局路径规划器，实现 nav2_core::GlobalPlanner 接口
*
* 支持 diagonal_movement 选项，允许使用启发式权重加速搜索。
* 规划结果通过 /plan 话题发布，供行为树使用。
*/
#ifndef AMR_NAVIGATION__SIMPLE_GLOBAL_PLANNER_HPP_
#define AMR_NAVIGATION__SIMPLE_GLOBAL_PLANNER_HPP_
#include <memory>
#include <vector>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "nav2_core/global_planner.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_costmap_2d/costmap_2d.hpp"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/buffer.h"
namespace amr_navigation
{
class SimpleGlobalPlanner : public nav2_core::GlobalPlanner
{
public:
SimpleGlobalPlanner() = default;
~SimpleGlobalPlanner() override = default;
/**
* @brief 配置规划器，从参数服务器加载参数并初始化代价地图指针
* @param parent 父节点 (lifecycle)
* @param name 规划器名称
* @param tf TF 缓冲
* @param costmap_ros 代价地图 ROS 接口
*/
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
/**
* @brief 激活规划器
*/
void activate() override;
/**
* @brief 去激活规划器
*/
void deactivate() override;
/**
* @brief 清理资源
*/
void cleanup() override;
/**
* @brief 计算全局路径
* @param start 起点位姿
* @param goal 目标位姿
* @return 全局路径，若无解则返回空 Path
*/
nav_msgs::msg::Path createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal) override;
private:
rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
nav2_costmap_2d::Costmap2D * costmap_;
std::string global_frame_;
double heuristic_weight_;
bool allow_unknown_;
rclcpp::Logger logger_{rclcpp::get_logger("SimpleGlobalPlanner")};
/**
* @brief 将 costmap 坐标转为 world 坐标
*/
void map_to_world(unsigned int mx, unsigned int my, double & wx, double & wy);
/**
* @brief 将 world 坐标转为 costmap 坐标
*/
bool world_to_map(double wx, double wy, unsigned int & mx, unsigned int & my);
/**
* @brief 检查给定网格是否是自由空间 (cost < lethal)
*/
bool is_free(unsigned int mx, unsigned int my);
};
} // namespace amr_navigation
#endif // AMR_NAVIGATION__SIMPLE_GLOBAL_PLANNER_HPP_
1.4.2 实现文件
/**
* @file simple_global_planner.cpp
* @brief 自定义 A* 全局规划器实现
*
* 该规划器利用 nav2_costmap_2d 代价地图进行启发式搜索。
* 代价地图中的障碍物值大于 LETHAL_OBSTACLE (254) 时视为不可通行。
* 搜索采用 8 邻域，允许对角线移动 (代价为 sqrt(2))。
*/
#include "amr_navigation/simple_global_planner.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include <queue>
#include <unordered_map>
#include <cmath>
namespace amr_navigation
{
// 网格索引哈希函数 (用于 unordered_map)
struct GridCell
{
unsigned int x, y;
bool operator==(const GridCell & other) const { return x == other.x && y == other.y; }
};
struct GridCellHasher
{
std::size_t operator()(const GridCell & c) const {
return static_cast<size_t>(c.x) << 32 | c.y;
}
};
struct AStarNode
{
GridCell cell;
double g; // 实际代价
double h; // 启发代价
double f() const { return g + h; }
GridCell parent;
};
struct CompareF
{
bool operator()(const AStarNode & a, const AStarNode & b) {
return a.f() > b.f();
}
};
void SimpleGlobalPlanner::configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name,
std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
node_ = parent.lock();
tf_ = tf;
costmap_ros_ = costmap_ros;
costmap_ = costmap_ros_->getCostmap();
global_frame_ = costmap_ros_->getGlobalFrameID();
// 参数声明
nav2_util::declare_parameter_if_not_declared(
node_, name + ".heuristic_weight", rclcpp::ParameterValue(1.0));
nav2_util::declare_parameter_if_not_declared(
node_, name + ".allow_unknown", rclcpp::ParameterValue(false));
node_->get_parameter(name + ".heuristic_weight", heuristic_weight_);
node_->get_parameter(name + ".allow_unknown", allow_unknown_);
RCLCPP_INFO(logger_, "Configured SimpleGlobalPlanner with h_weight=%.2f", heuristic_weight_);
}
void SimpleGlobalPlanner::activate() {}
void SimpleGlobalPlanner::deactivate() {}
void SimpleGlobalPlanner::cleanup() {}
nav_msgs::msg::Path SimpleGlobalPlanner::createPlan(
const geometry_msgs::msg::PoseStamped & start,
const geometry_msgs::msg::PoseStamped & goal)
{
nav_msgs::msg::Path path;
path.header.stamp = node_->now();
path.header.frame_id = global_frame_;
if (!costmap_) {
RCLCPP_ERROR(logger_, "Costmap not available");
return path;
}
unsigned int mx_start, my_start, mx_goal, my_goal;
if (!world_to_map(start.pose.position.x, start.pose.position.y, mx_start, my_start) ||
!world_to_map(goal.pose.position.x, goal.pose.position.y, mx_goal, my_goal)) {
RCLCPP_ERROR(logger_, "Start or goal out of map bounds");
return path;
}
if (!is_free(mx_goal, my_goal)) {
RCLCPP_ERROR(logger_, "Goal is occupied");
return path;
}
// A* 搜索
std::priority_queue<AStarNode, std::vector<AStarNode>, CompareF> open_set;
std::unordered_map<GridCell, double, GridCellHasher> g_costs;
std::unordered_map<GridCell, GridCell, GridCellHasher> parents;
AStarNode start_node;
start_node.cell = {mx_start, my_start};
start_node.g = 0;
start_node.h = heuristic_weight_ * std::hypot(
static_cast<double>(mx_goal) - mx_start,
static_cast<double>(my_goal) - my_start);
open_set.push(start_node);
g_costs[start_node.cell] = 0;
// 8邻域
const int dx[8] = {1, 0, -1, 0, 1, 1, -1, -1};
const int dy[8] = {0, 1, 0, -1, 1, -1, 1, -1};
const double move_cost[8] = {1.0, 1.0, 1.0, 1.0, 1.414, 1.414, 1.414, 1.414};
bool found = false;
while (!open_set.empty()) {
AStarNode current = open_set.top();
open_set.pop();
if (current.cell.x == mx_goal && current.cell.y == my_goal) {
found = true;
break;
}
for (int i = 0; i < 8; ++i) {
int nx = current.cell.x + dx[i];
int ny = current.cell.y + dy[i];
if (nx < 0 || ny < 0 || nx >= costmap_->getSizeInCellsX() || ny >= costmap_->getSizeInCellsY())
continue;
if (!is_free(nx, ny)) continue;
if (i >= 4 && (!is_free(current.cell.x + dx[i], current.cell.y) ||
!is_free(current.cell.x, current.cell.y + dy[i])))
continue; // 对角移动需保证两侧不碰撞
double new_g = current.g + move_cost[i];
GridCell neighbor{static_cast<unsigned int>(nx), static_cast<unsigned int>(ny)};
auto it = g_costs.find(neighbor);
if (it != g_costs.end() && new_g >= it->second) continue;
g_costs[neighbor] = new_g;
parents[neighbor] = current.cell;
AStarNode next;
next.cell = neighbor;
next.g = new_g;
next.h = heuristic_weight_ * std::hypot(
static_cast<double>(mx_goal) - nx,
static_cast<double>(my_goal) - ny);
next.parent = current.cell;
open_set.push(next);
}
}
if (!found) {
RCLCPP_WARN(logger_, "No valid path found");
return path;
}
// 回溯路径
std::vector<GridCell> raw_path;
GridCell cur = {mx_goal, my_goal};
while (!(cur.x == mx_start && cur.y == my_start)) {
raw_path.push_back(cur);
cur = parents[cur];
}
raw_path.push_back({mx_start, my_start});
std::reverse(raw_path.begin(), raw_path.end());
// 转换为 geometry_msgs/PoseStamped
for (const auto & cell : raw_path) {
double wx, wy;
map_to_world(cell.x, cell.y, wx, wy);
geometry_msgs::msg::PoseStamped pose;
pose.header.frame_id = global_frame_;
pose.header.stamp = node_->now();
pose.pose.position.x = wx;
pose.pose.position.y = wy;
pose.pose.orientation.w = 1.0; // 暂时无朝向
path.poses.push_back(pose);
}
return path;
}
void SimpleGlobalPlanner::map_to_world(unsigned int mx, unsigned int my, double & wx, double & wy)
{
wx = costmap_->getOriginX() + (mx + 0.5) * costmap_->getResolution();
wy = costmap_->getOriginY() + (my + 0.5) * costmap_->getResolution();
}
bool SimpleGlobalPlanner::world_to_map(double wx, double wy, unsigned int & mx, unsigned int & my)
{
if (!costmap_->worldToMap(wx, wy, mx, my)) {
return false;
}
return true;
}
bool SimpleGlobalPlanner::is_free(unsigned int mx, unsigned int my)
{
unsigned char cost = costmap_->getCost(mx, my);
if (cost >= nav2_costmap_2d::LETHAL_OBSTACLE)
return false;
if (cost == nav2_costmap_2d::NO_INFORMATION && !allow_unknown_)
return false;
return true;
}
} // namespace amr_navigation
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(amr_navigation::SimpleGlobalPlanner, nav2_core::GlobalPlanner)
1.5 控制器插件实现
实现一个基于 PID 的差速驱动机器人控制器，支持速度平滑和前向避障。
1.5.1 头文件
/**
* @file pid_controller.hpp
* @brief 纯追踪 + PID 角速度控制的简单控制器，实现 nav2_core::Controller 接口
*/
#ifndef AMR_NAVIGATION__PID_CONTROLLER_HPP_
#define AMR_NAVIGATION__PID_CONTROLLER_HPP_
#include <memory>
#include <string>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "nav2_core/controller.hpp"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "tf2_ros/buffer.h"
namespace amr_navigation
{
class PidController : public nav2_core::Controller
{
public:
PidController() = default;
~PidController() override = default;
void configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name,
std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
void activate() override;
void deactivate() override;
void cleanup() override;
/**
* @brief 计算速度指令
* @param robot_pose 机器人当前位姿
* @param robot_speed 当前速度
* @return 所需的 cmd_vel
*/
geometry_msgs::msg::TwistStamped computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & robot_pose,
const geometry_msgs::msg::Twist & robot_speed) override;
void setPlan(const nav_msgs::msg::Path & path) override;
void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
private:
rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
std::shared_ptr<tf2_ros::Buffer> tf_;
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
nav2_costmap_2d::Costmap2D * costmap_;
nav_msgs::msg::Path global_plan_;
geometry_msgs::msg::PoseStamped goal_pose_;
double kp_linear_, ki_linear_, kd_linear_;
double kp_angular_, ki_angular_, kd_angular_;
double angle_tolerance_; // 弧度，小于此值开始直线前进
double max_linear_speed_, max_angular_speed_;
double max_linear_accel_, max_angular_accel_;
rclcpp::Logger logger_{rclcpp::get_logger("PidController")};
};
} // namespace amr_navigation
#endif // AMR_NAVIGATION__PID_CONTROLLER_HPP_
1.5.2 实现文件
/**
* @file pid_controller.cpp
* @brief 差速 PID 控制器实现
*
* 使用纯追踪生成目标点 (lookahead point)，PID 控制线速度与角速度。
* 角度误差较大时优先原地旋转，以保证路径跟踪精度。
*/
#include "amr_navigation/pid_controller.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "nav2_costmap_2d/cost_values.hpp"
#include "tf2/utils.h"
#include <cmath>
#include <algorithm>
namespace amr_navigation
{
void PidController::configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name,
std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
node_ = parent.lock();
tf_ = tf;
costmap_ros_ = costmap_ros;
costmap_ = costmap_ros_->getCostmap();
nav2_util::declare_parameter_if_not_declared(node_, name + ".kp_linear", rclcpp::ParameterValue(1.0));
nav2_util::declare_parameter_if_not_declared(node_, name + ".ki_linear", rclcpp::ParameterValue(0.0));
nav2_util::declare_parameter_if_not_declared(node_, name + ".kd_linear", rclcpp::ParameterValue(0.0));
nav2_util::declare_parameter_if_not_declared(node_, name + ".kp_angular", rclcpp::ParameterValue(1.5));
nav2_util::declare_parameter_if_not_declared(node_, name + ".ki_angular", rclcpp::ParameterValue(0.0));
nav2_util::declare_parameter_if_not_declared(node_, name + ".kd_angular", rclcpp::ParameterValue(0.0));
nav2_util::declare_parameter_if_not_declared(node_, name + ".angle_tolerance", rclcpp::ParameterValue(0.2));
nav2_util::declare_parameter_if_not_declared(node_, name + ".max_linear_speed", rclcpp::ParameterValue(1.0));
nav2_util::declare_parameter_if_not_declared(node_, name + ".max_angular_speed", rclcpp::ParameterValue(1.5));
nav2_util::declare_parameter_if_not_declared(node_, name + ".max_linear_accel", rclcpp::ParameterValue(0.5));
nav2_util::declare_parameter_if_not_declared(node_, name + ".max_angular_accel", rclcpp::ParameterValue(0.8));
node_->get_parameter(name + ".kp_linear", kp_linear_);
node_->get_parameter(name + ".ki_linear", ki_linear_);
node_->get_parameter(name + ".kd_linear", kd_linear_);
node_->get_parameter(name + ".kp_angular", kp_angular_);
node_->get_parameter(name + ".ki_angular", ki_angular_);
node_->get_parameter(name + ".kd_angular", kd_angular_);
node_->get_parameter(name + ".angle_tolerance", angle_tolerance_);
node_->get_parameter(name + ".max_linear_speed", max_linear_speed_);
node_->get_parameter(name + ".max_angular_speed", max_angular_speed_);
node_->get_parameter(name + ".max_linear_accel", max_linear_accel_);
node_->get_parameter(name + ".max_angular_accel", max_angular_accel_);
RCLCPP_INFO(logger_, "PidController configured");
}
void PidController::activate() {}
void PidController::deactivate() {}
void PidController::cleanup() {}
void PidController::setPlan(const nav_msgs::msg::Path & path)
{
global_plan_ = path;
if (!path.poses.empty()) {
goal_pose_ = path.poses.back();
}
}
void PidController::setSpeedLimit(const double & speed_limit, const bool & percentage)
{
// 简单实现：若百分比为true，则按比例缩放最大速度
if (percentage) {
max_linear_speed_ *= speed_limit / 100.0;
max_angular_speed_ *= speed_limit / 100.0;
} else {
max_linear_speed_ = speed_limit;
}
}
geometry_msgs::msg::TwistStamped PidController::computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & robot_pose,
const geometry_msgs::msg::Twist & robot_speed)
{
geometry_msgs::msg::TwistStamped cmd;
cmd.header.stamp = node_->now();
cmd.header.frame_id = robot_pose.header.frame_id;
if (global_plan_.poses.empty()) {
return cmd;
}
// 1. 查找路径上最近的点，以及前方 lookahead 点
double robot_x = robot_pose.pose.position.x;
double robot_y = robot_pose.pose.position.y;
double robot_yaw = tf2::getYaw(robot_pose.pose.orientation);
int closest_idx = 0;
double min_dist = std::numeric_limits<double>::max();
for (size_t i = 0; i < global_plan_.poses.size(); ++i) {
double dx = global_plan_.poses[i].pose.position.x - robot_x;
double dy = global_plan_.poses[i].pose.position.y - robot_y;
double dist = dx*dx + dy*dy;
if (dist < min_dist) {
min_dist = dist;
closest_idx = static_cast<int>(i);
}
}
// lookahead 距离 (动态取决于速度)
double lookahead = 0.3 + robot_speed.linear.x * 0.5;
int target_idx = closest_idx;
double accum = 0.0;
for (size_t i = closest_idx; i < global_plan_.poses.size() - 1; ++i) {
double dx = global_plan_.poses[i+1].pose.position.x - global_plan_.poses[i].pose.position.x;
double dy = global_plan_.poses[i+1].pose.position.y - global_plan_.poses[i].pose.position.y;
accum += std::hypot(dx, dy);
if (accum >= lookahead) {
target_idx = static_cast<int>(i + 1);
break;
}
}
if (target_idx >= static_cast<int>(global_plan_.poses.size()))
target_idx = global_plan_.poses.size() - 1;
geometry_msgs::msg::Pose target_pose = global_plan_.poses[target_idx].pose;
// 2. 计算目标点与机器人当前朝向的偏差
double dx = target_pose.position.x - robot_x;
double dy = target_pose.position.y - robot_y;
double target_yaw = std::atan2(dy, dx);
double yaw_error = target_yaw - robot_yaw;
// 归一化到 [-pi, pi]
while (yaw_error > M_PI) yaw_error -= 2 * M_PI;
while (yaw_error < -M_PI) yaw_error += 2 * M_PI;
double dist_to_target = std::hypot(dx, dy);
// 3. 控制律
double linear = 0.0, angular = 0.0;
if (std::fabs(yaw_error) > angle_tolerance_) {
// 优先旋转
angular = kp_angular_ * yaw_error;
linear = 0.0;
} else {
// 朝向大致正确，前进并修正
linear = kp_linear_ * dist_to_target;
angular = kp_angular_ * yaw_error;
}
// 限幅
linear = std::clamp(linear, -max_linear_speed_, max_linear_speed_);
angular = std::clamp(angular, -max_angular_speed_, max_angular_speed_);
cmd.twist.linear.x = linear;
cmd.twist.angular.z = angular;
return cmd;
}
} // namespace amr_navigation
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(amr_navigation::PidController, nav2_core::Controller)
1.6 行为树配置
使用 BehaviorTree.CPP 描述导航任务。下面是一个简化的 navigate_to_pose.xml，它替代 Nav2 默认行为树，展示了如何集成自定义插件。
<?xml version="1.0" encoding="UTF-8"?>
<!--
Custom behavior tree for autonomous navigation.
Includes recovery on failure and fallback.
-->
<root BTCPP_format="4">
<BehaviorTree ID="NavigateToPose">
<PipelineSequence name="NavigateWithRecovery">
<RateController hz="10.0">
<RecoveryNode number_of_retries="2">
<SequenceStar name="NavigateTasks">
<ConditionNode name="isPathValid"
plugin="nav2_bt_condition::IsPathValid"
path="{path}" goal="{goal}"/>
<ActionNode name="ComputePath"
plugin="nav2_bt_action::ComputePathToPose"
goal="{goal}" path="{path}"/>
<ActionNode name="FollowPath"
plugin="nav2_bt_action::FollowPath"
path="{path}" controller_id="FollowPath"/>
</SequenceStar>
<Fallback name="RecoveryFallback">
<ActionNode name="Spin"
plugin="nav2_bt_action::Spin"
spin_dist="1.57"/>
<ActionNode name="BackUp"
plugin="nav2_bt_action::BackUp"
backup_dist="0.15"/>
</Fallback>
</RecoveryNode>
</RateController>
</PipelineSequence>
</BehaviorTree>
</root>
此行为树在 ComputePath 失败或 FollowPath 卡住时，会执行恢复动作（原地旋转、后退），并重新规划路径。
1.7 参数配置
1.7.1 全局规划器参数 (在 nav2_params.yaml 中)
controller_server: ros__parameters: controller_frequency: 20.0 FollowPath: plugin: "amr_navigation::PidController" kp_linear: 1.0 ki_linear: 0.0 kd_linear: 0.0 kp_angular: 1.5 ki_angular: 0.0 kd_angular: 0.0 angle_tolerance: 0.2 max_linear_speed: 1.0 max_angular_speed: 1.5 max_linear_accel: 0.5 max_angular_accel: 0.8 planner_server: ros__parameters: planner_plugins: ["GridBased"] GridBased: plugin: "amr_navigation::SimpleGlobalPlanner" heuristic_weight: 1.0 allow_unknown: false
1.7.2 代价地图参数
global_costmap_params.yaml:
global_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: map robot_base_frame: base_link resolution: 0.05 track_unknown_space: false footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]" plugins: ["static_layer", "inflation_layer"] static_layer: plugin: "nav2_costmap_2d::StaticLayer" inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" inflation_radius: 0.5
local_costmap_params.yaml:
local_costmap: ros__parameters: update_frequency: 20.0 publish_frequency: 10.0 global_frame: odom robot_base_frame: base_link rolling_window: true width: 6 height: 6 resolution: 0.05 footprint: "[[-0.2, -0.2], [-0.2, 0.2], [0.2, 0.2], [0.2, -0.2]]" plugins: ["obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" inflation_radius: 0.3
1.8 插件描述文件
plugins/plugins.xml:
<class_libraries> <library path="simple_global_planner"> <class type="amr_navigation::SimpleGlobalPlanner" base_class_type="nav2_core::GlobalPlanner"> <description>Custom A* global planner</description> </class> </library> <library path="pid_controller"> <class type="amr_navigation::PidController" base_class_type="nav2_core::Controller"> <description>Custom PID controller for differential drive</description> </class> </library> </class_libraries>
1.9 逻辑思维走向树形分析（导航行为决策）
接收到 /navigate_to_pose 动作 ├── 行为树启动 │ ├── 检查路径是否有效 (IsPathValid) │ │ ├── 无效 → 重新计算路径 (ComputePath) │ │ └── 有效 → 继续 │ ├── 调用 ComputePath (全局规划) │ │ ├── A* 搜索 │ │ │ ├── 起点/目标在自由空间 → 搜索 │ │ │ └── 障碍物阻挡 → 返回失败 → 行为树进入恢复 │ │ └── 发布 /plan │ ├── 调用 FollowPath (局部控制) │ │ ├── 根据 /plan 和 /odom 生成 cmd_vel │ │ ├── 检测碰撞风险 │ │ │ ├── 安全 → 继续 │ │ │ └── 风险 → 降低速度或停止，可能触发重新规划 │ │ └── 到达目标 → 成功 │ └── 失败时执行恢复 │ ├── Spin (原地旋转) │ ├── BackUp (后退) │ └── 重试规划 └── 动作完成
1.10 单位数据规划与缓存区规划
-
代价地图分辨率：0.05 m/cell，A* 搜索空间由地图大小决定（如 200×200 网格）。
-
路径存储：
nav_msgs::Path容量按网格点数，约几千个位姿。 -
PID 积分缓存：每个控制周期保存误差积分 (I 项)，需限制积分上限防止 windup。
-
速度平滑：通过加速度限制每次输出增量，内部维护前一周期速度值。
1.11 可能遇到的问题与解决方案
-
A* 搜索时间过长
-
原因：地图大，启发权重低或障碍物密集。
-
解决：增加启发权重
heuristic_weight（如 1.5），使用Smac Planner替代简单 A*，或减小 costmap 分辨率。
-
-
控制器振荡
-
原因：PID 增益过高、角度容差太小。
-
解决：调低
kp_angular，增大angle_tolerance。
-
-
机器人原地旋转不止
-
原因：目标点位于机器人后方，路径回溯导致角度误差永远无法收敛。
-
解决：在
computeVelocityCommands中加入前向点投影逻辑，或切换为局部路径规划器（DWB）。
-
-
行为树恢复无效
-
原因：恢复动作设计不足，或者代价地图未更新。
-
解决：确保
ObstacleLayer能实时更新扫描数据，增加恢复动作种类（如 ClearCostmap）。
-
-
插件加载失败
-
原因：
plugins.xml路径未在package.xml中导出。 -
解决：在
package.xml中添加<exec_depend>pluginlib</exec_depend>，并在CMakeLists.txt中通过pluginlib_export_plugin_description_file(nav2_core plugins.xml)导出。
-
欢迎加入 MCP 技术社区！与志同道合者携手前行，一同解锁 MCP 技术的无限可能！
更多推荐
6
0- 0
已为社区贡献4条内容
所有评论(0)