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

技术特征：
1.一种基于粒子滤波的无人车动态避障方法，其特征在于：所述的无人车设置有总控制系统，以及分别与总控制系统连接的运动控制系统、惯性导航系统、激光雷达模块和imu模块；所述的总控制系统中加载有地图和无人车操作系统ros控制软件；所述的无人车通过imu模块判断出无人车的速度与位置，通过激光雷达扫描无人车的周围环境，将扫描得到的图片与地图信息进行对比，先去除地图上已知的障碍物；再对剩余障碍物采用粒子滤波的方式进行采样，预测障碍物相对无人车的运动速度及运动趋势；采用dwa算法结合障碍物粒子滤波的方式，避开移动障碍物；当无人车距离移动障碍物达到安全距离后，继续沿着全局最优路径进行移动。2.根据权利要求1所述的一种基于粒子滤波的无人车动态避障方法，其特征在于：所述的无人车对剩余障碍物采用粒子滤波的方式进行采样，是由激光雷达向无人车四周进行扫描，识别出与地图上不同的障碍物，经过初始化、计算权重、重采样、状态转移的步骤后，粒子慢慢聚在了移动障碍物的真实位置，集中对不同于地图上的障碍物进行撒点，当障碍物偏离撒点的中心位置时，调整撒点的范围，并据此预测下一次撒点的位置，求取障碍物的位置公式如下：置公式如下：置公式如下：式中为第k秒激光雷达的旋转角，为第k秒无人车到障碍物距离，为障碍物所在平均旋转角，为障碍物初始旋转角，为障碍物终止旋转角，k为障碍物对激光雷达角度占比，为无人车初始旋转角，为无人车终止旋转角；通过对比k值的大小来确定散点调整的方向及幅度。3.根据权利要求2所述的一种基于粒子滤波的无人车动态避障方法，其特征在于：所述的初始化、计算权重、重采样、状态转移的具体操作步骤如下所示：s1.初始化后，由激光雷达向无人车四周撒点，随机分布，粒子代表假设的位置，完成初始化，撒点得到的结果与系统中加载的地图做对比后锁定新出现的障碍物；将激光雷达集中向新出现的障碍物集中撒点，产生定量的位置假设，在空间里记录粒子滤波估算障碍物的位置；s2.计算权重：产生定量的粒子假设，根据无人车所处的位置和到障碍物的距离，记录粒子的可能性，对所有的粒子都进行相同的评估，对于可能性大的粒子附一个较大的权重，较小可能性的粒子附较小的权重；s3.重采样：当障碍物偏离撒点的中心位置时，调整撒点的范围，抛弃旧粒子，生成新粒子，生成的新粒子集中出现在权重大的旧粒子位置上，并且有大量的新粒子叠加在同一个位置；s4.状态转移：无人车和障碍物继续前进，粒子也依据运动模型一起移动，根据传感器知道无人车的方向和速度范围。
4.根据权利要求2所述的一种基于粒子滤波的无人车动态避障方法，其特征在于：通过集中对不同于地图上的障碍物进行撒点，可以判断该障碍物是否为动态障碍物，若不是动态障碍物则直接避让，若为动态障碍物则一并计算出动态障碍物的移动速度，并根据撒点的调整来预测障碍物的运动趋势；移动障碍物的速度求解公式如下：解公式如下：式中l
t
为当前时刻障碍物到无人车的距离，分别为当前时刻前1秒、2秒、3秒、4秒、5秒激光雷达所在角度，秒、2秒、3秒、4秒、5秒激光雷达所在角度，分别为当前时刻前1秒、2秒、3秒、4秒、5秒无人车到障碍物的距离，v为当前障碍物移动速度，l
t-1
为当前时刻前t秒时刻障碍物到无人车的距离。5.根据权利要求1所述的一种基于粒子滤波的无人车动态避障方法，其特征在于：通过所述的对剩余障碍物采用粒子滤波的方式进行采样，预测障碍物相对无人车的运动速度及运动趋势，从而能确定无人车的运动方向，求解无人车运动方向的公式如下：g(υ，ω)＝σ(α
·
h(υ，ω) β
·
d(υ，ω) γ
·
v(υ，ω))h(υ，ω)为方位角评价函数，d(υ，ω)为无人车与最近障碍物之间的距离，v(υ，ω)为轨迹对应的速度大小；对每一项除以每一项的总和，算出各自在自己类别上面的占比，然后相加，进行归一化，算出它们各自在自己类别上的得分占比，然后相加，使小车避开障碍物，朝着目标，以设定的速度进行行驶。6.根据权利要求5所述的一种基于粒子滤波的无人车动态避障方法，其特征在于：所述的确定无人车的运动方向和速度的情况下，从而能得到无人车的自身位置以及运行速度；根据无人车的自身位置以及运行速度，以粒子滤波的方式对障碍物进行采样，在发现粒子的平均分数突然降低了的时候，在全局再重新的撒一些粒子，通过imu模块测量的概率来评估增加粒子，即：p(z
t
|z
1：t-1
，u
1：t
，m)并将其与平均测量概率联系起来，在粒子滤波中这个数量的近似容易根据重要性因子获取，因为重要性权重是这个概率的随机估计，其平均值为式：p为在某个位姿下，能够产生实际测量z
t
的概率，z
1：t-1
为传感器测量距离，u
1：t
为运动信息，m为1～m间的数，无具体意义，ω为测量概率。7.根据权利要求1所述的一种基于粒子滤波的无人车动态避障方法，其特征在于：所述的运动控制系统中设置有无人车的运动模型，无人车可以前进和旋转，将相邻两点之间的运动轨迹看成直线，无人车走的距离：δs＝v*δt；上式中，δs为无人车δt时间内移动的距离，v为无人车的移动速度，δt为很小一段时间，无具体意义；
变换成无人车的x，y坐标：δx＝vδt sinθ：δy＝υδt sinθ：上公式中，δx为δt内无人车相对于x轴的位移，δy为δt内无人车相对于y轴的位移；无人车下个状态的位置：x＝x
′
υδt cosθ；y＝y
′
υδt sinθ；θ＝θ ωδt；上列公式中，x’为上一时刻无人车位置横坐标，y’为上一时刻无人车位置纵坐标，θ为无人车总体移动方向，ω为δt时刻无人车移动角度。8.根据权利要求1所述的一种基于粒子滤波的无人车动态避障方法，其特征在于：所述的总控制系统基于stm32控制器，stm32控制器通过总线与树莓派平台连接，树莓派平台通过wi-fi无线网与上位机进行通讯，完成数据回传。9.根据权利要求1所述的一种基于粒子滤波的无人车动态避障方法，其特征在于：所述的惯性导航设备包括里程计和陀螺仪，里程计、陀螺仪和激光雷达测距装置通过信号线与总控制系统连接，向总控制系统提供无人车自身定位的原始数据。

技术总结
本发明公开了一种基于粒子滤波的无人车动态避障方法，所述的无人车设置有总控制系统，以及分别与总控制系统连接的运动控制系统、惯性导航系统、激光雷达模块和IMU模块；所述的总控制系统中加载有地图和无人车操作系统ROS控制软件；所述的无人车通过IMU模块判断出无人车的速度与位置，通过激光雷达扫描无人车的周围环境，将扫描得到的图片与地图信息进行对比，先去除地图上已知的障碍物；再对剩余障碍物采用粒子滤波的方式进行采样，预测障碍物相对无人车的运动速度及运动趋势；采用DWA算法结合障碍物粒子滤波的方式，避开移动障碍物；当无人车距离移动障碍物达到安全距离后，继续沿着全局最优路径进行移动。继续沿着全局最优路径进行移动。继续沿着全局最优路径进行移动。


技术研发人员：魏晓倩 赵志国 毛康康 王瑞 万小康 庞敏
受保护的技术使用者：淮阴工学院
技术研发日：2021.11.26
技术公布日：2022/4/1