CN105045293B - 云台控制方法、外部载体控制方法及云台 - Google Patents

Abstract

本发明公开了一种云台控制方法、飞行载体控制方法及云台，其方法包括：确定外部载体上的云台所搭载的承载物对地的目标旋转矩阵Rtcn；获取所述承载物对地的当前旋转矩阵Rcn；获取承载物对外部载体的当前旋转矩阵Rcv；根据承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到承载物对外部载体的目标旋转矩阵Rtcv；根据承载物对外部载体的目标旋转矩阵Rtcv，计算获取云台上驱动装置的补偿量，并作用于承载物。本发明能够实时快速的进行姿态补偿，避免了外部环境造成的抖动，保持搭载在云台上的承载物的平稳效果。

Description

云台控制方法、外部载体控制方法及云台

技术领域

本发明涉及云台及其相关技术领域，尤其涉及一种云台控制方法、外部载体控制方法及云台。

背景技术

目前，三轴航拍云台需要挂载在飞行器上，通过飞行器带动航拍云台上所搭载的相机进行航空拍摄。为保持飞行拍摄画面平稳的效果，要求能够时刻使得航拍云台所搭载相机保持一定的对地姿态朝向拍摄位置，并消除飞行器飞行与外部环境造成的画面抖动。因此，要求三轴航拍云台能够实时迅速的补偿飞行平台和外部环境造成的姿态的变化。而现有技术没有较好的解决该问题。

发明内容

本发明的主要目的在于提出一种云台控制方法、外部载体控制方法及云台，旨在实时迅速的补偿外部环境造成的姿态的变化，保持搭载在云台上的承载物的平稳效果。

为实现上述目的，本发明提供的一种云台控制方法，包括：

确定外部载体上的云台所搭载的承载物对地的目标旋转矩阵Rtcn；

获取所述承载物对地的当前旋转矩阵Rcn；

获取所述承载物对外部载体的当前旋转矩阵Rcv；

根据所述承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv；

根据所述承载物对外部载体的目标旋转矩阵Rtcv，计算获取所述云台上驱动装置的补偿量，并作用于所述承载物。

优选地，所述根据所述承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv的步骤包括：

根据所述承载物对地的当前旋转矩阵Rcn、所述承载物对外部载体的当前旋转矩阵Rcv，计算得到外部载体对地的当前旋转矩阵Rvn；

根据所述承载物对地的目标旋转矩阵Rtcn以及所述外部载体对地的当前旋转矩阵Rvn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv。

优选地，所述根据所述承载物对地的当前旋转矩阵Rcn、所述承载物对外部载体的当前旋转矩阵Rcv，计算得到外部载体对地的当前旋转矩阵Rvn的步骤包括：

获取所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc；

根据所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc以及所述承载物对地的当前旋转矩阵Rcn，计算得到外部载体对地的当前旋转矩阵Rvn；

所述根据所述承载物对地的目标旋转矩阵Rtcn以及所述外部载体对地的当前旋转矩阵Rvn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv的步骤包括：

获取所述外部载体对地的当前旋转矩阵Rvn的转置矩阵Rnv；

根据所述外部载体对地的当前旋转矩阵Rvn的转置矩阵Rnv以及承载物对地的目标旋转矩阵Rtcn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv。

优选地，所述驱动装置为PMSM电机；所述外部载体至少为飞行器。

优选地，所述获取所述承载物对外部载体的当前旋转矩阵Rcv的步骤包括：

通过所述承载物的当前位置相对原始位置的对比，并经过控制器的处理得到所述承载物对外部载体的当前旋转矩阵Rcv。

本发明实施例还提出一种云台，包括：

Rtcn确定模块，用于确定外部载体上的云台所搭载的承载物对地的目标旋转矩阵Rtcn；

Rcn获取模块，用于获取所述承载物对地的当前旋转矩阵Rcn；

Rcv获取模块，用于获取所述承载物对外部载体的当前旋转矩阵Rcv；

Rtcv计算模块，用于根据所述承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv；

补偿控制模块，用于根据所述承载物对外部载体的目标旋转矩阵Rtcv，计算获取所述云台上驱动装置的补偿量，并作用于所述承载物。

优选地，所述Rtcv计算模块，还用于根据所述承载物对地的当前旋转矩阵Rcn、所述承载物对外部载体的当前旋转矩阵Rcv，计算得到外部载体对地的当前旋转矩阵Rvn；根据所述承载物对地的目标旋转矩阵Rtcn以及所述外部载体对地的当前旋转矩阵Rvn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv。

优选地，所述Rtcv计算模块，还用于获取所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc；根据所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc以及所述承载物对地的当前旋转矩阵Rcn，计算得到外部载体对地的当前旋转矩阵Rvn；获取所述外部载体对地的当前旋转矩阵Rvn的转置矩阵Rnv；根据所述外部载体对地的当前旋转矩阵Rvn的转置矩阵Rnv以及承载物对地的目标旋转矩阵Rtcn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv。

本发明实施例还提出一种外部载体控制方法，包括：

获取承载物对地的当前旋转矩阵Rcn；

获取所述承载物对外部载体的当前旋转矩阵Rcv；

根据所述承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述外部载体对地的当前旋转矩阵Rvn。

优选地，所述根据所述承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述外部载体对地的当前旋转矩阵Rvn的步骤包括：

获取所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc；

根据所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc以及所述承载物对地的当前旋转矩阵Rcn，计算得到外部载体对地的当前旋转矩阵Rvn。

本发明提出的一种云台控制方法、外部载体控制方法及云台，通过确定外部载体上的云台所搭载的承载物对地的目标旋转矩阵Rtcn；获取承载物对地的当前旋转矩阵Rcn；获取承载物对外部载体的当前旋转矩阵Rcv；根据承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到承载物对外部载体的目标旋转矩阵Rtcv；根据承载物对外部载体的目标旋转矩阵Rtcv，计算获取航拍云台上驱动装置的补偿量，比如电机旋转的补偿角度，并将此补偿量传输到云台的驱动装置运行，从而能够实时快速的进行姿态补偿，避免了外部环境造成的抖动，保持搭载在云台上的承载物的平稳效果。此外，以外部载体为飞行器为例，本发明可以将IMU姿态测量单元设于云台上，与飞行器上的IMU姿态测量单元独立，云台与飞行器不发生通信，使得该云台可兼容任何飞行器等机体，也可用于手持云台中。

附图说明

图1是本发明云台控制方法较佳实施例的流程示意图；

图2是本发明云台较佳实施例的功能模块示意图；

图3是本发明外部载体控制方法较佳实施例的流程示意图；

图4是本发明飞行载体较佳实施例的功能模块示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明实施例的主要解决方案是：通过确定外部载体上的云台所搭载的承载物(比如相机、照明灯、攻击武器等)对地的目标旋转矩阵Rtcn；获取承载物对地的当前旋转矩阵Rcn；获取承载物对外部载体的当前旋转矩阵Rcv；根据承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到承载物对外部载体的目标旋转矩阵Rtcv；根据承载物对外部载体的目标旋转矩阵Rtcv，计算获取云台上驱动装置的补偿量，比如电机旋转的补偿角度，并将此补偿角度传输到云台驱动电机运行，从而能够实时快速的进行承载物的姿态补偿，避免外部环境造成的抖动，保持承载物平稳。

具体地，如图1所示，本发明第一实施例提出一种云台控制方法，包括：

步骤S101，确定外部载体上的云台所搭载的承载物对地的目标旋转矩阵Rtcn；

其中，云台所搭载的承载物包括但不限于相机、照明灯、发射或投射装置，本发明实施例以相机进行举例。

其中，云台可以为一轴、二轴或三轴等云台。用于拍摄的相机搭载在外部载体上，该外部载体可以是飞行器、船体、车体或人体(即该云台为手持云台)。本实施例以三轴航拍云台进行举例，可进行航空拍摄。

为保持飞行拍摄画面平稳的效果，要求能够时刻使得云台所搭载相机保持一定的对地姿态朝向拍摄位置，并消除飞行器飞行与外部环境造成的画面抖动。因此，要求三轴航拍云台能够实时迅速的补偿飞行平台和外部环境造成的姿态的变化。

本实施例在三轴航拍云台上设置有MCU主控单元以及IMU惯性测量单元等执行单元。为了使三轴航拍云台能够迅速补偿外部抖动带来的姿态变化，本发明实施例中使用计算能力较强的MCU主控单元，三轴航拍云台的执行单元上电机优选使用适用于精确定位控制的PMSM电机。由于三轴航拍云台挂载在飞行器外部直接与外部接触，因此保证随时IMU惯性测量单元的稳定工作(主要与外部温度相关)也需要解决。

为此，本实施例采用如下方案：

首先，确定云台所搭载的相机对地的目标姿态，并确定目标姿态对应的目标旋转矩阵Rtcn。

云台的设计目的是为了使得搭载相机能够保持已经的姿态对目标位置进行拍摄，该姿态称为相机的目标姿态。例如，飞行器在空中，需要时刻保持相机镜头相对于地平面45度向下拍摄，则对应的姿态角度为俯仰角pitch45度，横滚角roll与偏航角yaw均为0度。该姿态经过四元素法可转换为3X3的旋转矩阵，称为目标矩阵，命名为Rtcn。

其中，R表示旋转矩阵，c意为camera(相机)，n意为navigation(导航)，v意为vehicle(此处理解为云台载体飞行器)，t意为target目标姿态。Rcn的意义即是：camera相机对navigation导航坐标系(也即对地)的旋转矩阵。每个不同的姿态向量(roll，pitch，yaw)均对应一个3X3的旋转矩阵(下文同此)。

步骤S102，获取所述承载物对地的当前旋转矩阵Rcn；

然后，获取云台所搭载的相机对地的当前姿态，并确定相机对地的当前姿态所对应的当前旋转矩阵Rcn。

如前所述，本实施例在云台MCU主控板上载有IMU惯性测量单元，MCU主控单元通过读取IMU惯性测量单元数据，通过姿态算法能够获取当前相机实际的对导航坐标系(即对地)的姿态，以姿态向量(roll，pitch，yaw)表示。该姿态用四元素算法同样可得到一个3X3的旋转矩阵，称为Rcn。

因为相机时刻受到外部扰动，因此这个姿态/旋转矩阵是实时变动的。为保证该姿态稳定平滑，可以在使用该姿态前进行卡尔曼滤波处理，即对(roll，pitch，yaw)进行卡尔曼滤波处理。

步骤S103，获取所述承载物对外部载体的当前旋转矩阵Rcv；

之后，获取相机当前对飞行器载体的旋转矩阵Rcv(相机对机体坐标系的旋转矩阵)。

由于相机通过云台与飞行器挂载在一起，期间通过三轴云台上的电机运动改变位置。因此，相机相对飞行器载体的姿态事实上就是三轴云台上电机相对起始位置(对地)的旋转角度。这三个角度能够通过MCU主控单元与驱动电机的MCU通讯得知。它同样可对应3X3旋转矩阵，称为Rcv。

步骤S104，根据所述承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv；

然后，根据上述获取到的相机对地的目标旋转矩阵Rtcn、相机对地的当前旋转矩阵Rcn、相机对飞行器载体的当前旋转矩阵Rcv，计算得到相机对飞行器载体的目标旋转矩阵Rtcv，以便对相机姿态进行运动补偿。

首先，根据相机对地的当前旋转矩阵Rcn、相机对外部载体的当前旋转矩阵Rcv，计算得到外部载体对地的当前旋转矩阵Rvn；

然后，根据相机对地的目标旋转矩阵Rtcn以及所述外部载体对地的当前旋转矩阵Rvn，计算得到相机对外部载体的目标旋转矩阵Rtcv。

具体算法原理如下：

首先，介绍旋转矩阵的两个基本性质：

<1>Rab＝T(Rba)：表示从A到B的旋转矩阵等于B到A旋转矩阵的转置；

<2>Rac＝Rab*Rbc：表示从A到C的旋转矩阵等于从A到B的旋转矩阵乘上B到C的旋转矩阵。

根据以上两个性质，本提案使用如下四步算法：

Rvc＝T(Rcv)；

Rvn＝Rvc*Rcn；

Rnv＝T(Rvn)；

Rtcv＝Rtcn*Rnv。

其中，Rcv为相机对机体坐标系的当前旋转矩阵，通过云台的电机的当前位置相对原始位置的对比，并经过控制器的处理得到；

Rvc为相机对机体坐标系的当前旋转矩阵的转置矩阵；

从上述公式可以知道：

在获得Rcv后，通过矩阵的反置公式Rvc＝T(Rcv)，可获得Rvc。

Rcn为相机对地(即导航坐标系)的当前旋转矩阵，通过设于云台上的IMU惯性测量单元测得并经过控制器的处理得到；

从上述公式可以知道：

在获得Rvc和Rcn后，通过矩阵的乘积公式Rvn＝Rvc*Rcn，可获得Rvn(飞行器载体对地(导航坐标系)的当前旋转矩阵)；也即本提案通过设于云台上的IMU惯性测量单元、控制器及电机位置测量单元即可得到飞行器相对地面坐标系(导航坐标系)的当前旋转矩阵Rvn。

Rnv为飞行器载体相对地面坐标系(导航坐标系)的当前旋转矩阵的转置矩阵。

从上述公式可以知道：

在获得Rvn后，通过矩阵的反置公式Rnv＝T(Rvn)，可获得Rnv。

Rtcn为相机对地(即导航坐标系)的目标旋转矩阵；

Rtcv为相机对飞行器(机体坐标系)的目标旋转矩阵，也即相机应该相对飞行器旋转的角度；

从上述公式可以知道：

在获得Rnv及Rtcn后，通过矩阵的乘积公式Rtcv＝Rtcn*Rnv，可获得Rtcv。

优选的，Rtcn所对应的姿态向量(roll，pitch，yaw)与初始位置对应的姿态向量(roll，pitch，yaw)相同。

对上述算法，在此需要说明的是：

Rtcn如前文所述，为相机对地的目标姿态对应的目标旋转矩阵。事实上航拍三轴云台就是为了保证该姿态不变。因此，当目标姿态被用户确定，则该矩阵为固定矩阵。

Rnv导航系对飞行器的旋转矩阵。也就是飞行器对导航系旋转矩阵的转置矩阵。本实施例是通过上述算法倒推出飞行器对导航系姿态，而非从飞行器获取。采用此种实现方案可以保持云台的独立性，可以使使用本提案算法的云台挂载在任何飞行器上使用。因为飞行器在不断运动以及外部风力等原因，这个矩阵是时刻变化的。

步骤S105，根据所述承载物对外部载体的目标旋转矩阵Rtcv，计算获取所述云台上驱动装置的补偿量，并作用于所述驱动装置。

其中，驱动装置可以为云台上的电机或者其他装置，本实施例以电机进行举例。若驱动装置为电机，则驱动装置的补偿量可以为电机旋转的补偿角度。

Rtcv是相机对飞行器的目标旋转矩阵。因为云台与飞行器挂载在一起，因此实际上相机对飞行器的旋转就是云台三轴电机的旋转角度。由于Rtcn为固定量，Rnv为时变量，因此Rtcv＝Rtcn*Rnv也为时变量。

根据上述四步算法，就能通过Rnv的时变计算出Rcv的时变，继而通过Rtcv旋转矩阵与角度的关系计算出该时刻三轴云台电机需要转到的旋转角度，发送给PMSM电机执行单元运行。因此，就可以保证相机无论遇到什么外部扰动，都能实时的修正保持相机对地姿态，继而保证拍摄稳定。

需要说明的是，在具体执行时，上述步骤S101、步骤S102及步骤S103可以不分先后顺序。

相比现有技术，本发明实施例方案中，摒弃了通常使用的跟随IMU姿态测量单元速度补偿姿态的办法(外部干扰会导致速度跟随不稳定)，转而采用严格的数学推导和滤波算法计算姿态补偿角度传输到三轴云台执行单元驱动电机运行。其中，IMU主控单元使用普通性能设备即可，执行单元优选使用高精度的PMSM电机进行FOC软件算法控制。通过实际测试，能够实时快速的进行航拍姿态补偿，拍摄效果画面平稳，基本上能够完全隔离外界扰动造成的画面抖动。

此外，本发明实施例方案中，可以采用模块化设计，即IMU惯性测量单元(也即姿态测量单元)设于云台上，与飞行器上的IMU姿态测量单元独立，云台与飞行器不发生通信，使得该云台可兼容任何飞行器等机体，也可用于手持云台中。

如图2所示，本发明较佳实施例提出一种云台，包括：Rtcn确定模块201、Rcn获取模块202、Rcv获取模块203、Rtcv计算模块204及补偿控制模块205，其中：

Rtcn确定模块201，用于确定外部载体上的云台所搭载的承载物对地的目标旋转矩阵Rtcn；

Rcn获取模块202，用于获取所述承载物对地的当前旋转矩阵Rcn；

Rcv获取模块203，用于获取所述承载物对外部载体的当前旋转矩阵Rcv；

Rtcv计算模块204，用于根据所述承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv；

补偿控制模块205，用于根据所述承载物对外部载体的目标旋转矩阵Rtcv，计算获取所述云台上驱动装置的补偿量，并作用于所述驱动装置。其中，驱动装置可以为云台上的电机或者其他装置，本实施例以电机进行举例。若驱动装置为电机，则驱动装置的补偿量可以为电机旋转的补偿角度。

优选地，所述Rtcv计算模块204，还用于获取所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc；根据所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc以及所述承载物对地的当前旋转矩阵Rcn，计算得到外部载体对地的当前旋转矩阵Rvn；获取所述外部载体对地的当前旋转矩阵Rvn的转置矩阵Rnv；根据所述外部载体对地的当前旋转矩阵Rvn的转置矩阵Rnv以及承载物对地的目标旋转矩阵Rtcn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv。

具体地，本实施例中，云台所搭载的承载物包括但不限于相机、照明灯、发射或投射装置，本发明实施例以相机进行举例。其中，云台可以为一轴、二轴或三轴等云台。用于拍摄的相机搭载在外部载体上，该外部载体可以是飞行器、船体、车体或人体(即该云台为手持云台)。本实施例以三轴航拍云台进行举例，可进行航空拍摄。

为保持飞行拍摄画面平稳的效果，要求能够时刻使得云台所搭载相机保持一定的对地姿态朝向拍摄位置，并消除飞行器飞行与外部环境造成的画面抖动。因此，要求三轴航拍云台能够实时迅速的补偿飞行平台和外部环境造成的姿态的变化。

本实施例在三轴航拍云台上设置有MCU主控单元以及IMU惯性测量单元等执行单元。为了使三轴航拍云台能够迅速补偿外部抖动带来的姿态变化，本发明实施例中使用计算能力较强的MCU主控单元，三轴航拍云台的执行单元上电机优选使用适用于精确定位控制的PMSM电机。由于三轴航拍云台挂载在飞行器外部直接与外部接触，因此保证随时IMU惯性测量单元的稳定工作(主要与外部温度相关)也需要解决。

为此，本实施例采用如下方案：

首先，确定云台所搭载的相机对地的目标姿态，并确定目标姿态对应的目标旋转矩阵Rtcn。

云台的设计目的是为了使得搭载相机能够保持已经的姿态对目标位置进行拍摄，该姿态称为相机的目标姿态。例如，飞行器在空中，需要时刻保持相机镜头相对于地平面45度向下拍摄，则对应的姿态角度为俯仰角pitch45度，横滚角roll与偏航角yaw均为0度。该姿态经过四元素法可转换为3X3的旋转矩阵，称为目标矩阵，命名为Rtcn。

其中，R表示旋转矩阵，c意为camera(相机)，n意为navigation(导航)，v意为vehicle(此处理解为云台载体飞行器)，t意为target目标姿态。Rcn的意义即是：camera相机对navigation导航坐标系(也即对地)的旋转矩阵。每个不同的姿态向量(roll，pitch，yaw)均对应一个3X3的旋转矩阵(下文同此)。

然后，获取云台所搭载的相机对地的当前姿态，并确定相机对地的当前姿态所对应的当前旋转矩阵Rcn。

如前所述，本实施例在云台MCU主控板上载有IMU惯性测量单元，MCU主控单元通过读取IMU惯性测量单元数据，通过姿态算法能够获取当前相机实际的对导航坐标系(即对地)的姿态，以姿态向量(roll，pitch，yaw)表示。该姿态用四元素算法同样可得到一个3X3的旋转矩阵，称为Rcn。

因为相机时刻受到外部扰动，因此这个姿态/旋转矩阵是实时变动的。为保证该姿态稳定平滑，可以在使用该姿态前进行卡尔曼滤波处理，即对(roll，pitch，yaw)进行卡尔曼滤波处理。

之后，获取相机当前对飞行器载体的旋转矩阵Rcv(相机对机体坐标系的旋转矩阵)。

由于相机通过云台与飞行器挂载在一起，期间通过三轴云台上的电机运动改变位置。因此，相机相对飞行器载体的姿态事实上就是三轴云台上电机相对起始位置(对地)的旋转角度。这三个角度能够通过MCU主控单元与驱动电机的MCU通讯得知。它同样可对应3X3旋转矩阵，称为Rcv。

然后，根据上述获取到的相机对地的目标旋转矩阵Rtcn、相机对地的当前旋转矩阵Rcn、相机对飞行器载体的当前旋转矩阵Rcv，计算得到相机对飞行器载体的目标旋转矩阵Rtcv，以便对相机姿态进行运动补偿。

首先，根据相机对地的当前旋转矩阵Rcn、相机对外部载体的当前旋转矩阵Rcv，计算得到外部载体对地的当前旋转矩阵Rvn；

然后，根据相机对地的目标旋转矩阵Rtcn以及所述外部载体对地的当前旋转矩阵Rvn，计算得到相机对外部载体的目标旋转矩阵Rtcv。

具体算法原理如下：

首先，介绍旋转矩阵的两个基本性质：

<1>Rab＝T(Rba)：表示从A到B的旋转矩阵等于B到A旋转矩阵的转置；

<2>Rac＝Rab*Rbc：表示从A到C的旋转矩阵等于从A到B的旋转矩阵乘上B到C的旋转矩阵。

根据以上两个性质，本提案使用如下四步算法：

Rvc＝T(Rcv)；

Rvn＝Rvc*Rcn；

Rnv＝T(Rvn)；

Rtcv＝Rtcn*Rnv。

其中，Rcv为相机对机体坐标系的当前旋转矩阵，通过云台的电机的当前位置相对原始位置的对比，并经过控制器的处理得到；

Rvc为相机对机体坐标系的当前旋转矩阵的转置矩阵；

从上述公式可以知道：

在获得Rcv后，通过矩阵的反置公式Rvc＝T(Rcv)，可获得Rvc。

Rcn为相机对地(即导航坐标系)的当前旋转矩阵，通过设于云台上的IMU惯性测量单元测得并经过控制器的处理得到；

从上述公式可以知道：

在获得Rvc和Rcn后，通过矩阵的乘积公式Rvn＝Rvc*Rcn，可获得Rvn(飞行器载体对地(导航坐标系)的当前旋转矩阵)；也即本提案通过设于云台上的IMU惯性测量单元、控制器及电机位置测量单元即可得到飞行器相对地面坐标系(导航坐标系)的当前旋转矩阵Rvn。

Rnv为飞行器载体相对地面坐标系(导航坐标系)的当前旋转矩阵的转置矩阵。

从上述公式可以知道：

在获得Rvn后，通过矩阵的反置公式Rnv＝T(Rvn)，可获得Rnv。

Rtcn为相机对地(即导航坐标系)的目标旋转矩阵；

Rtcv为相机对飞行器(机体坐标系)的目标旋转矩阵，也即相机应该相对飞行器旋转的角度；

从上述公式可以知道：

在获得Rnv及Rtcn后，通过矩阵的乘积公式Rtcv＝Rtcn*Rnv，可获得Rtcv。

优选的，Rtcn所对应的姿态向量(roll，pitch，yaw)与初始位置对应的姿态向量(roll，pitch，yaw)相同。

对上述算法，在此需要说明的是：

Rtcn如前文所述，为相机对地的目标姿态对应的目标旋转矩阵。事实上航拍三轴云台就是为了保证该姿态不变。因此，当目标姿态被用户确定，则该矩阵为固定矩阵。

Rnv导航系对飞行器的旋转矩阵。也就是飞行器对导航系旋转矩阵的转置矩阵。本实施例是通过上述算法倒推出飞行器对导航系姿态，而非从飞行器获取。采用此种实现方案可以保持云台的独立性，可以使使用本提案算法的云台挂载在任何飞行器上使用。因为飞行器在不断运动以及外部风力等原因，这个矩阵是时刻变化的。

Rtcv是相机对飞行器的目标旋转矩阵。因为云台与飞行器挂载在一起，因此实际上相机对飞行器的旋转就是云台三轴电机的旋转角度。由于Rtcn为固定量，Rnv为时变量，因此Rtcv＝Rtcn*Rnv也为时变量。

根据上述四步算法，就能通过Rnv的时变计算出Rcv的时变，继而通过Rtcv旋转矩阵与角度的关系计算出该时刻三轴云台电机需要转到的旋转角度，发送给PMSM电机执行单元运行。因此，就可以保证相机无论遇到什么外部扰动，都能实时的修正保持相机对地姿态，继而保证拍摄稳定。

相比现有技术，本发明实施例方案中，摒弃了通常使用的跟随IMU姿态测量单元速度补偿姿态的办法(外部干扰会导致速度跟随不稳定)，转而采用严格的数学推导和滤波算法计算姿态补偿角度传输到三轴云台执行单元驱动电机运行。其中，IMU主控单元使用普通性能设备即可，执行单元优选使用高精度的PMSM电机进行FOC软件算法控制。通过实际测试，能够实时快速的进行航拍姿态补偿，拍摄效果画面平稳，基本上能够完全隔离外界扰动造成的画面抖动。

此外，本发明实施例方案中，可以采用模块化设计，即IMU惯性测量单元(也即姿态测量单元)设于云台上，与飞行器上的IMU姿态测量单元独立，云台与飞行器不发生通信，使得该云台可兼容任何飞行器等机体，也可用于手持云台中。

如图3所示，本发明第较佳实施例提出一种外部载体控制方法，包括：

步骤S301，获取承载物对地的当前旋转矩阵Rcn；

步骤S302，获取所述承载物对外部载体的当前旋转矩阵Rcv；

步骤S303，根据所述承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述外部载体对地的当前旋转矩阵Rvn；

本实施例中，承载物包括但不限于相机、照明灯、发射或投射装置，本发明实施例以相机进行举例。外部载体可以为四轴飞行器、无人机飞行器、手持拍摄云台或者搭载相机的其他航模。本实施例以三轴航拍云台挂载在飞行器上进行举例。通过飞行器带动云台上所搭载的相机进行航空拍摄。

本实施例方案中，无需在飞行器上设置IMU姿态测量单元，而是通过检测相机对地的当前旋转矩阵Rcn、相机对飞行器的当前旋转矩阵Rcv，并结合相应的算法计算得到飞行器对地的当前旋转矩阵Rvn，即可得到飞行器的姿态角度。

具体地，首先，获取相机对地的当前旋转矩阵Rcn；

然后，获取相机对飞行器的当前旋转矩阵Rcv；

之后，获取相机对飞行器的当前旋转矩阵Rcv的转置矩阵Rvc；

最后，根据相机对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc以及相机对地的当前旋转矩阵Rcn，计算得到飞行器对地的当前旋转矩阵Rvn，即可得到飞行器的姿态角度。

本实施例方案，由于无需在飞行器上设置IMU姿态测量单元，而是通过检测相机对地的当前旋转矩阵Rcn、相机对飞行器的当前旋转矩阵Rcv，并结合相应的算法计算得到飞行器对地的当前旋转矩阵Rvn，即可得到飞行器的姿态角度，由此减少了飞行器的结构和设计复杂度，并提高了飞行器姿态测量的准确性。

如图4所示，本发明较佳实施例提出一种飞行载体，包括：Rcn获取模块401、Rcv获取模块402及Rvn计算模块403，其中：

Rcn获取模块401，用于获取承载物对地的当前旋转矩阵Rcn；

Rcv获取模块402，用于获取所述承载物对外部载体的当前旋转矩阵Rcv；

Rvn计算模块403，用于根据所述承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述外部载体对地的当前旋转矩阵Rvn。

其中，承载物包括但不限于相机、照明灯、发射或投射装置，本发明实施例以相机进行举例。飞行载体可以为四轴飞行器、无人机飞行器、手持拍摄设备或者搭载相机的其他航模。本实施例以飞行器进行举例，并以三轴航拍云台挂载在飞行器上进行举例。通过飞行器带动云台上所搭载的相机进行航空拍摄。

本实施例方案中，无需在飞行器上设置IMU姿态测量单元，而是通过检测相机对地的当前旋转矩阵Rcn、相机对飞行器的当前旋转矩阵Rcv，并结合相应的算法计算得到飞行器对地的当前旋转矩阵Rvn，即可得到飞行器的姿态角度。

具体地，首先，获取相机对地的当前旋转矩阵Rcn；

然后，获取相机对飞行器的当前旋转矩阵Rcv；

之后，获取相机对飞行器的当前旋转矩阵Rcv的转置矩阵Rvc；

最后，根据相机对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc以及相机对地的当前旋转矩阵Rcn，计算得到飞行器对地的当前旋转矩阵Rvn，即可得到飞行器的姿态角度。

本实施例方案，由于无需在飞行器上设置IMU姿态测量单元，而是通过检测相机对地的当前旋转矩阵Rcn、相机对飞行器的当前旋转矩阵Rcv，并结合相应的算法计算得到飞行器对地的当前旋转矩阵Rvn，即可得到飞行器的姿态角度，由此减少了飞行器的结构和设计复杂度，并提高了飞行器姿态测量的准确性。

本发明实施例云台控制方法、外部载体控制方法及云台，通过确定外部载体上的云台所搭载的承载物对地的目标旋转矩阵Rtcn；获取承载物对地的当前旋转矩阵Rcn；获取承载物对外部载体的当前旋转矩阵Rcv；根据承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到承载物对外部载体的目标旋转矩阵Rtcv；根据承载物对外部载体的目标旋转矩阵Rtcv，计算获取云台上驱动装置的补偿量，比如电机旋转的补偿角度，并将此补偿量传输到云台的驱动装置运行，从而能够实时快速的进行姿态补偿，避免了外部环境造成的抖动，保持搭载在云台上的承载物的平稳效果。此外，以外部载体为飞行器为例，本发明可以将IMU姿态测量单元设于云台上，与飞行器上的IMU姿态测量单元独立，云台与飞行器不发生通信，使得该云台可兼容任何飞行器等机体，也可用于手持云台中。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种云台控制方法，其特征在于，包括：

确定外部载体上的云台所搭载的承载物对地的目标旋转矩阵Rtcn；

获取所述承载物对地的当前旋转矩阵Rcn；

获取所述承载物对外部载体的当前旋转矩阵Rcv；

根据所述承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv；

根据所述承载物对外部载体的目标旋转矩阵Rtcv，计算获取所述云台上驱动装置的补偿量，并作用于所述承载物。

2.根据权利要求1所述的方法，其特征在于，所述根据所述承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv的步骤包括：

根据所述承载物对地的当前旋转矩阵Rcn、所述承载物对外部载体的当前旋转矩阵Rcv，计算得到外部载体对地的当前旋转矩阵Rvn；

根据所述承载物对地的目标旋转矩阵Rtcn以及所述外部载体对地的当前旋转矩阵Rvn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv。

3.根据权利要求2所述的方法，其特征在于，

所述根据所述承载物对地的当前旋转矩阵Rcn、所述承载物对外部载体的当前旋转矩阵Rcv，计算得到外部载体对地的当前旋转矩阵Rvn的步骤包括：

获取所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc；

根据所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc以及所述承载物对地的当前旋转矩阵Rcn，计算得到外部载体对地的当前旋转矩阵Rvn；

所述根据所述承载物对地的目标旋转矩阵Rtcn以及所述外部载体对地的当前旋转矩阵Rvn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv的步骤包括：

获取所述外部载体对地的当前旋转矩阵Rvn的转置矩阵Rnv；

根据所述外部载体对地的当前旋转矩阵Rvn的转置矩阵Rnv以及承载物对地的目标旋转矩阵Rtcn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv。

4.根据权利要求1、2或3所述的方法，其特征在于，所述驱动装置为PMSM电机；所述外部载体至少为飞行器。

5.根据权利要求1、2或3所述的方法，其特征在于，所述获取所述承载物对外部载体的当前旋转矩阵Rcv的步骤包括：

通过所述承载物的当前位置相对原始位置的对比，并经过控制器的处理得到所述承载物对外部载体的当前旋转矩阵Rcv。

6.一种云台，其特征在于，包括：

Rtcn确定模块，用于确定外部载体上的云台所搭载的承载物对地的目标旋转矩阵Rtcn；

Rcn获取模块，用于获取所述承载物对地的当前旋转矩阵Rcn；

Rcv获取模块，用于获取所述承载物对外部载体的当前旋转矩阵Rcv；

Rtcv计算模块，用于根据所述承载物对地的目标旋转矩阵Rtcn、承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv；

补偿控制模块，用于根据所述承载物对外部载体的目标旋转矩阵Rtcv，计算获取所述云台上驱动装置的补偿量，并作用于所述承载物。

7.根据权利要求6所述的云台，其特征在于，

所述Rtcv计算模块，还用于根据所述承载物对地的当前旋转矩阵Rcn、所述承载物对外部载体的当前旋转矩阵Rcv，计算得到外部载体对地的当前旋转矩阵Rvn；根据所述承载物对地的目标旋转矩阵Rtcn以及所述外部载体对地的当前旋转矩阵Rvn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv。

8.根据权利要求7所述的云台，其特征在于，

所述Rtcv计算模块，还用于获取所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc；根据所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc以及所述承载物对地的当前旋转矩阵Rcn，计算得到外部载体对地的当前旋转矩阵Rvn；获取所述外部载体对地的当前旋转矩阵Rvn的转置矩阵Rnv；根据所述外部载体对地的当前旋转矩阵Rvn的转置矩阵Rnv以及承载物对地的目标旋转矩阵Rtcn，计算得到所述承载物对外部载体的目标旋转矩阵Rtcv。

9.一种外部载体控制方法，其特征在于，包括：

获取承载物对地的当前旋转矩阵Rcn；

获取所述承载物对外部载体的当前旋转矩阵Rcv；

根据所述承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述外部载体对地的当前旋转矩阵Rvn。

10.根据权利要求9所述的方法，其特征在于，所述根据所述承载物对地的当前旋转矩阵Rcn、承载物对外部载体的当前旋转矩阵Rcv，计算得到所述外部载体对地的当前旋转矩阵Rvn的步骤包括：

获取所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc；

根据所述承载物对外部载体的当前旋转矩阵Rcv的转置矩阵Rvc以及所述承载物对地的当前旋转矩阵Rcn，计算得到外部载体对地的当前旋转矩阵Rvn。