确定无人机位置和飞行方向的方法、装置及无人机与流程

本发明涉及无人机技术领域，尤其涉及确定无人机位置和飞行方向的方法、装置及无人机。

背景技术：

无人机已被广泛应用于航拍摄影、电力巡检、环境监测、森林防火、灾情巡查、防恐救生、军事侦察、战场评估等领域，有效克服了有人驾驶飞机空中作业的不足，降低了购买与维护成本，提高了运载工具的安全性。

在无人机进行空中作业时，需要得知无人机的实时位置，以便对无人机进行控制。

技术实现要素：

本发明提出确定无人机位置和飞行方向的方法、装置及无人机，以提高对无人机定位的精度。

本发明的技术方案是这样实现的：

根据无人机的云台搭载的相机拍摄的包含m个固定点的图像，计算m个固定点在相机坐标系下的坐标，m≥4；

根据m个固定点在世界坐标系下的坐标和在相机坐标系下的坐标，计算相机坐标系到世界坐标系的转换关系；

根据相机拍摄到包含m个固定点的图像时云台相对无人机的旋转角度，计算无人机坐标系到相机坐标系的转换关系；

根据相机坐标系到世界坐标系的转换关系，以及无人机坐标系到相机坐标系的转换关系，计算无人机在世界坐标系中的位置坐标以及无人机在世界坐标系中的飞行方向向量。

通过上述实施例，可同时计算出无人机在世界坐标系中的位置坐标以及无人机在世界坐标系中的飞行方向向量，从而能够同时得到无人机的位置和飞行方向，提高了对无人机定位的精度。

所述计算相机坐标系到世界坐标系的转换关系包括：

计算相机坐标系到世界坐标系的旋转矩阵r和平移向量t；

所述计算无人机坐标系到相机坐标系的转换关系包括：

计算无人机坐标系到相机坐标系的旋转矩阵q；

所述计算无人机在世界坐标系中的位置坐标以及无人机在世界坐标系中的飞行方向向量包括：

确定无人机在世界坐标系中的位置坐标为平移向量t，以及计算出无人机在世界坐标系中的飞行方向向量为：无人机在无人机坐标系中的飞行方向向量与旋转矩阵q和旋转矩阵r的乘积。

当采用无人机的正前方作为无人机在无人机坐标系中的飞行方向向量时，无人机在无人机坐标系中的飞行方向向量为[0,0,1]。

所述云台相对无人机的旋转角度包括：翻滚角θ、俯仰角ф和偏航角

所述计算无人机坐标系到相机坐标系的转换关系包括：

根据如下原则：将无人机先绕偏航角对应轴旋转角度、再绕俯仰角对应轴旋转ф角度、最后绕翻滚角对应轴旋转θ角度，构造无人机坐标系到相机坐标系的旋转矩阵q。

所述旋转矩阵q符合以下公式要求：

一种确定无人机位置和飞行方向的装置，该装置应用于无人机，所述无人机具有搭载相机和角度传感器的云台，该装置包括：

旋转角度获取模块，用于从角度传感器获取相机拍摄到包含m个固定点的图像时云台相对无人机的旋转角度，m≥4；

固定点坐标计算模块，用于根据相机拍摄到的包含m个固定点的图像，计算m个固定点在相机坐标系下的坐标；

第一坐标系转换关系计算模块，用于根据m个固定点在世界坐标系下的坐标和在相机坐标系下的坐标，计算相机坐标系到世界坐标系的转换关系；

第二坐标系转换关系计算模块，用于根据相机相对无人机的旋转角度，计算无人机坐标系到相机坐标系的转换关系；

无人机位置和飞行方向计算模块，用于根据相机坐标系到世界坐标系的转换关系，以及无人机坐标系到相机坐标系的转换关系，计算无人机在世界坐标系中的位置坐标以及无人机在世界坐标系中的飞行方向向量。

所述第一坐标系转换关系计算模块计算相机坐标系到世界坐标系的转换关系包括：

计算相机坐标系到世界坐标系的旋转矩阵r和平移向量t；

所述第二坐标系转换关系计算模块计算无人机坐标系到相机坐标系的转换关系包括：

计算无人机坐标系到相机坐标系的旋转矩阵q；

所述无人机位置和飞行方向计算模块计算无人机在世界坐标系中的位置坐标以及无人机在世界坐标系中的飞行方向向量包括：

确定无人机在世界坐标系中的位置坐标为平移向量t，以及计算出无人机在世界坐标系中的飞行方向向量为：无人机在无人机坐标系中的飞行方向向量与旋转矩阵q和旋转矩阵r的乘积。

当所述无人机位置和飞行方向计算模块采用无人机的正前方作为无人机在无人机坐标系中的飞行方向向量时，无人机在无人机坐标系中的飞行方向向量为[0,0,1]。

所述旋转角度获取模块获取的云台相对无人机的旋转角度包括：翻滚角θ、俯仰角ф和偏航角

所述第二坐标系转换关系计算模块计算无人机坐标系到相机坐标系的转换关系包括：

根据如下原则：将无人机先绕偏航角对应轴旋转角度、再绕俯仰角对应轴旋转ф角度、最后绕翻滚角对应轴旋转θ角度，构造无人机坐标系到相机坐标系的旋转矩阵q。

所述旋转矩阵q符合以下公式要求：

一种非瞬时计算机可读存储介质，所述非瞬时计算机可读存储介质存储指令，所述指令在由处理器执行时使得所述处理器执行如上任一项所述的确定无人机位置和飞行方向的方法的步骤。

一种无人机，包括：飞行控制器以及搭载有相机和角度传感器的云台，所述飞行控制器执行如上任一所述的确定无人机位置和飞行方向的方法。

本发明通过根据相机拍摄的包含m个固定点的图像，m≥4，计算相机坐标系到世界坐标系的转换关系，并根据相机相对无人机的旋转角度，计算无人机坐标系到相机坐标系的转换关系，从而得到无人机在世界坐标系中的位置坐标以及无人机在世界坐标系中的飞行方向向量，从而能够同时得到无人机的位置和飞行方向，提高了对无人机定位的精度。

附图说明

图1为本发明一实施例提供的确定无人机位置和飞行方向的方法流程图；

图2为本发明另一实施例提供的确定无人机位置和飞行方向的方法流程图；

图3为本发明实施例提供的确定无人机位置和飞行方向的装置的结构示意图。

具体实施方式

下面结合附图及具体实施例对本发明再作进一步详细的说明。

相关技术提出了基于二维标签的无人机定位及目标跟踪方法。该方法是在无人机上搭载相机、云台，将相机对准一个二维标签，已知二维标签在世界坐标系中的位置，通过二维标签在相机中的像的位置来计算相机在世界坐标系中的位置，并将相机在世界坐标系中的位置作为无人机的坐标，其步骤如下：

a、对相机进行标定。根据相机拍摄的图像，标定出相机的内参数矩阵其中，fx、fy为相机的焦距，u0、v0为相机的光心。

b、进行二维标签的检测。在云台相机拍摄的图像中识别出二维标签，得到二维标签在图像中的坐标[x，y，1]，x、y为用像素值表示的横、纵坐标。

c、进行二维标签的定位。根据相机内参数及二维标签在图像中的坐标，计算出相机的外参数，计算公式为：

其中，s表示一个比例系数；等式右边第二个矩阵为相机的外参数矩阵，即从世界坐标系到图像中的坐标系的变换矩阵，其中，r1～r9表征两个坐标系之间的旋转关系，t1-t3表征两个坐标系之间的平移关系，[x，y，z，1]为二维标签在世界坐标系中的坐标。

d、通过相机的外参数计算得到相机在世界坐标系中的坐标，即无人机的坐标。

e、以二维标签在画面中的移动速度和二维标签相对无人机的距离为输入，控制相机始终对准二维标签。

上述方法的缺点是，将无人机坐标等价于相机坐标，而未考虑到相机方向到无人机方向的转换，而实际上，相机方向要经过旋转才能得到无人机方向。

图1为本发明一实施例提供的确定无人机位置和飞行方向的方法流程图，其具体步骤如下：

步骤101：无人机的飞行控制器控制无人机的云台旋转，以使得云台搭载的相机拍摄周围环境中预设的m个固定点，m≥4。

步骤102：无人机的飞行控制器确定相机拍摄到预设的m个固定点，则从云台搭载的角度传感器获取云台相对无人机的旋转角度。

步骤103：无人机的飞行控制器根据相机拍摄的包含m个固定点的图像，计算m个固定点在相机坐标系下的坐标。

步骤104：无人机的飞行控制器根据m个固定点在世界坐标系下的坐标和在相机坐标系下的坐标，计算相机坐标系到世界坐标系的转换关系。

步骤105：无人机的飞行控制器根据相机相对无人机的旋转角度，计算无人机坐标系到相机坐标系的转换关系。

步骤106：无人机的飞行控制器根据相机坐标系到世界坐标系的转换关系，以及无人机坐标系到相机坐标系的转换关系，计算无人机在世界坐标系中的位置坐标以及无人机在世界坐标系中的飞行方向向量。

如此，通过无人机云台搭载的相机拍摄到的预设的m个固定点的图像以及拍摄到m个固定点时云台相对无人机的旋转角度，计算出相机坐标系到世界坐标系的转换关系、无人机坐标系到相机坐标系的转换关系，从而计算出无人机在世界坐标系中的位置坐标以及无人机在世界坐标系中的飞行方向向量，从而能够同时得到无人机的位置和飞行方向，提高了对无人机定位的精度。

以下对相机坐标系到世界坐标系的转换，以及无人机坐标系到相机坐标系的转换给出详细说明：

图2为本发明另一实施例提供的确定无人机位置和飞行方向的方法流程图，其具体步骤如下：

步骤201：预先在无人机的飞行控制器上存储周围环境中的m个固定点在世界坐标系下的坐标，m≥4。

这里的“周围环境”即无人机的飞行环境。

计算世界坐标系、相机坐标系、无人机坐标系中两两坐标系之间的转换关系，即相当于计算两个坐标系之间转换的旋转矩阵和平移向量，其中，旋转矩阵为3*3的矩阵，平移向量为3维向量，即共需要求得12个未知量；考虑到每个固定点的坐标是一个三维向量，从而至少需要4个固定点的坐标才能求解包含12个未知量的且能得到唯一解的方程。

在实际应用中，m个固定点可同时位于一个标记物上，该标记物可以是一个平面，该标记物可以是贴在墙上的一张纸、或是一个牌子、或是一个显示屏等。

可以使用标记物上具有明显特征的特征点来直接作为固定点，这样就可以直接根据特征点的特征找到特征点(即固定点)；或者将与特征点有固定关系的点作为固定点，这样先根据特征点的特征找到特征点，然后根据固定点与特征点的关系找到固定点。

例如：标记物上标记了1个或多个特征点，特征点在标记物上的位置必须被精确显示。以4个特征点为例，4个特征点可以连成一个四边形，四边形的4个角即是4个特征点的位置，可将该4个特征点直接作为固定点。或者，也可以用一个四边形来表示一个特征点，四边形的对角线交点为特征点的位置，由于对角线交点与四边形的四个顶点有固定关系，则将四个顶点作为4个固定点。

标记物上使用文字或二维码存储了各个特征点的世界坐标系位置，也可以是与这些信息对应的某个索引标记或索引号(无人机可以根据这个索引标记或索引号获取对应的世界坐标系位置信息)。

标记物上有一个特征用于表示本标记物是定位标记物，该特征可以是一个特殊的图形、或颜色、或标记物本身的形状，或者，就是上文所说的包含了特殊信息的二维码、文字、索引标记/索引号。

当无人机云台上的相机对准该标记物时，可以从标记物上的文字、二维码或索引标记/索引号中读取特征点的世界坐标系位置信息，从而获得m个固定点的世界坐标。

进一步地，相距一定距离的多个标记物可以组合起来用于对飞行范围更大的无人机的定位。

步骤202：当需要获取无人机的位置和飞行方向时，无人机的飞行控制器控制云台旋转，以使得相机拍摄同时包含m个固定点的图像。

具体地，飞行控制器可预先存储m个固定点的特征，如：形状、颜色等识别特征，可控制云台依次在每个方向上按照预设步长转动，在相机拍摄的图像中实时检测是否同时出现了m个固定点，若是，则确定相机拍摄到了同时包含m个固定点的图像。

步骤203：当相机拍摄到同时包含m个固定点的图像时，无人机的飞行控制器从云台搭载的角度传感器获取云台相对无人机的旋转角度：翻滚角(roll)θ、俯仰角(pitch)ф和偏航角(yaw)

步骤204：无人机的飞行控制器根据相机拍摄的包含m个固定点的图像，计算m个固定点在相机坐标系下的坐标。

本步骤属于现有成熟技术，通过相机的内存数(如：焦距等)以及m个固定点在世界坐标系下的坐标和在图像上的坐标，可计算出m个固定点在相机坐标系下的坐标。

步骤205：无人机的飞行控制器根据m个固定点在世界坐标系下的坐标和在相机坐标系下的坐标，计算相机坐标系到世界坐标系的旋转矩阵r和平移向量t。

例如：设任一固定点r(1≤r≤m)在世界坐标系下的坐标为pos_r，在相机坐标下的坐标为c_r，则有方程：pos_r＝c_r×r+t，其中，r为3*3的矩阵，t为3维向量，则将m个固定点的坐标分别代入上述方程进行计算后，就可得到r和t。

步骤206：无人机的飞行控制器根据相机相对无人机的旋转角度：翻滚角θ、俯仰角ф和偏航角计算无人机坐标系到相机坐标系的旋转矩阵q。

例如：可以根据如下原则：将无人机先绕偏航角对应轴旋转角度、再绕俯仰角对应轴旋转ф角度、最后绕翻滚角对应轴旋转θ角度，构造无人机坐标系到相机坐标系的旋转矩阵q；其中，旋转矩阵q可以符合以下公式要求：

步骤207：无人机的飞行控制器根据相机坐标系到世界坐标系的旋转矩阵r和平移向量t，以及无人机坐标系到相机坐标系的旋转矩阵q，得到无人机坐标系到世界坐标系的旋转矩阵为q×r，平移向量为t，则当前无人机在世界坐标系中的位置坐标为平移向量t，当前无人机在世界坐标系中的飞行方向向量为dir＝[0,0,1]×q×r，其中，[0,0,1]为当采用无人机的正前方作为无人机在无人机坐标系中的飞行方向向量时，当前无人机在无人机坐标系中的飞行方向向量。

由于相机固定在无人机上，因此可以认为相机坐标系和无人机坐标系到世界坐标系的平移向量是相同的，而无人机所在的位置就是无人机坐标系的原点，因此，无人机坐标系到世界坐标系的平移向量t即为无人机在世界坐标系中的位置坐标。

在无人机巡航时，可在相机第一次对准m个固定点后控制相机跟踪该固定点，即始终保持相机对准m个固定点，或者，每隔一段时间控制相机对准m个固定点，或者，在接收到针对无人机的定位指示时控制相机对准m个固定点，这样通过本发明提供的技术方案就能够实时或周期性地或按要求获取无人机在世界坐标系中的位置和飞行方向。

图3为本发明实施例提供的确定无人机位置和飞行方向的装置的结构示意图，该装置应用于无人机，该无人机具有搭载相机和角度传感器的云台，该装置主要包括：云台控制模块31、旋转角度获取模块32、固定点坐标计算模块33、第一坐标系转换关系计算模块34、第二坐标系转换关系计算模块35和无人机位置和飞行方向计算模块36，其中：

云台控制模块31，用于控制无人机的云台旋转，以使得相机拍摄周围环境中预设的m个固定点，m≥4。

旋转角度获取模块32，用于从角度传感器获取相机拍摄到m个固定点时云台相对无人机的旋转角度。

固定点坐标计算模块33，用于根据相机拍摄的包含m个固定点的图像，计算m个固定点在相机坐标系下的坐标。

第一坐标系转换关系计算模块34，用于根据预先存储的m个固定点在世界坐标系下的坐标和固定点坐标计算模块33计算得到的m个固定点在相机坐标系下的坐标，计算相机坐标系到世界坐标系的转换关系。

第二坐标系转换关系计算模块35，用于根据旋转角度获取模块32获取的相机相对无人机的旋转角度，计算无人机坐标系到相机坐标系的转换关系。

无人机位置和飞行方向计算模块36，用于根据第一坐标系转换关系计算模块34计算得到的相机坐标系到世界坐标系的转换关系，以及第二坐标系转换关系计算模块35计算得到的无人机坐标系到相机坐标系的转换关系，计算无人机在世界坐标系中的位置坐标以及无人机在世界坐标系中的飞行方向向量。

在实际应用中，第一坐标系转换关系计算模块34计算相机坐标系到世界坐标系的转换关系包括：

计算相机坐标系到世界坐标系的旋转矩阵r和平移向量t；

第二坐标系转换关系计算模块35计算无人机坐标系到相机坐标系的转换关系包括：

计算无人机坐标系到相机坐标系的旋转矩阵q；

无人机位置和飞行方向计算模块36计算无人机在世界坐标系中的位置坐标以及无人机在世界坐标系中的飞行方向向量包括：

确定无人机在世界坐标系中的位置坐标为平移向量t，以及计算出无人机在世界坐标系中的飞行方向向量为：无人机在无人机坐标系中的飞行方向向量与q和r的乘积。

当无人机位置和飞行方向计算模块采用无人机的正前方作为无人机在无人机坐标系中的飞行方向向量时，无人机在无人机坐标系中的飞行方向向量为[0,0,1]。

在实际应用中，旋转角度获取模块32获取的无人机的相机相对无人机的旋转角度包括：翻滚角θ、俯仰角ф和偏航角

第二坐标系转换关系计算模块35根据相机相对无人机的旋转角度，计算无人机坐标系到相机坐标系的转换关系包括：

根据如下原则：将无人机先绕偏航角对应轴旋转角度、再绕俯仰角对应轴旋转ф角度、最后绕翻滚角对应轴旋转θ角度，构造无人机坐标系到相机坐标系的旋转矩阵q。

其中，旋转矩阵q符合以下公式要求：

在实际应用中，云台控制模块31包括：

根据预设的m个固定点的特征，控制云台旋转，以使得相机拍摄同时包含m个固定点的图像。

本发明实施例还提供一种无人机，该无人机包括飞行控制器以及搭载相机和角度传感器的云台，其中，飞行控制器执行如步骤101-106，或者如步骤201-207所述的确定无人机位置和飞行方向的方法。

本发明实施例还提供一种非瞬时计算机可读存储介质，非瞬时计算机可读存储介质存储指令，该指令在由处理器执行时使得处理器执行如步骤101-106，或者如步骤201-207所述的确定无人机位置和飞行方向的方法的步骤。

本发明实施例还提供一种电子设备，包括如上所述的非瞬时计算机可读存储介质、以及可访问非瞬时计算机可读存储介质的上述处理器。

本发明的有益技术效果如下：

可同时得到无人机在世界坐标系中的位置和飞行方向，提高了无人机的定位精度，从而提高了对无人机的控制精度；且由于不采用gps技术进行定位，因此不受gps信号强弱的制约，适用范围更广泛，在户外和室内都可使用。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。