CN108009124A - 旋转矩阵计算方法及装置 - Google Patents

Abstract

本发明涉及无人机技术领域，提供一种旋转矩阵计算方法及装置，应用于无人机的软云台，所述软云台上安装有摄像装置，所述方法包括：利用摄像装置的透视锥体，计算软云台的投影矩阵；根据无人机的飞行姿态角，计算软云台的视图矩阵；根据摄像装置的安装角度与飞行姿态角，计算软云台的模型矩阵；将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵。本发明实施例可以准确、快速地计算出旋转矩阵，具有良好的实用性。

Description

旋转矩阵计算方法及装置

技术领域

本发明涉及无人机技术领域，具体而言，涉及一种旋转矩阵计算方法及装置。

背景技术

随着控制理论的不断发展，无人机也受到各国研究者的关注，世界上产生了各式各样的无人机。目前无人机的云台的姿态表述方法可分为：欧拉角、四元数和旋转矩阵，四元数避免了欧拉角所带有的奇异性问题，但欧拉角表述姿态更直观；四元数与旋转矩阵相比，四元数在进行向量旋转的时候没有旋转矩阵方便。因此，准确快速地计算旋转矩阵对表述机载云台的姿态具有重要意义。

发明内容

本发明实施例的目的在于提供一种旋转矩阵计算方法及装置，用以改善上述问题。

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

第一方面，本发明实施例提供了一种旋转矩阵计算方法，应用于无人机的软云台，所述软云台上安装有摄像装置，所述方法包括：利用所述摄像装置的透视锥体，计算所述软云台的投影矩阵；根据所述无人机的飞行姿态角，计算所述软云台的视图矩阵；根据所述摄像装置的安装角度与所述飞行姿态角，计算所述软云台的模型矩阵；将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵。

第二方面，本发明实施例还提供了一种旋转矩阵计算装置，应用于无人机的软云台，所述软云台上安装有摄像装置，所述装置包括投影矩阵计算模块、视图矩阵计算模块、模型矩阵计算模块及旋转矩阵计算模块。其中，投影矩阵计算模块用于利用所述摄像装置的透视锥体，计算所述软云台的投影矩阵；视图矩阵计算模块用于根据所述无人机的飞行姿态角，计算所述软云台的视图矩阵；模型矩阵计算模块用于根据所述摄像装置的安装角度与所述飞行姿态角，计算所述软云台的模型矩阵；旋转矩阵获得模块用于将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵。

相对现有技术，本发明实施例提供的一种旋转矩阵计算方法及装置，通过利用摄像装置的透视锥体，计算软云台的投影矩阵；并根据无人机的飞行姿态角，计算软云台的视图矩阵；再根据摄像装置的安装角度与飞行姿态角，计算软云台的模型矩阵；最后将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵。本发明实施例可以准确、快速地计算出旋转矩阵，具有良好的实用性。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1示出了本发明实施例提供的无人机的方框示意图。

图2示出了本发明实施例提供的旋转矩阵计算方法流程图。

图3为图2示出的步骤S102的子步骤流程图。

图4为图2示出的步骤S103的子步骤流程图。

图5示出了本发明实施例提供的旋转矩阵计算装置的方框示意图。

图标：100-无人机；101-存储器；102-存储控制器；103-处理器；104-外设接口；105-软云台；106-摄像装置；200-旋转矩阵计算装置；201-投影矩阵计算模块；202-视图矩阵计算模块；203-模型矩阵计算模块；204-旋转矩阵获得模块。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本发明的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

请参照图1，图1示出了本发明实施例提供的无人机100的方框示意图。无人机100可以是，但不限于固定翼无人机、无人直升机和多旋翼无人机、伞翼无人机、扑翼无人机和无人飞船等等。所述无人机100包括旋转矩阵计算装置200、存储器101、存储控制器102、处理器103、外设接口104、软云台105和摄像装置106。

所述存储器101、存储控制器102、处理器103、外设接口104、软云台105和摄像装置106各元件相互之间直接或间接地电性连接，以实现数据的传输或交互。例如，这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。所述旋转矩阵计算装置200包括至少一个可以软件或固件(firmware)的形式存储于所述存储器101中或固化在所述无人机100的操作系统(operating system，OS)中的软件功能模块。所述处理器103用于执行存储器101中存储的可执行模块，例如所述旋转矩阵计算装置200包括的软件功能模块或计算机程序。

其中，存储器101可以是，但不限于，随机存取存储器(Random Access Memory，RAM)，只读存储器(Read Only Memory，ROM)，可编程只读存储器(Programmable Read-OnlyMemory，PROM)，可擦除只读存储器(Erasable Programmable Read-Only Memory，EPROM)，电可擦除只读存储器(Electric Erasable Programmable Read-Only Memory，EEPROM)等。其中，存储器101用于存储程序，所述处理器103在接收到执行指令后，执行所述程序。

处理器103可以是一种集成电路芯片，具有信号处理能力。上述的处理器103可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(NetworkProcessor，NP)、语音处理器以及视频处理器等；还可以是数字信号处理器、专用集成电路、现场可编程门阵列或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器103也可以是任何常规的处理器等。

所述外设接口104用于将各种输入/输出装置耦合至处理器103以及存储器101。在一些实施例中，外设接口104、处理器103以及存储控制器102可以在单个芯片中实现。在其他一些实例中，他们可以分别由独立的芯片实现。

所述软云台105与处理器103电性连接，软云台105上安装有摄像装置106，且摄像装置106与处理器103电性连接。

第一实施例

请参照图2，图2示出了本发明实施例提供的旋转矩阵计算方法流程图。旋转矩阵计算方法包括以下步骤：

步骤S101，利用摄像装置的透视锥体，计算软云台的投影矩阵。

在本发明实施例中，利用摄像装置106的透视锥体，根据第一公式计算投影矩阵，其中，l、r、t、b、n、f均为透视锥体的参数，l、r、t、b、n、f分别表示左、右、上、下、近、远这六个透视锥体的参数。

步骤S102，根据无人机的飞行姿态角，计算软云台的视图矩阵。

在本发明实施例中，软云台105的视图矩阵由一个点和两个向量来描述，分别是眼睛点(eye)、目标向量(target)和向上向量(up)，软云台105的视图矩阵的计算方法可以是，但不限于首先，获取无人机100的飞行姿态角，飞行姿态角包括偏航角(yaw)、俯仰角(pitch)和滚转角(roll)；然后，设定眼睛点(eye)为E(0,0,0)，目标向量(target)为T(1,0,0)，向上向量(up)为U(0,0,-1)，并根据飞行姿态角，利用公式计算四元数；最后，利用公式将四元数转换为视图矩阵，具体公式为：

请参照图3，步骤S102可以包括以下子步骤：

子步骤S1021，获取无人机的飞行姿态角。

在本发明实施例中，飞行姿态角包括偏航角(yaw)、俯仰角(pitch)和滚转角(roll)。

子步骤S1022，根据飞行姿态角，利用第二公式计算四元数，其中，第二公式为yaw、pitch、roll分别为偏航角、俯仰角、滚转角。

子步骤S1023，利用第三公式，将四元数转换为视图矩阵，其中，第三公式为

步骤S103，根据摄像装置的安装角度与飞行姿态角，计算软云台的模型矩阵。

在本发明实施例中，软云台105的模型矩阵由摄像装置106与水平面Y轴的安装角度(θ)和无人机100的飞行姿态角(yaw，pitch，roll)进行计算。软云台105的模型矩阵的计算方法可以是，但不限于首先，利用步骤S102中的方法，把无人机100的飞行姿态角(yaw，pitch，roll)转换成四元数，再将四元数转换为矩阵M；然后，设水平Y轴为向量p(0,1,0)，水平Y轴经过姿态变换之后得到向量p'，p′＝M*p；再获取摄像装置106相对于水平Y轴的安装角度θ，根据公式计算围绕向量p(0,1,0)旋转的第一矩阵为M_r，计算公式为其中，r为向量p(0,1,0)的单位向量；第三，设围绕向量p'旋转θ角度的第二矩阵为M_r'，计算公式为其中r'为向量p'的单位向量；最后，根据公式M_m＝M_r'*M_r计算模型矩阵。

请参照图4，步骤S103可以包括以下子步骤：

子步骤S1031，获取摄像装置相对于水平轴的安装角度。

在本发明实施例中，水平轴可以是，但不限于水平Y轴，摄像装置106与水平面Y轴的安装角度为θ。

子步骤S1032，利用安装角度和飞行姿态数据，根据第四公式计算第一矩阵，其中，第四公式为其中，r为水平轴的单位向量。

在本发明实施例中，设水平Y轴为向量p(0,1,0)，r为向量p(0,1,0)的单位向量。

子步骤S1033，利用安装角度和飞行姿态数据，根据第五公式计算第二矩阵，其中，第五公式为其中，r'为水平轴经过姿态变换之后得到的单位向量。

在本发明实施例中，设水平Y轴为向量p(0,1,0)，水平Y轴经过姿态变换之后得到向量p'，p′＝M*p，r'为向量p'的单位向量。

子步骤S1034，根据第一矩阵和第二矩阵，利用第六公式M_m＝M_r'*M_r计算模型矩阵。

步骤S104，将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵。

在本发明实施例中，利用第五公式M＝M_p*M_v*M_m，将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵，其中，M_p为投影矩阵，M_v为视图矩阵，M_m为模型矩阵。

第二实施例

请参照图5，图5示出了本发明实施例提供的旋转矩阵计算装置200的方框示意图。旋转矩阵计算装置200包括投影矩阵计算模块201、视图矩阵计算模块202、模型矩阵计算模块203及旋转矩阵获得模块204。

投影矩阵计算模块201，用于利用摄像装置的透视锥体，计算软云台的投影矩阵。

在本发明实施例中，投影矩阵计算模块201可以用于执行步骤S101。

视图矩阵计算模块202，用于根据无人机的飞行姿态角，计算软云台的视图矩阵。

在本发明实施例中，视图矩阵计算模块202可以用于执行步骤S102。

在本发明实施例中，视图矩阵计算模块202还可以用于执行步骤S102的子步骤S1021～S1023。

模型矩阵计算模块203，用于根据摄像装置的安装角度与飞行姿态角，计算软云台的模型矩阵。

在本发明实施例中，模型矩阵计算模块203可以用于执行步骤S103。

在本发明实施例中，模型矩阵计算模块203还可以用于执行步骤S103的子步骤S1031～S1034。

旋转矩阵获得模块204，用于将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵。

在本发明实施例中，旋转矩阵获得模块204可以用于执行步骤S104。

综上所述，本发明提供的一种旋转矩阵计算方法及装置，应用于无人机的软云台，所述软云台上安装有摄像装置，所述方法包括：利用摄像装置的透视锥体，计算软云台的投影矩阵；根据无人机的飞行姿态角，计算软云台的视图矩阵；根据摄像装置的安装角度与飞行姿态角，计算软云台的模型矩阵；将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵。本发明实施例可以准确、快速地计算出旋转矩阵，具有良好的实用性。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

Claims (10)

1.一种旋转矩阵计算方法，其特征在于，应用于无人机，且所述无人机包括软云台，所述软云台上安装有摄像装置，所述方法包括：

利用所述摄像装置的透视锥体，计算所述软云台的投影矩阵；

根据所述无人机的飞行姿态角，计算所述软云台的视图矩阵；

根据所述摄像装置的安装角度与所述飞行姿态角，计算所述软云台的模型矩阵；

将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵。

2.如权利要求1所述的方法，其特征在于，所述利用透视投影，计算所述软云台的投影矩阵的步骤，包括：

利用所述摄像装置的透视锥体，根据第一公式计算所述投影矩阵，其中，l、r、t、b、n、f均为所述透视锥体的参数。

3.如权利要求1所述的方法，其特征在于，所述根据无人机的飞行姿态角，计算所述软云台的视图矩阵的步骤，包括：

获取所述无人机的飞行姿态角；

根据所述飞行姿态角，利用第二公式计算四元数，其中，所述第二公式为yaw、pitch、roll分别为偏航角、俯仰角、滚转角；

利用第三公式，将四元数转换为视图矩阵，其中，所述第三公式为

4.如权利要求1所述的方法，其特征在于，所述根据摄像装置的安装角度与所述飞行姿态角，计算所述软云台的模型矩阵的步骤，包括：

获取所述摄像装置相对于水平轴的安装角度；

利用所述安装角度和所述飞行姿态数据，根据第四公式计算第一矩阵，其中，所述第四公式为其中，r为水平轴的单位向量；

利用所述安装角度和所述飞行姿态数据，根据第五公式计算第二矩阵，其中，所述第五公式为其中，r'为水平轴经过姿态变换之后得到的单位向量；

根据所述第一矩阵和所述第二矩阵，利用第六公式M_m＝M′_r*M_r计算所述模型矩阵。

5.如权利要求1所述的方法，其特征在于，所述将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵的步骤，包括：

利用第五公式M＝M_p*M_v*M_m，将投影矩阵、视图矩阵和模型矩阵相乘，得到所述旋转矩阵，其中，M_p为投影矩阵，M_v为视图矩阵，M_m为模型矩阵。

6.一种旋转矩阵计算装置，其特征在于，应用于无人机的软云台，所述软云台上安装有摄像装置，所述装置包括：

投影矩阵计算模块，用于利用所述摄像装置的透视锥体，计算所述软云台的投影矩阵；

视图矩阵计算模块，用于根据所述无人机的飞行姿态角，计算所述软云台的视图矩阵；

模型矩阵计算模块，用于根据所述摄像装置的安装角度与所述飞行姿态角，计算所述软云台的模型矩阵；

旋转矩阵获得模块，用于将投影矩阵、视图矩阵和模型矩阵相乘，得到旋转矩阵。

7.如权利要求6所述的装置，其特征在于，所述投影矩阵计算模块还用于：

利用所述摄像装置的透视锥体，根据第一公式计算所述投影矩阵，其中，l、r、t、b、n、f均为所述透视锥体的参数。

8.如权利要求6所述的装置，其特征在于，所述视图矩阵计算模块还用于：

获取所述无人机的飞行姿态角；

根据所述飞行姿态角，利用第二公式计算四元数，其中，所述第二公式为yaw、pitch、roll分别为偏航角、俯仰角、滚转角；

利用第三公式，将四元数转换为视图矩阵，其中，所述第三公式为

9.如权利要求6所述的装置，其特征在于，所述模型矩阵计算模块还用于：

获取所述摄像装置相对于水平轴的安装角度；

利用所述安装角度和所述飞行姿态数据，根据第四公式计算第一矩阵，其中，所述第四公式为其中，r为水平轴的单位向量；

利用所述安装角度和所述飞行姿态数据，根据第五公式计算第二矩阵，其中，所述第五公式为其中，r'为水平轴经过姿态变换之后得到的单位向量；

根据所述第一矩阵和所述第二矩阵，利用第六公式M_m＝M′_r*M_r计算所述模型矩阵。

10.如权利要求6所述的装置，其特征在于，所述旋转矩阵获得模块还用于：

利用第五公式M＝M_p*M_v*M_m，将投影矩阵、视图矩阵和模型矩阵相乘，得到所述旋转矩阵，其中，M_p为投影矩阵，M_v为视图矩阵，M_m为模型矩阵。