[发明专利]一种基于动态窗口与虚拟目标点的避障方法

摘要

基于动态窗口与虚拟目标点的避障方法，利用虚拟目标点以引导机器人前进，结合动态窗口下达机器人的运动指令，使机器人避开障碍物到达目标点。首先，机器人根据其传感器上反馈的有关障碍物的角度与距离信息预测障碍物的运动轨迹；然后根据对障碍物的轨迹预测，结合机器人的运动状态与真实目标点的位置，生成多个虚拟目标点，并通过评价函数综合考虑机器人的朝向和虚拟目标点与真实目标点的距离，筛选出最优的虚拟目标；最后机器人根据对障碍物的轨迹预测与虚拟目标点的位置，通过动态窗口法生成机器人下一时刻的控制指令集，并通过评价函数综合考虑机器人的朝向、运行速度与其和目标点的距离，筛选出机器人下一时刻最优的控制指令。

主权项

一种基于动态窗口与虚拟目标点的避障方法，具体步骤如下：步骤1：预测障碍物运动；通过利用机器人传感器上反馈的有关障碍物的距离与角度信息预测障碍物的运动；定义在t＝ti时刻机器人的坐标为其前进方向与世界坐标系x轴的夹角为θ(ti)；通过机器人的传感器反馈有关障碍的信息为[α(ti) d(ti)]；机器人传感器的检测范围为rd；其中，i为正整数，表示机器人处于开始工作后的第i个工作周期；xr(ti)表示t＝ti时刻机器人坐标中的横坐标；yr(ti)表示t＝ti时刻机器人坐标中的横坐标；α(ti)为t＝ti时刻机器人的朝向与障碍物之间的角度；d(ti)为t＝ti时刻机器人与障碍物的距离；记在t＝ti时刻机器人的位置为O,障碍物的位置为A；在t＝ti+1时刻机器人的位置为O′,障碍物的位置为A′，障碍物运动矢量为其中表示由A至O的向量；表示由O至O′的向量；表示由O′至A′的向量；定义在t＝ti+k时刻下对障碍物A的预测坐标为obsA(ti+k)，则：其中，t＝ti+k中k为正整数，表示机器人对障碍物的预测是t＝ti后，第k个运动周期；表示由A至A′的向量；步骤2：生成虚拟目标点；定义机器人的半径为rr，在t＝ti时刻第j个虚拟目标点的坐标VGj=xvgj(ti)yvgj(ti)=xr(ti)yr(ti)+rrcos(j2πm)sin(j2πm),(j=1,2,3,......,m)---(6),]]>其中，m为虚拟坐标点的个数；为t＝ti时刻虚拟目标点VGj的横坐标，为t＝ti时刻虚拟目标点VGj的纵坐标；虚拟目标点VGj的评价函数G1(VGj)为：G1(VGj)＝α1heading1(VGj)+β1dist1(VGj)  (7)，heading1(VGj)=1θVGjΣn=1mθVGn---(8),]]>dist1(VGj)=1dVGjΣn=1mdVGn---(9),]]>其中表示机器人和第j个虚拟目标点VGj的连线与机器人和真实目标点连线的角度；表示虚拟目标点与真实目标点之间的距离；α1与β1为评价函数G1(VGj)的两个可调参数；最终选择评价函数值最小的VGj作为最优的虚拟目标点；步骤3：生成动态窗口；定义机器人的运动学参数为：其中，υmax与ωmax分别表示机器人的最大线速度与角速度；与表示机器人的最大线加速度与角加速度；accυ与accω表示机器人线速度与角速度的最小分辨率；定义机器人在t＝ti时刻的速度为[υr(ti) ωr(ti)]，则在t＝ti+1时刻下，机器人速度的动态窗口υd为：其中，Δt为相邻时刻间的时间间隔；动态窗口υd的评价函数G2(υ,ω)为：G2(υ,ω)＝α2heading2(υ,ω)+β2dist2(υ,ω)+γvelocity2(υ,ω)  (11)，heading2(υ,ω)=1-1γkΣn=1lγn---(12),]]>dist2(υ,ω)=1ρkΣn=1lρn---(13),]]>velocity2(υ,ω)=1υr(ti)kΣn=1lυr(ti)n---(14),]]>其中，l为动态窗口υd中备选速度的个数；γk表示第k组来自动态窗口υd的速度下机器人的朝向角与其和目标点连线夹角的角度；ρk表示第k组来自动态窗口υd的速度下机器人与最近一个障碍物的距离；υr(ti)k表示第k组来自动态窗口υd的速度下机器人的速度值；α2，β2与γ为评价函数G2(υ,ω)的三个可调参数；最终选择评价函数值最大的速度[υr(ti+1) ωr(ti+1)]作为机器人在t＝ti+1时刻的控制量；在速度控制量[υr(ti+1) ωr(ti+1)]下，机器人位姿更新如下：当ωr(ti+1)≠0：xr(ti+1)yr(ti+1)θ(ti+1)=xr(ti)yr(ti)θ(ti)-υr(ti+1)ωr(ti+1)sin(θ(ti))-sin(θ(ti)+ωr(ti+1)Δt)cos(θ(ti))+cos(θ(ti)+ωr(ti+1)Δt)0+00ωr(ti+1)Δt---(15),]]>当ωr(ti+1)＝0：xr(ti+1)yr(ti+1)θ(ti+1)=xr(ti)yr(ti)θ(ti)+υr(ti+1)Δtcos(θ(ti))sin(θ(ti))0---(16);]]>步骤4：判断是否到达目标位置；定义机器人在t＝ti+1时刻，机器人与目标点的距离为d：d=(xr(ti+1)-xg)2+(yr(ti+1)-yg)2---(17),]]>其中，xg为目标点的横坐标，yg为目标点的纵坐标；定义dreach判断机器人是否到达目标位置：若d≤dreach，则机器人到达目标位置，结束本避障方法；若d＞dreach，则机器人还未到目标位置，需要继续迭代步骤1、步骤2与步骤3，直至d≤dreach。