基于PSO移动机器人路径规划算法

使用vc编写PSO算法，实现粒子的寻优，其中运用了龙格库塔进行轨迹优化。

// MyPSO.cpp: implementation of the MyPSO class.
//
//////////////////////////////////////////////////////////////////////

#include “stdafx.h“
#include “MyPSO.h“
#include “math.h“
#include 
#include 
// #include “PARTICLE.h“

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

void MyPSO::PSOSimInitial（）
{
	int i;
	for （i=0;i<4;i++）
	{
		RKF[i]				=	0;
	}

	for （i=0;i<2;i++）
	{
		RKR[i]				=	0;
	}

	RKF[0] = 1;
	RKF[1] = 1;			// 对象的初始状态


	m_iType			= 1;			// 从第一个系统开始切换
	m_iStartTime	= 0;			// 起始时间
	m_iEndTime		= 2;			// 结束时间
	m_iSimSumStep	= 4000;			// 仿真总步数
	m_dSwitchTime	= 0;
	J  = 0;
	Ju = 0;
}

double MyPSO::GetFit（PARTICLE &p） //适合度计算方法，必须定义
{
	J  = 0;
	Ju = 0;
	m_iType = 1;


	RKF[0] = 1;
	RKF[1] = 1;			// 对象的初始状态
	
	double ControlValue;										// 控制量
	double MinStepTime;											// 最小步长时间
	MinStepTime = （（double）2 / m_iSimSumStep）;




	int phase;				// 当粒子维度为43时，一共均分了40段，用来表示当前在哪一段
	int phaseStepNum;		// 每一段的步数


	double slope;			// 线性插值的斜率
	double phasetime;		// 每一段的时间

	int switchi = 0;

	for （int i=0; i	{
		if （i*MinStepTime > p.X[PDim-1] && m_iType != 2）			// p.X[PDim-1] 表示该粒子的切换时刻当仿真时间大于切换时刻时，进入system2
		{
			m_iType = 2;
			switchi = i;
		}

		if （m_iType == 1）			// 当处于system1时，求当前的控制量
		{

			phasetime = p.X[PDim-1]/20;				// 每一个时间段的长度
			phaseStepNum = phasetime/MinStepTime;
			phase = （int）（i*MinStepTime/phasetime）;	// 表示当前时刻在哪个段
			
			slope = （p.X[phase+1]-p.X[phase]）/phaseStepNum;
			ControlValue = p.X[phase]+slope*（i-phaseStepNum*phase）;						
		}

		if （m_iType == 2）
		{
			phasetime = （m_iEndTime-p.X[PDim-1]）/20;				// 每一个时间段的长度
			phaseStepNum = phasetime/MinStepTime;
			phase = （int）（（i-switchi）*MinStepTime/phasetime）;		// 表示当前时刻在哪个段
			slope = （p.X[phase+21+1]-p.X[phase+21]）/phaseStepNum;
			ControlValue = p.X[phase+21]+slope*（i-switchi-phaseStepNum*phase）;	
		}

		ControlValue = p.X[phase]+slope*（i-phaseStepNum*phase）; // 求得当前的控制量

		CalRK4（ControlValue）;									// 进行龙格库塔仿真

		Ju += ControlValue*ControlValue/m_iSimSumStep;			// 控制量平方积分相
	}
	

	J = （Ju + （RKF[0]-10）*（RKF[0]-10） + （RKF[1]-6）*（RKF[1]-6））/2;

	return J;

}

void MyPSO::CalRK4（double ControlValue）
{
	RKR[0] = ControlValue;
	RBody.Setting（2 DiffBody）;											//设置RK4类的值
	RBody.step（m_iType2.0/4000RKFRKR2）;						//计算龙格库塔
}

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       6306  2011-12-21 09:33  PSOproject\Debug\MyPSO.obj

     文件       5022  2011-12-21 09:33  PSOproject\Debug\PARTICLE.obj

     文件      25866  2011-12-21 09:33  PSOproject\Debug\PSO.obj

     文件     254028  2011-12-21 09:33  PSOproject\Debug\PSOproject.exe

     文件     289452  2011-12-21 09:33  PSOproject\Debug\PSOproject.ilk

     文件      11537  2011-12-21 09:33  PSOproject\Debug\PSOproject.obj

     文件     188624  2011-12-21 09:33  PSOproject\Debug\PSOproject.pch

     文件     500736  2011-12-21 09:33  PSOproject\Debug\PSOproject.pdb

     文件       8270  2011-12-21 09:33  PSOproject\Debug\RK4.obj

     文件       4934  2011-12-21 09:33  PSOproject\Debug\StdAfx.obj

     文件      66560  2012-05-19 02:20  PSOproject\Debug\vc60.idb

     文件      77824  2011-12-21 09:33  PSOproject\Debug\vc60.pdb

     文件       2751  2009-03-06 15:40  PSOproject\MyPSO.cpp

     文件       3387  2012-03-01 08:32  PSOproject\MyPSO.dsp

     文件        516  2012-05-18 00:22  PSOproject\MyPSO.dsw

     文件       1307  2012-05-19 02:14  PSOproject\MyPSO.h

     文件      33792  2012-05-18 00:23  PSOproject\MyPSO.ncb

     文件      53760  2012-05-18 00:23  PSOproject\MyPSO.opt

     文件        878  2009-03-05 10:22  PSOproject\PARTICLE.cpp

     文件        787  2009-03-05 10:22  PSOproject\PARTICLE.h

     文件       5545  2009-06-08 10:40  PSOproject\PSO.cpp

     文件       2053  2009-03-05 16:00  PSOproject\PSO.h

     文件        942  2009-03-09 09:31  PSOproject\PSOproject.cpp

     文件       5274  2012-05-19 02:22  PSOproject\PSOproject.dsp

     文件        543  2009-03-05 10:16  PSOproject\PSOproject.dsw

     文件     115712  2012-05-19 02:22  PSOproject\PSOproject.ncb

     文件      99328  2012-05-19 02:22  PSOproject\PSOproject.opt

     文件        254  2012-05-19 02:14  PSOproject\PSOproject.plg

     文件       1232  2009-03-05 10:16  PSOproject\ReadMe.txt

     文件    1132230  2012-05-19 02:24  PSOproject\Result\a.dat

............此处省略11个文件信息