空间A*路径规划实战)
1. 项目背景与核心目标最近在做一个四足机器人自主导航相关的项目用到了宇树科技的Go2 Edu机器人。在调试过程中我发现一个挺有意思的需求如何让机器人在一个已知的二维地图上规划出一条能高效覆盖所有可通行区域的路径这不仅仅是“从A点走到B点”那么简单而是要让机器人像扫地机器人一样系统地“走遍”地图的每一个角落比如用于环境巡检、区域搜索等任务。传统的全局路径规划算法比如A*在解决“点到点”问题上非常出色但它通常只考虑位置x, y也就是SE(2)空间中的位置分量。然而对于像Go2这样的机器人其朝向yaw角同样至关重要。尤其是在狭窄的走廊或需要执行特定朝向扫描任务时规划出一条同时考虑位置和朝向的路径能显著提升任务执行的效率和成功率。这就是SE(2)空间路径规划的核心——在规划路径时将机器人的姿态x, y, θ作为一个整体来考虑。因此这个demo项目的目标就很明确了基于宇树Go2 Edu机器人及其官方tide-sdk实现一个在SE(2)空间即包含位置和朝向上进行A*路径规划的示例。我们不仅要让机器人到达目标点还要让它以我们期望的“姿态”到达比如在巡检点时正面朝向待检设备。2. 为什么是SE(2)上的A*—— 从SE(2)空间说起在深入代码之前有必要先聊聊SE(2)空间和为什么在这里A*算法需要一些“特别处理”。SE(2)空间即二维特殊欧几里得群简单来说就是平面上的刚体运动空间。一个SE(2)位姿包含三个自由度平面坐标 (x, y) 和绕垂直轴的旋转角 θ偏航角。对于地面移动机器人这正是其完整的平面运动状态。那么为什么在SE(2)空间上直接用A*会变得复杂状态空间爆炸如果我们在一个100x100的网格地图上进行规划纯位置搜索的状态数是10,000个。如果我们把朝向θ也离散化例如分成36个方向每10度一个那么SE(2)空间的状态数瞬间就变成了360,000个。搜索空间呈数量级增长。运动学约束机器人不是质点它不能瞬间改变朝向。从位姿A (x1, y1, θ1) 到位姿B (x2, y2, θ2)需要考虑机器人的转弯半径、最大角速度等约束。简单的8邻域或16邻域移动模型不再适用。代价函数的定义在SE(2)空间中从一位姿移动到另一位姿的代价不仅要考虑欧氏距离还要考虑转向的代价。一个微小的位置移动如果伴随一个180度的大转向其实际“代价”可能比一个稍长的直线移动还要高。A*算法在SE(2)上的变体就是为了解决这些问题。核心思路是在状态扩展时不再仅仅是向相邻的网格移动而是生成一系列符合机器人运动学模型的“运动基元”。每个运动基元代表机器人在一个控制周期内执行特定线速度和角速度后所能到达的下一个SE(2)状态。这样搜索图就从一个静态的网格变成了一个在状态空间中由运动基元连接而成的图。在本次的demo实现中为了简化并突出核心概念我们采用了一种分层规划的策略第一层SE(2)中的位置规划我们仍然使用标准的2D A*算法在地图网格上找到一条从起点到终点的无碰撞位置路径。这一步只关心(x, y)。第二层朝向平滑在得到位置路径后我们再为路径上的每个点分配合适的朝向θ。一个简单有效的方法是使用“前向朝向”或“基于路径切线的朝向”让机器人的朝向尽可能与其运动方向一致减少不必要的原地旋转。这种方法虽然不是严格意义上的在连续SE(2)状态空间中进行搜索但对于许多实际应用尤其是走廊、房间等结构化环境来说已经能极大地改善机器人的最终停靠姿态和移动流畅性是实现完全SE(2)规划的一个很好的起点。3. 环境搭建与tide-sdk初探宇树Go2 Edu的软件开发主要围绕其官方SDK进行。tide-sdk是宇树提供的核心开发工具包它基于ROS 2推荐Foxy或Humble和DDS通信中间件提供了对机器人底层状态关节、IMU、里程计和控制运动、姿态的全面接口。3.1 基础环境配置首先你需要一个运行Ubuntu 20.04或22.04的系统物理机或虚拟机均可。Go2 Edu的扩展坞内置了NVIDIA Jetson Orin可以直接作为开发主机。安装ROS 2按照ROS 2官方教程安装Foxy或Humble版本。这里以Foxy为例因为早期tide-sdk文档多基于此版本。# 设置locale sudo apt update sudo apt install locales sudo locale-gen en_US en_US.UTF-8 sudo update-locale LC_ALLen_US.UTF-8 LANGen_US.UTF-8 export LANGen_US.UTF-8 # 添加ROS 2仓库 sudo apt install software-properties-common sudo add-apt-repository universe sudo apt update sudo apt install curl -y sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg echo deb [arch$(dpkg --print-architecture) signed-by/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release echo $UBUNTU_CODENAME) main | sudo tee /etc/apt/sources.list.d/ros2.list /dev/null # 安装ROS 2桌面版 sudo apt update sudo apt install ros-foxy-desktop python3-argcomplete安装tide-sdk前往宇树官方支持网站下载对应你机器人软件版本的tide-sdk。通常是一个.deb安装包。sudo dpkg -i unitree_tide_sdk_version_amd64.deb安装后SDK的头文件和库文件会置于/opt/unitree/tide-sdk目录下。务必按照官方指南设置环境变量通常是source一个setup脚本。创建工作空间与依赖创建一个ROS 2工作空间用于存放我们的demo代码。mkdir -p ~/go2_se2_planning_ws/src cd ~/go2_se2_planning_ws/src我们需要一些额外的ROS 2包特别是用于导航和TF变换的。# 安装Nav2导航框架和TF2 sudo apt install ros-foxy-nav2-bringup ros-foxy-turtlebot3* ros-foxy-slam-toolbox3.2 tide-sdk核心模块概览tide-sdk通过ROS 2话题和服务暴露了机器人的主要功能。对于我们这个demo需要关注以下几个关键接口状态订阅/sportmodestate(unitree_go/msg/SportModeState)这是最重要的消息之一包含了机器人的里程计信息位置、姿态、IMU数据、关节状态等。我们将从中提取机器人的当前SE(2)位姿。/lidar/pointcloud(sensor_msgs/msg/PointCloud2)来自机器人头部L1激光雷达的点云数据用于建图和障碍物感知。控制发布/cmd_vel(geometry_msgs/msg/Twist)这是ROS中标准的控制接口。发布线速度和角速度指令可以控制机器人移动。tide-sdk会将其转换到底层电机指令。注意Go2也可能使用自定义控制话题需要查阅最新版本文档确认。有时需要将geometry_msgs/Twist通过一个转换节点发布到/wirelesscontroller这类话题。服务调用一些高级动作如“站立”、“坐下”、“趴下”需要通过特定的服务或Action来调用。tide-sdk提供了相应的接口。实操心得在第一次连接机器人时最容易出错的是网络配置。确保你的开发机与机器人在同一局域网并且正确设置了ROS_DOMAIN_ID如果网络中有多个ROS 2系统。建议先用ros2 topic list命令查看是否能收到来自机器人话题通常有/unitree或/go2等前缀的话题这是验证通信是否成功的第一步。4. SE(2) A* 路径规划器的实现我们的规划器将作为一个独立的ROS 2节点运行。它主要完成以下功能订阅地图和机器人位姿接收目标位姿运行SE(2) A*规划算法并将生成的路径以nav_msgs/Path消息形式发布出去供后续的路径跟踪控制器使用。4.1 节点结构与数据流我们创建一个名为se2_a_star_planner的ROS 2节点。其内部数据流如下图所示概念示意[订阅] /map (nav_msgs/OccupancyGrid) [订阅] /amcl_pose (geometry_msgs/PoseWithCovarianceStamped) 或 /odom [订阅] /goal_pose (geometry_msgs/PoseStamped) | V [SE(2) A* 规划器核心] | V [发布] /global_plan (nav_msgs/Path) [发布] /planned_path_vis (visualization_msgs/MarkerArray) (用于Rviz可视化)4.2 核心算法步骤详解以下是规划器核心函数planSE2()的伪代码和关键实现细节# 伪代码SE(2) A* 规划核心 def plan_SE2(start_pose, goal_pose, costmap): start_pose, goal_pose: 类型为 (x, y, theta) costmap: 2D占据栅格地图每个单元格有代价0-100-1未知 # 1. 离散化配置空间 xy_resolution costmap.resolution # 地图分辨率如0.05米/像素 yaw_resolution math.radians(15) # 朝向离散化分辨率如15度 # 将连续SE(2)状态离散化为网格索引 (x_idx, y_idx, theta_idx) start_node discretize_pose(start_pose, xy_resolution, yaw_resolution) goal_node discretize_pose(goal_pose, xy_resolution, yaw_resolution) # 2. 初始化开放集和关闭集 open_set PriorityQueue() open_set.put(start_node, heuristic(start_node, goal_node)) came_from {} # 记录父节点 g_score {start_node: 0} # 从起点到当前节点的实际代价 f_score {start_node: heuristic(start_node, goal_node)} # 估计总代价 # 3. 定义运动基元Motion Primitives # 这里我们定义一组简单的动作前进一段距离同时可以保持原朝向或进行小幅转向 motion_primitives [ (dx, dy, dtheta, cost) for ... in ... # 例如(0.2, 0, 0, 0.2), (0.141, 0.141, pi/4, 0.22)... ] # 4. A* 主循环 while not open_set.empty(): current open_set.get() if current goal_node or close_enough(current, goal_node): return reconstruct_path(came_from, current) # 生成邻居节点 for motion in motion_primitives: dx, dy, dtheta, motion_cost motion # 应用运动基元计算下一个离散状态 next_pose_cont apply_motion(current_pose_cont, dx, dy, dtheta) next_node discretize_pose(next_pose_cont, xy_resolution, yaw_resolution) # 检查下一状态是否碰撞需要将机器人轮廓投影到costmap上判断 if not is_state_valid(next_node, costmap, robot_radius): continue # 计算临时g_score tentative_g_score g_score[current] motion_cost turning_cost(dtheta) if next_node not in g_score or tentative_g_score g_score[next_node]: # 这是一个更好的路径 came_from[next_node] current g_score[next_node] tentative_g_score f_score[next_node] tentative_g_score heuristic(next_node, goal_node) if next_node not in open_set: open_set.put(next_node, f_score[next_node]) return None # 规划失败关键点解析与实现细节离散化与状态表示我们使用一个三维整数元组(x_idx, y_idx, theta_idx)来表示一个离散的SE(2)状态。x_idx, y_idx由地图坐标除以分辨率得到theta_idx由角度除以朝向分辨率得到。这本质上创建了一个3D网格状态空间。启发式函数heuristic这是A*性能的关键。一个在SE(2)空间有效的启发式函数需要同时考虑位置和朝向的差异。一个常用的方法是def heuristic(node, goal): # 欧几里得距离代价 dx (goal.x_idx - node.x_idx) * xy_resolution dy (goal.y_idx - node.y_idx) * xy_resolution distance_cost math.sqrt(dx*dx dy*dy) # 朝向差异代价归一化到[0, pi] dtheta abs(angle_diff(node.theta, goal.theta)) # angle_diff 处理角度环绕 orientation_cost dtheta * orientation_weight # orientation_weight是一个调节参数 return distance_cost orientation_cost通过调节orientation_weight你可以控制规划器在路径长度和最终朝向准确性之间的权衡。运动基元生成这是SE(2)规划区别于2D规划的核心。我们不能简单地移动到相邻的8个网格。我们需要生成一组短时间如0.5秒内机器人可执行的动作轨迹终点。例如以0.4米/秒的速度直行0.2米dx0.2, dy0, dtheta0。以0.3米/秒的速度和0.5弧度/秒的角速度弧线运动0.2秒计算终点位姿。 这些基元可以预先计算好存储在列表中。基元的数量和复杂度直接影响规划的精度和速度。碰撞检测is_state_valid在SE(2)空间中碰撞检测需要考虑机器人的轮廓和朝向。我们需要将机器人 footprint通常是一个矩形或多边形根据当前位姿(x, y, theta)进行旋转和平移然后检查这个footprint覆盖的所有栅格在costmap上是否都是“自由空间”。这是一个计算密集型操作需要优化例如使用bresenham画线算法快速检查边界。路径重建与插值A*找到的路径是由离散状态节点组成的。我们需要将其转换回连续的SE(2)位姿序列。此外由于我们使用了运动基元相邻状态间的运动本身就是平滑的。但为了给控制器一个更密集的参考路径可以在相邻位姿间进行线性插值对x, y和球面线性插值对四元数表示的朝向。4.3 与Nav2框架的集成虽然我们实现了自己的全局规划器但更好的方式是将其作为一个nav2_core::GlobalPlanner插件集成到Nav2框架中。这样可以利用Nav2已有的生命周期管理、参数配置和与控制器、恢复行为的协同。创建插件类继承nav2_core::GlobalPlanner实现configure,activate,deactivate,cleanup,createPlan这几个虚函数。实现createPlan方法在这个方法中调用我们上面实现的planSE2函数。输入是nav_msgs::msg::OccupancyGrid格式的地图和geometry_msgs::msg::PoseStamped格式的起点、终点输出是nav_msgs::msg::Path。注册插件在插件的源码文件中使用PLUGINLIB_EXPORT_CLASS宏注册并在package.xml中导出插件。Nav2配置在Nav2的BT行为树XML配置文件中将全局规划器设置为我们的插件名。这样做的好处是我们的规划器可以无缝替换Nav2默认的nav2_navfn_planner/NavfnPlanner或nav2_theta_star_planner/ThetaStarPlanner并享受Nav2提供的所有基础设施比如与nav2_amcl定位模块、nav2_dwb_controller控制器的配合。避坑指南在实现碰撞检测时一个常见的错误是只检查机器人中心点所在的栅格。对于Go2这样有一定尺寸的机器人这会导致规划出的路径紧贴障碍物甚至发生碰撞。务必使用机器人的完整footprint进行碰撞检查。你可以通过URDF文件获取机器人的轮廓尺寸或者保守地使用一个包围圆半径。在costmap中通常将障碍物进行膨胀inflation规划时只要检查机器人中心点是否在膨胀后的障碍物区域外即可这是一种高效的近似方法。5. 在Unitree Go2上的部署与测试算法实现后下一步就是让它在真实的Go2机器人上跑起来。5.1 启动与配置流程启动机器人底层驱动首先通过tide-sdk启动机器人的核心驱动节点这通常会发布/sportmodestate、/lidar/pointcloud等话题。# 具体命令取决于tide-sdk的版本和启动脚本 ros2 launch unitree_bringup go2_bringup.launch.py启动SLAM建图如需如果还没有现成的地图需要先让机器人建图。使用slam_toolbox是一个不错的选择。ros2 launch slam_toolbox online_async_launch.py params_file:/path/to/your_slam_config.yaml然后通过遥控或键盘控制机器人走遍环境完成地图构建并保存。加载地图并启动定位使用保存的地图启动nav2_amcl进行定位。ros2 launch nav2_bringup localization_launch.py params_file:/path/to/your_nav2_params.yaml启动我们的SE(2)规划器作为独立节点或Nav2插件启动。# 如果作为独立节点 ros2 run se2_a_star_planner planner_node --ros-args -p map_topic:/map -p start_topic:/amcl_pose -p goal_topic:/goal_pose # 如果集成到Nav2则在Nav2的启动配置中指定规划器插件即可发送目标位姿可以通过Rviz2的“2D Pose Estimate”和“2D Nav Goal”工具来指定起点和目标点。目标点现在包含了朝向。5.2 测试观察与参数调优在真实机器人上测试时重点关注以下几点规划成功率与速度在复杂环境中SE(2)规划可能比2D规划慢。观察规划耗时是否在可接受范围内通常希望1秒。如果太慢可以考虑增大状态离散化的分辨率降低xy_resolution或yaw_resolution但这会降低规划质量。减少运动基元的数量或复杂度。使用更高效的启发式函数或数据结构如二叉堆优化开放集。路径质量观察Rviz中发布的路径。路径是否平滑机器人的预期朝向是否合理例如在直道上朝前在拐弯处提前转向控制器跟踪效果将规划出的路径发送给nav2_dwb_controller等局部规划器/控制器。观察机器人是否能较好地跟踪路径特别是在拐弯处带有朝向信息的路径是否能减少机器人的“摇摆”或不必要的原地旋转关键参数调优表参数描述默认值/建议值调优影响xy_resolution位置离散化分辨率米0.05 - 0.1值越小规划越精确但状态空间越大速度越慢。yaw_resolution朝向离散化分辨率弧度π/12 (15°)值越小最终朝向越精确但搜索分支因子指数增长。orientation_weight启发式函数中朝向代价的权重0.1 - 0.5权重越大规划器越倾向于提前对齐朝向路径可能稍长但最终姿态更优。robot_radius用于碰撞检测的机器人半径米Go2半宽约0.2m可设0.25需略大于实际物理尺寸提供安全裕度。与costmap膨胀半径配合使用。motion_primitive_step运动基元的步长米0.1 - 0.3步长小路径更精细但规划节点多步长大规划快但路径可能不够平滑。max_planning_time最大规划时间秒5.0防止在复杂环境中规划器长时间阻塞。5.3 常见问题与调试技巧规划失败返回空路径检查起点/终点是否在自由空间在Rviz中确认amcl_pose和goal_pose没有落在膨胀后的障碍物区域。检查地图确认地图是完整的没有大片未知区域(-1)将起点/终点隔开。尝试增大robot_radius或costmap的inflation_radius。降低启发式函数的权重如果启发式函数过于激进可能会误导搜索。确保启发式函数是可采纳的never overestimates the true cost这是A*找到最优解的前提。规划路径不理想绕远、朝向奇怪调整orientation_weight如果机器人总是过早转向导致路径绕远尝试减小该权重。如果机器人到达目标时朝向总是不对尝试增大该权重。优化运动基元检查你的运动基元集合是否涵盖了机器人常见的运动模式直行、弧线转弯、原地旋转。可以尝试增加一些原地旋转的基元这对于在狭窄空间调整最终朝向很有用。规划速度慢分析性能瓶颈使用ros2 topic hz查看规划器发布路径的频率或使用rqt的Profiler工具。瓶颈通常在碰撞检测或状态扩展环节。简化碰撞检测如果使用 footprint 多边形检测太慢可以先使用圆形包围盒进行快速粗检测再对少数可疑状态进行精细检测。使用更高效的数据结构确保开放集使用的是优先队列如二叉堆。与控制器配合不佳路径点密度确保发布的nav_msgs/Path中的位姿点足够密集例如间隔0.1米这样局部控制器才能平滑跟踪。检查TF树确保所有坐标系map,odom,base_footprint,base_link的变换关系正确且连续。TF错误是导航失败的常见原因。6. 进阶思考从Demo到实用系统这个demo展示了SE(2) A*规划的基本框架但要应用于实际的巡检、探索任务还需要考虑更多因素动态障碍物处理当前的规划器基于静态地图。在实际中需要集成实时感知信息如激光雷达点云来更新costmap中的障碍物层实现动态避障。这通常由Nav2的costmap_2d层来完成我们的规划器只需要定期从更新的costmap中读取信息即可。路径平滑与优化A*搜索出的路径可能包含不必要的“锯齿”或尖角。在将路径发送给控制器之前可以进行后处理平滑例如使用梯度下降、样条插值或简单的卷积平滑使路径更符合机器人的运动学特性。与行为树BT集成在复杂的任务中如“巡逻A区域后返回充电桩”单一的路径规划节点不够用。可以将我们的规划器作为Nav2行为树中的一个动作节点ComputePathToPose与FollowPath、Spin等节点组合构建复杂的任务逻辑。多目标点序列规划对于覆盖性任务往往需要访问一系列目标点。可以在我们的规划器外层封装一个逻辑依次将各个目标点送入规划器并将得到的路径段拼接起来。更高级的做法是将其建模为旅行商问题(TSP)的变体但求解复杂度会急剧上升。利用机器人的特殊能力Go2是四足机器人具备更强的地形通过能力。当前的规划基于2D占据栅格地图忽略了地形高度信息。未来可以探索集成3D点云或高程地图在规划时考虑坡度、台阶等因素真正发挥四足机器的优势。实现这个demo的过程让我深刻体会到从理论算法到机器人实际运行之间的鸿沟。每一行代码、每一个参数都可能影响机器人的最终行为。最宝贵的经验往往来自于看着机器人因为一个微小的坐标转换错误而撞墙或者因为启发式函数权重设置不当而在门口反复“纠结”。这些实战中踩过的坑是任何教科书都无法完全覆盖的。希望这个详细的梳理和实现指南能帮助你更快地让Go2机器人沿着你期望的“姿态路径”稳健地跑起来。
1. 项目背景与核心目标最近在做一个四足机器人自主导航相关的项目用到了宇树科技的Go2 Edu机器人。在调试过程中我发现一个挺有意思的需求如何让机器人在一个已知的二维地图上规划出一条能高效覆盖所有可通行区域的路径这不仅仅是“从A点走到B点”那么简单而是要让机器人像扫地机器人一样系统地“走遍”地图的每一个角落比如用于环境巡检、区域搜索等任务。传统的全局路径规划算法比如A*在解决“点到点”问题上非常出色但它通常只考虑位置x, y也就是SE(2)空间中的位置分量。然而对于像Go2这样的机器人其朝向yaw角同样至关重要。尤其是在狭窄的走廊或需要执行特定朝向扫描任务时规划出一条同时考虑位置和朝向的路径能显著提升任务执行的效率和成功率。这就是SE(2)空间路径规划的核心——在规划路径时将机器人的姿态x, y, θ作为一个整体来考虑。因此这个demo项目的目标就很明确了基于宇树Go2 Edu机器人及其官方tide-sdk实现一个在SE(2)空间即包含位置和朝向上进行A*路径规划的示例。我们不仅要让机器人到达目标点还要让它以我们期望的“姿态”到达比如在巡检点时正面朝向待检设备。2. 为什么是SE(2)上的A*—— 从SE(2)空间说起在深入代码之前有必要先聊聊SE(2)空间和为什么在这里A*算法需要一些“特别处理”。SE(2)空间即二维特殊欧几里得群简单来说就是平面上的刚体运动空间。一个SE(2)位姿包含三个自由度平面坐标 (x, y) 和绕垂直轴的旋转角 θ偏航角。对于地面移动机器人这正是其完整的平面运动状态。那么为什么在SE(2)空间上直接用A*会变得复杂状态空间爆炸如果我们在一个100x100的网格地图上进行规划纯位置搜索的状态数是10,000个。如果我们把朝向θ也离散化例如分成36个方向每10度一个那么SE(2)空间的状态数瞬间就变成了360,000个。搜索空间呈数量级增长。运动学约束机器人不是质点它不能瞬间改变朝向。从位姿A (x1, y1, θ1) 到位姿B (x2, y2, θ2)需要考虑机器人的转弯半径、最大角速度等约束。简单的8邻域或16邻域移动模型不再适用。代价函数的定义在SE(2)空间中从一位姿移动到另一位姿的代价不仅要考虑欧氏距离还要考虑转向的代价。一个微小的位置移动如果伴随一个180度的大转向其实际“代价”可能比一个稍长的直线移动还要高。A*算法在SE(2)上的变体就是为了解决这些问题。核心思路是在状态扩展时不再仅仅是向相邻的网格移动而是生成一系列符合机器人运动学模型的“运动基元”。每个运动基元代表机器人在一个控制周期内执行特定线速度和角速度后所能到达的下一个SE(2)状态。这样搜索图就从一个静态的网格变成了一个在状态空间中由运动基元连接而成的图。在本次的demo实现中为了简化并突出核心概念我们采用了一种分层规划的策略第一层SE(2)中的位置规划我们仍然使用标准的2D A*算法在地图网格上找到一条从起点到终点的无碰撞位置路径。这一步只关心(x, y)。第二层朝向平滑在得到位置路径后我们再为路径上的每个点分配合适的朝向θ。一个简单有效的方法是使用“前向朝向”或“基于路径切线的朝向”让机器人的朝向尽可能与其运动方向一致减少不必要的原地旋转。这种方法虽然不是严格意义上的在连续SE(2)状态空间中进行搜索但对于许多实际应用尤其是走廊、房间等结构化环境来说已经能极大地改善机器人的最终停靠姿态和移动流畅性是实现完全SE(2)规划的一个很好的起点。3. 环境搭建与tide-sdk初探宇树Go2 Edu的软件开发主要围绕其官方SDK进行。tide-sdk是宇树提供的核心开发工具包它基于ROS 2推荐Foxy或Humble和DDS通信中间件提供了对机器人底层状态关节、IMU、里程计和控制运动、姿态的全面接口。3.1 基础环境配置首先你需要一个运行Ubuntu 20.04或22.04的系统物理机或虚拟机均可。Go2 Edu的扩展坞内置了NVIDIA Jetson Orin可以直接作为开发主机。安装ROS 2按照ROS 2官方教程安装Foxy或Humble版本。这里以Foxy为例因为早期tide-sdk文档多基于此版本。# 设置locale sudo apt update sudo apt install locales sudo locale-gen en_US en_US.UTF-8 sudo update-locale LC_ALLen_US.UTF-8 LANGen_US.UTF-8 export LANGen_US.UTF-8 # 添加ROS 2仓库 sudo apt install software-properties-common sudo add-apt-repository universe sudo apt update sudo apt install curl -y sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg echo deb [arch$(dpkg --print-architecture) signed-by/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release echo $UBUNTU_CODENAME) main | sudo tee /etc/apt/sources.list.d/ros2.list /dev/null # 安装ROS 2桌面版 sudo apt update sudo apt install ros-foxy-desktop python3-argcomplete安装tide-sdk前往宇树官方支持网站下载对应你机器人软件版本的tide-sdk。通常是一个.deb安装包。sudo dpkg -i unitree_tide_sdk_version_amd64.deb安装后SDK的头文件和库文件会置于/opt/unitree/tide-sdk目录下。务必按照官方指南设置环境变量通常是source一个setup脚本。创建工作空间与依赖创建一个ROS 2工作空间用于存放我们的demo代码。mkdir -p ~/go2_se2_planning_ws/src cd ~/go2_se2_planning_ws/src我们需要一些额外的ROS 2包特别是用于导航和TF变换的。# 安装Nav2导航框架和TF2 sudo apt install ros-foxy-nav2-bringup ros-foxy-turtlebot3* ros-foxy-slam-toolbox3.2 tide-sdk核心模块概览tide-sdk通过ROS 2话题和服务暴露了机器人的主要功能。对于我们这个demo需要关注以下几个关键接口状态订阅/sportmodestate(unitree_go/msg/SportModeState)这是最重要的消息之一包含了机器人的里程计信息位置、姿态、IMU数据、关节状态等。我们将从中提取机器人的当前SE(2)位姿。/lidar/pointcloud(sensor_msgs/msg/PointCloud2)来自机器人头部L1激光雷达的点云数据用于建图和障碍物感知。控制发布/cmd_vel(geometry_msgs/msg/Twist)这是ROS中标准的控制接口。发布线速度和角速度指令可以控制机器人移动。tide-sdk会将其转换到底层电机指令。注意Go2也可能使用自定义控制话题需要查阅最新版本文档确认。有时需要将geometry_msgs/Twist通过一个转换节点发布到/wirelesscontroller这类话题。服务调用一些高级动作如“站立”、“坐下”、“趴下”需要通过特定的服务或Action来调用。tide-sdk提供了相应的接口。实操心得在第一次连接机器人时最容易出错的是网络配置。确保你的开发机与机器人在同一局域网并且正确设置了ROS_DOMAIN_ID如果网络中有多个ROS 2系统。建议先用ros2 topic list命令查看是否能收到来自机器人话题通常有/unitree或/go2等前缀的话题这是验证通信是否成功的第一步。4. SE(2) A* 路径规划器的实现我们的规划器将作为一个独立的ROS 2节点运行。它主要完成以下功能订阅地图和机器人位姿接收目标位姿运行SE(2) A*规划算法并将生成的路径以nav_msgs/Path消息形式发布出去供后续的路径跟踪控制器使用。4.1 节点结构与数据流我们创建一个名为se2_a_star_planner的ROS 2节点。其内部数据流如下图所示概念示意[订阅] /map (nav_msgs/OccupancyGrid) [订阅] /amcl_pose (geometry_msgs/PoseWithCovarianceStamped) 或 /odom [订阅] /goal_pose (geometry_msgs/PoseStamped) | V [SE(2) A* 规划器核心] | V [发布] /global_plan (nav_msgs/Path) [发布] /planned_path_vis (visualization_msgs/MarkerArray) (用于Rviz可视化)4.2 核心算法步骤详解以下是规划器核心函数planSE2()的伪代码和关键实现细节# 伪代码SE(2) A* 规划核心 def plan_SE2(start_pose, goal_pose, costmap): start_pose, goal_pose: 类型为 (x, y, theta) costmap: 2D占据栅格地图每个单元格有代价0-100-1未知 # 1. 离散化配置空间 xy_resolution costmap.resolution # 地图分辨率如0.05米/像素 yaw_resolution math.radians(15) # 朝向离散化分辨率如15度 # 将连续SE(2)状态离散化为网格索引 (x_idx, y_idx, theta_idx) start_node discretize_pose(start_pose, xy_resolution, yaw_resolution) goal_node discretize_pose(goal_pose, xy_resolution, yaw_resolution) # 2. 初始化开放集和关闭集 open_set PriorityQueue() open_set.put(start_node, heuristic(start_node, goal_node)) came_from {} # 记录父节点 g_score {start_node: 0} # 从起点到当前节点的实际代价 f_score {start_node: heuristic(start_node, goal_node)} # 估计总代价 # 3. 定义运动基元Motion Primitives # 这里我们定义一组简单的动作前进一段距离同时可以保持原朝向或进行小幅转向 motion_primitives [ (dx, dy, dtheta, cost) for ... in ... # 例如(0.2, 0, 0, 0.2), (0.141, 0.141, pi/4, 0.22)... ] # 4. A* 主循环 while not open_set.empty(): current open_set.get() if current goal_node or close_enough(current, goal_node): return reconstruct_path(came_from, current) # 生成邻居节点 for motion in motion_primitives: dx, dy, dtheta, motion_cost motion # 应用运动基元计算下一个离散状态 next_pose_cont apply_motion(current_pose_cont, dx, dy, dtheta) next_node discretize_pose(next_pose_cont, xy_resolution, yaw_resolution) # 检查下一状态是否碰撞需要将机器人轮廓投影到costmap上判断 if not is_state_valid(next_node, costmap, robot_radius): continue # 计算临时g_score tentative_g_score g_score[current] motion_cost turning_cost(dtheta) if next_node not in g_score or tentative_g_score g_score[next_node]: # 这是一个更好的路径 came_from[next_node] current g_score[next_node] tentative_g_score f_score[next_node] tentative_g_score heuristic(next_node, goal_node) if next_node not in open_set: open_set.put(next_node, f_score[next_node]) return None # 规划失败关键点解析与实现细节离散化与状态表示我们使用一个三维整数元组(x_idx, y_idx, theta_idx)来表示一个离散的SE(2)状态。x_idx, y_idx由地图坐标除以分辨率得到theta_idx由角度除以朝向分辨率得到。这本质上创建了一个3D网格状态空间。启发式函数heuristic这是A*性能的关键。一个在SE(2)空间有效的启发式函数需要同时考虑位置和朝向的差异。一个常用的方法是def heuristic(node, goal): # 欧几里得距离代价 dx (goal.x_idx - node.x_idx) * xy_resolution dy (goal.y_idx - node.y_idx) * xy_resolution distance_cost math.sqrt(dx*dx dy*dy) # 朝向差异代价归一化到[0, pi] dtheta abs(angle_diff(node.theta, goal.theta)) # angle_diff 处理角度环绕 orientation_cost dtheta * orientation_weight # orientation_weight是一个调节参数 return distance_cost orientation_cost通过调节orientation_weight你可以控制规划器在路径长度和最终朝向准确性之间的权衡。运动基元生成这是SE(2)规划区别于2D规划的核心。我们不能简单地移动到相邻的8个网格。我们需要生成一组短时间如0.5秒内机器人可执行的动作轨迹终点。例如以0.4米/秒的速度直行0.2米dx0.2, dy0, dtheta0。以0.3米/秒的速度和0.5弧度/秒的角速度弧线运动0.2秒计算终点位姿。 这些基元可以预先计算好存储在列表中。基元的数量和复杂度直接影响规划的精度和速度。碰撞检测is_state_valid在SE(2)空间中碰撞检测需要考虑机器人的轮廓和朝向。我们需要将机器人 footprint通常是一个矩形或多边形根据当前位姿(x, y, theta)进行旋转和平移然后检查这个footprint覆盖的所有栅格在costmap上是否都是“自由空间”。这是一个计算密集型操作需要优化例如使用bresenham画线算法快速检查边界。路径重建与插值A*找到的路径是由离散状态节点组成的。我们需要将其转换回连续的SE(2)位姿序列。此外由于我们使用了运动基元相邻状态间的运动本身就是平滑的。但为了给控制器一个更密集的参考路径可以在相邻位姿间进行线性插值对x, y和球面线性插值对四元数表示的朝向。4.3 与Nav2框架的集成虽然我们实现了自己的全局规划器但更好的方式是将其作为一个nav2_core::GlobalPlanner插件集成到Nav2框架中。这样可以利用Nav2已有的生命周期管理、参数配置和与控制器、恢复行为的协同。创建插件类继承nav2_core::GlobalPlanner实现configure,activate,deactivate,cleanup,createPlan这几个虚函数。实现createPlan方法在这个方法中调用我们上面实现的planSE2函数。输入是nav_msgs::msg::OccupancyGrid格式的地图和geometry_msgs::msg::PoseStamped格式的起点、终点输出是nav_msgs::msg::Path。注册插件在插件的源码文件中使用PLUGINLIB_EXPORT_CLASS宏注册并在package.xml中导出插件。Nav2配置在Nav2的BT行为树XML配置文件中将全局规划器设置为我们的插件名。这样做的好处是我们的规划器可以无缝替换Nav2默认的nav2_navfn_planner/NavfnPlanner或nav2_theta_star_planner/ThetaStarPlanner并享受Nav2提供的所有基础设施比如与nav2_amcl定位模块、nav2_dwb_controller控制器的配合。避坑指南在实现碰撞检测时一个常见的错误是只检查机器人中心点所在的栅格。对于Go2这样有一定尺寸的机器人这会导致规划出的路径紧贴障碍物甚至发生碰撞。务必使用机器人的完整footprint进行碰撞检查。你可以通过URDF文件获取机器人的轮廓尺寸或者保守地使用一个包围圆半径。在costmap中通常将障碍物进行膨胀inflation规划时只要检查机器人中心点是否在膨胀后的障碍物区域外即可这是一种高效的近似方法。5. 在Unitree Go2上的部署与测试算法实现后下一步就是让它在真实的Go2机器人上跑起来。5.1 启动与配置流程启动机器人底层驱动首先通过tide-sdk启动机器人的核心驱动节点这通常会发布/sportmodestate、/lidar/pointcloud等话题。# 具体命令取决于tide-sdk的版本和启动脚本 ros2 launch unitree_bringup go2_bringup.launch.py启动SLAM建图如需如果还没有现成的地图需要先让机器人建图。使用slam_toolbox是一个不错的选择。ros2 launch slam_toolbox online_async_launch.py params_file:/path/to/your_slam_config.yaml然后通过遥控或键盘控制机器人走遍环境完成地图构建并保存。加载地图并启动定位使用保存的地图启动nav2_amcl进行定位。ros2 launch nav2_bringup localization_launch.py params_file:/path/to/your_nav2_params.yaml启动我们的SE(2)规划器作为独立节点或Nav2插件启动。# 如果作为独立节点 ros2 run se2_a_star_planner planner_node --ros-args -p map_topic:/map -p start_topic:/amcl_pose -p goal_topic:/goal_pose # 如果集成到Nav2则在Nav2的启动配置中指定规划器插件即可发送目标位姿可以通过Rviz2的“2D Pose Estimate”和“2D Nav Goal”工具来指定起点和目标点。目标点现在包含了朝向。5.2 测试观察与参数调优在真实机器人上测试时重点关注以下几点规划成功率与速度在复杂环境中SE(2)规划可能比2D规划慢。观察规划耗时是否在可接受范围内通常希望1秒。如果太慢可以考虑增大状态离散化的分辨率降低xy_resolution或yaw_resolution但这会降低规划质量。减少运动基元的数量或复杂度。使用更高效的启发式函数或数据结构如二叉堆优化开放集。路径质量观察Rviz中发布的路径。路径是否平滑机器人的预期朝向是否合理例如在直道上朝前在拐弯处提前转向控制器跟踪效果将规划出的路径发送给nav2_dwb_controller等局部规划器/控制器。观察机器人是否能较好地跟踪路径特别是在拐弯处带有朝向信息的路径是否能减少机器人的“摇摆”或不必要的原地旋转关键参数调优表参数描述默认值/建议值调优影响xy_resolution位置离散化分辨率米0.05 - 0.1值越小规划越精确但状态空间越大速度越慢。yaw_resolution朝向离散化分辨率弧度π/12 (15°)值越小最终朝向越精确但搜索分支因子指数增长。orientation_weight启发式函数中朝向代价的权重0.1 - 0.5权重越大规划器越倾向于提前对齐朝向路径可能稍长但最终姿态更优。robot_radius用于碰撞检测的机器人半径米Go2半宽约0.2m可设0.25需略大于实际物理尺寸提供安全裕度。与costmap膨胀半径配合使用。motion_primitive_step运动基元的步长米0.1 - 0.3步长小路径更精细但规划节点多步长大规划快但路径可能不够平滑。max_planning_time最大规划时间秒5.0防止在复杂环境中规划器长时间阻塞。5.3 常见问题与调试技巧规划失败返回空路径检查起点/终点是否在自由空间在Rviz中确认amcl_pose和goal_pose没有落在膨胀后的障碍物区域。检查地图确认地图是完整的没有大片未知区域(-1)将起点/终点隔开。尝试增大robot_radius或costmap的inflation_radius。降低启发式函数的权重如果启发式函数过于激进可能会误导搜索。确保启发式函数是可采纳的never overestimates the true cost这是A*找到最优解的前提。规划路径不理想绕远、朝向奇怪调整orientation_weight如果机器人总是过早转向导致路径绕远尝试减小该权重。如果机器人到达目标时朝向总是不对尝试增大该权重。优化运动基元检查你的运动基元集合是否涵盖了机器人常见的运动模式直行、弧线转弯、原地旋转。可以尝试增加一些原地旋转的基元这对于在狭窄空间调整最终朝向很有用。规划速度慢分析性能瓶颈使用ros2 topic hz查看规划器发布路径的频率或使用rqt的Profiler工具。瓶颈通常在碰撞检测或状态扩展环节。简化碰撞检测如果使用 footprint 多边形检测太慢可以先使用圆形包围盒进行快速粗检测再对少数可疑状态进行精细检测。使用更高效的数据结构确保开放集使用的是优先队列如二叉堆。与控制器配合不佳路径点密度确保发布的nav_msgs/Path中的位姿点足够密集例如间隔0.1米这样局部控制器才能平滑跟踪。检查TF树确保所有坐标系map,odom,base_footprint,base_link的变换关系正确且连续。TF错误是导航失败的常见原因。6. 进阶思考从Demo到实用系统这个demo展示了SE(2) A*规划的基本框架但要应用于实际的巡检、探索任务还需要考虑更多因素动态障碍物处理当前的规划器基于静态地图。在实际中需要集成实时感知信息如激光雷达点云来更新costmap中的障碍物层实现动态避障。这通常由Nav2的costmap_2d层来完成我们的规划器只需要定期从更新的costmap中读取信息即可。路径平滑与优化A*搜索出的路径可能包含不必要的“锯齿”或尖角。在将路径发送给控制器之前可以进行后处理平滑例如使用梯度下降、样条插值或简单的卷积平滑使路径更符合机器人的运动学特性。与行为树BT集成在复杂的任务中如“巡逻A区域后返回充电桩”单一的路径规划节点不够用。可以将我们的规划器作为Nav2行为树中的一个动作节点ComputePathToPose与FollowPath、Spin等节点组合构建复杂的任务逻辑。多目标点序列规划对于覆盖性任务往往需要访问一系列目标点。可以在我们的规划器外层封装一个逻辑依次将各个目标点送入规划器并将得到的路径段拼接起来。更高级的做法是将其建模为旅行商问题(TSP)的变体但求解复杂度会急剧上升。利用机器人的特殊能力Go2是四足机器人具备更强的地形通过能力。当前的规划基于2D占据栅格地图忽略了地形高度信息。未来可以探索集成3D点云或高程地图在规划时考虑坡度、台阶等因素真正发挥四足机器的优势。实现这个demo的过程让我深刻体会到从理论算法到机器人实际运行之间的鸿沟。每一行代码、每一个参数都可能影响机器人的最终行为。最宝贵的经验往往来自于看着机器人因为一个微小的坐标转换错误而撞墙或者因为启发式函数权重设置不当而在门口反复“纠结”。这些实战中踩过的坑是任何教科书都无法完全覆盖的。希望这个详细的梳理和实现指南能帮助你更快地让Go2机器人沿着你期望的“姿态路径”稳健地跑起来。
