无人机开发分享——基于行为树的无人机集群机载自主决策算法框架搭建及开发

** 在之前的开发分享中，我们分享了机载端开发任务管理、协同控制、无人机平台接口等一系列模块软件，但是要想实现真正的无人机高度智能化，机载的自主决策模块功能必不可少。机载自主决策模块主要实现无人机集群任务过程中地面任务获取分析、战场实时态势分析与自主决策，可在断连或自主模式时对作业中的无人机集群进行决策与控制，减少地面交互，提升任务效率。下面我们对机载自主决策模块的开发进行介绍。**

一、模块架构

我们搭建了任务管理器（接收任务或实时信息）、决策引擎（决策算法推选任务）、行为树（动态管理服务）的架构，实现从信息获取、决策算法推选、行为树执行的框架搭建。

二、自主决策软件算法

1、任务管理器

主要实现接收地面任务或情报信息

#pragma once#include <rclcpp/rclcpp.hpp>#include "autonomous_decision_msgs/msg/task_command.hpp"#include "autonomous_decision_msgs/msg/intelligence_report.hpp"#include <queue>#include <mutex>namespace autonomous_decision {class TaskManager : public rclcpp::Node {public:TaskManager();// 任务优先级枚举enum TaskPriority {CRITICAL = 0,HIGH = 1,NORMAL = 2,LOW = 3};// 任务结构体struct Task {uint32_t id;std::string type;geometry_msgs::msg::PoseStamped target;TaskPriority priority;rclcpp::Time deadline;std::map<std::string, std::string> parameters;};// 添加新任务void add_task(const autonomous_decision_msgs::msg::TaskCommand::SharedPtr msg);// 获取最高优先级任务Task get_next_task();// 更新情报信息void update_intelligence(const autonomous_decision_msgs::msg::IntelligenceReport::SharedPtr report);private:// 任务队列std::priority_queue<Task, std::vector<Task>, [](const Task& a, const Task& b) {return a.priority < b.priority || (a.priority == b.priority && a.deadline > b.deadline);}> task_queue_;// 情报数据库std::map<std::string, autonomous_decision_msgs::msg::IntelligenceReport> intelligence_db_;// 线程安全std::mutex task_mutex_;std::mutex intel_mutex_;// ROS接口rclcpp::Subscription<autonomous_decision_msgs::msg::TaskCommand>::SharedPtr task_sub_;rclcpp::Subscription<autonomous_decision_msgs::msg::IntelligenceReport>::SharedPtr intel_sub_;};} // namespace autonomous_decision

2、决策引擎（决策算法调用，选择待执行任务）

#pragma once#include <rclcpp/rclcpp.hpp>#include "behaviortree_cpp/bt_factory.h"#include "task_manager.hpp"#include "behavior_tree_nodes.hpp"namespace autonomous_decision {class DecisionEngine : public rclcpp::Node {public:DecisionEngine();// 决策算法枚举enum DecisionAlgorithm {RULE_BASED = 0,UTILITY_BASED = 1,MDP = 2,       // 马尔可夫决策过程POMDP = 3      // 部分可观察马尔可夫决策过程};// 初始化决策系统void initialize();// 执行决策循环void run_decision_cycle();private:// 行为树工厂BT::BehaviorTreeFactory factory_;// 当前行为树std::unique_ptr<BT::Tree> current_tree_;// 任务管理器std::shared_ptr<TaskManager> task_manager_;// 决策算法DecisionAlgorithm current_algorithm_;// 创建行为树void create_behavior_tree(const std::string& tree_type);// 基于规则的决策TaskManager::Task rule_based_decision();// 基于效用的决策TaskManager::Task utility_based_decision();// MDP决策TaskManager::Task mdp_decision();// 配置参数void load_parameters();// ROS定时器rclcpp::TimerBase::SharedPtr decision_timer_;// 决策频率double decision_frequency_;};} // namespace autonomous_decision

其中，可以选择决策算法，如规则算法、效用算法、mdp等

基于效用的决策算法：

TaskManager::Task DecisionEngine::utility_based_decision() {auto tasks = task_manager_->get_all_tasks();if (tasks.empty()) return TaskManager::Task();double max_utility = -std::numeric_limits<double>::max();TaskManager::Task best_task;for (auto& task : tasks) {// 计算任务效用double utility = 0.0;// 1. 优先级权重double priority_weight = 1.0;switch(task.priority) {case TaskManager::CRITICAL: priority_weight = 4.0; break;case TaskManager::HIGH: priority_weight = 2.0; break;case TaskManager::NORMAL: priority_weight = 1.0; break;case TaskManager::LOW: priority_weight = 0.5; break;}// 2. 时间紧迫性double time_factor = 1.0;auto time_left = (task.deadline - now()).seconds();if (time_left > 0) {time_factor = 1.0 / (1.0 + std::exp(-0.1 * time_left));}// 3. 资源匹配度double resource_match = calculate_resource_match(task);// 4. 情报价值double intel_value = calculate_intelligence_value(task.target);// 综合效用计算utility = priority_weight * 0.4 + time_factor * 0.3 + resource_match * 0.2 + intel_value * 0.1;// 选择最高效用任务if (utility > max_utility) {max_utility = utility;best_task = task;}}return best_task;}

基于mdp（马尔科夫）的决策算法

TaskManager::Task DecisionEngine::mdp_decision() {// 简化的MDP实现auto tasks = task_manager_->get_all_tasks();if (tasks.empty()) return TaskManager::Task();// 定义状态空间 (当前状态+任务类型)enum State { IDLE, RECON, SURVEILLANCE, EMERGENCY };// 定义奖励函数auto reward_function = [](State state, const TaskManager::Task& task) {double reward = 0.0;// 任务完成奖励if (task.priority == TaskManager::CRITICAL) reward += 100;else if (task.priority == TaskManager::HIGH) reward += 50;else reward += 20;// 状态转换惩罚if (state == EMERGENCY && task.type != "emergency") reward -= 30;if (state != IDLE && task.type == "emergency") reward += 80;// 时间惩罚auto time_left = (task.deadline - now()).seconds();if (time_left < 60) reward -= 50 * (1 - time_left/60.0);return reward;};// 简化的策略选择State current_state = IDLE; // 从系统状态获取实际值double max_reward = -std::numeric_limits<double>::max();TaskManager::Task best_task;for (auto& task : tasks) {double reward = reward_function(current_state, task);if (reward > max_reward) {max_reward = reward;best_task = task;}}return best_task;}

3、行为树执行

行为树的ROS工程中比较常见的管理架构，学习资料可以参考behavior_tee和navigation2。

建立行为树表（示例）：

<!-- config/behavior_trees/reconnaissance.xml --><root main_tree_to_execute="ReconTree"><BehaviorTree ID="ReconTree"><Sequence name="ReconSequence"><!-- 获取侦察任务 --><GetReconTask name="GetReconTask" task="{recon_task}"/><!-- 风险评估 --><RiskAssessment name="RiskAssessment" task="{recon_task}" risk_level="{risk}"/><!-- 资源检查 --><ResourceCheck name="ResourceCheck" task="{recon_task}" sufficient_resources="{has_resources}"/><!-- 根据风险决策 --><Fallback name="RiskDecision"><Condition name="LowRisk" if_risk="LOW"/><SubTree ID="HighRiskPlan" name="HighRiskResponse"/></Fallback><!-- 执行任务 --><ExecuteTask name="ExecuteRecon" task="{recon_task}"/><!-- 处理结果 --><ProcessIntel name="ProcessIntelData"/><!-- 报告完成 --><ReportCompletion name="ReportReconComplete"/></Sequence></BehaviorTree><!-- 高风险处理子树 --><BehaviorTree ID="HighRiskPlan"><Sequence name="HighRiskSequence"><RequestSupport name="RequestSupport"/><Delay delay="5000"/><ReevaluateRisk name="ReevaluateAfterSupport"/><Condition name="CheckRiskReduced" if_risk="LOW"/><AbortTask name="AbortIfHighRisk" if_condition="false"/></Sequence></BehaviorTree></root>

行为树执行：

#pragma once#include <behaviortree_cpp/action_node.h>#include <behaviortree_cpp/bt_factory.h>#include <rclcpp/rclcpp.hpp>#include "autonomous_decision_msgs/action/execute_task.hpp"namespace autonomous_decision {// 任务执行节点class ExecuteTask : public BT::StatefulActionNode {public:ExecuteTask(const std::string& name, const BT::NodeConfig& config);static BT::PortsList providedPorts() {return { BT::InputPort<TaskManager::Task>("task"),BT::OutputPort<std::string>("status")};}BT::NodeStatus onStart() override;BT::NodeStatus onRunning() override;void onHalted() override;private:rclcpp::Node::SharedPtr node_;rclcpp_action::Client<ExecuteTask>::SharedPtr action_client_;rclcpp_action::ClientGoalHandle<ExecuteTask>::SharedPtr goal_handle_;};// 风险评估节点class RiskAssessment : public BT::ConditionNode {public:RiskAssessment(const std::string& name, const BT::NodeConfig& config);static BT::PortsList providedPorts() {return { BT::InputPort<TaskManager::Task>("task"),BT::OutputPort<double>("risk_level")};}BT::NodeStatus tick() override;private:double calculate_risk(const TaskManager::Task& task);};// 资源检查节点class ResourceCheck : public BT::ConditionNode {public:ResourceCheck(const std::string& name, const BT::NodeConfig& config);static BT::PortsList providedPorts() {return { BT::InputPort<TaskManager::Task>("task"),BT::OutputPort<bool>("sufficient_resources")};}BT::NodeStatus tick() override;};// 注册所有节点void RegisterCustomNodes(BT::BehaviorTreeFactory& factory);} // namespace autonomous_decision

三、模块亮点

1架构清晰：采用“决策引擎（DecisionEngine）+任务管理器（TaskManager）+ 行为树（Behavior Tree）” 的分层设计，职责划分明确（决策引擎负责算法选择与循环，任务管理器负责任务生命周期管理，行为树负责任务执行流程控制）。

2多决策算法支持：预留了规则基（RuleBased）、效用基（UtilityBased）、MDP 等决策算法接口，便于根据场景切换或扩展。

以上对于无人机集群机载自主决策算法的开发思路和示例进行了介绍，有相关开发经验的可以加入v(UavFree95)进入无人机开发进行共同讨论。