一种基于仿真推演的编队队形调整方法

1.本发明涉及一种基于仿真推演的编队队形调整方法，属于队形控制技术领域。


背景技术：

2.为了完成国庆阅兵装备分列式检阅任务，参与人员需要驾驶车辆多次实地演练，但是车辆方阵在出现故障或者遇到障碍物时，无法按照原来队形前进，容易造成现场混乱，需要指挥车辆方阵迅速变换队形继续前进。传统实地训练不仅效率低，而且消耗大量人力物力，不利于工作人员快速找出车辆方阵队形变换的规律。
3.传统的车辆方阵队形变换方法，通常是根据预案设计方阵队列变换方式，然后再通过参与人员实际演练进行验证，效率低、训练效果不佳。为此有人提出在三维虚拟场景中模拟车辆方阵分列式行进和队形变换，这样不仅可以降低阅兵方阵训练所需要的花费，而且可以多次快速高效的对车辆方阵队形变换方法进行验证，有效提高了工作人员对车辆方阵分列式行进和快速队形变换规律的掌握情况，为工作人员规划阅兵方阵队形变换决策提供依据。但是目前的这种仿真方式没有考虑遇到障碍物后队形如何进行变换。


技术实现要素：

4.本发明的目的是提供一种基于仿真推演的编队队形调整方法，以解决目前仿真过程中缺乏遇到障碍物如何进行规划的问题。
5.本发明为解决上述技术问题而提供一种基于仿真推演的编队队形调整方法，其特征在于，该调整方法包括以下步骤：
6.1)根据车辆方阵行走路线构建三维场景；
7.2)获取车辆方阵的位置信息、编队信息；位置信息包括经度、纬度以及高度；编队信息包括队形、数量；
8.3)获取车辆方阵行驶的道路环境信息，提取道路上阻碍车辆方阵行驶的障碍物的位置信息；障碍物的位置信息包括经度、纬度，以及障碍物的长度、宽度；
9.4)根据车辆方阵队形信息和障碍物信息判断障碍物是否阻碍车辆方阵前行，若障碍物阻碍车辆方阵前行，根据障碍物的位置信息规划行走路线；
10.在根据障碍物的位置信息规划行走路线时时将仿真实体假设为带同种电荷的点电荷，将障碍物假设为带同种电荷的导体，构建人工势场，根据仿真实体在人工势场中的受力情况规划车辆方阵行走路线，避免车辆方阵与障碍物发生碰撞；
11.5)根据规划的行走路线控制车辆方阵的队列变换，在车辆方阵穿过障碍物时，控制车辆方阵恢复原来队形并规划行走路线继续前行。
12.本发明通过构建三维场景和车辆模型仿真，将车辆方阵模型在三维场景中显示；如果道路中出现障碍物阻碍车辆方阵移动，则车辆方阵开始自动调整队形通过障碍物，通过后恢复原来方阵队形并按计划继续前行。其中，在根据障碍物的位置信息规划行走路线时将仿真实体假设为带同种电荷的点电荷，将障碍物假设为带同种电荷的导体，构建人工
势场，根据仿真实体在人工势场中的受力情况规划车辆方阵行走路线，这种方式能够准确、快速规划出变换路线，不仅提高了模拟车辆方阵队形变换及恢复的效率，更便于处理车辆方阵在行进过程中出现的突发情况。
13.进一步地，所述步骤4)中的构建的人工势场的函数模型为：
[0014][0015][0016]
η表示为引力系数；x表示目标点位置，x1表示仿真实体位置，ρ(x，x1)2表示仿真实体与目标点的相对位置，u
att
为目标点对仿真实体所产生的引力场，k
rep
表示斥力系数，x0表示障碍物位置，ρ(x1，x0)表示仿真实体与机器人的相对位置，ρ0表示斥力场最大影响距离，u
rep
表示目标点对仿真实体所产生的斥力场。
[0017]
本发明通过人工势场来模拟车辆通过障碍物的过程，将所有仿真实体假设为带同种电荷的点电荷，将障碍物假设为带同种电荷的导体，整个车辆编队假设为由点电荷组成的编队，通过构建人工势场，确定点电荷在电势场中运动情况，为后续控制提供可靠依据。
[0018]
进一步地，所述步骤4)中的仿真实体在人工势场中的受力为：
[0019]
f＝f
att
f
rep
[0020]fatt
＝ηρ(x，x1)
[0021][0022]
其中f表示仿真实体在人工势场中所受合力，f
att
表示仿真实体在人工势场中所受的引力，f
rep
表示仿真实体在人工势场中所受的斥力。
[0023]
本发明构建的人工势场中，仿真实体在斥力的作用进行运动，仿真实体会偏离预定队形目标点位置，仿真实体与目标点之间会产生引力；本发明基于仿真实体所受引力与斥力之和作为所受到的合力，能够准确模拟仿真实体在碰到障碍物时的运动情况。
[0024]
进一步地，仿真实体采取leader-follower模式保持编队队形，后一个编队成员以前一个编队成员为leader，并跟随其运动所述步骤5)中对队形变换指的是leader在队伍中的位置不发生变化，follower在队形中位置发生变化。
[0025]
本发明采用的leader-follower模式控制仿真实体的运行，能够准确模拟真实车辆仿真的运行情况。
[0026]
进一步地，follower的运动模型为：
[0027][0028]
其中θ，θ1，θ
′1分别表示leader，follower以及虚拟follower的速度方向与水平方向的夹角，x(t)、y(t)分别表示leader的位置在x轴方向与y轴方向上的分量，x
′1(t)和y
′1(t)表示虚拟follower的位置在x轴方向与y轴方向上的分量，d1表示leader与虚拟
follower的相对距离，θ0为leader和虚拟follower位置连线与虚拟follower运动方向的夹角，θ与θ
′i的大小相等。
[0029]
本发明通过leader在队伍中的位置不发生变化，follower在队形中位置发生变化，引入虚拟follower代表处于新队形位置处的follower，其中引入的虚拟follower的速度方向与水平方向的夹角和leader的速度方向与水平方向的夹角相同，follower根据与虚拟follower的相对位置关系调整自身运动状态，使得follower驶向虚拟follower位置处，完成队形的变换。
[0030]
进一步地，单位时间内follower相对于虚拟follower的位置偏差采用的计算公式为：
[0031][0032]
其中δx为follower相对于虚拟follower的x轴的位置偏差，δy为follower相对于虚拟follower的y轴的位置偏差，δθ为follower相对于虚拟follower的角度偏差，x1(t)、y1(t)分别表示follower的位置在x轴方向与y轴方向上的分量。
[0033]
本发明以follower为中心的坐标系下，根据leader和虚拟follower之间的相对距离以及follower的速度方向与水平方向夹角等来计算follower相对于虚拟follower的位置偏差，能够准确描述中二者的位置偏差，为后续控制follower提供可靠地支撑。
[0034]
进一步地，若障碍物没有阻碍车辆方阵前行，车辆方阵按预定队形继续前行。
附图说明
[0035]
图1是本发明基于仿真推演的编队队形调整方法的流程图；
[0036]
图2是本发明实施例中采用编队队形结构示意图；
[0037]
图3是本发明采用的leader-follower的运动模型示意图；
[0038]
图4是本发明所构建的人工势场示意图；
[0039]
图5是本发明中队形变换过程示意图。
具体实施方式
[0040]
下面结合附图对本发明的具体实施方式作进一步地说明。
[0041]
本发明通过车辆模型，在车辆方阵行走路线构建三维场景，在车辆模型方阵按照机动路线进行前行时，实时传输车辆方阵队形信息，并将车辆方阵的队形信息结合车辆方阵模型的位置、姿态快速找到车辆方阵模型在三维场景中的位置，将车辆方阵模型在三维场景中显示，并根据车辆方阵移动情况不断更新道路信息，如果道路中出现障碍物阻碍车辆方阵移动，则车辆方阵开始自动调整队形通过障碍物，通过后恢复原来方阵队形并按计划继续前行。该方法不仅提高了模拟车辆方阵队形变换及恢复的效率，更便于处理车辆方阵在行进过程中出现的突发情况。其实现流程如图1所示，具体实施过程如下。
[0042]
1.根据车辆方阵行走路线构建三维场景。
[0043]
根据车辆方阵的行驶路线构建该行驶路线下的三维场景，三维场景包括行驶路线下的地理信息模型、以及车辆方阵模型。
[0044]
2.获取行驶过程中车辆方阵的位置信息、编队信息。
[0045]
车辆按照行驶路线进行实际行驶，得到行驶数据；行驶数据包括阅兵方阵的位置信息、姿态信息以及行驶路线环境信息，将行驶数据实时发送至仿真系统。本实施例中，车辆方阵的行驶路线为直线，行驶路线的起点和终点包括以下参数：车辆方阵所处经度xi，车辆方阵所处纬度yi，车辆方阵的行走高度zi，车辆方阵的行走速度vi，具体参数如下：
[0046]
起点
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
//航路点名称
[0047]
113.076250
°ꢀꢀꢀꢀꢀꢀ
//航路点经度
[0048]
34.458422
°ꢀꢀꢀꢀꢀꢀꢀ
//航路点纬度
[0049]2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
//航路点高度
[0050]
10
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
//航路点速度
[0051]
终点
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
//航路点名称
[0052]
113.084751
°ꢀꢀꢀꢀꢀꢀ
//航路点经度
[0053]
34.458459
°ꢀꢀꢀꢀꢀꢀꢀ
//航路点纬度
[0054]2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
//航路点高度
[0055]
10
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
//航路点速度
[0056]
根据计划路线行驶时，车辆方阵持偏航角为0
°
，俯仰角0
°
，翻滚角为0
°
的姿态沿着指定路线行驶；并且根据车辆方阵模型行驶路线和行驶速度，得到车辆方阵在大地坐标系下位置信息，位置信息包括经度、纬度、高度。
[0057]
3.获取车辆方阵行驶的道路环境信息，提取道路上阻碍车辆方阵行驶的障碍物的位置信息。
[0058]
获取的障碍物位置信息包括经度、纬度，以及障碍物的长度、宽度和高度。本实施例中，车辆方阵行驶路线上存在障碍物，障碍物中心点的经度：113.080687；纬度：34.458416；长度:80m；宽度:55m，其中障碍物中存在障碍物1的经度：113.080506；纬度：34.458580；长度：5m；宽度：3.8m；障碍物2的经度：113.080982；纬度：34.458366；长度6；宽度1.5m；障碍物3的经度：113.080964；纬度：34.458502；长度：3m；宽度：15m。
[0059]
4.根据车辆方阵队形信息和障碍物信息规划车辆方阵的队形变换，若障碍物没有阻挡车辆方阵队列前行，车辆方阵按预定队形继续前行。
[0060]
在车辆方阵仿真实体中，采取leader-follower模式保持编队队形，即后一个编队成员以前一个编队成员为领导者，并跟随其运动。
[0061]
本实施例中，以五个仿真实体为例，如图2所示，图中每个节点代表一个仿真实体，箭头表示跟随关系，可以根据跟随关系将编队队形分成为四组，分别是f1f2，f1f3，f2f4，f
3 f4。f1f2表示f1为f2的leader。其矩阵一般表达式如下：
[0062][0063]
在一般表达式中，fi表示第i个仿真实体的leader，di表示第i个仿真实体与其leader的相对距离，表示第i个仿真实体的速度矢量和相对于其leader的位置矢量的夹
角。
[0064]
在形成队形后，编队在leader感知信息作出判断后向前运动，follower跟随leader向前移动，其运动关系如图3所示，其中，l1与l2表示运动的路径，f1(x1，y1)与f2(x2，y2)表示leader与follower的坐标位置，d表示f1与f2的相对位置距离，δx与δy表示d在x轴方向与y轴方向上的相对位置距离，v
ix
与v
iy
则表示第i个仿真实体的速度在x轴方向与y轴方向上的分量。
[0065]
任意时刻t下，leader的位置坐标都可表示如下：
[0066][0067]
在不受干扰的情况下，整个队伍是保持一定稳定的队形行进的，即f1与f2的相对位置距离固定，所以任意时刻下都可以由leader的位置推算出follower的坐标位置：
[0068][0069]
5.若障碍物阻挡车辆方阵队列前行，重新规划行驶路线。
[0070]
若障碍物阻挡车辆方阵队列前行，车辆方阵在通过障碍物域时队形进行自动调整；在仿真实体运动过程中，为实现整个过程中所有成员的避免碰撞，本发明参考了带电电荷在电势场中，与带同种电荷的导体之间会产生斥力，使得带电电荷与导体不会发生碰撞的原理，将所有仿真实体假设为带同种电荷的点电荷，将障碍物假设为带同种电荷的导体，整个过程就能假设为由点电荷组成的编队，在电势场中运动，不与带电导体发生碰撞。其势场示意如图4所示。
[0071]
人工势场通过仿真实体感知周围的环境信息，根据目标点和障碍物的位置，分别构建目标点所产生的引力场和障碍物所产生的斥力场，仿真实体在人工势场中根据受力的情况进行运动。其中引力场函数模型表达如下：
[0072][0073]
式中：η表示为引力系数；x表示目标点位置，x1表示仿真实体位置，ρ(x，x1)2表示仿真实体与目标点的相对位置；u
att
为目标点对仿真实体所产生的引力场。由势力场函数可得，仿真实体距离目标点位置越远，其所受引力势能越大，反之越小，故其引力大小可表示为：
[0074]fatt
＝ηp(x，x1)
ꢀꢀꢀꢀ
(5)
[0075]
斥力场由环境中的障碍物产生，其具有正势能，对于环境中的仿真实体存在斥力，且随着其与障碍物的距离的减小而增大，方向为仿真实体与障碍物连线且远离障碍物。当仿真实体与障碍物越接近，斥力势能越大，当抵达障碍物时，其斥力势能可表示为无穷大，体现出仿真实体尽量避免与障碍物发生碰撞；反之，距离障碍物足够远时，斥力势能趋近于0，斥力场函数模型表达如下：
[0076][0077]
其中k
rep
表示斥力系数，x1表示仿真实体位置，x0表示障碍物位置，ρ(x1，x0)表示仿
真实体与机器人的相对位置，ρ0表示斥力场最大影响距离。可以看出当仿真实体达到某一位置，与障碍物距离超过最大影响距离ρ0时，将不再受到斥力的影响，即不会出现碰撞的情况。当仿真实体进入障碍物影响范围后，距离障碍物越近，其受斥力影响程度越大。故其斥力函数可表示为：
[0078][0079]
根据上述的势场函数和受力函数，可以得到控制仿真实体运动的合势场函数以及仿真实体所受到的合力，引力场以及斥力场函数之和表示为：
[0080]
s＝u
att
u
rep
ꢀꢀ
(8)
[0081]
仿真实体所受合力为引力与斥力之和，表示为：
[0082]
f＝f
att
f
rep
ꢀꢀꢀꢀ
(9)
[0083]
当仿真实体通过人工势场时，在引力场和斥力场的作用下，根据受力情况进行运动，由于设定仿真实体位置与障碍物重合时，斥力为无限大，所以仿真实体不会与障碍物相撞，且运动方向与合力f的方向保持一致。
[0084]
6.根据规划的行走路线控制车辆方阵的队列变换，在车辆方阵穿过障碍物时，控制车辆方阵恢复原来队形并规划行走路线继续前行。
[0085]
若阅兵方阵穿越完障碍物，阅兵方阵恢复原来队形并规划行走路线继续前行；在整个过程中，编队队形主要经历了队形重组这一过程，在这个过程中，假设leader在队伍中的位置不发生变化，follower在队形中位置发生变化，本发明引入虚拟follower代表处于新队形位置处的follower。
[0086]
图5中f，f1，f
′1分别表示leader，follower以及虚拟follower的位置，(x，y)，(x1，y1)，(x
′1，y
′1)分别表示leader，follower以及虚拟follower的坐标，θ，θ1，θ
′1分别表示leader，follower以及虚拟follower的速度方向与水平方向的夹角。根据图5中所示相对位置关系可得：
[0087][0088]
其中d1为leader和虚拟follower之间的距离值，θ0为leader和虚拟follower位置连线与虚拟follower运动方向的夹角，θ与θ
′1的大小相等，即θ＝θ
′1。
[0089]
在编队队形变换过程中，本发明假设leader的运动状态不发生改变，follower根据与虚拟follower的相对位置关系调整自身运动状态，使得follower驶向虚拟follower位置处，完成队形的变换。故此，在以follower为中心的坐标系下，单位时间内follower相对于虚拟follower的位置偏差：
[0090][0091]
因此可以根据偏差的变化得出关于编队队形变化过程中follower的运动模型，计算follower的位置，使编队恢复原来的队形并继续前进。
[0092]
上述实施例中，为了方便描述，采用步骤的形式描述车辆方阵队形的自动调整方法，但是仿真系统在模拟过程中并不局限于上述步骤，其中步骤4和步骤5是可以同时进行的，只要将将方阵模型的位置信息传入系统即可。
[0093]
本发明将虚拟车辆方阵在经过障碍物时队形变换过程在三维场景中实时显示，根据车辆方阵移动情况不断更新道路信息，如果道路中出现障碍物阻碍车辆方阵移动，则车辆方阵开始自动调整队形通过障碍区，通过后恢复原来方阵队形并按计划继续前行，并在三维场景中进行显示，本发明提高了实时模拟测试车辆方阵队形变换的效率，更便于组织车辆方阵的队形调整。