ROS1 导航栈与路径规划实战

1.3 导航栈安装

# 安装导航栈
sudo apt-get install ros-noetic-navigation
# 安装额外的规划器
sudo apt-get install ros-noetic-teb-local-planner
sudo apt-get install ros-noetic-dwa-local-planner
# 安装可视化工具
sudo apt-get install ros-noetic-rviz-plugin-tutorials

2. move_base 框架

2.1 move_base 架构

#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" move_base 节点接口 """
import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped, Twist

class MoveBaseInterface:
    """move_base 接口封装"""
    def __init__(self):
        rospy.init_node('move_base_interface')
        self.move_base_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
        self.move_base_client.wait_for_server()
        self.goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=1)

    def send_goal(self, x, y, theta):
        """发送导航目标"""
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = 'map'
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose.position.x = x
        goal.target_pose.pose.position.y = y
        goal.target_pose.pose.position.z = 0.0
        # 设置目标姿态（四元数）
        import tf.transformations as tf_trans
        q = tf_trans.quaternion_from_euler(0, 0, theta)
        goal.target_pose.pose.orientation.x = q[0]
        goal.target_pose.pose.orientation.y = q[1]
        goal.target_pose.pose.orientation.z = q[2]
        goal.target_pose.pose.orientation.w = q[3]
        self.move_base_client.send_goal(goal)
        return goal

2.2 move_base 配置

# move_base_params.yaml
shutdown_costmaps: false
controller_frequency: 5.0
controller_patience: 3.0
planner_frequency: 1.0
planner_patience: 5.0
oscillation_timeout: 10.0
oscillation_distance: 0.2
base_global_planner: "navfn/NavfnROS"
base_local_planner: "dwa_local_planner/DWAPlannerROS"
recovery_behavior_enabled: true
clearing_rotation_allowed: true
recovery_behaviors:
  - name: 'conservative_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'
  - name: 'rotate_recovery'
    type: 'rotate_recovery/RotateRecovery'
  - name: 'aggressive_reset'
    type: 'clear_costmap_recovery/ClearCostmapRecovery'

3. 代价地图（Costmap）

3.1 代价地图层

#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" 代价地图配置和管理 """
import numpy as np
import rospy
from nav_msgs.msg import OccupancyGrid

class CostmapManager:
    COST_UNKNOWN = 255
    COST_LETHAL = 254
    COST_INSCRIBED = 253
    COST_FREE = 0

    def __init__(self):
        self.layers = {
            'static_layer': {'enabled': True, 'type': 'costmap_2d::StaticLayer'},
            'obstacle_layer': {'enabled': True, 'type': 'costmap_2d::ObstacleLayer'},
            'inflation_layer': {'enabled': True, 'type': 'costmap_2d::InflationLayer'}
        }

3.2 代价地图配置文件

# costmap_common_params.yaml
footprint: [[-0.25,-0.15],[-0.25,0.15],[0.25,0.15],[0.25,-0.15]]
footprint_padding: 0.01
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 2.0
transform_tolerance: 0.5
resolution: 0.05
obstacle_range: 2.5
raytrace_range: 3.0
obstacle_layer:
  enabled: true
  observation_sources: laser_scan_sensor
  laser_scan_sensor:
    sensor_frame: laser_link
    data_type: LaserScan
    topic: /scan
    marking: true
    clearing: true
inflation_layer:
  enabled: true
  inflation_radius: 0.55
  cost_scaling_factor: 10.0
static_layer:
  enabled: true
  map_topic: /map

4. 全局路径规划

4.1 全局规划器实现

#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" 全局路径规划器 """
import heapq
import numpy as np

class AStarPlanner:
    def __init__(self):
        self.cost_threshold = 252

    def heuristic(self, a, b):
        return np.sqrt((a[0]-b[0])**2 + (a[1]-b[1])**2)

    def plan(self, start, goal):
        if self.costmap is None:
            return None
        start_map = self.world_to_map(start[0], start[1])
        goal_map = self.world_to_map(goal[0], goal[1])
        open_set = [(0, start_map)]
        came_from = {}
        g_score = {start_map: 0}
        f_score = {start_map: self.heuristic(start_map, goal_map)}
        while open_set:
            current = heapq.heappop(open_set)[1]
            if current == goal_map:
                path = []
                while current in came_from:
                    wx, wy = self.map_to_world(current[0], current[1])
                    path.append((wx, wy))
                    current = came_from[current]
                path.reverse()
                return path
            for neighbor, move_cost in self.get_neighbors(current):
                tentative_g_score = g_score[current] + move_cost
                if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
                    came_from[neighbor] = current
                    g_score[neighbor] = tentative_g_score
                    f_score[neighbor] = tentative_g_score + self.heuristic(neighbor, goal_map)
                    heapq.heappush(open_set, (f_score[neighbor], neighbor))
        return None

4.2 全局规划器配置

# global_planner_params.yaml
NavfnROS:
  visualize_potential: false
  allow_unknown: true
  use_dijkstra: true
GlobalPlanner:
  old_navfn_behavior: false
  use_quadratic: true
  use_dijkstra: true
  lethal_cost: 253
  neutral_cost: 50
  cost_factor: 3.0

5. 局部路径规划

5.1 DWA 局部规划器

#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" DWA（动态窗口法）局部规划器 """
import numpy as np
from geometry_msgs.msg import Twist

class DWAPlanner:
    def __init__(self):
        self.max_vel_x = 0.5
        self.min_vel_x = -0.1
        self.max_vel_theta = 1.0
        self.min_vel_theta = -1.0
        self.acc_lim_x = 0.5
        self.acc_lim_theta = 1.0
        self.dt = 0.1
        self.predict_time = 3.0
        self.velocity_samples_x = 20
        self.velocity_samples_theta = 40
        self.path_distance_bias = 32.0
        self.goal_distance_bias = 24.0
        self.occdist_scale = 0.01

    def dynamic_window(self, current_vel):
        vs = [self.min_vel_x, self.max_vel_x, self.min_vel_theta, self.max_vel_theta]
        vd = [current_vel[0] - self.acc_lim_x * self.dt, current_vel[0] + self.acc_lim_x * self.dt,
              current_vel[1] - self.acc_lim_theta * self.dt, current_vel[1] + self.acc_lim_theta * self.dt]
        dw = [max(vs[0], vd[0]), min(vs[1], vd[1]), max(vs[2], vd[2]), min(vs[3], vd[3])]
        return dw

5.2 局部规划器配置

# dwa_local_planner_params.yaml
DWAPlannerROS:
  max_vel_x: 0.5
  min_vel_x: 0.0
  max_vel_theta: 1.0
  acc_lim_x: 2.5
  acc_lim_theta: 3.2
  xy_goal_tolerance: 0.2
  yaw_goal_tolerance: 0.1
  sim_time: 1.7
  vx_samples: 20
  vtheta_samples: 40
  path_distance_bias: 32.0
  goal_distance_bias: 24.0
  occdist_scale: 0.01

6. 恢复行为

6.1 恢复行为实现

#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" 导航恢复行为 """
import rospy
from geometry_msgs.msg import Twist
from std_srvs.srv import Empty

class RecoveryBehaviors:
    def rotate_recovery(self, angle=3.14, speed=0.5):
        """旋转恢复"""
        rospy.loginfo("Executing rotate recovery...")
        duration = abs(angle / speed)
        cmd = Twist()
        cmd.angular.z = speed if angle > 0 else -speed
        start_time = rospy.Time.now()
        rate = rospy.Rate(10)
        while (rospy.Time.now() - start_time).to_sec() < duration:
            self.cmd_vel_pub.publish(cmd)
            rate.sleep()
        self.cmd_vel_pub.publish(Twist())
        return True

    def clear_costmap_recovery(self):
        """清除代价地图恢复"""
        try:
            self.clear_costmap_srv()
            return True
        except Exception as e:
            return False

7. 导航配置与调优

7.1 导航参数调优

#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" 导航参数自动调优 """
import dynamic_reconfigure.client

class NavigationTuner:
    def __init__(self):
        self.dwa_client = dynamic_reconfigure.client.Client("/move_base/DWAPlannerROS")
        self.costmap_client = dynamic_reconfigure.client.Client("/move_base/global_costmap/inflation_layer")

    def evaluate_performance(self, test_goals):
        results = []
        for goal in test_goals:
            success = self.send_goal_and_wait(goal)
            results.append({'success': success})
        return results

    def grid_search(self, param_names, test_goals, resolution=3):
        best_params = {}
        best_score = -float('inf')
        param_grid = self.generate_param_grid(param_names, resolution)
        for params in param_grid:
            self.apply_params(params)
            metrics = self.evaluate_performance(test_goals)
            score = self.calculate_score(metrics)
            if score > best_score:
                best_score = score
                best_params = params.copy()
        return best_params, best_score

8. 导航目标发送

8.1 多目标导航

#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" 多目标导航管理 """
import yaml
from visualization_msgs.msg import MarkerArray

class MultiGoalNavigation:
    def __init__(self):
        self.goals = []
        self.current_goal_index = 0
        self.patrol_mode = False

    def load_goals_from_file(self, filename='goals.yaml'):
        try:
            with open(filename, 'r') as f:
                data = yaml.safe_load(f)
                for goal_data in data['goals']:
                    goal = PoseStamped()
                    goal.header.frame_id = 'map'
                    goal.pose.position.x = goal_data['x']
                    goal.pose.position.y = goal_data['y']
                    self.goals.append(goal)
        except Exception as e:
            rospy.logwarn(f"Failed to load goals: {e}")

    def start_navigation(self):
        if not self.goals:
            return
        self.current_goal_index = 0
        self.send_next_goal()

9. 多机器人导航

9.1 多机器人协调

#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" 多机器人导航协调 """
import threading

class MultiRobotCoordinator:
    def __init__(self, robot_names):
        self.robots = {}
        for name in robot_names:
            self.robots[name] = {'namespace': name, 'status': 'idle', 'priority': 0}

    def detect_and_resolve_conflicts(self):
        robot_names = list(self.robots.keys())
        for i in range(len(robot_names)):
            for j in range(i + 1, len(robot_names)):
                robot1 = robot_names[i]
                robot2 = robot_names[j]
                if self.check_path_conflict(robot1, robot2):
                    self.resolve_conflict(robot1, robot2)

    def resolve_conflict(self, robot1, robot2):
        if self.robots[robot1]['priority'] > self.robots[robot2]['priority']:
            self.stop_robot(robot2)
        elif self.robots[robot2]['priority'] > self.robots[robot1]['priority']:
            self.stop_robot(robot1)

10. 实战案例：完整导航系统

10.1 完整导航系统实现

#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" 完整的自主导航系统 """
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

class AutonomousNavigationSystem:
    def __init__(self):
        self.state = 'idle'
        self.setup_localization()
        self.setup_navigation()
        self.setup_sensors()
        self.setup_recovery()

    def navigate_to_goal(self, goal_pose):
        if not self.system_check():
            return False
        goal = MoveBaseGoal()
        goal.target_pose.header.frame_id = 'map'
        goal.target_pose.header.stamp = rospy.Time.now()
        goal.target_pose.pose = goal_pose
        self.move_base_client.send_goal(goal)
        self.state = 'navigating'
        return True

    def execute_recovery(self):
        try:
            self.clear_costmaps_srv()
        except:
            pass

11. 总结与最佳实践

11.1 本文总结

通过本文的学习，你已经掌握了：

1. ROS 导航栈架构：理解导航系统的整体结构
2. move_base 框架：掌握导航核心节点的使用
3. 代价地图机制：配置和优化代价地图
4. 全局路径规划：实现 A*、Dijkstra 等算法
5. 局部路径规划：掌握 DWA 等局部规划器
6. 恢复行为：处理导航失败情况
7. 参数调优：优化导航性能
8. 多目标导航：管理多个导航目标
9. 多机器人协调：实现多机器人导航
10. 完整系统：构建完整的导航系统

11.2 最佳实践

11.3 常见问题解决

1. 机器人原地打转：检查代价地图配置，调整振荡参数。
2. 无法到达目标：检查目标是否在障碍物中，调整目标容差。
3. 路径规划失败：确认地图完整性，检查起点和终点。
4. 导航速度过慢：增加最大速度限制，优化加速度参数。