一种六车编队中快速队形变换的自主编号方法

技术领域

本发明涉及一种六车编队中快速队形变换的自主编号方法，属于编队控制技术领域。

背景技术

编队是多智能体无人系统的主要有效控制方式，一般分为领航者跟随者方法、行为规则法、虚拟结构法、强化学习等方法。目前占主流的工程应用方法为领航者跟随者方法，该方法具有控制方式简单、控制稳定的优点；但同时也存在需要提前人为指定领航者和跟随者，通信交互复杂，队形变换过程各跟随者轨迹规划容易出错和发生交叉、碰撞，队形变换困难等不足。

发明内容

本发明的目的在于克服现有技术中的不足，提供一种六车编队中快速队形变换的自主编号方法，实现了多种队形变换的快速、无碰撞。

为了实现上述目标，本发明采用如下的技术方案：

一种六车编队中快速队形变换的自主编号方法，包括如下步骤：

所有参与编队的智能体在触发编队后，以触发时刻为基准时间，按照自身ID号大小，计算广播信息时刻并在该时刻广播自身位姿数据给所有其它智能体；

每个智能体在获取到所有其它智能体位姿后，结合自身位姿，计算从左到右的排列位置，基于统一编队编号规则计算自身编队编号，并定义领航者和跟随者；

当智能体计算结果为自身是领航者时，领航者根据自身目标路径独立导航，并开始广播自身位姿；其它跟随者以收到领航者广播信息为时间基准，按照编队编号大小依次发送自身位姿信息给领航者；跟随者根据领航者位姿和编队编号进行实时目标位置计算并控制自身前往该目标点。

进一步地，前述所有参与编队的智能体在触发编队后，以触发时刻为基准时间，按照自身ID号大小，计算广播信息时刻并在该时刻广播自身位姿数据给所有其它智能体的步骤包括：

对每个智能体赋予一个ID号，且初始阶段所有智能体随意排列；

智能体根据ID号从小到大并间隔固定时间段，依次向其他智能体广播自身位姿，位姿信息包括GNSS位置和当前车头朝向角度值。

进一步地，前述固定时间段为30ms。

进一步地，前述每个智能体在获取到所有其它智能体位姿后，结合自身位姿，计算从左到右的排列位置，基于统一编队编号规则计算自身编队编号，并定义领航者和跟随者的步骤包括：

各智能体在收到所有其它智能体的位姿后，根据朝向计算出所有智能体从左到右的位置顺序：每个智能体将接收到的其它5个智能体的位姿信息转换为自身坐标系里的坐标(X

基于统一编队编号规则和当前智能体从左到右排列的顺序，计算智能体自身编队编号N，编号规则计算式为：

N＝A[i]，其中A＝[4 2 1 5 3 6]

其中i为智能体从左到右的排列顺序；

将编号为1的智能体定义为领航者，其余编号的智能体定义为跟随者。

进一步地，前述当智能体计算结果为自身是领航者时，领航者根据自身目标路径独立导航，并开始广播自身位姿的步骤包括：

当智能体计算结果为自身的编队编号为1时，该智能体根据自身目标路径独立导航，并开始向其他跟随者广播自身位姿。

进一步地，前述其它跟随者以收到领航者广播信息为时间基准，按照编队编号大小依次发送自身位姿信息给领航者的步骤包括：

其它跟随者以收到领航者广播信息为时间基准，根据编队编号从小到大并间隔固定时间段，依次发送自身位姿信息给领航者。

进一步地，前述跟随者根据领航者位姿和编队编号进行实时目标位置计算的步骤包括：

以领航者在世界坐标系中的位置[x

式中，[x

进一步地，前述当队形为横向一字编队时，队形矩阵A表达式为：

其中/>

式中，d为编队间距，θ

进一步地，前述当队形为纵向一字编队时，队形矩阵A表达式为：

其中θ

式中，d为编队间距，θ

进一步地，前述当队形为三角编队时，队形矩阵A表达式为：

其中/>

式中，d为编队间距，θ

与现有技术相比，本发明所达到的有益效果：

本发明通过为领航者与各跟随者制定统一的分时广播机制，以实现同一频段通信系统中信息交互的顺利进行；在此基础上，基于统一的位置和编队编号映射机制，实现所有智能体都能独立计算自身的编队编号并保证所有分布计算结果一致；更进一步，基于该编队编号排列方式，结合实时编队过程中的各从智能体计算实时目标点方法，能够实现六个智能体编队的不同队形快速、轨迹无碰撞切换。另外，通过建立各跟随者的离线队形矩阵表，各跟随者能够快速获取当前时刻的目标位置，加快了编队运行过程中的计算速度。

附图说明

图1是本发明六车编队中快速队形变换的自主编号方法的主要工作流程图；

图2是本发明提供的初始位置编队编号分配示意图；

图3是本发明三角队形变换为横向一字队形轨迹图；

图4a是本发明三角队形变换为横向一字队形变换前的三角队形仿真图；

图4b是本发明三角队形变换为横向一字队形变换后的横向一字队形仿真图；

图5是本发明横向一字队形变换为纵向一字队形轨迹图；

图6a是本发明横向一字队形变换为纵向一字队形变换前的横向一字队形仿真图；

图6b是本发明横向一字队形变换为纵向一字队形变换后的纵向一字队形仿真图。

具体实施方式

下面通过附图以及具体实施例对本发明技术方案做详细的说明，应当理解本申请实施例以及实施例中的具体特征是对本申请技术方案的详细的说明，而不是对本申请技术方案的限定，在不冲突的情况下，本申请实施例以及实施例中的技术特征可以相互组合。

本实施例公开了一种六车编队中快速队形变换的自主编号方法，结合图1和图2，包括如下步骤：

S1：所有参与编队的智能体在触发编队后，以触发时刻为基准时间，按照自身ID号大小，计算广播信息时刻并在该时刻广播自身位姿数据给所有其它智能体。

如图2所示，每个智能体在初始都赋予一个ID号，且初始阶段所有智能体随意排列，根据ID号从小到大并间隔固定时间段，依次向其他智能体广播自身的GNSS位置和当前车头朝向角度值，这样各智能体地广播数据在同一频段地通信系统中不会互相干扰，保证通信的顺利进行：首先ID为1的智能体立即将自身的GNSS位置和当前车头朝向角度值发送给其它所有智能体，ID为2的智能体在延迟30ms后同样将自身的GNSS位置和当前车头朝向角度值广播给其它所有智能体，后续ID为3、4、5、6的智能体依次延迟60ms，90ms，120ms，150ms后将自身的GNSS位置和当前车头朝向角度值广播给其它所有智能体。

S2：每个智能体在获取到所有其它智能体位姿后，结合自身位姿，计算从左到右的排列位置，基于统一编队编号规则计算自身编队编号。

各智能体在收到所有其它智能体的位姿后，根据朝向计算出所有智能体从左到右的位置顺序：每个智能体将将接收到的其它5个智能体的位姿信息转换为自身坐标系里的坐标(X

然后，基于统一编队编号规则计算自身编队编号N，计算方法为：

N＝A[i]，其中A＝[4 2 1 5 3 6]

其中i为本智能体在初始朝向上从左到右的排列顺序，即从左到右依次编号为4、2、1、5、3、6，其中1号定义为领航者，其他编号定义为跟随者：在各车车头方向最左侧的智能体设置自身编队编号为4，左侧第二位置的智能体设置自身编队编号为2，左侧第三位置的智能体设置自身编队编号为1，左侧第四位置的智能体设置自身编队编号为5，左侧第五位置的智能体设置自身编队编号为3，左侧第六位置的智能体设置自身编队编号为6。

S3：当智能体计算结果为自身是领航者时，领航者根据自身目标路径独立导航，并开始广播自身位姿；其它跟随者以收到领航者广播信息为时间基准，按照编队编号大小依次发送自身位姿信息给领航者；跟随者根据领航者位姿和编队编号进行实时目标位置计算并控制自身前往该目标点。

S31：当智能体计算结果为自身的编队编号为1，是领航者时，领航者根据自身目标路径独立导航，并开始广播自身位姿。

S32：其它跟随者以收到领航者广播信息为时间基准，根据编队编号从小到大并间隔固定时间段，依次发送自身位姿信息给领航者：如当跟随者编队编号为2时，则在收到领航者广播数据后立即广播自身位姿信息，跟随者编队编号为3时，则在收到领航者广播数据后延迟时间Δt再广播数据。

S33：跟随者实时目标位置计算过程如下：以领航者在世界坐标系中的位置[x

具体地，在横向一字编队中的目标点矩阵公式如下：

其中/>

在纵向一字编队中的目标点矩阵公式如下：

其中θ

在三角编队中的目标点矩阵公式如下：

其中

其中，d为编队间距，θ

以三角队形为例，假设当前领航者智能体在世界坐标系中的坐标为[12.7 33.4]，队形间距为2m，队形夹角为左侧θ

举例说明，当领航者智能体分配给该跟随者智能体的编队编号为4，则该跟随者智能体当前的目标点坐标为[x

参阅图3，参照前述六车编队中快速队形变换的自主编号方法，当从三角队形变换为横向一字队形时，领航者智能体保持原行进状态不变，各跟随者智能体根据横向一字编队中的目标点矩阵公式计算生成的队形矩阵表，以及当前领航者智能体的位置坐标，获得当前时刻在变换为横向一字形后的目标点位置，根据图3可知，各跟随者智能体在队形变换过程中，运行轨迹没有交叉，不会产生碰撞，且各跟随者智能体到达新队形的轨迹为最优。三角队形变换为横向一字队形仿真图如图4a、图4b所示。

参阅图5，参照前述六车编队中快速队形变换的自主编号方法，当从横向一字队形变换为纵向一字队形时，领航者智能体保持原行进状态不变，各跟随者智能体根据纵向一字编队中的目标点矩阵公式计算生成的队形矩阵表，以及当前领航者智能体的位置坐标，获得当前时刻在变换为纵向一字形后的目标点，根据图5可知，各跟随者智能体在队形变换过程中，运行轨迹没有交叉，不会产生碰撞，且各跟随者智能体到达新队形的轨迹为最优。横向一字队形变换为纵向一字队形仿真图如图6a、图6b所示。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。