一种基于高斯概率模型的无人车辆导航障碍物建模方法及系统

本发明涉及无人车辆导航障碍物建模，具体而言，涉及一种基于高斯概率模型的无人车辆导航障碍物建模方法及系统。

背景技术：

1、随着计算机技术和5g技术的发展，无人驾驶技术迅速发展。无人车辆在移动过程中，需要依靠各种传感器(如里程计、视觉相机、雷达等)对周边环境进行感知，并依据感知信息建立障碍物环境模型。但由于传感器本身的限制，以及数据之间传输的时延性，导致感知模块获取的感知信息存在不确定性。感知信息的不确定性必将导致障碍物环境建模的不确定性，环境建模的不确定性必将导致决策规划的不确定性。严重情况下，由于感知障碍物位置的不确定，无人车辆发出错误的避障策略，导致与障碍物发生碰撞几率大幅度提高，存在较大的安全隐患。

2、随着系统信息不断地循环更新，不确定信息量逐渐累积，导致感知信息完全错误以及决策规划完全背离实际情况。

技术实现思路

1、本发明要解决的技术问题是：

2、为了解决现有的依据传感器进行建模时，由于其本身限制及数据间传输的时延性，导致感知模块获取信息存在不确定性，造成避障测量出错甚至完全背离事实，存在较大的安全隐患的问题。

3、本发明为解决上述技术问题所采用的技术方案：

4、本发明提供了一种基于高斯概率模型的无人车辆导航障碍物建模方法，包括以下步骤：

5、步骤一、对不同的障碍物创建不同数据记忆存储空间memory，将定位感知模块对同一障碍物依时间采样序列收集到的障碍物状态数据存放在各自的memory中；

6、步骤二、障碍物不确定模块基于步骤一存储的障碍物状态数据计算得到静态障碍物概率模型和动态障碍物滤波模型；

7、步骤三、障碍物膨胀模块基于步骤二获得的概率模型膨胀障碍物，用于建立障碍物膨胀矩形；

8、步骤四、动态环境建模模块将步骤三获取的障碍物膨胀矩形依据投影规则，投影到静态栅格地图中；

9、步骤五、判断下一时刻的障碍物是否在感知区间内，若在，则将障碍物信息存储在memory空间内并更新障碍物在静态栅格地图的投影；若不再，则删除该障碍物的memory空间及在该障碍物在静态栅格地图上的投影；若感知到新的障碍物，则重复上述步骤的判断，直至完成障碍物建模。

10、进一步地，在步骤一中，具体包括，依据无人车辆长宽大小，设定栅格地图分析窗口的分辨率；基于感知点云数据信息，提取栅格分析窗口内的地形特征，分析可通过性系数，建立一定区域的静态栅格地图；对不同的障碍物创建不同数据记忆存储空间memory，将对同一障碍物依时间采样序列收集到的障碍物状态数据存放在各自的memory中。

11、进一步地，在步骤二中，具体包括，基于存储的数据计算静态障碍物状态信息的概率密度函数和置信区间上下限以及动态障碍物的预测状态。

12、进一步地，在步骤二中，为了计算静态障碍物概率模型，通过依据高斯理论，选取正态分布概率密度来描述位置的概率取值，用极大似然估计法估计障碍物的位置参数，包括，

13、设p0＝[x0,y0]t为障碍物的真实位置，实际测量值p'＝[x',y']t的取值分布在以p0为中心，偏差半径值为rex和rey的矩形区域内；

14、实际测量值p'的横纵坐标x'和y'的主要取值范围为x0-rex≤x'≤x0+rx，y0-rey≤y'≤y0+rey，且x'和y'相互独立；

15、利用正态分布概率密度来描述x'和y'的概率取值；由此可得，x'的分布服从x'～n(u1,σ1)，y'的分布服从y'～n(u2,σ2)；其中，u1和σ1为横坐标x'的均值和标准差，u2和σ2为纵坐标y'的均值和标准差；

16、设x0,x1,x2,……,xn来自x的随机样本观察值，通过卡尔曼滤波器对样本值进行修正，所述卡尔曼滤波器迭代方程为，

17、x'k＝xk-1

18、pk'＝pk-1

19、

20、xk＝x'k+kk(zk-x'k)

21、pk＝(1-kk)pk'

22、其中，pk为预估误差协方差；r为感知测量误差协方差；

23、通过卡尔曼滤波器迭代方程修正后，得到新的样本数据x'0,x'1,x'2,……,x'n，通过求最大似然估计函数来取值均值u和标准差σ，得到个体值x的概率密度为，

24、

25、则，似然函数为：

26、

27、

28、

29、

30、可得u和σ的最大似然估计量分别为，

31、

32、

33、其中，

34、同理可得，关于y的概率密度为，

35、

36、其中，

37、由三倍标准差原理，把(u-3σ,u+3σ)看作是随机变量的取值空间，由此可得，

38、

39、

40、

41、取无限小的一个区间内的概率作为测量值x和y的概率，可得，

42、

43、

44、其中，ε无限逼近于0，但不等于0。

45、进一步地，在步骤二中，为了计算动态障碍物滤波模型，建立障碍物运动模型、随机噪声建立障碍物预测模型和观测模型，基于卡尔曼滤波器的动态障碍物不确定模块用以修正当前状态量以及预测未来状态量，从而得到当前时刻的最优估计；

46、包括，

47、基于障碍物运动模型和随机噪声建立障碍物预测模型和观测模型，

48、xk＝axk-1+buk+wk-1

49、

50、其中，xk＝[lk,vk]k为预测状态量，xk＝[xk,yk,θk,vk,ωk]t，uk＝[akσk]t，zk为测量状态量，zk＝[x'k,y'k,θk',v'k,ω'k]t，wk-1为过程噪声，为观测噪声，a状态矩阵系数，b为控制矩阵系数，h为测量矩阵系数；

51、其中，

52、

53、

54、

55、卡尔曼滤波器包含两个方程集，即预测方程组和修正方程组，两个方程组在滤波器运行的每一时间步下都会执行，

56、所述预测方程组为，

57、x'k＝axk-1+buk

58、pk'＝apk-1at+q

59、所述修正方程组为，

60、kk＝pk'ht(hpk'ht+r)-1

61、xk＝x'k+kk(zk-hx'k)

62、pk＝(1-kkh)pk'

63、其中，q和r分别是过程噪声和观测噪声的协方差矩阵，q的取值跟计算机的舍入误差、模型的线性化误差、离散化引入的误差以及输入误差有关，通过最高阶状态量对其他量的扰动来初始化；r的取值与传感器的特性有关，根据高斯假设，可取各个观测值最大偏差的三倍；pk是状态量之间的协方差矩阵，可通过采样的数据信息计算方差来赋予初值，

64、

65、进一步地，在步骤三中，选用矩形包围框对障碍物进行膨胀处理，包括障碍物中心的不确定位置范围的矩形1、包围障碍物的最小矩形的矩形2和膨胀后的障碍物矩形3，设包围障碍物的矩形2的长度分别为a和b，膨胀后的包围障碍物的矩形3长度分别为a+2rex和b+2rey。

66、进一步地，在步骤四中，在将障碍物信息存储在memory空间内时，若此时的memory空间数据已满，则删除最早放入的数据。

67、进一步地，在步骤四中，对不满覆盖一个单位栅格按完全覆盖一个栅格处理。

68、一种基于高斯概率模型的无人车辆导航障碍物建模系统，包括，

69、定位感知模块，用于收集障碍物依时间采样序列的状态数据信息；

70、障碍物不确定模块，用于根据存储定位感知模块获取的状态信息计算静态障碍物概率模型和动态障碍物滤波模型；

71、障碍物膨胀模块，用于根据障碍物不确定模块计算得到的静态障碍物概率模型和动态障碍物滤波模型，建立障碍物膨胀矩形，避免障碍物形状不准确而导致移动平台与障碍物发生碰撞；

72、动态环境建模模块，用于根据定位感知模块、障碍物不确定模块和障碍物膨胀模块得到的信息进行障碍物建模。

73、一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现基于高斯概率模型的无人车辆导航障碍物建模方法的步骤。

74、相较于现有技术，本发明的有益效果是：

75、本发明一种基于高斯概率模型的无人车辆导航障碍物建模方法及系统，对不同的障碍物创建不同数据记忆存储空间memory，将定位感知模块对同一障碍物依时间采样序列收集到的障碍物状态数据存放在各自的memory中；障碍物不确定模型基于存储的状态数据计算静态障碍物概率模型和动态障碍物滤波模型；障碍物膨胀模块基于概率模型膨胀障碍物，建立障碍物膨胀矩形；动态环境建模模块将该障碍物膨胀矩形依据投影规则，投影到静态栅格地图中，得到最终障碍物模型；

76、本发明一种基于高斯概率模型的无人车辆导航障碍物建模方法及系统，考虑感知系统的不确定性，依据高斯理论，用极大似然估计法估计障碍物的位置参数，采用基于卡尔曼滤波器的方法对动态障碍物位置进行修正和预测，能较好的适应不确定性环境下的动态障碍物状态估计和预测；

77、本发明一种基于高斯概率模型的无人车辆导航障碍物建模方法及系统，结合障碍物的概率模型分析，选用矩形包络框对障碍物进行膨胀处理，提高了无人车辆安全性；根据障碍物概率模型，设计动态环境建模算法流程；本发明系统结构层次简单，反应速率快，且安全性高。