一种基于A*与改进人工势场法的编队路径规划控制方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN114326726A(43)申请公布日2022.04.12(21)申请号202111597829.0(22)申请日2021.12.24(71)申请人杭州电子科技大学地址310018浙江省杭州市下沙高教园区2号大街(72)发明人童啸龙余善恩(74)专利代理机构杭州君度专利代理事务所(特殊普通合伙)33240代理人杨舟涛(51)Int.Cl.G05D1/02(2020.01)权利要求书2页说明书5页附图6页(54)发明名称一种基于A*与改进人工势场法的编队路径规划控制方法(57)摘要本发明公开了一种基于A*与改进人工势场法的编队路径规划控制方法，主要由全局路径规划和局部路径两部分组成，采用A*算法作为全局路径规划算法，生成最优路径，引导多机器人编队安全、快速的到达目标点。将最优路径划分为多个子目标点，构成子目标点序列，采用改进的人工势场法作为局部路径规划算法，将每一个子目标点作为人工势场法的虚拟目标点，在静态地图中有效避开障碍物，引导多机器人编队安全通过障碍物区域，保证路径规划的顺利进行，解决传统人工势场法目标点不可达的问题。并且缩短了多机器人系统编队路径规划的运动时间和运动距离。CN114326726ACN114326726A权利要求书1/2页1.一种基于A*与改进人工势场法的编队路径规划控制方法，其特征在于：具体包括以下步骤：步骤一：指定一个机器人为编队领导者，然后采用代数图论法进行多机器人系统的编队；步骤二：以编队中领导者的机器人位置作为父节点，通过A*算法规划出全局最优路径；步骤三：在步骤二得到的全局最优路径中划分多个子目标点，然后将子目标点作为虚拟目标点，多机器人系统在改进人工势场法的引导下向该子目标点移动；所述改进人工势场法在人工势场法的障碍物斥力势场函数中加入了机器人位置与障碍物位置之间的相对距离作为调节因子，使机器人驶向虚拟目标点的过程中，斥力分量逐渐减小，而引力分量逐渐增大，当机器人到达目标点时，引力分量达到最大，斥力分量逐渐减至0；s3.1、在改进人工势场法的引导下，编队领导者向虚拟目标点移动，系统中的其余机器人跟随编队领导者的路径移动；当系统中的任意一个机器人到达虚拟目标点时，下一个子目标点出现；否则，直至本次程序迭代运行完成后，下一个子目标点出现；s3.2、在多机器人系统朝子目标点运动的过程中，如果系统中任意一个机器人与障碍物的距离小于阈值d，编队临时解散，所有机器人在改进人工势场法的作用下避开障碍物，然后继续向虚拟目标点运动；在到达虚拟目标点后恢复编队；s3.3、重复步骤s3.1、s3.2直至多机器人系统到达最后一个子目标点即全局目标点。2.如权利要求1所述一种基于A*与改进人工势场法的编队路径规划控制方法，其特征在于：所述代数图论法具体为：设g＝(G，E，V)是一个N阶有向加权图，其中V表示节点集合V＝{v1，v2，v3，……vN}；E表示有向边集合，有向边eij由有序节点(vi，vj)表示；在有向加权图g中，从节点vi到节点vj的有向路径是有向网络中的一系列边(vi，vj)，当且仅当其子图是有向生成树时，有向加权图g包含有向生成树；度矩阵D用于描述连接到每个节点或顶点的链接数：邻接矩阵A用于描述节点之间的信息流或连接关系：计算得到拉普拉斯矩阵L：L＝D‑A(3)对于n个节点，动力学公式为：其中x(t)∈R，v(t)∈R，分别表示当前节点在t时刻的位置和速度；x(0)＝x0，v(0)＝v0，TTx＝[x1，x2，x3，......xN]并且v＝[v1，v2，v3，......vN]；u(t)∈R表示当前节点的在t时刻的力输入，x(t)、v(t)、u(t)三者关系为：2CN114326726A权利要求书2/2页u(t)＝‑Lx(t)‑Lv(t)(5)对于多机器人系统的共识协议，必须满足以下条件，即所有节点必须收敛到与其他相邻节点的相同状态，收敛表达式为：根据动力学公式将多机器人系统的二阶共识改写为：TTTTTTTT其中，s＝(s1，s2，......sN)，w＝(w1，w2，......wN)，并且第i个节点位置的纵坐标TTTSyi＝(si，wi)；然后，将网络改写为紧密的矩阵形式：其中，sy(t)表示当前节点在t时刻位置的纵坐标，是matlab中的Kronecker积，In是一个n阶单位矩阵。3.如权利要求1所述一种基于A*与改进人工势场法的编队路径规划控制方法，其特征在于：所述改进人工势场法具体为：设计如下的障碍物斥力势场函数Urep(q)：其中，0≤n＜1；β＞0，表示斥力系数，ρ(q，qobs)表示机器人位置q与障碍物位置qobs之间n的相对距离，ρ(q，qobs)为添加的调节因子，ρ0表示距离阈值；计算障碍物斥力势场U