一种基于运动模型的动态窗口局部轨迹规划方法及系统技术方案

本发明专利技术公开了一种基于运动模型的动态窗口局部轨迹规划方法及系统，包括构建运动模型；将速度控制在一定的范围，并采样速度推算出轨迹；采用评价函数评价每条所述轨迹，并选取出最优的轨迹，改进运动约束模型，替换单车模型，提高了预测轨迹的真实性及精确度；同时增加三层与逻辑停止判断，在到达位置、到达航向、速度满足等停止条件，解决实际规划停止状态车辆状态问题。

【技术实现步骤摘要】
一种基于运动模型的动态窗口局部轨迹规划方法及系统
本专利技术涉及自动驾驶运动规划的
，尤其涉及一种基于运动模型的动态窗口局部轨迹规划方法及系统。
技术介绍
动态窗口方法是一种基于速度的局部规划器，可计算达到目标所需的机器人的最佳无碰撞(允许)速度，将笛卡尔目标(x，y)转换成移动机器人的速度(v，w)命令。该算法有两个主要的步骤：计算有效的速度搜索空间，并选择最佳速度。搜索空间由一组能够产生安全的运动轨迹(即允许机器人在碰撞前停止)的速度构成，这些速度是机器人在其动态(动态窗口)的下一个时间片中可以实现的速度集；选择最佳速度以最大化机器人与障碍物的间隙，最大化速度并获得最接近目标的航向。现有的动态窗口规划运动约束模型基于单车模型，预测采样的轨迹精确度不高；且动态窗口规划在到达目标停止判断只考虑了车和终点距离，未考虑车的最终朝向状态以及速度停止条件，导致车辆最终停止不符合实际自动驾驶要求。
技术实现思路
本部分的目的在于概述本专利技术的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和专利技术名称中可能会做些简化或省略以避免使本部分、说明书摘要和专利技术名称的目的模糊，而这种简化或省略不能用于限制本专利技术的范围。鉴于上述现有动态窗口规划存在的问题，提出了本专利技术。因此，本专利技术解决的技术问题是：解决现有动态窗口规划预测采样的轨迹精确度不高，以及未考虑车的最终朝向状态以及速度停止条件，导致车辆最终停止不符合实际自动驾驶要求的问题。为解决上述技术问题，本专利技术提供如下技术方案：一种基于运动模型的动态窗口局部轨迹规划方法，包括：构建运动模型；将速度控制在一定的范围，并采样速度推算出轨迹；采用评价函数评价每条所述轨迹，并选取出最优的轨迹。作为本专利技术所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：作车辆圆弧轨迹运动的半径为R＝V/W，当W不为0时，构建的所述运动模型如下，X＝x-R*sin(yaw)+R*sin(yaw+Wt*dt)Y＝y-R*cos(yaw)+R*cos(yaw+Wt*dt)Yaw＝yaw+Wt*dt其中，在笛卡尔坐标系下X表示下一时刻的X轴位置，Y表示下一时刻的Y轴位置，Yaw表示下一时刻航向角，x表示上一时刻的X轴位置，y表示上一时刻的Y轴位置，yaw表示上一时刻航向角，R表示最小转弯半径，W表示旋转角速度，t表示时间。作为本专利技术所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：将速度控制在一定的范围，并采样速度推算出轨迹具体包括，限制车辆自身的最大速度和最小速度；限制车辆最大加速度和最大减速度；初步采样出速度；依据所述初步采样出的速度初步模拟出机器人运动轨迹；定义障碍物的位置后计算得到所述机器人到所述障碍物的距离；根据所述最大减速度判断出所述初步采样的速度是否合格，并选择出合格的速度，进一步限制速度范围；依据所述进一步限制的速度模拟出机器人的运动轨迹。作为本专利技术所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：限制车辆自身的所述最大速度和所述最小速度如下公式，Vm＝{v∈[vmin，vmax]，w∈「wmin，Wmax]}其中，Vmin表示最小速度，Vmax表示最大速度，Wmin表示最小旋转角速度，Wmax表示最大旋转角速度。作为本专利技术所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：限制车辆所述最大加速度和所述最大减速度如下公式，其中，Vc、Wc表示机器人当前的速度和旋转角速度，表示瞬时减速度，表示瞬时旋转减速度，表示瞬时加速度，表示瞬时旋转加速度。作为本专利技术所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：根据所述最大减速度判断出所述初步采样的速度是否合格具体为，依据所述障碍物的位置和所述机器人到所述障碍物的距离计算出在所述最大减速度的条件下，所述初步采样的速度能否在碰到所述障碍物之前降为0，若一组所述初步采样的速度能够在碰到所述障碍物之前降为0，则保留，否则抛去。作为本专利技术所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：判断所述初步采样的速度能否在碰到所述障碍物之前降为0的公式如下，其中，dist(v,w)表示在这组采样的速度v以及旋转角速度w下能够行驶的距离。作为本专利技术所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：所述机器人在往前模拟轨迹的这段时间内速度不变，直至下一时刻采样的规定新的速度命令。作为本专利技术所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：所述评价函数如下，G(v，w)＝σ(α·heading(v，w)+(β·dist(v，w)+γ·velocity(v，w))。为解决上述技术问题，本专利技术还提供如下技术方案：一种基于运动模型的动态窗口局部轨迹规划系统，包括：构建模块，用于构建运动模型；采样模块，用于进行速度的筛选采集；轨迹生成模块，用于根据所述采样模块采集的所述速度生成轨迹；评价模块，用于对所述轨迹生成模块生成的所述轨迹进行评价，并选取出最优的轨迹。本专利技术的有益效果：本专利技术改进运动约束模型，替换单车模型，提高了预测轨迹的真实性及精确度；同时增加三层与逻辑停止判断，在到达位置、到达航向、速度满足等停止条件，解决实际规划停止状态车辆状态问题。附图说明为了更清楚地说明本专利技术实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：图1为本专利技术提供的基于运动模型的动态窗口局部轨迹规划方法的流程示意图；图2为本专利技术提供的基于运动模型的动态窗口局部轨迹规划系统的模块示意图；图3为本专利技术提供的基于运动模型的动态窗口局部轨迹规划方法中在最大减速度条件下进行再次速度限制的弧线示意图；图4为本专利技术提供的基于运动模型的动态窗口局部轨迹规划方法的采样轨迹示意图；图5为本专利技术提供的基于运动模型的动态窗口局部轨迹规划方法的评价函数中的方位角评价函数示意图；图6为本专利技术提供的基于运动模型的动态窗口局部轨迹规划方法的一运行效果图；图7本专利技术提供的基于运动模型的动态窗口局部轨迹规划方法的又一运行效果图；图8本专利技术提供的基于运动模型的动态窗口局部轨迹规划方法的再一运行效果图。具体实施方式为使本专利技术的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本专利技术的具体实施方式做详细的说明，显然所描述的实施例是本专利技术的一部分实施例，而不是全部实施例。基于本专利技术中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本专利技术的保护的范围。在下面的描述中阐本文档来自技高网...

【技术保护点】
1.一种基于运动模型的动态窗口局部轨迹规划方法，其特征在于：包括，/n构建车辆运动模型；/n将车辆速度控制在一定的范围，并采样速度推算出轨迹；/n采用评价函数评价每条所述轨迹，并选取出最优的轨迹。/n

【技术特征摘要】
1.一种基于运动模型的动态窗口局部轨迹规划方法，其特征在于：包括，
构建车辆运动模型；
将车辆速度控制在一定的范围，并采样速度推算出轨迹；
采用评价函数评价每条所述轨迹，并选取出最优的轨迹。


2.根据权利要求1所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：定义车辆圆弧轨迹运动的半径为R＝V/W，当W不为0时，
构建的所述运动模型如下，
X＝x-R*sin(yaw)+R*sin(yaw+Wt*dt)
Y＝y-R*cos(yaw)+R*cos(yaw+Wt*dt)
Yaw＝yaw+Wt*dt
其中，在笛卡尔坐标系下X表示下一时刻的X轴位置，Y表示下一时刻的Y轴位置，Yaw表示下一时刻航向角，x表示上一时刻的X轴位置，y表示上一时刻的Y轴位置，yaw表示上一时刻航向角，R表示最小转弯半径，W表示旋转角速度，t表示时间。


3.根据权利要求1所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：将速度控制在一定的范围，并采样速度推算出轨迹包括，
限制车辆自身的最大速度和最小速度；
限制车辆最大加速度和最大减速度；
初步采样出速度；
依据所述初步采样出的速度初步模拟出机器人运动轨迹；
定义障碍物的位置后计算得到所述机器人到所述障碍物的距离；
根据所述最大减速度判断出所述初步采样的速度是否合格，并选择出合格的速度，进一步限制速度范围；
依据所述进一步限制的速度模拟出机器人的运动轨迹。


4.根据权利要求3所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：限制车辆自身的所述最大速度和所述最小速度如下公式，
Vm＝{v∈[Vmin，Vmax]，w∈[Wmin，Wmax]}
其中，Vmin表示最小速度，Vmax表示最大速度，Wmin表示最小旋转角速度，Wmax表示最大旋转角速度。


5.根据权利要求3所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：限制车辆所述最大加...

【专利技术属性】
技术研发人员：吴月路，
申请(专利权)人：的卢技术有限公司，
类型：发明
国别省市：江苏;32