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

权利要求书

1.一种基于动态窗口与虚拟目标点的避障方法，具体步骤如下：

步骤1：预测障碍物运动；

通过利用机器人传感器上反馈的有关障碍物的距离与角度信息预测障碍物的运动；定义在t＝t_i时刻机器人的坐标为其前进方向与世界坐标系x轴的夹角为θ(t_i)；通过机器人的传感器反馈有关障碍的信息为[α(t_i) d(t_i)]；机器人传感器的检测范围为r_d；其中，i为正整数，表示机器人处于开始工作后的第i个工作周期；x_r(t_i)表示t＝t_i时刻机器人坐标中的横坐标；y_r(t_i)表示t＝t_i时刻机器人坐标中的纵坐标；α(t_i)为t＝t_i时刻机器人的朝向与障碍物之间的角度；d(t_i)为t＝t_i时刻机器人与障碍物的距离；记在t＝t_i时刻机器人的位置为O,障碍物的位置为A；在t＝t_i+1时刻机器人的位置为O′,障碍物的位置为A′，障碍物运动矢量为

其中表示由A至O的向量；表示由O至O′的向量；表示由O′至A′的向量；定义在t＝t_i+k时刻下对障碍物A的预测坐标为obsA(t_i+k)，则：

其中，t＝t_i+k中k为正整数，表示机器人对障碍物的预测是t＝t_i后，第k个运动周期；表示由A至A′的向量；

步骤2：生成虚拟目标点；

定义机器人的半径为r_r，在t＝t_i时刻第j个虚拟目标点的坐标

其中，j＝1,2,3,......,m；m为虚拟坐标点的个数；为t＝t_i时刻虚拟目标点VG_j的横坐标，为t＝t_i时刻虚拟目标点VG_j的纵坐标；虚拟目标点VG_j的评价函数G₁(VG_j)为：

G₁(VG_j)＝α₁ heading₁(VG_j)+β₁ dist₁(VG_j) (7)，

其中表示机器人和第j个虚拟目标点VG_j的连线与第j个虚拟目标点VG_j和真实目标点连线的角度；表示虚拟目标点与真实目标点之间的距离；α₁与β₁为评价函数G₁(VG_j)的两个可调参数；最终选择评价函数值最小的VG_j作为最优的虚拟目标点；

步骤3：生成动态窗口；

定义机器人的运动学参数为：其中，υ_max与ω_max分别表示机器人的最大线速度与角速度；与表示机器人的最大线加速度与角加速度；acc_υ与acc_ω表示机器人线速度与角速度的最小分辨率；定义机器人在t＝t_i时刻的速度为[υ_r(t_i) ω_r(t_i)]，则在t＝t_i+1时刻下，机器人速度的动态窗口υ_d为：

其中，△t为相邻时刻间的时间间隔；动态窗口υ_d的评价函数G₂(υ,ω)为：

G₂(υ,ω)＝α₂ heading₂(υ,ω)+β₂ dist₂(υ,ω)+γvelocity₂(υ,ω) (11)，

其中，l为动态窗口υ_d中备选速度的个数；γ_k表示第k组来自动态窗口υ_d的速度下机器人的朝向角与其和目标点连线夹角的角度；ρ_k表示第k组来自动态窗口υ_d的速度下机器人与最近一个障碍物的距离；υ_r(t_i)_k表示第k组来自动态窗口υ_d的速度下机器人的速度值；α₂，β₂与γ为评价函数G₂(υ,ω)的三个可调参数；最终选择评价函数值最大的速度[υ_r(t_i+1) ω_r(t_i+1)]作为机器人在t＝t_i+1时刻的控制量；

在速度控制量[υ_r(t_i+1) ω_r(t_i+1)]下，机器人位姿更新如下：

当ω_r(t_i+1)≠0：

当ω_r(t_i+1)＝0：

步骤4：判断是否到达目标位置；

定义机器人在t＝t_i+1时刻，机器人与目标点的距离为d：

其中，x_g为目标点的横坐标，y_g为目标点的纵坐标；定义d_reach判断机器人是否到达目标位置：若d≤d_reach，则机器人到达目标位置，结束本避障方法；若dd_reach，则机器人还未到目标位置，需要继续迭代步骤1、步骤2与步骤3，直至d≤d_reach。