https://github.com/ZJU-FAST-Lab/Swarm-Formation
高飞实验室集群

配置

sudo apt-get install libarmadillo-dev
git clone https://github.com/ZJU-FAST-Lab/Swarm-Formation.git
cd Swarm-Formation
catkin_make -j1
source devel/setup.bash
roslaunch ego_planner rviz.launch

SwarmGraph类

private:
    std::vector<Eigen::Vector3d> nodes;  // 图顶点的位置
    std::vector<Eigen::Vector3d> nodes_des;  // 期望图的顶点位置（分配后可能会发生变化）
    std::vector<Eigen::Vector3d> nodes_des_init; // 初始期望图的顶点位置（一旦设置后不会改变）

    std::vector<Eigen::Vector3d> agent_grad; // 包含图的梯度信息

    bool have_desired; // 表示是否存在期望图

    Eigen::MatrixXd A;   // 邻接矩阵
    Eigen::VectorXd D;   // 度矩阵
    Eigen::MatrixXd Lhat; // 对称归一化拉普拉斯矩阵

    Eigen::MatrixXd A_des;   // 期望邻接矩阵
    Eigen::VectorXd D_des;   // 期望度矩阵
    Eigen::MatrixXd Lhat_des;  // 期望的对称归一化拉普拉斯矩阵
    
    Eigen::MatrixXd DLhat; // 对称归一化拉普拉斯矩阵的差异

public:

    SwarmGraph(); //构造函数
    ~SwarmGraph(){}//析构函数，用于销毁类实例

    //更新节点，特征矩阵和期望矩阵，它接受一个类型为std::vector<Eigen::Vector3d>的参数swarm，该参数包含了更新后的节点位置信息。返回一个布尔值，表示更新是否成功。
    bool updateGraph( const std::vector<Eigen::Vector3d> &swarm); 

    //设定群的期望节点
    bool setDesiredForm( const std::vector<Eigen::Vector3d> &swarm_des );

    //计算蜂群图的特征矩阵。它接受一个类型为std::vector<Eigen::Vector3d>的参数swarm，表示蜂群图的节点位置。它还接受三个引用参数，Adj、Deg和SNL，分别表示邻接矩阵、度矩阵和对称归一化拉普拉斯矩阵。返回一个布尔值，表示计算是否成功。
    bool calcMatrices( const std::vector<Eigen::Vector3d> &swarm,
                       Eigen::MatrixXd &Adj, Eigen::VectorXd &Deg,
                       Eigen::MatrixXd &SNL);

    //计算两个三维向量之间的E-范数距离。它接受两个Eigen::Vector3d类型的参数v1和v2，表示要计算距离的向量。返回一个双精度浮点数，表示计算得到的距离。
    double calcDist2( const Eigen::Vector3d &v1, const Eigen::Vector3d &v2);                   

    //计算对称归一化拉普拉斯矩阵（SNL矩阵）的平方F-范数差。它接受一个引用参数cost，表示计算得到的差值。返回一个布尔值，表示计算是否成功。
    bool calcFNorm2( double &cost ); 

    //计算位置梯度（gradp）的F-范数。它接受一个引用参数gradp，表示计算得到的位置梯度，以及一个整数参数idx，表示要计算梯度的位置索引。返回一个布尔值，表示计算是否成功。
    bool calcFGrad( Eigen::Vector3d &gradp, int idx );

    //获取指定位置的位置梯度。它接受一个整数参数id，表示要获取梯度的位置索引。返回一个Eigen::Vector3d类型的向量，表示获取到的位置梯度。
    Eigen::Vector3d getGrad(int id);
	//获取所有位置的位置梯度。它接受一个引用参数swarm_grad，表示获取到的所有位置梯度。返回一个布尔值，表示获取是否成功。
    bool getGrad(std::vector<Eigen::Vector3d> &swarm_grad);

    //获取初始期望图的节点位置。返回一个std::vector<Eigen::Vector3d>类型的向量，表示初始期望图的节点位置。
    std::vector<Eigen::Vector3d> getDesNodesInit() { return nodes_des_init; }
	//获取当前期望图的节点位置。返回一个std::vector<Eigen::Vector3d>类型的向量，表示当前期望图的节点位置。
    std::vector<Eigen::Vector3d> getDesNodesCur() { return nodes_des; }
	//定义了一个SwarmGraph类的指针类型Ptr，使用了std::unique_ptr智能指针。
    typedef std::unique_ptr<SwarmGraph> Ptr;
	//该宏用于在Eigen库中对新的对齐内存分配器进行声明，确保使用正确的内存对齐方式。
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

};

函数实现

#include <swarm_graph/swarm_graph.hpp>
   
SwarmGraph::SwarmGraph(){   
    have_desired = false;
}//构造函数，false表示不存在期望图

updateGraph 更新节点位置，特征矩阵和期望矩阵，它接受一个类型为std::vector<Eigen::Vector3d>的参数swarm，该参数包含了更新后的节点位置信息。返回一个布尔值，表示更新是否成功。

bool SwarmGraph::updateGraph( const std::vector<Eigen::Vector3d> &swarm){ 

    //Update the vertices
    nodes = swarm; //将当前节点位置赋值给类的私有成员变量nodes，更新蜂群图的节点位置。
    
    if(nodes.size() != nodes_des.size()){
        std::cout <<"swarm size : " << nodes.size() << std::endl;
        ROS_WARN("Size of swarm formation vector is incorrect. ");
        return false;
    }//检查图的节点数量是否与期望图的节点数量相等。如果不相等，则输出警告并返回false表示更新失败。

    //计算特征矩阵
    calcMatrices(nodes, A, D, Lhat);

    if( have_desired ){//是否存在期望图

        DLhat = Lhat - Lhat_des;//计算对称归一化拉普拉斯矩阵的差异，存储在DLhat中。

        //Update the gradient vector
        Eigen::Vector3d gradp;
        agent_grad.clear();//清空agent_grad向量
        for( int idx = 0; idx < nodes.size(); idx++ ){
            calcFGrad(gradp, idx);//计算每个节点的梯度
            agent_grad.push_back(gradp);//将梯度存储在agent_grad向量中
        }
    }else{
        ROS_WARN( "Please enter the desired formation!!" );
    }
    
    return true;
}

setDesiredForm 函数的作用是设置期望的蜂群图节点，并根据期望节点位置计算相关的特征矩阵，同时更新类的私有成员变量。

bool SwarmGraph::setDesiredForm( const std::vector<Eigen::Vector3d> &swarm_des ){
    
    //Save the initial desired nodes
    if( !have_desired )
        nodes_des_init = swarm_des;

    nodes_des = swarm_des;
    calcMatrices(swarm_des, A_des, D_des, Lhat_des);
    have_desired = true;
    return have_desired;
}   

calcMatrices

计算图的邻接矩阵(Adj)，入度矩阵(Deg)和对称归一化拉普拉斯矩阵(SNL)，函数实现：

1. 初始化邻接矩阵、度矩阵和对称归一化拉普拉斯矩阵，将它们的元素值都设为0
2. 使用两层循环遍历蜂群图的节点，计算节点之间的距离，并将其存储在邻接矩阵Adj中。同时，计算每个节点的度并存储在度矩阵Deg中。
3. 使用两层循环遍历蜂群图的节点，计算对称归一化拉普拉斯矩阵SNL。对角线元素为1，非对角线元素根据节点之间的距离、度以及归一化因子计算得到。

bool SwarmGraph::calcMatrices( const std::vector<Eigen::Vector3d> &swarm,
                       Eigen::MatrixXd &Adj, Eigen::VectorXd &Deg,
                       Eigen::MatrixXd &SNL )
{   
    //Init the matrices
    Adj = Eigen::MatrixXd::Zero( swarm.size(), swarm.size() );
    Deg = Eigen::VectorXd::Zero( swarm.size() );
    SNL = Eigen::MatrixXd::Zero( swarm.size(), swarm.size() );

    //Adjacency and Degree
    for( int i = 0; i < swarm.size(); i++ ){
        for( int j = 0; j < swarm.size(); j++ ){
            Adj(i,j) = calcDist2( swarm[i], swarm[j] );
            Deg(i) += Adj(i,j);
        }
    }

    //Symmetric Normalized Laplacian
    for( int i = 0; i < swarm.size(); i++ ){
        for( int j = 0; j < swarm.size(); j++ ){
            if( i == j){
                SNL(i,j) = 1;
            }else{
                SNL(i,j) = -Adj(i,j) * pow(Deg(i),-0.5) * pow(Deg(j),-0.5);
            }
        }
    }
    
    return true;
}

function [Adj, Deg, SNL] = calcMatrices(swarm)
    n = size(swarm, 1);
    
    % 初始化矩阵
    Adj = zeros(n);
    Deg = zeros(n, 1);
    SNL = zeros(n);
    
    % 计算邻接矩阵和度矩阵
    for i = 1:n
        for j = 1:n
            Adj(i, j) = calcDist2(swarm(i, :), swarm(j, :));
            Deg(i) = Deg(i) + Adj(i, j);
        end
    end
    
    % 计算对称归一化拉普拉斯矩阵
    for i = 1:n
        for j = 1:n
            if i == j
                SNL(i, j) = 1;
            else
                SNL(i, j) = -Adj(i, j) * sqrt(1 / (Deg(i) * Deg(j)));
            end
        end
    end
end

calcDist2

计算两个三维向量之间的E-范数距离的平方。

double SwarmGraph::calcDist2( const Eigen::Vector3d &v1, const Eigen::Vector3d &v2){
    return (v1-v2).cwiseAbs2().sum();   
}

function dist_squared = calcDist2(v1, v2)
    dist_squared = (norm(v1 - v2)).^2;
end