通过无人机位置和云台角度估测目标坐标的方法和系统与流程

1.本发明涉及无人机侦测技术领域，具体涉及一种通过无人机位置和云台角度估测目标坐标的方法和系统。


背景技术：

2.目前，无人机在侦察任务中通过机载的测距模块上的激光雷达或者双目摄像头来获取目标的位置坐标信息，并根据该位置坐标信息计算出目标的位置。然而，相关技术需要在无人机上额外配备传感器模块，使得相关技术存在成本高的缺点。


技术实现要素：

3.有鉴于此，提供一种通过无人机位置和云台角度估测目标坐标的方法和系统，以解决相关技术的无人机存在的侦察成本高的问题。
4.本发明采用如下技术方案：
5.第一方面，本发明提供了一种通过无人机位置和云台角度估测目标坐标的方法，包括：
6.获取无人机的坐标数据和云台的角度数据；所述云台设置在所述无人机上；所述坐标数据与所述角度数据一一对应；
7.根据所述坐标数据和所述角度数据确定出多条第一直线；
8.确定出距离所有所述第一直线最短的第一目标点；
9.将所述第一目标点的坐标定义为待测物体的位置坐标。
10.优选的，所述根据所述坐标数据和所述角度数据确定出多条第一直线之前，本发明的通过无人机位置和云台角度估测目标坐标的方法还包括：
11.获取固定在所述云台上的相机的相机参数和所述相机画面中的目标框中心点坐标；所述相机参数包括焦距、视场角和相机画面的像素；
12.根据所述相机参数和所述相机画面中的目标框中心点坐标确定所述云台的各角度数据对应的补偿角度；
13.根据所述云台的各角度数据对应的补偿角度对应修正所述云台的角度数据，得到修正后的所述云台的角度数据；
14.所述根据所述坐标数据和所述角度数据确定出多条第一直线，包括：
15.根据所述坐标数据和修正后的所述云台的角度数据确定出多条第一直线。
16.优选的，所述确定出距离所有所述第一直线最短的第一目标点之后，本发明的通过无人机位置和云台角度估测目标坐标的方法还包括：
17.采集所述无人机的当前坐标数据、所述云台的当前角度数据、固定在所述云台上的相机的相机参数和所述相机当前画面中的目标框中心点坐标；
18.根据所述相机参数和所述相机当前画面中的目标框中心点坐标确定所述云台的当前角度数据对应的补偿角度；
19.根据所述云台的当前角度数据对应的补偿角度修正所述云台的当前角度数据，得到修正后的所述云台的当前角度数据；
20.根据所述当前坐标数据和修正后的所述云台的当前角度数据确定出对应的第二直线；
21.确定出距离所有所述第一直线和所述第二直线最短的第二目标点；
22.判断所述第一目标点的位置坐标和所述第二目标点的位置坐标是否满足预设收敛条件；
23.当所述第一目标点的位置坐标和所述第二目标点的位置坐标满足预设收敛条件时，执行步骤：将所述第一目标点的坐标定义为待测物体的位置坐标；
24.当所述第一目标点的位置坐标和所述第二目标点的位置坐标不满足预设收敛条件时，执行步骤：采集所述无人机的当前坐标数据、所述云台的当前角度数据、固定在所述云台上的相机的相机参数和所述相机当前画面中的目标框中心点坐标。
25.优选的，所述第一直线为以所述无人机位置为起点，以固定在所述云台上的相机的视线角度为方向的射线。
26.优选的，所述根据所述坐标数据和所述角度数据确定出多条第一直线，包括：
27.将所述wgs84坐标数据转换为笛卡尔坐标数据；
28.根据所述笛卡尔坐标数据和所述角度数据确定出多条第一直线。
29.优选的，所述笛卡尔坐标数据对应的笛卡尔坐标系的原点为所述无人机取得的第一个wgs84坐标点。
30.优选的，确定出距离所有所述第一直线最短的第一目标点，包括：
31.通过如下计算公式计算出空间中任意一点p到所有所述第一直线的距离之和：
[0032][0033]
其中，d(p；x,v)表示距离之和；x
k
表示第k条第一直线对应的所述无人机的笛卡尔坐标数据；v
k
表示第k条第一直线对应的所述云台的角度数据；
[0034]
通过如下计算公式计算所述距离之和的最小值，得到距离所有所述第一直线最短的第一目标点：
[0035][0036]
第二方面，本发明还提供了一种通过无人机位置和云台角度估测目标坐标的系统，包括：无人机、云台、相机和图像处理设备；
[0037]
所述云台设置在所述无人机上，用于固定所述相机；
[0038]
所述图像处理设备与所述相机连接，用于获取所述相机采集的图像，并根据所述图像实现本发明所述的通过无人机位置和云台角度估测目标坐标的方法。
[0039]
本发明采用以上技术方案，一种通过无人机位置和云台角度估测目标坐标的方法，包括：获取无人机的坐标数据和云台的角度数据；所述云台设置在所述无人机上；所述坐标数据与所述角度数据一一对应；根据所述坐标数据和所述角度数据确定出多条第一直线；确定出距离所有所述第一直线最短的第一目标点；将所述第一目标点的坐标定义为待
测物体的位置坐标。基于此，本发明仅通过无人机、摄像头和图像处理设备便可完成对目标坐标的估测，具有成本低和估测过程简单便捷的优点。
附图说明
[0040]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0041]
图1是本发明实施例提供的一种工作情景图；
[0042]
图2是本发明实施例提供的一种通过无人机位置和云台角度估测目标坐标的方法的流程示意图；
[0043]
图3是本发明实施例提供的另一种工作情景图；
[0044]
图4是本发明实施例提供的另一种通过无人机位置和云台角度估测目标坐标的方法的流程示意图；
[0045]
图5是本发明实施例提供的一种通过无人机位置和云台角度估测目标坐标的系统的结构示意图。
具体实施方式
[0046]
为使本发明的目的、技术方案和优点更加清楚，下面将对本发明的技术方案进行详细的描述。显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所得到的所有其它实施方式，都属于本发明所保护的范围。
[0047]
图1是本发明实施例提供的一种工作情景图。如图1所示，无人机绕目标盘旋，期间，相机和云台需要一直保持对目标的锁定。在理想状态下，以无人机的位置为端点，以相机的视线角度为方向，可以得到一条指向目标的射线，所有射线汇集的位置，即为目标点的位置。
[0048]
然而，在实际工作环境中，由于多种原因的影响，比如云台的抖动、云台的转向速度、无人机的位置偏移和相机的跟踪速度等原因，多条射线往往难以汇聚到同一个交点。
[0049]
考虑到上述情况，本实施例提供了一种通过无人机位置和云台角度估测目标坐标的方法，如图2所示，本实施例的通过无人机位置和云台角度估测目标坐标的方法，包括：
[0050]
s201、获取无人机的坐标数据和云台的角度数据；所述云台设置在所述无人机上；所述坐标数据与所述角度数据一一对应。
[0051]
具体的，坐标数据可以是wgs84坐标数据，还可以是现有技术中其他能够实现本技术的坐标数据。其中，wgs84坐标系是gps定位用的坐标系，由(经度，纬度，高度)组成。无人机的坐标数据为目前无人机所取得的的所有坐标数据，每个坐标数据对应一个云台的角度数据。
[0052]
此外，无人机的坐标数据和云台的角度数据的获取方式为实时获取，即在无人机环绕目标飞行的过程中实时获取无人机的坐标数据和云台的角度数据，并实时运算。
[0053]
s202、根据所述坐标数据和所述角度数据确定出多条第一直线。
[0054]
具体的，可以以wgs84坐标为端点，以wgs84坐标对应的角度为方向可以得到一条以所述无人机位置为起点，以固定在所述云台上的相机的视线角度为方向的射线，如此，根据无人机的坐标数据和云台的角度数据确定出所有的射线。
[0055]
s203、确定出距离所有所述第一直线最短的第一目标点。
[0056]
详细的，在实际工作环境中，由于多种原因的影响，比如云台的抖动、云台的转向速度、无人机的位置偏移和相机的跟踪速度等原因，多条射线往往难以汇聚到同一个交点。因此，确定出距离所有所述第一直线最短的第一目标点，该第一目标点为目标最可能存在的位置。
[0057]
s204、将所述第一目标点的坐标定义为待测物体的位置坐标。
[0058]
本实施例采用以上技术方案，一种通过无人机位置和云台角度估测目标坐标的方法，包括：获取无人机的坐标数据和云台的角度数据；所述云台设置在所述无人机上；所述坐标数据与所述角度数据一一对应；根据所述坐标数据和所述角度数据确定出多条第一直线；确定出距离所有所述第一直线最短的第一目标点；将所述第一目标点的坐标定义为待测物体的位置坐标。基于此，本实施例仅通过无人机、摄像头和图像处理设备便可完成对目标坐标的估测，具有成本低和估测过程简单便捷的优点。
[0059]
优选的，当获取到的无人机的坐标数据为wgs84坐标数据时，所述根据所述坐标数据和所述角度数据确定出多条第一直线，包括：
[0060]
将所述wgs84坐标数据转换为笛卡尔坐标数据；
[0061]
根据所述笛卡尔坐标数据和所述角度数据确定出多条第一直线。
[0062]
具体的，要从wgs84坐标系转换为笛卡尔坐标系，首先要确定一个坐标作为笛卡尔坐标系的原点，这个点可以是空间中任意一个点。具体的转换过程如下：
[0063]
1、将array1中的经度longi换算为array2中的x。详细的，经度所在的半径radius恒等于赤道半径radius_true(假设地球是正球体)，然后计算wgs84坐标与原点坐标的角度差θ，即array1(1)
‑
base(1),将该角度差θ转换为弧度制后与radius相乘得到x。
[0064]
2、将array1中的纬度latit换算为array2中的y。详细的，纬度所在的半径radius取决于纬度本身，所以需要先计算当前纬度半径radius_now，然后将当前纬度转换为弧度制，得到当前纬度的余弦，最后，将当前纬度的余弦乘赤道半径radius_true得到当前维度半径radius_now。接下来计算wgs84坐标与原点坐标的角度差θ，即array1(2)
‑
base(2),将该角度差θ转换为弧度制后与radius相乘得到y。
[0065]
3、将array1中的纬度altit换算为array2中的z，即array1(3)
‑
base(3)。
[0066]
需要说明的是，本说明书中，所有加粗字体对应的变量名(例如：array1)表示向量数组，非加粗字体对应的变量名(例如：longi)表示标量。wgs84坐标系到笛卡尔坐标系的转换函数，输入为wgs84坐标系数组array1＝[longi1,latit1,altit1；longi2,latit2,altit2。...],转换用坐标原点base＝[longi1,latit1,altit1]，输出为笛卡尔坐标系数组array2＝[x1,y1,z1；x2,y2,z2；...]。array1需与array2长度相同。
[0067]
优选的，所述笛卡尔坐标数据对应的笛卡尔坐标系的原点为所述无人机取得的第一个wgs84坐标点，如此，计算之后的笛卡尔坐标系的第一个点永远是[0,0,0]。
[0068]
优选的，确定出距离所有所述第一直线最短的第一目标点，包括：
[0069]
1、通过如下计算公式计算出空间中任意一点p到所有所述第一直线的距离之和：
[0070][0071]
其中，d(p；x,v)表示距离之和；x
k
表示第k条第一直线对应的所述无人机的笛卡尔坐标数据；v
k
表示第k条第一直线对应的所述云台的角度数据。
[0072]
详细的，第一直线为一条用y＝x+vt描述的射线，其中，x＝[x1；x2；x3]是一个描述射线端点的向量，v＝[v1；v2；v3]是一个描述射线方向的向量。空间内任意一个点p＝[p1；p2；p3]到射线y的距离是d(p；x,v)。在最小二乘法中，假如射线有k条，那么点p到所有k条射线的距离之和是d(p；x,v)。
[0073]
2、通过如下计算公式计算所述距离之和的最小值，得到距离所有所述第一直线最短的第一目标点：
[0074][0075]
具体的，将上述计算所述距离之和的最小值的计算公式整理成线性方程组的格式ap＝b后，得到如下计算公式：
[0076][0077][0078]
最后，通过p＝b\a，可以得到离所有射线距离最近的点p的坐标，即第一目标点的坐标。
[0079]
在实际应用过程中，当无人机移动速度较快时，不能保证追踪的目标始终在图像的中心点位置。此时，仅凭云台角度确定出射线是不准确的，需要根据目标在图像中的实际位置和相机的视场角dfov，计算云台的补偿角度。如图3所示，相机的视场角dfov即∠pom，补偿角度即∠aob。
[0080]
具体的，所述根据所述坐标数据和所述角度数据确定出多条第一直线之前，本实施例的通过无人机位置和云台角度估测目标坐标的方法还包括：
[0081]
获取固定在所述云台上的相机的相机参数和所述相机画面中的目标框中心点坐标；所述相机参数包括焦距、视场角和相机画面的像素；
[0082]
根据所述相机参数和所述相机画面中的目标框中心点坐标确定所述云台的各角度数据对应的补偿角度；
[0083]
根据所述云台的各角度数据对应的补偿角度对应修正所述云台的角度数据，得到修正后的所述云台的角度数据；
[0084]
所述根据所述坐标数据和所述角度数据确定出多条第一直线，包括：
[0085]
根据所述坐标数据和修正后的所述云台的角度数据确定出多条第一直线。
[0086]
更具体的，所述相机当前画面中的目标框中心点坐标即图3中的点b的位置坐标。无人机的云台角度l0s数组是由偏航角yaw＝[yaw1；yaw2；...]和俯仰角pitch＝[pitch1；pitch2；...]组成的，los＝[yaw,pitch]，因此计算得到的补偿角度也要符合这个格式，即
补偿角角度ca＝[dx,dy]，其中，画面中横向的补偿量dx＝[dx1；dx2；...]对应偏航角yaw,画面中纵向的补偿量dy＝[dy1；dy2；...]对应俯仰角pitch。
[0087]
补偿角度具体的计算方法如下：
[0088]
1、获取相机的焦距f，相机的视场角dfov，相机画面的像素[xmax,ymax],图像算法给出的目标坐标fov＝[x,y](即相机画面中的目标框中心点坐标)。
[0089]
2、根据如下计算公式计算相机画面的对角线长度：
[0090]
dmax＝sqrt(xmax^2+ymax^2)。
[0091]
3、根据如下计算公式分别计算dx补偿量和dy补偿量。
[0092][0093][0094]
如此，得到补偿角角度ca＝[dx,dy]。
[0095]
根据补偿角度修正所述云台的角度数据，得到修正后的所述云台的角度数据如下：
[0096]
a＝los+ca，a＝[yaw,pitch]。
[0097]
用修正后的所述云台的角度数据，可以得到射线y射线方向v，v＝[sin(a(1)),cos(a(1)),tan(a(2))]。
[0098]
图4是本发明实施例提供的另一种通过无人机位置和云台角度估测目标坐标的方法的流程示意图。如图4所示，本实施例的通过无人机位置和云台角度估测目标坐标的方法包括：
[0099]
s401、获取无人机的坐标数据和云台的角度数据；所述云台设置在所述无人机上；所述坐标数据与所述角度数据一一对应；
[0100]
s402、根据所述坐标数据和所述角度数据确定出多条第一直线；
[0101]
s403、确定出距离所有所述第一直线最短的第一目标点；
[0102]
s404、采集所述无人机的当前坐标数据、所述云台的当前角度数据、固定在所述云台上的相机的相机参数和所述相机当前画面中的目标框中心点坐标；
[0103]
s405、根据所述相机参数和所述相机当前画面中的目标框中心点坐标确定所述云台的当前角度数据对应的补偿角度；
[0104]
s406、根据所述云台的当前角度数据对应的补偿角度修正所述云台的当前角度数据，得到修正后的所述云台的当前角度数据；
[0105]
s407、根据所述当前坐标数据和修正后的所述云台的当前角度数据确定出对应的第二直线；
[0106]
s408、确定出距离所有所述第一直线和所述第二直线最短的第二目标点；
[0107]
s409、判断所述第一目标点的位置坐标和所述第二目标点的位置坐标是否满足预
设收敛条件；当所述第一目标点的位置坐标和所述第二目标点的位置坐标满足预设收敛条件时，执行步骤s410，当所述第一目标点的位置坐标和所述第二目标点的位置坐标不满足预设收敛条件时，执行步骤s404；
[0108]
s410、将所述第一目标点的坐标定义为待测物体的位置坐标。
[0109]
具体的，预设收敛条件为p(t)
‑
p(t
‑
1)<tol,其中，tol为可被接受的错误界限值。通过上述方法，可以更加精确的确定目标的位置坐标。
[0110]
基于一个总的发明构思，本发明还提供了一种通过无人机位置和云台角度估测目标坐标的系统。如图5所示，本实施例的通过无人机位置和云台角度估测目标坐标的系统，包括：无人机51、云台52、相机53和图像处理设备54；
[0111]
所述云台52设置在所述无人机51上，用于固定所述相机53；
[0112]
所述图像处理设备54与所述相机53连接，用于获取所述相机53采集的图像，并根据所述图像实现本发明上述实施例所述的通过无人机位置和云台角度估测目标坐标的方法。
[0113]
需要说明的是，本实施例和上述实施例基于一个总的发明构思，具备相同或相应的执行过程和有益效果，在此不再赘述。
[0114]
可以理解的是，上述各实施例中相同或相似部分可以相互参考，在一些实施例中未详细说明的内容可以参见其他实施例中相同或相似的内容。
[0115]
需要说明的是，在本发明的描述中，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性。此外，在本发明的描述中，除非另有说明，“多个”的含义是指至少两个。
[0116]
流程示意图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。
[0117]
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。
[0118]
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
[0119]
此外，在本发明各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。
[0120]
上述提到的存储介质可以是只读存储器，磁盘或光盘等。
[0121]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
[0122]
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。