自动驾驶定位监控模块设计

1. 定位失效的因素

1.1 传感器失效或者提供错误数据 (Sensor fail or provide bad data)

        例如，隧道中的GPS失效；

1.2 状态估计出错 (Estimation error)

        例如，EKF线性化引入较大误差，如图，linearization error 过大；

1.3 大状态不确定性 (Large state uncertainty)

        例如，依赖IMU太长时间；

---

2. 定位精度范围

2.1 定位精度定义

        定位的估计值与真值之间的误差，可以视为定位的精度。例如，定位的估计值输出为EKF算法输出值，真实值为RTK输出。
即，

                        EKF定位精度 = 定位估计值EKF - 真值RTK

                        NDT定位精度 = 定位估计值NDT - 真值RTK

2.2 定位差值计算

        首先，需要同步两差值项的时间戳。

        1）节点同步两信号源，利用message_filter。

         2）手动对齐，例如，message数量4440-4439比较接近，故左边图为默认从第一项开始相减；右边图为查看NDT和RTK初始第一列时间，手工对齐后开始相减。

EKF为重新降采样5倍后类似处理办法手动相减。timeGap1 = rtk_timestamp - ekf_timestamp; timeGap2 = rtk_timestamp - ndt_timestamp

  

2.3 定位精度结果分析

        2种时间同步方式：1，节点同步时间（node-processing）；2，或者离线数据手工同步时间（offline-processing）

---

3. 定位失效案例分析 —— Lidar NDT匹配定位算法失效

3.1 马氏距离求概率异常值(离群值)

        参考文章：https://towardsdatascience.com/multivariate-outlier-detection-in-python-e946cfc843b3

3.2 NDT 数据维度

        如下表格中展示，截取NDT原数据维度“Time”,"pose.position.x","pose.position.y"：

	Time	pose.position.x	pose.position.y
0	1.625477e+09	1207.557373	-831.669373
1	1.625477e+09	1207.572632	-831.685608
2	1.625477e+09	1207.517700	-831.660889
3	1.625477e+09	1207.570435	-831.686340
4	1.625477e+09	1207.563477	-831.688843
...	...	...	...
1636	1.625477e+09	2398.566650	-489.532379
1637	1.625477e+09	2393.997314	-492.212952
1638	1.625477e+09	2392.208984	-493.365295
1639	1.625477e+09	2393.579346	-493.364105
1640	1.625477e+09	2391.307617	-498.299683

df_ndt = df_ndt[['Time','pose.position.x','pose.position.y']]
df_ndt = df_ndt.dropna()
df_ndt = df_ndt.to_numpy()
df = []
for i in range(len(df_ndt)-1):
    df.append(df_ndt[i+1]-df_ndt[i])


# Covariance matrix
covariance = np.cov(df, rowvar = False)
# Covariance matrix power of -1
covariance_pm1 = np.linalg.matrix_power(covariance, -1)
# Center point
centerpoint = np.mean(df, axis=0)

        用数据后项减去前项，得NDT误差数据， 且centerpoint = np.mean(df, axis=0) = array([0.11882379, 0.72179893, 0.2032742 ])

	Time_error	pose.position.x_error	pose.position.y_error
0	0.10095835	0.01525879	-0.01623535
1	0.09639025	-0.05493164	0.02471924
2	0.10338044	0.05273438	-0.02545166
3	0.11147022	-0.00695801	-0.00250244
4	0.09405279	0.	0.03540039
...	...	...	...

3.3 NDT 异常数据筛选

# Distance between center point and
distances = []
for i,val in enumerate(df):
    p1 = val
    p2 = centerpoint 
    distance = (p1-p2).T.dot(covariance_pm1).dot(p1-p2)
#     distance = (p1-p2).T.dot(covariance).dot(p1-p2)
    distances.append(distance)
distances = np.array(distances)

# Cutoff (threshold) value from Chi-Sqaure Distribution for detecting outliers
#cutoff = chi2.ppf(0.95, df.shape[1])
cutoff = chi2.ppf(0.95, 3)

# index of cutliers
outlierIndexes = np.where(distances > cutoff)

其中，cutoff = 7.814727903251179， outlierIndexes = 筛选出异常数据的Index

(array([1411, 1414, 1416, 1417, 1436, 1442, 1446, 1469, 1472, 1473, 1474,
        1481, 1490, 1491, 1493, 1498, 1499, 1504, 1506, 1508, 1510, 1513,
        1515, 1516, 1519, 1521, 1524, 1529, 1530, 1531, 1534, 1535, 1536,
        1537, 1539, 1541, 1542, 1543, 1544, 1545, 1547, 1549, 1550, 1551,
        1553, 1554, 1556, 1558, 1563, 1572, 1574, 1575, 1576, 1578, 1580,
        1587, 1594, 1596, 1622, 1623, 1626, 1630, 1633, 1634, 1635, 1636,
        1639]),)

3.4 NDT 异常数据画图

df_new = []
for i,val in enumerate(outlierIndexes):
    df_new.append(df_ndt[val])

plt.scatter(df_ndt[:,1],df_ndt[:,2],s=2,label="ndt pose output")
plt.scatter(df_new[0][:,1],df_new[0][:,2],s=2, label='error monitor outlier detection')
plt.legend()
plt.savefig("error monitor outlier detection.png")