局部路径规划算法——实现DWA（dynamic window approach）控制空间采样

DWA算法是局部路径规划算法，在全局路径规划算法完成后，DWA算法能够根据当前小车（机器人）位置、障碍物、终点的位置进行控制空间（速度、角速度）的采用，从而完成局部路径规划。

DWA算法流程：

初始化一一小车最大最小速度、加速度，评价函数权重等循环
{
判断是否到达目的地
计算当前采样的速度范围(动态窗口)
遍历所有速度v&w，根据模型模拟一段时间的路径根据评价函数打分(包括评价函数、归一化、权重)选取最优解——v&w，下发给运动底盘
小车继续移动
}

下面是DWA算法的代码实现，主要步骤给了注释：
变量定义：

#include <iostream>
#include <vector>

using namespace std;

#define M_PI 3.1415927
#define MAX_VELOCITY 1.0						//弧形轨迹：最大速度
#define MIN_VELOCITY 0							//弧形轨迹：最小速度
#define MAX_OMEGA 20.0 / 180.0 * M_PI			//弧形轨迹：最大角速度
#define MIN_OMEGA 0								//弧形轨迹：最小角速度
#define MAX_ACCELERATE 0.2						//动态窗口：最大加速度
#define MAX_ACCOMEGA 50.0 / 180.0 * M_PI		//动态窗口：最大角加速度
#define SAMPLING_VELOCITY 0.01					//速度采样间隔
#define SAMPLING_OMEGA 1 / 180.0 * M_PI			//角速度采样间隔
#define DT 0.1									//采样时间间隔
#define PREDICT_TIME 3.0						//预测时间
#define WEIGHT_HEADING 0.05						//HEADING权重——小车与终点的角度
#define WEIGHT_CLEARANCE 0.2					//CLEARANCE权重——小车与最近障碍物的距离
#define WEIGHT_VELOCITY 0.1						//VELOCITY权重——小车速度
#define GOAL_X 10								//目标横坐标
#define GOAL_Y 10								//目标纵坐标
#define ROBOT_RADIUS 0.5						//机器人半径

struct RobotState
{
	// x坐标，y坐标，机器朝向，速度，角速度
	float xPosition, yPosition, orientation, velocity, omega;
};

// 障碍物
int obstacle[18][2] = { { 0, 2 },
							{ 4, 2 },
							{ 4, 4 },
							{ 5, 4 },
							{ 5, 5 },
							{ 5, 6 },
							{ 5, 9 },
							{ 8, 8 },
							{ 8, 9 },
							{ 7, 9 },
							{ 6, 5 },
							{ 6, 3 },
							{ 6, 8 },
							{ 6, 7 },
							{ 7, 4 },
							{ 9, 8 },
							{ 9, 11 },
							{ 9, 6 } };

//评估指标与速度采样空间
struct EvaluationPara
{
	float heading, clearance, velocity, v, w;
};

具体实现：

#include <iostream>
#include "Constant.h"
#include <numeric>
#include <fstream>

using namespace std;

vector<float> DynamicWindowApproach(RobotState rState, int obs[][2], int target[]);
RobotState Motion(RobotState curState, float velocity, float omega);
vector<float> CreateDW(RobotState state);
vector<RobotState> GenerateTraj(RobotState initState, float vel, float ome);
float CalcHeading(RobotState rState, int goal[]);
float CalcClearance(RobotState rState, int obs[][2]);
float CalcBreakingDist(float velo);


void main()
{
	// 初始化机器人当前的参数
	RobotState currentState = { 0, 0, M_PI / 2, 0, 0 };
	int goal[2] = { GOAL_X, GOAL_Y };
	vector<RobotState> path;
	cout << "Begin!" << endl;

	int i = 0;

	while (1)
	{
		// 到达目标点退出循环
		if (currentState.xPosition < (goal[0] + 0.1) && currentState.xPosition > (goal[0] - 0.1) && currentState.yPosition < (goal[1] + 0.1) && currentState.yPosition > (goal[1] - 0.1))
		{
			cout << "Reach the Goal!" << endl;
			break;
		}

		//dwa算法选择速度和角速度
		vector<float> selectedVelocity = DynamicWindowApproach(currentState, obstacle, goal);

		// 机器人移动
		currentState = Motion(currentState, selectedVelocity[0], selectedVelocity[1]);

		path.push_back(currentState);

		cout << selectedVelocity[0] << " " << selectedVelocity[1] << " ";

		cout << currentState.xPosition << " " << currentState.yPosition <<  endl;
	}

	//输出坐标到txt
	ofstream file("map.txt");
	for (vector<RobotState>::iterator i = path.begin(); i < path.end(); i++)
	{
		file << i->xPosition << " " << i->yPosition << endl;
	}
	file.close();

	system("pause");

}

//dwa算法实现
vector<float> DynamicWindowApproach(RobotState rState, int obs[][2], int target[])
{
	// 0:minVelocity, 1:maxVelocity, 2:minOmega, 3:maxOmega
	vector<float> velocityAndOmegaRange = CreateDW(rState); //计算小车的速度和角速度变化范围
	vector<EvaluationPara>evalParas; 
	float sumHeading = 0;
	float sumClearance = 0;
	float sumVelocity = 0;

	//遍历速度和角速度区间范围内的量
	for (double v = velocityAndOmegaRange[0]; v < velocityAndOmegaRange[1]; v += SAMPLING_VELOCITY)
	{
		for (double w = velocityAndOmegaRange[2]; w < velocityAndOmegaRange[3]; w += SAMPLING_OMEGA)
		{
			vector<RobotState> trajectories = GenerateTraj(rState, v, w);

			//计算评估参数
			EvaluationPara tempEvalPara;
			//计算距小车最近的障碍物和小车在当前速度行驶的最短距离
			float tempClearance = CalcClearance(trajectories.back(), obstacle);	
			float stopDist = CalcBreakingDist(v);
			//如果在最近的障碍物前能停下来则此速度和角速度可行
			if (tempClearance > stopDist)
			{
				tempEvalPara.heading = CalcHeading(trajectories.back(), target); //小车与指点角度
				tempEvalPara.clearance = tempClearance;
				tempEvalPara.velocity = abs(v);
				tempEvalPara.v = v;
				tempEvalPara.w = w;

				sumHeading = sumHeading + tempEvalPara.heading;
				sumClearance = sumHeading + tempEvalPara.clearance;
				sumVelocity = sumVelocity + tempEvalPara.velocity;

				evalParas.push_back(tempEvalPara); //存入可行状态空间
			}
		}
	}

	//平滑评价参数并选择最优速度
	float selectedVelocity = 0;
	float selectedOmega = 0;
	float G = 0;
	for (vector<EvaluationPara>::iterator i = evalParas.begin(); i < evalParas.end(); i++)
	{
		//归一化
		float smoothHeading = i->heading / sumHeading;
		float smoothClearance = i->clearance / sumClearance;
		float smoothVelocity = i->velocity / sumVelocity;
		
		//计算评估函数并比较选最优
		float tempG = WEIGHT_HEADING*smoothHeading + WEIGHT_CLEARANCE*smoothClearance + WEIGHT_VELOCITY*smoothVelocity;

		if (tempG > G)
		{
			G = tempG;
			selectedVelocity = i->v;
			selectedOmega = i->w;
		}
	}

	vector<float> selVelocity(2);
	selVelocity[0] = selectedVelocity;
	selVelocity[1] = selectedOmega;

	return selVelocity;
}

//计算小车的速度和角速度变化范围
vector<float> CreateDW(RobotState curState)
{
	vector<float> dw(4);
	float tmpMinVelocity = curState.velocity - MAX_ACCELERATE*DT;
	float tmpMaxVelocity = curState.velocity + MAX_ACCELERATE*DT;
	float tmpMinOmega = curState.omega - MAX_ACCOMEGA*DT;
	float tmpMaxOmega = curState.omega + MAX_ACCOMEGA*DT;

	dw[0] = tmpMinVelocity > MIN_VELOCITY ? tmpMinVelocity : MIN_VELOCITY;
	dw[1] = tmpMaxVelocity < MAX_VELOCITY ? tmpMaxVelocity : MAX_VELOCITY;
	dw[2] = tmpMinOmega;
	dw[3] = tmpMaxOmega < MAX_OMEGA ? tmpMaxOmega : MAX_OMEGA;

	return dw;
}

//小车运动
RobotState Motion(RobotState curState, float velocity, float omega)
{
	RobotState afterMoveState;

	//if (omega != 0)
	//{
	//	afterMoveState.xPosition = curState.xPosition + velocity / omega*sin(curState.orientation)
	//		- velocity / omega*sin(curState.orientation + omega*DT);

	//	afterMoveState.yPosition = curState.yPosition - velocity / omega*cos(curState.orientation)
	//		- velocity / omega*cos(curState.orientation + omega*DT);
	//}
	//else
	//{
	//	afterMoveState.xPosition = curState.xPosition + velocity*cos(curState.orientation)*DT;

	//	afterMoveState.yPosition = curState.yPosition + velocity*sin(curState.orientation)*DT;
	//}

	afterMoveState.xPosition = curState.xPosition + velocity*DT*cos(curState.orientation);
	afterMoveState.yPosition = curState.yPosition + velocity*DT*sin(curState.orientation);

	afterMoveState.orientation = curState.orientation + omega * DT;
	afterMoveState.velocity = velocity;
	afterMoveState.omega = omega;	

	return afterMoveState;
}

//生成路径
vector<RobotState> GenerateTraj(RobotState initState, float vel, float ome)
{
	RobotState tempState = initState;
	vector<RobotState> trajectories;
	float time = 0;
	trajectories.push_back(initState);
	while (time < PREDICT_TIME)
	{
		tempState = Motion(tempState, vel, ome);
		trajectories.push_back(tempState);
		time += DT;
	}

	return trajectories;
}

//计算小车与终点的角度
float CalcHeading(RobotState rState, int goal[])
{
	float heading;

	float dy = goal[1] - rState.yPosition;
	float dx = goal[0] - rState.xPosition;

	float goalTheta = atan2(dy, dx);
	float targetTheta;
	if (goalTheta > rState.orientation)
	{
		targetTheta = goalTheta - rState.orientation;
	}
	else
	{
		targetTheta = rState.orientation - goalTheta;
	}

	heading = 180 - targetTheta / M_PI * 180;

	return heading;
}

//计算小车到最近的障碍物的距离
float CalcClearance(RobotState rState, int obs[][2])
{
	float dist = 100;
	float distTemp;
	for (int i = 0; i < 18; i++)
	{
		float dx = rState.xPosition - obs[i][0];
		float dy = rState.yPosition - obs[i][1];
		distTemp = sqrt(pow(dx,2) + pow(dy,2)) - ROBOT_RADIUS;

		if (dist > distTemp)
		{
			dist = distTemp;
		}
	}

	if (dist >= 2 * ROBOT_RADIUS)
	{
		dist = 2 * ROBOT_RADIUS;
	}

	return dist;
}

//计算小车当前状态最少能行驶多少
float CalcBreakingDist(float velo)
{
	float stopDist = 0;
	while (velo > 0)
	{
		stopDist = stopDist + velo*DT;
		velo = velo - MAX_ACCELERATE*DT;
	}

	return stopDist;
}

ROS 2D导航原理系列（六）|局部路径规划-DWA算法
DWA算法总结