一种基于双目视觉的非合作目标相对状态解算方法与流程

本发明涉及计算机视觉技术领域，尤其涉及一种基于双目视觉的非合作目标相对状态解算方法，该方法利用双目视觉原理解算目标坐标系相对相机坐标系状态。

背景技术：

双目视觉作为两坐标系之间相对状态的测量方案，一直都是科学研究的热点，尤其在室内定位、非合作目标相对姿态估计等领域，相关研究的相对状态包括，两坐标系之间的相对姿态、相对转速和相对位置。然而相对转速解算的研究相对较少，且解算精度较低，导致该结果的原因包括：

1)特征点位置精度较低；

2)由于复杂环境下，前后时刻特征点匹配难度较大，即跟踪同一组测量点难度较大；

3)载体运动信息未知。

经过检索发现：

文章号为1674-1579(2010)01-0024-07刘智勇等在《空间控制技术与应用》在2010年第1期36卷24～29页上发表的《转动惯量未知的非合作目标角速度估计方法研究》，提出了一种针对转动惯量未知的非合作目标的姿态角速度估计问题，将解算得到的非合作目标的惯性姿态作为测量信息，估计姿态角速度和姿态动力学参数(即转动惯量比)。首先，应用非线性控制系统的几何理论对待估计的状态扩展系统进行能观性分析。然后，利用ukf设计相应的滤波估计方法。仿真结果表明，该文章所设计的方法能够估计出非合作目标的姿态角速度与转动惯量比。但是，该文章所设计的方法，仍然存在如下问题：

该文中提出的观测量未给出具体的计算方法，而基于双目视觉实现特征匹配，其坐标计算、四算数估计时误差较大，该文章所提出的方法仍无法解决解算精度较低的问题。

目前没有发现同本发明类似技术的说明或报道，也尚未收集到国内外类似的资料。

技术实现要素：

针对现有技术中存在的上述不足，本发明的目的是提供一种基于双目视觉的非合作目标相对状态解算方法，该方法能够提高相机坐标系与目标坐标系相对姿态解算精度以及提高相机坐标系与目标坐标系相对转速解算精度。

为实现上述目的，本发明是通过以下技术方案实现的。

一种基于双目视觉的非合作目标相对状态解算方法，包括如下步骤：

步骤s1：解算载体表面特征点在相机坐标系下的3d坐标；

步骤s2：解算目标坐标系相对相机坐标系的相对姿态；

步骤s3：解算目标坐标系相对相机坐标系的相对转速。

优选地，步骤s1，包括如下步骤：

步骤s1.1：如图1所示，基于双目视觉原理解算非合作目标相对航天器的相对状态的原理是在航天器的质心处安装双目相机，航天器记为l,以左相机的相机坐标系作为参考坐标系，记为非合作目标记为f,由初始时刻获取的特征点建立的坐标系建立目标坐标系，记为利用双目相机采集图像，记录图像采集的时刻ti或记录相邻两个图像采集的间隔δti，其中i表示采集图像的索引；

δti＝ti+1-ti(1)；

步骤s1.2：利用sift或surf算法匹配双目相机的左、右相机图像的载体表面特征点，记ⁱn表示在ti时刻匹配成功的特征点个数；

步骤s1.3：利用双目视觉原理计算载体表面特征点在双目相机的左相机坐标系下的3d坐标。如图6所示，利用双目相机计算特征点的3d坐标时涉及到计算机图像坐标系，成像平面坐标系，相机坐标系和目标坐标系。of-xyz表示目标坐标系，也即^lo0-uv和^ro0-uv分别表示左右相机的计算机图像坐标系；和分别表示左右相机的成像平面坐标系，ol-xyz和or-xyz分别表示左右相机坐标系，(u0,v0)为^lo0-uv和^ro0-uv的中心，也是和的原点，在本发明中，ol-xyz即为空间中一点p在中的坐标为(x,y,z)，该点在^lo0-uv和^ro0-uv中的坐标记为(ul,vl)和(ur,vr)。

特征点p在^lo0-uv和^ro0-uv中的坐标与(x,y,z)的关系用(1a)～(1b)表示，联立式(1a)～(1b)可解出(x,y,z)，如式(1c)所示。

其中，f表示相机的焦距；dx,dy分别表示每个像素点在计算机成像平面坐标系的x轴和y轴方向上的物理尺寸；b表示左右相机之间的基线长度；tb＝[-b,0,0]′。

记表示在ti时刻匹配成功的第k个特征点在下的3d坐标，k＝1,2,…,ⁱn。

优选地，步骤s2，包括以下步骤：

步骤s2.1：建立目标坐标系

利用载体表面特征点在相机坐标系的坐标建立目标坐标系在目标坐标系下的坐标为二者存在以下关系：

其中，t3×1＝[t1t2t3]′；r表示旋转矩阵，t表示平移向量；r1,r2,r3分别表示旋转矩阵的列向量rij表示旋转矩阵的元素；t1t2t3分别表示平移向量的元素；符号[·]′表示矩阵或向量的转置，x′k,y′k,z′k表示在目标坐标系下的坐标值；0表示在t0时刻；

目标坐标系建立方法如下：1)假设在t0时刻，三个特征点在中的坐标值分别为相应地在中的坐标值分别记为如图3所示；2)因相机坐标系遵守右手法则，为使目标坐标系与其一致，为坐标系原点，选择向量的方向为坐标系的x轴正向，此时，应保证x2＞x1，改变特征点的顺序，易满足条件；3)坐标系的y轴在由点确定的平面内，且与x轴垂直；4)z轴的方向与确定的平面垂直，遵守右手法则。

根据坐标系建立的原则可知，特征点在的坐标为，x′2为特征点之间的距离，如(3b)所示，式(3c)～(3d)确定及y轴正向：

其中，α表示向量与的夹角；

将式(3a)～(3d)代入式(2)求解r和t：

r3＝r1×r2(3g)

步骤s2.2：建立七参数模型

坐标系与之间的变换关系利用七参数模型表示，即：

μi,ri,ti分别表示在ti时刻变换到的尺度因子、旋转矩阵和平移向量；为解算出旋转矩阵，应保证在ti时刻ⁱn≥4；

欧拉角与旋转矩阵存在变换关系，即可以用欧拉角表示旋转矩阵，在ti时刻，经过一定顺序的三次欧拉角θi,ψi旋转可以与平行，相应的旋转矩阵采用下式表示，为便于简洁表达，在此将欧拉角的下标省略掉：

步骤s2.3：求解载体表面特征点在下的坐标

在t0时刻，已建立目标坐标系利用ti-1时刻的变换关系计算ti时刻载体表面特征点在目标坐标系中的坐标，进而得到ti时刻同一组载体表面特征点分别在和中的坐标。只要相邻时刻，匹配成功的特征点ⁱn≥4即可解算旋转矩阵ri，μi和ti；

步骤s2.4：求解目标坐标系与相机坐标系的相对姿态

从步骤s2.3，得到在ti时刻ⁱn个匹配成功的特征点在和下的坐标，分别记为和利用两个坐标系下的特征点计算出七参数其中，δx,δy,δz分别表示和之间的平移量，也即ti＝[δx,δy,δz]′；

在处做泰勒展开如下：

其中，y表示在ti时刻特征点在中坐标组成的列向量，大小为3ⁱn×1；a表示系数矩阵，矩阵维数为3ⁱn×7；l表示在ti时刻与dx无关的常数项，大小为3ⁱn×1。

a＝[e3×3,μm,n](8b)

其中，e3×3表示单位矩阵，m表示对旋转矩阵求导结果相关的系数矩阵，n表示与dμ相关的系数向量。

当ⁱn＞3时，利用最小二乘方法求解七参数向量，迭代过程如下：

步骤a，设置初始参数

步骤b，将代入上式(8a)～(8d)，计算a,l；

步骤c，根据误差方程求解其中，v表示误差；

步骤d，根据最小二乘原理求解改正数其中，a'表示a的转置；

步骤e，更新七参数值

步骤f，若则停止迭代；否则执行步骤g

步骤g，重复步骤b～步骤f；

步骤s2.5：利用整体最小二乘算法进一步提高解算精度

在步骤s2.4迭代过程中，仅考虑特征点在中的误差，而实际应用中，系数矩阵也存在误差。针对这一问题，采用整体最小二乘原理进一步提高七参数的解算精度。新建立的eiv模型如式(10a)所示。

ea＝vec(eai)，vec(·)表示将矩阵按列堆叠转化为列向量；ea表示系数矩阵的残差，表示每次迭代中系数矩阵残差的预测值；

目标函数：

其中，ey表示观测量y的残差，ea表示系数矩阵的残差重构的列向量λ分别表示m×1拉格朗日乘数向量，qy和qa分别表示观测量y权逆阵和系数矩阵残差的权逆阵，表示直乘，im表示m×m单位矩阵，y表示由特征点在坐标系下的坐标构成的列向量；

为求解分别对变量求导，求目标函数最值过程如下，其中“～”表示相应变量的预测值。

对ey求导得：

对ea求导得：

对λ求导得：

对求导得：

由式(12a～12b)可得

将式(13a)～(13b)代入(12c)得

其中，

其中，下标i表示第i次迭代，表示单位权方差，ql表示q0表示系数矩阵方差，qe表示权逆阵，表示第i+1次迭代的观测量误差，表示第i+1次迭代的系数矩阵误差；

步骤s2.5中，迭代计算的初始值由步骤s2.4的结果给出，根据以上迭代计算，当时迭代停止，求解出的x即为进一步迭代后的七参数；

步骤s2.6：计算相对姿态

根据步骤s2.5中得到的欧拉角求解出旋转矩阵，依据下式求解出四元数q＝[ξ1ξ2ξ3ξ4]^t：

优选地，步骤s3，包括如下步骤：

步骤s3.1，建立动力学模型

步骤s3.2，建立运动学模型

ρ＝[ξ1ξ2ξ3]^t(17)

其中：

步骤s3.3，相对转速计算公式

ω＝d(q)ωf-ωl(18)

ωl,ωf,ω分别表示相机坐标系的转速、目标坐标系的转速及目标坐标系相对相机坐标系的转速；表示相对转速的导数；il和if分别表示航天器和非合作目标的转动惯量，nl和nf分别表示航天器和非合作目标受到的外力，d(q)表示由四元数表示的旋转矩阵，q表示四元数，表示四元数的导数，ρ表示四元数的矢量部分；

d(q)＝[(ξ4)²-ρ^tρ]i3×3+2ρρ^t-2ξ4[ρ×](19)

[ρ×]表示叉乘。

步骤3.4，无损卡尔曼滤波估计转速

定义状态方程如下：

其中，表示状态量的导数，η＝[ξ1,ξ2,ξ3,ξ4,ωx,ωy,ωz,it1,it2,it3,it4,it5,it6]^t

f(η)表示状态量与其导数映射函数，w表示均值为零的高斯噪声；

观测方程如下：

z＝h(η)+v(21)

其中，z表示观测量，η表示状态量，v表示均值为零的高斯白噪声，h(η)表示状态量与观测量的映射关系；

其中，ξ1,ξ2,ξ3,ξ4,分别为相对姿态的四元数；ω＝[ωx,ωy,ωz]^t分别表示相对转速；it1,it2,it3,it4,it5,it6分别表示转动惯量的元素；

其中，转动惯量为观测量为：z＝[ξ1,ξ2,ξ3,ξ4]^t

根据观测量和测量量的关系建立模型，采用ukf滤波解算状态，包括如下步骤：

步骤a：利用ut变换计算sigma点

其中，λ＝α²(n+κ)-nα表示sigma点围绕ηk的分布特性，κ表示比例因子；

步骤b：一步预测

步骤c：二次采样

步骤d：测量更新

(zk+1|k)i＝h((χk+1|k)i)(26)

步骤e：滤波更新

β结合η的先验分布信息得出，最优值可设置为2；

χk表示k时刻的sigma点集；

ηk表示第k次迭代的状态量；

表示变换后sigma点集的第i列；

w^m表示均值的加权值；

表示一步预测状态；

pk+1|k表示后验方差阵；

w^c表示方差的权值；

qk表示噪声的方差矩阵；

zk+1|k表示k时刻预测k+1的测量值的估计值；

表示预测观测值；

χk+1|k表示根据一步预测值再次使用ut变换，产生的新sigma点集；

kk+1表示在k+1时刻的滤波增益矩阵；

表示k+1时刻的状态量估计值；

表示量测输出变量的方差矩阵；

是协方差阵；

ζ表示一个标量，由经验得到；

n表示系统状态维数；

ukf滤波器的输入量为由四元数组成的测量值，根据不同时刻的输入通过不断迭代，最终可求出不同时刻，非合作目标相对相机坐标系的相对状态。

非合作相对相机坐标系的相对状态可表示为

η′＝[ξ1，ξ2，ξ3，ξ4，ωx，ωy，ωz，t1，t2，t3]。

与现有技术相比，本发明具有如下有益效果：

1)本发明利可以计算相对位置、相对姿态和相对转速；

2)本发明可以不需要跟踪一组特征点，只要保证相邻时刻匹配成功特征点数不少于3即可实现相对状态估计；

3)本发明相对状态估计精度高；

4)本发明不仅估计两坐标系的相对转速，还估计两坐标系的相对位置和相对姿态。

5)本发明提出了一种基于双目视觉的非合作目标相对状态解算方法，该方法不需要持续跟踪同一组特征点，只需在前后时刻匹配成功3个以上特征点即可，在相对转速解算算法中，采用ukf滤波器提高了相对转速解算精度。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明方法过程示意图；

图2为航天器与非合作目标示意图；

图3为目标坐标系与相机坐标系关系图；

图4为特征点匹配过程示意图；

图5为本发明方法流程图；

图6为双目视觉原理图。

具体实施方式

下面对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

实施例

本实施例提供了一种基于双目视觉的非合作目标相对状态解算方法，包括以下步骤：

步骤s1：解算载体表面特征点在相机坐标系下的3d坐标；

步骤s2：解算目标坐标系相对相机坐标系的相对姿态；

步骤s3：解算目标坐标系相对相机坐标系的相对转速。

其中：

步骤1包括如下步骤：

步骤s1.1：如图1所示，基于双目视觉原理解算非合作目标相对航天器的相对状态的原理是在航天器的质心处安装双目相机，航天器记为l,以左相机的相机坐标系作为参考坐标系，记为非合作目标记为f,由初始时刻获取的特征点建立的坐标系建立目标坐标系，记为利用双目相机采集图像，记录图像采集的时刻ti或记录相邻两个图像采集的间隔δti，其中i表示采集图像的索引；

δti＝ti+1-ti(1)；

步骤s1.2：利用sift或surf算法匹配双目相机的左、右相机图像的载体表面特征点，记ⁱn表示在ti时刻匹配成功的特征点个数；

步骤s1.3：利用双目视觉原理计算载体表面特征点在双目相机的左相机坐标系下的3d坐标。如图6所示，利用双目相机计算特征点的3d坐标时涉及到计算机图像坐标系，成像平面坐标系，相机坐标系和目标坐标系。of-xyz表示目标坐标系，也即^lo0-uv和^ro0-uv分别表示左右相机的计算机图像坐标系；和分别表示左右相机的成像平面坐标系，ol-xyz和or-xyz分别表示左右相机坐标系，(u0,v0)为^lo0-uv和^ro0-uv的中心，也是和的原点，在本发明专利中，ol-xyz即为空间中一点p在中的坐标为(x,y,z)，该点在^lo0-uv和^ro0-uv中的坐标记为(ul,vl)和(ur,vr)。

特征点p在^lo0-uv和^ro0-uv中的坐标与(x,y,z)的关系用(1a)～(1b)表示，联立式(1a)～(1b)可解出(x,y,z)，如式(1c)所示。

其中，f表示相机的焦距；dx,dy分别表示每个像素点在计算机成像平面坐标系的x轴和y轴方向上的物理尺寸；b表示左右相机之间的基线长度；tb＝[-b,0,0]′。

记表示在ti时刻匹配成功的第k个特征点在下的3d坐标，k＝1,2,…,ⁱn。

步骤2包括以下步骤：

步骤s2.1：建立目标坐标系

利用载体表面特征点在相机坐标系的坐标建立目标坐标系在目标坐标系下的坐标为二者存在以下关系：

其中，t3×1＝[t1t2t3]′；r表示旋转矩阵，t表示平移向量；r1,r2,r3分别表示旋转矩阵的列向量rij表示旋转矩阵的元素；t1t2t3分别表示平移向量的元素；符号[·]′表示矩阵或向量的转置，x′k,y′k,z′k表示在目标坐标系下的坐标值；0表示在t0时刻；

目标坐标系建立方法可以用图3表示，相机坐标系遵守右手法则，为使目标坐标系与其一致，为坐标系原点，选择向量的方向为坐标系的x轴正向，此时，应保证x2＞x1，改变特征点的顺序，易满足条件。坐标系的y轴在由点确定的平面内，且与x轴垂直。

根据坐标系建立的原则可以，特征点在的坐标为，x2′为特征点的距离，如(3b)所示，式(3c)～(3d)确定的及y轴正向。

其中，α表示向量与的夹角；

将式(3a)～(3d)代入式(2)求解r和t：

r3＝r1×r2(3g)

步骤s2.2：建立七参数模型

坐标系与之间的变换关系利用七参数模型表示，即：

μi,ri,ti分别表示在ti时刻变换到的尺度因子、旋转矩阵和平移向量；为解算出旋转矩阵，应保证在ti时刻ⁱn≥4；

欧拉角与旋转矩阵存在变换关系，即可以用欧拉角表示旋转矩阵，在ti时刻，经过一定顺序的三次欧拉角θi,ψi旋转可以与平行，相应的旋转矩阵采用下式表示，为便于简洁表达，在此将欧拉角的下标省略掉：

步骤s2.3：求解载体表面特征点在下的坐标

在t0时刻，已建立目标坐标系利用ti-1时刻的变换关系计算ti时刻载体表面特征点在目标坐标系中的坐标，进而得到ti时刻同一组载体表面特征点分别在和中的坐标。理论上讲，只要相邻时刻，匹配成功的特征点ⁱn≥4即可解算旋转矩阵ri，μi和ti；

步骤s2.4：求解目标坐标系与相机坐标系的相对姿态

从步骤s2.3，得到在ti时刻ⁱn个匹配成功的特征点在和下的坐标，分别记为和利用两个坐标系下的特征点计算出七参数其中，δx,δy,δz分别表示和之间的平移量，也即ti＝[δx,δy,δz]′；

在处做泰勒展开如下：

其中，y表示在ti时刻特征点在中坐标组成的列向量，大小为3ⁱn×1；a表示系数矩阵，矩阵维数为3ⁱn×7；l表示在ti时刻与dx无关的常数项，大小为3ⁱn×1。

a＝[e3×3,μm,n](8b)

其中，e3×3表示单位矩阵，m表示对旋转矩阵求导结果相关的系数矩阵，n表示与dμ相关的系数向量。

当ⁱn＞3时，利用最小二乘方法求解七参数向量，迭代过程如下：

步骤a，设置初始参数

步骤b，将代入上式(8a)～(8d)，计算a,l；

步骤c，根据误差方程求解其中，v表示误差；

步骤d，根据最小二乘原理求解改正数其中，a'表示a的转置；

步骤e，更新七参数值

步骤f，若则停止迭代；否则执行步骤g

步骤g，重复步骤b～步骤f；

步骤s2.5：利用整体最小二乘算法进一步提高解算精度

在步骤s2.4迭代过程中，仅考虑特征点在中的误差，而实际应用中，系数矩阵也存在误差。针对这一问题，采用整体最小二乘原理进一步提高七参数的解算精度。新建立的eiv模型如式(10a)所示：

ea＝vec(eai)，vec(·)表示将矩阵按列堆叠转化为列向量；ea表示系数矩阵的残差，表示每次迭代中系数矩阵残差的预测值；

目标函数：

其中，ey表示观测量y的残差，ea表示系数矩阵的残差重构的列向量λ分别表示m×1拉格朗日乘数向量，qy和qa分别表示观测量y权逆阵和系数矩阵残差的权逆阵，表示直乘，im表示m×m单位矩阵，y表示由特征点在坐标系下的坐标构成的列向量；

为求解分别对变量求导，求目标函数最值过程如下，其中“～”表示相应变量的预测值；

对ey求导得：

对ea求导得：

对λ求导得：

对求导得：

由式(12a～12b)可得

将式(13a)～(13b)代入(12c)得

其中，

其中，下标i表示第i次迭代，表示单位权方差，ql表示q0表示系数矩阵方差，qe表示权逆阵，表示第i+1次迭代的观测量误差，表示第i+1次迭代的系数矩阵误差；

步骤s2.5中，迭代计算的初始值由步骤s2.4的结果给出，根据以上迭代计算，当时迭代停止，求解出的x即为进一步迭代后的七参数；

步骤s2.6：计算相对姿态

根据步骤s2.5中得到的欧拉角求解出旋转矩阵，依据下式求解出四元数q＝[ξ1ξ2ξ3ξ4]^t：

步骤3包括如下步骤：

步骤s3.1，建立动力学模型

步骤s3.2，建立运动学模型

ρ＝[ξ1ξ2ξ3]^t(17)

其中：

步骤s3.3，相对转速计算公式

ω＝d(q)ωf-ωl(18)

ωl,ωf,ω分别表示相机坐标系的转速、目标坐标系的转速及目标坐标系相对相机坐标系的转速；表示相对转速的导数；il和if分别表示航天器和非合作目标的转动惯量，nl和nf分别表示航天器和非合作目标受到的外力，d(q)表示由四元数表示的旋转矩阵，q表示四元数，表示四元数的导数，ρ表示四元数的矢量部分；

d(q)＝[(ξ4)²-ρ^tρ]i3×3+2ρρ^t-2ξ4[ρ×](19)

[ρ×]表示叉乘。

步骤3.4，无损卡尔曼滤波估计转速

定义状态方程如下：

其中，表示状态量的导数，η＝[ξ1,ξ2,ξ3,ξ4,ωx,ωy,ωz,it1,it2,it3,it4,it5,it6]^t

f(η)表示状态量与其导数映射函数，w表示均值为零的高斯噪声；

观测方程如下：

z＝h(η)+v(21)

其中，z表示观测量，η表示状态量，v表示均值为零的高斯白噪声，h(η)表示状态量与观测量的映射关系；

其中，ξ1,ξ2,ξ3,ξ4,分别为相对姿态的四元数；ω＝[ωx,ωy,ωz]^t分别表示相对转速；it1,it2,it3,it4,it5,it6分别表示转动惯量的元素；

其中，转动惯量为观测量为：z＝[ξ1,ξ2,ξ3,ξ4]^t

根据观测量和测量量的关系建立模型，采用ukf滤波解算状态，包括如下步骤：

步骤a：利用ut变换计算sigma点

其中，λ＝α²(n+κ)-nα表示sigma点围绕ηk的分布特性，κ表示比例因子；

步骤b：一步预测

步骤c：二次采样

步骤d：测量更新

(zk+1|k)i＝h((χk+1|k)i)(26)

步骤e：滤波更新

β结合η的先验分布信息得出，最优值可设置为2；

χk表示k时刻的sigma点集；

ηk表示第k次迭代的状态量；

表示变换后sigma点集的第i列；

w^m表示均值的加权值；

表示一步预测状态；

pk+1|k表示后验方差阵；

w^c表示方差的权值；

qk表示噪声的方差矩阵；

zk+1|k表示k时刻预测k+1的测量值的估计值；

表示预测观测值；

χk+1|k表示根据一步预测值再次使用ut变换，产生的新sigma点集；

kk+1表示在k+1时刻的滤波增益矩阵；

表示k+1时刻的状态量估计值；

表示量测输出变量的方差矩阵；

是协方差阵；

ζ表示一个标量，由经验得到；

n表示系统状态维数；

ukf滤波器的输入量为由四元数组成的测量值，根据不同时刻的输入通过不断迭代，最终可求出不同时刻，非合作目标相对相机坐标系的相对状态。

非合作相对相机坐标系的相对状态可表示为

η′＝[ξ1，ξ2，ξ3，ξ4，ωx，ωy，ωz，t1，t2，t3]。

本实施例提供的方法，可以应用于非合作目标相对状态的估计，在航天器质心位置或者相对质心平移一定距离安装双目相机，可以测量太空中非合作目标(姿态、速度、位置未知的目标)相对相机坐标系的位置、相对姿态和相对转速。具体实施过程包括：1)安装相机；2)采集双目图像；3)特征坐标解算；4)相对姿态和位置解算；5)相对转速估计。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。