自动驾驶-决策规划算法九：人工势场算法(C++) (1)

自动驾驶-决策规划算法九：人工势场算法(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;//道路边界斥力开关
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337

测试结果：

先测试没有障碍物时的场景：

没有障碍物时，将只有目标点的引力和道路边界的斥力有影响，如果目标点靠近车道中心，那道路边界斥力影响将非常小，在上图的场景中，是否有道路边界的斥力，轨迹都是近似的一条直线；

只有一个障碍物时轨迹：

可以看到障碍物的斥力起到作用了，此时道路边界斥力影响仍然较小；

然后测试五个障碍物的场景：

关闭道路边界斥力：

开启道路边界斥力：

关闭与开启道路边界时的轨迹对比：

可以看出在道路边界斥力的作用下，轨迹更倾向于靠近道路中心；

把障碍物斥力增益系数k调大，此时障碍物斥力作用明显增强，轨迹已经超出车道边界了，而此时道路边界斥力的作用就体现出来了，如果开启，轨迹就不会超出车道边界：

总结：

通过上述分析可知：

1，人工势场算法公式中的几个参数，对规划出的轨迹有直接影响，实际应用时，应反复调参直到轨迹合适；

2，引力系数和斥力系数的大小，会直接决定引力和斥力的大小，如果引力过大、斥力过小，则轨迹可能会和障碍物碰撞；反之，则可能会出现无法到达终点的情况；

3，而道路边界斥力对车辆保持在车道中，不驶出车道有约束作用；

从目前的公式来看，虽然解决了终点不可达、局部最优、路径可能会碰到障碍物的问题，但是生成的轨迹仍然不一定可以直接用于车辆驾驶，因为目前的轨迹仍然有很多不平滑的地方，并不适合车辆运动学要求。后续如果有更好的改进算法，再尝试加上。