ycsf.c

通过两点之间的距离来实现多点之间的相互定位

// 基于遗传算法的多目标定位C实现.
#include "stdafx.h"
#include "Math.h"
#include "stdio.h"
#include "iostream.h"
#include "stdlib.h"

//m为目标数，n为定位器数
double  mb[8][2]={
        { 11.0, 19.0 },
        { 11.0, 32.0 },
        { 21.0, 30.0 },
        { 1.0, 9.0 },
        { 12.0, 7.0 },
        { 24.0, 17.0 },
        { 6.0, 7.0 },
        { 18.0, 34.0 }};
double  mb1[8][2];
double  dwq[3][2]={
        { 0.0, 0.0 },
        { 21.0, 12.0},
        { 19.0, 27.0}};
const   int     m=8, n=3,
                genenum=500;
int     gene[genenum][m*n],
        genenext[genenum][m*n],
        j, i;

///下面的函数把第cn个定位器与第cm个目标的角度求出，以后的运算只用目标角度，不用目标坐标
double  cor( int cm, int cn )
{
        return  atan2( mb[cm][1]- dwq[cn][1],
                       mb[cm][0]- dwq[cn][0]);
}

////下面的函数判断GENE生成的是否符合规则（各基因段不能相同）返回1为正确生成  
int     ifgene(int i3)
{
        int     vgene[m], flag=1;           
        for( int i1=0; i1<m; i1++ )
        {  
                vgene[i1]= 0;
                for( int j1=0; j1<n; j1++ )  
                vgene[i1]= vgene[i1]*m+ gene[i3][j1+n*i1];   
        }
        for (int i2=0;i2<m;i2++)
        { 
                for (int j1=0;j1<m;j1++)
                {
                        if (i2!=j1) 
                        {
                                if (vgene[i2]==vgene[j1])
                                        flag=0;
                        }
                }
        }

        return flag;
}

//////以下根据基因代表的角度求目标的坐标，ti表示第ti个基因，tm表示第tm个基因段得出的坐标
double  targetx( int ti, int tm )
{
        double  ta=0.0, tx=0.0, tt=0.0;
        int     ti1, tj1;

        for ( ti1=0+tm*n; ti1<tm*n+n; ti1++ )
        {
                for ( tj1=ti1+1; tj1<tm*n+n; tj1++ )
                        ta= ta +
                              sin( cor(gene[ti][tj1], tj1%n)- cor(gene[ti][ti1], ti1%n) )
                            * sin( cor(gene[ti][tj1], tj1%n)- cor(gene[ti][ti1], ti1%n) );
        }
        for (ti1=0+tm*n; ti1<tm*n+n; ti1++)
        {  
                double  tt=0.0;
                for (tj1=tm*n+0; tj1<tm*n+n; tj1++)
                {
                        tt= tt +
                              cos( cor(gene[ti][tj1], tj1%n) )
                            * sin( -cor(gene[ti][tj1],tj1%n) + cor(gene[ti][ti1], ti1%n) );
                }
                tx= tx+ tt* (
                            dwq[ti1%n][0] * sin( cor(gene[ti][ti1], ti1%n) )
                          - dwq[ti1%n][1] * cos( cor(gene[ti][ti1], ti1%n) )
                            );
        }
        return tx/ta;
}

//yy-
double  targety(int ti,int tm)
{
        double  ta=0.0, ty=0.0, tt=0.0;
        int     ti1, tj1;
        for (ti1=0+tm*n; ti1<tm*n+n; ti1++)
        {
                for (tj1=ti1+1; tj1<tm*n+n; tj1++)
                        ta= ta + sin( cor(gene[ti][tj1], tj1%n)- cor(gene[ti][ti1], ti1%n) )
                               * sin( cor(gene[ti][tj1], tj1%n)- cor(gene[ti][ti1], ti1%n) );
        }
        for (ti1=0+tm*n; ti1<tm*n+n; ti1++)
        {  
                double  tt=0.0;
                for (tj1=tm*n+0; tj1<tm*n+n; tj1++)
                {
                        tt= tt + sin( cor(gene[ti][tj1], tj1%n) )
                               * sin(-cor(gene[ti][tj1], tj1%n)
                               + cor( gene[ti][ti1], ti1%n) );
                }
                ty= ty + tt*(
                         dwq[ti1%n][0] * sin( cor(gene[ti][ti1], ti1%n) )
                       - dwq[ti1%n][1] * cos( cor(gene[ti][ti1], ti1%n) )
                            );
        }
        return ty/ta;
}

//////以下求基因段的适应度的倒数，di是表示第di个基因，dm表示第dm基因段
double depiece(int di,int dm)
{
        double dgtt=0.0;
        for (int i4=0;i4<n;i4++)
        {
                dgtt= dgtt+ 
                      (   ( targetx(di, dm)- dwq[i4][0] )* sin( cor(dm, i4) )
                        - ( targety(di, dm)- dwq[i4][1] )* cos( cor(dm, i4) )
                      )
                    * (   ( targetx(di, dm)- dwq[i4][0] )* sin( cor(dm, i4) )
                        - ( targety(di, dm)- dwq[i4][1] )* cos( cor(dm, i4) )
                      ); 
        }
        if (dgtt<0.0000000001)
                dgtt=0.00000000001;
        return dgtt;
}

////主函数-----------------------
void main() 
{
        int     endflag=-1,
                runn=0,
                runmax=1000,
                count=0;

        // 基因段共有同M的N次方个，以下生成基因数genenum=100个    
        for ( i=0; i<genenum; i++)
        {
                for ( j=0; j<m*n; j++ ) 
                {
                        gene[i][j]= rand()%m;  
                } 
                if ( ifgene(i)== 0 )
                {
                        i=i-1;
                }
        }

        ///以下求各基因的适应度，如果适应度大于demax>10,认为些基因为最佳基因
        double  degree[genenum],
                degtt,
                degreeall= 0.0; 
        int     imax= 0;
        for ( i=0; i<genenum; i++ )
        {
                degtt= 0.0;
                for ( j=0; j<m; j++ )
                {
                        degtt= degtt+ depiece( i, j ); 
                }
                degree[i]= 1/degtt;

                // cout<<"degree[i]degree[i]degree["<<i<<"]"<<degree[i]<<endl;
                
                if ( degree[i] > degree[imax] )
                        imax= i;
                if ( degree[i] > 10 )
                        endflag= i;
                degreeall= degreeall+ degree[i]; 

                // cout<<"degreealldegreeall"<<degreeall<<endl; 
        }

        while( (endflag==-1) && (runn < runmax) )
        {    
                ///以下对基因进行选择,deselect为根据适应度求出的选择参数,

                double  deselect[genenum],
                        destt=0.0,
                        randnum;
                imax=0;
                for ( i=0; i<genenum; i++ )
                {
                        deselect[i]= 0.0;            
                        deselect[i]= destt+ ( degree[i]* 10000/degreeall ); 
                        destt= deselect[i];

                        // cout<<"degree[i]"<<degree[i]<<endl; 
                        // cout<<"degreeall"<<degreeall<<endl; 
                        // cout<<"deselect[i]"<<deselect[i]<<endl; 
                }

                for ( i=0; i<genenum; i++ )
                {
                        j= 0;
                        randnum= rand()* 10000.0/( 128* 256- 1 );
                        do
                        { 
                                for (int k=0;k<m*n;k++)
                                        genenext[i][k]=gene[j][k]; 
                                j= j+1;
                        } while( (j < genenum) && (randnum > deselect[j-1]) );                

                        // cout<<"jjjjjjjjj:"<<j<<endl;
                        // cout<<"randnum"<<randnum<<endl; 
                        // cout<<"jdeselect[j-1]"<<deselect[j-1]<<endl;
                }

                /////选择出的前10%与后10%进行交叉，10%变异，生成新一代genenext[][]
     
                for (i=0;i<genenum*0.1;i++)
                { 
                        int     randchange= rand()% m* n;          
                        for ( j=0; j<m*n; j++ ) 
                        {
                                if ( j < randchange )
                                {
                                        gene[0][j]= genenext[i][j];
                                        gene[1][j]= genenext[genenum-i-2][j];
                                }
                                else
                                {
                                        gene[0][j]= genenext[genenum-i-2][j];
                                        gene[1][j]= genenext[i][j];
                                }
                        } 
                        if ( (ifgene(0)==0) || (ifgene(1)==0) )
                                i= 1-1;
                        else
                                for ( j=0; j<m*n; j++)  
                                {
                                        genenext[i][j]= gene[0][j];
                                        genenext[genenum-i-2][j]= gene[1][j]; 
                                } 
                        count= count+1;
                        if ( count >= 5 )
                        { 
                                i= i+1;
                                count=0;
                        }
                }

                /////选择10%变异
                count=0;
                for ( i=0; i<genenum*0.1; i++ )
                { 
                        int     randi= rand()% genenum,
                                randj= rand()% m* n,
                                randt= rand()% m;
                        for ( j=0; j<m*n; j++)  
                        {
                                gene[0][j]= genenext[randi][j]; 
                        }
                        gene[0][randj]=randt;
                        if ( ifgene(0) == 0 )
                                i= 1-1;
                        else
                                for ( j=0; j<m*n; j++ )  
                                        genenext[randi][j]= gene[0][j];
                        count=count+1;
                        if ( count >= 5 )
                        { 
                                i= i+1;
                                count= 0;
                        } 
                }

                ///把下代基因作为上代进行迭代
                for ( i=0; i<genenum; i++ )
                {
                        for ( j=0; j<m*n; j++ ) 
                        {
                                gene[i][j]= genenext[i][j];
                        }
                }
                //以下求各基因的适应度，如果适应度大于demax>10000,认为些基因为最佳基因
                degreeall= 0.0;
                for ( i=0; i<genenum; i++ )
                {
                        degtt= 0.0;
                        for ( j=0; j<m; j++ )
                        {
                                degtt= degtt+ depiece(i,j); 
                        }
                        degree[i]= 1/degtt;
                
                        // cout<<"degree[i]degree[i]degree["<<i<<"]"<<degree[i]<<endl;

                        if ( degree[i] > degree[imax] )
                                imax=i;
                        if ( degree[i] > 10 )
                                endflag=i;
                        degreeall= degreeall+ degree[i]; 
                }
                runn=runn+1;
        }

        ///求出目标并输出 //
       for (endflag=0;endflag<genenum;endflag++)
        {
                if ( endflag == -1 )
                        endflag= imax;
                for ( i=0; i<m; i++ )
                {
                        mb1[i][0]= targetx( endflag, i );
                        mb1[i][1]= targety( endflag, i );   
                        cout<<"目标"<<i<<":"<<mb1[i][0]<<","<<mb1[i][1];
                }
                cout<<endl;
        }
        cout<<"迭代次数为"<<runn<<endl;
}