一种模块化飞行器的在线构型识别及控制分配方法

1.本发明涉及一种模块化飞行器的在线构型识别及控制分配方法，属于模块化飞行器领域。


背景技术：

2.随着科技技术的发展，我国无人机技术得到极大的提升，广泛应用在测绘、航拍、农业植保、医疗运输以及电力巡检等领域。在不同领域中，对于无人机的飞行速度、飞行高度、运行时间性能需求也有很大的差异。无人机按照执行器的不同，可分为固定翼无人机、旋翼无人机和扑翼无人机等。固定翼无人机的靠流过机翼的气流提供升力，具有飞行时间长、速度快、载重能力强等优点。然而，固定翼需要气流持续不断地流过机翼的下方，因此不能进行悬停操作。旋翼无人机通过多个旋翼共同产生升力，实现无人机的运动控制，其优点是可折叠、垂直起降、可悬停、对场地要求低。
3.然而，传统无人机难以在面临复杂环境完成相应的任务，由于传统无人机结构是固定，难以在狭窄的环境中进行作业，且机动性差等缺点。模块化飞行器凭借着可重构、强鲁棒性及具有容错性等特点，用于执行不同地形、不同环境的任务。因此，对于模块化飞行器的在线构型识别及控制分配的研究具有重要的意义。
4.文献“李伟昌,黄尚樱,梁梓熙.一种模块化机器人运动链构型识别方法及系统[p].广东省：cn113920322a,2022-01-11.”为了实现模块化机器人构型识别，基于图像处理技术提出一种模块化机器人运动链构型识别方法及系统。通过获取模块化机器人图像，将输入图像输入到预设的目标检测模型进行多模块识别，得到模块中心点位置、模块种类和模块包围框尺寸，并对其包围框尺寸进行处理，以确定模块连接关系，完成运动链构型识别。然而，该方法需要基于图像传感器在第三视角进行拍摄图像，从而模块化机器人的连接形状，存在适用性差。同时，当多个模块之间的距离与交并比都差异较小时，会发现中心点距离远大于链表尾模块包围框尺寸等异常行为。


技术实现要素：

[0005]
本发明提供了一种模块化飞行器的在线构型识别及控制分配方法，一方面，通过各飞行单元模块之间的相互通信，将其各飞行单元模块的连接关系向量共享，进而可以形成一个构型矩阵；因此，可以通过该构型矩阵用于表示当前模块化飞行器的构型。另一方面，采用广度优先遍历算法对构型矩阵进行遍历，得到各飞行单元模块相对起始模块坐标以及飞行器构型的中心相对起始模块坐标，在模块化飞行器机体坐标系下，在线计算模块化飞行器的控制分配矩阵，可以得到每个执行电机的控制量。
[0006]
本发明的技术方案是：
[0007]
根据本发明的一方面，提供了一种模块化飞行器的在线构型识别方法，包括：获取飞行单元模块1对应物理地址的id；建立模块化飞行器构型矩阵表达式；模块化飞行器中各飞行单元模块1广播自身的id到其他的飞行单元模块1；各个飞行单元模块1依据统计出的
不同id的数量，以确定模块化飞行器中飞行单元模块1的数量n；获取各飞行单元模块1的各连接面所接收到相邻飞行单元模块的id；
[0008]
依据各飞行单元模块1的各连接面所接收到相邻飞行单元模块的id，确定模块化飞行器的构型矩阵。
[0009]
所述模块化飞行器构型矩阵表达式为：
[0010][0011]
式中，表示具有n个飞行单元模块1的构型矩阵，表示构型矩阵的维度为n
×
p；表示第i个飞行单元模块的连接关系向量，i表示飞行单元模块的id，j表示飞行单元模块的第j个连接面；a
ij
表示具有顺逆桨信息的第i个飞行单元模块的第j个连接面所连接的相邻飞行单元模块的id。
[0012]
所述依据各飞行单元模块1的各连接面所接收到相邻飞行单元模块的id，确定模块化飞行器的构型矩阵，包括：
[0013]
步骤5.1：初始化第i个飞行单元模块所对应的相邻的连接关系中间变量令j＝1；执行步骤5.2；其中，p表示单个飞行单元模块连接面的总数；
[0014]
步骤5.2：判断各飞行单元模块1连接面sj的红外模块4是否被触发，如果第i个飞行单元模块1连接面sj的红外模块4被触发，则执行步骤5.3；否则，执行步骤5.6；
[0015]
步骤5.3：通过连接面sj的串口通信发送“请发送您的id信息”命令，并判断连接面sj的串口通信是否接收到相邻单元模块的id信息；如果接收到对方的id信息，则执行步骤5.4；否则，继续执行步骤5.3；
[0016]
步骤5.4：依据所接收到的第i个飞行单元模块的第j个连接面sj所连接的相邻飞行单元模块的id，获得具有顺逆桨信息的第i个飞行单元模块的第j个连接面所连接的相邻飞行单元模块的id并存储在对应的连接关系中间变量qi的第j位；执行步骤5.5；
[0017]
步骤5.5：计算j＝j+1，若j小于p+1，则执行步骤5.2；否则，执行步骤5.6；
[0018]
步骤5.6：将飞行单元模块的自身id保存在连接关系中间变量qi的最后一位，将连接关系中间变量qi共享给其他飞行单元模块；
[0019]
步骤5.7：各飞行单元模块1判断接收其他飞行单元模块连接关系中间变量的数量nr，若nr+1＝n，按照连接关系中间变量最后一位数据的大小，将连接关系中间变量的前p位元素排列在构型矩阵中，形成完整的构型矩阵；否则，继续执行步骤5.1；其中，n表示模块化飞行器中飞行单元模块1的总数。
[0020]
所述具有顺逆桨信息的第i个飞行单元模块的第j个连接面所连接的相邻飞行单元模块的id，表达式为：
[0021][0022]
其中，a
ij
表示具有顺逆桨信息的第i个飞行单元模块1的第j个连接面所连接的相邻飞行单元模块的id；id
ij
表示第i个飞行单元模块1的第j个连接面所连接的相邻飞行单元模块的id；ξ
ij
表示第i个飞行单元模块的连接面sj的连接状态：若ξ
ij
＝0，则第i个飞行单元模块1的连接面sj未连接相邻的飞行单元模块；否则，若ξ
ij
＝1，则第i个飞行单元模块1的连接面sj连接有相邻的飞行单元模块；表示第i个飞行单元模块的螺旋桨旋转方向，表示第i个飞行单元模块1的螺旋桨为逆时针，表示第i个飞行单元模块1的螺旋桨为顺时针。
[0023]
对于红外模块，会设定一个最小触发距离d
min
，当红外模块4检测到相邻飞行单元模块的距离d《d
min
时，红外模块被触发，产生一个高电平信号，则相对应的ξ
ij
＝1，且表示红外模块4所在的连接面连接有相邻飞行单元模块1。
[0024]
根据本发明的另一方面，提供了一种模块化飞行器的在线构型识别装置，包括：第一获取模块，用于获取飞行单元模块1对应物理地址的id；建立模块，用于建立模块化飞行器构型矩阵表达式；第一确定模块，用于模块化飞行器中各飞行单元模块1广播自身的id到其他的飞行单元模块1；各个飞行单元模块1依据统计出的不同id的数量，以确定模块化飞行器中飞行单元模块1的数量n；第二获取模块，用于获取各飞行单元模块1的各连接面所接收到相邻飞行单元模块的id；第二确定模块，用于依据各飞行单元模块1的各连接面所接收到相邻飞行单元模块的id，确定模块化飞行器的构型矩阵。
[0025]
根据本发明的另一方面，提供了一种模块化飞行器的控制分配方法，包括：基于构型矩阵，以当前构型中id编号最小的飞行单元模块1为起始模块，在起始模块坐标系下递推各飞行单元模块1相对于起始模块的坐标；根据几何中心计算公式，获得在起始模块坐标系下当前构型几何中心相对起始模块的坐标；依据飞行单元模块1相对于起始模块的坐标、当前构型几何中心相对起始模块的坐标，获得每个飞行单元模块相对机体坐标系的坐标；根据机体坐标系中飞行单元模块1的位置，构建模块化飞行器的控制效率矩阵，通过求取控制效率矩阵的伪逆，将机体坐标系中的控制输入量分配给飞行单元模块中的电机，至此实现模块化飞行器的在线控制分配。
[0026]
所述在起始模块坐标系下递推飞行单元模块1相对于起始模块的相对坐标，包括：
[0027]
步骤6.1：输入构型矩阵cmn，连接面的长度l，初始化构型矩阵中连接关系向量mi的访问标志位vflag＝0，缓存队列que为空；
[0028]
步骤6.2：以物理地址id最小的飞行单元模块1作为起始模块startnode，且起始模块的中心坐标为(0,0)；把起始模块的id放入缓存队列que的队列尾部，更新访问标志vflag，并标记已访问模块vflag(startnode)＝1；
[0029]
步骤6.3：依据访问标志位判断所有飞行单元模块是否全部被访问，如果飞行单元模块未被全部访问，则执行步骤6.4，否则执行步骤6.6；
[0030]
步骤6.4：取出缓存队列中que的队首元素，执行步骤6.5；
[0031]
步骤6.5：针对队首元素对应的第i飞行单元模块，以s1,s2,s3,...,sp的顺序搜索
相邻飞行单元模块，并判断搜索到的相邻飞行单元模块是否被访问过，如果当前连接面相邻飞行单元模块被访问过，则继续搜索下一个连接面；否则根据相邻飞行单元模块的位置关系，计算出相邻飞行单元模块相对起始模块位置坐标，更新访问标志vflag，将计算了相对起始模块位置坐标的相邻飞行单元模块标记为已访问节点，即vflag(id)＝1，把搜索到的相邻飞行单元模块id放入缓存队列尾部；直至所有连接面全部搜索完成，执行步骤6.3。
[0032]
所述计算出相邻飞行单元模块相对起始模块位置坐标有以下几种状态：
[0033]
当相邻飞行单元模块连接在第i个飞行单元模块的连接面s1或s4时，其相邻飞行单元模块相对起始模块的坐标为：
[0034][0035]
当相邻飞行单元模块连接在第i个飞行单元模块的连接面s2或s5时，其相邻飞行单元模块相对起始模块的坐标为：
[0036][0037]
当相邻飞行单元模块连接在第i个飞行单元模块的连接面s3或s6时，其相邻飞行单元模块相对起始模块的坐标为：
[0038][0039]
通过第i个飞行单元模块连接面的编号nf与相邻飞行单元模块的连接面的编号nr之间的关系，获得相邻飞行单元模块与起始模块间夹角，表示为：
[0040][0041]
式中，i表示第i个飞行单元模块的物理地址id，j表示第i个飞行单元模块连接面的编号；ψi为第i个飞行单元模块与起始模块之间的夹角，夹角的大小取决于两个飞行单元模块连接的相对关系；分别表示第i个飞行单元模块相对于起始模块的坐标。
[0042]
根据本发明的另一方面，提供了一种模块化飞行器的控制分配系统，包括：第三获得模块，用于基于构型矩阵，以当前构型中id编号最小的飞行单元模块1为起始模块，在起始模块坐标系下递推各飞行单元模块1相对于起始模块的坐标；根据几何中心计算公式，获得在起始模块坐标系下当前构型几何中心相对起始模块的坐标；依据飞行单元模块1相对于起始模块的坐标、当前构型几何中心相对起始模块的坐标，获得每个飞行单元模块相对机体坐标系的坐标；分配模块，用于根据机体坐标系中飞行单元模块1的位置，构建模块化
飞行器的控制效率矩阵，通过求取控制效率矩阵的伪逆，将机体坐标系中的控制输入量分配给飞行单元模块中的电机，至此实现模块化飞行器的在线控制分配。
[0043]
本发明的有益效果是：
[0044]
1、本发明仅需要计算各飞行单元模块中心相对于飞行器构型几何中心的位置，飞行器控制系统可以识别当前的构型。因此，本发明不需要构建飞行器构型的数据库，进一步减少了控制系统的存储空间。
[0045]
2、与现有基于图像的构型识别技术相比，本发明仅需要串口通信和无wifi模块通信，模块化飞行器系统可以获得局部的连接关系和全局构型信息。同时，不需要额外增加图像传感器，减少了模块化飞行器的成本。
[0046]
3、根据飞行器构型矩阵，本发明可以构成一个控制分配矩阵。进一步，控制器可以在线计算飞行器的控制分配矩阵。
[0047]
4、本发明提出一种模块化飞行器模块构型表述方法，该方法与传统的邻接矩阵相比，不仅能够反应各飞行单元模块之间的连接关系，同时，反应了飞行器模块之间的相对位置和螺旋桨的顺逆信息。
附图说明
[0048]
图1为本发明一种模块化飞行器的在线构型识别及控制分配方法的步骤流程图；
[0049]
图2为本发明的飞行单元模块结构示意图；
[0050]
图3为本发明的飞行单元模块的坐标系示意图；
[0051]
图4为本发明的具体实施例模块化飞行器构型的示意结构图；
[0052]
图5为本发明的具体实施例三角构型示意框图；
[0053]
图6为本发明的具体实施例构型中模块位置信息获取流程图；
[0054]
图7为本发明的具体实施例matlab可视化仿真图；
[0055]
图8为本发明的基于在线控制分配的轨迹跟踪实验结果图；
[0056]
图标各标号：1-飞行单元模块、2-舵机、3-t型连接件、4-红外模块、5-uart通信接口、6-无线模块。
具体实施方式
[0057]
下面结合附图和实施例，对发明做进一步的说明，但本发明的内容并不限于所述范围。
[0058]
实施例1：如图1-8所示，根据本发明实施例的一方面，提供了一种模块化飞行器的在线构型识别方法，包括：获取飞行单元模块1对应物理地址的id；建立模块化飞行器构型矩阵表达式；模块化飞行器中各飞行单元模块1广播自身的id到其他的飞行单元模块1；各个飞行单元模块1依据统计出的不同id的数量，以确定模块化飞行器中飞行单元模块1的数量n；获取各飞行单元模块1的各连接面所接收到相邻飞行单元模块的id；依据各飞行单元模块1的各连接面所接收到相邻飞行单元模块的id，确定模块化飞行器的构型矩阵。
[0059]
根据本发明的另一方面，提供了一种模块化飞行器的在线构型识别装置，包括：第一获取模块，用于获取飞行单元模块1对应物理地址的id；建立模块，用于建立模块化飞行器构型矩阵表达式；第一确定模块，用于模块化飞行器中各飞行单元模块1广播自身的id到
其他的飞行单元模块1；各个飞行单元模块1依据统计出的不同id的数量，以确定模块化飞行器中飞行单元模块1的数量n；第二获取模块，用于获取各飞行单元模块1的各连接面所接收到相邻飞行单元模块的id；第二确定模块，用于依据各飞行单元模块1的各连接面所接收到相邻飞行单元模块的id，确定模块化飞行器的构型矩阵。
[0060]
根据本发明的另一方面，提供了一种模块化飞行器的控制分配方法，包括：基于构型矩阵，以当前构型中id编号最小的飞行单元模块1为起始模块，在起始模块坐标系下递推各飞行单元模块1相对于起始模块的坐标；根据几何中心计算公式，获得在起始模块坐标系下当前构型几何中心相对起始模块的坐标；依据飞行单元模块1相对于起始模块的坐标、当前构型几何中心相对起始模块的坐标，获得每个飞行单元模块相对机体坐标系的坐标；根据机体坐标系中飞行单元模块1的位置，构建模块化飞行器的控制效率矩阵，通过求取控制效率矩阵的伪逆，将机体坐标系中的控制输入量分配给飞行单元模块中的电机，至此实现模块化飞行器的在线控制分配。
[0061]
根据本发明的另一方面，提供了一种模块化飞行器的控制分配系统，包括：第三获得模块，用于基于构型矩阵，以当前构型中id编号最小的飞行单元模块1为起始模块，在起始模块坐标系下递推各飞行单元模块1相对于起始模块的坐标；根据几何中心计算公式，获得在起始模块坐标系下当前构型几何中心相对起始模块的坐标；依据飞行单元模块1相对于起始模块的坐标、当前构型几何中心相对起始模块的坐标，获得每个飞行单元模块相对机体坐标系的坐标；分配模块，用于根据机体坐标系中飞行单元模块1的位置，构建模块化飞行器的控制效率矩阵，通过求取控制效率矩阵的伪逆，将机体坐标系中的控制输入量分配给飞行单元模块中的电机，至此实现模块化飞行器的在线控制分配。
[0062]
图1是本发明一种模块化飞行器的在线构型识别方法的步骤流程图。
[0063]
图2和4所示，本发明的一种模块化飞行器，由多个飞行单元模块1共同组成，其飞行单元模块1包括主面板、侧面板、舵机2、t型连接件3、红外模块4、uart通信接口5以及无线模块6；主面板正中间设有固定直流电机的直流电机固定件，且直流电机固定件与主面板连接，直流电机轴端安装有螺旋桨；侧面板合围在主面板外周，各飞行单元模块1通过侧板板作为连接面进行连接，且每个连接面设有对称布置的插口、卡口；所述舵机2固定在飞行单元模块1每一个连接面的内侧，所述t型连接件3固定在所述舵机2的输出端，通过舵机2输出的扭矩带动t型连接件3旋转，实现从一个飞行单元模块1插口穿出进入另一个飞行单元模块1的卡口，通过t型连接件3与卡口配合，将两个飞行单元模块连接；所述红外模块4和uart通信接口5均安装在飞行单元模块1内每一个连接面内侧，且红外模块4位于uart通信接口5的正上方；所述无线模块6固定在飞行单元模块1连接面。
[0064]
如图3所示，以飞行单元模块1的几何中心om建立模块坐标系xmomym，几何中心垂直于其连接面s1的垂线为ym轴，以ym轴顺时针旋转90
°
为xm轴；对飞行单元模块1的六个连接面以顺时针编号为s1,s2,s3,...,s6。
[0065]
如图8所示，基于在线控制分配的轨迹跟踪实验结果，设置pid的比例增益系数k
p
＝12.5，微分增益系数kd＝5，积分增益系数ki＝0.5，在模块化飞行器悬停状态下，设定横滚角、俯仰角和偏航角的期望值为0
°
，通过所提出的在线分配可以实现模块化飞行器的轨迹跟踪控制。
[0066]
可选地，一种模块化飞行器的在线构型识别及控制分配方法，所述方法具体步骤
如下：
[0067]
步骤1：获取飞行单元模块1对应物理地址的id；然后，对飞行单元模块1的六个连接面以顺时针编号为s1,s2,s3,...,s6，以飞行单元模块1的几何中心建立单模块坐标系xmomym，几何中心垂直于其连接面s1的垂线为ym轴，以ym轴顺时针旋转90
°
为xm轴；其中，模块化飞行器中飞行单元模块的物理地址id唯一，id可以为1,2,...n，也可以采用其它方式编号；
[0068]
步骤2：采用一种新的模块化飞行器构型矩阵，描述构型中飞行单元模块1之间的连接关系，其构型矩阵具体可以：
[0069][0070]
式中，表示具有n个飞行单元模块的构型矩阵，表示构型矩阵的维度为n
×
p；表示第i个飞行单元模块的连接关系向量，i表示飞行单元模块的id，j表示飞行单元模块的第j个连接面；a
ij
表示具有顺逆桨信息的第i个飞行单元模块的第j个连接面所连接的相邻飞行单元模块的id；
[0071][0072]
其中，id
ij
表示第i个飞行单元模块的第j个连接面所连接的相邻飞行单元模块的id，ξ
ij
表示第i个飞行单元模块的连接面sj的连接状态：若ξ
ij
＝0，则第i个飞行单元模块1的连接面sj未连接相邻的飞行单元模块；否则，若ξ
ij
＝1，则第i个飞行单元模块1的连接面sj连接有相邻的飞行单元模块；表示第i个飞行单元模块的螺旋桨旋转方向，表示第i个飞行单元模块1的螺旋桨为逆时针，表示第i个飞行单元模块1的螺旋桨为顺时针；
[0073]
步骤3：通过舵机2带动t型连接件3，将所述多个飞行单元模块1连接在一起，构成一个模块化飞行器；
[0074]
步骤4：若飞行单元模块1的一个或多个红外模块4被触发，通过无线通信块6建立飞行单元模块1之间的全局通信，飞行单元模块发送自身的id到其他的飞行单元模块1，各个飞行单元模块统计出不同id的数量，以确定模块化飞行器中飞行单元模块1的数量n；需要说明的是，通信应当确保发送的信息能够被确定接收，即上述各个飞行单元模块统计出不同id的数量值为相同；各个飞行单元模块统计出不同id的数量包括自身id；
[0075]
步骤5：采用uart通信接口5建立飞行器的各相邻飞行单元模块1之间的点对点串口通信，识别飞行单元模块1与相邻飞行单元模块之间的连接关系中间变量，并通过无线模块6将飞行单元模块1自身的相邻连接关系中间变量发送其他的飞行单元模块；当相邻关系向量的个数等于构型中模块的数量时，以模块id的顺序将相邻连接关系中间变量组成一个
模块化飞行器的构型矩阵，其具体过程如下：
[0076]
步骤5.1：初始化第i个飞行单元模块所对应的相邻的连接关系中间变量令j＝1；执行步骤5.2；p表示单个飞行单元模块连接面的总数，在本发明的实施例中，各飞行单元模块连接面总数相同；此处以六个连接面为例建立的连接关系中间变量
[0077]
步骤5.2：判断各飞行单元模块1连接面sj的红外模块4是否被触发，如果第i个飞行单元模块1连接面sj的红外模块4被触发，则执行步骤5.3；否则，执行步骤5.6；需要说明的是，对于红外模块，会设定一个最小触发距离d
min
，当红外模块4检测到相邻飞行单元模块的距离d《d
min
时，红外模块被触发，产生一个高电平信号，则相对应的ξ
ij
＝1，且表示红外模块4所在的连接面连接有相邻飞行单元模块1；
[0078]
步骤5.3：通过连接面sj的串口通信发送“请发送您的id信息”命令，并判断连接面sj的串口通信是否接收到相邻单元模块的id信息；如果接收到对方的id信息，则执行步骤5.4；否则，继续执行步骤5.3；
[0079]
步骤5.4：依据所接收到的第i个飞行单元模块的第j个连接面sj所连接的相邻飞行单元模块的id，获得具有顺逆桨信息的第i个飞行单元模块的第j个连接面所连接的相邻飞行单元模块的id并存储在对应的连接关系中间变量qi的第j位；执行步骤5.5；
[0080]
步骤5.5：计算j＝j+1，若j小于p+1，则执行步骤5.2；否则，执行步骤5.6；
[0081]
步骤5.6：将飞行单元模块的自身id保存在连接关系中间变量qi的最后一位，并使用无线模块6将连接关系中间变量qi共享给其他飞行单元模块；
[0082]
步骤5.7：各飞行单元模块1判断通过无线模块接收其他飞行单元模块连接关系中间变量的数量nr，若nr+1＝n，按照连接关系中间变量最后一位数据的大小，将连接关系中间变量的前p位元素排列在构型矩阵中，形成完整的构型矩阵，也即完成了模块化飞行器的构型识别；如下为图5所示构型的构型矩阵：
[0083][0084]
否则，继续执行步骤5.1。
[0085]
步骤6：基于飞行单元模块1间的位置关系，以当前构型中id编号最小的飞行单元模块为起始模块，在起始模块坐标系下利用广度优先遍历算法递推其他飞行单元模块1相对于起始模块的相对坐标；根据几何中心计算公式，获得在起始模块坐标系下当前构型几何中心相对起始模块的坐标，如图6所示，其具体过程如下：
[0086]
步骤6.1：输入构型矩阵cmn，连接面的长度l＝0.145m，构型中模块的数量n＝6，初始化构型矩阵中mi的访问标志位vflag＝0，缓存队列que为空；
[0087]
步骤6.2：以物理地址id最小的飞行单元模块作为起始模块，且起始模块的中心坐
标为(0,0)；把起始模块的id放入缓存队列que的队列尾部，更新访问标志vflag，并标记已访问模块vflag(startnode)＝1；
[0088]
步骤6.3：依据访问标志位判断所有飞行单元模块是否全部被访问，如果飞行单元模块未被全部访问，则执行步骤6.4，否则执行步骤6.6；
[0089]
步骤6.4：取出缓存队列中que的队首元素，执行步骤6.5；
[0090]
步骤6.5：针对队首元素对应的第i飞行单元模块，以s1,s2,s3,...,s6的顺序搜索相邻飞行单元模块的中心坐标，并判断搜索到的相邻飞行单元模块是否被访问过，如果当前连接面相邻飞行单元模块被访问过，则继续搜索下一个连接面；否则根据相邻飞行单元模块的位置关系，计算出相邻飞行单元模块相对起始模块位置坐标，更新访问标志vflag，将计算了相对起始模块位置坐标相邻飞行单元模块标记为已访问节点，即vflag(id)＝1，把搜索到的相邻飞行单元模块id放入缓存队列尾部；直至所有连接面全部搜索完成，执行步骤6.3；
[0091]
计算出相邻飞行单元模块相对起始模块位置坐标有以下几种状态：
[0092]
当相邻飞行单元模块连接在第i个飞行单元模块的连接面s1或s4时，其相邻飞行单元模块相对起始模块的坐标为：
[0093][0094]
上式中，如果是针对连接面s1，则j取1；否则如果针对s4，则j取4；
[0095]
当相邻飞行单元模块连接在第i个飞行单元模块的连接面s2或s5时，其相邻飞行单元模块相对起始模块的坐标为：
[0096][0097]
当相邻飞行单元模块连接在第i个飞行单元模块的连接面s3或s6时，其相邻飞行单元模块相对起始模块的坐标为：
[0098][0099]
式中，i表示第i个飞行单元模块的物理地址id，j表示第i个飞行单元模块连接面的编号；ψi为第i个飞行单元模块与起始模块之间的夹角，夹角的大小取决于两个飞行单元模块连接的相对关系；分别表示第i个飞行单元模块相对于起始模块的坐标；需要说明的是，起始模块相对于起始模块的坐标为(0,0)(以六个飞行单元模块构型为例，其id依次为1,2,...,6；当以第1个飞行单元模块作为起始模块，则起始模块与起始模块之间的夹角为0度；
[0100]
通过第i个飞行单元模块连接面的编号nf与相邻飞行单元模块的连接面的编号nr之间的关系，获得相邻飞行单元模块与起始模块间夹角，表示为：
[0101][0102]
步骤6.6：根据模块化飞行器的位置几何关系，计算出各飞行单元模块相对几何中心的坐标位置，完成模块化飞行器构型信息获取，如表1所示；假设飞行单元模块1是均质的，且飞行器的质心和几何中心重合，因此在机体坐标系与几何中心坐标系xroryr相同，坐标系的方向和起始模块的坐标系一致，并将其他模块坐标位置映射到机体系中，飞行单元模块相对于构型几何中心的坐标如表2所示；
[0103]
设第i个飞行单元模块相对起始模块的位置坐标为在起始模块坐标系下，构型的几何中心坐标为表达式为：
[0104][0105]
在几何中心坐标系中，坐标的方向和起始模块的坐标方向一致，因此，每个飞行单元模块相对几何中心的坐标为：
[0106][0107]
式中，和表示第i个飞行单元模块在几何中心坐标系中位置。
[0108]
表1基于广度优先遍历算法中心位置搜索的仿真结果
[0109]
飞行单元模块相对起始模块坐标(单位：m)角度(单位：度)m1(0,0)0m2(0.2175,-0.1256)0m3(0,-0.2511)-120m4(-0.2175,0.1256)-180m5(-0.2175,-0.1256)120m6(-0.2175,-0.3767)0
[0110]
表2单元模块相对于机体坐标系的仿真结果
[0111]
飞行单元模块相对机体坐标系下坐标(单位：m)m1(0.073,0.126)m2(0.290,0)m3(0.073,-0.126)m4(-0.145,0.251)m5(-0.145,0)m6(-0.145,-0.251)
[0112]
步骤7：根据机体坐标系中飞行单元模块1的位置，构建模块化飞行器的控制效率矩阵，通过求取控制效率矩阵的伪逆，将机体坐标系中的控制输入量分配给飞行单元模块中的电机，至此实现模块化飞行器的在线控制分配。具体而言：
[0113]
针对模块化飞行器轨迹跟踪控制，设计一个pid控制器为：
[0114][0115]
式中，表示模块化飞行器的高度位置、横滚角、俯仰角和偏航角的跟踪误差，k
p
、kd和ki分别表示pid控制器的比例、微分、积分增益，dt为控制器的采样时间。
[0116]
根据机体坐标系中飞行单元模块1相对机体坐标系下的位置坐标，构建模块化飞行器的控制效率矩阵为：
[0117][0118]
通过在线求取控制效率矩阵的伪逆为：
[0119][0120]
其中b
t
(bb
t
)-1
表示模块化飞行的控制分配矩阵，可以将机体坐标系中的控制输入量分配给飞行单元模块中的电机，其控制飞行器电机控制量为进而实现模块化飞行器的在线控制分配。
[0121]
另外，针对控制效率矩阵给出如下：
[0122]
根据遍历出的各模块的坐标位置得到模块化飞行器的转速与力矩之间的关系为：
[0123][0124]
上式中，c
t
为螺旋桨的推力系数，c
τ
为螺旋桨的力矩系数，和是通过遍历算法
得到飞行单元模块mi在机体坐标系下的坐标，ωi表示第i个飞行单元模块电机的转速，并令u＝[f
t
,τ
x
,τy,τz]
t
,其力矩和转速的关系可以简化为：
[0125][0126]
其中，为模块化飞行器的控制效率矩阵；在上述的实施例中，将c
t
、c
τ
取值为1。
[0127]
通过求取矩阵b的伪逆，飞行单元模块1中电机的转速分配量为：
[0128][0129]
根据所述构型识别方法获得模块化飞行器的构型矩阵，在基于广度遍历算法获得飞行器单元模块的位置坐标，将其位置坐标带入伪逆b
t
(bb
t
)-1
中，实现模块化飞行器的控制分配。
[0130]
上面结合附图对本发明的具体实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

技术特征：
1.一种模块化飞行器的在线构型识别方法，其特征在于，包括：获取飞行单元模块(1)对应物理地址的id；建立模块化飞行器构型矩阵表达式；模块化飞行器中各飞行单元模块(1)广播自身的id到其他的飞行单元模块(1)；各个飞行单元模块(1)依据统计出的不同id的数量，以确定模块化飞行器中飞行单元模块(1)的数量n；获取各飞行单元模块(1)的各连接面所接收到相邻飞行单元模块的id；依据各飞行单元模块(1)的各连接面所接收到相邻飞行单元模块的id，确定模块化飞行器的构型矩阵。2.根据权利要求1所述的模块化飞行器的在线构型识别方法，其特征在于，所述模块化飞行器构型矩阵表达式为：式中，表示具有n个飞行单元模块(1)的构型矩阵，表示构型矩阵的维度为n
×
p；表示第i个飞行单元模块的连接关系向量，i表示飞行单元模块的id，j表示飞行单元模块的第j个连接面；a
ij
表示具有顺逆桨信息的第i个飞行单元模块的第j个连接面所连接的相邻飞行单元模块的id。3.根据权利要求1所述的模块化飞行器的在线构型识别方法，其特征在于，所述依据各飞行单元模块(1)的各连接面所接收到相邻飞行单元模块的id，确定模块化飞行器的构型矩阵，包括：步骤5.1：初始化第i个飞行单元模块所对应的相邻的连接关系中间变量令j＝1；执行步骤5.2；其中，p表示单个飞行单元模块连接面的总数；步骤5.2：判断各飞行单元模块(1)连接面s
j
的红外模块(4)是否被触发，如果第i个飞行单元模块(1)连接面s
j
的红外模块(4)被触发，则执行步骤5.3；否则，执行步骤5.6；步骤5.3：通过连接面s
j
的串口通信发送“请发送您的id信息”命令，并判断连接面s
j
的串口通信是否接收到相邻单元模块的id信息；如果接收到对方的id信息，则执行步骤5.4；否则，继续执行步骤5.3；步骤5.4：依据所接收到的第i个飞行单元模块的第j个连接面s
j
所连接的相邻飞行单元模块的id，获得具有顺逆桨信息的第i个飞行单元模块的第j个连接面所连接的相邻飞行单元模块的id并存储在对应的连接关系中间变量q
i
的第j位；执行步骤5.5；步骤5.5：计算j＝j+1，若j小于p+1，则执行步骤5.2；否则，执行步骤5.6；步骤5.6：将飞行单元模块的自身id保存在连接关系中间变量q
i
的最后一位，将连接关
系中间变量q
i
共享给其他飞行单元模块；步骤5.7：各飞行单元模块(1)判断接收其他飞行单元模块连接关系中间变量的数量n
r
，若n
r
+1＝n，按照连接关系中间变量最后一位数据的大小，将连接关系中间变量的前p位元素排列在构型矩阵中，形成完整的构型矩阵；否则，继续执行步骤5.1；其中，n表示模块化飞行器中飞行单元模块(1)的总数。4.根据权利要求3所述的模块化飞行器的在线构型识别方法，其特征在于，所述具有顺逆桨信息的第i个飞行单元模块的第j个连接面所连接的相邻飞行单元模块的id，表达式为：其中，a
ij
表示具有顺逆桨信息的第i个飞行单元模块(1)的第j个连接面所连接的相邻飞行单元模块的id；id
ij
表示第i个飞行单元模块(1)的第j个连接面所连接的相邻飞行单元模块的id；ξ
ij
表示第i个飞行单元模块的连接面s
j
的连接状态：若ξ
ij
＝0，则第i个飞行单元模块(1)的连接面s
j
未连接相邻的飞行单元模块；否则，若ξ
ij
＝1，则第i个飞行单元模块(1)的连接面s
j
连接有相邻的飞行单元模块；表示第i个飞行单元模块的螺旋桨旋转方向，表示第i个飞行单元模块(1)的螺旋桨为逆时针，表示第i个飞行单元模块(1)的螺旋桨为顺时针。5.根据权利要求4所述的模块化飞行器的在线构型识别方法，其特征在于，对于红外模块，会设定一个最小触发距离d
min
，当红外模块(4)检测到相邻飞行单元模块的距离d<d
min
时，红外模块被触发，产生一个高电平信号，则相对应的ξ
ij
＝1，且表示红外模块(4)所在的连接面连接有相邻飞行单元模块(1)。6.一种模块化飞行器的在线构型识别装置，其特征在于，包括：第一获取模块，用于获取飞行单元模块(1)对应物理地址的id；建立模块，用于建立模块化飞行器构型矩阵表达式；第一确定模块，用于模块化飞行器中各飞行单元模块(1)广播自身的id到其他的飞行单元模块(1)；各个飞行单元模块(1)依据统计出的不同id的数量，以确定模块化飞行器中飞行单元模块(1)的数量n；第二获取模块，用于获取各飞行单元模块(1)的各连接面所接收到相邻飞行单元模块的id；第二确定模块，用于依据各飞行单元模块(1)的各连接面所接收到相邻飞行单元模块的id，确定模块化飞行器的构型矩阵。7.一种模块化飞行器的控制分配方法，其特征在于，包括：基于构型矩阵，以当前构型中id编号最小的飞行单元模块(1)为起始模块，在起始模块坐标系下递推各飞行单元模块(1)相对于起始模块的坐标；根据几何中心计算公式，获得在起始模块坐标系下当前构型几何中心相对起始模块的坐标；依据飞行单元模块(1)相对于起始模块的坐标、当前构型几何中心相对起始模块的坐标，获得每个飞行单元模块相对机体坐标系的坐标；
根据机体坐标系中飞行单元模块(1)的位置，构建模块化飞行器的控制效率矩阵，通过求取控制效率矩阵的伪逆，将机体坐标系中的控制输入量分配给飞行单元模块中的电机，至此实现模块化飞行器的在线控制分配。8.根据权利要求7所述的模块化飞行器的控制分配方法，其特征在于，所述在起始模块坐标系下递推飞行单元模块(1)相对于起始模块的坐标，包括：步骤6.1：输入构型矩阵cm
n
，连接面的长度l，初始化构型矩阵中连接关系向量m
i
的访问标志位vflag＝0，缓存队列que为空；步骤6.2：以物理地址id最小的飞行单元模块(1)作为起始模块startnode，且起始模块的中心坐标为(0,0)；把起始模块的id放入缓存队列que的队列尾部，更新访问标志vflag，并标记已访问模块vflag(startnode)＝1；步骤6.3：依据访问标志位判断所有飞行单元模块是否全部被访问，如果飞行单元模块未被全部访问，则执行步骤6.4，否则执行步骤6.6；步骤6.4：取出缓存队列中que的队首元素，执行步骤6.5；步骤6.5：针对队首元素对应的第i飞行单元模块，以s1,s2,s3,...,sp的顺序搜索相邻飞行单元模块，并判断搜索到的相邻飞行单元模块是否被访问过，如果当前连接面相邻飞行单元模块被访问过，则继续搜索下一个连接面；否则根据相邻飞行单元模块的位置关系，计算出相邻飞行单元模块相对起始模块位置坐标，更新访问标志vflag，将计算了相对起始模块位置坐标的相邻飞行单元模块标记为已访问节点，即vflag(id)＝1，把搜索到的相邻飞行单元模块id放入缓存队列尾部；直至所有连接面全部搜索完成，执行步骤6.3。9.根据权利要求8所述的模块化飞行器的控制分配方法，其特征在于，所述计算出相邻飞行单元模块相对起始模块位置坐标有以下几种状态：当相邻飞行单元模块连接在第i个飞行单元模块的连接面s1或s4时，其相邻飞行单元模块相对起始模块的坐标为：当相邻飞行单元模块连接在第i个飞行单元模块的连接面s2或s5时，其相邻飞行单元模块相对起始模块的坐标为：当相邻飞行单元模块连接在第i个飞行单元模块的连接面s3或s6时，其相邻飞行单元模块相对起始模块的坐标为：
通过第i个飞行单元模块连接面的编号n
f
与相邻飞行单元模块的连接面的编号n
r
之间的关系，获得相邻飞行单元模块与起始模块间夹角，表示为：式中，i表示第i个飞行单元模块的物理地址id，j表示第i个飞行单元模块连接面的编号；ψ
i
为第i个飞行单元模块与起始模块之间的夹角，夹角的大小取决于两个飞行单元模块连接的相对关系；分别表示第i个飞行单元模块相对于起始模块的坐标。10.一种模块化飞行器的控制分配系统，其特征在于，包括：第三获得模块，用于基于构型矩阵，以当前构型中id编号最小的飞行单元模块(1)为起始模块，在起始模块坐标系下递推各飞行单元模块(1)相对于起始模块的坐标；根据几何中心计算公式，获得在起始模块坐标系下当前构型几何中心相对起始模块的坐标；依据飞行单元模块(1)相对于起始模块的坐标、当前构型几何中心相对起始模块的坐标，获得每个飞行单元模块相对机体坐标系的坐标；分配模块，用于根据机体坐标系中飞行单元模块(1)的位置，构建模块化飞行器的控制效率矩阵，通过求取控制效率矩阵的伪逆，将机体坐标系中的控制输入量分配给飞行单元模块中的电机，至此实现模块化飞行器的在线控制分配。

技术总结
本发明公开了一种模块化飞行器的在线构型识别及控制分配方法，属于模块化飞行器领域。本发明一方面，通过各飞行单元模块之间的相互通信，将其各飞行单元模块的连接关系向量共享，进而可以形成一个构型矩阵；因此，可以通过该构型矩阵用于表示当前模块化飞行器的构型。另一方面，采用广度优先遍历算法对构型矩阵进行遍历，得到各飞行单元模块相对起始模块坐标以及飞行器构型的中心相对起始模块坐标，在模块化飞行器机体坐标系下，在线计算模块化飞行器的控制分配矩阵，可以得到每个执行电机的控制量。的控制量。的控制量。


技术研发人员：那靖 张方方 杨春曦 张秀峰 李一鸣 彭勇 杨健全 高贯斌 黄英博
受保护的技术使用者：昆明理工大学
技术研发日：2023.04.10
技术公布日：2023/7/25