自动驾驶-决策规划算法九：人工势场算法(C++)
摘要：
本文介绍无人驾驶决策规划算法中人工势场算法。
语言用的是C++，图形基于easyX库输出。
如有错误或者遗漏之处，欢迎指出。
//限于时间关系，公式就不用公式编辑器了，使用截图表示。
附赠自动驾驶学习资料和量产经验：链接
基本概念：
人工势场法（Artificial Potential Field）简称 APF，属于局部路径规划；
人工势场法的基本思想是在障碍物周围构建障碍物斥力势场，在目标点周围构建引力势场，类似于物理学中的电磁场；
被控对象在这两种势场组成的复合场中受到斥力作用和引力作用，斥力和引力的合力指引着被控对象的运动，生成无碰撞的路径；
一种比喻是，势场法可以将障碍物比作是平原上具有高势能值的山峰，而目标点则是具有低势能值的低谷；
目标点对物体产生引力，引导汽车朝向其运动，距离越大，汽车所受的引力就越大，这一点有点类似于A*算法中的启发函数h；
常用的引力场函数：
其中η为比例增益系数，ρ(q,qg)是一个向量，表示汽车位置q与目标点位置qg之间的欧式距离，也就是直线距离，方向是从汽车位置指向目标点位置；
从公式可以看出，离目标点越远，引力场越大，且是与距离的平方成正比。所以引力场模型是一个抛物面，在下图中左下角是目标点，则引力场的形状为：
相应的引力就是引力场的负梯度：
证明过程：
设车辆位置为(x, y)，目标点位置为(xg, yg)，则有：
所以梯度为：
也就是分别对x和y求偏导；
障碍物对车辆产生斥力，决定斥力的主要因素是汽车与障碍物的距离，还有一个作用范围，当汽车在作用范围之外时，不产生斥力，进入作用范围时，离障碍物距离越近，斥力越大；
常用的斥力场函数：
其中，k为比例增益系数，ρ(q,qo)是一个向量，表示汽车位置q与目标点位置qo之间的欧式距离，方向是从障碍物位置指向汽车位置；ρo为障碍物起作用的距离；
从公式可以看出，在作用范围内，离目标点越近，引力场越大，斥力场模型的形状如下图所示：
相应的斥力就是斥力场的负梯度：
证明过程：
设车辆位置为(x, y)，障碍物位置为(xo, yo)，则有：
所以梯度为：
对x求偏导：
对y求偏导类似，略去；
最后代入梯度公式可得：
缺陷与改进：
以上介绍的是标准的人工势场法，但存在几个缺陷：
1，目标不可达问题：当障碍物与目标点距离太近时，引力很小，而障碍物的斥力合力可能较大，因此可能会无法在目标点停下来；
2，局部最优问题：车辆在某个位置时，如果障碍物的斥力合力与目标点的引力大小相等方向相反，则车辆受到的合力为0，此时陷入局部最优，车辆不再向前搜索路径；
3，当物体离目标点比较远时，引力将变的特别大，相对较小的斥力在甚至可以忽略的情况下，物体路径上可能会碰到障碍物；
一种改进方法是：
在原斥力函数最后一项gradρ(q,qo)替换成ρg^n，也就是离目标点的距离的n次方，这样当车辆离目标点很近时，斥力也会很小，离目标点很远时，斥力不会很小，从而避免目标不可达问题，以及可能会和障碍物碰撞的问题；其中n是正整数，根据需要设置；
另外，增加一个指向目标点的斥力，令这个斥力由障碍物产生，但是方向是指向目标点，这样所有的合力不会出现完全抵消的情况，从而避免局部最优问题：
如下图所示，车辆在任意位置时，受到的总合力为F，既不会为0，也不会在目标点附近时还很大，且方向一定是把车辆往目标点指引：
道路边界斥力定义：
对于道路边界，也设置斥力，防止车辆驶出道路边界，保持在车道中。一种定义方法是：
x表示车辆中心点的横向坐标，离道路边界越近，斥力越大；
v表示车辆的速度，速度越大，斥力越大，因为偏离车道的危险更大；
η为增益系数；
测试一下这个函数，设200为分界点，更靠近道路的时候，斥力会明显增大，符合预期：
算法代码：
ArtificialPotential.h
#pragma once
#include<iostream>
#include<easyX.h>
#include<cmath>
#include<ctime>
#include<vector>
#include<string>
using namespace std;
constexpr auto Swidth = 800;
constexpr auto Sheight = 1200;
constexpr auto e = 2.71828;
class Point//点
{
public:
Point(int _x, int _y) :x(_x), y(_y)//初始化列表
{
}
public:
void drawoint(COLORREF color);//画点
double x;
double y;
int r = 5;//画图半径
};
class newVector//向量
{
public:
newVector(Point p1, Point p2) :pBegin(p1), pEnd(p2)//, length(len)//初始化列表
{
length = sqrt(pow((pEnd.x - pBegin.x), 2) + pow(pEnd.y - pBegin.y, 2));//计算长度
//drawVector();
}
newVector addVector(newVector v1, newVector v2);//向量求和
void drawVector();//绘制向量
void printVector(string name);//打印向量
public:
Point pBegin;
Point pEnd;
double length;
};
class Obs//障碍物
{
public:
Obs(Point p) :po(p)//初始化列表
{
cout << "ObsLoc: (" << po.x << ", " << po.y << ")" << endl;
}
void drawObs();//绘制障碍物
public:
Point po;
double r = 20.0;//半径
};
class Car//车辆
{
public:
Car(Point p) :pc(p)
{
cout << "CarLoc: (" << pc.x << ", " << pc.y << ")" << endl;
}
void drawCar(Point p);//绘制车辆
public:
double length = 100.0;//长度
double width = 50.0;//宽度
Point pc;//中心点
};
class Road//道路
{
public:
Road();
~Road();
void showRoad(Point p);//绘制道路
public:
double RWidth = 200.0;
vector<Obs> obTotal;//所有障碍物
Car *car0;//车辆
Point *pTarget;//终点
bool bObs = true;//障碍物开关
};
class ArtificialPotential//人工势场法
{
public:
double disCal(Point p1, Point p2);//两点距离计算
newVector Fatt();//计算引力
newVector Fobs();//计算障碍物斥力
newVector Fedge();//计算道路边界斥力
void process();//整个过程
void delay(int time); //延时函数，单位ms
public:
Road road;
double delta_t = 0.02;//时间步长
double Rho_att;//与目标点的距离
double eta = 0.3;//引力增益系数//已考虑车辆质量，算出的力直接当作加速度用
double k = 0.3;//障碍物斥力增益系数//已考虑车辆质量，算出的力直接当作加速度用
int n = 3;
double Rho0 = 500.0;//斥力起作用的范围
vector<newVector> vFreo;//存储每个障碍物产生的合斥力
double v0 = 0.0;//起始速度为0
double Eta = 0.02;//道路边界斥力增益系数
bool bEdgeF = true;//道路边界斥力开关
};
ArtificialPotential.cpp
#include"ArtificialPotential.h"
#if 0
void test_for_RoadF()//道路斥力测试
{
setlinestyle(PS_SOLID, 3);
setlinecolor(BLACK);
setfillcolor(BLACK);
setorigin(400, 100);
double e = 2.71828;
double Eta = 0.02;
double v = 10.0;
double x = 0.0;
double Fedge = 0.0;
while (-300.0 < x && x < 300.0)
{
if (-200.0 <= x && x <= 200.0)
{
Fedge = Eta * pow(x, 2) / 3;
}
else if (-300.0 < x && x < 300.0)
{
Fedge = Eta * v * pow(e, abs(x));
}
solidcircle(x, Fedge, 3);
solidcircle(-x, Fedge, 3);
cout << "(" << x << ", " << Fedge << ")" << endl;
x += 0.5;
}
}
void testVector()//向量计算与绘制测试
{
setlinestyle(PS_SOLID, 3);
setlinecolor(BLACK);
setfillcolor(BLACK);
setorigin(0, Swidth / 2);
Point p0(0, 0);
Point p1(200, 300);
Point p2(400, -100);
p0.drawoint();
p1.drawoint();
p2.drawoint();
Point pt(p2.x + p1.x - p0.x, p2.y + p1.y - p0.y);
pt.drawoint();
Point p3(750, -150);
p3.drawoint();
Point pt1(p3.x + p2.x - pt.x, p3.y + p2.y - pt.y);
pt1.drawoint();
system("pause");
cleardevice();
setorigin(Swidth / 2, Sheight);
Point p4(0.0, 0.0);
Point p5(200.5, -300.5);
Point p6(-300.5, -400.5);
newVector v1(p4, p5);
newVector v2(p4, p6);
newVector vt = v1.addVector(v1, v2);
}
#endif
void Point::drawoint(COLORREF color)//画点
{
setfillcolor(color);
solidcircle(x, y, r);
}
newVector newVector::addVector(newVector v1, newVector v2)//向量求和
{
Point pEndtmp(v2.pEnd.x + v1.pEnd.x - v1.pBegin.x, v2.pEnd.y + v1.pEnd.y - v1.pBegin.y);
newVector vt(v1.pBegin, pEndtmp);
return vt;
}
void newVector::drawVector()//绘制向量
{
pBegin.drawoint(BLACK);
pEnd.drawoint(BLACK);
setlinestyle(PS_SOLID, 3);
setlinecolor(LIGHTRED);
line(pBegin.x, pBegin.y, pEnd.x, pEnd.y);
}
void newVector::printVector(string name)//打印向量
{
cout << name
<< ": pBegin: (" << pBegin.x << ", " << pBegin.y << ")"
<< ", pEnd: (" << pEnd.x << ", " << pEnd.y << ")"
<< ", length: " << length << endl;
}
void Obs::drawObs()//绘制障碍物
{
setfillcolor(RED);
fillcircle(po.x, po.y, r);
}
void Car::drawCar(Point p)//绘制车辆
{
setlinestyle(PS_SOLID, 3);
setlinecolor(BLACK);
double leftPos = p.x - width / 2;
double rightPos = p.x + width / 2;
double topPos = p.y - length / 2;
double bottomPos = p.y + length / 2;
rectangle(leftPos, topPos, rightPos, bottomPos);
}
Road::Road()
{
car0 = new Car(Point(0.0, -80.0));//定义车辆
pTarget = new Point(-20.0, -1100.0);//定义终点
cout << "pTarget: (" << pTarget->x << ", " << pTarget->y << ")" << endl;
if (bObs)
{
obTotal.push_back(Obs(Point(-100.0, -250.0)));
obTotal.push_back(Obs(Point(0.0, -460.0)));
obTotal.push_back(Obs(Point(100.0, -660.0)));
obTotal.push_back(Obs(Point(-60.0, -830.0)));
obTotal.push_back(Obs(Point(50.0, -1000.0)));
}
showRoad(car0->pc);//绘制道路
pTarget->drawoint(GREEN);//绘制终点
system("pause");
}
Road::~Road()
{
if (car0 != nullptr)
{
delete car0;
car0 = nullptr;
}
if (pTarget != nullptr)
{
delete pTarget;
pTarget = nullptr;
}
}
void Road::showRoad(Point p)//绘制道路
{
setlinecolor(BLACK);
setorigin(Swidth / 2, Sheight);
//绘制道路中心
setlinestyle(PS_DASH, 5);
line(0, 0, 0, -Sheight);
//绘制左右边界
setlinestyle(PS_SOLID, 5);
line(-RWidth, 0, -RWidth, -Sheight);
line(RWidth, 0, RWidth, -Sheight);
for (auto it = obTotal.begin(); it != obTotal.end(); it++)
{
it->drawObs();//绘制障碍物
}
car0->drawCar(p);//绘制车辆
}
double ArtificialPotential::disCal(Point p1, Point p2)//两点距离计算
{
return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2));
}
newVector ArtificialPotential::Fatt()//计算引力
{
Rho_att = disCal(*road.pTarget, road.car0->pc);//与目标点距离
double F_att = eta * Rho_att;//引力大小
Point pFatt(F_att / Rho_att * (road.pTarget->x - road.car0->pc.x) + road.car0->pc.x, F_att / Rho_att * (road.pTarget->y - road.car0->pc.y) + road.car0->pc.y);
newVector vFatt(road.car0->pc, pFatt);
vFatt.printVector("vFatt");
//vFatt.drawVector();
return vFatt;
}
newVector ArtificialPotential::Fobs()//计算斥力
{
if (road.obTotal.empty())//没有障碍物
{
return newVector (road.car0->pc, road.car0->pc);
}
for (auto it = road.obTotal.begin(); it != road.obTotal.end(); it++)//分别计算每个障碍物产生的合斥力
{
double Rho_obs = disCal(it->po, road.car0->pc);//与这个障碍物中心点的距离
double Freo1 = 0.0;
double Freo2 = 0.0;//这个障碍物产生的两个引力的大小
if (Rho_obs >= Rho0)
{
Freo1 = 0.0;
Freo2 = 0.0;
}
else
{
Freo1 = k * (1 / Rho_obs - 1 / Rho0) * pow(Rho_att, n) / pow(Rho_obs, 2);
Freo2 = n * k / 2 * pow(1 / Rho_obs - 1 / Rho0, 2) * pow(Rho_att, n - 1);
}
Point pFreo1((road.car0->pc.x - it->po.x) / Rho_obs * (Freo1 + Rho_obs) + it->po.x, (road.car0->pc.y - it->po.y) / Rho_obs * (Freo1 + Rho_obs) + it->po.y);//由这个障碍物指向车辆
Point pFreo2(Freo2 / Rho_att * (road.pTarget->x - road.car0->pc.x) + road.car0->pc.x, Freo2 / Rho_att * (road.pTarget->y - road.car0->pc.y) + road.car0->pc.y);//由车辆指向目标点
newVector vFreo1(road.car0->pc, pFreo1);
newVector vFreo2(road.car0->pc, pFreo2);
newVector vFobs = vFreo1.addVector(vFreo1, vFreo2);//计算这个障碍物的合斥力
vFreo.push_back(vFobs);//存入容器
vFreo1.printVector("vFreo1");
vFreo2.printVector("vFreo2");
vFobs.printVector("vFobs");
//vFreo1.drawVector();
//vFreo2.drawVector();
//vFobs.drawVector();
}
//计算所有障碍物产生的合斥力
newVector vFobsTotal = vFreo[0];
for (int i = 1; i < vFreo.size(); i++)
{
vFobsTotal = vFobsTotal.addVector(vFobsTotal, vFreo[i]);
}
vFobsTotal.printVector("vFobsTotal");
//vFobsTotal.drawVector();
return vFobsTotal;
}
newVector ArtificialPotential::Fedge()//计算道路边界斥力
{
if (!bEdgeF)//不考虑道路边界斥力
{
return newVector(road.car0->pc, road.car0->pc);
}
double Fedge = 0.0;
Point pFedge(0.0, 0.0);
if (-(road.RWidth - road.car0->width / 2) <= road.car0->pc.x && road.car0->pc.x < 0.0)
{
Fedge = Eta * pow(road.car0->pc.x, 2) / 3;
pFedge = Point(road.car0->pc.x + Fedge, road.car0->pc.y);
}
else if (road.car0->pc.x >= 0.0 && road.car0->pc.x <= (road.RWidth - road.car0->width / 2))
{
Fedge = Eta * pow(road.car0->pc.x, 2) / 3;
pFedge = Point(road.car0->pc.x - Fedge, road.car0->pc.y);
}
else if (-road.RWidth < road.car0->pc.x && road.car0->pc.x < -(road.RWidth - road.car0->width / 2))
{
Fedge = Eta * v0 * pow(e, -road.car0->pc.x);
pFedge = Point(road.car0->pc.x + Fedge, road.car0->pc.y);
}
else if ((road.RWidth - road.car0->width / 2) < road.car0->pc.x && road.car0->pc.x < road.RWidth)
{
Fedge = Eta * v0 * pow(e, road.car0->pc.x);
pFedge = Point(road.car0->pc.x - Fedge, road.car0->pc.y);
}
newVector vFedge(road.car0->pc, pFedge);
vFedge.printVector("vFedge");
//vFedge.drawVector();
return vFedge;
}
void ArtificialPotential::process()//整个过程
{
while (true)
{
newVector vFatt = Fatt();//引力
newVector vFobsTotal = Fobs();//总障碍物斥力
newVector vFedge = Fedge();//道路边界斥力
newVector vFtotal = vFatt.addVector(vFatt, vFobsTotal);//引力与斥力的合力
vFtotal = vFtotal.addVector(vFtotal, vFedge);//再与道路斥力计算合力
vFtotal.printVector("vFtotal");
//vFtotal.drawVector();
double dis = v0 * delta_t + vFtotal.length * pow(delta_t, 2) / 2;//每个时间间隔内走过的距离
v0 += vFtotal.length * delta_t;//速度
road.car0->pc.x = dis / vFtotal.length * (vFtotal.pEnd.x - vFtotal.pBegin.x) + vFtotal.pBegin.x;
road.car0->pc.y = dis / vFtotal.length * (vFtotal.pEnd.y - vFtotal.pBegin.y) + vFtotal.pBegin.y;//更新车辆坐标位置
road.car0->pc.drawoint(BLUE);
cout << "CarLoc: (" << road.car0->pc.x << ", " << road.car0->pc.y << "), dis to goal: " << Rho_att << ", dis: " << dis << ", v: " << v0 << endl << endl;
//road.car0->drawCar(road.car0->pc);
vFreo.clear();
if (Rho_att <= road.car0->length / 2)//已到达目标点
{
break;
}
//delay(100);
//system("pause");
}
}
void ArtificialPotential::delay(int time) //延时函数，单位ms
{
clock_t now = clock();
while (clock() - now < time)
{
}
}
int main()
{
initgraph(Swidth, Sheight, EW_SHOWCONSOLE);
setbkcolor(WHITE);
cleardevice();
ArtificialPotential ap;
ap.process();
system("pause");
closegraph();
return 0;
}
测试结果：
先测试没有障碍物时的场景：
没有障碍物时，将只有目标点的引力和道路边界的斥力有影响，如果目标点靠近车道中心，那道路边界斥力影响将非常小，在上图的场景中，是否有道路边界的斥力，轨迹都是近似的一条直线；
只有一个障碍物时轨迹：
可以看到障碍物的斥力起到作用了，此时道路边界斥力影响仍然较小；
然后测试五个障碍物的场景：
关闭道路边界斥力：
开启道路边界斥力：
关闭与开启道路边界时的轨迹对比：
可以看出在道路边界斥力的作用下，轨迹更倾向于靠近道路中心；
把障碍物斥力增益系数k调大，此时障碍物斥力作用明显增强，轨迹已经超出车道边界了，而此时道路边界斥力的作用就体现出来了，如果开启，轨迹就不会超出车道边界：
总结：
通过上述分析可知：
1，人工势场算法公式中的几个参数，对规划出的轨迹有直接影响，实际应用时，应反复调参直到轨迹合适；
2，引力系数和斥力系数的大小，会直接决定引力和斥力的大小，如果引力过大、斥力过小，则轨迹可能会和障碍物碰撞；反之，则可能会出现无法到达终点的情况；
3，而道路边界斥力对车辆保持在车道中，不驶出车道有约束作用；
从目前的公式来看，虽然解决了终点不可达、局部最优、路径可能会碰到障碍物的问题，但是生成的轨迹仍然不一定可以直接用于车辆驾驶，因为目前的轨迹仍然有很多不平滑的地方，并不适合车辆运动学要求。后续如果有更好的改进算法，再尝试加上。
403
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
