蚁群算法C++版本

参考

蚁群算法及MATLAB实例仿真

一张效果图

ants_plan.png

其他

目前关于那些交叉的地方会后面会直接从几何上面去进行判交，然后把相交的重新连线。

头文件

Ants.h

/** ***************************************************************************
 * @file CAnts.h
 * @author  003(you@domain.com)
 * @brief 蚁群算法头文件
 * @version 0.1
 * @date 2022-03-24
 *
 * @copyright Copyright (c) 2022
 *
 * ************************************************************************** */

#ifndef ANTS_H
#define ANTS_H


#ifdef CORE_H

#else
template <typename _T>
struct Point2_
{
    _T x;
    _T y;

    Point2_() : x((_T)0.0), y((_T)0.0) {}
    Point2_(_T _x, _T _y) : x(_x), y(_y) {}

    /**
     * @brief 向量或点绕中心点旋转
     *
     * @param dAngle 旋转角度，弧度制
     * @param pOri 原点
     * @return Point2_<_T>
     */
    inline Point2_<_T> Rotate(double dAngle, const Point2_<_T> &pOri = Point2_<_T>((_T)0.0, (_T)0.0)) const
    {
        return Point2_<_T>(pOri.x + (x - pOri.x) * cos(dAngle) - (y - pOri.y) * sin(dAngle),
                           pOri.y + (x - pOri.x) * sin(dAngle) + (y - pOri.y) * cos(dAngle));
    }

    inline Point2_<_T> operator+(const Point2_<_T> &pt) const
    {
        return Point2_<_T>(this->x + pt.x, this->y + pt.y);
    }

    template <typename _T2>
    inline Point2_<_T> operator+(const Point2_<_T2> &pt) const
    {
        return Point2_<_T>(this->x + (_T)pt.x, this->y + (_T)pt.y);
    }

    inline Point2_<_T> operator-(const Point2_<_T> &pt) const
    {
        return Point2_<_T>(this->x - pt.x, this->y - pt.y);
    }

    inline Point2_<_T> operator*(const _T d) const
    {
        return Point2_<_T>(x * d, y * d);
    }

    inline Point2_<_T> operator/(const _T d) const
    {
        return Point2_<_T>(x / d, y / d);
    }

    friend std::ostream &operator<<(std::ostream &os, Point2_<_T> &pt)
    {
        os << "(" << pt.x << "," << pt.y << ")";
        return os;
    }
};

typedef Point2_<int> Pointi;
typedef Point2_<float> Pointf;
typedef Point2_<double> Pointd;
#endif

/** ***************************************************************************
 * @brief
 *
 * @param[in] vCitys 所有城市点的坐标
 * @param[out] vListNums 按顺序应该走哪一个城市
 * @param[out] dMinDistance 最短路径的路径长度
 * @param[in] nAntsNums 蚂蚁数量
 * @param[in] fAlpha    信息素重要程度参数
 * @param[in] fRho      信息蒸发参数
 * @param[in] nMaxIterator  最大迭代次数
 * @param[in] fQ        信息素增加强度系数
 * @return int
 * ************************************************************************** */
int ants(
    std::vector<Pointf> &vCitys,
    std::vector<int> &vListNums,
    double &dMinDistance,
    int nAntsNums = 20,
    float fAlpha = 1,
    float fBeta = 5,
    float fRho = 0.3,
    int nMaxIterator = 400,
    float fQ = 100);

#endif // ANTS_H

源文件

Ants.cpp

/** ***************************************************************************
 * @file CAnts.cpp
 * @author 003 (you@domain.com)
 * @brief 蚁群算法实现文件
 * @version 0.1
 * @date 2022-03-24
 *
 * @copyright Copyright (c) 2022
 *
 * ************************************************************************** */

#include "Ants.h"
#include <assert.h>
/** ***************************************************************************
 * @brief Get the Distance Mat object
 * 获得距离矩阵,指针要在外面申请好内存才行
 * @param[in] vCitys
 * @param[out] pdDistance
 * @return int
 * ************************************************************************** */
int getDistanceMat(std::vector<Pointf> &vCitys, double *pdDistance)
{
    //检查
    if (pdDistance == nullptr)
    {
        return 1;
    }

    int n = vCitys.size();

    if (n == 0)
    {
        return 2;
    }

    for (size_t i = 0; i < n; i++)
    {
        pdDistance[i * n + i] = -1; //自身向自身走的路径距离设为负数，并在使用的时候的判断当中排除掉
        for (size_t j = i + 1; j < n; j++)
        {
            float x_delta = vCitys[i].x - vCitys[j].x;
            float y_delta = vCitys[i].y - vCitys[j].y;
            pdDistance[i * n + j] = std::sqrt(std::pow(x_delta, 2) + std::pow(y_delta, 2));
            pdDistance[j * n + i] = pdDistance[i * n + j];
        }
    }

    return 0;
}

/** ***************************************************************************
 * @brief Set the Tau Mat object
 * 设置信息素矩阵为指定初始值
 * @param[out] pdTau 信息素矩阵指针
 * @param[in] value 需要设置的值
 * @param[in] n 指针总长度
 * @return int
 * ************************************************************************** */
int setTauMat(double *pdTau, double value, int n)
{
    for (size_t i = 0; i < n; i++)
    {
        pdTau[i] = value;
    }

    return 0;
}

/** ***************************************************************************
 * @brief Set the Tabu Mat object
 * 设置禁忌表矩阵为指定值
 * @param[in] pdTabu 禁忌表
 * @param[in] value 指定值
 * @param[in] n 表长度
 * @return int
 * ************************************************************************** */
int setTabuMat(int *pdTabu, int value, int n)
{
    for (size_t i = 0; i < n; i++)
    {
        pdTabu[i] = value;
    }

    return 0;
}

/** ***************************************************************************
 * @brief Get the Eta Mat object
 * 获取Eta矩阵，初始设置为距离矩阵的倒数
 * @param[in] pdDistance
 * @param[out] pdEta
 * @param[in] n 矩阵总长度
 * @return int
 * ************************************************************************** */
int getEtaMat(double *pdDistance, double *pdEta, int n)
{
    if (pdDistance == nullptr || pdEta == nullptr)
    {
        return 3;
    }

    for (size_t i = 0; i < n; i++)
    {
        if (pdDistance[i] < 0)
        {
            pdEta[i] = -1;
        }
        else
        {
            pdEta[i] = 1 / pdDistance[i];
        }
    }

    return 0;
}

/** ***************************************************************************
 * @brief 生成min到max之间的不重复的随机整数序列
 * 指针长度为max-nin+1
 * @param[in] min 最小值
 * @param[in] max 最大值
 * @return int*
 * ************************************************************************** */
int *randperm(int min, int max)
{
    if (max < min)
    {
        std::swap(max, min);
    }

    int n = max - min + 1;
    int *p = new int[n];
    for (size_t i = 0; i < n; i++)
    {
        p[i] = min + i;
    }

    for (int i = 1; i < n; i++)
    {
        std::swap(p[i], p[rand() % i]);
    }

#ifdef _DEBUG
    // std::cout << "max = " << max << ", min = " << min << std::endl;
    // for (size_t i = 0; i < n; i++)
    // {
    //     std::cout << p[i] << "\t";
    // }
    // std::cout << std::endl
    //           << std::endl;
#endif
    return p;
}

/** ***************************************************************************
 * @brief 检查城市是否被访问
 *
 * @param[in] nCity 需要检查的城市
 * @param[in] nAnt 需要检查城市的蚂蚁编号
 * @param[in] pnTabu 禁忌表
 * @param[in] nCitys 总共城市数量
 * @param[in] nAntsNums 总共蚂蚁数量
 * @return true 已经被访问了
 * @return false 没有被访问
 * ************************************************************************** */
bool isVisited(int nCity, int nAnt, int *pnTabu, int nCitys /*,int nAntsNums*/)
{
    for (size_t i = 0; i < nCitys; i++)
    {
        if (nCity == pnTabu[nAnt * nCitys + i])
        {
            return true;
        }
    }
    return false;
}

/** ***************************************************************************
 * @brief 计算概率总和
 *
 * @param[in] pdProb
 * @param[in] n
 * @return double
 * ************************************************************************** */
double sumProb(double *pdProb, int n)
{
    double sum = 0.0f;
    for (size_t i = 0; i < n; i++)
    {
        sum += pdProb[i];
    }
    return sum;
}

/** ***************************************************************************
 * @brief   蚁群算法
 *
 * @param[in] vCitys 所有城市点的坐标
 * @param[out] vListNums 按顺序应该走哪一个城市
 * @param[out] dMinDistance 最短路径的路径长度
 * @param[in] nAntsNums 蚂蚁数量
 * @param[in] fAlpha    信息素重要程度参数
 * @param[in] fRho      信息蒸发参数
 * @param[in] nMaxIterator  最大迭代次数
 * @param[in] fQ        信息素增加强度系数
 * @return int
 * ************************************************************************** */
int ants(
    std::vector<Pointf> &vCitys,
    std::vector<int> &vListNums,
    double &dMinDistance,
    int nAntsNums /*= 20*/,
    float fAlpha /*= 1*/,
    float fBeta /*=5*/,
    float fRho /*= 0.3*/,
    int nMaxIterator /*= 400*/,
    float fQ /*= 100*/)
{
    int n = vCitys.size();
    if (n == 0)
    {
        return 2;
    }

    double *pdTau = new double[n * n];      ///<信息素矩阵
    double *pdDistance = new double[n * n]; ///<距离矩阵
    double *pdEta = new double[n * n];      ///<启发因子矩阵
    int *pnTabu = new int[nAntsNums * n];   ///<禁忌表，每个蚂蚁用一行
    int *pnBestRoad = new int[n];           ///<最优路径

    double dBestLength = MAXFLOAT; ///<最短距离

    setTauMat(pdTau, 1.0f, n * n);
    setTabuMat(pnTabu, -1, nAntsNums * n);
    getDistanceMat(vCitys, pdDistance);
    getEtaMat(pdDistance, pdEta, n * n);


    srand(time(NULL));
    //开始迭代
    int nc = 0;
    int antStart = 0; //开始搜寻的蚂蚁编号
    while (nc < nMaxIterator)
    {
        if (nc > 0)
            antStart = 1;
        setTabuMat(pnTabu, -1, nAntsNums * n);
        int P = std::ceil((double)nAntsNums / n); //把蚂蚁随机任意甩到城市集中的城市上
        int *pRandPos = new int[P * n];
        for (size_t i = 0; i < P; i++)
        {
            int *p = randperm(0, n - 1);
            for (size_t j = 0; j < n; j++)
            {
                pRandPos[i * P + j] = p[j];
            }
            delete[] p;
        }
        //更新每只蚂蚁第一轮的禁忌表
        for (size_t i = 0; i < nAntsNums; i++)
        {
            pnTabu[i * n + 0] = pRandPos[i];
        }
        //只要不是第一次迭代，第一只蚂蚁的路径永远使用当前所得的全局最优
        if (nc > 0 && antStart == 1)
        {
            for (size_t j = 0; j < n; j++)
            {
                pnTabu[j] = pnBestRoad[j];
            }
        }

        for (size_t j = 1; j < n; j++) //访问n次
        {
            for (size_t i = antStart /*0*/; i < nAntsNums; i++) // m只蚂蚁
            {
                //获取待访问城市列表
                int jc = 0;
                int *pnJ = new int[n - j + 1];          //未访问城市列表
                double *pdProb = new double[n - j + 1]; //未访问列表对应的概率
                setTabuMat(pnJ, -1, n - j + 1);         //借用禁忌表的函数把pnJ设置为-1
                setTauMat(pdProb, 1.0f, n - j + 1);     //借用信息素的函数把所有的概率设为1
                for (size_t k = 0; k < n; k++)
                {
                    if (!isVisited(k, i, pnTabu, n))
                    {
                        pnJ[jc] = k;
                        jc++;
                    }
                }

                //计算访问概率
                for (size_t k = 0; k < jc; k++)
                {
                    int nLast = pnTabu[i * n + j - 1]; //第i只蚂蚁已经访问过的最后一个城市
                    int nNew = pnJ[k];
                    pdProb[k] = pow(pdTau[nLast * n + nNew], fAlpha) * pow(pdEta[nLast * n + nNew], fBeta);
                }

                //搞成累加的概率
                double dSum = sumProb(pdProb, jc);
                pdProb[0] = pdProb[0] / dSum;
                for (size_t k = 1; k < jc; k++)
                {
                    pdProb[k] = pdProb[k - 1] + pdProb[k] / dSum;
                }
                //这儿的随机整数区间如果太大，会导致收敛太快，迅速陷入局部最优，如果太小，又会导致收敛变慢
                // double rd = rand() / (double)RAND_MAX;
                    
                double rd = rand() %10000/ 10000.0;
                int nSelect(-1);
                for (size_t k = 0; k < jc; k++)
                {
                    if (pdProb[k] > rd)
                    {
                        nSelect = pnJ[k];
                        break;
                    }
                }
                assert(nSelect != -1); //选择的值不应该为-1

                pnTabu[i * n + j] = nSelect;

                delete[] pdProb;
                delete[] pnJ;
            }
        }

        //获取最佳长度和最佳路径
        double *pL = new double[nAntsNums];
        memset(pL, 0, sizeof(double) * nAntsNums);
        for (size_t i = 0; i < nAntsNums; i++)
        {
            for (size_t j = 0; j < n; j++)
            {
                int nLast = pnTabu[i * n + j];
                int nNew = pnTabu[i * n + (j + 1) % n];
                pL[i] += pdDistance[nLast * n + nNew];
            }
        }
        double dLmin(MAXFLOAT);
        int nMinIndex(-1);
        for (size_t i = 0; i < nAntsNums; i++)
        {
            if (dLmin > pL[i])
            {
                dLmin = pL[i];
                nMinIndex = i;
            }
        }

        //得出当前全局最佳
        dBestLength = dLmin;
        for (size_t j = 0; j < n; j++)
        {
            pnBestRoad[j] = pnTabu[nMinIndex * n + j];
        }

#ifdef _DEBUG
        std::cout << "the iter " << nc << " dBestLength = " << dBestLength << std::endl;
// for (size_t j = 0; j < n; j++)
// {
//     std::cout<<pnBestRoad[j]<<"\t";
// }
// std::cout<<std::endl;
#endif

        //更新信息素
        double *pdTauDelta = new double[n * n];
        setTauMat(pdTauDelta, 0.0f, n * n); //把要新增的变动量设为零
        for (size_t i = 0; i < nAntsNums; i++)
        {
            for (size_t j = 0; j < n; j++)
            {
                int nLast = pnTabu[i * n + j];
                int nNew = pnTabu[i * n + (j + 1) % n];
                pdTau[nNew * n + nLast] += fQ / pL[i];
                pdTauDelta[nLast * n + nNew] = pdTau[nNew * n + nLast];
            }
        }
        for (size_t j = 0; j < n * n; j++)
        {
            pdTau[j] = (1 - fRho) * pdTau[j] + pdTauDelta[j];
        }

        nc++;
        delete[] pdTauDelta;
        delete[] pL;
        delete[] pRandPos;
    }

    /** ***************************************************************************
     * @brief 输出最佳路径及其距离
     *
     * ************************************************************************** */
    vListNums.clear();
    vListNums.resize(n);
    for (size_t j = 0; j < n; j++)
    {
        vListNums[j] = pnBestRoad[j];
    }

    //清除指针数组
    delete[] pdTau;
    delete[] pdDistance; ///<距离矩阵
    delete[] pdEta;      ///<启发因子矩阵
    delete[] pnTabu;     ///<禁忌表，每个蚂蚁用一行
    delete[] pnBestRoad; ///<最优路径

    return 0;
}

调用

测试数据直接硬写的，所以那部分显得比较长。
然后里面有绘制结果图的部分，使用了opencv。
main.cpp

/** ***************************************************************************
 * @file Ants_test.cpp
 * @author 003 (you@domain.com)
 * @brief 单独测试蚁群的规划效果的
 * @version 0.1
 * @date 2022-03-29
 *
 * @copyright Copyright (c) 2022
 *
 * ************************************************************************** */
//蚁群头文件
#include "Ants.h"

#include <opencv2/opencv.hpp>

std::vector<Pointf> vCitys = {
    Pointf(23247, 28174),
    Pointf(17528, 62464),
    Pointf(40678, 62420),
    Pointf(29103, 62443),
    Pointf(63832, 62372),
    Pointf(51999, 62395),
    Pointf(86985, 62322),
    Pointf(75405, 62345),
    Pointf(17504, 51474),
    Pointf(28727, 51451),
    Pointf(40657, 51429),
    Pointf(53680, 51402),
    Pointf(63811, 51378),
    Pointf(75382, 51353),
    Pointf(86960, 51326),
    Pointf(17482, 40483),
    Pointf(29054, 40459),
    Pointf(40633, 40085),
    Pointf(52208, 40411),
    Pointf(63785, 40387),
    Pointf(75011, 40362),
    Pointf(86936, 40339),
    Pointf(17107, 29491),
    Pointf(29032, 29468),
    Pointf(40609, 27931),
    Pointf(52183, 29422),
    Pointf(65206, 27847),
    Pointf(75335, 29376),
    Pointf(86915, 29349),
    Pointf(17435, 18503),
    Pointf(29013, 18481),
    Pointf(40589, 18454),
    Pointf(53612, 18432),
    Pointf(63392, 18408),
    Pointf(75311, 18384),
    Pointf(86888, 18357),
    Pointf(17410, 7510),
    Pointf(28987, 7485),
    Pointf(40563, 7459),
    Pointf(52140, 7435),
    Pointf(63365, 7413),
    Pointf(75291, 7390),
    Pointf(86862, 7366),
    Pointf(17424, 13014),
    Pointf(11745, 62536),
    Pointf(11713, 51549),
    Pointf(11667, 29571),
    Pointf(11698, 40629),
    Pointf(11621, 7585),
    Pointf(10205, 22917),
    Pointf(23320, 61148),
    Pointf(23297, 51540),
    Pointf(23271, 41918),
    Pointf(5836, 8070),
    Pointf(23228, 19861),
    Pointf(23207, 8938),
    Pointf(34906, 62502),
    Pointf(34875, 51515),
    Pointf(34855, 40525),
    Pointf(34830, 29534),
    Pointf(34805, 18538),
    Pointf(34780, 7552),
    Pointf(46463, 62470),
    Pointf(46437, 51482),
    Pointf(46413, 40489),
    Pointf(46391, 29508),
    Pointf(46369, 18507),
    Pointf(46345, 7513),
    Pointf(58055, 62454),
    Pointf(58028, 51470),
    Pointf(58006, 40479),
    Pointf(57980, 29490),
    Pointf(57960, 18485),
    Pointf(57936, 7506),
    Pointf(68164, 61055),
    Pointf(69590, 51457),
    Pointf(69568, 40462),
    Pointf(69540, 29475),
    Pointf(69521, 18477),
    Pointf(68057, 8849),
    Pointf(82641, 61027),
    Pointf(81177, 51435),
    Pointf(81156, 40440),
    Pointf(81130, 29452),
    Pointf(82084, 25296),
    Pointf(82534, 8823),
    Pointf(11381, 12905),
    Pointf(7322, 27648),
    Pointf(7301, 15440),
    Pointf(17472, 35123),
    Pointf(57992, 34977),
    Pointf(28846, 24408),
    Pointf(86905, 23908),
    Pointf(9955, 17062),
    Pointf(10198, 20549),
    Pointf(75123, 23375),
    Pointf(5856, 11689),
    Pointf(29066, 46019),
    Pointf(52238, 56969),
    Pointf(5982, 62250),
    Pointf(5975, 58442),
    Pointf(5944, 44686),
    Pointf(17449, 24055),
    Pointf(5958, 35143),
    Pointf(81099, 18552),
    Pointf(5924, 50734),
    Pointf(5879, 29868),
    Pointf(5899, 39838),
    Pointf(40609, 29794),
    Pointf(40634, 42013),
    Pointf(63761, 29747),
    Pointf(40573, 10267),
    Pointf(40670, 59735),
    Pointf(67993, 44618),
    Pointf(82320, 44548),
    Pointf(5223, 20803),
    Pointf(40598, 23049),
    Pointf(52170, 23036),
    Pointf(45914, 13060),
    Pointf(11395, 47571),
    Pointf(57597, 46065),
    Pointf(80693, 13145),
    Pointf(22771, 13229),
    Pointf(22840, 46137),
    Pointf(34423, 46164),
    Pointf(34353, 13037),
    Pointf(45983, 46059)};

int main(int argc, char *argv[])
{
    std::vector<int> vListNums;
    double dMinDistance;

    ants(vCitys, vListNums, dMinDistance);

    /** ***************************************************************************
     * @brief 绘制路线
     *
     * ************************************************************************** */
    double xmin(INT_MAX), ymin(INT_MAX);
    double xmax(INT_MIN), ymax(INT_MIN);
    for (size_t i = 0; i < vCitys.size(); i++)
    {
        xmin = xmin > vCitys[i].x ? vCitys[i].x : xmin;
        xmax = xmax < vCitys[i].x ? vCitys[i].x : xmax;
        ymin = ymin > vCitys[i].y ? vCitys[i].y : ymin;
        ymax = ymax < vCitys[i].y ? vCitys[i].y : ymax;
    }

    cv::Mat mDraw = cv::Mat::zeros(
        cv::Size((xmax - xmin) / 20, (ymax - ymin) / 20), CV_8UC3) + 128;

    int n = vListNums.size();
    for (size_t i = 0; i < n; i++)
    {
        Pointf &pt = vCitys[vListNums[i]];
        Pointf &ptNext = vCitys[vListNums[(i + 1) % n]];
        cv::Rect re((pt.x - xmin) / 20 - 20, (pt.y - ymin) / 20 - 20, 40, 40);
        cv::rectangle(mDraw, re, cv::Scalar(0, 0, 255), -1);
        cv::line(mDraw, cv::Point((pt.x - xmin) / 20, (pt.y - ymin) / 20),
                 cv::Point((ptNext.x - xmin) / 20, (ptNext.y - ymin) / 20), cv::Scalar(0, 255, 0), 10);
    }

    cv::imwrite("ants_plan.png", mDraw);
    return 0;
}