概述
垂直车位的检测存在许多技术上的瓶颈，通过超声波一次性检测很难达到较高的精度，而通过车辆泊车过程中的多次的库位定位，可以有效提高车位的定位精度。本文主要介绍垂直车位情形下，车辆已经进库的情形，如何通过辆边的障碍物，重新定位库位的中心。
![菊花展]()
菊花展
数据处理
超声波的数据处理，主要将冗余的数据剔除和一些无效数据滤除。下面介绍了一些实际采用的超声波数据的滤波算法。
原始数据
如下图所示，是车辆垂直进库过程中实际采集的数据，蓝色代表车辆左侧采集到的数据，橘黄色代表车辆右侧采集的数据。从图中可以看出，原始超声数据比较杂乱，很多无效数据都记录了下来。比如未检测到超声回波的数据，首先应该把这些无效数据滤除。 ![原始数据]()
第一级滤波
如下图所示，是超声波数据第一级滤波后的结果，第一级滤波主要滤波无效的超声波数据，包括超声无回波的数据、超声回波幅值小于阀值的数据和车辆停止采集的冗余数据。将这些无效数据滤除后，得到如下比较整洁的数据图。图中绿色数据代表车辆后轴中心的轨迹坐标。虽然经过一级滤波后数据趋势有所改善，但仍然不便于直接进行线性拟合。因为图中存在车头的弧线段和一些突变边沿数据，所以数据还需进一步的滤波。 ![第一级滤波]()
第二级滤波
如下图所示，是超声数据进行二级滤波后的效果。从图中可以看出二级滤波后，数据更加平滑，可以进行线性拟合。二级滤波的算法，主要采用数据的分布特性，提取数据中分布集中的数据点，将分散的数据剔除，这样就可以得到平滑的车辆边沿曲线，最后采用最小二乘法拟合直线。 ![第二级滤波]()
根据库位边沿拟合的两个直线，求得新的中心线所在的直线，然后根据求得的中心线与车辆轨迹的偏差去调整车辆的入库轨迹。
软件实现
软件实现部分使用Python进行前期的算法验证，利用python具有的强大的数学运算库，可以方便地进行前期的算法验证。
代码实现
第一级滤波
1
2
3
4
5
6
7
8
9
10
|
def ValidDataProcess(datx,daty,level,level_threshold):
valid_process_x = []
valid_process_y = []
for i in range(len(level)):
if level[i] > level_threshold:
valid_process_x.append(datx[i])
valid_process_y.append(daty[i])
return valid_process_x,valid_process_y
|
第二级滤波
1
2
3
4
5
6
7
8
9
10
|
def FitDataProcess(datx,daty,threshold,step):
valid_fit_x =[]
valid_fit_y =[]
for i in range(len(daty)):
if (daty[i] >= threshold) and (daty[i] < (threshold + step)):
valid_fit_x.append(datx[i])
valid_fit_y.append(daty[i])
return valid_fit_x,valid_fit_y
|
数据分布
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
|
def ValueDistributed_v2(step,dat):
value_max = max(dat)
value_min = min(dat)
print("最大值:",value_max)
print("最小值:",value_min)
array_cnt = int((value_max - value_min)/step) + 1
print("分组数:",array_cnt)
DistributedCnt = np.zeros(array_cnt)
for v in range(len(dat)):
for i in range(0,array_cnt):
if (dat[v] >= (value_min + step * i)) and (dat[v] < (value_min + step * (i+1))):
DistributedCnt[i] = DistributedCnt[i] + 1
for i in range(0,array_cnt):
print("分布:",i,"，值：",DistributedCnt[i])
list_distribute_max_cnt = DistributedCnt.tolist()
distribute_max_cnt = list_distribute_max_cnt.index(max(list_distribute_max_cnt))
print("最高分布索引:",distribute_max_cnt)
return value_min + step * distribute_max_cnt
|
直线拟合函数的实现
使用Python的Scipy库中的optimize模块，可以实现一阶线性函数的拟合，具体使用如下：
1
2
3
|
def f_1(x, A, B):
return A*x + B
|
- 使用curve_fit方法实现 其中A1和B1就是所要求的系数
1
2
3
| A1, B1 = optimize.curve_fit(f_1, valid_fit_11x, valid_fit_11y)[0]
x1 = np.arange(-7,5,0.1)
y1 = A1 * x1 + B1
|
为了后期便于移植到C++/C平台，基于Python语言，实现了简单的直线最小二乘法拟合算法
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
|
def LineFit(x,y):
sum_x = sum(x)
sum_y = sum(y)
sum_xy = 0
for i in range(len(x)):
sum_xy = sum_xy + x[i]*y[i]
sum_x2 = 0
for i in range(len(x)):
sum_x2 = sum_x2 + x[i]*x[i]
sum_n = len(x)
Denominator = sum_n*sum_x2 - sum_x*sum_x
molecule_a = sum_n*sum_xy - sum_x*sum_y
molecule_b = sum_x2*sum_y - sum_x*sum_xy
return molecule_a/Denominator,molecule_b/Denominator
|
关于曲线拟合的具体内容可以参照最小二乘法的线性拟合。
最后的实现
1
2
| valid_process_11x,valid_process_11y = ValidDataProcess(GroundLocation_x11,GroundLocation_y11,LRU11_UltrasonicLevel,level_threadhold)
valid_process_12x,valid_process_12y = ValidDataProcess(GroundLocation_x12,GroundLocation_y12,LRU12_UltrasonicLevel,level_threadhold)
|
- 使用ValueDistributed_v2函数，求取数据分布
1
2
| DistributeValue_11y = ValueDistributed_v2(0.05,valid_process_11y)
DistributeValue_12y = ValueDistributed_v2(0.05,valid_process_12y)
|
- 根据分布特性，使用FitDataProcess进行二级滤波
1
2
| valid_fit_11x,valid_fit_11y = FitDataProcess(valid_process_11x,valid_process_11y,DistributeValue_11y,0.05)
valid_fit_12x,valid_fit_12y = FitDataProcess(valid_process_12x,valid_process_12y,DistributeValue_12y,0.05)
|
1
2
| A1, B1 = LineFit(valid_fit_11x, valid_fit_11y)
A2, B2 = LineFit(valid_fit_12x, valid_fit_12y)
|
根据库位边沿拟合的两个直线，求得新的中心线所在的直线，然后根据求得的中心线与车辆轨迹的偏差去调整车辆的入库轨迹。