System.ArgumentOutOfRangeException: 索引和长度必须引用该字符串内的位置。 参数名: length 在 System.String.Substring(Int32 startIndex, Int32 length) 在 zhuanliShow.Bind()
【技术实现步骤摘要】
本专利技术属于移动测量,更具体地,涉及一种惯性系下位姿精密积分解算方法及系统。
技术介绍
1、惯性导航系统基于牛顿力学的原理,利用陀螺仪测量载体相对于惯性空间的姿态变化信息、利用加速度计测量载体相对于惯性空间的加速度信息,通过给定初始状态,该系统可以根据陀螺的输出建立相应的姿态矩阵,并根据所建立的姿态矩阵将加速度计的测量值在相应的坐标系下进行积分,得出运载体的三维速度、位置和姿态。由于惯性导航系统采用积分解算的方式来解算载体的位置姿态等状态信息,使得惯性系统解算中的误差不断累积,并最终影响导航解算的精度和可靠性。为了提高解算精度和性能,可通过改进惯性导航系统中陀螺仪和加速度计的性能来减小这类硬件本身的误差。但由于惯导传感器本身就是很精密的仪器,其制作工艺极其复杂,惯性仪表性能的微小提升所需要付出较高得的代价。从理论方法层面解决此问题不仅成本较低,且通用性广。
2、积分运算是惯性导航系统实际应用过程中基础的、必不可少的环节,积分解算模型的特点和性能极大的影响位置姿态的精度及正确性。传统方法解算都以地心地固坐标系为基准,相应的惯性导航更新算法本质上是绑定在弯曲的地球表面的,每一时刻的更新都要将直线距离换成角度。公开号为cn117236072b的中国专利公开了一种基于隧道设计轴线的盾构目标位姿解算方法,包括:给定隧道设计轴线的参数化方程;进行盾构掘进绝对目标位置矢量解算,得到盾构掘进绝对目标位置;基于盾构掘进绝对目标位置,进行盾构掘进绝对目标姿态矩阵解算,得到盾构掘进绝对日标姿态;基于盾构掘进绝对日标位置和盾构掘进绝对目标
3、公开号为cn117236072b的中国专利,在计算过程中引入的数据参数较多,存在计算复杂、精度损失、模型内部参数交织,难以建立待求参数和观测量直接关系的问题,在复杂的地球结构参数反复转换时,无法确保位姿计算的准确性和效率。
技术实现思路
1、针对现有技术的以上缺陷或改进需求,本专利技术提供一种惯性系下位姿精密积分解算方法及系统,从惯性导航系统解算的底层理论入手,直接在地心惯性坐标系下积分解算,并扣除地球相关参数造成的影响,省略中间过程中涉及到的与地球参数相关的转换、积分及其他改正运算,可以有效提升计算效率,并降低当前惯性定位定姿解算方法造成的误差。
2、为实现以上目的,按照本专利技术实施例的第一方面,提供一种惯性系下位姿精密积分解算方法,包括:
3、s100、设定地心惯性坐标系为导航解算坐标系,并设定初始时刻的地心惯性坐标系与地心地固坐标系重合;
4、s200、根据初始时刻的地心惯性坐标系获取载体包括初始位置、初始速度以及初始姿态在内的初始状态信息;
5、s300、测量所述载体在中间时刻下的加速度和角速度,并将该加速度与角速度在地心惯性坐标系下积分,得到载体在终止时刻下的坐标;
6、s400、计算得到地心地固坐标系在地心惯性坐标系下的变化角度,地心地固坐标系绕z轴旋转该变化角度得到地心地固坐标系在地心惯性坐标系下的运动量,将所述载体在终止时刻下的坐标扣除地心地固坐标系在地心惯性坐标系下的运动量得到新的载体位姿信息;
7、s500、通过步骤s300中的信息计算得到误差方程,利用该误差方程对新的载体位姿信息进行校正,得到最终的载体位姿信息。
8、进一步地,步骤s200中,所述初始状态信息的获取包括:
9、s201、根据初始时刻的载体坐标系到地心地固坐标系的坐标转换矩阵和载体坐标系到导航坐标系的坐标转换矩阵,获取载体初始时刻的载体坐标系到地心惯性坐标系的坐标转换矩阵(初始姿态);
10、s202、根据载体在导航坐标系中的沿各方向的速度与位置以及地球参数确定载体在地心惯性坐标系下的初始三维速度;
11、s203、根据地心地固坐标系确定载体的初始位置。
12、进一步地,步骤s201中,所述初始时刻的载体坐标系到地心惯性坐标系的坐标转换矩阵(初始姿态)为:
13、;
14、其中,为初始状态下载体坐标系到地心地固坐标系的坐标转换矩阵。
15、进一步地,所述初始时刻的载体坐标系到地心地固坐标系的坐标转换矩阵为:
16、;
17、其中,为初始时刻的经度和纬度,
18、为初始时刻的纬度。
19、进一步地,所述初始时刻的载体坐标系到导航坐标系的坐标转换矩阵为:
20、;
21、其中,为初始时刻的航向姿态角,
22、为初始时刻的俯仰姿态角,
23、为初始时刻的横滚姿态角。
24、进一步地,步骤s202中,所述载体在地心惯性坐标系下的初始三维速度为:
25、;
26、其中,为载体在地心惯性坐标系下的初始三维速度,
27、为载体在导航坐标系中沿x轴方向的速度,
28、为载体在导航坐标系中沿y轴方向的速度,
29、为载体在导航坐标系中沿z轴方向的速度,
30、为地球卯酉圈的曲率半径,
31、为载体离地面的高程,
32、为地球自转角速度,
33、为载体所处地的纬度。
34、进一步地,步骤s203中,所述载体的初始位置为:
35、;
36、其中,为载体的初始三维位置矢量,
37、为初始时刻载体在地心地固坐标系下的坐标。
38、进一步地,步骤s300中,所述终止时刻为解算的目标时刻,所述载体在终止时刻的位姿信息通过状态更新公式进行更新,包括:
39、s301、位置更新:
40、;
41、其中,为m时刻的位置向量,
42、为m-1时刻的位置向量,
43、为m-1到m时刻的位置增量,
44、为m-1到m中间时刻的坐标转换矩阵。
45、进一步地,步骤s300还包括:
46、s302、姿态更新:
47、;
48、;
49、;
50、其中,为m时刻载体坐标系到导航坐标系的坐标转换矩阵,
51、为m-1时刻载体坐标系到导航坐标系的坐标转换矩阵,
52、为m-1时刻载体坐标系到m时刻载体坐标系的坐标转换矩阵,
53、为m时刻载体坐标系下的三维旋转矢量,
54、为m时刻的三维角度变化矢量,
55、为m-1时刻的三维角度变化矢量。
56、进一步地,步骤s300还包括:
57、s303、速度更新:
58、;
59、其中,为惯性系下的速度变化矢量,
60、为载体系相对于惯性系的坐标旋转矩阵,
61、为载体系下测量得到的比力信息,
62、为惯性系下地球重力矢量。
63、本文档来自技高网...
【技术保护点】
1.一种惯性系下位姿精密积分解算方法,其特征在于,包括:
2.根据权利要求1所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤S200中,所述初始状态信息的获取包括:
3.根据权利要求2所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤S201中,所述初始时刻的载体坐标系到地心惯性坐标系的坐标转换矩阵(初始姿态)为:
4.根据权利要求3所述的一种惯性系下位姿精密积分解算方法,其特征在于,所述初始时刻的载体坐标系到地心地固坐标系的坐标转换矩阵为:
5.根据权利要求3所述的一种惯性系下位姿精密积分解算方法,其特征在于,所述初始时刻的载体坐标系到导航坐标系的坐标转换矩阵为:
6.根据权利要求2所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤S202中,所述载体在地心惯性坐标系下的初始三维速度为:
7.根据权利要求2所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤S203中,所述载体的初始位置为:
8.根据权利要求1-7中任一项所述的一种惯性系下位姿精密积分解算方法,其特征在于
9.根据权利要求8所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤S300还包括:
10.根据权利要求8所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤S300还包括:
11.根据权利要求1-7中任一项所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤S500中,所述误差方程包括位置误差方程、姿态误差方程以及速度误差方程。
12.根据权利要求11所述的一种惯性系下位姿精密积分解算方法,其特征在于,所述速度误差方程为:
13.根据权利要求11所述的一种惯性系下位姿精密积分解算方法,其特征在于,所述姿态误差方程为:
14.根据权利要求11所述的一种惯性系下位姿精密积分解算方法,其特征在于,所述位置误差方程为:
15.一种惯性系下位姿精密积分解算系统,用于实现如权利要求1-4中任一项所述的一种惯性系下位姿精密积分解算方法,其特征在于,包括:
16.一种计算设备,其特征在于,包括:
17.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质上存储有计算机程序,所述计算机程序被处理器执行时实现如权利要求1至14任一项所述方法的步骤。
...【技术特征摘要】
1.一种惯性系下位姿精密积分解算方法,其特征在于,包括:
2.根据权利要求1所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤s200中,所述初始状态信息的获取包括:
3.根据权利要求2所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤s201中,所述初始时刻的载体坐标系到地心惯性坐标系的坐标转换矩阵(初始姿态)为:
4.根据权利要求3所述的一种惯性系下位姿精密积分解算方法,其特征在于,所述初始时刻的载体坐标系到地心地固坐标系的坐标转换矩阵为:
5.根据权利要求3所述的一种惯性系下位姿精密积分解算方法,其特征在于,所述初始时刻的载体坐标系到导航坐标系的坐标转换矩阵为:
6.根据权利要求2所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤s202中,所述载体在地心惯性坐标系下的初始三维速度为:
7.根据权利要求2所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤s203中,所述载体的初始位置为:
8.根据权利要求1-7中任一项所述的一种惯性系下位姿精密积分解算方法,其特征在于,步骤s300中,所述终止时刻为解算的目标时刻,所述载体在终止时刻的位姿信息通过状态更新公式进...
【专利技术属性】
技术研发人员:董翠军,毛庆洲,周宝定,唐超,熊勇钢,朱旭波,胡伟,李玉路,蒋志强,谭京坤,王中玉,李蔚铮,朱碧堂,党海龙,
申请(专利权)人:武汉汉宁轨道交通技术有限公司,
类型:发明
国别省市:
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。