移动机器人运动规划避障的一些算法代码

移动机器人的避障和运动规划算法代码，包括D算法A*算法基于模糊控制的算法等

// AStart.cpp : 定义控制台应用程序的入口点。
//

#include “stdafx.h“
#include “math.h“
#include “list“
using namespace  std;
/*
a->a1->a2->a3    b
A*:
	把地图分成一个一个的格子；
	A* 算法有一个评估函数： F=G+H
	F: 从A到B的路径长度
	G: 从起点，沿着产生的路径，移动到指定点的耗费；
	H:预估值，从a到B的预估值
A*：A*需要两个表保存数据，一个启动列表一个关闭列表；

1.把A加到启动列表中；
2. 在启动列表中找到权值最小的格子，在当前格子周围只要能走且不再启动列表和关闭列表中就把它加入启动列表中，
   并且把周围格子的父节点设置为A， 把A从启动列表中删除，并把他放到关闭列表；
3. 直到终点在启动列表中找到了！ 如果启动列表中没有方格饿了，说名没有找到：
*/

//定义每个格子的宽度
#define LatticeLen 10
//用1表示阻断点，3表示起始点，4表示终点，2表示路径~
//虚拟一个地图出来
int G_PathLattice[10][10]=
{
	{ 0 0 0 0 0 0 0 0 0 0 }
	{ 0 0 0 0 1 0 0 0 0 0 }
	{ 0 0 0 0 1 0 1 0 0 0 }
	{ 0 0 0 0 1 0 0 0 0 0 }
	{ 0 0 3 0 1 0 1 4 0 0 }
	{ 0 0 0 0 1 1 0 0 0 0 }
	{ 0 0 0 0 0 0 0 0 0 0 }
	{ 0 0 0 0 0 0 0 0 0 0 }
	{ 0 0 0 0 1 0 0 0 0 0 }
	{ 0 0 0 0 1 0 0 0 0 0 }
};

struct stNode
{
	int row;
	int rant;
	int f;
	int g;
	int h;
	struct stNode* pParent; //当前节点的前一个节点
};


int Distance（int row1 int rank1 int row2 int rank2）
{
	//得到格子中心点的坐标
	int x1 = rank1*LatticeLen + LatticeLen / 2;
	int y1 = row1*LatticeLen + LatticeLen / 2;

	int x2 = rank2*LatticeLen + LatticeLen / 2;
	int y2 = row2*LatticeLen + LatticeLen / 2;

	//计算2个格子的中心坐标
	return （int）sqrt（（float）（x1 - x2）*（x1 - x2） + （y1 - y2）*（y1 - y2））;
}

//判断一个节点是否在俩目标中
bool IsNodeInList（stNode* pNode listnodeList）
{
	for （list::iterator it = nodeList.begin（）; it != nodeList.end（）; it++）
	{
		if （pNode->row == （*it）->row && pNode->rant == （*it）->rant）
		{
			return true;
		}
	}
	return false;
}
stNode* GetNearestNode（listnodeList）
{
	stNode* pNode = NULL; 
	int tempF = 10000000;  //随便设置一个大数
	for （list::iterator it = nodeList.begin（）; it != nodeList.end（）; it++）
	{
		//比较F值， 如果当前点的F小于tempF
		if （（*it）->f < tempF）
		{
			pNode = （*it）;
			tempF = （*it）->f;
		}
	}
	return pNode;
}
//pNode 周围的点放到ListNear 中， 要求点不在启动列表和关闭列表，且不能是障碍点
void GetNearNodeList（stNode* pNode list & listNear
	list listStart list listEnd stNode* pEndNode）
	
{
	//把一个节点旁边的8个点3*3的格子放到ListNear
	for （int i = -1; i <= 1; i++）
	{
		for（int j = -1; j <= 1; j++）
		{
			//点自己不加
			if （i == 0 && j == 0） 
			{
				continue;
			}
			//得到周围点的座标
			int rowTemp = pNode->row + i;
			int rankTemp = pNode->rant + j;

			//越界的格子不加
			if （rowTemp < 0 || rowTemp > 9 ||
				rankTemp <0 || rankTemp > 9）
			{
				continue;
			}
			//阻断点不加
			if （G_PathLattice[rowTemp][rankTemp] == 1）
			{
				continue;
			}

			//在开启表
			stNode node;
			node.row = rowTemp;
			node.rant = rankTemp;
			if （IsNodeInList（&node listStart））
			{
				continue;
			}
			if （IsNodeInList（&node listEnd））
			{
				continue;
			}

			stNode* pNearNode = new stNode;
			pNearNode->g = pNode->g + Distance（pNode->row pNode->rant rowTemp rankTemp）;
			pNearNode->h = Distance（rowTemp rankTemp

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

    .......       341  2015-10-30 17:25  算法\RRT\RRT-master\.gitignore

    .......      1082  2015-10-30 17:25  算法\RRT\RRT-master\LICENSE

    .......     10067  2015-10-30 17:25  算法\RRT\RRT-master\map.mat

    .......      6168  2015-10-30 17:25  算法\RRT\RRT-master\maze.mat

    .......       531  2015-10-30 17:25  算法\RRT\RRT-master\README.md

    .......    119843  2015-10-30 17:25  算法\RRT\RRT-master\Report\map.png

    .......     31986  2015-10-30 17:25  算法\RRT\RRT-master\Report\maze.png

    .......    305772  2015-10-30 17:25  算法\RRT\RRT-master\Report\RRTAlgorithm.png

    .......    263741  2015-10-30 17:25  算法\RRT\RRT-master\Report\RRTAlgorithm2.png

    .......     54450  2015-10-30 17:25  算法\RRT\RRT-master\Report\rrtMap.png

    .......     95920  2015-10-30 17:25  算法\RRT\RRT-master\Report\rrtMaze.png

    .......    279487  2015-10-30 17:25  算法\RRT\RRT-master\Report\rrtSmoothing.png

    .......       877  2015-10-30 17:25  算法\RRT\RRT-master\Report\RRT_Report.out

    .......    880025  2015-10-30 17:25  算法\RRT\RRT-master\Report\RRT_Report.pdf

    .......     13437  2015-10-30 17:25  算法\RRT\RRT-master\Report\RRT_Report.tex

    .......     61275  2015-10-30 17:25  算法\RRT\RRT-master\Report\smoothMap.png

    .......     78178  2015-10-30 17:25  算法\RRT\RRT-master\Report\smoothMaze.png

    .......      9860  2015-10-30 17:25  算法\RRT\RRT-master\rrt.m

    .......    528039  2015-10-30 17:25  算法\RRT\RRT-master\RRT.pdf

    .......      1793  2015-10-30 17:25  算法\RRT\RRT-master\RRT_Test_script.m

    .......      3156  2015-10-30 17:25  算法\RRT\RRT-master\smooth.m

     文件    2611961  2018-10-11 22:34  算法\RRT\RRT-master.zip

     文件       1350  2018-10-11 22:48  算法\基于神经网络的\FZ2.m

     文件        743  2018-10-11 22:40  算法\基于神经网络的\matlan仿真程序.rar

     文件      35405  2010-04-16 17:58  算法\基于神经网络的\避障小车\qlearningcar.mdl

     文件      18184  2010-04-16 14:12  算法\基于神经网络的\避障小车\QlearningCar.wrl

     文件        727  2018-10-11 22:52  算法\基于神经网络的\避障小车\slprj\_jitprj\jitEngineAccessInfo.mat

     文件      85612  2018-10-11 22:52  算法\基于神经网络的\避障小车\slprj\_jitprj\sqFagFM239bDhDBno52naZ.l

     文件       1445  2018-10-11 22:52  算法\基于神经网络的\避障小车\slprj\_jitprj\sqFagFM239bDhDBno52naZ.mat

     文件        726  2018-10-11 22:52  算法\基于神经网络的\避障小车\slprj\_sfprj\EMLReport\emlReportAccessInfo.mat

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