本发明涉及多机器人编队控制领域，具体涉及一种多机器人编队自适应零空间行为融合方法。
背景技术
机器人的应用已经越来越广泛，几乎渗透到了科学发展的各个领域，而多移动机器人系统的成本低、鲁棒性强、完成任务好的特点使得多移动机器人系统受到了越来越多的关注。近年来，多移动机器人系统的协调问题已成为一个新兴的研究热点。机器人编队控制，是指多个机器人在到达目的地的过程中，保持某种队形，同时又要适应环境约束(例如存在障碍物和空间的物理限制等)的控制技术。多机器人编队控制问题是一个典型性和通用性的多机器人协调问题，是多机器人协调问题的基础，是多机器人系统中最重要也是最基本的问题，值得深入细致的研究。
为解决多机器人编队问题，常采用2种思想：集中式编队控制、分布式编队控制。相比于集中式方法，分布式因其可靠性高、开放性强、灵活性强，而成为对机器人集群编队的研究热点。零空间行为融合为分布式编队控制的一种典型应用。
由于传统零空间行为融合方法中其增益系数为常量，基于一组常量系数求得的速度只能满足一组工况(给定起点和终点)下的机器人实际运行能力，而无法适应于其他工况；若想适应其他工况，每一个工况下都需要进行3个增益系数的离线调试，不利于工程实践应用。
目前，filippoarrichiello等人发表的论文《formationcontrolofunderactuatedsurfacevesselsusingthenullspacebasedbehavioralcontrol》，将零空间行为融合方法用于欠驱动水面无人艇的编队控制，以模拟实现在有障碍和海流环境下的编队航行任务；邬林波发表的论文《基于nsb方法的多机器人编队控制》，采用了基于零空间行为融合的编队控制方法，将其应用到分布式的多足求机器人环境中来，从二维t(x,y)的空间推导到了三维t(x,y,θ)空间中，最后通过仿真实验，探讨了该方法在分布式环境中的应用。
上述方法是基于传统零空间行为融合方法进行编队控制，在应用中由于其增益系数是常量系数，又受到机器人运动因素(速度、起始点与终点)约束，导致在每一工况下其值都需要重新进行选择调试，缺乏自适应性，不利于工程应用。
技术实现要素：
本发明的目的在于提供一种多机器人编队自适应零空间行为融合方法，以使得最终得到的速度不受工况环境改变而改变，具有很好的自适应性，同时兼顾效率与性能。
本发明实施例提供一种多机器人编队自适应零空间行为融合方法，包括：
步骤一：分解运动过程步骤，根据机器人编队运动从起始点到终点的运动过程中运动规划的不同意图，将机器人运动过程分解，得到奔向目标行为、避障行为和编队行为3种运动行为；
步骤二：确定行为顺序步骤，根据零空间运动规划理论以及机器人运动规划不同需求意图，将步骤一所述的3种运动行为进行排序，得到3种运动行为的执行顺序；
步骤三：建立运动模型步骤，根据初始得到的起点位置、终点位置、障碍物位置、编队期望队形等条件，对步骤一所述的3种运动行建立每一种行为的运动模型，结合步骤二所述的3种运动行为的执行顺序进行求解，得到3种运动模型；
步骤四：解算增益系数步骤，根据机器人在运动过程中反馈得到的运动信息，解算得到增益系数；
步骤五：融合运动行为步骤，根据零空间数学方法，结合步骤二所述的3种运动行为的执行顺序，对步骤三所述的3种运动模型进行耦合，得到最终的速度和方向；
步骤六：结束任务判断步骤，根据机器人实时位置点判断机器人是否到达终点，若是，则结束任务，否则，返回步骤三继续任务；
所述步骤二，包括：
确定行为顺序步骤，根据零空间运动规划理论以及机器人运动规划不同需求意图，将步骤一所述的3种运动行为进行排序，得到3种运动行为的执行顺序；
其中，所述3种运动行为的执行顺序为：优先级依次为避障行为、奔向目标行为、编队行为；
所述步骤三以及步骤四，包括：
建立运动模型步骤，根据初始得到的起点位置、终点位置、障碍物位置、编队期望队形等条件，对步骤一所述的3种运动行建立每一种行为的运动模型，结合步骤二所述的3种运动行为的执行顺序进行求解，得到3种运动模型；解算增益系数步骤，根据机器人在运动过程中反馈得到的运动信息，解算得到增益系数；
其中，所述建立运动模型的具体方法为：
设机器人在大地坐标系下的坐标为pi＝[xiyi]t，机器人行驶的线速度为
δ＝f(p)
对上式两边求导得：
上式中，j(p)为δ的雅克比矩阵，得机器人的期望速度为：
上式中，v是对于机器人单行为的期望速度，若系统需要同时执行多个行为δ1,δ2,δ3...，根据制定的每个行为的优先级，用下标表示，得机器人多个行为的期望速度为v1,v2,v3....，用零空间方法求得最终的速度模型为：
上式中，其方向即为机器人规划运动方向；
其中，所述机器人在运动过程中反馈得到的运动信息包括：机器人在运动时能达到的最大速度vmax、机器人在运动时实时位置点pi＝(xi,yi)t、机器人运动终点pt＝(xt,yt)t；
其中，所述最终运动模型以及增益系数分别为：
(1)奔向目标行为的运动模型以及增益系数：
上式中，变量r表示为机器人距目标点距离为r时开始大幅降速；
(2)避障行为的运动模型以及增益系数：
上式中，da0、ja0为da、ja在避障初始时求得的值；
(3)编队行为的运动模型以及增益系数：
λf＝i
上式中，δf为机器人当前位置点距离队形中心的实际横、纵坐标偏差，δf，d为机器人当前位置点距离队形中心的期望横、纵坐标偏差；
所述步骤五，包括：
融合运动行为步骤，根据零空间数学方法，结合步骤二所述的3种运动行为的执行顺序，对步骤三所述的3种运动模型进行耦合，得到最终的速度和方向；
其中，所述最终的速度和方向为：
v＝v1+(i-j1t(j1j1t)-1j1)[(v2+(i-j2t(j2j2t)-1j2)v3]；
本发明的有益效果在于：
1.在传统零空间行为融合方法基础上，引入具有实际意义的变量系数：机器人可达到最大速度、终点坐标和当前定位坐标，重新定义行为运动模型，使得求解出来的速度不受工况环境改变而改变，具有很好的自适应性，同时兼顾效率与性能；
2.本发明的方法是在传统的零空间行为融合方法基础上，对约束速度的增益系数进行重新定义，可以对速度进行有效控制，在运动规划上具有显著进步；
附图说明
图1为一种多机器人编队自适应零空间行为融合方法的流程图；
具体实施方式
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明做进一步描述：
图1为一种多机器人编队自适应零空间行为融合方法的流程图。
本发明的技术方案是这样实现的：
(1)将运动过程分解：在多机器人编队运动过程中，可分解成3种运动行为，奔向目标点、避障、编队。
(2)确定行为优先级：确定3种运动行为的执行顺序，其优先级为避障、奔向目标点、编队。
(3)建立并求解每一行为的运动模型：已知起始点、终点、障碍物位置、编队期望队形等条件，建立每一行为的运动模型，并求解。
设定机器人在大地坐标系下的坐标为pi＝[xiyi]t，行驶的线速度
δ＝f(p)(1)
对(1)式两边求导：
j(p)为δ的雅克比矩阵，则机器人的期望速度为：
(3)中的v是对于单行为的期望速度，若系统需要同时执行多个行为δ1,δ2,δ3...，首先制定每个行为的优先级，用下标表示，可得多个行为的期望速度为v1,v2,v3....，用零空间方法可以求得最终的速度为：
(4)用机器人运动信息来解算增益系数：运动信息包括机器人在运动时能达到的最大速度vmax、实时位置点pi＝(xi,yi)t、终点pt＝(xt,yt)t等，增益系数用于对速度做约束。
奔向目标行为的运动模型为：
其中，变量r表示为机器人距目标点距离为r时开始大幅降速。
避障行为的运动模型为：
其中，da0、ja0为da、ja在避障初始时求得的值。
编队行为的运动模型为：
λf＝i(11)
其中，δf为机器人当前位置点距离队形中心的实际横、纵坐标偏差，δf，d为机器人当前位置点距离队形中心的期望横、纵坐标偏差。
(5)行为融合：基于零空间数学概念进行3种运动模型的耦合，继而求出最终的速度和方向。
考虑3种行为的优先级顺序，可以得到最终的速度输出见式(12)。
v＝v1+(i-j1t(j1j1t)-1j1)[(v2+(i-j2t(j2j2t)-1j2)v3](12)
(6)判断机器人是否到达终点，若到达则该过程结束，若没有到达则返回步骤(3)。
本领域技术人员按照本发明提供的机器人编队的自适应零空间行为融合方法，能够实现多机器人编队在各种工况下的运动规划。
1、多机器人编队的自适应零空间行为融合方法，包括以下步骤：
(1)将运动过程分解：在机器人编队运动过程中，可分解成3种运动行为，奔向目标点、避障、编队。
(2)确定行为优先级：确定3种运动行为的执行顺序，其优先级为避障、奔向目标点、编队。
(3)建立并求解每一行为的运动模型：已知起始点、终点、障碍物位置、编队期望队形等条件，建立每一行为的运动模型，并求解。
(4)用机器人运动信息来解算增益系数：运动信息包括机器人在运动时能达到的最大速度、实时位置点、终点等，增益系数用于对速度做约束。
(5)行为融合：基于零空间数学方法进行3种运动模型的耦合，继而求出最终的速度和方向。
(6)判断机器人是否到达终点，若到达则该过程结束，若没有到达则返回步骤(3)。
2、如权利1所要求的多机器人编队的自适应零空间行为融合方法，其中第(1)步，将机器人运动过程分解，是指机器人从起始点到终点的运动过程中，其运动规划可以分解为三种意图，奔向目标行为、避障行为、编队行为。。
3、如权利1所要求的多机器人编队的自适应零空间行为融合方法，其中第(2)步，确定行为优先级的特点在于：运动过程分解的3种行为，在机器人执行时会有一定的先后顺序，优先级的确定需要根据零空间理论以及运动规划需求意图进行确定。第(1)步中分解的3种行为，其优先级为避障、奔向目标点、编队。
4、如权利1所要求的多机器人编队的自适应零空间行为融合方法，其中第(3)步，建立每一分解行为的运动模型的特征在于：δ(x,y)为可控的任务变量，将其对时间t求导，变换可以求得速度模型。由于速度是矢量，所以其方向即为机器人规划运动方向。
5、如权利1所要求的多机器人编队的自适应零空间行为融合方法，其中第(4)步，用机器人运动信息来解算增益系数的特征在于：此步的作用在于引入具有实际意义的变量系数(机器人可达到最大速度、终点坐标、当前定位坐标)来代替传统零空间方法中常量的增益系数，使得此种方法的运动规划具有自适应性，同时可以对速度进行把控。
6、如权利1所要求的多机器人编队的自适应零空间行为融合方法，其中第(5)步，行为融合的特征在于：基于第(2)步中确定的行为优先级，基于零空间数学概念进行3种运动模型的耦合，继而求出最终的速度和方向。