[发明专利]基于直线偏离度方法的智能体路径规划方法

权利要求书

1.基于直线偏离度方法的智能体路径规划方法，其特征在于，包括以下步骤：

一、建立环境模型，即按照实际环境建立等比例缩小的模型，并对应建立坐标系；

根据原始图构建连通矩阵，矩阵元素只有0和1，1代表两个节点间相连通，0表示两个节点间不连通；

二、添加智能节点，删除与添加节点毫无关联的路线：

在原二维地形图中添加智能节点，并删去与添加节点毫无关联的路线，调整后的环境中障碍物数量不变，路线图以拓扑图形式展示；智能节点也是节点；

三、直线偏离度建模：

(3.1)规划的目的是使机器人由起点start，安全地、避免碰撞地沿一条较短路径到达终点end，定义从起点到终点的最佳路径为path，path＝{p₀，p₁，....p_n}，其中p₀是起点，也就是start；p_n是终点，也就是end；P_i是途中经过的第i个节点；

(3.2)将二维地形转换为拓扑图，并在拓扑图的基础上定义一个关联矩阵map，矩阵元素是表示两个相连通节点的距离；

定义候选集合s记录当前节点的所有相邻节点，并从中选出移动机器人下一步应要前往的节点位置；

(3.3)规定移动机器人下一时刻的目标节点就是拥有最小直线偏离度的节点；具体过程为：

步骤(3.3)所述规定移动机器人下一时刻的目标节点就是拥有最小直线偏离度的节点的过程如下：

定义直线偏离度Q，基准线Line，最小直线偏离度Qmin；规定移动机器人下一时刻的目标节点就是拥有最小直线偏离度Qmin的节点，

①基准线Line：当前移动机器人位置与下一个目标节点的连线；

②直线偏离度Q：

Q＝dis_ik+dis_kj‘-dis_ij‘ (3)

其中，i为起始点，k为中间节点，j‘为最终目标节点，dis表示两节点之间的距离；

③最小直线偏离度Qmin：

Q min＝min(dis_ik+dis_kj‘-dis_ij‘) (4)

其中，k∈[1，n]，n为候选集合s所含的节点的个数；

设i为起始点，k为中间节点，j‘为最终目标节点，计算偏离角度θ：

θ＝arctan((k2-k1)/(1+k1*k2)) (5)

其中，k1、k2分别是直线ik与直线ij‘的斜率，且他们的夹角不为90度，如果为90度时，k不存在，即k1*k2＝-1；

四、实现策略：

4.1、路径path实现策略：

(4.1.1)定义起始节点为start，终点为end，start加入到path中；

(4.1.2)把start能够到达的所有节点加入候选集合s中，通过计算θ去除掉不满足条件的路线，最后计算得到Qmin的节点作为移动机器人下一个前往的目标节点；

(4.1.3)把start替换成(4.1.2)中计算出的下一个目标节点，判断其坐标是否等于end，若相等则终止运算，把end节点添加到path中；否则把下一个目标节点添加到path中跳转至(4.1.2)继续执行，直到到达目标节点end；

4.2、基于连通判断的直线偏离度策略：

(4.2.1)将path中未进行过计算的第一个节点，记为当前运算的第一个节点，开始计算，根据连通矩阵进行判断，如果path中的第一个节点除了可以到达第二个节点之外，还可以无碰撞的到达path中的其余节点，则从该路线中的第一个节点开始向终点查找，直至找到一个可以直达的且距离该节点最远的节点，执行(4.2.2)；

(4.2.2)找到可以直达的最远的节点j，且j并不是当前节点的下一个节点，则把起始节点到j节点之间的其他非直达路线从path中移除，此时j为当前节点，若j为终止节点end，则跳转到(4.2.3)，否则跳转到(4.2.1)；

若没有找到符合条件的j，则判断当前节点是不是终止节点end，如果是，则跳转到(4.2.3)，否则跳转到(4.2.1)；

(4.2.3)最终找到的最佳路线，记为path^*。