WO2019178887A1 - 基于函数迭代积分的刚体姿态解算方法及系统 - Google Patents

基于函数迭代积分的刚体姿态解算方法及系统 Download PDF

Info

Publication number
WO2019178887A1
WO2019178887A1 PCT/CN2018/081179 CN2018081179W WO2019178887A1 WO 2019178887 A1 WO2019178887 A1 WO 2019178887A1 CN 2018081179 W CN2018081179 W CN 2018081179W WO 2019178887 A1 WO2019178887 A1 WO 2019178887A1
Authority
WO
WIPO (PCT)
Prior art keywords
chebyshev polynomial
vector
angular velocity
order
rodrigue
Prior art date
Application number
PCT/CN2018/081179
Other languages
English (en)
French (fr)
Inventor
武元新
Original Assignee
上海交通大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 上海交通大学 filed Critical 上海交通大学
Priority to US16/963,515 priority Critical patent/US20210048297A1/en
Publication of WO2019178887A1 publication Critical patent/WO2019178887A1/zh
Priority to US17/824,927 priority patent/US20220282974A1/en

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C19/00—Gyroscopes; Turn-sensitive devices using vibrating masses; Turn-sensitive devices without moving masses; Measuring angular rate using gyroscopic effects

Definitions

  • the invention relates to the field of test and measurement technology, in particular to a fast calculation method and system for rigid body attitude calculation based on function iterative integration.
  • the calculation or estimation of rigid body motion in three-dimensional space is the core problem in many fields such as physics, robotics, navigation guidance, machinery, and computer vision. Unlike the translational motions such as speed and position, the attitude cannot be directly measured and can only be obtained by indirect methods such as angular velocity integration or vector matching.
  • the attitude resolution of the angular velocity integration method is completely autonomous and does not require external information assistance, so it is favored in many applications (such as satellite navigation systems cannot function).
  • This method has the advantage of high computational accuracy, but does not make full use of the good properties of the Chebyshev polynomial in the iterative process, and the polynomial order of the Rodrigue vector increases sharply with the iterative process, and the calculation amount is large, which is difficult to meet the real-time application.
  • the Rodrigue vector polynomial has more than one thousand in the seventh iteration! In fact, due to errors in angular velocity measurements, such high-order polynomials are not required for Rodrigue vectors.
  • an object of the present invention is to provide a rigid body attitude solving method and system based on function iterative integration.
  • a rigid body attitude solving method based on function iterative integration includes:
  • Fitting step fitting a Chebyshev polynomial function of angular velocity according to the gyroscopic measurement value in the time interval;
  • Iterative step Iteratively calculates the Chebyshev polynomial coefficients of the Rodrigue vector using the Chebyshev polynomial coefficients of the obtained angular velocity and the Rodrigue vector integral equation, and performs polynomial truncation according to the preset order for the result of each iteration. ;
  • Attitude solving step Calculating the Rodrigue vector according to the Chebyshev polynomial coefficients of the obtained Rodrigue vector and the corresponding Chebyshev polynomial, and giving the attitude change in the time interval in the form of quaternion.
  • the gyro measurement value comprises an angular velocity measurement value or an angular increase measurement value.
  • the fitting step specifically includes:
  • N angular velocity measurements for time t k Angle increment measurement k 1,2,...N; Map the original time interval to [-1 1]; the angular velocity is approximated by a Chebyshev polynomial that does not exceed the N-1 order
  • n is the order of the angular velocity Chebyshev polynomial
  • c i is the coefficient vector of the i-th Chebyshev polynomial
  • F i ( ⁇ ) is the i-th order first Chebyshev polynomial
  • ⁇ is the time after mapping variable.
  • the iterative steps specifically include:
  • n T is the preset truncation order
  • the Chebyshev polynomial coefficients of the Rodrigue vector are calculated as follows:
  • a rigid body attitude solving system based on function iterative integration includes:
  • Fitting module fitting a Chebyshev polynomial function of the angular velocity according to the gyroscopic measurement value in the time interval;
  • Iterative module Iteratively calculates the Chebyshev polynomial coefficients of the Rodrigue vector using the Chebyshev polynomial coefficients of the obtained angular velocity and the Rodrigue vector integral equation, and performs polynomial truncation according to the preset order of the results of each iteration. ;
  • the attitude solving module Calculates the Rodrigue vector according to the Chebyshev polynomial coefficients of the obtained Rodrigue vector and the corresponding Chebyshev polynomial, and gives the attitude change in the time interval in the form of quaternion.
  • the gyro measurement value comprises an angular velocity measurement value or an angular increase measurement value.
  • the fitting module specifically includes:
  • N angular velocity measurements for time t k Angle increment measurement k 1,2,...N; Map the original time interval to [-1 1]; the angular velocity is approximated by a Chebyshev polynomial that does not exceed the N-1 order
  • n is the order of the angular velocity Chebyshev polynomial
  • c i is the coefficient vector of the i-th Chebyshev polynomial
  • F i ( ⁇ ) is the i-th order first Chebyshev polynomial
  • ⁇ is the time after mapping variable.
  • the iterative module specifically includes:
  • n T is the preset truncation order
  • the Chebyshev polynomial coefficients of the Rodrigue vector are calculated as follows:
  • the present invention has the following beneficial effects:
  • the invention is based on the technique of function iterative integration, and uses the Rodrigue vector to realize the fast reconstruction posture from the gyro measurement.
  • the gyroscopic measurement reconstruction uses a Chebyshev polynomial with good numerical properties to transform the iterative integral of the Rodrigue vector into the iterative calculation of the corresponding Chebyshev polynomial coefficients, and uses the method of order truncation to not significantly reduce the calculation accuracy. Improve the calculation speed.
  • Figure 1 is a flow chart of the present invention.
  • a rigid body attitude solving method based on function iterative integration includes:
  • Fitting step fitting the Chebyshev polynomial function of the angular velocity according to the gyroscopic measurement value in the time interval.
  • Gyro measurements include angular velocity measurements or angular gain measurements.
  • Iterative step Iteratively calculates the Chebyshev polynomial coefficients of the Rodrigue vector using the Chebyshev polynomial coefficients of the obtained angular velocity and the Rodrigue vector integral equation, and performs polynomial truncation according to the preset order for the result of each iteration. .
  • Attitude solving step Calculating the Rodrigue vector according to the Chebyshev polynomial coefficients of the obtained Rodrigue vector and the corresponding Chebyshev polynomial, and giving the attitude change in the time interval in the form of quaternion.
  • Chebyshev polynomial is defined on the interval [-1 1] and is given by the following iterative relationship:
  • Step 1) fitting a Chebyshev polynomial function of the angular velocity according to the gyro measurement value in the time interval;
  • the angular velocity is approximated by a Chebyshev polynomial of no more than N-1 order
  • n is the order of the angular velocity Chebyshev polynomial
  • c i is the coefficient vector of the i-th Chebyshev polynomial
  • F i ( ⁇ ) is the i-th order first Chebyshev polynomial
  • ⁇ is the time after mapping variable.
  • T is a vector or matrix transpose
  • Step 2 Iteratively calculates the Chebyshev polynomial coefficients of the Rodrigue vector by using the Chebyshev polynomial coefficients of the angular velocity and the Rodrigue vector integral equation, and perform the polynomial truncation according to the predetermined truncation order.
  • n T is a previously determined truncation order
  • b l i is the coefficient of the i-th order Chebyshev polynomial at l iteration.
  • g l 0.
  • the Chebyshev polynomial coefficients of the Rodrigue vector can be calculated as follows:
  • ⁇ represents a vector cross product. According to the formula (1), since the polynomial approximation accuracy of the angular velocity does not exceed N-1 steps, the truncation order n T ⁇ N can be set.
  • Step 3 Calculate the Rodrigue vector from the Chebyshev polynomial coefficient of the Rodrigue vector and the corresponding Chebyshev polynomial, and give the attitude quaternion with reference to the start time of the time interval.
  • the Rodrigue vector is calculated with reference to equation (5), and the attitude quaternion with reference to the start time of the time interval is obtained.
  • the attitude solving fast method of the present invention is also applicable to other three-dimensional attitude parameters, such as rotation vectors, if a certain degree of precision loss can be accepted.
  • Step 2 The Chebyshev polynomial coefficients of the rotation vector can be calculated as follows:
  • Step 3 Calculate the rotation vector according to the Chebyshev polynomial coefficients of the rotation vector and the corresponding Chebyshev polynomial, and give the attitude quaternion with reference to the start time of the time interval.
  • the present invention also provides a rigid body attitude solving system based on function iterative integration, comprising:
  • Fitting module Fit the Chebyshev polynomial function of the angular velocity according to the gyroscopic measurement value in the time interval.
  • Gyro measurements include angular velocity measurements or angular gain measurements.
  • Iterative module Iteratively calculates the Chebyshev polynomial coefficients of the Rodrigue vector using the Chebyshev polynomial coefficients of the obtained angular velocity and the Rodrigue vector integral equation, and performs polynomial truncation according to the preset order of the results of each iteration. .
  • the attitude solving module Calculates the Rodrigue vector according to the Chebyshev polynomial coefficients of the obtained Rodrigue vector and the corresponding Chebyshev polynomial, and gives the attitude change in the time interval in the form of quaternion.
  • n is the order of the angular velocity Chebyshev polynomial
  • c i is the coefficient vector of the i-th Chebyshev polynomial
  • F i ( ⁇ ) is the i-th order first Chebyshev polynomial
  • ⁇ is the time after mapping variable.
  • T is a vector or matrix transpose
  • the Chebyshev polynomial calculation of the Rodrigue vector in the iterative module includes:
  • n T is the preset truncation order
  • ⁇ represents a vector cross product. According to the formula (1), since the polynomial approximation accuracy of the angular velocity does not exceed N-1 steps, the truncation order n T ⁇ N can be set.
  • the attitude calculation module calculates the Rodrigue vector from the Chebyshev polynomial coefficient of the Rodrigue vector and the corresponding Chebyshev polynomial, and gives the attitude quaternion with reference to the start time of the time interval.
  • the Rodrigue vector is calculated with reference to equation (5), and the attitude quaternion with reference to the start time of the time interval is obtained.
  • the rigid body attitude solving system of the present invention is also applicable to other three-dimensional attitude parameters, such as rotation vectors, if a certain degree of precision loss can be accepted.
  • the corresponding adjustments to equations (6) and (7) need to be made as follows:
  • Chebyshev polynomial coefficients of the rotation vector can be calculated as follows:
  • the attitude solving module calculates the rotation vector according to the Chebyshev polynomial coefficients of the rotation vector and the corresponding Chebyshev polynomial, and gives the attitude quaternion with reference to the start time of the time interval.
  • the system provided by the present invention and its various devices can be fully implemented by logically programming the method steps, except that the system provided by the present invention and its various devices, modules, and units are implemented in a purely computer readable program code.
  • Modules and units implement the same functions in the form of logic gates, switches, ASICs, programmable logic controllers, and embedded microcontrollers. Therefore, the system and its various devices, modules and units provided by the present invention can be regarded as a hardware component, and the devices, modules and units included therein for implementing various functions can also be regarded as hardware components.
  • the device, module, and unit for implementing various functions can also be regarded as a software module that can be both a method and a hardware component.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)
  • Automation & Control Theory (AREA)

Abstract

一种基于函数迭代积分的刚体姿态解算方法及系统，包括：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数；利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。基于函数迭代积分的技术，利用罗德里格向量，实现从陀螺测量快速重建姿态，在不显著降低计算精度的情况下提高计算速度。

Description

基于函数迭代积分的刚体姿态解算方法及系统 技术领域
本发明涉及测试测量技术领域，具体而言，涉及一种基于函数迭代积分的刚体姿态解算快速计算方法及系统。
背景技术
三维空间刚体运动的计算或估计是物理、机器人、导航制导、机械、计算机视觉等众多领域中的核心问题。与速度、位置等平移运动不同，姿态不能被直接测量，只能通过角速度积分或向量匹配等间接方式获得。角速度积分方式的姿态解算是完全自主的，不需要外部信息辅助，因此在很多(如卫星导航系统不能发挥作用的)应用场合备受青睐。
近年来，本领域研究人员提出了若干高精度姿态解算方法。申请人在申请号为CN201710273489.3的发明专利中提出一种基于函数迭代积分的刚体姿态解算方法，即：根据时间区间上的陀螺测量值，拟合出角速度的多项式函数；利用角速度的多项式拟合函数以及罗德里格向量(Rodrigues)积分方程，迭代计算罗德里格向量，进而根据迭代结果，以四元数的形式给出时间区间上的姿态变化。该方法具有计算精度高的优势，但在迭代过程中没有充分利用切比雪夫多项式的良好性质，且罗德里格向量的多项式阶数随着迭代过程急剧增长，计算量大，难以满足实时应用。例如，对于利用八个陀螺测量值进行角速度多项式拟合的情况，在第七次迭代时，罗德里格向量多项式的阶数超过一千！实际上，由于角速度测量存在误差，对罗德里格向量不需要用到如此高阶多项式。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于函数迭代积分的刚体姿态解算方法及系统。
根据本发明提供的一种基于函数迭代积分的刚体姿态解算方法，包括：
拟合步骤：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数；
迭代步骤：利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；
姿态解算步骤：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。
较佳的，所述陀螺测量值包括角速度测量值或者角增量测量值。
较佳的，拟合步骤具体包括：
对于t k时刻的N个角速度测量值
Figure PCTCN2018081179-appb-000001
或角增量测量值
Figure PCTCN2018081179-appb-000002
k＝1,2,…N；令
Figure PCTCN2018081179-appb-000003
将原时间区间映射到[-1 1]上；角速度采用不超过N-1阶的切比雪夫多项式进行拟合近似
Figure PCTCN2018081179-appb-000004
其中n为角速度切比雪夫多项式的阶数，c i为第i阶切比雪夫多项式的系数向量，F i(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。
较佳的，迭代步骤具体包括：
在l次迭代时，罗德里格向量的切比雪夫多项式记做：
Figure PCTCN2018081179-appb-000005
其中n T为预设的截断阶数，b l,i为l次迭代时第i阶切比雪夫多项式的系数，当l＝0时，g l＝0。
较佳的，罗德里格向量的切比雪夫多项式系数按如下迭代计算：
Figure PCTCN2018081179-appb-000006
迭代计算直到满足收敛条件或达到事先设定的最大迭代次数，角速度的多项式近似精度不超过N-1阶，设置截断阶数n T≥N。
根据本发明提供的一种基于函数迭代积分的刚体姿态解算系统，包括：
拟合模块：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数；
迭代模块：利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；
姿态解算模块：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比 雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。
较佳的，所述陀螺测量值包括角速度测量值或者角增量测量值。
较佳的，拟合模块具体包括：
对于t k时刻的N个角速度测量值
Figure PCTCN2018081179-appb-000007
或角增量测量值
Figure PCTCN2018081179-appb-000008
k＝1,2,…N；令
Figure PCTCN2018081179-appb-000009
将原时间区间映射到[-1 1]上；角速度采用不超过N-1阶的切比雪夫多项式进行拟合近似
Figure PCTCN2018081179-appb-000010
其中n为角速度切比雪夫多项式的阶数，c i为第i阶切比雪夫多项式的系数向量，F i(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。
较佳的，迭代模块具体包括：
在l次迭代时，罗德里格向量的切比雪夫多项式记做：
Figure PCTCN2018081179-appb-000011
其中n T为预设的截断阶数，b l,i为l次迭代时第i阶切比雪夫多项式的系数，当l＝0时，g l＝0。
较佳的，罗德里格向量的切比雪夫多项式系数按如下迭代计算：
Figure PCTCN2018081179-appb-000012
迭代计算直到满足收敛条件或达到事先设定的最大迭代次数，角速度的多项式近似精度不超过N-1阶，设置截断阶数n T≥N。
与现有技术相比，本发明具有如下的有益效果：
本发明基于函数迭代积分的技术，利用罗德里格向量，实现从陀螺测量快速重建姿态。陀螺测量重建采用具有良好数值特性的切比雪夫多项式，将罗德里格向量的迭代积分变换为对应的切比雪夫多项式系数的迭代计算，并运用阶数截断的方法在不显著降低计算精度的情况下提高计算速度。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明的流程图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
如图1所示，本发明提供的一种基于函数迭代积分的刚体姿态解算方法，包括：
拟合步骤：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数。陀螺测量值包括角速度测量值或者角增量测量值。
迭代步骤：利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断。
姿态解算步骤：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。
第一类切比雪夫多项式在区间[-1 1]上定义，并由以下迭代关系给出：
F 0(x)＝1,F 1(x)＝x,F i+1(x)＝2xF i(x)-F i-1(x)
其中F i(x)为第i阶第一类切比雪夫多项式。
步骤1)根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数；
对于t k时刻的N个角速度测量值
Figure PCTCN2018081179-appb-000013
或角增量测量值
Figure PCTCN2018081179-appb-000014
k＝1,2,…N。令
Figure PCTCN2018081179-appb-000015
将原时间区间映射到[-1 1]上。角速度采用不超过N-1阶的切比雪夫多项式进行拟合近似
Figure PCTCN2018081179-appb-000016
其中n为角速度切比雪夫多项式的阶数，c i为第i阶切比雪夫多项式的系数向量，F i(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。
在角速度测量的情况下，系数c i通过求解如下方程来确定：
Figure PCTCN2018081179-appb-000017
而在角增量测量的情况下，系数c i通过求解如下方程来确定：
Figure PCTCN2018081179-appb-000018
其中，T表示向量或矩阵转置，函数
Figure PCTCN2018081179-appb-000019
按如下定义：
Figure PCTCN2018081179-appb-000020
步骤2)利用角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照事先确定的截断阶数进行多项式截断；
假定在l次迭代时，罗德里格向量的切比雪夫多项式记做
Figure PCTCN2018081179-appb-000021
其中n T为事先确定的截断阶数，b l,i为l次迭代时第i阶切比雪夫多项式的系数。当l＝0时，g l＝0。罗德里格向量的切比雪夫多项式系数可按如下迭代计算：
Figure PCTCN2018081179-appb-000022
直到满足收敛条件或达到事先设定的最大迭代次数。上式中，×表示向量叉乘。根据(1)式，因角速度的多项式近似精度不超过N-1阶，可设置截断阶数n T≥N。
步骤3)根据罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，给出以时间区间开始时刻为参考的姿态四元数。
根据罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算，参考(5)式计算罗德里格向量，得到以时间区间开始时刻为参考的姿态四元数。
Figure PCTCN2018081179-appb-000023
对于长时间区间上的姿态解算，可将其划分为若干个小时间区间，依次计算实现。
原则上，如果能接受一定程度的精度损失，本发明的姿态解算快速方法也适用于其他三维姿态参数，如旋转向量。此时，需要对步骤2)中的(6)式和步骤3)中的(7)式 做相应的调整如下：
步骤2)旋转向量的切比雪夫多项式系数可按如下迭代计算：
Figure PCTCN2018081179-appb-000024
步骤3)根据旋转向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到旋转向量，给出以时间区间开始时刻为参考的姿态四元数。
Figure PCTCN2018081179-appb-000025
在上述一种基于函数迭代积分的刚体姿态解算方法的基础上，本发明还提供一种基于函数迭代积分的刚体姿态解算系统，包括：
拟合模块：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数。陀螺测量值包括角速度测量值或者角增量测量值。
迭代模块：利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断。
姿态解算模块：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。
具体的，拟合模块对于t k时刻的N个角速度测量值
Figure PCTCN2018081179-appb-000026
或角增量测量值
Figure PCTCN2018081179-appb-000027
k＝1,2,…N；令
Figure PCTCN2018081179-appb-000028
将原时间区间映射到[-1 1]上；角速度采用不超过N-1阶的切比雪夫多项式进行拟合近似
Figure PCTCN2018081179-appb-000029
其中n为角速度切比雪夫多项式的阶数，c i为第i阶切比雪夫多项式的系数向量，F i(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。
在角速度测量的情况下，系数c i通过求解如下方程来确定：
Figure PCTCN2018081179-appb-000030
而在角增量测量的情况下，系数c i通过求解如下方程来确定：
Figure PCTCN2018081179-appb-000031
其中，T表示向量或矩阵转置，函数
Figure PCTCN2018081179-appb-000032
按如下定义：
Figure PCTCN2018081179-appb-000033
具体的，迭代模块中罗德里格向量的切比雪夫多项式计算包括：
在l次迭代时，罗德里格向量的切比雪夫多项式记做：
Figure PCTCN2018081179-appb-000034
其中n T为预设的截断阶数，b l,i为l次迭代时第i阶切比雪夫多项式的系数，当l＝0时，g l＝0。
罗德里格向量的切比雪夫多项式系数按如下迭代计算：
Figure PCTCN2018081179-appb-000035
直到满足收敛条件或达到事先设定的最大迭代次数。上式中，×表示向量叉乘。根据(1)式，因角速度的多项式近似精度不超过N-1阶，可设置截断阶数n T≥N。
姿态解算模块根据罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，给出以时间区间开始时刻为参考的姿态四元数。
根据罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算，参考(5)式计算罗德里格向量，得到以时间区间开始时刻为参考的姿态四元数。
Figure PCTCN2018081179-appb-000036
对于长时间区间上的姿态解算，可将其划分为若干个小时间区间，依次计算实现。
原则上，如果能接受一定程度的精度损失，本发明的刚体姿态解算系统也适用于其他三维姿态参数，如旋转向量。此时，需要对(6)式和(7)式做相应的调整如下：
迭代模块中，旋转向量的切比雪夫多项式系数可按如下迭代计算：
Figure PCTCN2018081179-appb-000037
姿态解算模块根据旋转向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到旋转向量，给出以时间区间开始时刻为参考的姿态四元数。
Figure PCTCN2018081179-appb-000038
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

  1. 一种基于函数迭代积分的刚体姿态解算方法，其特征在于，包括：
    拟合步骤：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数；
    迭代步骤：利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；
    姿态解算步骤：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。
  2. 根据权利要求1所述的基于函数迭代积分的刚体姿态解算方法，其特征在于，所述陀螺测量值包括角速度测量值或者角增量测量值。
  3. 根据权利要求1所述的基于函数迭代积分的刚体姿态解算方法，其特征在于，拟合步骤具体包括：
    对于tk时刻的N个角速度测量值
    Figure PCTCN2018081179-appb-100001
    或角增量测量值
    Figure PCTCN2018081179-appb-100002
    k＝1,2,...N；令
    Figure PCTCN2018081179-appb-100003
    将原时间区间映射到[-1 1]上；角速度采用不超过N-1阶的切比雪夫多项式进行拟合近似
    Figure PCTCN2018081179-appb-100004
    其中n为角速度的切比雪夫多项式的阶数，c i为第i阶切比雪夫多项式的系数向量，F i(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。
  4. 根据权利要求3所述的基于函数迭代积分的刚体姿态解算方法，其特征在于，所述迭代步骤具体包括：
    在l次迭代时，罗德里格向量的切比雪夫多项式记做：
    Figure PCTCN2018081179-appb-100005
    其中n T为预设的截断阶数，b l,i为l次迭代时第i阶切比雪夫多项式的系数，当l＝0时，g l＝0。
  5. 根据权利要求4所述的基于函数迭代积分的刚体姿态解算方法，其特征在于，罗德里格向量的切比雪夫多项式系数按如下迭代计算：
    Figure PCTCN2018081179-appb-100006
    Figure PCTCN2018081179-appb-100007
    迭代计算直到满足收敛条件或达到事先设定的最大迭代次数，角速度的多项式近似精度不超过N-1阶，设置截断阶数n T≥N。
  6. 一种基于函数迭代积分的刚体姿态解算系统，其特征在于，包括：
    拟合模块：根据时间区间上的陀螺测量值，拟合出角速度的切比雪夫多项式函数；
    迭代模块：利用得到的角速度的切比雪夫多项式系数以及罗德里格向量积分方程，迭代计算罗德里格向量的切比雪夫多项式系数，并对每次迭代的结果按照预设的阶数进行多项式截断；
    姿态解算模块：根据得到的罗德里格向量的切比雪夫多项式系数以及对应的切比雪夫多项式计算得到罗德里格向量，以四元数的形式给出时间区间上的姿态变化。
  7. 根据权利要求6所述的基于函数迭代积分的刚体姿态解算系统，其特征在于，所述陀螺测量值包括角速度测量值或者角增量测量值。
  8. 根据权利要求6所述的基于函数迭代积分的刚体姿态解算系统，其特征在于，拟合模块具体包括：
    对于t k时刻的N个角速度测量值
    Figure PCTCN2018081179-appb-100008
    或角增量测量值
    Figure PCTCN2018081179-appb-100009
    k＝1,2,...N；令
    Figure PCTCN2018081179-appb-100010
    将原时间区间映射到[-1 1]上；角速度采用不超过N-1阶的切比雪夫多项式进行拟合近似
    Figure PCTCN2018081179-appb-100011
    其中n为角速度切比雪夫多项式的阶数，c i为第i阶切比雪夫多项式的系数向量，F i(τ)为第i阶第一类切比雪夫多项式，τ为映射后的时间自变量。
  9. 根据权利要求8所述的基于函数迭代积分的刚体姿态解算系统，其特征在于，迭代模块具体包括：
    在l次迭代时，罗德里格向量的切比雪夫多项式记做：
    Figure PCTCN2018081179-appb-100012
    其中n T为预设的截断阶数，b l,i为l次迭代时第i阶切比雪夫多项式的系数，当l＝0时，g l＝0。
  10. 根据权利要求9所述的基于函数迭代积分的刚体姿态解算系统，其特征在于，罗德里格向量的切比雪夫多项式系数按如下迭代计算：
    Figure PCTCN2018081179-appb-100013
    Figure PCTCN2018081179-appb-100014
    迭代计算直到满足收敛条件或达到事先设定的最大迭代次数，角速度的多项式近似精度不超过N-1阶，设置截断阶数n T≥N。
PCT/CN2018/081179 2018-03-21 2018-03-29 基于函数迭代积分的刚体姿态解算方法及系统 WO2019178887A1 (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
US16/963,515 US20210048297A1 (en) 2018-03-21 2018-03-29 Method and system for solving rigid body attitude based on functional iterative integration
US17/824,927 US20220282974A1 (en) 2018-03-21 2022-05-26 Method and system for solving rigid body attitude based on functional iterative integration

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201810236436.9 2018-03-21
CN201810236436.9A CN108534774B (zh) 2018-03-21 2018-03-21 基于函数迭代积分的刚体姿态解算方法及系统

Related Child Applications (2)

Application Number Title Priority Date Filing Date
US16/963,515 A-371-Of-International US20210048297A1 (en) 2018-03-21 2018-03-29 Method and system for solving rigid body attitude based on functional iterative integration
US17/824,927 Continuation-In-Part US20220282974A1 (en) 2018-03-21 2022-05-26 Method and system for solving rigid body attitude based on functional iterative integration

Publications (1)

Publication Number Publication Date
WO2019178887A1 true WO2019178887A1 (zh) 2019-09-26

Family

ID=63485052

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2018/081179 WO2019178887A1 (zh) 2018-03-21 2018-03-29 基于函数迭代积分的刚体姿态解算方法及系统

Country Status (3)

Country Link
US (1) US20210048297A1 (zh)
CN (1) CN108534774B (zh)
WO (1) WO2019178887A1 (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109724597B (zh) * 2018-12-19 2021-04-02 上海交通大学 一种基于函数迭代积分的惯性导航解算方法及系统
CN112102377B (zh) * 2020-08-04 2023-02-03 广东工业大学 基于切比雪夫的icp点云全局最优配准方法及装置
CN114396936B (zh) * 2022-01-12 2024-03-12 上海交通大学 基于多项式优化的惯性与磁传感器姿态估计方法及系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102494690A (zh) * 2011-11-30 2012-06-13 西北工业大学 基于角速度的欧拉角任意步长正交级数近似输出方法
CN102506866A (zh) * 2011-11-17 2012-06-20 西北工业大学 基于角速度的飞行器极限飞行时四元数切比雪夫近似输出方法
CN102519466A (zh) * 2011-11-25 2012-06-27 西北工业大学 基于角速度的欧拉角勒让德指数近似输出方法
CN105808508A (zh) * 2016-03-15 2016-07-27 北京航空航天大学 一种求解不确定热传导问题的随机正交展开方法
CN106767780A (zh) * 2016-11-28 2017-05-31 郑州轻工业学院 基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法
CN107339987A (zh) * 2017-04-21 2017-11-10 上海交通大学 一种基于函数迭代积分的刚体姿态解算方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102506866A (zh) * 2011-11-17 2012-06-20 西北工业大学 基于角速度的飞行器极限飞行时四元数切比雪夫近似输出方法
CN102519466A (zh) * 2011-11-25 2012-06-27 西北工业大学 基于角速度的欧拉角勒让德指数近似输出方法
CN102494690A (zh) * 2011-11-30 2012-06-13 西北工业大学 基于角速度的欧拉角任意步长正交级数近似输出方法
CN105808508A (zh) * 2016-03-15 2016-07-27 北京航空航天大学 一种求解不确定热传导问题的随机正交展开方法
CN106767780A (zh) * 2016-11-28 2017-05-31 郑州轻工业学院 基于Chebyshev多项式插值逼近的扩展椭球集员滤波方法
CN107339987A (zh) * 2017-04-21 2017-11-10 上海交通大学 一种基于函数迭代积分的刚体姿态解算方法

Also Published As

Publication number Publication date
US20210048297A1 (en) 2021-02-18
CN108534774A (zh) 2018-09-14
CN108534774B (zh) 2020-02-21

Similar Documents

Publication Publication Date Title
CN107984472B (zh) 一种用于冗余度机械臂运动规划的变参神经求解器设计方法
WO2018192004A1 (zh) 一种基于函数迭代积分的刚体姿态解算方法
CN109724597B (zh) 一种基于函数迭代积分的惯性导航解算方法及系统
CN108592945B (zh) 一种惯性/天文组合系统误差的在线标定方法
WO2019178887A1 (zh) 基于函数迭代积分的刚体姿态解算方法及系统
CA2735262C (en) Inverse kinematics
CN103234556B (zh) 基于星光矢量校正的在轨标定星敏感器透镜畸变的方法及卫星姿态确定方法
WO2018076211A1 (zh) 基于几何误差优化的图像中二次曲线拟合方法
CN103438907B (zh) 一种星敏感器六自由度像平面误差的在轨标定方法
Yuzhen et al. The application of adaptive extended Kalman filter in mobile robot localization
Mahony et al. A port-Hamiltonian approach to image-based visual servo control for dynamic systems
CN107146236B (zh) 视频卫星的点状运动目标状态估计方法及系统
Kulakov Active force-torque robot control without using wrist force-torque sensors
McCann et al. Rigid body pose estimation on TSE (3) for spacecraft with unknown moments of inertia
CN115655285B (zh) 一种权值及参考四元数修正的无迹四元数姿态估计方法
CN103954289B (zh) 一种光学成像卫星敏捷机动姿态确定方法
CN113985887A (zh) 差速移动机器人运动轨迹生成方法和运动控制装置
CN114764830A (zh) 一种基于四元数ekf和未标定手眼系统的物体位姿估算方法
CN114502338A (zh) 用于生成机器人的控制器的技术
CN114396936B (zh) 基于多项式优化的惯性与磁传感器姿态估计方法及系统
CN104463884A (zh) 最小化空间距离的线性交会测量方法
CN114396942B (zh) 基于多项式优化的高精度快速惯导解算方法和系统
CN115839726B (zh) 磁传感器和角速度传感器联合标定的方法、系统及介质
Castellanos et al. Design and simulation of an attitude determination system based on the Extended Kalman Filter for Cube-Sat Colombia I
Zhou et al. A robot state estimator based on multi-sensor information fusion

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 18910704

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

32PN Ep: public notification in the ep bulletin as address of the adressee cannot be established

Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205A DATED 21/01/2021)

122 Ep: pct application non-entry in european phase

Ref document number: 18910704

Country of ref document: EP

Kind code of ref document: A1