一种关节角的初始化方法、装置及飞行器与流程

本发明涉及飞行器技术领域，特别是涉及一种关节角的初始化方法、装置及飞行器。

背景技术：

飞行器，如无人飞行器(unmannedaerialvehicle，uav)，也称无人机，以其具有体积小、重量轻、机动灵活、反应快速、无人驾驶、操作要求低等优点，得到了越来越广泛的应用。而无人飞行器一般都设置有云台，例如三轴云台，而三轴云台包括三轴电机，一般都会在三轴电机上面装有线性霍尔传感器，用来检测电机转子的磁场，进而计算出电机的电气角度，从而确定关节角度，进而实现电机的控制。

目前为了获取电机的关节角度，在上电后都会让电机进行自检，从而实现云台的正常工作，但是这种方式需要在上电后三轴电机都要进行自检，从而影响工作效率。

技术实现要素：

本发明实施例提供一种关节角的初始化方法、装置及飞行器，解决目前飞行器的三轴电机均需要自检导致效率低的问题，提高三轴云台上电后的自检效率。

为解决上述技术问题，本发明实施例提供以下技术方案：

第一方面，本发明实施例提供一种关节角的初始化方法，应用于飞行器，所述飞行器设置有三轴云台，所述三轴云台包括三轴电机，分别为第一电机、第二电机以及第三电机，所述第二电机对应第二极对数，所述第三电机对应第三极对数，所述三轴云台还包括基座端和工具端，所述工具端设置有三轴加速度计，所述方法包括：

获取所述第一电机上电时刻对应的第一关节角以及所述第二电机上电时刻对应的第二电气角度和第三电机上电时刻对应的第三电气角度；

根据所述第二电机的第二电气角度、第二极对数以及所述第三电机的第三电气角度、第三极对数，遍历所述第二电机和第三电机的第二关节角和第三关节角，确定所述第一电机、第二电机以及第三电机的关节角组合；

确定所述关节角组合对应的姿态四元数以及所述基座端的姿态四元数，根据所述基座端的姿态四元数以及关节角组合的姿态四元数，确定所述工具端的姿态四元数；

根据所述工具端的姿态四元数，结合所述工具端的三轴加速度计的读数，确定所述三轴电机的初始化关节角组合。

在一些实施例中，所述基座端设置有三轴加速度计，所述确定所述基座端的姿态四元数，包括：

获取所述基座端的三轴加速度计的读数，固定所述基座端的偏航角，根据所述基座端的三轴加速度计的读数，计算所述基座端的俯仰角和横滚角；

根据所述基座端的偏航角、俯仰角和横滚角，确定所述基座端的姿态四元数。

在一些实施例中，所述确定所述第一电机、第二电机以及第三电机的关节角组合之后，所述方法还包括：对所述第一电机、第二电机以及第三电机的关节角组合进行筛选，确定筛选后的关节角组合，所述确定所述关节角组合对应的姿态四元数，包括：

确定每一筛选后的关节角组合对应的姿态四元数。

在一些实施例中，所述获取上电时刻所述第一电机对应的第一关节角，包括：

通过自检碰限位的方式，确定所述第一电机对应的第一关节角。

在一些实施例中，所述方法还包括：

计算所述基座端的加速度计模值和工具端的加速度计模值；

根据所述基座端的加速度计模值和工具端的加速度计模值，确定所述云台是否处于静止状态。

在一些实施例中，所述根据所述基座端的姿态四元数以及关节角组合的姿态四元数，确定所述工具端的姿态四元数，包括：

对所述基座端的姿态四元数和所述关节角组合的姿态四元数进行四元数乘法，确定所述工具端的姿态四元数。

在一些实施例中，所述根据所述工具端的姿态四元数，结合所述工具端的三轴加速度计的读数，确定所述三轴电机的初始化关节角组合，包括：

根据所述工具端的姿态四元数，确定旋转矩阵；

根据所述工具端的三轴加速度计的读数，生成重力向量；

根据所述旋转矩阵和所述重力向量，确定所述三轴电机的初始化关节角组合。

在一些实施例中，所述根据所述旋转矩阵和所述重力向量，确定所述三轴电机的初始化关节角组合，包括：

获取所述旋转矩阵的列向量；

对所述旋转矩阵的列向量和所述重力向量进行点乘运算，生成多个点乘结果；

将点乘结果的最大值对应的第一关节角、第二关节角以及第三关节角作为所述三轴电机的初始化关节角组合。

第二方面，本发明实施例提供一种关节角的初始化装置，应用于飞行器，所述飞行器设置有三轴云台，所述三轴云台包括三轴电机，分别为第一电机、第二电机以及第三电机，所述第二电机对应第二极对数，所述第三电机对应第三极对数，所述三轴云台还包括基座端和工具端，所述工具端设置有三轴加速度计，所述装置包括：

获取单元，用于获取所述第一电机上电时刻对应的第一关节角以及所述第二电机上电时刻对应的第二电气角度和第三电机上电时刻对应的第三电气角度；

关节角组合单元，用于根据所述第二电机的第二电气角度、第二极对数以及所述第三电机的第三电气角度、第三极对数，遍历所述第二电机和第三电机的第二关节角和第三关节角，确定所述第一电机、第二电机以及第三电机的关节角组合；

姿态四元数单元，用于确定所述关节角组合对应的姿态四元数以及所述基座端的姿态四元数，根据所述基座端的姿态四元数以及关节角组合的姿态四元数，确定所述工具端的姿态四元数；

初始化关节角组合单元，用于根据所述工具端的姿态四元数，结合所述工具端的三轴加速度计的读数，确定所述三轴电机的初始化关节角组合。

在一些实施例中，所述姿态四元数单元，具体用于：

获取所述基座端的三轴加速度计的读数，固定所述基座端的偏航角，根据所述基座端的三轴加速度计的读数，计算所述基座端的俯仰角和横滚角；

根据所述基座端的偏航角、俯仰角和横滚角，确定所述基座端的姿态四元数。

在一些实施例中，所述装置还包括：

筛选单元，用于对所述第一电机、第二电机以及第三电机的关节角组合进行筛选，确定筛选后的关节角组合，所述确定所述关节角组合对应的姿态四元数。

在一些实施例中，所述获取单元，具体用于：

通过自检碰限位的方式，确定所述第一电机对应的第一关节角。

在一些实施例中，所述装置还包括：

静止状态单元，用于计算所述基座端的加速度计模值和工具端的加速度计模值；

根据所述基座端的加速度计模值和工具端的加速度计模值，确定所述云台是否处于静止状态。

在一些实施例中，所述姿态四元数单元，具体用于：

对所述基座端的姿态四元数和所述关节角组合的姿态四元数进行四元数乘法，确定所述工具端的姿态四元数。

在一些实施例中，所述初始化关节角组合单元，具体用于：

获取所述旋转矩阵的列向量；

对所述旋转矩阵的列向量和所述重力向量进行点乘运算，生成多个点乘结果；

将点乘结果的最大值对应的第一关节角、第二关节角以及第三关节角作为所述三轴电机的初始化关节角组合。

第三方面，本发明实施例提供一种飞行器，包括：

机身；

机臂，与所述机身相连；

动力装置，设于所述机身和/或所述机臂，用于为所述飞行器提供飞行的动力；

三轴云台，设置于所述机身，所述三轴云台包括三轴电机，分别为第一电机、第二电机以及第三电机，所述三轴云台还包括基座端和工具端；

飞行控制器，设于所述机身；

其中，所述飞行控制器包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如上所述的关节角的初始化方法。

第四方面，本发明实施例还提供了一种非易失性计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使飞行器能够执行如上所述的关节角的初始化方法。

本发明通过提供一种关节角的初始化方法，应用于飞行器，所述飞行器设置有三轴云台，所述三轴云台包括三轴电机，分别为第一电机、第二电机以及第三电机，所述第二电机对应第二极对数，所述第三电机对应第三极对数，所述三轴云台还包括基座端和工具端，所述工具端设置有三轴加速度计，所述方法包括：获取所述第一电机上电时刻对应的第一关节角以及所述第二电机上电时刻对应的第二电气角度和第三电机上电时刻对应的第三电气角度；根据所述第二电机的第二电气角度、第二极对数以及所述第三电机的第三电气角度、第三极对数，遍历所述第二电机和第三电机的第二关节角和第三关节角，确定所述第一电机、第二电机以及第三电机的关节角组合；确定所述关节角组合对应的姿态四元数以及所述基座端的姿态四元数，根据所述基座端的姿态四元数以及关节角组合的姿态四元数，确定所述工具端的姿态四元数；根据所述工具端的姿态四元数，结合所述工具端的三轴加速度计的读数，确定所述三轴电机的初始化关节角组合。通过对第一电机进行自检，确定三轴电机的初始化关节角组合，本发明能够提高三轴云台上电后的自检效率。

附图说明

一个或多个实施例通过与之对应的附图中的图片进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。

图1是本发明实施例提供的一种飞行器的具体结构图；

图2是本发明实施例提供的一种三轴云台的示意图；

图3是本发明实施例提供的一种关节角的初始化方法的流程示意图；

图4是图3中的步骤s30的细化流程图；

图5是图3中的步骤s40的细化流程图；

图6是图5中的步骤s43的细化流程图；

图7是本发明实施例提供的一种关节角的初始化装置的结构示意图；

图8是本发明实施例提供的一种飞行器的硬件结构示意图；

图9是本发明实施例提供的一种飞行器的连接框图；

图10是图9中的动力系统的示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明实施例提供的关节角的初始化方法可以应用到各种通过电机或马达驱动的可移动物体上，包括但不限于飞行器、机器人等。其中飞行器可包括无人飞行器(unmannedaerialvehicle，uav)，无人飞船等。

其中，本发明实施例的关节角的初始化方法，应用于飞行器的飞行控制器。

请参阅图1，图1是本发明实施例提供的一种飞行器的具体结构图；

如图1所示，该飞行器10包括：机身11、与所述机身11相连的机臂12、设置于所述机臂12的动力装置13，连接至该机身11底部的云台14，安装在云台14上的摄像头15以及设置于机身11内的飞行控制器(图未示)。

其中，飞行控制器与动力装置13连接，动力装置13安装在所述机身11上，用于为所述飞行器10提供飞行动力。具体的，飞行控制器用于执行上述的关节角的初始化方法以生成控制指令，并将该控制指令发送给动力装置13的电调，电调通过该控制指令控制动力装置13的驱动电机。或者，飞行控制器用于执行关节角的初始化方法，以便生成控制指令，并通过该控制指令控制动力装置13的驱动电机。

机身11包括：中心壳体以及与中心壳体连接的一个或多个机臂，一个或多个机臂呈辐射状从中心壳体延伸出。机臂与中心壳体的连接可以是一体连接或者固定连接。动力装置安装于机臂上。

飞行控制器用于执行上述关节角的初始化方法以确定三轴电机的关节角度，方便生成控制指令，并将该控制指令发送给动力装置的电调，以便电调通过该控制指令控制动力装置的驱动电机。控制器为具有一定逻辑处理能力的器件，如控制芯片、单片机、微控制单元(microcontrollerunit，mcu)等。

动力装置13包括：电调，驱动电机和螺旋桨。电调位于机臂或中心壳体所形成的空腔内。电调分别与控制器及驱动电机连接。具体的，电调与驱动电机电连接，用于控制所述驱动电机。驱动电机安装在机臂上，驱动电机的转动轴连接螺旋桨。螺旋桨在驱动电机的驱动下产生使得飞行器10移动的力，例如，使得飞行器10移动的升力或者推力。

飞行器10完成各个规定速度、动作(或姿态)是通过电调控制驱动电机以实现的。电调全称电子调速器，根据控制信号调节飞行器10的驱动电机的转速。其中，控制器为执行上述关节角的初始化方法的执行主体，电调生成控制指令来控制驱动电机。电调控制驱动电机的原理大致为：驱动电机是将电脉冲信号转变为角位移或线位移的开环控制元件。在非超载的情况下，驱动电机的转速、停止的位置只取决于脉冲信号的频率和脉冲数，而不受负载变化的影响，当驱动器接收到一个脉冲信号，它就驱动动力装置的驱动电机按设定的方向转动一个固定的角度，它的旋转是以固定的角度运行的。因此，电调可以通过控制脉冲个数来控制角位移量，从而达到准确定位的目的；同时可以通过控制脉冲频率来控制驱动电机转动的速度和加速度，从而达到调速的目的。

目前飞行器10主要功能为航拍、影像实时传输、高危地区探测等。为了实现航拍、影像实时传输、高危地区探测等功能，飞行器10上会连接有摄像组件。具体的，飞行器10和摄像组件通过连接结构，如减振球等进行连接。该摄像组件用于在飞行器10进行航拍的过程中，获取拍摄画面。

具体的，摄像组件包括：云台及拍摄装置。云台与飞行器10连接。其中，拍摄装置搭载于所述云台上，拍摄装置可以为图像采集装置，用于采集图像，该拍摄装置包括但不限于：相机、摄影机、摄像头、扫描仪、拍照手机等。云台用于搭载拍摄装置，以实现拍摄装置的固定、或随意调节拍摄装置的姿态(例如，改变拍摄装置的高度、倾角和/或方向)以及使所述拍摄装置稳定保持在设定的姿态上。例如，当飞行器10进行航拍时，云台主要用于使所述拍摄装置稳定保持在设定的姿态上，防止拍摄装置拍摄画面抖动，保证拍摄画面的稳定。

云台14与飞行控制器连接，以实现云台14与飞行控制器之间的数据交互。例如，飞行控制器发送偏航指令至云台14，云台14获取偏航的速度和方向指令并执行，且将执行偏航指令后所产生的数据信息发送至飞行控制器，以便飞行控制器检测当前偏航状况。

云台包括：云台电机及云台基座。其中，云台电机安装于云台基座。飞行控制器也可通过动力装置13的电调来控制云台电机，具体的，飞行控制器与电调连接，电调与云台电机电连接，飞行控制器生成云台电机控制指令，电调通过云台电机控制指令以控制云台电机。

云台基座与飞行器的机身连接，用于将摄像组件固定安装于飞行器的机身上。

云台电机分别与云台基座及拍摄装置连接。该云台可以为多轴云台，与之适应的，云台电机为多个，也即每个轴设置有一个云台电机。云台电机一方面可带动拍摄装置的转动，从而满足拍摄转轴的水平旋转和俯仰角度的调节，通过手动远程控制云台电机旋转或利用程序让电机自动旋转，从而达到全方位扫描监控的作用；另一方面，在飞行器进行航拍的过程中，通过云台电机的转动实时抵消拍摄装置受到的扰动，防止拍摄装置抖动，保证拍摄画面的稳定。

在本发明中，云台为三轴云台，云台电机为三轴电机，三轴电机分别包括第一电机、第二电机以及第三电机。

具体的，三轴云台可以是用来安装、固定相机或传感器等负载的增稳设备和支撑设备。在本发明实施例中，三轴云台包括偏航轴轴臂、横滚轴轴臂以及俯仰轴轴臂，每一轴臂对应一电机，分别为用于控制偏航轴轴臂转动的偏航轴电机(第一电机)、用于控制横滚轴轴臂转动的横滚轴电机(第二电机)和用于控制俯仰轴轴臂转动的俯仰轴电机(第三电机)，通过偏航轴电机、横滚轴电机和俯仰轴电机对应控制偏航轴轴臂、横滚轴轴臂和俯仰轴轴臂转动，实现三轴云台姿态的控制。

其中，所述三轴云台包括工具端，其中，所述工具端可以为拍摄装置，拍摄装置搭载于云台上，拍摄装置上设置有惯性测量单元(inertialmeasurementunit，imu)，该惯性测量单元为用于测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个imu内会装有三轴的陀螺仪和三个方向的加速度计，即通过三轴陀螺仪、三轴加速度计来测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。为了提高可靠性，还可以为每个轴配备更多的传感器。一般而言imu要安装在飞行器的重心上。

三轴云台在工作过程中，需要将末端的陀螺仪信号通过雅克比逆矩阵转换为三轴的关节角速度，而雅克比逆矩阵的计算需要用到三轴关节角度。

目前大部分小型云台都会在三轴无刷电机上面装有线性霍尔传感器，用来检测电机转子的磁场，进而计算出电机的电气角度，从而实现电机的控制。

电机的电气角度和关节角度并非一一对应，和电机的极对数有关。对于一个n对极的电机来说，在一个关节角度周期里面包含有n个电气角度周期。所以，为了获得电机的关节角度，在上电后都会让电机去自检，让电机碰到每个轴的机械限位，然后以此为一个参考点，通过电气角度来推算出电机的关节角度，从而实现云台的正常工作。这种方法需要上电后三轴电机都进行自检，效率低，给用户带来的体感也较差。

基于上述问题，本发明实施例提出一种关节角的初始化方法、装置及飞行器，以提高三轴云台上电后的自检效率。

下面结合附图，对本发明实施例作进一步阐述。

实施例一

请参阅图2，图2是本发明实施例提供的一种三轴云台的示意图；

如图2所示，该三轴云台包括三轴电机，分别为第一电机m1、第二电机m2以及第三电机m3，其中，所述第一电机用于控制三轴云台的偏航轴轴臂转动，所述第二电机用于控制三轴云台的横滚轴轴臂转动，所述第三电机用于控制三轴云台的俯仰轴轴臂转动，进而实现三轴云台姿态的控制，其中，所述第一电机m1对应基座端，所述第二电机m2对应工具端，所述第三电机m3对应末端，即所述第一电机位于基座端所在的位置，所述第二电机位于所述工具端所在的位置，所述第三电机位于所述末端所在的位置，所述末端为所述工具端的末端，即工具末端，其中，每一电机均对应一极对数，假设所述第一电机m1对应的第一极对数为n1，所述第二电机m2对应的第二极对数为n2，所述第三电机m3对应的的第三极对数为n3，其中，上电时刻第一电机的电气角度为θe1，第二电机的电气角度为θe2，第三电机的电气角度为θe3，第一电机对应的第一关节角为θj1，第二电机对应的第二关节角为θj2，第三电机对应的第三关节角为θj3，其中，关节角度和电气角度的对应关系分别为下式(1)：

其中，wrap(·)函数的功能为将关节角度限制在[-π,π]之间。

由于第一电机m1的轴向和基座端的z轴方向平行，因此当基座端处于水平时，第一电机m1的转动不会影响重力作用在工具端的三轴加速度计上面的值，所以可以通过基座端加速度计和工具端加速度计来推算第二电机m2以及第三电机m3的关节角度。

具体的，请再参阅图3，图3是本发明实施例提供的一种关节角的初始化方法的流程示意图；

其中，关节角指的是电机的关节角度，所述关节角的初始化方法，应用于如上所述的飞行器，所述飞行器设置有三轴云台，所述三轴云台包括三轴电机，分别为第一电机m1、第二电机m2以及第三电机m3，所述三轴电机分别对应一极对数，即，所述第一电机m1对应第一极对数n1，所述第二电机m2对应第二极对数n2，所述第三电机m3对应第三极对数n3，所述三轴云台还包括基座端和工具端，所述基座端和工具端均设置有三轴加速度计，如图3所示，该关节角的初始化方法，包括：

步骤s10:获取所述第一电机上电时刻对应的第一关节角以及所述第二电机上电时刻对应的第二电气角度和第三电机上电时刻对应的第三电气角度；

具体的，在上电时刻，获取第一电机对应的第一关节角，具体的，所述获取第一电机对应的第一关节角，包括：

通过自检碰限位的方式，确定所述第一电机对应的第一关节角。具体的，通过控制第一电机碰触其机械限位，从而确定所述第一电机对应的第一关节角θj1，通过确定所述第一电机对应的第一关节角之后，可以通过基座端加速度计和工具端加速度计推算所述第二电机的第二关节角以及第三电机的第三关节角。

具体的，由于云台上的三轴电机上安装有线性霍尔传感器，其用于检测电机转子的磁场，从而可以计算得出第一电机对应的第一电气角度、所述第二电机对应的第二电气角度和所述第三电机对应的第三电气角度，进而获取到所述第一电机对应的第一电气角度、所述第二电机对应的第二电气角度和所述第三电机对应的第三电气角度。

在本发明实施例中，所述方法还包括：

计算所述基座端的加速度计模值和工具端的加速度计模值；

根据所述基座端的加速度计模值和工具端的加速度计模值，确定所述云台是否处于静止状态。

具体的，当所述基座端的加速度计模值和工具端的加速度计模值同时处于一个重力加速度时，则确定所述云台处于静止状态，可以理解的是，在所述基座端的加速度计模值和工具端的加速度计模值同时处于一个重力加速度附近时，也可以认为所述云台处于静止状态，具体的，预设误差值，当所述基座端的加速度计模值与重力加速度的误差小于所述误差值，和/或，所述工具端的加速度计模值与重力加速度的误差小于所述误差值，则确定所述云台处于静止状态。

步骤s20：根据所述第二电机的第二电气角度、第二极对数以及所述第三电机的第三电气角度、第三极对数，遍历所述第二电机和第三电机的第二关节角和第三关节角，确定所述第一电机、第二电机以及第三电机的关节角组合；

具体的，每一电机对应一极对数，第一电机对应第一极对数，第二电机对应第二极对数，第三电机对应第三极对数，由于第一电机的第一关节角已通过自检碰限位的方式确定，而第二电机的极对数为n2以及第三电机的极对数为n3，则所述第一电机、第二电机以及第三电机的关节角有n2*n3种组合，从而确定所述第一电机、第二电机以及第三电机的关节角组合。

可以理解的是，在所述n2*n3种组合中，存在部分不符合要求的关节角组合，需要进行筛选以排除，因此，在确定所述第一电机、第二电机以及第三电机的关节角组合之后，所述方法还包括：对所述第一电机、第二电机以及第三电机的关节角组合进行筛选，确定筛选后的关节角组合，具体的，确定不符合云台机械限位的关节角组合，排除所述不符合云台机械限位的关节角，例如：第二关节角以及第三关节角中的不符合云台机械限位的关节角，从而确定筛选后的关节角组合。

步骤s30：确定所述关节角组合对应的姿态四元数以及所述基座端的姿态四元数，根据所述基座端的姿态四元数以及关节角组合的姿态四元数，确定所述工具端的姿态四元数；

具体的，所述确定所述关节角组合对应的姿态四元数，包括：

确定每一关节角组合对应的姿态四元数，具体的，对于三轴正交的云台来说，按照“m1-m2-m3”的旋转顺序，即基座端-工具端-末端的旋转顺序，从而确定一个基于基座坐标系的四元数，具体的，所述基于基座坐标系的四元数，其表达式为下式(2)：

其中，θj1为第一电机对应的第一关节角，θj2为第二电机对应的第二关节角，θj3为第三电机对应的第三关节角。

具体的，请再参阅图4，图4是图3中的步骤s30的细化流程图；

如图4所示，所述步骤s30：确定所述基座端的姿态四元数，包括：

步骤s31：获取所述基座端的三轴加速度计的读数，固定所述基座端的偏航角，根据所述基座端的三轴加速度计的读数，计算所述基座端的俯仰角和横滚角；

具体的，通过获取所述基座端的三轴加速度计的读数，并固定所述基座端的偏航角，即令所述基座端的偏航角为0，根据所述基座端的三轴加速度计的读数，计算所述基座端的俯仰角和横滚角，其具体计算方式为现有技术，在此不再赘述。通过固定所述基座端的偏航角，并通过基座端的三轴加速度计的读数，计算得出所述基座端的俯仰角和横滚角，从而确定一组欧拉角，有利于根据确定得到的一组欧拉角，确定所述基座端的姿态四元数。

步骤s32：根据所述基座端的偏航角、俯仰角和横滚角，确定所述基座端的姿态四元数。

具体的，假设所述欧拉角中的俯仰角为θp，横滚角为θr，偏航角为θy，且它们的旋转角顺序为“zxy”，则根据所述欧拉角，确定所述基座端的姿态四元数的计算方法如下式(3)：

其中，θp为俯仰角，θr为横滚角，θy为偏航角，θj1为第一电机对应的第一关节角，θj2为第二电机对应的第二关节角，θj3为第三电机对应的第三关节角。

可以理解的是，在对所述第一电机、第二电机以及第三电机的关节角组合进行筛选，确定筛选后的关节角组合，所述确定所述关节角组合对应的姿态四元数，包括：

确定每一筛选后的关节角组合对应的姿态四元数。

具体的，所述根据所述基座端的姿态四元数以及关节角组合的姿态四元数，确定所述工具端的姿态四元数，包括：

对所述基座端的姿态四元数和所述关节角组合的姿态四元数进行四元数乘法，确定所述工具端的姿态四元数。

具体的，假设所述基座端的姿态四元数为p＝[p0p1p2p3]，所述关节角组合的姿态四元数为q＝[q0q1q2q3]，所述工具端的姿态四元数为r，则所述工具端的姿态四元数为所述基座端的姿态四元数与所述关节角组合的姿态四元数的四元数乘法所得到的结果，即所述四元数乘法，如下式(4)：

即，计算得到的工具端的姿态四元数r＝[r0r1r2r3]。

步骤s40：根据所述工具端的姿态四元数，结合所述工具端的三轴加速度计的读数，确定所述三轴电机的初始化关节角组合。

具体的，请再参阅图5，图5是图3中的步骤s40的细化流程图；

如图5所示，所述根据所述工具端的姿态四元数，结合所述工具端的三轴加速度计的读数，确定所述三轴电机的初始化关节角组合，包括：

步骤s41：根据所述工具端的姿态四元数，确定旋转矩阵；

具体的，假设所述工具端的姿态四元数为q＝[q0q1q2q3]，则其对应的旋转矩阵m的计算方式为下式(5)：

即所述旋转矩阵m为：

步骤s42：根据所述工具端的三轴加速度计的读数，生成重力向量；

具体的，获取所述工具端的三轴加速度计的读数，将所述工具端的三轴加速度计的读数作为重力向量[ax2ay2az2]。

步骤s43：根据所述旋转矩阵和所述重力向量，确定所述三轴电机的初始化关节角组合。

请再参阅图6，图6是图5中的步骤s43的细化流程图；

如图6所示，所述根据所述旋转矩阵和所述重力向量，确定所述三轴电机的初始化关节角组合，包括：

步骤s431：获取所述旋转矩阵的列向量；

具体的，获取所述旋转矩阵的最后一列的列向量，即所述旋转矩阵的最后一列元素。

步骤s432：对所述旋转矩阵的列向量和所述重力向量进行点乘运算，生成多个点乘结果；

具体的，获取所述旋转矩阵的最后一列元素[ax1ay1az1]，将所述重力向量[ax2ay2az2]与所述旋转矩阵的最后一列元素[ax1ay1az1]进行向量点乘运算，得到点乘结果，并保存所述点乘结果。

可以理解的是，所述三轴加速度计的读数可能有多个，因此可能出现多个点乘结果，因此，需要从所述多个点乘结果中确定对应的关节角组合作为初始化关节角组合。

步骤s433：将点乘结果的最大值对应的第一关节角、第二关节角以及第三关节角作为所述三轴电机的初始化关节角组合。

具体的，从计算得到的所有点乘结果中，确定所述点乘结果的最大值，所述点乘结果的最大值对应的第二关节角θj2，以及第三关节角θj3，即为所述第二电机和第三电机的真实关节角，由于所述第一电机对应的第一关节角θj1已经确定，进而确定所述三轴电机的第一电机对应的第一初始化关节角、第二电机对应的第二初始化关节角以及第三电机对应的第三初始化关节角，至此，所述三轴电机的关节角初始化完成。

在本发明实施例中，通过提供一种关节角的初始化方法，应用于飞行器，所述飞行器设置有三轴云台，所述三轴云台包括三轴电机，分别为第一电机、第二电机以及第三电机，所述第二电机对应第二极对数，所述第三电机对应第三极对数，所述三轴云台还包括基座端和工具端，所述工具端设置有三轴加速度计，所述方法包括：获取所述第一电机上电时刻对应的第一关节角以及所述第二电机上电时刻对应的第二电气角度和第三电机上电时刻对应的第三电气角度；根据所述第二电机的第二电气角度、第二极对数以及所述第三电机的第三电气角度、第三极对数，遍历所述第二电机和第三电机的第二关节角和第三关节角，确定所述第一电机、第二电机以及第三电机的关节角组合；确定所述关节角组合对应的姿态四元数以及所述基座端的姿态四元数，根据所述基座端的姿态四元数以及关节角组合的姿态四元数，确定所述工具端的姿态四元数；根据所述工具端的姿态四元数，结合所述工具端的三轴加速度计的读数，确定所述三轴电机的初始化关节角组合。通过对第一电机进行自检，确定三轴电机的初始化关节角组合，本发明实施例能够提高三轴云台上电后的自检效率。

实施例二

请参阅图7，图7是本发明实施例提供的一种关节角的初始化装置的示意图；

其中，该关节角的初始化装置70，应用于飞行器，所述飞行器设置有三轴云台，所述三轴云台包括三轴电机，分别为第一电机、第二电机以及第三电机，所述第二电机对应第二极对数，所述第三电机对应第三极对数，所述三轴云台还包括基座端和工具端，所述工具端设置有三轴加速度计，

该三轴云台包括三轴电机，分别为第一电机m1、第二电机m2以及第三电机m3，其中，所述第一电机用于控制三轴云台的偏航轴轴臂转动，所述第二电机用于控制三轴云台的横滚轴轴臂转动，所述第三电机用于控制三轴云台的俯仰轴轴臂转动，进而实现三轴云台姿态的控制，其中，所述第一电机m1对应基座端，所述第二电机m2对应工具端，所述第三电机m3对应末端，其中，每一电机均对应一极对数，假设所述第一电机m1对应的第一极对数为n1，所述第二电机m2对应的第二极对数为n2，所述第三电机m3对应的的第三极对数为n3，其中，上电时刻第一电机的电气角度为θe1，第二电机的电气角度为θe2，第三电机的电气角度为θe3，第一电机对应的第一关节角为θj1，第二电机对应的第二关节角为θj2，第三电机对应的第三关节角为θj3，其中，关节角度和电气角度的对应关系分别为下式(1)：

其中，wrap(·)函数的功能为将关节角度限制在[-π,π]之间。

由于第一电机m1的轴向和基座端的z轴方向平行，因此当基座端处于水平时，第一电机m1的转动不会影响重力作用在工具端的三轴加速度计上面的值，所以可以通过基座端加速度计和工具端加速度计来推算第二电机m2以及第三电机m3的关节角度。

如图7所示，所述装置包括：

获取单元71，用于获取所述第一电机上电时刻对应的第一关节角以及所述第二电机上电时刻对应的第二电气角度和第三电机上电时刻对应的第三电气角度；

具体的，在上电时刻，获取第一电机对应的第一关节角，具体的，所述获取第一电机对应的第一关节角，包括：

通过自检碰限位的方式，确定所述第一电机对应的第一关节角。具体的，通过控制第一电机碰触其机械限位，从而确定所述第一电机对应的第一关节角θj1，通过确定所述第一电机对应的第一关节角之后，可以通过基座端加速度计和工具端加速度计推算所述第二电机的第二关节角以及第三电机的第三关节角。

具体的，由于云台上的三轴电机上安装有线性霍尔传感器，其用于检测电机转子的磁场，从而可以计算得出第一电机对应的第一电气角度、所述第二电机对应的第二电气角度和所述第三电机对应的第三电气角度，进而获取到所述第一电机对应的第一电气角度、所述第二电机对应的第二电气角度和所述第三电机对应的第三电气角度。

在本发明实施例中，所述方法还包括：

计算所述基座端的加速度计模值和工具端的加速度计模值；

根据所述基座端的加速度计模值和工具端的加速度计模值，确定所述云台是否处于静止状态。

具体的，当所述基座端的加速度计模值和工具端的加速度计模值同时处于一个重力加速度时，则确定所述云台处于静止状态，可以理解的是，在所述基座端的加速度计模值和工具端的加速度计模值同时处于一个重力加速度附近时，也可以认为所述云台处于静止状态，具体的，预设误差值，当所述基座端的加速度计模值与重力加速度的误差小于所述误差值，和/或，所述工具端的加速度计模值与重力加速度的误差小于所述误差值，则确定所述云台处于静止状态。

关节角组合单元72，用于根据所述第二电机的第二电气角度、第二极对数以及所述第三电机的第三电气角度、第三极对数，遍历所述第二电机和第三电机的第二关节角和第三关节角，确定所述第一电机、第二电机以及第三电机的关节角组合；

具体的，每一电机对应一极对数，第一电机对应第一极对数，第二电机对应第二极对数，第三电机对应第三极对数，由于第一电机的第一关节角已通过自检碰限位的方式确定，而第二电机的极对数为n2以及第三电机的极对数为n3，则所述第一电机、第二电机以及第三电机的关节角有n2*n3种组合，从而确定所述第一电机、第二电机以及第三电机的关节角组合。

可以理解的是，在所述n2*n3种组合中，存在部分不符合要求的关节角组合，需要进行筛选以排除，因此，在确定所述第一电机、第二电机以及第三电机的关节角组合之后，所述方法还包括：对所述第一电机、第二电机以及第三电机的关节角组合进行筛选，确定筛选后的关节角组合，具体的，确定不符合云台机械限位的关节角组合，排除所述不符合云台机械限位的关节角，例如：第二关节角以及第三关节角中的不符合云台机械限位的关节角，从而确定筛选后的关节角组合。

姿态四元数单元73，用于确定所述关节角组合对应的姿态四元数以及所述基座端的姿态四元数，根据所述基座端的姿态四元数以及关节角组合的姿态四元数，确定所述工具端的姿态四元数；

具体的，所述确定所述关节角组合对应的姿态四元数，包括：

确定每一关节角组合对应的姿态四元数，具体的，对于三轴正交的云台来说，按照“m1-m2-m3”的旋转顺序，即基座端-工具端-末端的旋转顺序，从而确定一个基于基座坐标系的四元数，具体的，所述基于基座坐标系的四元数，其表达式为下式(2)：

其中，θj1为第一电机对应的第一关节角，θj2为第二电机对应的第二关节角，θj3为第三电机对应的第三关节角。

初始化关节角组合单元74，用于根据所述工具端的姿态四元数，结合所述工具端的三轴加速度计的读数，确定所述三轴电机的初始化关节角组合。

在本发明实施例中，所述姿态四元数单元，具体用于：

获取所述基座端的三轴加速度计的读数，固定所述基座端的偏航角，根据所述基座端的三轴加速度计的读数，计算所述基座端的俯仰角和横滚角；

具体的，通过获取所述基座端的三轴加速度计的读数，并固定所述基座端的偏航角，即令所述基座端的偏航角为0，根据所述基座端的三轴加速度计的读数，计算所述基座端的俯仰角和横滚角，其具体计算方式为现有技术，在此不再赘述。通过固定所述基座端的偏航角，并通过基座端的三轴加速度计的读数，计算得出所述基座端的俯仰角和横滚角，从而确定一组欧拉角，有利于根据确定得到的一组欧拉角，确定所述基座端的姿态四元数。

根据所述基座端的偏航角、俯仰角和横滚角，确定所述基座端的姿态四元数。

具体的，假设所述欧拉角中的俯仰角为θp，横滚角为θr，偏航角为θy，且它们的旋转角顺序为“zxy”，则根据所述欧拉角，确定所述基座端的姿态四元数的计算方法如下式(3)：

其中，θp为俯仰角，θr为横滚角，θy为偏航角，θj1为第一电机对应的第一关节角，θj2为第二电机对应的第二关节角，θj3为第三电机对应的第三关节角。

可以理解的是，在对所述第一电机、第二电机以及第三电机的关节角组合进行筛选，确定筛选后的关节角组合，所述确定所述关节角组合对应的姿态四元数，包括：

确定每一筛选后的关节角组合对应的姿态四元数。

具体的，所述根据所述基座端的姿态四元数以及关节角组合的姿态四元数，确定所述工具端的姿态四元数，包括：

对所述基座端的姿态四元数和所述关节角组合的姿态四元数进行四元数乘法，确定所述工具端的姿态四元数。

具体的，假设所述基座端的姿态四元数为p＝[p0p1p2p3]，所述关节角组合的姿态四元数为q＝[q0q1q2q3]，所述工具端的姿态四元数为r，则所述工具端的姿态四元数为所述基座端的姿态四元数与所述关节角组合的姿态四元数的四元数乘法所得到的结果，即所述四元数乘法，如下式(4)：

即，计算得到的工具端的姿态四元数r＝[r0r1r2r3]。

在本发明实施例中，所述装置还包括：

筛选单元，用于对所述第一电机、第二电机以及第三电机的关节角组合进行筛选，确定筛选后的关节角组合，所述确定所述关节角组合对应的姿态四元数。

在本发明实施例中，所述获取单元，具体用于：

通过自检碰限位的方式，确定所述第一电机对应的第一关节角。

在本发明实施例中，所述装置还包括：

静止状态单元，用于计算所述基座端的加速度计模值和工具端的加速度计模值；

根据所述基座端的加速度计模值和工具端的加速度计模值，确定所述云台是否处于静止状态。

在本发明实施例中，所述姿态四元数单元，具体用于：

对所述基座端的姿态四元数和所述关节角组合的姿态四元数进行四元数乘法，确定所述工具端的姿态四元数。

具体的，假设所述工具端的姿态四元数为q＝[q0q1q2q3]，则其对应的旋转矩阵m的计算方式为下式(5)：

即所述旋转矩阵m为：

在本发明实施例中，所述初始化关节角组合单元，具体用于：

获取所述旋转矩阵的列向量；

具体的，获取所述旋转矩阵的最后一列的列向量，即所述旋转矩阵的最后一列元素。

对所述旋转矩阵的列向量和所述重力向量进行点乘运算，生成多个点乘结果；

具体的，获取所述旋转矩阵的最后一列元素[ax1ay1az1]，将所述重力向量[ax2ay2az2]与所述旋转矩阵的最后一列元素[ax1ay1az1]进行向量点乘运算，得到点乘结果，并保存所述点乘结果。

可以理解的是，所述三轴加速度计的读数可能有多个，因此可能出现多个点乘结果，因此，需要从所述多个点乘结果中确定对应的关节角组合作为初始化关节角组合。

将点乘结果的最大值对应的第一关节角、第二关节角以及第三关节角作为所述三轴电机的初始化关节角组合。

具体的，从计算得到的所有点乘结果中，确定所述点乘结果的最大值，所述点乘结果的最大值对应的第二关节角θj2，以及第三关节角θj3，即为所述第二电机和第三电机的真实关节角，由于所述第一电机对应的第一关节角θj1已经确定，进而确定所述三轴电机的第一电机对应的第一初始化关节角、第二电机对应的第二初始化关节角以及第三电机对应的第三初始化关节角，至此，所述三轴电机的关节角初始化完成。

需要说明的是，上述装置可执行本申请实施例所提供的方法，具备执行方法相应的功能模块和有益效果。未在装置实施例中详尽描述的技术细节，可参见本申请实施例所提供的方法。

在本发明实施例中，通过提供一种关节角的初始化装置，应用于飞行器，所述飞行器设置有三轴云台，所述三轴云台包括三轴电机，分别为第一电机、第二电机以及第三电机，所述第二电机对应第二极对数，所述第三电机对应第三极对数，所述三轴云台还包括基座端和工具端，所述工具端设置有三轴加速度计，所述装置包括：获取单元，用于获取所述第一电机上电时刻对应的第一关节角以及所述第二电机上电时刻对应的第二电气角度和第三电机上电时刻对应的第三电气角度；关节角组合单元，用于根据所述第二电机的第二电气角度、第二极对数以及所述第三电机的第三电气角度、第三极对数，遍历所述第二电机和第三电机的第二关节角和第三关节角，确定所述第一电机、第二电机以及第三电机的关节角组合；姿态四元数单元，用于确定所述关节角组合对应的姿态四元数以及所述基座端的姿态四元数，根据所述基座端的姿态四元数以及关节角组合的姿态四元数，确定所述工具端的姿态四元数；初始化关节角组合单元，用于根据所述工具端的姿态四元数，结合所述工具端的三轴加速度计的读数，确定所述三轴电机的初始化关节角组合。通过对第一电机进行自检，确定三轴电机的初始化关节角组合，本发明实施例能够提高三轴云台上电后的自检效率。

请参阅图8，图8是本发明实施例提供一种飞行器的硬件结构示意图。其中，该飞行器可以是无人飞行器(unmannedaerialvehicle，uav)，无人飞船等电子设备。

如图8所示，该飞行器800包括一个或多个处理器801以及存储器802。其中，图8中以一个处理器801为例。

处理器801和存储器802可以通过总线或者其他方式连接，图8中以通过总线连接为例。

存储器802作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块，如本发明实施例中的一种关节角的初始化方法对应的单元(例如，图7所述的各个模块或单元)。处理器801通过运行存储在存储器802中的非易失性软件程序、指令以及模块，从而执行关节角的初始化方法的各种功能应用以及数据处理，即实现上述方法实施例关节角的初始化方法以及上述装置实施例的各个模块和单元的功能。

存储器802可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器802可选包括相对于处理器801远程设置的存储器，这些远程存储器可以通过网络连接至处理器801。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

所述模块存储在所述存储器802中，当被所述一个或者多个处理器801执行时，执行上述任意方法实施例中的关节角的初始化方法，例如，执行以上描述的图3至图6所示的各个步骤；也可实现图7所述的各个模块或单元的功能。

请参阅图9和图10，所述飞行器800还包括动力系统803，所述动力系统803用于飞行器提供飞行动力，所述动力系统803与处理器801连接。所述动力系统803包括：驱动电机8031及电调8032，所述电调8032与驱动电机8031电连接，用于控制所述驱动电机8031。具体的，所述电调8032基于处理器801执行上述关节角的初始化方法，以方便生成控制指令，通过控制指令控制该驱动电机8031。

所述飞行器800可执行本发明实施例一所提供的关节角的初始化方法，具备执行方法相应的功能模块和有益效果。未在飞行器实施例中详尽描述的技术细节，可参见本发明实施例一所提供的关节角的初始化方法。

本发明实施例提供了一种计算机程序产品，所述计算机程序产品包括存储在非易失性计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，使所述计算机执行如上所述的关节角的初始化方法。例如，执行以上描述的图3中的方法步骤s10至步骤s40。

本发明实施例还提供了一种非易失性计算机存储介质，所述计算机存储介质存储有计算机可执行指令，该计算机可执行指令被一个或多个处理器执行，例如图8中的一个处理器801，可使得上述一个或多个处理器可执行上述任意方法实施例中的关节角的初始化方法，例如，执行上述任意方法实施例中的关节角的初始化方法，例如，执行以上描述的图3至图6所示的各个步骤；也可实现图7所示的各个模块或单元的功能。

以上所描述的装置或设备实施例仅仅是示意性的，其中所述作为分离部件说明的单元模块可以是或者也可以不是物理上分开的，作为模块单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络模块单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对相关技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如rom/ram、磁碟、光盘等，包括若干指令用直至得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的范围。

起点商标作为专业知识产权交易平台，可以帮助大家解决很多问题，如果大家想要了解更多知产交易信息请点击 【在线咨询】或添加微信 【19522093243】与客服一对一沟通，为大家解决相关问题。

此文章来源于网络,如有侵权,请联系删除