局部路径规划算法——实现DWAdynamic 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算法总结
这篇好文章是转载于:学新通技术网
- 版权申明: 本站部分内容来自互联网,仅供学习及演示用,请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系,请提供相关证据及您的身份证明,我们将在收到邮件后48小时内删除。
- 本站站名: 学新通技术网
- 本文地址: /boutique/detail/tanhgabkhc
系列文章
更多
同类精品
更多
-
photoshop保存的图片太大微信发不了怎么办
PHP中文网 06-15 -
《学习通》视频自动暂停处理方法
HelloWorld317 07-05 -
word里面弄一个表格后上面的标题会跑到下面怎么办
PHP中文网 06-20 -
Android 11 保存文件到外部存储,并分享文件
Luke 10-12 -
photoshop扩展功能面板显示灰色怎么办
PHP中文网 06-14 -
微信公众号没有声音提示怎么办
PHP中文网 03-31 -
excel下划线不显示怎么办
PHP中文网 06-23 -
excel打印预览压线压字怎么办
PHP中文网 06-22 -
TikTok加速器哪个好免费的TK加速器推荐
TK小达人 10-01 -
怎样阻止微信小程序自动打开
PHP中文网 06-13