本发明涉及自动控制与机器人导航,尤其涉及一种速度采样稀疏方法及其路径规划算法、存储介质、电子设备。
背景技术:
1、目前机器人的自动驾驶规控算法上主要分为全局规划和局部规划,其中局部规划作为控制算法,有着更高的效率和求解车辆精确运动学解的需求,现有的局部规划的主流算法有dwa(dynamic window approaches)和teb(timed elastic band)算法,其中dwa算法的基本思路为:在机器人工作空间进行速度离散采样,在代价地图中预测模拟下一段时间内生成的所有运动轨迹(此处的所有运动轨迹为合法运动轨迹,非法轨迹指障碍物发生碰撞的轨迹),并通过评价函数对每个轨迹进行评价,挑出评价得分最高的轨迹并发送相应速度给移动底座,重复上面步骤直至到达目的地。由于机器人工作空间进行速度离散采样是等密度采样,如果要求解更精确的车辆运动学的话,需要的采样数过多,造成性能上的浪费。
技术实现思路
1、为此,需要提供一种速度采样稀疏方法,解决因速度进行等密度采样,造成采样数过多,造成性能上的浪费的问题。
2、为实现上述目的,本发明提供了一种速度采样稀疏方法,其包括以下步骤:
3、通过机器人当前速度、最大限制速度、最小限制速度、控制周期、最大加速度以及最小加速度获得速度采样区间;
4、通过速度采样区间和机器人当前速度获得概率密度采样区间;
5、通过高斯分布的累积分布函数获得概率密度采样区间对应的分布区间;
6、将分布区间进行等分,获得分布区间内的采样点;
7、通过高斯分布的累积分布函数的逆函数获得采样点对应采样点速度;
8、通过采样点速度和当前速度获得机器人采样速度。
9、进一步地,通过机器人当前速度、最大限制速度、最小限制速度、控制周期、最大加速度以及最小加速度获得速度采样区间,包括以下步骤:
10、计算当前速度在控制周期内的最小速度和最大速度:
11、控制周期内的最小速度=当前速度+控制周期*最小加速度;
12、控制周期内的最大速度=当前速度+控制周期*最大加速度;
13、将控制周期内的最小速度和最小限制速度进行比较,若控制周期内的最小速度小于最小限制速度,则将最小限制速度作为速度采样区间的下界值,否则将控制周期内的最小速度作为速度采样区间的下界值;
14、将控制周期内的最大速度和最大制速度进行比较,若控制周期内的最大速度大于最大限制速度,则将最大限制速度作为速度采样区间的上界值,否则将控制周期内的最大速度作为速度采样区间的上界值;
15、获得速度采样区间。
16、进一步地,所述通过速度采样区间和机器人当前速度获得概率密度采样区间,包括以下步骤:将速度采样区间减去当前速度,获得概率密度采样区间。
17、进一步地,通过高斯分布的累积分布函数获得概率密度采样区间对应的分布区间,包括以下步骤:
18、若随机变量x服从标准差为σ的高斯分布;
19、则累积分布函数:
20、其中,σ>0;
21、将概率密度采样区间输入累积分布函数,即概率密度采样区间的下界值<x<概率密度采样区间的上界值;
22、获得分布区间。
23、进一步地,将分布区间进行等分,获得分布区间内的采样点,包括以下步骤:
24、设定采样数量;
25、按采样数量对分布区间进行等分;
26、将分布区间等分的边界值作为分布区间内的采样点。
27、进一步地,通过高斯分布的累积分布函数的逆函数获得采样点对应采样点速度,包括以下步骤:
28、若随机变量x服从标准差为σ的高斯分布;
29、则累积分布函数:
30、其中,σ>0;
31、将概率密度采样区间输入累积分布函数的逆函数,即取y=分布区间内的采样点;
32、求得x即为采样点速度。
33、进一步地,通过采样点速度和当前速度获得机器人采样速度,包括以下步骤:将采样点速度加上当前速度,获得机器人采样速度。
34、一种路径规划算法,机器人采样速度应用上述速度采样稀疏方法。
35、存储介质,所述存储介质存储有计算机程序,所述计算机程序被处理器执行时实现上述速度采样稀疏方法的步骤。
36、电子设备,其包括存储器、处理器,所述存储器上存储有计算机程序,所述计算机程序被处理器执行时实现如上述速度采样稀疏方法的步骤。
37、区别于现有技术,上述技术方案通过机器人当前速度、最大限制速度、最小限制速度、控制周期、最大加速度以及最小加速度获得速度采样区间,在速度采样区间里进行采样;采样的时候,将当前速度作为机器人采样速度的概率密度的峰值,通过高斯分布的累积分布函数获得机器人采样速度,保障当前速度附近的采样密度,远离当前速度的采样密度被稀疏;减少速度总采样数的同时,实现了指定重要方向上的采样密度保持不变甚至更高,降低了因等密度采样速度产生的路径规划算法的算力消耗,同时控制采样速度的精度,平衡了算力消耗和控制精度,有效控制成本,提高机器人采样速度的有效性,保证机器人行驶安全。
1.一种速度采样稀疏方法,其特征在于,包括以下步骤:
2.根据权利要求1所述的速度采样稀疏方法,其特征在于,通过机器人当前速度、最大限制速度、最小限制速度、控制周期、最大加速度以及最小加速度获得速度采样区间,包括以下步骤:
3.根据权利要求1所述的速度采样稀疏方法,其特征在于,所述通过速度采样区间和机器人当前速度获得概率密度采样区间,包括以下步骤:将速度采样区间减去当前速度,获得概率密度采样区间。
4.根据权利要求1所述的速度采样稀疏方法,其特征在于,通过高斯分布的累积分布函数获得概率密度采样区间对应的分布区间,包括以下步骤:
5.根据权利要求1所述的速度采样稀疏方法,其特征在于,将分布区间进行等分,获得分布区间内的采样点,包括以下步骤:
6.根据权利要求1所述的速度采样稀疏方法,其特征在于,通过高斯分布的累积分布函数的逆函数获得采样点对应采样点速度,包括以下步骤:
7.根据权利要求1所述的速度采样稀疏方法,其特征在于,通过采样点速度和当前速度获得机器人采样速度,包括以下步骤:将采样点速度加上当前速度,获得机器人采样速度。
8.一种路径规划算法,其特征在于,机器人采样速度应用权利要求1-7任意一项所述速度采样稀疏方法。
9.存储介质,其特征在于:所述存储介质存储有计算机程序,所述计算机程序被处理器执行时实现如权利要求1-7任意一项所述速度采样稀疏方法的步骤。
10.电子设备,其特征在于:包括存储器、处理器,所述存储器上存储有计算机程序,所述计算机程序被处理器执行时实现如权利要求1-7任意一项所述速度采样稀疏方法的步骤。