参考源码:https://www.cmu-exploration.com/development-environment
1.采样路径的生成
用采样的离散点做前向模拟,基于机器人当前状态,预测一段范围内可能出现的情况。
生成沿着不同路径到达传感器范围边界的路径集合。这些示例路径是基于车辆运动约束生成的,每条路径生成三次样条曲线,一组路径可以看作是从起始状态到传感器范围边界的可行路径。
在导航过程中,感知传感器(激光雷达)检测到阻塞某些路径的障碍物,便将该部分路径删除,仅保留可通行的路径子集。以上路径组是脱机生成的,在生成路径的时,定义几个关键参数:
dis = 1.0;
angle = 27;
deltaAngle = angle / 3;
这里定义的最大转弯半径angle
为27,deltaAngle
为9,因此会生成7条(27/9*2+1)路径组。
以第一条路径组为例,此时shift1
偏移量即为-27,每条路径半径dis
为1。
pathStartR = 0 : 0.01 : dis;
pathStartShift = spline(wayptsStart(:, 1), wayptsStart(:, 2), pathStartR)
其中wayptsStart(:, 1)
即为dis
=(0,1),wayptsStart(:, 2)
为shift1
=(0,-27)。
因此pathStartShift
为一个
1
∗
101
1*101
1∗101的行向量,代表对0->shift1
进行100次数据均匀插值,即[0.00,-0.27,-0.54,-0.81,…,-26.73,-27.00]
pathStartX = pathStartR .* cos(pathStartShift * pi / 180);
pathStartY = pathStartR .* sin(pathStartShift * pi / 180);
pathStartZ = zeros(size(pathStartX));
接着对pathStartR
这条0-1的直线,通过pathStartShift
转变成如下图所示的曲线
由于采用的三次样条模拟路径,第一段则由shift1 = -angle : deltaAngle : angle
计算,当shift1 = -27
时即如上图示例所示。在第二次第三次的样条采用时,原理同第一次。
for shift2 = -angle * scale + shift1 : deltaAngle * scale : angle * scale + shift1
for shift3 = -angle * scale^2 + shift2 : deltaAngle * scale^2 : angle * scale^2 + shift2
...
end
end
相比于第一次,其角度进行了一定比例scale
的缩放,此时的pathR
相比于第一次的pathStartR
,从0-1变成了0-3的范围,从
1
∗
101
1*101
1∗101的行向量变成
1
∗
301
1*301
1∗301的行向量,相同的是他的值仍是以0.01递增。
下图为waypts
,1-101为第一次样条采样的路径点,第一栏代表距离从0-1,第二栏代表角度,而101-104则是第二次和第三次样条采样的路径点。通过spline
对前一百次数据的拟合,采样出第二次和第三次的路径点。
waypts = [pathStartR', pathStartShift', pathStartZ';
2 * dis, shift2, 0;
3 * dis - 0.001, shift3, 0;
3 * dis, shift3, 0];
pathR = 0 : 0.01 : waypts(end, 1); % 301维的行向量
pathShift = spline(waypts(:, 1), waypts(:, 2), pathR); % 采样出第二次和第三次的路径点
pathX = pathR .* cos(pathShift * pi / 180);
pathY = pathR .* sin(pathShift * pi / 180);
pathZ = zeros(size(pathX));
最终该一次循环所计算的路径,即如上图所示,分为三段,蓝色为第一次采样,橘色为第二次,黄色为第三次。
在一组完整的路径中,根据第一次采样分组groupID
,即7组,再对第二次和第三次采样后的路径进行命名pathID
。
根据以上命名方式会生成3个文件数据,分别是:
startPaths.ply
:记录了第一次采样的路径点,是一个
4
∗
707
4*707
4∗707的矩阵,即一共7组路径组,每一个路径组包含了101个路径点,里面所保存的数据为:{x,y,z,groupID}
pathAll
:该文件则记录了三次采样所生成的所有路径点,是一个
5
∗
103243
5*103243
5∗103243的矩阵,即在每一个路径组中,包含14749个路径点(103243/7 = 14749)。而每一条路径包括301个路径点(14749/7/7 = 301)。
pathList
该文件记录了每条路径的最后一个路径点,是一个
5
∗
343
5*343
5∗343的矩阵,即
7
∗
7
∗
7
7*7*7
7∗7∗7个末端点。
2.体素网格产生
对于碰撞检查,使用覆盖了传感器范围的体素网格。根据样条距离,传感器范围为3.2*4.5,在该区域生成体素网格,如下图所示,在这里考虑了车辆半径的遮挡。在生成体素网格时,定义如下参数,
voxelSize = 0.02; // 体素方格大小
searchRadius = 0.45; //
offsetX = 3.2; // 范围
offsetY = 4.5;
voxelNumX = 161; //数量 offsetX/voxelSize
voxelNumY = 451;
在外层循环中,从外向内计算,同采样的路径一样,靠近车体的位置Y的宽度也会随着scaleY
减小。
for indX = 0 : voxelNumX - 1
x = offsetX - voxelSize * indX;
scaleY = x / offsetX + searchRadius / offsetY * (offsetX - x) / offsetX;
for indY = 0 : voxelNumY - 1
y = scaleY * (offsetY - voxelSize * indY);
end
end
下面对scaleY
的计算详细说明:
如上图所示,以最上层边缘计算为例,即indY = 0
的时候,此时可以看成一条线,而y = scaleY * offsetY
,
当 x = 0
时,y = searchRadius
当 x = offsetX
时,y = offsetY
得到:
y
=
o
f
f
s
e
t
Y
−
s
e
a
r
c
h
R
a
d
i
u
s
o
f
f
s
e
t
X
x
+
s
e
a
r
c
h
R
a
d
i
u
s
y = {offsetY -searchRadius \over offsetX}x+searchRadius
y=offsetXoffsetY−searchRadiusx+searchRadius
或通过相似三角形可得到:
y
−
s
e
a
r
c
h
R
a
d
i
u
s
o
f
f
s
e
t
Y
−
s
e
a
r
c
h
R
a
d
i
u
s
=
x
o
f
f
s
e
t
X
{y-searchRadius \over offsetY - searchRadius} = { x \over offsetX}
offsetY−searchRadiusy−searchRadius=offsetXx
y
=
x
o
f
f
s
e
t
X
(
o
f
f
s
e
t
Y
−
s
e
a
r
c
h
R
a
d
i
u
s
)
+
s
e
a
r
c
h
R
a
d
i
u
s
y = { x \over offsetX}(offsetY - searchRadius)+searchRadius
y=offsetXx(offsetY−searchRadius)+searchRadius因此:
s
c
a
l
e
Y
=
y
o
f
f
s
e
t
Y
=
x
o
f
f
s
e
t
X
(
o
f
f
s
e
t
Y
−
s
e
a
r
c
h
R
a
d
i
u
s
)
+
s
e
a
r
c
h
R
a
d
i
u
s
o
f
f
s
e
t
Y
scaleY = {y \over offsetY } = { { x \over offsetX}(offsetY - searchRadius)+searchRadius \over offsetY }
scaleY=offsetYy=offsetYoffsetXx(offsetY−searchRadius)+searchRadius
=
x
o
f
f
s
e
t
X
−
x
∗
s
e
a
r
c
h
R
a
d
i
u
s
o
f
f
s
e
t
X
∗
o
f
f
s
e
t
Y
+
s
e
a
r
c
h
R
a
d
i
u
s
o
f
f
s
e
t
Y
= { x \over offsetX} - { x*searchRadius \over offsetX*offsetY}+{searchRadius \over offsetY}
=offsetXx−offsetX∗offsetYx∗searchRadius+offsetYsearchRadius
=
x
o
f
f
s
e
t
X
+
(
o
f
f
s
e
t
X
−
x
)
s
e
a
r
c
h
R
a
d
i
u
s
o
f
f
s
e
t
X
∗
o
f
f
s
e
t
Y
= { x \over offsetX} +{{(offsetX - x)searchRadius }\over offsetX*offsetY}
=offsetXx+offsetX∗offsetY(offsetX−x)searchRadius
=
x
o
f
f
s
e
t
X
+
s
e
a
r
c
h
R
a
d
i
u
s
o
f
f
s
e
t
Y
∗
(
o
f
f
s
e
t
X
−
x
)
o
f
f
s
e
t
X
= { x \over offsetX} +{{searchRadius }\over offsetY} *{(offsetX - x) \over offsetX}
=offsetXx+offsetYsearchRadius∗offsetX(offsetX−x)
3.碰撞检测
在生成体素网格后,为了在局部路径的筛选中更高效,将每个网格与路径索引离线计算。通过查找路径集合中的路径点周围指定距离内的邻居,并且该邻居点属于体素网格的集合。最终得到每一个体素网格与待遮挡路径之间的索引关系表。
rangesearch()
函数:查找某个点周围指定距离内的所有邻居。
[ind, dis] = rangesearch(pathAll(1 : 2, :)', voxelPoints, searchRadius);
记录路径点附近的所有体素网格。