一种基于代码生成的多矢量行进间对准方法及装置与流程

1.本发明涉及惯性导航技术领域，特别涉及一种基于代码生成的多矢量行进间对准方法及装置。


背景技术：

2.目前，惯性导航系统能够提供完全自主的导航信息，具有反应时间短、可靠性高、体积小、重量轻等优点，广泛应用于飞机、舰船等军用和民用导航领域,初始对准一直以来都是惯导领域研究的热点。
3.当前国内外大多数惯导均采用静态或者准静态初始对准方式，即在运动前，利用惯性测量、旋转调制、多位置转动等完成初始对准，这种方式需要系统静止等待一段时间后才能出发。为提高系统的机动能力和实时反应能力，可采用行进间对准方式，无需静止等待，可有效提高反应速度，因此行进间对准成为提升系统反应能力的关键技术。
4.但是相关技术在进行工程应用时仍然存在计算量大，解算精度不够等问题，尤其对多矢量其中涉及最大特征值和奇异值分解的问题，均在具体解算的实现方面缺少有效的解决方案。


技术实现要素：

5.本发明实施例提供一种基于代码生成的多矢量行进间对准方法及装置，实现多矢量定姿的精确计算，实用性强、易于工程实现。
6.一方面，本发明实施例提供一种基于代码生成的多矢量行进间对准方法，其特征在于，所述方法包括步骤：
7.根据卫星导航提供的位置和速度信息计算初始时刻地球惯性坐标系e
′
i0
相对于导航系n的转换矩阵所述导航系n的x轴、y轴和z轴分别指向东、北、天；
8.利用陀螺输出的角运动信息计算惯性测量单元本体坐标系b系相对初始时刻捷联惯导惯性坐标系b
i0
的转换矩阵
9.基于所述和计算不同时刻所述e
′
i0
下的比速度和所述b
i0
下的比速度的乘积累加和矩阵m(t)；
10.利用代码生成方式对m(t)矩阵进行奇异值分解获取输出结果并基于所述获得行进间对准结果
11.一些实施例中，所述根据卫星导航提供的位置和速度信息计算初始时刻地球惯性坐标系e
′
i0
相对于导航系n的转换矩阵包括步骤：
12.基于第一公式计算所述所述第一公式包括：
[0013][0014]
δλ
t
＝λ
t-λ0，
[0015]
其中，λ
t
和l
t
分别为初始对准t时刻的卫星导航设备实时经度和纬度，λ0为对准起始时刻t0的纬度，δλ
t
表示当前时刻t相对t0的变化，ω
ie
为地球自转角速度。
[0016]
一些实施例中，所述利用陀螺输出的角运动信息计算捷联惯导b系相对初始时刻捷联惯导惯性坐标系b
i0
的转换矩阵包括步骤：
[0017]
基于第二公式计算所述所述第二公式包括：
[0018][0019][0020]
其中，为当前时刻惯性测量单元imu的陀螺输出b系下的角速度，i为单位矩阵，t0为对准起始时刻。
[0021]
一些实施例中，所述基于所述和计算不同时刻所述e
′
i0
下的比速度和所述b
i0
下的比速度的乘积累加和矩阵m(t)，包括步骤：
[0022]
基于第三公式计算所述比速度所述第三公式包括：
[0023][0024][0025][0026][0027]gn
＝[0 0
ꢀ‑
g]
t
，
[0028]
其中，为的转置，表示加速度，h为高度，r为地球半径，和为所述导航系n下当前时刻的速度vn(t)在x、y和z向的分量，g为当地重力加速度，τ为积分表达式的变量。
[0029]
一些实施例中，所述基于所述和计算不同时刻所述e
′
i0
下的比速度和所述b
i0
下的比速度的乘积累加和矩阵m(t)，包括步骤：
[0030]
基于第四公式计算所述比速度所述第四公式包括：
[0031][0032]
其中，fb为当前时刻惯性测量单元imu的加速度计输出b系下的比力，
[0033]
一些实施例中，所述基于所述和计算不同时刻所述e
′
i0
下的比速度和所述b
i0
下的比速度的乘积累加和矩阵m(t)，包括步骤：
[0034]
基于第五公式获取所述和的乘积累加和矩阵m(t)，所述第五公式包括：
[0035][0036]
其中，m(t-1)表示前一个周期的计算值,表示的转置，初始m(0)＝03×3。
[0037]
一些实施例中，所述利用代码生成方式对m(t)矩阵进行奇异值分解获取包括步骤：
[0038]
构建用于代码生成的奇异值分解函数svd_cg()；
[0039]
将所述函数作为入口函数输入m(t)矩阵后获得对应的生成代码文件；
[0040]
根据所述代码文件获取函数的输出结果
[0041]
一些实施例中，所述构建用于代码生成的奇异值分解函数svd_cg()，包括步骤：
[0042]
在matlab中构建第一代码表示并基于所述第一代码新建所述svd_cg()，所述第一代码包括：
[0043]
function[u,s,v]＝svd_cg(m)
[0044]
[u,s,v]＝svd(m)；
[0045]
其中，m为所述m(t)矩阵，s为3x3矩阵且其对角线元素为奇异值，u为3x3矩阵且各列是奇异值的左奇异向量，v为3x3矩阵且各列是奇异值的右奇异向量。
[0046]
一些实施例中，所述根据所述代码文件获取函数的输出结果包括步骤：
[0047]
基于第六公式计算所述所述第六公式包括：
[0048][0049]
其中，det(x)表示矩阵x的行列式。
[0050]
一些实施例中，所述基于所述获得行进间对准结果包括步骤：
[0051]
基于第八公式获取所述所述第八公式包括：
[0052][0053][0054]
第二方面，本发明实施例还提供一种基于代码生成的多矢量行进间对准装置，其特征在于，其包括：
[0055]
第一转换矩阵获取模块，其用于根据卫星导航提供的位置和速度信息计算初始时刻地球惯性坐标系e
′
i0
相对于导航系n的转换矩阵所述导航系n的x轴、y轴和z轴分别指向东、北、天；
[0056]
第二转换矩阵获取模块，其用于利用陀螺输出的角运动信息计算惯性测量单元本体坐标系b系相对初始时刻捷联惯导惯性坐标系b
i0
的转换矩阵
[0057]
比速度乘积累加和矩阵获取模块，其用于基于所述和计算不同时刻所述e
′
i0
下的比速度和所述b
i0
下的比速度的乘积累加和矩阵m(t)；
[0058]
奇异值分解解算模块，其用于利用代码生成方式对m(t)矩阵进行奇异值分解获取输出结果并基于所述获得行进间对准结果
[0059]
本发明实施例提供了一种基于代码生成的多矢量行进间对准方法及装置，利用卫星导航提供的位置和速度信息，采用惯性系多矢量定姿方法实时输出对准结果，其特征主要为通过代码生成方式实现多矢量定姿中的奇异值分解解算，大大提升了对准精度，缩短了工程实现周期，此外，相比于逆向及数据储存复用方法，本方法具有能够实时输出的特点，即从第2个间隔周期便可以输出有效数据，更有利于提高武器系统的机动性。
附图说明
[0060]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0061]
图1为本发明实施例提供的一种基于代码生成的多矢量行进间对准方法流程示意图；
[0062]
图2为本发明实施例提供的一种基于代码生成的多矢量行进间对准方法流程示意图；
[0063]
图3为本发明实施例提供的一种基于代码生成的多矢量行进间对准装置结构示意图。
具体实施方式
[0064]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0065]
如图1所示，本发明实施例提供了一种基于代码生成的多矢量行进间对准方法，包括步骤：
[0066]
s100:根据卫星导航提供的位置和速度信息计算初始时刻地球惯性坐标系e
′
i0
相对于导航系n的转换矩阵所述导航系n的x轴、y轴和z轴分别指向东、北、天；
[0067]
s200:利用陀螺输出的角运动信息计算惯性测量单元本体坐标系b系相对初始时
刻捷联惯导惯性坐标系b
i0
的转换矩阵
[0068]
s300:基于所述和计算不同时刻所述e
′
i0
下的比速度和所述b
i0
下的比速度的乘积累加和矩阵m(t)；
[0069]
s400:利用代码生成方式对m(t)矩阵进行奇异值分解获取输出结果并基于所述获得行进间对准结果
[0070]
可以理解的是，本实施例利用卫星导航提供的位置和速度信息；再采用惯性系多矢量定姿方法实时输出对准结果；并通过代码生成方式实现多矢量定姿中的奇异值分解解算。通过代码生成方式实现多矢量定姿中的奇异值分解解算，大大提升了对准精度，缩短了工程实现周期，此外，相比于逆向及数据储存复用方法，本方法具有能够实时输出的特点，即从第2个间隔周期便可以输出有效数据，更有利于提高武器系统的机动性。
[0071]
一些实施例中，s100中基于第一公式计算所述且第一公式包括：
[0072][0073]
δλ
t
＝λ
t-λ0，
[0074]
其中，λ
t
和l
t
分别为初始对准t时刻的卫星导航设备实时经度和纬度，λ0为对准起始时刻t0的纬度，δλ
t
表示当前时刻t相对t0的变化，ω
ie
为地球自转角速度。
[0075]
一些实施例中，s200中基于第二公式计算所述所述第二公式包括：
[0076][0077][0078]
其中，为当前时刻惯性测量单元imu的陀螺输出b系下的角速度，i为单位矩阵，t0为对准起始时刻。
[0079]
需要说明的是，第二公式中表示对的更新，等号左边为更新后的等号右边为更新前的等号右边为更新前的表示的叉乘。
[0080]
一些实施例中，s300中基于第三公式计算所述比速度所述第三公式包括：
[0081][0082][0083][0084][0085]gn
＝[0 0
ꢀ‑
g]
t
，
[0086]
其中，为的转置，表示加速度，h为高度，r为地球半径，和为所述导航系n下当前时刻的速度vn(t)在x、y和z向的分量，g为当地重力加速度，τ为积分表达式的变量。
[0087]
一些实施例中，s300基于第四公式计算所述比速度所述第四公式包括：
[0088][0089]
其中，fb为当前时刻惯性测量单元imu的加速度计输出b系下的比力，
[0090]
一些实施例中，s300基于第五公式获取所述和的乘积累加和矩阵m(t)，所述第五公式包括：
[0091][0092]
其中，m(t-1)表示前一个周期的计算值,表示的转置，初始m(0)＝03×3。
[0093]
一些实施例中，s400利用代码生成方式对m(t)矩阵进行奇异值分解获取包括步骤：
[0094]
s401:构建用于代码生成的奇异值分解函数svd_cg()；
[0095]
s402:将所述函数作为入口函数输入m(t)矩阵后获得对应的生成代码文件；
[0096]
s403:根据所述代码文件获取函数的输出结果
[0097]
可优选地，s401中构建用于代码生成的奇异值分解函数svd_cg()时在matlab中构建第一代码表示并基于所述第一代码新建所述svd_cg()，所述第一代码包括：
[0098]
function[u,s,v]＝svd_cg(m)
[0099]
[u,s,v]＝svd(m)；
[0100]
其中，m为所述m(t)矩阵，s为3x3矩阵且其对角线元素为奇异值，u为3x3矩阵且各列是奇异值的左奇异向量，v为3x3矩阵且各列是奇异值的右奇异向量。
[0101]
一些实施例中，s403基于第六公式计算所述所述第六公式包括：
[0102][0103]
其中，det(x)表示矩阵x的行列式。
[0104]
一些实施例中，在s403计算出所述后还基于第七公式对所述进行规范正交化，所述第七公式包括：
[0105][0106]
[0107]
其中，表示中间变量，i表示单位矩阵。
[0108]
一些实施例中，s400基于第八公式获取所述所述第八公式包括：
[0109][0110][0111]
在一个具体的实施例中，惯性测量单元imu的陀螺输出本体坐标系b系相对于地心惯性坐标系i系的角速度在b系下的投影为imu的加速度计输出本体坐标系b系下的比力fb，地心惯性坐标系i系中原点位于地球的中心，坐标轴方向在惯性空间中不变；导航系n系中xyz轴分别指向东、北、天；导航系l系中x、y、z轴分别指向北、东、地。
[0112]
如图2所示，基于代码生成的多矢量行进间对准方法，包括以下步骤：
[0113]
s1:利用卫星导航设备当前的位置信息和初始时刻位置信息获得初始时刻地球惯性坐标系e
′
i0
相对于导航系n的转换矩阵具体地，通过采集当前时刻卫星导航设备的经度和纬度数据，计算获得初始时刻地球惯性坐标系e
′
i0
相对于导航系n的转换矩阵时根据公式：
[0114][0115]
δλ
t
＝λ
t-λ0，
[0116]
其中，λ
t
和l
t
分别为初始对准t时刻的卫星导航设备实时经度和纬度，λ0为对准起始t0时刻(通常指定为0时刻)的纬度，δλ
t
表示当前t时刻相对t0时刻的变化，ω
ie
为地球自转角速度。
[0117]
s2：利用陀螺输出的角运动信息，获取捷联惯导b系相对初始时刻捷联惯导惯性坐标系b
i0
的转换矩阵具体地：
[0118]
通过采集当前时刻惯性测量单元imu的陀螺输出利用捷联惯导姿态更新式计算当前时刻的b系相对初始时刻捷联惯导惯性坐标系b
i0
的旋转矩阵其中，i为单位矩阵。
[0119]
s3：获得多个不同时刻e
′
i0
下比速度和b
i0
下比速度的乘积累加和m(t)矩阵。
[0120]
具体地，步骤s3包括s31至s33，
[0121]
s31：计算不同时刻e
′
i0
下比速度根据公式：
[0122]
[0123]
其中，为的转置，的转置，表示加速度，可以用当前周期速度和前一周期速度获取，即vn(t)表示当前时刻速度，vn(t-δt)表示前一时刻速度，δt表示采样间隔，h为高度，r为地球半径，和为n系下速度vn(t)在x、y和z向的分量，vn(t)表示当前时刻的速度，gn＝[0 0
ꢀ‑
g]
t
，g为当地重力加速度，τ为积分表达式的变量。
[0124]
s32：获得b
i0
下比速度具体地，
[0125]
采集当前时刻惯性测量单元imu的加速度计输出fb，根据公式计算得到初始时刻捷联惯导惯性坐标系b
i0
下的比速度τ为积分表达式的变量，在s2步骤中获得。
[0126]
s33：获得和的乘积累加和m(t)矩阵。具体地，
[0127]
每间隔一定采用周期(如每1s，若惯性产品输出周期为5ms，则间隔200个采样)计算和的乘积累加和m(t)矩阵，根据公式：
[0128][0129]
其中，m(t-1)表示上一次计算值。表示的转置，初始m(0)＝03×3。
[0130]
s4：利用matlab代码生成svd，对m(t)矩阵进行奇异值分解获取
[0131]
s4具体可包括步骤s41至s45,
[0132]
s41：在matlab中新建用于代码生成的奇异值分解函数svd_cg()，示例代码如下：
[0133]
function[u,s,v]＝svd_cg(m)
[0134]
[u,s,v]＝svd(m)；
[0135]
end
[0136]
其中，m为步骤s33获得的3x3矩阵m(t)，s为3x3矩阵，其对角线元素为奇异值，u为3x3矩阵，各列是奇异值的左奇异向量，v为3x3矩阵，各列是奇异值的右奇异向量。
[0137]
s42：在matlab中新建代码生成工程文件svd_cg.prj。具体地，利用matlab coder工具新建代码生成工程文件svd_cg.prj，并将svd_cg()作为入口函数，并设定输入m为3x3的矩阵，将生成语言设定为c语言，然后开始代码生成，输出对应的源文件*.c和头文件*.h文件。
[0138]
s43：将生成的文件添加到惯导设备dsp软件的ccs(code composer studio)工程中，进行硬件在环测试，验证代码和软件的正确性。
[0139]
s44：利用代码生成函数输出的结果获取具体地，可利用公式：
[0140]
计算
[0141]
其中，det(x)表示矩阵x的行列式。
[0142]
s45：对获的进行规范正交化。具体地，可对步骤s44获得的按公式：
[0143]
进行规范正交化。
[0144]
s5：利用矩阵链式乘法获得行进间对准结果具体地，根据公式：
[0145]
获得当前时刻的对准结果，
[0146]
其中，其中，按步骤s1每个采样周期更新一次，按步骤s3和步骤s4每间隔一定采用周期更新一次，按步骤s2每个采样周期更新一次。
[0147]
如图3所述，本发明实施例还提供一种基于代码生成的多矢量行进间对准装置，其特征在于，其包括：
[0148]
第一转换矩阵获取模块，其用于根据卫星导航提供的位置和速度信息计算初始时刻地球惯性坐标系e
′
i0
相对于导航系n的转换矩阵所述导航系n的x轴、y轴和z轴分别指向东、北、天；
[0149]
第二转换矩阵获取模块，其用于利用陀螺输出的角运动信息计算惯性测量单元本体坐标系b系相对初始时刻捷联惯导惯性坐标系b
i0
的转换矩阵
[0150]
比速度乘积累加和矩阵获取模块，其用于基于所述和计算不同时刻所述e
′
i0
下的比速度和所述b
i0
下的比速度的乘积累加和矩阵m(t)；
[0151]
奇异值分解解算模块，其用于利用代码生成方式对m(t)矩阵进行奇异值分解获取输出结果并基于所述获得行进间对准结果
[0152][0153]
一些实施例中，第一转换矩阵获取模块还用于：
[0154]
基于第一公式计算所述且第一公式包括：
[0155][0156]
δλ
t
＝λ
t-λ0，
[0157]
其中，λ
t
和l
t
分别为初始对准t时刻的卫星导航设备实时经度和纬度，λ0为对准起始时刻t0的纬度，δλ
t
表示当前时刻t相对t0的变化，ω
ie
为地球自转角速度。
[0158]
一些实施例中，第二转换矩阵获取模块还用于：
[0159]
基于第二公式计算所述所述第二公式包括：
[0160][0161][0162]
其中，为当前时刻惯性测量单元imu的陀螺输出b系下的角速度，i为单位矩阵，t0为对准起始时刻。
[0163]
一些实施例中，比速度乘积累加和矩阵获取模块还用于：
[0164]
基于第三公式计算所述比速度所述第三公式包括：
[0165][0166][0167][0168][0169][0170]
其中，为的转置，表示加速度，h为高度，r为地球半径，和为所述导航系n下当前时刻的速度vn(t)在x、y和z向的分量，g为当地重力加速度，τ为积分表达式的变量。
[0171]
一些实施例中，比速度乘积累加和矩阵获取模块还用于：
[0172]
基于第四公式计算所述比速度所述第四公式包括：
[0173][0174]
其中，fb为当前时刻惯性测量单元imu的加速度计输出b系下的比力，
[0175]
一些实施例中，比速度乘积累加和矩阵获取模块还用于：
[0176]
基于第五公式获取所述和的乘积累加和矩阵m(t)，所述第五公式包括：
[0177][0178]
其中，m(t-1)表示前一个周期的计算值,表示的转置，初始m(0)＝03×3。
[0179]
一些实施例中，奇异值分解解算模块还用于利用代码生成方式对m(t)矩阵进行奇异值分解获取具体地，奇异值分解解算模块还用于构建用于代码生成的奇异值分解函数svd_cg()；将所述函数作为入口函数输入m(t)矩阵后获得对应的生成代码文件；根据
所述代码文件获取函数的输出结果
[0180]
可优选地，构建用于代码生成的奇异值分解函数svd_cg()时在matlab中构建第一代码表示并基于所述第一代码新建所述svd_cg()，所述第一代码包括：
[0181]
function[u,s,v]＝svd_cg(m)
[0182]
[u,s,v]＝svd(m)；
[0183]
其中，m为所述m(t)矩阵，s为3x3矩阵且其对角线元素为奇异值，u为3x3矩阵且各列是奇异值的左奇异向量，v为3x3矩阵且各列是奇异值的右奇异向量。
[0184]
一些实施例中，奇异值分解解算模块还用于基于第六公式计算所述所述第六公式包括：
[0185][0186]
其中，det(x)表示矩阵x的行列式。
[0187]
一些实施例中，奇异值分解解算模块计算出所述后还基于第七公式对所述进行规范正交化，所述第七公式包括：
[0188][0189][0190]
其中，表示中间变量，i表示单位矩阵。
[0191]
一些实施例中，奇异值分解解算模块还用于：
[0192]
基于第八公式获取所述所述第八公式包括：
[0193][0194][0195]
本领域普通技术人员可以理解，上文中所公开方法中的全部或某些步骤、系统、装置中的功能模块/单元可以被实施为软件、固件、硬件及其适当的组合。在硬件实施方式中，在以上描述中提及的功能模块/单元之间的划分不一定对应于物理组件的划分；例如，一个物理组件可以具有多个功能，或者一个功能或步骤可以由若干物理组件合作执行。某些物理组件或所有物理组件可以被实施为由处理器，如中央处理器、数字信号处理器或微处理器执行的软件，或者被实施为硬件，或者被实施为集成电路，如专用集成电路。这样的软件可以分布在计算机可读存储介质上，计算机可读存储介质可以包括计算机可读存储介质(或非暂时性介质)和通信介质(或暂时性介质)。
[0196]
需要说明的是，在本发明中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作
之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
[0197]
以上所述仅是本发明的具体实施方式，使本领域技术人员能够理解或实现本发明。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。