无人机开发分享——基于一致性算法的无人机集群协同避障与目标跟踪算法开发

一致性算法是分布式协同控制的基础方法，通过邻居间的信息交换使整个集群达成一致状态。在一致性算法基础上增加机间避障和目标跟踪功能，即可实现基于一致性算法的无人机集群协同避障与目标跟踪，接下来我将分享如何应用这一算法。

具体的ROS2的功能包的定义、创建和运行在之前的文章已经写过了，就不再赘述了，下面只讲原理和代码。
1、系统架构

2. 算法原理

一致性控制

维持编队形状和速度同步

避障算法

基于人工势场法避免无人机间碰撞

目标跟踪

引导整个编队向目标点移动

一致性算法原理

避障算法设计（人工势场法）

目标跟踪设计

3、代码实现

#include <rclcpp/rclcpp.hpp>
#include <vector>
#include <map>
#include <mutex>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <mavros_msgs/msg/position_target.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <consensus_ros2/msg/drone_state.hpp>
#include <consensus_ros2/msg/formation_target.hpp>

class ConsensusObstacleController : public rclcpp::Node {
public:
    ConsensusObstacleController(const std::string & node_name, int drone_id)
        : Node(node_name), drone_id_(drone_id) {
        // 初始化参数
        init_parameters();
        
        // 创建订阅者、发布者
        init_communication();
        
        // 创建定时器，定期执行控制算法
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(static_cast<int>(dt_ * 1000)),
            std::bind(&ConsensusObstacleController::timer_callback, this));
    }

private:
    // 无人机参数
    int drone_id_;
    geometry_msgs::msg::Point current_position_;
    geometry_msgs::msg::Vector3 current_velocity_;
    float current_yaw_;
    
    // 编队目标
    geometry_msgs::msg::Point formation_target_;
    bool target_received_{false};
    
    // 控制算法参数
    double dt_{0.1};             // 采样时间
    double kp_{0.5};             // 位置控制增益
    double kv_{0.3};             // 速度控制增益
    double neighbor_distance_{10.0};  // 邻居检测距离
    double collision_distance_{2.0};  // 避碰距离阈值
    double repulsive_gain_{1.0};  // 排斥力增益
    double attractive_gain_{0.8}; // 吸引力增益
    double formation_weight_{0.5}; // 编队权重
    
    // 邻居状态
    std::map<uint32_t, consensus_ros2::msg::DroneState> neighbor_states_;
    std::mutex neighbor_mutex_;
    
    // ROS2通信
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::Publisher<consensus_ros2::msg::DroneState>::SharedPtr state_pub_;
    rclcpp::Subscription<consensus_ros2::msg::DroneState>::SharedPtr neighbor_sub_;
    rclcpp::Publisher<mavros_msgs::msg::PositionTarget>::SharedPtr cmd_pub_;
    rclcpp::Subscription<consensus_ros2::msg::FormationTarget>::SharedPtr target_sub_;
    rclcpp::TimerBase::SharedPtr timer_;
    
    // 初始化参数
    void init_parameters() {
        // 从参数服务器加载配置
        this->declare_parameter("dt", dt_);
        this->declare_parameter("kp", kp_);
        this->declare_parameter("kv", kv_);
        this->declare_parameter("neighbor_distance", neighbor_distance_);
        this->declare_parameter("collision_distance", collision_distance_);
        this->declare_parameter("repulsive_gain", repulsive_gain_);
        this->declare_parameter("attractive_gain", attractive_gain_);
        this->declare_parameter("formation_weight", formation_weight_);
        
        this->get_parameter("dt", dt_);
        this->get_parameter("kp", kp_);
        this->get_parameter("kv", kv_);
        this->get_parameter("neighbor_distance", neighbor_distance_);
        this->get_parameter("collision_distance", collision_distance_);
        this->get_parameter("repulsive_gain", repulsive_gain_);
        this->get_parameter("attractive_gain", attractive_gain_);
        this->get_parameter("formation_weight", formation_weight_);
    }
    
    // 初始化通信
    void init_communication() {
        // 订阅自身状态
        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "mavros/global_position/odom", 10, 
            std::bind(&ConsensusObstacleController::odom_callback, this, std::placeholders::_1));
        
        // 发布自身状态
        state_pub_ = this->create_publisher<consensus_ros2::msg::DroneState>(
            "drone_state", 10);
        
        // 订阅邻居状态
        neighbor_sub_ = this->create_subscription<consensus_ros2::msg::DroneState>(
            "neighbor_states", 10, 
            std::bind(&ConsensusObstacleController::neighbor_callback, this, std::placeholders::_1));
        
        // 订阅编队目标
        target_sub_ = this->create_subscription<consensus_ros2::msg::FormationTarget>(
            "formation_target", 10, 
            std::bind(&ConsensusObstacleController::target_callback, this, std::placeholders::_1));
        
        // 发布控制指令到飞控
        cmd_pub_ = this->create_publisher<mavros_msgs::msg::PositionTarget>(
            "mavros/setpoint_raw/local", 10);
    }
    
    // 里程计回调函数
    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
        current_position_ = msg->pose.pose.position;
        current_velocity_ = msg->twist.twist.linear;
        
        // 从四元数计算偏航角
        tf2::Quaternion q(
            msg->pose.pose.orientation.x,
            msg->pose.pose.orientation.y,
            msg->pose.pose.orientation.z,
            msg->pose.pose.orientation.w);
        tf2::Matrix3x3 m(q);
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);
        current_yaw_ = yaw;
        
        // 发布自身状态
        publish_self_state();
    }
    
    // 发布自身状态
    void publish_self_state() {
        auto state_msg = std::make_unique<consensus_ros2::msg::DroneState>();
        state_msg->drone_id = drone_id_;
        state_msg->position = current_position_;
        state_msg->velocity = current_velocity_;
        state_msg->yaw = current_yaw_;
        
        state_pub_->publish(std::move(state_msg));
    }
    
    // 目标回调函数
    void target_callback(const consensus_ros2::msg::FormationTarget::SharedPtr msg) {
        formation_target_ = msg->position;
        target_received_ = true;
        RCLCPP_INFO(this->get_logger(), "Received formation target: x=%.2f, y=%.2f, z=%.2f",
                   formation_target_.x, formation_target_.y, formation_target_.z);
    }
    
    // 邻居状态回调函数
    void neighbor_callback(const consensus_ros2::msg::DroneState::SharedPtr msg) {
        // 忽略自身状态
        if (msg->drone_id == drone_id_) return;
        
        // 计算与邻居的距离
        double dx = current_position_.x - msg->position.x;
        double dy = current_position_.y - msg->position.y;
        double dz = current_position_.z - msg->position.z;
        double distance = std::sqrt(dx*dx + dy*dy + dz*dz);
        
        // 只考虑在邻居距离范围内的无人机
        if (distance <= neighbor_distance_) {
            std::lock_guard<std::mutex> lock(neighbor_mutex_);
            neighbor_states_[msg->drone_id] = *msg;
        }
    }
    
    // 定时器回调函数，执行控制算法
    void timer_callback() {
        if (!target_received_) {
            RCLCPP_WARN(this->get_logger(), "No formation target received yet!");
            return;
        }
        
        // 计算一致性控制输入
        geometry_msgs::msg::Vector3 consensus_control = calculate_consensus_control();
        
        // 计算机间避障控制输入
        geometry_msgs::msg::Vector3 obstacle_avoidance = calculate_obstacle_avoidance();
        
        // 计算目标跟踪控制输入
        geometry_msgs::msg::Vector3 target_tracking = calculate_target_tracking();
        
        // 综合控制输入
        geometry_msgs::msg::Vector3 control_input;
        control_input.x = formation_weight_ * consensus_control.x + 
                         (1.0 - formation_weight_) * target_tracking.x + 
                         obstacle_avoidance.x;
        control_input.y = formation_weight_ * consensus_control.y + 
                         (1.0 - formation_weight_) * target_tracking.y + 
                         obstacle_avoidance.y;
        control_input.z = formation_weight_ * consensus_control.z + 
                         (1.0 - formation_weight_) * target_tracking.z + 
                         obstacle_avoidance.z;
        
        // 发送控制指令到飞控
        send_control_to_fc(control_input);
    }
    
    // 计算一致性控制输入
    geometry_msgs::msg::Vector3 calculate_consensus_control() {
        geometry_msgs::msg::Vector3 control;
        control.x = 0.0;
        control.y = 0.0;
        control.z = 0.0;
        
        std::lock_guard<std::mutex> lock(neighbor_mutex_);
        
        // 如果没有邻居，返回零控制
        if (neighbor_states_.empty()) {
            return control;
        }
        
        // 计算所有邻居的平均位置和速度
        geometry_msgs::msg::Point avg_position;
        geometry_msgs::msg::Vector3 avg_velocity;
        int neighbor_count = 0;
        
        for (const auto& pair : neighbor_states_) {
            const auto& state = pair.second;
            
            avg_position.x += state.position.x;
            avg_position.y += state.position.y;
            avg_position.z += state.position.z;
            
            avg_velocity.x += state.velocity.x;
            avg_velocity.y += state.velocity.y;
            avg_velocity.z += state.velocity.z;
            
            neighbor_count++;
        }
        
        if (neighbor_count > 0) {
            avg_position.x /= neighbor_count;
            avg_position.y /= neighbor_count;
            avg_position.z /= neighbor_count;
            
            avg_velocity.x /= neighbor_count;
            avg_velocity.y /= neighbor_count;
            avg_velocity.z /= neighbor_count;
        }
        
        // 一致性控制律：位置误差 + 速度同步
        control.x = kp_ * (avg_position.x - current_position_.x) + 
                    kv_ * (avg_velocity.x - current_velocity_.x);
        control.y = kp_ * (avg_position.y - current_position_.y) + 
                    kv_ * (avg_velocity.y - current_velocity_.y);
        control.z = kp_ * (avg_position.z - current_position_.z) + 
                    kv_ * (avg_velocity.z - current_velocity_.z);
        
        return control;
    }
    
    // 计算机间避障控制输入
    geometry_msgs::msg::Vector3 calculate_obstacle_avoidance() {
        geometry_msgs::msg::Vector3 control;
        control.x = 0.0;
        control.y = 0.0;
        control.z = 0.0;
        
        std::lock_guard<std::mutex> lock(neighbor_mutex_);
        
        // 对每个邻居计算机间排斥力
        for (const auto& pair : neighbor_states_) {
            const auto& neighbor = pair.second;
            
            // 计算相对位置
            double dx = current_position_.x - neighbor.position.x;
            double dy = current_position_.y - neighbor.position.y;
            double dz = current_position_.z - neighbor.position.z;
            double distance = std::sqrt(dx*dx + dy*dy + dz*dz);
            
            // 如果距离小于碰撞阈值，计算排斥力
            if (distance > 0 && distance < collision_distance_) {
                // 排斥力大小
                double repulsive_force = repulsive_gain_ * 
                                        (1.0/distance - 1.0/collision_distance_) * 
                                        (1.0/(distance*distance));
                
                // 排斥力方向（归一化）
                control.x += repulsive_force * dx / distance;
                control.y += repulsive_force * dy / distance;
                control.z += repulsive_force * dz / distance;
            }
        }
        
        return control;
    }
    
    // 计算目标跟踪控制输入
    geometry_msgs::msg::Vector3 calculate_target_tracking() {
        geometry_msgs::msg::Vector3 control;
        
        // 计算到目标的误差
        double ex = formation_target_.x - current_position_.x;
        double ey = formation_target_.y - current_position_.y;
        double ez = formation_target_.z - current_position_.z;
        
        // 目标吸引力（非线性，距离远时使用常速，距离近时使用比例控制）
        double distance = std::sqrt(ex*ex + ey*ey + ez*ez);
        double max_speed = 2.0;  // 最大速度限制
        
        if (distance > 0) {
            // 限制最大速度
            double speed = std::min(attractive_gain_ * distance, max_speed);
            
            control.x = speed * ex / distance;
            control.y = speed * ey / distance;
            control.z = speed * ez / distance;
        }
        
        return control;
    }
    
    // 发送控制指令到飞控
    void send_control_to_fc(const geometry_msgs::msg::Vector3& control) {
        mavros_msgs::msg::PositionTarget cmd;
        
        // 设置控制帧和类型掩码
        cmd.coordinate_frame = mavros_msgs::msg::PositionTarget::FRAME_LOCAL_NED;
        cmd.type_mask = mavros_msgs::msg::PositionTarget::IGNORE_PX |
                        mavros_msgs::msg::PositionTarget::IGNORE_PY |
                        mavros_msgs::msg::PositionTarget::IGNORE_PZ |
                        mavros_msgs::msg::PositionTarget::IGNORE_YAW |
                        mavros_msgs::msg::PositionTarget::IGNORE_YAW_RATE;
        
        // 设置速度控制
        cmd.velocity.x = control.x;
        cmd.velocity.y = control.y;
        cmd.velocity.z = control.z;
        
        // 发布控制指令
        cmd_pub_->publish(cmd);
    }
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    
    // 获取无人机ID（从命令行参数或环境变量）
    int drone_id = 0;
    if (argc > 1) {
        drone_id = std::stoi(argv[1]);
    }
    
    auto node = std::make_shared<ConsensusObstacleController>("consensus_obstacle_controller", drone_id);
    rclcpp::spin(node);
    rclcpp::shutdown();
    
    return 0;
}  

4. 算法参数调整
   关键参数及其影响：
   位置增益kp
   ：增大可提高位置收敛速度，但可能导致震荡。
   速度增益kv
   ：增大可提高速度同步效果，但可能增加能耗。
   采样时间dt
   ：减小可提高控制精度，但要求更高计算频率。
   邻居距离neighbor_distance
   ：决定通信范围，过大会增加通信负担，过小会导致孤立。
   formation_weight
   ：编队保持权重（0~1），值越大越注重编队形状
   repulsive_gain
   ：避障排斥力增益，值越大避障反应越强烈
   attractive_gain
   ：目标吸引力增益，值越大趋向目标速度越快
   collision_distance
   ：避障作用距离阈值，建议设为无人机翼展的 1.5~2 倍
   5.注意事项
   局部最小值问题
   人工势场法可能导致局部最小值，可通过添加随机扰动或路径规划算法解决。
   通信延迟处理
   高延迟环境下，可采用状态预测或卡尔曼滤波估计邻居位置。
   避障与编队权衡
   调整formation_weight参数平衡编队保持和避障需求。
   三维避障
   本实现考虑了三维空间的避障，适合立体编队飞行。
   
   以上就是本期关于基于一致性算法的无人机协同控制算法的开发分享，更多分享可关注公众号(无人机自由开发坊）或添加UavFree95加入讨论群