车辆入库后的库位中心定位(垂直车位)

1、概述

垂直车位的检测存在许多技术上的瓶颈，通过超声波一次性检测很难达到较高的精度，而通过车辆如果过程中的多次的库位定位，可以有效提高车位的定位精度。本文主要介绍垂直车位情形下，车辆已经进库的情形，如何通过辆边的障碍物，重新定位库位的中心。

2、数据处理

2.1、概述

超声波的数据处理，主要将冗余的数据剔除和一些无效数据滤除。下面介绍了一些实际采用的超声波数据的滤波算法。

2.2、原始数据

如下图所示，是车辆垂直进库过程中实际采集的数据，蓝色代表车辆左侧采集到的数据，橘黄色代表车辆右侧采集的数据。从图中可以看出，原始超声数据比较杂乱，很多无效数据都记录了下来。比如未检测到超声回波的数据，首先应该把这些无效数据滤除。

2.3、第一级滤波

如下图所示，是超声波数据第一级滤波后的结果，第一级滤波主要滤波无效的超声波数据，包括超声无回波的数据、超声回波幅值小于阀值的数据和车辆停止采集的冗余数据。将这些无效数据滤除后，得到如下比较整洁的数据图。图中绿色数据代表车辆后轴中心的轨迹坐标。虽然经过一级滤波后数据趋势有所改善，但仍然不便于直接进行线性拟合。因为图中存在车头的弧线段和一些突变边沿数据，所以数据还需进一步的滤波。

2.4、第二级滤波

如下图所示，是超声数据进行二级滤波后的效果。从图中可以看出二级滤波后，数据更加平滑，可以进行线性拟合。二级滤波的算法，主要采用数据的分布特性，提取数据中分布集中的数据点，将分散的数据剔除，这样就可以得到平滑的车辆边沿曲线，最后采用最小二乘法拟合直线。

根据库位边沿拟合的两个直线，求得新的中心线所在的直线，然后根据求得的中心线与车辆轨迹的偏差去调整车辆的入库轨迹。

3、软件实现

3.1、概述

软件实现部分使用Python进行前期的算法验证，利用python具有的强大的数学运算库，可以方便地进行前期的算法验证。

3.2、代码实现

3.2.1、第一级滤波

# 第一级滤波
# 滤除无效的数据
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

3.2.2、第二级滤波

# 第二级滤波
# 根据数据的分布特性滤除分布比较零散的数据
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

3.3.3、数据分布

#数值分布求取函数 该进版本v2
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

3.3.4、直线拟合函数的实现

使用Python的Scipy库中的optimize模块，可以实现一阶线性函数的拟合，具体使用如下：

• 定义函数形式

#直线方程函数
def f_1(x, A, B):
    return A*x + B

• 使用curve_fit方法实现

其中A1和B1就是所要求的系数

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语言，实现了简单的直线最小二乘法拟合算法

# 一阶线性函数的拟合，采用最小二乘法
# 拟合函数形式:y = a*x +b
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

关于曲线拟合的具体内容可以参照最小二乘法的线性拟合。

3.3.5、最后的实现

1. 使用ValidDataProcess进行一级滤波

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)  

2. 使用ValueDistributed_v2函数，求取数据分布

DistributeValue_11y = ValueDistributed_v2(0.05,valid_process_11y)
DistributeValue_12y = ValueDistributed_v2(0.05,valid_process_12y)

3. 根据分布特性，使用FitDataProcess进行二级滤波

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)

4. 使用二级滤波的数据，进行线性拟合，求得直线方程

A1, B1 = LineFit(valid_fit_11x, valid_fit_11y)
A2, B2 = LineFit(valid_fit_12x, valid_fit_12y)