CN111998822B - 一种空间角度姿态计算方法 - Google Patents

Abstract

本发明提供了一种空间角度姿态计算方法，包括：获取IMU各轴在梢坐标系中的初始方向矢量和初始欧拉角；计算IMU各轴在导航坐标系中的初始方向矢量；利用IMU各轴在系统坐标系和导航坐标系中的初始方向矢量，计算从导航坐标系到系统坐标系的转换矩阵；系统工作时，利用IMU输出的实时欧拉角，计算IMU各轴在导航坐标系内的实时方向矢量；利用转换矩阵，计算IMU各轴在系统坐标系中的实时方向矢量；获取根元件各轴在系统坐标系内的实时方向矢量；运用空间矢量运算计算惯性测量元件与根元件在各自由度上的转角。本发明能够解决现有技术只能测量一个维度上的角度值，不适用于多旋转自由度零件的问题。

Description

一种空间角度姿态计算方法

技术领域

本发明涉及数据处理技术领域，特别是涉及一种空间角度姿态计算方法。

背景技术

空间角度姿态计算是机械系统中的常见需求，现有技术中，有多种不同类型的角度测量传感器，如电位型、霍尔型、栅格型等，来实现空间角度姿态的测量和计算，这些角度传感器各自的工作原理不尽相同，但利用这些角度传感器进行测量时，都只能测量一个维度上的角度值。

对于多旋转自由度零件，例如，根、转、枢、梢四个零件通过三个旋转自由度（d1、d2、d3）相互连接，形成如图1所示的类似人手执笔的结构，在实际应用中，需要测量d1、d2、d3的旋转角度，使用传统的角度测量传感器将无法实现d1、d2、d3旋转角度的测量，此外，角度传感器在工作中必须以一侧零件为固定参考，测量另一侧零件的相对转动，而无法感知其参考点自身的姿态变化，根零件自身的位置、朝向等的变化也需要进行测量，这些因素都限制了传统角度传感器的使用。

发明内容

本发明的目的在于提供一种空间角度姿态计算方法，以解决现有技术只能测量一个维度上的角度值，不适用于多旋转自由度零件的问题。

本发明提供一种空间角度姿态计算方法，应用于机械旋转装置中，所述机械旋转装置包括依次转动连接的根元件、腕元件、枢元件和梢元件，根元件和腕元件之间的运动副为桡关节，腕元件和枢元件之间的运动副为腕关节，枢元件和梢元件之间的运动副为掌关节，梢元件在枢元件内绕梢轴旋转，梢元件上安装有惯性测量元件；

所述空间角度姿态计算方法包括：

在初始状态下对系统进行标定，获取初始状态下惯性测量元件各轴在梢坐标系中的初始方向矢量，并读取此时惯性测量元件输出的初始欧拉角；

通过惯性测量元件各轴在梢坐标系中的初始方向矢量和初始欧拉角，计算惯性测量元件各轴在导航坐标系中的初始方向矢量；

利用惯性测量元件各轴在系统坐标系中的初始方向矢量和在导航坐标系中的初始方向矢量，计算从导航坐标系到系统坐标系的转换矩阵；

系统工作时，利用惯性测量元件输出的实时欧拉角，计算惯性测量元件各轴在导航坐标系内的实时方向矢量；

利用从导航坐标系到系统坐标系的转换矩阵，计算惯性测量元件各轴在系统坐标系中的实时方向矢量；

获取根元件各轴在系统坐标系内的实时方向矢量；

运用空间矢量运算计算惯性测量元件与根元件在各自由度上的转角。

进一步地，计算惯性测量元件各轴在系统坐标系中的实时方向矢量的步骤具体包括：

根据初始欧拉角和惯性测量元件各轴的方向矢量，利用初始的从梢坐标系到导航坐标系的转换矩阵计算惯性测量元件各轴在导航坐标系中的初始方向矢量；

利用惯性测量元件各轴在导航坐标系中的初始方向矢量和惯性测量元件各轴在系统坐标系中的初始方向矢量计算从导航坐标系到系统坐标系的转换矩阵；

当惯性测量元件输出实时欧拉角时，实时计算从梢坐标系到导航坐标系的转换矩阵；

利用惯性测量元件各轴的方向矢量和实时计算得到的从梢坐标系到导航坐标系的转换矩阵计算惯性测量元件各轴在导航坐标系中的实时方向矢量；

利用从导航坐标系到系统坐标系的转换矩阵计算惯性测量元件各轴在系统坐标系中的实时方向矢量。

进一步地，从梢坐标系到导航坐标系的转换矩阵为：

其中，ψ、θ、φ分别为三向旋转角，（x₀,y₀,z₀）为系统坐标系中的坐标，（x,y,z）为导航坐标系中的坐标。

进一步地，从导航坐标系到系统坐标系的转换矩阵Tns的计算公式如下：

其中，imu_ENU_n0为惯性测量元件各轴在导航坐标系中的初始方向矢量，imu_S_n0为惯性测量元件各轴在系统坐标系中的初始方向矢量，n=1,2,3。

进一步地，根元件的坐标系为（X,Y,Z），梢元件的坐标系为（x,y,z），运用空间矢量运算计算惯性测量元件与根元件在各自由度上的转角的步骤中，计算的转角包括桡关节转角、腕关节转角和掌关节转角；

桡关节转角为梢元件z轴在根元件XZ平面内的投影与根元件Z轴之间的夹角；

腕关节转角为梢元件z轴与根元件XZ平面之间的夹角；

掌关节转角为根元件Y轴在梢元件xy平面内的投影与梢元件x轴之间的夹角。

本发明的有益效果：

本发明提供的空间角度姿态计算方法，通过使用惯性测量元件（IMU）对机械旋转结构两端所成夹角进行实时测量和计算，可同时完成多个自由度的相对角度测量和计算，解决了现有技术只能测量一个维度上的角度值，不适用于多旋转自由度零件的问题；此外，惯性测量元件价格低廉，在许多嵌入式机电一体化系统中已经被安装，因此采用本发明可以有效节省成本。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是人手执笔的结构示意图；

图2是本发明一实施例的机械旋转装置的结构示意图；

图3是本发明一实施例的空间角度姿态计算方法的流程图；

图4是本发明一实施例的空间角度姿态计算方法的逻辑原理图。

具体实施方式

为使本发明的目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方式做详细的说明。附图中给出了本发明的若干实施例。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容更加透彻全面。

需要说明的是，当元件被称为“固设于”另一个元件，它可以直接在另一个元件上或者也可以存在居中的元件。当一个元件被认为是“连接”另一个元件，它可以是直接连接到另一个元件或者可能同时存在居中元件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”、“上”、“下”以及类似的表述只是为了说明的目的，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。本文所使用的术语“及／或”包括一个或多个相关的所列项目的任意的和所有的组合。

本发明的一实施例提出的空间角度姿态计算方法，应用于机械旋转装置中，请参阅图2，所述机械旋转装置包括依次转动连接的根元件10、腕元件20、枢元件30和梢元件40，根元件10和腕元件20之间的运动副为桡关节11，腕元件20和枢元件30之间的运动副为腕关节12，枢元件30和梢元件40之间的运动副为掌关节13，梢元件40在枢元件30内绕梢轴旋转，其中，根元件10形成了类似图1中的手臂，腕元件20形成了类似图1中的手腕，枢元件30形成了类似图1中的手掌，梢元件40形成了类似图1中的笔。

梢元件40上安装有惯性测量元件（可以安装在内部，图2未示），因为惯性测量元件（IMU）与梢元件40固连，故使用梢元件40的坐标系(x, y, z)来替代IMU坐标系（IMU自身的坐标系）。IMU的输出的数据是其自身相对于地磁绝对坐标系的欧拉偏转角。通常使用的导航坐标系是北半球导航坐标系（简称导航坐标系），也即东北天坐标系（O-ENU），在计算欧拉角时遵循ZYX的旋转顺序。图2中，根元件10的坐标系为（X,Y,Z），根元件10的坐标系也称系统坐标系；梢元件40的坐标系为（x,y,z）。

请结合图3和图4，所述空间角度姿态计算方法包括步骤S101~S107：

S101，在初始状态下对系统进行标定，获取初始状态下惯性测量元件各轴在梢坐标系中的初始方向矢量，并读取此时惯性测量元件输出的初始欧拉角。

其中，在初始状态下对系统进行标定，此时可知IMU各轴在梢坐标系（即IMU坐标系）中的初始方向矢量xi,yi,zi，也知道IMU各轴在系统坐标系中的初始的方向矢量xs0,ys0,zs0，并可读取此时IMU输出的欧拉角，IMU在清零位的初始欧拉角 imu_euler_0由系统从硬件直接读取。

在IMU自身坐标轴中选择3个相互垂直的方向矢量 imu_i_1, imu_i_2, imu_i_3，清零位时与系统X轴最接近的为imu_i_1，与系统Z方向最接近的是imu_i_2，再通过右手定则选择imu_i_3，以下称这三个方向矢量为IMU各轴。

IMU各轴在系统坐标系中的方向矢量为imu_S_1, imu_S_2, imu_S_3。系统在清零位时，IMU各轴在系统坐标系中的初始方向矢量为imu_S_10, imu_S_20, imu_S_30（即xs0,ys0,zs0），此为设定值，由程序初始化时赋值。

S102，通过惯性测量元件各轴在梢坐标系中的初始方向矢量和初始欧拉角，计算惯性测量元件各轴在导航坐标系中的初始方向矢量。

其中，惯性测量元件各轴在导航坐标系中的初始方向矢量为xn0,yn0,zn0。

S103，利用惯性测量元件各轴在系统坐标系中的初始方向矢量和在导航坐标系中的初始方向矢量，计算从导航坐标系到系统坐标系的转换矩阵。

S104，系统工作时，利用惯性测量元件输出的实时欧拉角，计算惯性测量元件各轴在导航坐标系内的实时方向矢量。

其中，IMU的实时欧拉角imu_euler由系统从硬件直接读取。IMU各轴在导航坐标系中的实时方向矢量为xn,yn,zn。

S105，利用从导航坐标系到系统坐标系的转换矩阵，计算惯性测量元件各轴在系统坐标系中的实时方向矢量。

其中，步骤S105中，计算惯性测量元件各轴在系统坐标系中的实时方向矢量的步骤具体包括：

根据初始欧拉角imu_euler_0和惯性测量元件各轴的方向矢量imu_i_n，n=1,2,3，利用初始的从梢坐标系到导航坐标系的转换矩阵计算惯性测量元件各轴在导航坐标系中的初始方向矢量imu_ENU_n0（即xn0,yn0,zn0），在ZYX旋序下，从梢坐标系到导航坐标系的转换矩阵为：

其中，ψ、θ、φ分别为三向旋转角，（x₀,y₀,z₀）为系统坐标系中的坐标，（x,y,z）为导航坐标系中的坐标；

然后利用惯性测量元件各轴在导航坐标系中的初始方向矢量和惯性测量元件各轴在系统坐标系中的初始方向矢量计算从导航坐标系到系统坐标系的转换矩阵，其中，从导航坐标系到系统坐标系的转换矩阵Tns的计算公式如下：

其中，imu_ENU_n0为惯性测量元件各轴在导航坐标系中的初始方向矢量，n=1,2,3。

当惯性测量元件输出实时欧拉角imu_euler时，实时计算从梢坐标系到导航坐标系的转换矩阵Tin；

利用惯性测量元件各轴的方向矢量imu_i_n和实时计算得到的从梢坐标系到导航坐标系的转换矩阵Tin计算惯性测量元件各轴在导航坐标系中的实时方向矢量imu_ENU_n（即xn,yn,zn）；

最后利用从导航坐标系到系统坐标系的转换矩阵Tns计算惯性测量元件各轴在系统坐标系中的实时方向矢量imu_S_n（即xs,ys,zs）。

S106，获取根元件各轴在系统坐标系内的实时方向矢量。

其中，具体实施时，可以通过系统传动链结构参数和实时角度信息获取根元件各轴在系统坐标系内的实时方向矢量Xs,Ys,Zs。

S107，运用空间矢量运算计算惯性测量元件与根元件在各自由度上的转角。

其中，运用空间矢量运算计算惯性测量元件与根元件在各自由度上的转角的步骤中，计算的转角包括桡关节转角a1、腕关节转角a2和掌关节转角a3；

桡关节转角a1为梢元件z轴在根元件XZ平面内的投影与根元件Z轴之间的夹角；

腕关节转角a2为梢元件z轴与根元件XZ平面之间的夹角；

掌关节转角a3为根元件Y轴在梢元件xy平面内的投影与梢元件x轴之间的夹角。

综上，根据本发明提供的空间角度姿态计算方法，通过使用惯性测量元件（IMU）对机械旋转结构两端所成夹角进行实时测量和计算，可同时完成多个自由度的相对角度测量和计算，解决了现有技术只能测量一个维度上的角度值，不适用于多旋转自由度零件的问题；此外，惯性测量元件价格低廉，在许多嵌入式机电一体化系统中已经被安装，采用本发明可以有效节省成本。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、 “示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (4)

1.一种空间角度姿态计算方法，应用于机械旋转装置中，所述机械旋转装置包括依次转动连接的根元件、腕元件、枢元件和梢元件，根元件和腕元件之间的运动副为桡关节，腕元件和枢元件之间的运动副为腕关节，枢元件和梢元件之间的运动副为掌关节，梢元件在枢元件内绕梢轴旋转，其特征在于，梢元件上安装有惯性测量元件；

所述空间角度姿态计算方法包括：

在初始状态下对系统进行标定，获取初始状态下惯性测量元件各轴在梢坐标系中的初始方向矢量，并读取此时惯性测量元件输出的初始欧拉角；

通过惯性测量元件各轴在梢坐标系中的初始方向矢量和初始欧拉角，计算惯性测量元件各轴在导航坐标系中的初始方向矢量；

利用惯性测量元件各轴在系统坐标系中的初始方向矢量和在导航坐标系中的初始方向矢量，计算从导航坐标系到系统坐标系的转换矩阵；

系统工作时，利用惯性测量元件输出的实时欧拉角，计算惯性测量元件各轴在导航坐标系内的实时方向矢量；

利用从导航坐标系到系统坐标系的转换矩阵，计算惯性测量元件各轴在系统坐标系中的实时方向矢量；

获取根元件各轴在系统坐标系内的实时方向矢量；

运用空间矢量运算计算惯性测量元件与根元件在各自由度上的转角。

2.根据权利要求1所述的空间角度姿态计算方法，其特征在于，计算惯性测量元件各轴在系统坐标系中的实时方向矢量的步骤具体包括：

根据初始欧拉角和惯性测量元件各轴的方向矢量，利用初始的从梢坐标系到导航坐标系的转换矩阵计算惯性测量元件各轴在导航坐标系中的初始方向矢量；

利用惯性测量元件各轴在导航坐标系中的初始方向矢量和惯性测量元件各轴在系统坐标系中的初始方向矢量计算从导航坐标系到系统坐标系的转换矩阵；

当惯性测量元件输出实时欧拉角时，实时计算从梢坐标系到导航坐标系的转换矩阵；

利用惯性测量元件各轴的方向矢量和实时计算得到的从梢坐标系到导航坐标系的转换矩阵计算惯性测量元件各轴在导航坐标系中的实时方向矢量；

利用从导航坐标系到系统坐标系的转换矩阵计算惯性测量元件各轴在系统坐标系中的实时方向矢量。

3.根据权利要求2所述的空间角度姿态计算方法，其特征在于，从导航坐标系到系统坐标系的转换矩阵Tns的计算公式如下：

Tns=[imu_ENU_10,imu_ENU_20,imu_ENU_30]^-1×[imu_S_10,imu_S_20,imu_S_30]

其中，imu_ENU_n0为惯性测量元件各轴在导航坐标系中的初始方向矢量，imu_S_n0为惯性测量元件各轴在系统坐标系中的初始方向矢量，n=1,2,3。

4.根据权利要求1所述的空间角度姿态计算方法，其特征在于，根元件的坐标系为（X,Y,Z），梢元件的坐标系为（x,y,z），运用空间矢量运算计算惯性测量元件与根元件在各自由度上的转角的步骤中，计算的转角包括桡关节转角、腕关节转角和掌关节转角；

桡关节转角为梢元件z轴在根元件XZ平面内的投影与根元件Z轴之间的夹角；

腕关节转角为梢元件z轴与根元件XZ平面之间的夹角；

掌关节转角为根元件Y轴在梢元件xy平面内的投影与梢元件x轴之间的夹角。

Images