一种基于粒子滤波的无人车动态避障方法与流程

1.本发明涉及无人车技术领域，具体涉及一种基于粒子滤波的无人车动态避障方法。


背景技术：

2.随着无人车相关技术的快速发展，人们对无人车的需求越来越高，特别是无人车的定位技术。无人车的定位技术通常提供无人车在环境中世界坐标系下的位姿(包括位置和航向角)，同时，无人车定位技术是无人车实现自主导航的关键技术。
3.针对无人车在预设环境地图的全局定位，现有方法是采用粒子滤波定位技术对移动无人车进行定位。粒子滤波定位的自适应蒙特卡洛算法采用概率栅格地图、激光雷达的测量数据和里程位移数据，同时通过带随机噪声的运动模型结合实现随机粒子的运动更新，粒子滤波定位的自适应蒙特卡洛算法根据当前观测数据与预设静态局部环境的匹配程度，调节粒子权重，实现观测更新，用自适应重采样保持有效粒子数，使用权重较高的粒子簇输出估计位姿。
4.但是，现有的自适应蒙特卡洛定位算法的粒子滤波位姿估计存在如下缺点：1、无人车当前运行的局部环境与预设静态地图局部区域相比已经发生较大改变时，当前观测数据与预设静态局部环境之间存在差异，针对已有的预设静态局部环境进行匹配进而调节粒子权重会导致位姿估计误差；2、当里程位移数据有明显误差时，估计位姿也相应发生明显误差。


技术实现要素：

5.本发明提出了一种基于粒子滤波的无人车动态避障方法；能有效的解决上述技术问题。
6.本发明通过以下技术方案实现：
7.一种基于粒子滤波的无人车动态避障方法，所述的无人车设置有总控制系统，以及分别与总控制系统连接的运动控制系统、惯性导航系统、激光雷达模块和imu模块；所述的总控制系统中加载有地图和无人车操作系统ros控制软件；所述的无人车通过imu模块判断出无人车的速度与位置，通过激光雷达扫描无人车的周围环境，将扫描得到的图片与地图信息进行对比，先去除地图上已知的障碍物；再对剩余障碍物采用粒子滤波的方式进行采样，预测障碍物相对无人车的运动速度及运动趋势；采用dwa算法结合障碍物粒子滤波的方式，避开移动障碍物；当无人车距离移动障碍物达到安全距离后，继续沿着全局最优路径进行移动。
8.进一步的，所述的无人车对剩余障碍物采用粒子滤波的方式进行采样，是由激光雷达向无人车四周进行扫描，识别出与地图上不同的障碍物，经过初始化、计算权重、重采样、状态转移的步骤后，粒子慢慢聚在了移动障碍物的真实位置，集中对不同于地图上的障碍物进行撒点，当障碍物偏离撒点的中心位置时，调整撒点的范围，并据此预测下一次撒点
的位置，求取障碍物的位置公式如下：
[0009][0010][0011][0012]
式中为第k秒激光雷达的旋转角，为第k秒无人车到障碍物距离，为障碍物所在平均旋转角，为障碍物初始旋转角，为障碍物终止旋转角，k为障碍物对激光雷达角度占比，为无人车初始旋转角，为无人车终止旋转角；通过对比k值的大小来确定散点调整的方向及幅度。
[0013]
进一步的，所述的初始化、计算权重、重采样、状态转移的具体操作步骤如下所示：
[0014]
s1.系统初始化后，由激光雷达向无人车四周撒点，随机分布，粒子代表假设的位置，完成初始化，撒点得到的结果与系统中加载的地图做对比后锁定新出现的障碍物；将激光雷达集中向新出现的障碍物集中撒点，产生定量的位置假设，在空间里记录粒子滤波估算障碍物的位置；
[0015]
s2.计算权重：产生定量的粒子假设，根据无人车所处的位置和到障碍物的距离，记录粒子的可能性，对所有的粒子都进行相同的评估，对于可能性大的粒子附一个较大的权重，较小可能性的粒子附较小的权重；
[0016]
s3.重采样：当障碍物偏离撒点的中心位置时，调整撒点的范围，抛弃旧粒子，生成新粒子，生成的新粒子集中出现在权重大的旧粒子位置上，并且有大量的新粒子叠加在同一个位置；
[0017]
s4.状态转移：无人车和障碍物继续前进，粒子也依据运动模型一起移动，根据传感器知道无人车的方向和速度范围。
[0018]
进一步的，通过集中对不同于地图上的障碍物进行撒点，可以判断该障碍物是否为动态障碍物，若不是动态障碍物则直接避让，若为动态障碍物则一并计算出动态障碍物的移动速度，并根据撒点的调整来预测障碍物的运动趋势；移动障碍物的速度求解公式如下：
[0019][0020][0021]
式中l
t
为当前时刻障碍物到无人车的距离，分别为当前时刻前1秒、2秒、3秒、4秒、5秒激光雷达所在角度，前1秒、2秒、3秒、4秒、5秒激光雷达所在角度，分别为为当前时刻前1秒、2秒、3秒、4秒、5秒无人车到障碍物的距离，v为当前障碍物移动速度，l
t-1
为当前时刻前t秒时刻障碍物到无人车的距离。
[0022]
进一步的，通过所述的对剩余障碍物采用粒子滤波的方式进行采样，预测障碍物相对无人车的运动速度及运动趋势，从而能确定无人车的运动方向，求解无人车运动方向
的公式如下：
[0023]
g(υ，ω)＝σ(α
·
h(υ,ω) β
·
d(υ,ω) γ
·
v(υ,ω))
[0024]
h(υ,ω)为方位角评价函数，d(υ,ω)为无人车与最近障碍物之间的距离，v(υ,ω)为轨迹对应的速度大小；
[0025]
对每一项除以每一项的总和，算出各自在自己类别上面的占比，然后相加，进行归一化，算出它们各自在自己类别上的得分占比，然后相加，使小车避开障碍物，朝着目标，以设定的速度进行行驶。
[0026]
进一步的，所述的确定无人车的运动方向和速度的情况下，从而能得到无人车的自身位置以及运行速度；根据无人车的自身位置以及运行速度，以粒子滤波的方式对障碍物进行采样，在发现粒子的平均分数突然降低了的时候，在全局再重新的撒一些粒子，通过imu模块测量的概率来评估增加粒子，即：
[0027]
p(z
t
|z
1:t-1
,u
1:t
,m)
[0028]
并将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，因为重要性权重是这个概率的随机估计，其平均值为式：
[0029][0030]
p为在某个位姿下，能够产生实际测量z
t
的概率，z
1:t-1
为传感器测量距离，u
1:t
为运动信息，m为1～m间的数，无意义，ω为测量概率。
[0031]
在运动控制系统中设置有无人车的运动模型，运动模型中，无人车可以前进和旋转，将相邻两点之间的运动轨迹看成直线，可以通过下式计算无人车走的距离，具体的计算公式如下：
[0032]
δs＝ν*δt；
[0033]
上式中，δs为无人车δt时间内移动的距离，ν为无人车的移动速度，δt为很小一段时间，无意义；
[0034]
变换成无人车的x,y坐标：
[0035]
δx＝νδtsinθ；
[0036]
δy＝υδtsinθ；
[0037]
上公式中，δx为δt内无人车相对于x轴的位移，δy为δt内无人车相对于y轴的位移；
[0038]
无人车下个状态的位置：
[0039]
x＝x' υδtcosθ；
[0040]
y＝y' υδtsinθ；
[0041]
θ＝θ ωδt；
[0042]
上列公式中，x’为上一时刻无人车位置横坐标，y’为上一时刻无人车位置纵坐标，θ为无人车总体移动方向，ω为δt时刻无人车移动角度。
[0043]
进一步的，所述的总控制系统基于stm32控制器，stm32控制器通过总线与树莓派平台连接，树莓派平台通过wi-fi无线网与上位机进行通讯，完成数据回传。
[0044]
进一步的，所述的惯性导航设备包括里程计和陀螺仪，里程计、陀螺仪和激光雷达测距装置通过信号线与总控制系统连接，向总控制系统提供无人车自身定位的原始数据。
[0045]
有益效果
[0046]
本发明提出的一种基于粒子滤波的无人车动态避障方法，与传统的现有技术相比较，其具有以下有益效果：
[0047]
(1)本技术方案通过惯性导航系统、激光雷达模块和imu模块的数据融合，无人车通过imu模块判断出无人车的速度与实时位置，通过激光雷达扫描无人车的周围环境，能得到无人车运行环境下的地图，并将扫描得到的图片与原先系统中的地图信息进行对比，先去除地图上已知的障碍物；并通过粒子滤波算法实现对障碍物的定位，结合dwa算法，控制无人车行动。
[0048]
(2)本技术方案通过粒子滤波步骤精确移动障碍物的移动，能够识别出动态障碍物，并将其考虑到路径规划中来，有效解决了其他采用将动态障碍物当静态障碍物处理出现的效率低及会出现路径规划过长的问题，使无人车可在整个工作环境下实现准确定位，有效完成任务。
[0049]
(3)本技术方案通过无人车运动控制系统中设置的无人车的运动模型，将相邻两点之间的运动轨迹看成直线，从而计算出无人车走的距离，对无人车的里程位移数据进行计算发送至总控制器进行比对，对无人车的里程位移数据进行验算，避免无人车在运行时，其里程位移数据出现误差，导致位姿也相应发生明显误差的情况。
附图说明
[0050]
图1为本发明中基于粒子滤波的定位流程示意图。
[0051]
图2为本发明中基于粒子滤波的采样流程示意图。
具体实施方式
[0052]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
[0053]
实施例：
[0054]
一种基于粒子滤波的无人车动态避障方法，所述的无人车设置有总控制系统，以及分别与总控制系统连接的运动控制系统、惯性导航系统、激光雷达模块和imu模块；所述的总控制系统中加载有地图和无人车操作系统ros控制软件。
[0055]
总控制系统基于stm32控制器，stm32控制器通过总线与树莓派平台连接，树莓派平台通过wi-fi无线网与上位机进行通讯，完成数据回传。
[0056]
惯性导航设备包括里程计和陀螺仪，里程计、陀螺仪和激光雷达测距装置通过信号线与总控制系统连接，向总控制系统提供无人车自身定位的原始数据。
[0057]
无人车通过imu模块判断出无人车的速度与位置，通过激光雷达扫描无人车的周围环境，将扫描得到的图片与地图信息进行对比，先去除地图上已知的障碍物；再对剩余障碍物采用粒子滤波的方式进行采样，产生定量的粒子假设，已经有了静态的地图，以及无人车到障碍物的距离，根据无人车所处的位置和到障碍物的距离，记录粒子的可能性，对所有的粒子都进行相同的评估，对于可能性大的粒子附一个较大的权重，较小可能性的粒子附较小的权重；预测障碍物相对无人车的运动速度及运动趋势；采用dwa算法结合障碍物粒子滤波的方式，避开移动障碍物；当无人车距离移动障碍物达到安全距离后，继续沿着全局最
优路径进行移动。
[0058]
所述的无人车对剩余障碍物采用粒子滤波的方式进行采样，是由激光雷达向无人车四周进行扫描，识别出与地图上不同的障碍物，经过下列的步骤后，粒子会聚集在移动障碍物的真实位置。
[0059]
s1.系统初始化后，由激光雷达向无人车四周撒点，随机分布，粒子代表假设的位置，完成初始化，撒点得到的结果与系统中加载的地图做对比后锁定新出现的障碍物；将激光雷达集中向新出现的障碍物集中撒点，产生定量的位置假设，在空间里记录粒子滤波估算障碍物的位置；
[0060]
s2.计算权重：产生定量的粒子假设，根据无人车所处的位置和到障碍物的距离，记录粒子的可能性，对所有的粒子都进行相同的评估，对于可能性大的粒子附一个较大的权重，较小可能性的粒子附较小的权重；
[0061]
s3.重采样：当障碍物偏离撒点的中心位置时，调整撒点的范围，抛弃旧粒子，生成新粒子，生成的新粒子集中出现在权重大的旧粒子位置上，并且有大量的新粒子叠加在同一个位置；
[0062]
s4.状态转移：无人车和障碍物继续前进，粒子也依据运动模型一起移动，根据传感器知道无人车的方向和速度范围。
[0063]
经过上述的初始化、计算权重、重采样、状态转移几个步骤，粒子慢慢聚在了移动障碍物的真实位置，集中对不同于地图上的障碍物进行撒点，当障碍物偏离撒点的中心位置时，调整撒点的范围，并据此预测下一次撒点的位置，求取障碍物的位置公式如下：
[0064][0065][0066][0067]
式中为第k秒激光雷达的旋转角，为第k秒无人车到障碍物距离，为障碍物所在平均旋转角，为障碍物初始旋转角，为障碍物终止旋转角，k为障碍物对激光雷达角度占比，为无人车初始旋转角，为无人车终止旋转角；通过对比k值的大小来确定散点调整的方向及幅度。
[0068]
通过集中对不同于地图上的障碍物进行撒点，可以判断该障碍物是否为动态障碍物，若不是动态障碍物则直接避让，若为动态障碍物则一并计算出动态障碍物的移动速度，并根据撒点的调整来预测障碍物的运动趋势；移动障碍物的速度求解公式如下：
[0069][0070][0071]
式中l
t
为当前时刻障碍物到无人车的距离，分别为当前时刻前1秒、2秒、3秒、4秒、5秒激光雷达所在角度，前1秒、2秒、3秒、4秒、5秒激光雷达所在角度，分别为当前时刻前1秒、2
秒、3秒、4秒、5秒无人车到障碍物的距离，v为当前障碍物移动速度，l
t-1
为当前时刻前t秒时刻障碍物到无人车的距离。
[0072]
通过对剩余障碍物采用粒子滤波的方式进行采样，预测障碍物相对无人车的运动速度及运动趋势，从而能确定无人车的运动方向，求解无人车运动方向的公式如下：
[0073]
g(υ，ω)＝σ(α
·
h(υ,ω) β
·
d(υ,ω) γ
·
v(υ,ω))
[0074]
h(υ,ω)为方位角评价函数，d(υ,ω)为无人车与最近障碍物之间的距离，v(υ,ω)为轨迹对应的速度大小；
[0075]
对每一项除以每一项的总和，算出各自在自己类别上面的占比，然后相加，进行归一化，算出它们各自在自己类别上的得分占比，然后相加，使小车避开障碍物，朝着目标，以设定的速度进行行驶。
[0076]
在确定无人车的运动方向和速度的情况下，从而能得到无人车的自身位置以及运行速度；根据无人车的自身位置以及运行速度，以粒子滤波的方式对障碍物进行采样，在发现粒子的平均分数突然降低了的时候，在全局再重新的撒一些粒子，通过imu模块测量的概率来评估增加粒子，即：
[0077]
p(z
t
|z
1:t-1
,u
1:t
,m)
[0078]
并将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，因为重要性权重是这个概率的随机估计，其平均值为式：
[0079][0080]
p为在某个位姿下，能够产生实际测量z
t
的概率，z
1:t-1
为传感器测量距离，u
1:t
为运动信息，m为1～m间的数，无意义，ω为测量概率。
[0081]
在运动控制系统中设置有无人车的运动模型，运动模型中，无人车可以前进和旋转，将相邻两点之间的运动轨迹看成直线，可以通过下式计算无人车走的距离，具体的计算公式如下：
[0082]
δs＝ν*δt；
[0083]
上式中，δs为无人车δt时间内移动的距离，ν为无人车的移动速度，δt为很小一段时间，无意义；
[0084]
变换成无人车的x,y坐标：
[0085]
δx＝νδtsinθ；
[0086]
δy＝υδtsinθ；
[0087]
上公式中，δx为δt内无人车相对于x轴的位移，δy为δt内无人车相对于y轴的位移；
[0088]
无人车下个状态的位置：
[0089]
x＝x' υδtcosθ；
[0090]
y＝y' υδtsinθ；
[0091]
θ＝θ ωδt；
[0092]
上列公式中，x’为无人车上一时刻位置横坐标，y’为无人车上一时刻位置纵坐标，θ为无人车总体移动方向，ω为δt时刻无人车移动角度。
[0093]
通过上式在得到无人车走的距离后，再将计算得到的距离发送给总控制系统进行核验。