[发明专利]一种基于机器人的跟随且同时避障的方法与系统

权利要求书

1.一种基于机器人的跟随同时避障的方法，其特征在于，包括：

S1:根据所述机器人当前位置与目标的位置关系，规划导航路径；

S2:根据机器人当前的状态，进行路径向前仿真；

S3:根据评价指标对每条仿真路径进行评分；

S4:选取得分最高的路径得到控制指令。

2.如权利要求1所述的基于机器人的跟随同时避障的方法，其特征在于，

所述S1:根据所述机器人当前位置与目标的位置关系，规划导航路径，包括：

S11:通过深度相机获取目标在map坐标系下的位置；

S12:将目标位置作为机器人导航的目标点的位置发送给movebase；

S13:将目标点输入到局部规划器；

S14:对局部代价地图进行处理；

S15:规划参考路径。

3.如权利要求1所述的基于机器人的跟随同时避障的方法，其特征在于，

所述S2:根据机器人当前的状态，进行路径向前仿真，包括：

S21:获取所述机器人当前状态；

S22:生成采样集；

S23:生成仿真路径集。

4.如权利要求3所述的基于机器人的跟随同时避障的方法，其特征在于，所述S23中，用户设定仿真步长还有总的仿真时间；假设机器人在总的仿真时间内匀速运动，对总的采样集内每个速度往前仿真，从而得到向前仿真路径集；包括以下步骤：已知机器人当前位姿(x₀,y₀,θ₀)，仿真步长Δt，仿真线速度与角速度(v_i,ω_k)，那么使用下方代码更新仿真路径中每个路径点的位置与朝向，

p_i,k＝[(x₀,y₀,θ₀) (x_i,k,1,y_i,k,1,θ_i,k,1) … (x_i,k,m,y_i,k,m,θ_i,k,m)]

x_i,k,j+1＝x_i,k,j+v_i·Δt·cos(θ_j)

y_i,k,j+1＝y_i,k,j+v_i·Δt·sin(θ_j)

θ_i,k,j+1＝θ_i,k,j+ω_k·Δt

其中，j＝0,1,...,m其中m·Δt≤t_sim，t_sim为仿真的总时长；相同的总时长的情况下，仿真步长越大，则路径的精度越低，但计算量越小。选择合适的步长，避免计算耗时超过控制周期。

5.如权利要求1所述的基于机器人的跟随同时避障的方法，其特征在于，

所述S3:根据评价指标对每条仿真路径进行评分，这一步骤中，所述评价指标包括：

a)获取代价地图上的代价值；

b)最终位姿的角度；

c)与路径的贴合程度；

d)所述机器人与目标点之间的距离；

e)远离障碍物的代价。

6.如权利要求5所述的基于机器人的跟随同时避障的方法，其特征在于，

所述b)最终位姿的角度的获取方法为：获取最后一个点的位姿的朝向角ψ，以及最后一个点的指向目标位置的向量的角度φ，评价项a为判断两个角度之差的绝对值其中θ_max表示用户期望的最大偏差角度。

7.如权利要求5所述的基于机器人的跟随同时避障的方法，其特征在于，

所述c)与路径的贴合程度的方法包括：

其中d_end为仿真路径最后一个点离目标点的距离，d2_max表示用户期望的最大距离。