CN118092508A - 一种基于主动光源阵列的无人机多机自动协同装置及方法 - Google Patents

Abstract

本发明提出一种基于主动光源阵列的无人机多机自动协同装置及方法，所述协同方法中，无人机分为主机（1）和一个以上的从机（2），主机和从机均安装有主动光源阵列（12），从机安装有协同控制装置；所述主动光源阵列包括多个主动光源，主动光源用于发出表述无人机姿态信息数据的光信号，以及发出用于无人机定位的光信号；所述从机通过协同控制装置识读主动光源阵列光信号中的主机姿态信息和定位信息，通过跟踪算法控制无人机飞行，以实现多无人机的多机自动协同；本发明在白天和夜晚都有较为良好的协同效果，不依赖于无线电通讯。

Description

一种基于主动光源阵列的无人机多机自动协同装置及方法

技术领域

本发明涉及无人机技术领域，尤其是一种基于主动光源阵列的无人机多机自动协同装置及方法。

背景技术

随着科技的日新月异，无人机技术得到了飞速的发展，其应用领域不断扩展，应用场景也变得越来越丰富多样。无论是在室内、树林，还是在白天、夜晚，无人机的身影都可以见到。特别是在多机协同的应用上，无人机展现出了前所未有的潜力和价值。

多机协同是指多架无人机协同完成一项任务或目标，这种协同可以大大提高无人机的作业效率、准确度和安全性。目前，无人机多机协同的方法主要有两种。

一种是多无人机之间建立通信，利用无线通信模块进行信息的传递。无人机之间通过交换信息，实现编队、跟踪等协同操作。这种方法的优点在于通信速度快，能够实现实时协同。然而，无线通信在复杂环境下可能会受到干扰，如树林、高楼等障碍物会阻碍信号的传输，导致协同效果不佳。

另一种方法是通过视觉识别技术实现多机协同。这种方法中，从机会通过摄像头等传感器识别主机的当前姿态或特征，然后计算出主机的姿态信息，进而预测主机的轨迹，实现多机跟踪。这种方法的优点在于不依赖无线通信，可以在无线通信受限的环境下实现协同。然而，视觉识别技术在夜晚或光线暗的场景下识别效果较差，且识别出来的姿态数据相较于陀螺仪的数据较为不精确。

发明内容

本发明提出一种基于主动光源阵列的无人机多机自动协同装置及方法，在白天和夜晚都有较为良好的协同效果，不依赖于无线电通讯。

本发明采用以下技术方案。

一种基于主动光源阵列的无人机多机自动协同方法，所述协同方法中，无人机分为主机(1)和一个以上的从机(2)，主机和从机均安装有主动光源阵列(12)，从机安装有协同控制装置；所述主动光源阵列包括多个主动光源，主动光源用于发出表述无人机姿态信息数据的光信号，以及发出用于无人机定位的光信号；所述从机通过协同控制装置识读主动光源阵列光信号中的主机姿态信息和定位信息，通过跟踪算法控制无人机飞行，以实现多无人机的多机自动协同。

一种基于主动光源阵列的无人机多机自动协同装置，用于实施以上所述的基于主动光源阵列的无人机多机自动协同方法，所述主动光源阵列包括一个发出白光作为无人机定位特征的白色光主动光源，还包括三个发出彩光的彩色光主动光源，各主动光源的光信号为红、绿、蓝三色激光合成并使用遮光罩处理后的光信号，在多机自动协同时，彩色光主动光源与无人机的无人机姿态感应模块相连以通过陀螺仪获取该无人机姿态信息，根据无人机当前飞行状态不同的姿态信息旋转角度，合成不同颜色的光信号，为从机提供识别目标和主机飞行参数；

所述协同控制装置包括视觉传感模块(23)和解算控制模块(24)；所述视觉传感模块通过带红外测距模块的深度摄像头来获取图像信息，并通过红外测距模块获取图像中的深度信息，并将图像信息和深度信息传给解算控制模块。

所述解算控制模块为无人机处的板载计算机，板载计算机通过接收图像信息及深度信息来识别图像中的无人机位置，进一步识别无人机上的主动光源位置，并通过识别算法将光源颜色转换为无人机的姿态信息数据；

识别算法工作时，先对无人机的主动光源阵列进行识别，再识别主动光源阵列中白色光主动光源的位置，识别到白色光主动光源后，根据白色光主动光源在四个主动光源中的位置，标记出表示偏航角数据Yaw、滚动角数据Roll、俯仰角数据Pitch的三个彩色光主动光源，最后分别对表示Yaw、Roll、Pitch的三个彩色光主动光源光源颜色进行R、G、B值的提取，并将R、G、B解算成对应旋转角度，

所述板载计算机存有根据姿态信息和深度信息控制无人机从机跟踪的算法，该算法通过旋转角度和深度信息计算出无人机跟踪前方主机所要的移动速度和角度，并控制无人机进行相应的飞行。

所述主动光源阵列装置包括四个相同结构的主动光源合成装置和供电控制装置；每个主动光源均含一个主动光源合成装置(121)和一个供电控制装置(122)；

主动光源合成装置包括底座(1210)和设于底座处的彩光发光器件；彩光发光器件的输出光路处顺序设置激光合成模块、激光调整模块；

所述激光合成模块位于发光器件的光输出端上方，包括顺序设置的合光棱镜(1214)、第一平凸镜(1215)，第一平凸镜的平面朝向合光棱镜；

彩光发光器件包括分别位于合光棱镜三面处的红色发光器件(1211)、蓝色发光器件(1213)、绿色发光器件(1212)；红色发光器件、蓝色发光器件、绿色发光器件发出的三色激光经激光合成模块合为一个合成激光束从激光合成模块的光输出端出射；

所述激光调整模块的光输入端与激光合成模块的光输出端相通；激光调整模块包括沿光路顺序设置的第二平凸镜(1216)、反射镜(1217)、第三平凸镜(1218)、遮光罩(1219)；所述第二平凸镜的凸面朝向合光棱镜以把合成激光调整为固定大小；所述反射镜位于第二平凸镜后用呈45°摆放，用于将合成激光向上反射；第三平凸镜、遮光罩位于反射镜上方，第三平凸镜截取并汇聚的激光光束在遮光罩处形成可识别的光源；

供电控制装置的功率控制模块包括第一控制器(1221)、第二控制器(1222)、第三控制器(1223)、第四控制器(1224)；

所述无人机姿态感应模块(1225)经线缆与主控模块(1226)相连；

所述合光棱镜设于底座中部的挖槽处；

所述无人机姿态感应模块通过陀螺仪获取无人机的姿态数据；

所述红色发光器件、蓝色发光器件、绿色发光器件均为激光二极管发光器件，各激光二极管发光器件均包括激光二极管驱动板(12111)、激光二极管(12112)、聚光镜(12113)，激光二极管发光器件工作时，激光二极管驱动板驱动激光二极管发出光子，然后通过聚光镜将光子聚集成成束的激光。

所述彩光发光器件均内置激光二极管，三个彩光发光器件通过激发不同的半导体，使各激光二极管分别发射红、绿、蓝三色光子，各彩光发光器件激光合成的主动光源阵列装置光信号，其光源颜色随着所代表的Yaw、Roll、Pitch的旋转角度而变化，具体如下：

C＝[r(x)*255，g(x)*255，b(x)*255] 公式一；

公式中，C表示复合的激光颜色，公式一表达式的涵义是混合而成的光谱色在视觉上由红、绿、蓝三种基本色各自按相应的比例r(x)、g(x)、b(x)混合相加而形成；r(x)、g(x)、b(x)与旋转角度的关系如下：

其中x为当前所要表示的Yaw或Pitch或Roll的旋转角度，r(x)、g(x)、b(x)即为计算出来的颜色系数，主控模块根据计算出的r(x)、g(x)、b(x)颜色系数，输出相应的PWM信号值给功率控制模块。

所述无人机多机自动协同时的方法包括以下步骤；

步骤S1：无人机姿态感应模块获取无人机姿态信息，并向主控模块发送所获取的无人机姿态信息；

步骤S2：无人机姿态信息包括Yaw、Roll、Pitch三轴旋转角度，主控模块根据公式二、公式三、公式四，将旋转角度转换为要发出颜色的颜色系数r(x)、g(x)、b(x)，通过颜色系数计算出所需要的功率，再将对应的PWM信号值给各功率控制模块；所述PWM信号值包括与功率控制模块最大控制功率对应的输出功率百分比数据；

步骤S3：各功率控制模块根据主控模块输出的PWM值，向各对应的彩光发光器件提供对应PWM值的功率，来控制相对应的三个发光器件的发射功率；步骤S4：主动光源合成装置的三个彩光发光器件发出符合计算得到的颜色系数的红、蓝、绿色激光，即根据公式一所得的三个激光，通过主动光源合成装置的激光合成模块，合成为沿同一直线输出且颜色能表述对应旋转角度值的复合激光；

步骤S5：主动光源合成装置先以激光调节模块对复合激光处理，形成远近激光点大小恒定的激光，再顺序经反射镜、平凸镜到达遮光罩，经遮光罩处理后形成易被外部电子设备或人类所观测并记录的光源信号并出射；

步骤S6：四个主动光源合成装置重复经过以上过程，在无人机上形成四个具有识别特征的主动光源信号，其中有一个为固定发出白色光的白色光源，以这一光源作为无人机的定位特征，其它三个为彩色光源，其光源颜色分别表示无人机Yaw、Roll、Pitch的旋转数据值，以实现基于主动光源阵列的无人机姿态信息传递。

所述解算控制模块对图像中无人机的主动光源阵列进行识别时，具体方法为：接收来自视觉传感模块的图像和深度信息，深度信息值以D表示，解算控制模块的解算控制算法通过霍夫变换进行圆的识别，公式为：

(x_i-a)²+(y_i-b)²＝r²

通过设定一个霍夫空间，并以(a,b,r)这三个参数构成空间中一点，公式中，x_i和y_i是图像中的点，a和b是圆心的坐标，r是圆的半径。遍历图像中每一个点(x_i，y_i)，对于每个点，计算出其到对应可能的圆心(a,b)的距离r，并在霍夫空间上增加(a,b,r)这点的计数S_a，b，r，S_a，b，r越大，则a、b、r能表示圆的确信度就越高，当S_a，b，r大于一定的值时即：

S_a，b，r≥S_阈值

a、b、r表示以(a,b)为中心，r为半径的圆，S_阈值是设定的值，通过设定不同的值来调节识别圆的准确度；

通过识别一定范围内圆的数量，来识别出图像中的主动光源阵列的位置，通过对主动光源的区域分别进行每一个点的R、G、B值的加权运算，越靠中心区域的权值越大，进而计算出光源颜色的R、G、B值，从而识别每一个主动光源的颜色，

识别到白色后，以白色的主动光源为坐标原点，向相邻的光源延伸两条线，若其中一条线顺时针旋转90°能和另一条线重合，那么以这条线作为Y正轴，另一条作为X正轴建立光源阵列坐标系，其中表示Yaw的光源在X正轴上，表示Pitch的光源在Y正轴上，进而识别出分别表示Yaw、Roll、Pitch的光源位置，并计算出三个光源颜色的R、G、B值，并通过下列公式：

获得颜色系数r(x)、g(x)、b(x)，再通过下列公式：

计算出Yaw或Roll或Pitch的旋转角度x，注意x表示为旋转角度仅在公式五中；

重复公式五，依次解算出主机的Yaw、Roll、Pitch的旋转角度，以Yaw、Roll、Pitch分别代表上述三个的旋转角度数值；

更进一步的，通过前面识别前方无人机的主动光源阵列位置，并计算与图像中心的像素差值Pix_x差和Pix_y差，和计算出的Yaw、Roll、Pitch通过以下公式：

计算出从机跟踪主机时的X、Y、Z轴和Yaw角旋转的速度，以此控制从机来跟踪主机。

公式六、公式七中的max和min用来限制无人机最大速度在6m/s，负号不代表大小，是代表飞行的方向，

无人机的机头方向为X正轴时，及往前飞的速度为正，往后飞速度为负，若无人机的机头方向对于Y轴时无人机机头向前，则右飞的速度为正，左飞速度为负，若无人机的机头方向对于Z轴，则向上飞为正，反之为负；

公式六、公式七中的ΔYaw为一定时间内Yaw角的变化度数；Y轴在图像中为X轴，Z轴在图像中为Y轴，并且图像中的Y轴为自上而下递增。

所述无人机多机自动协同时的方法还包括以下步骤；

步骤S7：从机通过深度摄像头获取主机的图像和深度信息给解算控制模块，通过图像获取主机主动光源阵列的不同光源颜色的R、G、B值；

步骤S8：通过公式五计算出不同光源所代表的Yaw、Roll、Pitch的旋转数据值，并利用旋转数据值和深度信息，通过公式六、公式七、公式八、公式九计算出从机跟踪主机的X、Y、Z轴和Yaw角的速度控制值，来控制从机进行主机的跟踪。对于多从机，重复上述步骤发出主动光源阵列并且跟踪前面的从机或主机，实现基于主动光源阵列的无人机多机自动协同。

当进行无人机多机自动协同作业时，主机、从机均安装有主动光源阵列，协同控制装置仅安装在从机处。

本发明提供一种基于主动光源阵列的无人机多机自动协同装置及方法，该装置由主动光源阵列和自动协同装置组成。主动光源阵列装载于无人机主机上，由四个主动光源组成，每个主动光源都是三个红、绿、蓝色激光所合成的，并使用遮光罩处理，使最后发出的主动光源能够更好的被视觉采集设备所观测。其中一个光源固定发出白光，以此作为定位特征，其它三个主动光源通过陀螺仪获取无人机姿态信息，根据不同的姿态信息旋转角度，合成不同颜色的激光，从而为从机提供更清晰的识别目标和更加准确的飞行参数。主动光源在白天和夜晚都有较为明显的显示效果，不依赖于通讯，从机上装置视觉采集设备，对主机的光源阵列不同位置的主动光源颜色进行识别，即可获得无人机姿态信息，并通过跟踪算法进行控制，以实现在多场景中无人机多机的自动协同。

本发明与现有技术相比具有以下有益效果：

1、本发明通过遮光罩处理合成激光，解决了激光类光源难以被电子设备识别的问题。

2、本发明使用主动光源阵列来表示无人机不同姿态的信息，有利于在各个场景，包括但不限于夜晚、白天、无GPS、有磁场干扰等多个复杂场景下使用，从而使从机能够在多场景下更有效地获取主机信息，并且跟踪主机。

3、本发明使用姿态信息融合深度信息和图像信息进行速度的控制，较为快速且精确地让从机跟踪主机。

附图说明

下面结合附图和具体实施方式对本发明进一步详细的说明：

附图1是本发明装置的立体结构示意图；

附图2是本发明主机装置的立体结构示意图；

附图3是本发明从机1装置的立体结构示意图；

附图4是本发明主机装置的内部结构示意图；

附图5是本发明一实施例中主动光源合成装置立体结构示意图；

附图6是本发明一实施例中供电控制装置立体结构示意图；

附图7是本发明一实施例中红色发光器件的立体结构示意图；

附图8是本发明一实施例中光源阵列坐标系的示意图；

附图9是本发明一实施例中激光合成模块和激光调整模块的光路示意图；

附图10是本发明一实施例中一种基于主动光源阵列的无人机多机自动协同方法的控制流程示意图；

图中：1主机、2从机、3另一从机、11主机无人机本体、12主动光源阵列、21从机无人机本体、22解算控制模块、23视觉传感器模块、24从机主动光源阵列装置、121主动光源合成装置、122供电控制装置、1210底座、1211红色发光器件、1212绿色发光器件、1213蓝色发光器件、1214合光棱镜、1215平凸镜、1216平凸镜、1217反射镜、1218平凸镜、1219遮光罩、1221三路控制器、1222三路控制器、1223三路控制器、1224三路控制器、1225无人机姿态感应模块、1226主控模块、12111激光二极管驱动板、12112激光二极管、12113聚光镜。

具体实施方式

如图所示，一种基于主动光源阵列的无人机多机自动协同方法，所述协同方法中，无人机分为主机1和从机2、另一从机3，从机的数量为一个以上，主机和从机均安装有主动光源阵列12，从机安装有协同控制装置；所述主动光源阵列包括多个主动光源，主动光源用于发出表述无人机姿态信息数据的光信号，以及发出用于无人机定位的光信号；所述从机通过协同控制装置识读主动光源阵列光信号中的主机姿态信息和定位信息，通过跟踪算法控制无人机飞行，以实现多无人机的多机自动协同。

一种基于主动光源阵列的无人机多机自动协同装置，用于实施以上所述的基于主动光源阵列的无人机多机自动协同方法，所述主动光源阵列包括一个发出白光作为无人机定位特征的白色光主动光源，还包括三个发出彩光的彩色光主动光源，各主动光源的光信号为红、绿、蓝三色激光合成并使用遮光罩处理后的光信号，在多机自动协同时，彩色光主动光源与无人机的无人机姿态感应模块相连以通过陀螺仪获取该无人机姿态信息，根据无人机当前飞行状态不同的姿态信息旋转角度，合成不同颜色的光信号，为从机提供识别目标和主机飞行参数；

所述协同控制装置包括视觉传感模块23和解算控制模块24；所述视觉传感模块通过带红外测距模块的深度摄像头来获取图像信息，并通过红外测距模块获取图像中的深度信息，并将图像信息和深度信息传给解算控制模块。

所述解算控制模块为无人机处的板载计算机，板载计算机通过接收图像信息及深度信息来识别图像中的无人机位置，进一步识别无人机上的主动光源位置，并通过识别算法将光源颜色转换为无人机的姿态信息数据；

识别算法工作时，先对无人机的主动光源阵列进行识别，再识别主动光源阵列中白色光主动光源的位置，识别到白色光主动光源后，根据白色光主动光源在四个主动光源中的位置，标记出表示偏航角数据Yaw、滚动角数据Roll、俯仰角数据Pitch的三个彩色光主动光源，最后分别对表示Yaw、Roll、Pitch的三个彩色光主动光源光源颜色进行R、G、B值的提取，并将R、G、B解算成对应旋转角度，

所述板载计算机存有根据姿态信息和深度信息控制无人机从机跟踪的算法，该算法通过旋转角度和深度信息计算出无人机跟踪前方主机所要的移动速度和角度，并控制无人机进行相应的飞行。

所述主动光源阵列装置包括四个相同结构的主动光源合成装置和供电控制装置；每个主动光源均含一个主动光源合成装置121和一个供电控制装置122；

主动光源合成装置包括底座1210和设于底座处的彩光发光器件；彩光发光器件的输出光路处顺序设置激光合成模块、激光调整模块；

所述激光合成模块位于发光器件的光输出端上方，包括顺序设置的合光棱镜1214、第一平凸镜1215，第一平凸镜的平面朝向合光棱镜；

彩光发光器件包括分别位于合光棱镜三面处的红色发光器件1211、蓝色发光器件1213、绿色发光器件1212；红色发光器件、蓝色发光器件、绿色发光器件发出的三色激光经激光合成模块合为一个合成激光束从激光合成模块的光输出端出射；

所述激光调整模块的光输入端与激光合成模块的光输出端相通；激光调整模块包括沿光路顺序设置的第二平凸镜1216、反射镜1217、第三平凸镜1218、遮光罩1219；所述第二平凸镜的凸面朝向合光棱镜以把合成激光调整为固定大小；所述反射镜位于第二平凸镜后用呈45°摆放，用于将合成激光向上反射；第三平凸镜、遮光罩位于反射镜上方，第三平凸镜截取并汇聚的激光光束在遮光罩处形成可识别的光源；

供电控制装置的功率控制模块包括第一控制器1221、第二控制器1222、第三控制器1223、第四控制器1224；

本例中，不同的控制器用于控制主动光源阵列的不同位置的主动光源中的发光器件的功率。

所述无人机姿态感应模块1225经线缆与主控模块1226相连；

所述合光棱镜设于底座中部的挖槽处；

所述无人机姿态感应模块通过陀螺仪获取无人机的姿态数据；

所述红色发光器件、蓝色发光器件、绿色发光器件均为激光二极管发光器件，各激光二极管发光器件均包括激光二极管驱动板12111、激光二极管12112、聚光镜12113，激光二极管发光器件工作时，激光二极管驱动板驱动激光二极管发出光子，然后通过聚光镜将光子聚集成成束的激光。

所述彩光发光器件均内置激光二极管，三个彩光发光器件通过激发不同的半导体，使各激光二极管分别发射红、绿、蓝三色光子，各彩光发光器件激光合成的主动光源阵列装置光信号，其光源颜色随着所代表的Yaw、Roll、Pitch的旋转角度而变化，具体如下：

C＝[r(x)*255，g(x)*255，b(x)*255] 公式一；

公式中，C表示复合的激光颜色，公式一表达式的涵义是混合而成的光谱色在视觉上由红、绿、蓝三种基本色各自按相应的比例r(x)、g(x)、b(x)混合相加而形成；本例中，若r(x)、g(x)、b(x)分别为0.4、0.6、0，则复合激光由红、绿激光组成，蓝色激光不产生激光，为关闭状态，而r(x)、g(x)为0.4、0.6表示绿色比红色功率更高，亮度更高，而红绿色混色为黄色，所以混合而成的复合激光呈现黄绿色。r(x)、g(x)、b(x)与旋转角度的关系如下：

其中x为当前所要表示的Yaw或Pitch或Roll的旋转角度，r(x)、g(x)、b(x)即为计算出来的颜色系数，主控模块根据计算出的r(x)、g(x)、b(x)颜色系数，输出相应的PWM信号值给功率控制模块。

所述无人机多机自动协同时的方法包括以下步骤；

步骤S1：无人机姿态感应模块获取无人机姿态信息，并向主控模块发送所获取的无人机姿态信息；

步骤S2：无人机姿态信息包括Yaw、Roll、Pitch三轴旋转角度，主控模块根据公式二、公式三、公式四，将旋转角度转换为要发出颜色的颜色系数r(x)、g(x)、b(x)，通过颜色系数计算出所需要的功率，再将对应的PWM信号值给各功率控制模块；所述PWM信号值包括与功率控制模块最大控制功率对应的输出功率百分比数据；

本例中，若颜色系数r(x)、g(x)、b(x)为0.5、0.5、0，而功率的满值(功率控制模块最大控制功率)为100W，则要输出的功率对应红色、绿色、蓝色发光器件为0.5*100W、0.5*100W、0*100W，即50W、50W、0W。

若计算出所需要的功率为50W，假设功率控制模块的上限为100W，则PWM即PWM信号输出为50％可以让功率控制模块提供50W的功率给被控制器件；

步骤S3：各功率控制模块根据主控模块输出的PWM值，向各对应的彩光发光器件提供对应PWM值的功率，来控制相对应的三个发光器件的发射功率；步骤S4：主动光源合成装置的三个彩光发光器件发出符合计算得到的颜色系数的红、蓝、绿色激光，即根据公式一所得的三个激光，通过主动光源合成装置的激光合成模块，合成为沿同一直线输出且颜色能表述对应旋转角度值的复合激光；

步骤S5：主动光源合成装置先以激光调节模块对复合激光处理，形成远近激光点大小恒定的激光，再顺序经反射镜、平凸镜到达遮光罩，经遮光罩处理后形成易被外部电子设备或人类所观测并记录的光源信号并出射；

步骤S6：四个主动光源合成装置重复经过以上过程，在无人机上形成四个具有识别特征的主动光源信号，其中有一个为固定发出白色光的白色光源，以这一光源作为无人机的定位特征，其它三个为彩色光源，其光源颜色分别表示无人机Yaw、Roll、Pitch的旋转数据值，以实现基于主动光源阵列的无人机姿态信息传递。

所述解算控制模块对图像中无人机的主动光源阵列进行识别时，具体方法为：接收来自视觉传感模块的图像和深度信息，深度信息值以D表示，解算控制模块的解算控制算法通过霍夫变换进行圆的识别，公式为：

(x_i-a)²+(y_i-b)²＝r²

通过设定一个霍夫空间，并以(a,b,r)这三个参数构成空间中一点，公式中，x_i和y_i是图像中的点，a和b是圆心的坐标，r是圆的半径。遍历图像中每一个点(x_i，y_i)，对于每个点，计算出其到对应可能的圆心(a,b)的距离r，并在霍夫空间上增加(a,b,r)这点的计数S_a，b，r，S_a，b，r越大，则a、b、r能表示圆的确信度就越高，当S_a，b，r大于一定的值时即：

S_a，b，r≥S_阈值

a、b、r表示以(a,b)为中心，r为半径的圆，S_阈值是设定的值，通过设定不同的值来调节识别圆的准确度；

通过识别一定范围内圆的数量，来识别出图像中的主动光源阵列的位置，通过对主动光源的区域分别进行每一个点的R、G、B值的加权运算，越靠中心区域的权值越大，进而计算出光源颜色的R、G、B值，从而识别每一个主动光源的颜色，

识别到白色后，以白色的主动光源为坐标原点，向相邻的光源延伸两条线，若其中一条线顺时针旋转90°能和另一条线重合，那么以这条线作为Y正轴，另一条作为X正轴建立光源阵列坐标系，其中表示Yaw的光源在X正轴上，表示Pitch的光源在Y正轴上，进而识别出分别表示Yaw、Roll、Pitch的光源位置，并计算出三个光源颜色的R、G、B值，并通过下列公式：

获得颜色系数r(x)、g(x)、b(x)，再通过下列公式：

计算出Yaw或Roll或Pitch的旋转角度x，注意x表示为旋转角度仅在公式五中；

重复公式五，依次解算出主机的Yaw、Roll、Pitch的旋转角度，以Yaw、Roll、Pitch分别代表上述三个的旋转角度数值；

更进一步的，通过前面识别前方无人机的主动光源阵列位置，并计算与图像中心的像素差值Pix_x差和Pix_y差，和计算出的Yaw、Roll、Pitch通过以下公式：

计算出从机跟踪主机时的X、Y、Z轴和Yaw角旋转的速度，以此控制从机来跟踪主机。

公式六、公式七中的max和min用来限制无人机最大速度在6m/s，负号不代表大小，是代表飞行的方向，

无人机的机头方向为X正轴时，及往前飞的速度为正，往后飞速度为负，若无人机的机头方向对于Y轴时无人机机头向前，则右飞的速度为正，左飞速度为负，若无人机的机头方向对于Z轴，则向上飞为正，反之为负；

公式六、公式七中的ΔYaw为一定时间内Yaw角的变化度数；Y轴在图像中为X轴，Z轴在图像中为Y轴，并且图像中的Y轴为自上而下递增。

所述无人机多机自动协同时的方法还包括以下步骤；

步骤S7：从机通过深度摄像头获取主机的图像和深度信息给解算控制模块，通过图像获取主机主动光源阵列的不同光源颜色的R、G、B值；

步骤S8：通过公式五计算出不同光源所代表的Yaw、Roll、Pitch的旋转数据值，并利用旋转数据值和深度信息，通过公式六、公式七、公式八、公式九计算出从机跟踪主机的X、Y、Z轴和Yaw角的速度控制值，来控制从机进行主机的跟踪。对于多从机，重复上述步骤发出主动光源阵列并且跟踪前面的从机或主机，实现基于主动光源阵列的无人机多机自动协同。

当进行无人机多机自动协同作业时，主机、从机均安装有主动光源阵列，协同控制装置仅安装在从机处。

本例中，当无人机工作过程的飞行姿态相对稳定没有频繁变化时，还可以在遮光罩处设置可吸收激光能量的长余辉发光材料，通过吸收激光能量来激发长余辉发光材料发光，形成延时发光效果。

本例中，每个主动光源均含一个主动光源合成装置和一个供电控制装置，白光主动光源的白色光，也是由三色激光按特定比例混合形成，以使白色光主动光源与彩色光主动光源可以相互切换。

本例中，在多无人机协同作业时，从机根据主机通过光信号广播的姿态变化，获知主机将要执行的动作，来与主机配合，或是主机通过其无人机姿态的变化，来向从机传递后续将要执行的任务信息。

以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (10)

1.一种基于主动光源阵列的无人机多机自动协同方法，其特征在于：所述协同方法中，无人机分为主机(1)和一个以上的从机(2)，主机和从机均安装有主动光源阵列(12)，从机安装有协同控制装置；所述主动光源阵列包括多个主动光源，主动光源用于发出表述无人机姿态信息数据的光信号，以及发出用于无人机定位的光信号；所述从机通过协同控制装置识读主动光源阵列光信号中的主机姿态信息和定位信息，通过跟踪算法控制无人机飞行，以实现多无人机的多机自动协同。

2.一种基于主动光源阵列的无人机多机自动协同装置，用于实施权利要求1中所述的基于主动光源阵列的无人机多机自动协同方法，其特征在于：所述主动光源阵列包括一个发出白光作为无人机定位特征的白色光主动光源，还包括三个发出彩光的彩色光主动光源，各主动光源的光信号为红、绿、蓝三色激光合成并使用遮光罩处理后的光信号，在多机自动协同时，彩色光主动光源与无人机的无人机姿态感应模块相连以通过陀螺仪获取该无人机姿态信息，根据无人机当前飞行状态不同的姿态信息旋转角度，合成不同颜色的光信号，为从机提供识别目标和主机飞行参数；

所述协同控制装置包括视觉传感模块(23)和解算控制模块(24)；所述视觉传感模块通过带红外测距模块的深度摄像头来获取图像信息，并通过红外测距模块获取图像中的深度信息，并将图像信息和深度信息传给解算控制模块。

3.根据权利要求2所述的一种基于主动光源阵列的无人机多机自动协同装置，其特征在于：所述解算控制模块为无人机处的板载计算机，板载计算机通过接收图像信息及深度信息来识别图像中的无人机位置，进一步识别无人机上的主动光源位置，并通过识别算法将光源颜色转换为无人机的姿态信息数据；

识别算法工作时，先对无人机的主动光源阵列进行识别，再识别主动光源阵列中白色光主动光源的位置，识别到白色光主动光源后，根据白色光主动光源在四个主动光源中的位置，标记出表示偏航角数据Yaw、滚动角数据Roll、俯仰角数据Pitch的三个彩色光主动光源，最后分别对表示Yaw、Roll、Pitch的三个彩色光主动光源光源颜色进行R、G、B值的提取，并将R、G、B解算成对应旋转角度；

所述板载计算机存有根据姿态信息和深度信息控制无人机从机跟踪的算法，该算法通过旋转角度和深度信息计算出无人机跟踪前方主机所要的移动速度和角度，并控制无人机进行相应的飞行。

4.根据权利要求2所述的一种基于主动光源阵列的无人机多机自动协同装置，其特征在于：所述主动光源阵列装置包括四个相同结构的主动光源合成装置和供电控制装置；每个主动光源均含一个主动光源合成装置(121)和一个供电控制装置(122)；

主动光源合成装置包括底座(1210)和设于底座处的彩光发光器件；彩光发光器件的输出光路处顺序设置激光合成模块、激光调整模块；

所述激光合成模块位于发光器件的光输出端上方，包括顺序设置的合光棱镜(1214)、第一平凸镜(1215)，第一平凸镜的平面朝向合光棱镜；

彩光发光器件包括分别位于合光棱镜三面处的红色发光器件(1211)、蓝色发光器件(1213)、绿色发光器件(1212)；红色发光器件、蓝色发光器件、绿色发光器件发出的三色激光经激光合成模块合为一个合成激光束从激光合成模块的光输出端出射；

所述激光调整模块的光输入端与激光合成模块的光输出端相通；激光调整模块包括沿光路顺序设置的第二平凸镜(1216)、反射镜(1217)、第三平凸镜(1218)、遮光罩(1219)；所述第二平凸镜的凸面朝向合光棱镜以把合成激光调整为固定大小；所述反射镜位于第二平凸镜后用呈45°摆放，用于将合成激光向上反射；第三平凸镜、遮光罩位于反射镜上方，第三平凸镜截取并汇聚的激光光束在遮光罩处形成可识别的光源；

供电控制装置的功率控制模块包括第一控制器(1221)、第二控制器(1222)、第三控制器(1223)、第四控制器(1224)；

所述无人机姿态感应模块(1225)经线缆与主控模块(1226)相连；

所述合光棱镜设于底座中部的挖槽处；

所述无人机姿态感应模块通过陀螺仪获取无人机的姿态数据；

所述红色发光器件、蓝色发光器件、绿色发光器件均为激光二极管发光器件，各激光二极管发光器件均包括激光二极管驱动板(12111)、激光二极管(12112)、聚光镜(12113)，激光二极管发光器件工作时，激光二极管驱动板驱动激光二极管发出光子，然后通过聚光镜将光子聚集成成束的激光。

5.根据权利要求4所述的一种基于主动光源阵列的无人机多机自动协同装置，其特征在于：所述彩光发光器件均内置激光二极管，三个彩光发光器件通过激发不同的半导体，使各激光二极管分别发射红、绿、蓝三色光子，各彩光发光器件激光合成的主动光源阵列装置光信号，其光源颜色随着所代表的Yaw、Roll、Pitch的旋转角度而变化，具体如下：

C＝[r(x)*255，g(x)*255，b(x)*255] 公式一；

公式中，C表示复合的激光颜色，公式一表达式的涵义是混合而成的光谱色在视觉上由红、绿、蓝三种基本色各自按相应的比例r(x)、g(x)、b(x)混合相加而形成；r(x)、g(x)、b(x)与旋转角度的关系如下：

其中x为当前所要表示的Yaw或Pitch或Roll的旋转角度，r(x)、g(x)、b(x)即为计算出来的颜色系数，主控模块根据计算出的r(x)、g(x)、b(x)颜色系数，输出相应的PWM信号值给功率控制模块。

6.根据权利要求4所述的一种基于主动光源阵列的无人机多机自动协同装置，其特征在于：所述无人机多机自动协同时的方法包括以下步骤；

步骤S1：无人机姿态感应模块获取无人机姿态信息，并向主控模块发送所获取的无人机姿态信息；

步骤S2：无人机姿态信息包括Yaw、Roll、Pitch三轴旋转角度，主控模块根据公式二、公式三、公式四，将旋转角度转换为要发出颜色的颜色系数r(x)、g(x)、b(x)，通过颜色系数计算出所需要的功率，再将对应的PWM信号值给各功率控制模块；所述PWM信号值包括与功率控制模块最大控制功率对应的输出功率百分比数据；

步骤S3：各功率控制模块根据主控模块输出的PWM值，向各对应的彩光发光器件提供对应PWM值的功率，来控制相对应的三个发光器件的发射功率；

步骤S4：主动光源合成装置的三个彩光发光器件发出符合计算得到的颜色系数的红、蓝、绿色激光，即根据公式一所得的三个激光，通过主动光源合成装置的激光合成模块，合成为沿同一直线输出且颜色能表述对应旋转角度值的复合激光；

步骤S5：主动光源合成装置先以激光调节模块对复合激光处理，形成远近激光点大小恒定的激光，再顺序经反射镜、平凸镜到达遮光罩，经遮光罩处理后形成易被外部电子设备或人类所观测并记录的光源信号并出射；

步骤S6：四个主动光源合成装置重复经过以上过程，在无人机上形成四个具有识别特征的主动光源信号，其中有一个为固定发出白色光的白色光源，以这一光源作为无人机的定位特征，其它三个为彩色光源，其光源颜色分别表示无人机Yaw、Roll、Pitch的旋转数据值，以实现基于主动光源阵列的无人机姿态信息传递。

7.根据权利要求3所述的一种基于主动光源阵列的无人机多机自动协同装置，其特征在于：所述解算控制模块对图像中无人机的主动光源阵列进行识别时，具体方法为：接收来自视觉传感模块的图像和深度信息，深度信息值以D表示，解算控制模块的解算控制算法通过霍夫变换进行圆的识别，公式为：

(x_i-a)²+(y_i-b)²＝r²

通过设定一个霍夫空间，并以(a,b,r)这三个参数构成空间中一点，公式中，x_i和y_i是图像中的点，a和b是圆心的坐标，r是圆的半径。遍历图像中每一个点(x_i，y_i)，对于每个点，计算出其到对应可能的圆心(a,b)的距离r，并在霍夫空间上增加(a,b,r)这点的计数S_a，b，r，S_a，b，r越大，则a、b、r能表示圆的确信度就越高，当S_a，b，r大于一定的值时即：

S_a，b，r≥S_阈值

a、b、r表示以(a,b)为中心，r为半径的圆，S_阈值是设定的值，通过设定不同的值来调节识别圆的准确度；

通过识别一定范围内圆的数量，来识别出图像中的主动光源阵列的位置，通过对主动光源的区域分别进行每一个点的R、G、B值的加权运算，越靠中心区域的权值越大，进而计算出光源颜色的R、G、B值，从而识别每一个主动光源的颜色，

识别到白色后，以白色的主动光源为坐标原点，向相邻的光源延伸两条线，若其中一条线顺时针旋转90°能和另一条线重合，那么以这条线作为Y正轴，另一条作为X正轴建立光源阵列坐标系，其中表示Yaw的光源在X正轴上，表示Pitch的光源在Y正轴上，进而识别出分别表示Yaw、Roll、Pitch的光源位置，并计算出三个光源颜色的R、G、B值，并通过下列公式：

获得颜色系数r(x)、g(x)、b(x)，再通过下列公式：

计算出Yaw或Roll或Pitch的旋转角度x，注意x表示为旋转角度仅在公式五中；

重复公式五，依次解算出主机的Yaw、Roll、Pitch的旋转角度，以Yaw、Roll、Pitch分别代表上述三个的旋转角度数值；

更进一步的，通过前面识别前方无人机的主动光源阵列位置，并计算与图像中心的像素差值Pix_x差和Pix_y差，和计算出的Yaw、Roll、Pitch通过以下公式：

计算出从机跟踪主机时的X、Y、Z轴和Yaw角旋转的速度，以此控制从机来跟踪主机。

8.根据权利要求7所述的一种基于主动光源阵列的无人机多机自动协同装置，其特征在于：公式六、公式七中的max和min用来限制无人机最大速度在6m/s，负号不代表大小，是代表飞行的方向，

无人机的机头方向为X正轴时，及往前飞的速度为正，往后飞速度为负，若无人机的机头方向对于Y轴时无人机机头向前，则右飞的速度为正，左飞速度为负，若无人机的机头方向对于Z轴，则向上飞为正，反之为负；公式六、公式七中的ΔYaw为一定时间内Yaw角的变化度数；Y轴在图像中为X轴，Z轴在图像中为Y轴，并且图像中的Y轴为自上而下递增。

9.根据权利要求7所述的一种基于主动光源阵列的无人机多机自动协同装置，其特征在于：所述无人机多机自动协同时的方法还包括以下步骤；

步骤S7：从机通过深度摄像头获取主机的图像和深度信息给解算控制模块，通过图像获取主机主动光源阵列的不同光源颜色的R、G、B值；

步骤S8：通过公式五计算出不同光源所代表的Yaw、Roll、Pitch的旋转数据值，并利用旋转数据值和深度信息，通过公式六、公式七、公式八、公式九计算出从机跟踪主机的X、Y、Z轴和Yaw角的速度控制值，来控制从机进行主机的跟踪。对于多从机，重复上述步骤发出主动光源阵列并且跟踪前面的从机或主机，实现基于主动光源阵列的无人机多机自动协同。

10.根据权利要求2所述的一种基于主动光源阵列的无人机多机自动协同装置，其特征在于：当进行无人机多机自动协同作业时，主机、从机均安装有主动光源阵列，协同控制装置仅安装在从机处。