本发明涉及一种路况环境约束下分布式多机器人的低能耗运动规划方法，属于移动机器人路径规划。
背景技术：
1、随着移动机器人在各个领域的广泛应用，非平坦的户外环境(如矿区、农田等)逐渐成为了移动机器人的主要应用场景。许多现有的路径规划研究都以路程距离或能耗作为优化目标，无法应对复杂且多变的实际环境。具有复杂障碍物、坡度和地面路况等因素的环境，为路径规划的研究带来了挑战。此外，现实世界中，机器人经常需要在各种环境中工作，而多个机器人在非平坦地面以及充斥未知障碍物干扰下的运动是较为复杂的约束问题。由于多机器人具有非常复杂的动态和非线性的行为，因此对于每一个机器人而言，在此类环境中的路径规划都是一个具有挑战性的问题，尤其多机器人系统规模较大以及环境较复杂时。
2、本申请第一发明人在论文“多因素改进蚁群算法的路径规划[j].计算机集成制造系统”中公开了一种适用于非平坦地形约束的多因素蚁群算法，实现了移动机器人的全局综合最优路径规划，但未考虑未知障碍约束和移动机器人实际运动中的避障问题。有鉴于此，特提出本发明。
技术实现思路
1、为了解决考虑运动学约束的分布式多机器人的路径规划，本发明提供了一种路况环境约束下分布式多机器人的低能耗运动规划方法。
2、本发明的技术方案是：
3、一种路况环境约束下分布式多机器人的低能耗运动规划方法，包括：步骤s1：获取路况约束环境地图；初始化机器人运动参数；步骤s2：利用优化算法分别规划所有机器人的全局运动路径，获得路径综合性能指标；步骤s3：依据路径综合性能指标分配机器人优先级；步骤s4：多机器人进行自适应导航点追踪；步骤s5：采用动态窗口法进行速度采样，获得局部运动轨迹；步骤s6：构建综合评价函数对局部运动轨迹进行评估，选择综合评价最优的局部运动轨迹作为局部运动路径；步骤s7：判断分布式多机器人间是否存在运动冲突，若是则依据运动冲突消解策略进行避障，否则依据步骤s6的局部运动路径运动；步骤s8：判断机器人是否运动到终点，若是则结束运动并输出运动参数，否则执行步骤s4。
4、所述优化算法采用蚁群算法，具体为：从机器人的当前位置向周围八个方向搜索路径；可行的路径节点原则为：有障碍物的不搜索，对角方向与障碍物发生碰撞的不搜索，已搜索过的路径不搜索；每轮迭代都依据多属性路径指标来分配蚁群信息素，并计算路径综合性能指标。
5、所述多属性路径指标分配蚁群信息素策略为：其中，τk(t1)是第t1轮迭代中编号为k的蚂蚁的信息素增量，l、t和h是当前蚂蚁行经路径的长度、转弯次数和高度均方差，q1、q2和q3是各个指标的权重，t1和t1+1分别表示前一轮和当前轮；所述路径综合性能指标s＝l+t+h。
6、所述步骤s3具体为：利用步骤2的优化算法为多机器人规划全局运动路径，得到最优全局运动路径和路径综合性能指标，建立集合c＝{s1,s2,...sm}，对路径综合性能指标升序排列后得到集合c*＝{c1,c2,...,cm}，机器人的优先级依次对应于c*的查询位置，满足c1的优先级高于c2，以此类推；其中，sm表示第m个机器人的路径综合性能指标，cm表示进行升序排序后的第m个路径综合性能指标。
7、所述步骤s4具体为：多机器人提取各自的全局运动路径的关键节点作为导航点，机器人确定运动方向进行自适应运动选择当前导航点；所述全局路径的关键节点包括路径起始点、转折点和目标点。
8、所述机器人确定运动方向规则为：以当前位置(xs,ys)和当前导航点(xi,yi)来动态调整航向角
9、所述自适应运动策略为：考虑机器人到当前导航点的距离，当距离小于机器人期望追踪的距离时选取下一关键节点作为机器人当前导航点。
10、所述步骤s5中采用动态窗口法进行速度采样，具体为：依据机器人速度约束、电机动力学约束、制动距离约束形成速度集合，以当前速度集合(v,ω)和机器人运动学预测预测时间周期t秒内的所有轨迹；速度集合(v,ω)为vs∩vd∩va，vs为机器人速度约束，vd为电机动力学约束，va为制动距离约束。
11、所述机器人速度约束vs＝{(v,ω)|v∈[vmin,vmax],ω∈[ωmin,ωmax]}；其中，v和ω为线速度和角速度，vmin和vmax为最小线速度和最大线速度，ωmin和ωmax为最小角速度和最大角速度；
12、所述电机动力学约束为vd＝{(v,ω)|v∈[v-avminδt,v+avmaxδt],ω∈[ω-aωminδt,ω+aωmaxδt]}；其中，和为最小线减速度和最小角减速度，和为最大线加速度和最大角加速度，δt为采样时间；
13、所述制动距离约束其中，dis(v,ω)为预测轨迹末端与最近障碍物距离。
14、所述综合评价函数为：g(v,ω)＝σ[x·heading(v,ω)+y·dist(v,ω)+z·vel(v,ω)+p·energy(v,ω)]；其中，heading(v,ω)为方位角评价函数，dist(v,ω)为机器人预测轨迹末端到障碍物距离的评价函数；vel(v,ω)为机器人当前速度大小评价函数；energy(v,ω)为能耗评价函数，energy(v,ω)＝1/e，e表示能耗，v和ω为线速度和角速度；σ为归一化处理，x、y、z和p为各评价函数的加权系数。
15、所述步骤s7，具体为：
16、判断潜在碰撞风险：若机器人间的实际距离小于期望避障距离，则存在潜在碰撞风险，执行极大碰撞风险判断，否则依据步骤s6得到的局部运动路径通行；
17、判断极大碰撞风险：以低优先级机器人建立局部坐标轴，判断|α-β|是否小于预设角度，若满足该条件则存在极大碰撞风险，则执行运动冲突消解策略，否则依据步骤s6得到的局部运动路径通行；其中，α为两机器人的连线与x轴正方向的夹角，β为低优先级机器人的运动方向角；
18、运动冲突消解：低优先级机器人进行减速直到冲突解除，高优先级机器人依据步骤s6得到的局部运动路径通行。
19、本发明的有益效果是：本发明在考虑运动学约束的基础上，结合动态窗口法提出了分布式多机器人的冲突判断和消解机制，实现了分布式多机器人的低能耗无冲突运动。进一步地，通过仿真证明，路况约束下本发明提供方法下分布式多机器人运动的运动冲突得以避免，并且运动能耗相较于传统方法减低。
1.一种路况环境约束下分布式多机器人的低能耗运动规划方法，其特征在于，包括：
2.根据权利要求1所述的路况环境约束下分布式多机器人的低能耗运动规划方法，其特征在于，所述优化算法采用蚁群算法，具体为：从机器人的当前位置向周围八个方向搜索路径；可行的路径节点原则为：有障碍物的不搜索，对角方向与障碍物发生碰撞的不搜索，已搜索过的路径不搜索；每轮迭代都依据多属性路径指标来分配蚁群信息素，并计算路径综合性能指标。
3.根据权利要求2所述的路况环境约束下分布式多机器人的低能耗运动规划方法，其特征在于，所述多属性路径指标分配蚁群信息素策略为：其中，τk(t1)是第t1轮迭代中编号为k的蚂蚁的信息素增量，l、t和h是当前蚂蚁行经路径的长度、转弯次数和高度均方差，q1、q2和q3是各个指标的权重，t1和t1+1分别表示前一轮和当前轮；所述路径综合性能指标s＝l+t+h。
4.根据权利要求1所述的路况环境约束下分布式多机器人的低能耗运动规划方法，其特征在于，所述步骤s3具体为：利用步骤2的优化算法为多机器人规划全局运动路径，得到最优全局运动路径和路径综合性能指标，建立集合c＝{s1,s2,...sm}，对路径综合性能指标升序排列后得到集合c*＝{c1,c2,...,cm}，机器人的优先级依次对应于c*的查询位置，满足c1的优先级高于c2，以此类推；其中，sm表示第m个机器人的路径综合性能指标，cm表示进行升序排序后的第m个路径综合性能指标。
5.根据权利要求1所述的路况环境约束下分布式多机器人的低能耗运动规划方法，其特征在于，所述步骤s4具体为：多机器人提取各自的全局运动路径的关键节点作为导航点，机器人确定运动方向进行自适应运动选择当前导航点；所述全局路径的关键节点包括路径起始点、转折点和目标点。
6.根据权利要求5所述的路况环境约束下分布式多机器人的低能耗运动规划方法，其特征在于，所述机器人确定运动方向规则为：以当前位置(xs,ys)和当前导航点(xi,yi)来动态调整航向角
7.根据权利要求1所述的路况环境约束下分布式多机器人的低能耗运动规划方法，其特征在于，所述步骤s5中采用动态窗口法进行速度采样，具体为：依据机器人速度约束、电机动力学约束、制动距离约束形成速度集合，以当前速度集合(v,ω)和机器人运动学预测预测时间周期t秒内的所有轨迹；速度集合(v,ω)为vs∩vd∩va，vs为机器人速度约束，vd为电机动力学约束，va为制动距离约束。
8.根据权利要求7所述的路况环境约束下分布式多机器人的低能耗运动规划方法，其特征在于，所述机器人速度约束vs＝{(v,ω)|v∈[vmin,vmax],ω∈[ωmin,ωmax]}；其中，v和ω为线速度和角速度，vmin和vmax为最小线速度和最大线速度，ωmin和ωmax为最小角速度和最大角速度；
9.根据权利要求1所述的路况环境约束下分布式多机器人的低能耗运动规划方法，其特征在于，所述综合评价函数为：g(v,ω)＝σ[x·heading(v,ω)+y·dist(v,ω)+z·vel(v,ω)+p·energy(v,ω)]；其中，heading(v,ω)为方位角评价函数，dist(v,ω)为机器人预测轨迹末端到障碍物距离的评价函数；vel(v,ω)为机器人当前速度大小评价函数；energy(v,ω)为能耗评价函数，energy(v,ω)＝1/e，e表示能耗，v和ω为线速度和角速度；σ为归一化处理，x、y、z和p为各评价函数的加权系数。
10.根据权利要求1所述的路况环境约束下分布式多机器人的低能耗运动规划方法，其特征在于，所述步骤s7，具体为：
