CN115167440A - 一种基于虚拟领航-领航-跟随的多机器人编队控制方法 - Google Patents
一种基于虚拟领航-领航-跟随的多机器人编队控制方法 Download PDFInfo
- Publication number
- CN115167440A CN115167440A CN202210884676.6A CN202210884676A CN115167440A CN 115167440 A CN115167440 A CN 115167440A CN 202210884676 A CN202210884676 A CN 202210884676A CN 115167440 A CN115167440 A CN 115167440A
- Authority
- CN
- China
- Prior art keywords
- robot
- formation
- virtual
- following
- azimuth
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 265
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000009466 transformation Effects 0.000 claims abstract description 32
- 230000008859 change Effects 0.000 claims abstract description 15
- 238000005457 optimization Methods 0.000 claims abstract description 9
- 230000006870 function Effects 0.000 claims description 34
- 238000006243 chemical reaction Methods 0.000 claims description 19
- 230000004888 barrier function Effects 0.000 claims description 14
- 230000008878 coupling Effects 0.000 claims description 12
- 238000010168 coupling process Methods 0.000 claims description 12
- 238000005859 coupling reaction Methods 0.000 claims description 12
- 230000002159 abnormal effect Effects 0.000 claims description 8
- 230000007613 environmental effect Effects 0.000 claims description 8
- 239000000284 extract Substances 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 5
- 230000002194 synthesizing effect Effects 0.000 claims description 3
- 230000008439 repair process Effects 0.000 claims description 2
- 238000012217 deletion Methods 0.000 claims 1
- 230000037430 deletion Effects 0.000 claims 1
- 238000005755 formation reaction Methods 0.000 description 217
- 239000000126 substance Substances 0.000 description 11
- 238000011156 evaluation Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 8
- 238000005265 energy consumption Methods 0.000 description 7
- 230000000694 effects Effects 0.000 description 6
- 239000011159 matrix material Substances 0.000 description 6
- 230000009471 action Effects 0.000 description 3
- 230000008901 benefit Effects 0.000 description 3
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 150000001875 compounds Chemical class 0.000 description 3
- 229910003460 diamond Inorganic materials 0.000 description 3
- 239000010432 diamond Substances 0.000 description 3
- 108010074864 Factor XI Proteins 0.000 description 2
- 239000003795 chemical substances by application Substances 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000008602 contraction Effects 0.000 description 1
- 238000001816 cooling Methods 0.000 description 1
- 230000000249 desinfective effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 239000002360 explosive Substances 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 230000002262 irrigation Effects 0.000 description 1
- 238000003973 irrigation Methods 0.000 description 1
- 238000012886 linear function Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001988 toxicity Effects 0.000 description 1
- 231100000419 toxicity Toxicity 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0291—Fleet control
- G05D1/0295—Fleet control by at least one leading vehicle of the fleet
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种基于虚拟领航‑领航‑跟随的多机器人编队控制方法,包括:建立虚拟领航‑领航‑跟随的运动结构;领航机器人实时跟踪虚拟领航机器人的位置,根据检测和提取的包括道路边界及障碍物在内的环境信息,进行队形变换动态优化得到跟随机器人与虚拟领航机器人之间的期望距离和期望方位角;通过分布式协同耦合的编队控制器对编队中的多个机器人进行运动控制;使每个机器人对虚拟领航机器人的实时位置进行跟踪;通过跟踪使领航机器人与虚拟领航机器人之间的期望距离和方位角收敛到零;使跟随机器人与虚拟领航机器人之间的距离和方位角收敛到期望的相对距离和方位角。本发明能够使多机器人编队以设定队形沿编队参考轨迹避障行进和队形变换。
Description
技术领域
本发明属于机器人编队控制技术领域,具体涉及一种基于虚拟领航-领航-跟随的多机器人编队控制方法。
背景技术
近年来,消防机器人在实战中也得到越来越多的应用。但是从应用效果来看,目前的消防机器人绝大多数需要依赖后台消防人员的远程操作,并且存在侦察视野窄、人机交互差、火源定位受限等技术挑战,从而无法适应更加复杂的灾情。面向强干扰、高动态等复杂火灾环境下的消防需求,为实现机器人***的任务能力扩展及整体消防效能提升,采取多种智能消防机器人协同作业的模式,实现能力互补和行动协调,已成为未来消防应用的主要发展方向。智能群组消防机器人在执行编队追踪任务时,由于行驶道路上存在各种未知障碍物以及受道路两侧边缘约束,机器人既要保证行驶在安全区域内,又要在行驶过程中快速避开前方障碍物,同时当通道宽度发生变化时,编队机器人能进行动态调整队形形状以最优构型通过通道。因此需要一种高效、安全、可靠的编队避障算法,以提高编队避障的灵活性和编队结构的稳定性。
发明内容
鉴于上述的分析,本发明旨在公开了一种基于虚拟领航-领航-跟随的多机器人编队控制方法,用于解决多机器人的编队控制问题。
本发明公开了一种基于虚拟领航-领航-跟随的多机器人编队控制方法,包括以下步骤:
建立虚拟领航-领航-跟随的运动结构;指定编队中一台机器人为领航机器人,其余为跟随机器人,并设定一台虚拟领航机器人在编队参考轨迹点上随队行进;
领航机器人实时跟踪虚拟领航机器人的位置,根据检测和提取的包括道路边界及障碍物在内的环境信息,进行队形变换动态优化,得到跟随机器人与虚拟领航机器人之间的期望距离和期望方位角以适应当前环境;
通过分布式协同耦合的编队控制器对编队中的多个机器人进行运动控制;使每个机器人对虚拟领航机器人的实时位置进行跟踪;通过跟踪使领航机器人与虚拟领航机器人之间的期望距离和方位角收敛到零;使跟随机器人与虚拟领航机器人之间的距离和方位角收敛到期望的相对距离和方位角。
进一步地,跟随机器人与虚拟领航机器人之间的期望距离和期望方位角的获取方法,包括:
机器人编队以期望的初始队列构型,跟随虚拟领航机器人行进;
领航机器人利用自身携带的环境感知传感器检测和提取出道路边界信息以及障碍物信息,并计算出队形的伸缩因子;
根据队形的伸缩因子确定出队形变换指令分发到跟随机器人,使跟随机器人得到与虚拟领航机器人之间的期望距离和期望方位角。
进一步地,所述领航机器人实时跟踪虚拟领航机器人的位置,并且通过自身携带的多线激光雷达实时感知环境信息,提取道路的边界、计算两侧道路宽度,检测和识别障碍物信息;并将检测到的部分障碍物边界点作为候选边界点,对道路边界点进行缺失修补,以得到最终的边界点信息。
进一步地,通过对道路边界和障碍物的轮廓进行膨胀处理,设置膨胀层,使编队中机器人与道路边界和障碍物之间的距离大于膨胀层宽度。
进一步地,领航机器人根据测得的最小通行宽度和初始队列构型的期望队形宽度之间的比值,作为队形的伸缩因子。
进一步地,根据队形的伸缩因子确定出队形变换指令包括:
当伸缩因子大于1时,输出队形零变换指令,使编队保持原队形继续前进;
当伸缩因子处于第一阈值和1的取值区间内时;输出队形同构变换指令,使编队进行队形收缩后通过前方的通道;
当伸缩因子处于第二阈值和第一阈值的取值区间内时;输出队形异构变换指令,使编队变换成新的队形通过前方通道;
当伸缩因子不大于第二阈值时,输出队形异常状态指令,编队无法通过前方通道,等待下一步指令。
进一步地,所述分布式协同耦合的编队控制器的编队控制方法包括:
根据虚拟领航机器人和编队机器人的一阶积分器模型,确定出编队机器人与虚拟领航机器人之间的相对位移和相对速度;
建立圆形运动控制律,引导每个编队机器人以平滑圆弧运动的方式收敛到以虚拟领航机器人为中心、以与虚拟领航机器人之间期望距离为半径的圆上;
在圆形运动控制律的基础上耦合一个方位角定位的控制律;根据耦合了方位角定位的圆形运动控制律和避障控制律,对每个编队机器人进行具有避障功能的编队控制。
进一步地,所述避障控制律通过设置斥力势场产生排斥力进行避障。
进一步地,在设置斥力势场中,通过建立道路边界作用的斥力速度,以限制机器人的行驶区域;将障碍物作用的斥力速度和道路边界作用的斥力速度进行合成得到斥力合速度;通过调整斥力合速度的方向输出最终的避障控制律,以解决局部极小值和目标不可达问题,使编队机器人在道路的安全区域内进行避障。
进一步地,建立动态权重因子,对耦合了方位角定位的圆形运动控制律和避障控制律进行比值调整,得到分布式协同耦合的编队控制器最终输出的控制律。
本发明至少可实现以下有益效果之一:
本发明的基于虚拟领航-领航-跟随的多机器人编队控制方法,解决多机器人的编队控制问题。
本发明采用虚拟领航-领航-跟随模式,领航机器人既负责整个***规划和协调,但又不影响到其他机器人的运动状态。解决了传统的领航者-跟随者模式的链式结构中跟随机器对领航机器的过度依赖,容易造成跟踪误差累积以及领航机器出现故障会导致整个***出现崩溃等问题。
本发明建立了队形的伸缩因子,在常规的队形变换状态基础上还考虑了异常状态情况,即当前方通道宽度不满足编队个体通过时的情况,需等待下一步指令;提出了衡量编队变换策略优劣的性能综合评价指标函数,该指标能够不依赖于***的动力学模型,其表达形式简单明了,不仅能够适用于多消防机器人的时变编队变换而且能适用于其他多智能体***。
本发明从实际应用出发,考虑了道路边界和停靠在路边的障碍物会与机器人发生碰撞,通过对道路边界和障碍物的轮廓添加了膨胀处理,使得机器人尽可能与道路边界和障碍物之间保持一个安全距离。
本发明建立了一种分布式协同耦合的编队控制器,在圆形运动控制律的基础上建立增加方位角定位的控制律,与避障控制律结合,能同时实现按编队队形行进的过程中有效地避开障碍物。
本发明设置斥力势场产生排斥力进行避障,能够不依赖于***的精确数学模型,以非常简单的算法实现不确定性高的非线性耦合动态***的避障控制,以及实现高效、安全、可靠的编队避障。并且把道路边界作用效果也考虑进去,使得机器人能够在道路的安全区域内有效避障,同时有效地解决传统人工势场法存在的局部最优和目标不可达的问题。
附图说明
附图仅用于示出具体实施例的目的,而并不认为是对本发明的限制,在整个附图中,相同的参考符号表示相同的部件。
图1为本发明实施例中的基于虚拟领航-领航-跟随的多机器人编队控制方法流程图;
图2为本发明实施例中的坐标系***图;
图3为本发明实施例中的多机器人编队几何结构示意图;
图4为本发明实施例中的队形形状示例图;
图5为本发明实施例中的多机器人编队行走过程中遇到的通道示意图;
图6为本发明实施例中的队形变换动态优化流程图;
图7为本发明实施例中的传统人工势场法存在局部最优和目标不可达的问题示意图;
图8为本发明实施例中的改进的人工势场法实现效果图;
图9为本发明实施例中的对数障碍函数图;
图10为本发明实施例中的分布式协同耦合的编队控制器结构框图。
具体实施方式
下面结合附图来具体描述本发明的优选实施例,其中,附图构成本申请一部分,并与本发明的实施例一起用于阐释本发明的原理。
本发明的一个实施例公开了一种基于虚拟领航-领航-跟随的多机器人编队控制方法,如图1所示,包括以下步骤:
步骤S101、建立虚拟领航-领航-跟随的运动结构;指定编队中一台机器人为领航机器人,其余为跟随机器人,并设定一台虚拟领航机器人在编队参考轨迹点上随队行进;
步骤S102、领航机器人实时跟踪虚拟领航机器人的位置,根据检测和提取的包括道路边界及障碍物在内的环境信息,进行队形变换动态优化,得到跟随机器人与虚拟领航机器人之间的期望距离和期望方位角以适应当前环境;
步骤S103、通过分布式协同耦合的编队控制器对编队中的多个机器人进行运动控制;使每个机器人对虚拟领航机器人的实时位置进行跟踪;通过跟踪使领航机器人与虚拟领航机器人之间的期望距离和方位角收敛到零;使跟随机器人与虚拟领航机器人之间的距离和方位角收敛到期望的相对距离和方位角。
考虑一个由n(n≥2)台消防机器人组成的多机器人编队。由于传统的领航者-跟随者模式中跟随者对领航者的过度依赖,这种链式结构容易造成跟踪误差累积以及领航者出现故障会导致整个***出现崩溃等问题,本实施例中建立虚拟领航-领航-跟随运动结构。
具体的,在本实施例中以多消防机器人编队的编队控制为例。消防机器人在室外场景下使用内置双差分GNSS+4G模块辅助的EPSILON-D4G模组(RTK),可通过千寻服务实现稳定的厘米级定位。EPSILON-D4G模块提供的定位信息有经度、纬度、高度和航向角等信息。为方便机器人的位姿表示,需要将RTK测得的经纬度和高度数据转换到惯性坐标系下机器人的位置,航向角转换到导航坐标系下机器人的偏航角。
为了更加准确和方便地描述消防机器人的运动状态,需定义合适的坐标系***以及各坐标系之间的转换关系,定义的坐标系***的如图2所示。其中,∑{Oe,Xe,Ye,Ze}为地心地固坐标系(ECEF),坐标原点Oe与地球中心重合,Xe和Ye位于赤道平面,但是分别指向初始子午线和东经90度的子午线,Ze与地球自转轴重合;∑{On,Xn,Yn,Zn}为惯性坐标系与地面固联,按照东北天(ENU)坐标系建立,即三轴Xn、Yn、Zn分别指向地理正东、正北、正上方。Π{Or,Xr,Yr,Zr}为消防机器人的本体坐标系,其中Or与机器人的几何中心固联,其三轴Xr,Yr,Zr分别按机器人运动正前、正左、正上方向选取。
首先需要通过椭球参数变换将经纬度和高度数据转换到ECEF笛卡尔位置,转换公式为:
其中,λB、λL、λH分别表示测得的维度、经度和高度,N为卯酉圈曲率半径,Lа和e分别表示地球椭圆的长半轴和第一偏心率。其中,Lа=6378137±2m,e2=0.0066943799013m。
接着,将ECEF坐标系转换到惯性坐标系下,转换关系为:
通过上述三式可以得到消防机器人以基准点作为原点在惯性坐标系下的位置。同时,为了获得机器人在惯性坐标系下的偏航角θ∈[-π,π),需要将指北的航向角ψ∈[0,2π)转换到绝对偏航角,转换关系为:
具体的,在步骤S101中虚拟领航-领航-跟随运动结构的建立包括:
1)建立多机器人编队几何结构;
如图3所示;其中,虚拟领航机器人R0在编队参考轨迹点上随队行进,虚拟领航机器人R0的运动轨迹由一系列连续可导的轨迹点组成;指定其中一台机器人为领航机器人R1,实时跟踪虚拟领航机器人的位置,即满足领航机器人与虚拟领航机器人之间的期望距离和方位角收敛到零,即另外,其他机器人为跟随机器人Ri(i≥2),将各跟随机器人与领航机器人R1之间保持期望的距离和方位角转换到与虚拟领航机器人R0之间保持相应的期望值;这种编队结构设计的好处在于领航机器人R1既负责整个***规划和协调,但又不影响到其他机器人的运动状态。
其中,
x0,y0为虚拟领航机器人在惯性坐标系下的位置坐标,xi,yi为机器人i在惯性坐标系下的位置坐标;
根据机器人i与虚拟领航机器人的相对位置,利用光滑的反正切函数atan2(y,x)得到范围为[-π,π)的唯一值,再利用g(atan2(y,x))函数将atan2(y,x)范围转换到[0,2π)。
将每个机器人建模成一阶积分器模型:
将虚拟领航机器人也建模成一阶积分器模型:
定义机器人i与虚拟领航者之间的相对位移为:
定义机器人i与虚拟领航者之间的相对速度为:
2)建立多机器人编队队形结构知识库
建立对多消防机器人***的队形结构是研究多机器人编队变换的基础,通过对一些典型队形结构建立空间位置上的数学关系,可以大大简化多机器人编队变换过程中的数学计算。在构建队形形状主要考虑有一字型(line)、锲形(wedge)、柱状型(columnar)、三角形(triangle)、菱形(diamond)、圆形(circular)等,如图4所示,图中o为编队目标点,例如火源。
首先,假设有n台消防机器人和规划的一条参考轨迹(虚拟领航机器人),我们指定队形中某一机器人为队形的领航机器人,其余机器人为跟随机器人。采用图论中的有向无环图来描述一个可伸缩的队形,每个机器人看作成一个顶点,两个机器人之间的关系看作是边。每个机器人都有唯一的ID号,这里将虚拟领航机器人设为R0,领航者设为R1,其余跟随机器人依次设为R2,R3,…,Rn。为了表示机器人之间的相互关系及队形的形状参数,定义队形参数矩阵的通式为:
Fi d=[fi1,fi2,fi3] (10)
其中,Fd为某个队形形状的参数信息矩阵,为机器人的ID号。每个机器人用3组参数表示,fi1为机器人i的编号,fi2为机器人i与虚拟领航机器人之间需要保持的期望距离fi3为机器人i与虚拟领航机器人之间的期望方位角则消防机器人编队的队形形状可以描述为:
针对上述几种典型的编队队形形状,通过不同的队形参数矩阵建立队形知识库,其期望队形参数矩阵表示如下:
上述建立的期望队形参数矩阵并不能表示这类队形所有的结构,只是这类形状的一个特例,比如一字队形,通过调整机器人的顺序或者各机器人与虚拟领航机器人之间的距离可以得到新的一字队形,但是所有的队形都可以通过调整和得到。
3)选择编队队形,从多机器人编队队形结构知识库中获取队形参数,进行编队控制实现多机器人***以指定队形运动;
若要实现多消防机器人***以指定队形运动,并且能根据感知到的环境变化进行有效的队形变换,根据具有反馈控制的编队控制器,使得领航机器人能够跟踪提前规划好的一系列轨迹点,各机器人与虚拟领航机器人之间保持期望的距离和方位角,即满足
在本实施例的一个具体方案中,多机器人编队***协同作业是针对室外复杂多变环境,例如石化厂,气田,石油灌溉区,机场地铁,仓库,林道和其他高风险,高温,有毒,强辐射,易燃,易爆的地方以及容易倒塌的消防车和人员无法接近时,需要派遣消防灭火机器人执行其职责,并进入危险区域进行消防和救援,调查和救援,对化学污染场所进行冷却和消毒。机器人编队行走的区域多为窄过道、回廊或者有障碍物的通道。以三台机器人为例,分析多机器人编队行走过程中遇到的通道示意图,如图5所示。
在本实施例中的编队中的领航机器人和跟随机器人均携带的多线激光雷达实时感知环境信息;其中,
领航机器人实时跟踪虚拟领航机器人的位置,并且通过自身携带的多线激光雷达实时感知环境信息,提取结构化道路的边界以及检测和识别障碍物。
跟随机器人与领航机器人组成特定队形执行编队任务,同时也利用自身携带的多线激光雷达检测和识别障碍物。
为实现根据环境约束来避开障碍物同时能动态调节队形宽度以适应不同道路宽度,使得多消防机器人***队形变换适应当前环境的综合性能达到最优。
在步骤S102中,领航机器人实时跟踪虚拟领航机器人的位置,根据检测和提取的包括道路边界及障碍物在内的环境信息,进行队形变换动态优化,得到跟随机器人与虚拟领航机器人之间的期望距离和期望方位角以适应当前环境;
具体包括:
1)机器人编队以期望的初始队列构型,跟随虚拟领航机器人行进;
具体的,机器人群组通过编队控制器快速形成期望的初始构型,并一起跟随参考轨迹行进,执行编队跟随任务;
2)领航机器人利用自身携带的环境感知传感器检测和提取出道路边界信息以及障碍物信息,并计算出队形的伸缩因子;
具体的,所述领航机器人实时跟踪虚拟领航机器人的位置,并且通过自身携带的多线激光雷达实时感知环境信息,提取道路的边界并计算两侧道路宽度、检测和识别障碍物信息;并将检测到的部分障碍物边界点作为候选边界点,作进一步判别,修补缺失的道路边界点,以得到边界点信息。
并且通过对道路边界和障碍物的轮廓进行膨胀处理,设置膨胀层,使编队中机器人与道路边界和障碍物之间的距离大于膨胀层宽度。优选的,膨胀层的宽度为B/2;B为机器人的宽度。
在实际道路场景中,由于车辆等其他障碍物停靠在道路边界位置,从而对激光雷达发射的部分射线产生遮挡而无法获得完整的道路边界点,因此可将检测到的该部分障碍物边界点作为候选边界点,并作进一步判别,“修补”缺失的道路边界点,如图5所示。由图5可得,领航机器人利用激光雷达检测到的道路边界点位置为:
考虑到激光雷达可能会受安装位置的限制,以及编队行进过程中只需要对前方道路宽度和边界点提取,而不需要再对后方进行数据处理。因此,只需取激光雷达特定角度范围内α∈[αmin,αmax]的有效数据,这样既可以排除一些异常数据和减小计算量,又可以余留充足的时间进行队形调节。
L=ρ|sinβ| (13)
定义Dmin=2L为机器人的最小通行宽度。
领航机器人根据测得的最小通行宽度Dmin和初始队列构型的期望队形宽度D0之间的比值,作为队形的伸缩因子ξ;
3)根据队形的伸缩因子确定出队形变换指令分发到跟随机器人,使跟随机器人得到与虚拟领航机器人之间的期望距离和期望方位角。
根据队形的伸缩因子ξ确定出队形变换指令σ包括:
领航机器人根据计算得到的ξ发布以下四种指令来动态调节队形以适应不同通道宽度的变化:
(1)当伸缩因子大于1时,输出队形零变换指令σ=0,表明前方通道足够宽,使编队保持原队形继续前进;此时,队形宽度设置为D=D0。
(2)当伸缩因子处于第一阈值和1的取值区间内时;输出队形同构变换指令σ=1,表明机器人编队如果按照原队形继续前进则可能和道路边界发生碰撞,使编队进行队形收缩后通过前方的通道;此时,队形宽度设置为D=D0ξ-B。
(3)当伸缩因子处于第二阈值和第一阈值的取值区间内时;输出队形异构变换指令σ=2,使编队变换成新的队形通过前方通道;
表明前方道路宽度只允许单个机器人通过,必须变换程柱状队形才能够通过。此时,队形宽度设置为D=B。
(4)当伸缩因子不大于第二阈值时,输出队形异常状态指令σ=3,编队无法通过前方通道,等待下一步指令。即等待总控平台发布是否结束本次编队任务或者执行新的编队路径指令。
可由此得出,领航机器人与虚拟领航机器人间的期望距离、期望方位角如下式:
各跟随机器人和虚拟领航机器人间的期望距离、期望方位角如下式:
其中,
具体的队形变换动态优化流程如图6所示。
为进一步评价时变编队变换方法的性能,本实施例中建立了新的评价指标函数,包括能量损耗率、队形调节时间比、队形失真度在内的队形变换性能评价函数。具体的,评价指标函数设计如下:
(1)能量损耗函数
目前,机器人大多采用机载电池供电,电能的消耗将对机器人执行任务时长起到决定性作用。而在编队变换过程中较小的能耗可以为机器人实现更长时间的连续工作提供一定的保障,这对实际多机器人编队任务的执行有着重要的现实意义。机器人i的能量函数可以建模为:
式中,vi为机器人i的速度。
则,机器人i在时间[t0,t]内总的能耗为:
则,编队在时间[t0,t]内队形变换总的能量损耗率函数设置为:
式中,ΔEr(t)和ΔEt(t)分别为编队在时间[t0,t]内的实际能耗和理论能耗。
由此可知,fe(t)值越小表明能耗越少,机器人连续工作的时间越长。
(2)队形调节时间函数
当编队接受到队形变换指令时从当前队形变换到目标队形需要一定的调节时间,为了使调节时间尽可能短,因此,队形调节时间效率也是影响编队整体执行任务效率的影响因素之一。将队形调节时间比作为评价队形变换的一个重要指标,一方面可以用来描述队形结构的分散程度;另一方面,反映编队变换的响应速度,当编队通过不同宽度的通道时需要多次变换队形,而队形调节总时间与行驶总时间的比值从侧面反映出编队从当前队形变换到目标队形的收敛速度。则,编队在时间[t0,t]内队形调节时间比函数设置为:
由此可知,fr(t)值越小表现编队变换所花费的时间越小,变换效率越高。
(3)队形结构失真函数
多机器人***在行进过程中由于通道宽度的变化,这时需要变换合适的队形以适应环境变换,为最大的优化队形,其队形变形量应尽可能小。将队形失真度作为评价队形变换性能的函数之一,来描述编队当前队形与期望队形的畸变程度。队形结构失真函数为:
由此可知,fs(t)值越小表明队形失真度越小,与期望队形越接近。
由于不同的多机器人***对各性能函数的要求不一样,比如,对于多消防机器人***,能量损耗和队形调节时间的权重相比队形失真度会更高,所以我们对各性能函数设置了权重因子,以区分各性能指标的重视程度。则,多机器人***的队形变换的性能综合评价指标函数为:
f(t)=λefe(t)+λafa(t)+λsfs(t) (26)
其中,λe>0、λr>0、λs>0分别为能量损耗函数权重因子、队形调节时间函数权重因子、队形稳定性函数权重因子,并且满足λe+λr+λs=1。
环境适应度函数f(t)可以作为衡量编队变换策略优劣的综合效能评价指标。其值越小表明编队变换策略越优。
具体的,步骤S103中通过分布式协同耦合的编队控制器对编队中的多个机器人进行运动控制方法包括:
1)根据虚拟领航机器人和编队机器人的一阶积分器模型,确定出编队机器人与虚拟领航机器人之间的相对位移和相对速度;
2)建立圆形运动控制律,引导每个编队机器人以平滑圆弧运动的方式收敛到以虚拟领航机器人为中心、以与虚拟领航机器人之间期望距离为半径的圆上;
具体的,所述圆形运动控制律,引导每个机器人以平滑圆弧运动的方式收敛到以虚拟领航机器人为中心所形成的圆上,而不考虑圆上具***置。该控制是通过旋转一个固定的参考矢量来实现以目标点p0为中心逆时针旋转,可以得到一个旋转变化的圆形轨迹。然后,将这个旋转变化轨迹作为参考轨迹,就能得到圆形参考轨迹,机器人跟踪这条轨迹做圆周运动。控制目标可描述如下:
则,圆形运动控制器设计为:
式中,kp>0为位置调节参数,用于调整机器人圆形轨迹跟踪的收敛速度。
在本实施例中对控制律(31)的控制目标的实现进行了证明;
证明:定义位置跟踪误差为:
如果式(32收敛到零,则误差的差分方程可以写为:
则可以继续展开为要使得误差收敛到零,误差动态方程可写成
其中,
3)在圆形运动控制律的基础上耦合一个方位角定位的控制律;根据耦合了方位角定位的圆形运动控制律和避障控制律,对每个编队机器人进行具有避障功能的编队控制。
其中,
考虑到实际应用,有时有必要确定机器人在自身的圆上的具***置。例如,当一个多机器人***监控感兴趣的区域,需要预先规划每个机器人的准确位置监视特定区域。因此,有必要设计一种方位角间距分配控制律,使机器人能够到达圆上指定的位置。我们在控制器(36)中引入一个非线性函数:
证明:选择一个正定的李亚普洛夫函数:
式(39)两端对时间求导,得到差分方程为:
考虑到编队成员运动过程或队形变换中将不可避免地与环境中障碍物和其他成员发生碰撞,有必要设计一个有效的避障控制律,不仅使机器人能够有效避开障碍物,同时也保证队形的稳定性。
具体的,所述避障控制律通过设置斥力势场产生排斥力进行避障。
人工势场法作为一种经典的避障控制方法,广泛应用于机器人的运动控制中。在实施例中没有必要设置额外的引力场函数产生吸引力,因为每个机器人有自己的轨迹需要跟踪,这种由目标位置牵引的作用表现可以看作是引力势场产生“引力速度”,所以仅通过设置斥力场势场产生排斥力来避开障碍物,传统的斥力场函数定义为:
然后,由于机器人接收到的斥力沿斥力场函数的负梯度,因此避障控制律表示如下:
由于传统的人工势场法存在局部极小值和目标不可达的问题。如图7所示
本实施例中通过改进的人工势场法设置斥力势场进行多机器人编队的避障控制,输出避障控制律;
在设置斥力势场中,通过建立道路边界作用的斥力速度,以限制机器人的行驶区域;将障碍物作用的斥力速度和道路边界作用的斥力速度进行合成得到斥力合速度;通过调整斥力合速度的方向输出最终的避障控制律,以解决局部极小值和目标不可达问题,使编队机器人在道路的安全区域内进行避障。
其实现效果如图8所示。
这里假设建立的惯性坐标系位于道路中间位置,即道路两端关于Xn对称。
设置道路边界作用的“斥力速度”为:
其中,ke>0是道路边界作用的斥力增益系数,W为道路宽度,B为机器人宽度(假设两驱动轮轴心距等于机器人车体宽度)。定义 是机器人i的偏置点pi距离道路边界上最近点的位置。ξi为对数障碍函数,其函数图像如图9所示。可以看出,当机器人靠近道路势场区域时,受到来自道路边界的法向方向的“斥力速度”,并随着机器人离道路边界越近,其“斥力速度”也越大,最后趋于无穷大;当机器人处于安全区域内,其“斥力速度”为0。
则,机器人i受到的“斥力合速度”为:
从图9可以看出“斥力速度”产生了2种作用,一是使机器人前往目标方向加减速,二是使得机器人产生转弯的切向力绕开障碍物,因此存在机器人在目标方向上合速度为0的情况,这时机器人容易陷入局部最优和目标不可达的问题。本实施例中通过调整“斥力合速度”的方向,使其变为与目标作用的“引力速度”方向垂直且远离障碍物的方向,使得斥力只改变运动方向,不改变前往目标的速度。
则,设置新的“斥力合速度”为:
最终,建立动态权重因子,对耦合了方位角定位的圆形运动控制律和避障控制律进行比值调整,得到分布式协同耦合的编队控制器对机器人i输出的最终输出的控制律ui;
如图10所示,为分布式协同耦合的编队控制器的控制框图,图中的控制器为A、B、C三部分耦合而成,A部分主要用于执行圆形运动控制律,B部分主要用于执行方位角定位控制律,C部分主要用于执行避障控制律。
综上所述,本发明的基于虚拟领航-领航-跟随的多机器人编队控制方法,解决多机器人的编队控制问题。
本发明采用虚拟领航-领航-跟随模式,领航机器人既负责整个***规划和协调,但又不影响到其他机器人的运动状态。解决了传统的领航者-跟随者模式的链式结构中跟随机器对领航机器的过度依赖,容易造成跟踪误差累积以及领航机器出现故障会导致整个***出现崩溃等问题。
本发明建立了队形的伸缩因子,在常规的队形变换状态基础上还考虑了异常状态情况,即当前方通道宽度不满足编队个体通过时的情况,需等待下一步指令;提出了衡量编队变换策略优劣的性能综合评价指标函数,该指标能够不依赖于***的动力学模型,其表达形式简单明了,不仅能够适用于多消防机器人的时变编队变换而且能适用于其他多智能体***。
本发明从实际应用出发,考虑了道路边界和停靠在路边的障碍物会与机器人发生碰撞,通过对道路边界和障碍物的轮廓添加了膨胀处理,使得机器人尽可能与道路边界和障碍物之间保持一个安全距离。
本发明建立了分布式协同耦合的编队控制器,在圆形运动控制律的基础上建立增加方位角定位的控制律,与避障控制律结合,能同时实现按编队队形行进的过程中有效地避开障碍物。
本发明设置斥力势场产生排斥力进行避障,能够不依赖于***的精确数学模型,以非常简单的算法实现不确定性高的非线性耦合动态***的避障控制,以及实现高效、安全、可靠的编队避障。并且把道路边界作用效果也考虑进去,使得机器人能够在道路的安全区域内有效避障,同时有效地解决传统人工势场法存在的局部最优和目标不可达的问题。
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。
Claims (10)
1.一种基于虚拟领航-领航-跟随的多机器人编队控制方法,其特征在于,包括以下步骤:
建立虚拟领航-领航-跟随的运动结构;指定编队中一台机器人为领航机器人,其余为跟随机器人,并设定一台虚拟领航机器人在编队参考轨迹点上随队行进;
领航机器人实时跟踪虚拟领航机器人的位置,根据检测和提取的包括道路边界及障碍物在内的环境信息,进行队形变换动态优化,得到跟随机器人与虚拟领航机器人之间的期望距离和期望方位角以适应当前环境;
通过分布式协同耦合的编队控制器对编队中的多个机器人进行运动控制;使每个机器人对虚拟领航机器人的实时位置进行跟踪;通过跟踪使领航机器人与虚拟领航机器人之间的期望距离和方位角收敛到零;使跟随机器人与虚拟领航机器人之间的距离和方位角收敛到期望的相对距离和方位角。
2.根据权利要求1所述的多机器人编队控制方法,其特征在于,跟随机器人与虚拟领航机器人之间的期望距离和期望方位角的获取方法,包括:
机器人编队以期望的初始队列构型,跟随虚拟领航机器人行进;
领航机器人利用自身携带的环境感知传感器检测和提取出道路边界信息以及障碍物信息,并计算出队形的伸缩因子;
根据队形的伸缩因子确定出队形变换指令分发到跟随机器人,使跟随机器人得到与虚拟领航机器人之间的期望距离和期望方位角。
3.根据权利要求2所述的多机器人编队控制方法,其特征在于,
所述领航机器人实时跟踪虚拟领航机器人的位置,并且通过自身携带的多线激光雷达实时感知环境信息,提取道路的边界、计算两侧道路宽度,检测和识别障碍物信息;并将检测到的部分障碍物边界点作为候选边界点,对道路边界点进行缺失修补,以得到最终的边界点信息。
4.根据权利要求3所述的多机器人编队控制方法,其特征在于,
通过对道路边界和障碍物的轮廓进行膨胀处理,设置膨胀层,使编队中机器人与道路边界和障碍物之间的距离大于膨胀层宽度。
5.根据权利要求2所述的多机器人编队控制方法,其特征在于,
领航机器人根据测得的最小通行宽度和初始队列构型的期望队形宽度之间的比值,作为队形的伸缩因子。
6.根据权利要求5所述的多机器人编队控制方法,其特征在于,根据队形的伸缩因子确定出队形变换指令包括:
当伸缩因子大于1时,输出队形零变换指令,使编队保持原队形继续前进;
当伸缩因子处于第一阈值和1的取值区间内时;输出队形同构变换指令,使编队进行队形收缩后通过前方的通道;
当伸缩因子处于第二阈值和第一阈值的取值区间内时;输出队形异构变换指令,使编队变换成新的队形通过前方通道;
当伸缩因子不大于第二阈值时,输出队形异常状态指令,编队无法通过前方通道,等待下一步指令。
7.根据权利要求1-6任一项所述的多机器人编队控制方法,其特征在于,所述分布式协同耦合的编队控制器的编队控制方法包括:
根据虚拟领航机器人和编队机器人的一阶积分器模型,确定出编队机器人与虚拟领航机器人之间的相对位移和相对速度;
建立圆形运动控制律,引导每个编队机器人以平滑圆弧运动的方式收敛到以虚拟领航机器人为中心、以与虚拟领航机器人之间期望距离为半径的圆上;
在圆形运动控制律的基础上耦合一个方位角定位的控制律;根据耦合了方位角定位的圆形运动控制律和避障控制律,对每个编队机器人进行具有避障功能的编队控制。
8.根据权利要求7所述的多机器人编队控制方法,其特征在于,所述避障控制律通过设置斥力势场产生排斥力进行避障。
9.根据权利要求8所述的多机器人编队控制方法,其特征在于,在设置斥力势场中,通过建立道路边界作用的斥力速度,以限制机器人的行驶区域;将障碍物作用的斥力速度和道路边界作用的斥力速度进行合成得到斥力合速度;通过调整斥力合速度的方向输出最终的避障控制律,以解决局部极小值和目标不可达问题,使编队机器人在道路的安全区域内进行避障。
10.根据权利要求9所述的多机器人编队控制方法,其特征在于,
建立动态权重因子,对耦合了方位角定位的圆形运动控制律和避障控制律进行比值调整,得到分布式协同耦合的编队控制器最终输出的控制律。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210884676.6A CN115167440A (zh) | 2022-07-25 | 2022-07-25 | 一种基于虚拟领航-领航-跟随的多机器人编队控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210884676.6A CN115167440A (zh) | 2022-07-25 | 2022-07-25 | 一种基于虚拟领航-领航-跟随的多机器人编队控制方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115167440A true CN115167440A (zh) | 2022-10-11 |
Family
ID=83497269
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210884676.6A Pending CN115167440A (zh) | 2022-07-25 | 2022-07-25 | 一种基于虚拟领航-领航-跟随的多机器人编队控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115167440A (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116300970A (zh) * | 2023-05-17 | 2023-06-23 | 斯润天朗(北京)科技有限公司 | 车辆自主编队方法及装置 |
CN116400604A (zh) * | 2023-06-06 | 2023-07-07 | 浙江大学海南研究院 | 具有避碰性能的多智能体***球形编队控制方法 |
CN116483096A (zh) * | 2023-06-25 | 2023-07-25 | 中国第一汽车股份有限公司 | 车辆编队控制方法、***、云平台及存储介质 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180074520A1 (en) * | 2016-09-13 | 2018-03-15 | Arrowonics Technologies Ltd. | Formation flight path coordination of unmanned aerial vehicles |
CN111694357A (zh) * | 2020-06-19 | 2020-09-22 | 国网福建省电力有限公司福州供电公司 | 基于遗传算法和人工势场法的机器人行走路径规划方法 |
CN111813121A (zh) * | 2020-07-13 | 2020-10-23 | 陕西理工大学 | 基于距离-角度优先级的多移动机器人编队避障方法 |
CN111880571A (zh) * | 2020-08-19 | 2020-11-03 | 武汉中海庭数据技术有限公司 | 一种无人机刚性队形切换方法及装置 |
CN113110495A (zh) * | 2021-05-08 | 2021-07-13 | 广东省智能机器人研究院 | 一种考虑外部干扰下移动机器人的编队控制方法 |
-
2022
- 2022-07-25 CN CN202210884676.6A patent/CN115167440A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180074520A1 (en) * | 2016-09-13 | 2018-03-15 | Arrowonics Technologies Ltd. | Formation flight path coordination of unmanned aerial vehicles |
CN111694357A (zh) * | 2020-06-19 | 2020-09-22 | 国网福建省电力有限公司福州供电公司 | 基于遗传算法和人工势场法的机器人行走路径规划方法 |
CN111813121A (zh) * | 2020-07-13 | 2020-10-23 | 陕西理工大学 | 基于距离-角度优先级的多移动机器人编队避障方法 |
CN111880571A (zh) * | 2020-08-19 | 2020-11-03 | 武汉中海庭数据技术有限公司 | 一种无人机刚性队形切换方法及装置 |
CN113110495A (zh) * | 2021-05-08 | 2021-07-13 | 广东省智能机器人研究院 | 一种考虑外部干扰下移动机器人的编队控制方法 |
Non-Patent Citations (9)
Title |
---|
LIQIAN DOU: "Distributed Finite-time Formation Control for Quadrotors Based on Adaptive State Observer", 《PROCEEDINGS OF THE 38TH CHINESE CONTROL CONFERENCE》, 30 July 2019 (2019-07-30) * |
SHUFENG WANG: "Artificial potential field algorithm for path control of unmanned ground vehicles formation in highway", 《ELECTRONICS LETTERS》, 4 October 2018 (2018-10-04) * |
任立敏: "障碍环境下多移动机器人动态优化队形变换", 《机器人》, 30 September 2013 (2013-09-30), pages 539 - 540 * |
夏长高: "融合B 样条与改进APF 的路径规划算法研究", 《重庆理工大学学报》, 15 June 2022 (2022-06-15), pages 49 * |
杨庆凯;潘云龙;周勃;刘虹;方浩;: "基于仿射变换的微型无人机编队飞行控制技术", 指挥与控制学报, no. 02, 15 June 2020 (2020-06-15) * |
王刚: "Cooperative Target Enclosing and Tracking Control with Obstacles Avoidance for Multiple Nonholonomic Mobile Robots", 《APPLIED SCIENCES》, 11 March 2022 (2022-03-11), pages 1 - 8 * |
王刚: "面向多智能消防机器人的编队避障控制方法", 《机器人》, 11 December 2023 (2023-12-11) * |
王树凤: "基于人工势场和虚拟领航者的智能车辆编队控制", 《上海交通大学学报》, 31 March 2020 (2020-03-31) * |
郑延斌: "基于模糊人工势场法的多智能体编队控制及避障方法", 《计算机工程与科学》, 31 August 2019 (2019-08-31) * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116300970A (zh) * | 2023-05-17 | 2023-06-23 | 斯润天朗(北京)科技有限公司 | 车辆自主编队方法及装置 |
CN116300970B (zh) * | 2023-05-17 | 2023-09-29 | 斯润天朗(北京)科技有限公司 | 车辆自主编队方法及装置 |
CN116400604A (zh) * | 2023-06-06 | 2023-07-07 | 浙江大学海南研究院 | 具有避碰性能的多智能体***球形编队控制方法 |
CN116400604B (zh) * | 2023-06-06 | 2023-08-18 | 浙江大学海南研究院 | 具有避碰性能的多智能体***球形编队控制方法 |
CN116483096A (zh) * | 2023-06-25 | 2023-07-25 | 中国第一汽车股份有限公司 | 车辆编队控制方法、***、云平台及存储介质 |
CN116483096B (zh) * | 2023-06-25 | 2023-09-22 | 中国第一汽车股份有限公司 | 车辆编队控制方法、***、云平台及存储介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN115167440A (zh) | 一种基于虚拟领航-领航-跟随的多机器人编队控制方法 | |
CN110262495B (zh) | 可实现移动机器人自主导航与精确定位的控制***及方法 | |
Lacaze et al. | Path planning for autonomous vehicles driving over rough terrain | |
CN104881025B (zh) | 一种地下矿用车辆的反应式导航控制方法 | |
CN109947123A (zh) | 一种基于视线导引律的无人机路径跟踪与自主避障方法 | |
CN110398969A (zh) | 自动驾驶车辆自适应预测时域转向控制方法及装置 | |
CN112684807A (zh) | 无人机集群三维编队方法 | |
WO2022083292A1 (zh) | 一种智能移动机器人全局路径规划方法和*** | |
CN106406338A (zh) | 一种基于激光测距仪的全向移动机器人的自主导航装置及其方法 | |
CN111258316B (zh) | 一种动态环境下趋势感知的机器人轨迹规划方法 | |
CN114442621A (zh) | 一种基于四足机器人的自主探索和建图*** | |
Jian et al. | Putn: A plane-fitting based uneven terrain navigation framework | |
CN114022824A (zh) | 一种面向狭窄环境的四足机器人运动规划方法 | |
Fu et al. | Path planning and decision making for autonomous vehicle in urban environment | |
Li et al. | Hybrid filtering framework based robust localization for industrial vehicles | |
CN114200945A (zh) | 一种移动机器人的安全控制方法 | |
Funk et al. | Autonomous electric race car design | |
Yang et al. | Mobile robot motion control and autonomous navigation in GPS-denied outdoor environments using 3D laser scanning | |
CN115032999A (zh) | 面向多机器人编队的动态优化队形变换方法 | |
CN114942642A (zh) | 一种无人驾驶汽车轨迹规划方法 | |
Hu et al. | Path planning of UGV based on Bézier curves | |
Brock et al. | Coordination and control of multiple autonomous vehicles. | |
CN117249817A (zh) | 野外环境下管道巡检机器人轻量化自主导航***及方法 | |
Jordan et al. | Real-time model based path planning for wheeled vehicles | |
AU2021448614A1 (en) | Precise stopping system and method for multi-axis flatbed vehicle |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |