[发明专利]基于行人轨迹预测的全向移动机器人路径规划算法

权利要求书

1.基于行人轨迹预测的全向移动机器人路径规划算法，其特征在于,包括以下步骤：

S1、利用机器人底盘和安装在底盘上的麦克纳姆轮进行运动学建模；

S2、机器人根据当前坐标、目标位置坐标进行路径规划；

S3、机器人通过激光雷达模块对视野内的行人进行行为轨迹预测，计算行人一定时间内可能处于的范围；

S4、机器人根据行人可能处于的范围进行躲避行人路径规划；

步骤S3中，所述行人轨迹预测通过行为轨迹预测算法来实现，当系统发现人时，便输出行人与激光雷达模块之间的相对坐标，且相对坐标值位于机器人本体坐标系，具体方法如下：

设当前及前三个行人在全局坐标系中的坐标分别为p_k＝(x_k,y_k)，p_k-1＝(x_k-1,y_k-1)，p_k-2＝(x_k-2,y_k-2)，p_k-3＝(x_k-3,y_k-3)，系统采集到这几个坐标的时间分别为t₄，t₃，t₂，t₁；根据这四组数据，可以计算这一段时间内机器人的平均速度：

设预测时间为则时间后，人可能处于的位置坐标p_{pre_k}＝(x_{pre_k},y_{pre_k})可以表示为：

以p_{pre_k}为圆心，半径为r画圆，圆的范围内为预测时间后，人可能处于的范围S_k；

当人跟丢，或者重新跟踪时，需要重新开始循环，并初始化所有变量；

步骤S4中，所述躲避行人路径规划通过算法实现，具体如下：

S301、根据局部路径规划算法，机器人规划出未来Δt时间的最佳轨迹，设该轨迹的最后一个点p_i坐标为(x_i,y_i)；若存在行人跟踪结果，并假设此时预测行人的坐标点p_pre坐标为(x_pre,y_pre)；

S302、若则机器人路径规划点不会在行人预测点的辐射圈内，机器人相对安全，以最佳轨迹规划行走即可；

S303、若则机器人路径规划点位于辐射圈内或边缘，下一刻可能会碰到行人，此时要做快速逃离辐射圈的路径规划；

当机器人位于行人预测点辐射圈内或边缘时，机器人运动线速度沿圆的切线方向最快的逃离辐射圈，具体方法如下：

设机器人局部路径预测点最后一个点的坐标为p_i＝(x_i,y_i)，行人预测点p_pre坐标为(x_pre,y_pre)，此时两点间距离设机器人运动最大线速度大小为v_max，最小线速度大小为v_min，且两点间距离、线速度之间存在线性关系：

因此，此时机器人的逃离线速度大小应为：

速度方向分为两个分别为v₁和v₂，根据旋转矩阵分别得到两个速度方向的单位向量：

根据A式和B式，得到两个切线方向的速度和

根据局部路径规划算法，分别以和为中心，取正、负各30°范围，作为规划速度进行机器人局部路径规划。

2.根据权利要求1所述的基于行人轨迹预测的全向移动机器人路径规划算法，其特征在于：所述步骤S1中，机器人底盘安装有三个麦克纳姆轮，所述麦克纳姆轮为左旋麦克纳姆轮。