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

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

背景技术：

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

技术实现思路

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

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

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

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

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

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

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

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

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

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

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

12、各智能体在收到所有其它智能体的位姿后，根据朝向计算出所有智能体从左到右的位置顺序：每个智能体将接收到的其它5个智能体的位姿信息转换为自身坐标系里的坐标(xi,yi),i＝1，2，…，5，坐标系以该智能体位置作为坐标系原点，其前进方向为该坐标系的y轴，前进方向的右侧为该坐标系的x轴，依次计算坐标(xi,yi)相对自身坐标系x轴的夹角若-90°≤α≤90°，说明坐标(xi,yi)代表的智能体位于当前智能体的右边，计算出位于当前智能体右边的智能体总数目n，依据每个智能体右边的智能体总数目，得到所有智能体从左到右排列的顺序；

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

14、n＝a[i]，其中a＝[4 2 1 5 3 6]

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

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

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

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

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

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

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

22、以领航者在世界坐标系中的位置[xm ym]为基准，基于队形矩阵a，计算各跟随者在世界坐标系中的位置：

23、

24、式中，[xsi ysi]，i＝2，3，…，6，为编队编号为i的跟随者在世界坐标系中的实时目标位置。

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

26、其中

27、式中，d为编队间距，θl为领航者朝向左侧跟随者的矢量角度，θr为领航者朝向右侧跟随者的矢量角度。

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

29、其中θd＝1.5π

30、式中，d为编队间距，θd为领航者朝向正后方跟随者的矢量角度。

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

32、其中

33、式中，d为编队间距，θl为领航者朝向左侧跟随者的矢量角度，θr为领航者朝向右侧跟随者的矢量角度，θd为领航者朝向正后方跟随者的矢量角度。

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

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

技术特征：

1.一种六车编队中快速队形变换的自主编号方法，其特征在于，包括如下步骤：

2.根据权利要求1所述的一种六车编队中快速队形变换的自主编号方法，其特征在于，所述所有参与编队的智能体在触发编队后，以触发时刻为基准时间，按照自身id号大小，计算广播信息时刻并在该时刻广播自身位姿数据给所有其它智能体的步骤包括：

3.根据权利要求1所述的一种六车编队中快速队形变换的自主编号方法，其特征在于，所述固定时间段为30ms。

4.根据权利要求1所述的一种六车编队中快速队形变换的自主编号方法，其特征在于，每个智能体在获取到所有其它智能体位姿后，结合自身位姿，计算从左到右的排列位置，基于统一编队编号规则计算自身编队编号，并定义领航者和跟随者的步骤包括：

5.根据权利要求4所述的一种六车编队中快速队形变换的自主编号方法，其特征在于，当智能体计算结果为自身是领航者时，领航者根据自身目标路径独立导航，并开始广播自身位姿的步骤包括：

6.根据权利要求5所述的一种六车编队中快速队形变换的自主编号方法，其特征在于，其它跟随者以收到领航者广播信息为时间基准，按照编队编号大小依次发送自身位姿信息给领航者的步骤包括：

7.根据权利要求6所述的一种六车编队中快速队形变换的自主编号方法，其特征在于，所述跟随者根据领航者位姿和编队编号进行实时目标位置计算的步骤包括：

8.根据权利要求7所述的一种六车编队中快速队形变换的自主编号方法，其特征在于，当队形为横向一字编队时，队形矩阵a表达式为：

9.根据权利要求7所述的一种六车编队中快速队形变换的自主编号方法，其特征在于，当队形为纵向一字编队时，队形矩阵a表达式为：

10.根据权利要求7所述的一种六车编队中快速队形变换的自主编号方法，其特征在于，当队形为三角编队时，队形矩阵a表达式为：

技术总结
本发明公开了一种六车编队中快速队形变换的自主编号方法，包括如下步骤：所有参与编队的智能体在触发编队后，以触发时刻为基准时间，按照自身ID号大小，计算广播信息时刻并在该时刻广播自身位姿数据给所有其它智能体；每个智能体在获取到所有其它智能体位姿后，结合自身位姿，计算从左到右的排列位置，基于统一编队编号规则计算自身编队编号；当智能体计算结果为自身是领航者时，领航者根据自身目标路径独立导航，并开始广播自身位姿；其它跟随者以收到领航者广播信息为时间基准，按照编队编号大小依次发送自身位姿信息给领航者；跟随者根据领航者位姿和编队编号进行实时目标位置计算并控制自身前往该目标点。

技术研发人员：王智,许站魁
受保护的技术使用者：南京信息职业技术学院
技术研发日：
技术公布日：2024/1/11