CN112683267B - 一种附有gnss速度矢量辅助的车载姿态估计方法 - Google Patents

一种附有gnss速度矢量辅助的车载姿态估计方法 Download PDF

Info

Publication number
CN112683267B
CN112683267B CN202011373852.7A CN202011373852A CN112683267B CN 112683267 B CN112683267 B CN 112683267B CN 202011373852 A CN202011373852 A CN 202011373852A CN 112683267 B CN112683267 B CN 112683267B
Authority
CN
China
Prior art keywords
representing
gnss
attitude
accelerometer
variance
Prior art date
Legal status (The legal status 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 status listed.)
Active
Application number
CN202011373852.7A
Other languages
English (en)
Other versions
CN112683267A (zh
Inventor
周泽波
张泽亮
吴荩
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
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 University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN202011373852.7A priority Critical patent/CN112683267B/zh
Publication of CN112683267A publication Critical patent/CN112683267A/zh
Application granted granted Critical
Publication of CN112683267B publication Critical patent/CN112683267B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Abstract

本发明公开了一种附有GNSS速度矢量辅助的车载姿态估计方法，属于导航领域。该方法首先利用陀螺仪与加速度计检验GNSS速度，通过滑动递推窗口补偿运动加速度，在第一级滤波中融合陀螺仪与加速度计数据；然后利用GNSS速度检验磁力计模型中的磁场干扰，在第二级滤波中融合磁力计数据；最后建立GNSS速度观测矢量对方程，建立滚转/滚转‑偏航约束方程，完成第三级滤波。设计了级联式间接卡尔曼滤波器结构，对于不同传感器同步和异步均适用，每一级更新都使得下一级的量测模型线性化更加精准。所用的测量传感器间交叉检验，有益于误差补偿与修正。在整个过程中本发明实现了GNSS速度矢量信息价值的最大化。

Description

一种附有GNSS速度矢量辅助的车载姿态估计方法
技术领域
本发明属于导航领域，具体涉及附有GNSS速度矢量辅助的MARG多传感器级联式车载姿态估计方法。
背景技术
近几十年来，随着自动驾驶技术需求的快速增长，获取准确可靠的车辆姿态(俯仰、滚转和偏航)变得非常重要。典型应用场景包括车辆导航和智能感知、移动卫星通信、车辆编队、车辆横向/稳定性控制系统等。随着微机电(Micro-Electro-Mechanical System,MEMS)技术的发展，MEMS陀螺仪与加速度计已被广泛应用于估计载体的姿态信息。然而，由于存在陀螺漂移与数值积分误差，往往需要其他的低成本传感器进行辅助，以抑制偏航累积误差。与惯性传感器集成在一起的最为常用的一种传感器是磁力计。这种集成设备也被称为MARG(magnetic,angular rate and gravity,MARG)传感器阵列。总体而言，使用MARG传感器进行车辆姿态估计仍然具有挑战性，特别是对于一些未知的应用场景，其间通常伴随内部或外部未建模的误差源，如运动加速度和磁场干扰。作为重要的补充，全球卫星导航系统(Global Navigation Satellite System,GNSS)速度能够给MARG姿态估计带来众多好处。然而，在GNSS和MARG传感器数据融合过程中仍存在一些关键问题：(1)传统GNSS辅助惯性传感器测姿主要通过构造多维惯导误差状态的卡尔曼滤波器同时估计载体位置、速度与姿态参数(PVA)，但该架构姿态信息可观性差，PVA环路误差耦合严重。(2)低成本传感器观测数据质量不高，平台计算资源受限，需建立高效、鲁棒的数据融合测姿架构；(3)目前GNSS速度信息在姿态估计中的使用过于简单，未能实现其价值的最大化。比如，采用一阶GNSS速度差分计算补偿加速度计观测与参考矢量对的外部加速度干扰项，其无法适应具有高阶动力学特性的载体测姿场景。又如将GNSS速度矢量所导出的欧拉角直接融合MARG的方式使得部分误差内在统计特性对姿态求解的影响缺失。而在GNSS弱信号场景下所输出的速度信息未经系统级的交叉检验反而极易导致MARG姿态解不稳定甚至发散。(4)当发生磁场干扰、载体机动、GNSS多路效应等复杂境况时，如何保障姿态估计融合算法的适应性与鲁棒性。
发明内容
针对上述问题，本发明提出一种附有GNSS速度矢量辅助的MARG多传感器级联式车载姿态估计方法。
本发明方法首先利用陀螺仪与加速度计检验GNSS速度，通过滑动递推窗口补偿运动加速度，在第一级滤波中融合陀螺仪与加速度计数据；然后利用GNSS速度检验磁力计模型中的磁场干扰，在第二级滤波中融合磁力计数据；最后建立GNSS速度观测矢量对方程，建立滚转/滚转-偏航约束方程，完成第三级滤波。设计了级联式间接卡尔曼滤波器结构，对于不同传感器同步和异步均适用，每一级更新都使得下一级的量测模型线性化更加精准。所用的测量传感器间交叉检验，有益于误差补偿与修正。在整个过程中本发明实现了GNSS速度矢量信息价值的最大化，主要表现在以下四个方面：(1)使用GNSS速度的滑动时间窗口提取运动加速度时变特征，从而精准补偿MARG系统模型中的外部加速度干扰项；(2)将车辆载体系与GNSS坐标系关联，形成GNSS速度观测矢量对，以实现与MARG多矢量对匹配测姿模型的统一；(3)利用GNSS速度矢量构造关联参数以实现磁场干扰的实时可靠检测；(4)GNSS速度矢量对模型用于车载姿态估计过程中的陀螺漂移与加速度计零偏误差的同步估计。
本发明的技术方案为：一种附有GNSS速度矢量辅助的车载姿态估计方法，包括以下步骤：
步骤1：利用陀螺仪数据进行姿态推算；
步骤2：利用加速度计和陀螺仪对GNSS速度进行检测：
步骤2-1：利用长度为l的滑动递推窗口对GNSS速度进行p阶多项式拟合，进一步对其求导，获得运动加速度拟合多项式；对GNSS速度得到的运动加速度与陀螺仪和加速度计计算的运动加速度进行比较，检验GNSS速度是否异常；
步骤2-2：利用GNSS速度进行俯仰角求解，将其与陀螺仪获得的俯仰角进行比较，检验GNSS速度是否异常；
步骤3：利用GNSS速度对运动加速度进行补偿，进行陀螺仪/加速度计一级滤波；
步骤4：利用GNSS速度检测与排除磁力计模型中的磁场干扰；
步骤5：进行二级滤波，融合磁力计数据；
步骤6：进行三级滤波，融合GNSS速度信息。具体步骤如下：
步骤6-1：计算三级滤波状态及其方差的一步预测：
步骤6-2：利用GNSS速度建立载体速度矢量对方程，进一步建立三级滤波量测方程；
步骤6-3：建立三级滤波滚转/滚转-偏航约束方程；
步骤6-4：基于贝叶斯理论进行最优估计，得到含滚转/偏航-滚转约束的三级滤波状态估计值，完成姿态估计。
本发明公开了一种附有GNSS速度矢量辅助的MARG多传感器级联式车载姿态估计方法。该方法首先利用陀螺仪与加速度计检验GNSS速度，通过滑动递推窗口补偿运动加速度，在第一级滤波中融合陀螺仪与加速度计数据；然后利用GNSS速度检验磁力计模型中的磁场干扰，在第二级滤波中融合磁力计数据；最后建立GNSS速度观测矢量对方程，建立滚转/滚转-偏航约束方程，完成第三级滤波。设计了级联式间接卡尔曼滤波器结构，对于不同传感器同步和异步均适用，每一级更新都使得下一级的量测模型线性化更加精准。所用的测量传感器间交叉检验，有益于误差补偿与修正。在整个过程中本发明实现了GNSS速度矢量信息价值的最大化，主要表现在以下四个方面：(1)使用GNSS速度的滑动时间窗口提取运动加速度时变特征，从而精准补偿MARG系统模型中的外部加速度干扰项；(2)将车辆载体系与GNSS坐标系关联，形成GNSS速度观测矢量对，以实现与MARG多矢量对匹配测姿模型的统一；(3)利用GNSS速度矢量构造关联参数以实现磁场干扰的实时可靠检测；(4)GNSS速度矢量对模型用于车载姿态估计过程中的陀螺漂移与加速度计零偏误差的同步估计。
附图说明
图1为级联卡尔曼滤波算法框图。
具体实施方式
下面结合附图，对本发明采用的一种附有GNSS速度矢量辅助的MARG多传感器级联式车载姿态估计方法进行详细说明：
步骤1：利用陀螺仪数据进行姿态预测：
建立陀螺仪输出模型：
zg＝ω+bg+εg (1)
其中，zg表示陀螺输出，ω表示角速率，bg为陀螺漂移误差，εg为零均值高斯噪声；根据四元数微分方程进行姿态更新：
Figure BDA0002806798130000031
其中，q表示姿态四元数真值；顶标·表示微分运算(一阶导数运算)；
Figure BDA0002806798130000036
表示四元数乘法；上标
Figure BDA0002806798130000037
表示矩阵转置；利用陀螺仪输出进行姿态更新将引入姿态误差，即有
Figure BDA0002806798130000032
其中，
Figure BDA0002806798130000033
表示姿态估计值，qe为姿态误差，可近似写为
Figure BDA0002806798130000034
其中，qξ表示四元数的第2至第4个分量，对于3维向量qξ，有状态微分方程：
Figure BDA0002806798130000035
步骤2：利用加速度计和陀螺仪，对GNSS速度进行检测；
步骤2-1：利用长度为l的滑动递推窗口对GNSS速度进行p阶多项式拟合，然后对拟合的多项式进行求导，获得相应的运动加速度拟合多项式；对GNSS速度得到的运动加速度与陀螺仪和加速度计计算的运动加速度进行比较，检验GNSS速度是否异常；具体步骤如下：
给定滑动窗口长度l，则e系(WGS84坐标系)下的GNSS速度序列可表示为：
Figure BDA0002806798130000041
其中，
Figure BDA0002806798130000042
表示k时刻的速度，βi为p阶回归模型的参数，Δts为GNSS采样间隔，ηi表示模型噪声，
Figure BDA0002806798130000043
其中，I3×3表示3阶单位阵，符号ο表示克罗内克积运算；进一步可得最小二乘准则下的参数估计值
Figure BDA0002806798130000044
Figure BDA0002806798130000045
其中，rv,k-l:k表示从k-l时刻到k时刻的GNSS速度观测序列；
Figure BDA0002806798130000046
表示rv,k-l:k的方差矩阵；上标-1表示矩阵求逆运算。
利用回归模型计算k时刻的速度
Figure BDA0002806798130000047
Figure BDA0002806798130000048
其中，
Figure BDA00028067981300000411
对其求导得e系下的加速度为：
Figure BDA0002806798130000049
其中，
Figure BDA00028067981300000410
p表示速度序列回归模型的阶数；根据方差传递准则，其方差
Figure BDA0002806798130000051
为：
Figure BDA0002806798130000052
其中，
Figure BDA0002806798130000053
表示参数估计值
Figure BDA0002806798130000054
的方差；
对GNSS速度得到的运动加速度与陀螺仪和加速度计计算的运动加速度进行比较，检验GNSS速度是否异常：
利用GNSS速度计算b系(载体坐标系)下的运动加速度
Figure BDA0002806798130000055
Figure BDA0002806798130000056
其中，角标n表示导航坐标系；
Figure BDA0002806798130000057
表示利用陀螺更新的姿态四元数，
Figure BDA0002806798130000058
表示将四元数转为n系到b系的捷联姿态矩阵；
Figure BDA0002806798130000059
表示e系到n系的姿态矩阵，
Figure BDA00028067981300000510
由式(10)计算获得。
加速度计输出模型为：
Figure BDA00028067981300000511
其中，za表示加速度计输出；ba为加速度计零偏误差；ab表示载体外部加速度，εa为加速度计噪声，g表示重力加速度；利用加速度计与陀螺仪可计算运动加速度
Figure BDA00028067981300000512
Figure BDA00028067981300000513
对两种方法计算的运动加速度进行比较，以检验GNSS速度是否有效：
Figure BDA00028067981300000514
其中，κv为经验阈值，其值与GNSS精度和加速度计的零偏稳定性有关；|| ||表示取模运算；
步骤2-2：利用GNSS速度进行俯仰角求解，将其与陀螺计算的俯仰角进行比较，进一步检验GNSS速度是否异常；
两种方法计算俯仰角：
Figure BDA00028067981300000515
其中，θs表示利用GNSS速度计算的俯仰角，θa表示根据加速度计与陀螺仪计算的俯仰角，
Figure BDA00028067981300000516
Figure BDA0002806798130000061
分别表示GNSS速度在地向、北向和东向的分量；arctan表示反正切运算；arcsin表示反正弦运算；矩阵下标(1,3)表示取矩阵第1行第3列的元素；
检验GNSS速度是否有效：
Figure BDA0002806798130000062
其中，κθ为俯仰角阈值，||表示取绝对值；
步骤3：当GNSS速度可用的情况下，对运动加速度进行补偿，进行陀螺仪/加速度计一级滤波；
取3维姿态误差qξ，陀螺漂移误差bg和加速度计零偏误差ba构建滤波状态矢量
Figure BDA0002806798130000063
并建立传感器漂移噪声模型：
Figure BDA0002806798130000064
其中，顶标·表示一阶导数运算，
Figure BDA0002806798130000065
表示陀螺漂移模型噪声，
Figure BDA0002806798130000066
表示噪声
Figure BDA0002806798130000067
服从0均值方差为
Figure BDA0002806798130000068
的高斯分布；
Figure BDA0002806798130000069
表示加速度计零偏模型噪声，
Figure BDA00028067981300000610
表示噪声
Figure BDA00028067981300000611
服从0均值方差为
Figure BDA00028067981300000612
的高斯分布；
记状态转移模型噪声
Figure BDA00028067981300000613
可建立系统状态模型：
Figure BDA00028067981300000614
其中，转移矩阵B与系统噪声方差Q分别为：
Figure BDA00028067981300000615
其中，Rg表示陀螺随机噪声的方差；
Figure BDA00028067981300000616
表示陀螺漂移模型噪声的方差；
Figure BDA00028067981300000617
表示加速度计零偏模型的噪声方差；[·×]表示反对称矩阵运算。进一步地，在系统状态模型的基础上，离散化的系统状态模型可写为：
Figure BDA0002806798130000071
其中，xk表示k时刻的状态矢量，
Figure BDA0002806798130000072
表示离散化的系统状态模型噪声，状态转移矩阵为Φk,k-1＝I9×9+BΔts；
利用加速度计建立量测模型：
Figure BDA0002806798130000073
其中，
Figure BDA0002806798130000074
Figure BDA0002806798130000075
Figure BDA0002806798130000076
表示k时刻的量测信息，Ha,k表示k时刻的加速度计量测模型矩阵，za,k表示k时刻的加速度计输出，
Figure BDA0002806798130000077
表示利用陀螺估计的姿态四元数，
Figure BDA0002806798130000078
表示k时刻的运动加速度估计，利用(12)获得。基于线性卡尔曼滤波进行状态估计，获得融合加速度计信息后的状态
Figure BDA0002806798130000079
及方差
Figure BDA00028067981300000710
将一级滤波的结果更新至姿态四元数并做归一化处理：
Figure BDA00028067981300000711
其中，
Figure BDA00028067981300000712
表示一级滤波k时刻融合加速度计信息后的姿态误差，
Figure BDA00028067981300000713
表示一级滤波k时刻融合加速度计信息后的状态估计量，
Figure BDA00028067981300000714
表示取矢量
Figure BDA00028067981300000715
的第1至第3个分量；
Figure BDA00028067981300000716
表示k时刻利用陀螺估计的姿态四元数；
Figure BDA00028067981300000717
表示一级滤波k时刻融合加速度计信息后的姿态四元数；用
Figure BDA00028067981300000718
更新
Figure BDA00028067981300000719
并对状态矢量
Figure BDA00028067981300000720
中qξ置零；
步骤4：在GNSS速度可用的情况下，利用GNSS速度进行磁力计模型磁场干扰检测：对GNSS速度计算的偏航角和磁力计计算的偏航角进行比较，以此检测是否存在磁场干扰。
利用GNSS速度计算载体偏航角：
Figure BDA00028067981300000721
磁力计计算的偏航角记为γm，设定阈值κγ，通过比较两种方法计算的偏航角，检验是否存在磁场干扰；
Figure BDA00028067981300000722
步骤5：当检测结果为无磁场干扰时，进行二级滤波，融合磁力计数据；
进行二级滤波状态及其方差一步预测：
Figure BDA0002806798130000081
Figure BDA0002806798130000082
其中，
Figure BDA0002806798130000083
表示k时刻融合加速度计信息后的状态估计，在步骤3中获得，其方差为Pa,k；
Figure BDA0002806798130000084
表示k时刻融合磁力计信息后的状态一步预测，
Figure BDA0002806798130000085
表示状态
Figure BDA0002806798130000086
方差，Pa,k(1:3,1:3)表示取矩阵Pa,k第1至3行，1至3列的矩阵；
建立磁力计量测模型：
Figure BDA0002806798130000087
其中，zm表示磁力计输出，mn表示n系下的地磁参考矢量；εm为磁力计噪声，将模型记为
Figure BDA0002806798130000088
其中，量测信息
Figure BDA0002806798130000089
量测矩阵
Figure BDA00028067981300000810
融合加速度计后的姿态四元数
Figure BDA00028067981300000811
通过步骤3中(23)获得；为保证磁力计数据仅用于偏航角修正，而不影响俯仰和滚转，引入约束矩阵计算系统滤波增益Km,k：
Figure BDA00028067981300000812
其中，Rm表示磁力计的噪声方差，一步预测方差
Figure BDA00028067981300000813
由(27)获得。约束矩阵Λm,k为：
Figure BDA00028067981300000814
进行二级滤波观测修正：
Figure BDA00028067981300000815
Figure BDA00028067981300000816
其中，
Figure BDA00028067981300000817
表示k时刻融合磁力计信息后的状态估计，Pm,k表示
Figure BDA00028067981300000818
的方差；一步预测值
Figure BDA00028067981300000819
由(26)计算获得，增益Km,k利用(30)计算获得；利用二级滤波的结果更新姿态四元数并做归一化处理：
Figure BDA0002806798130000091
其中，
Figure BDA0002806798130000092
表示二级滤波k时刻融合磁力计信息后的姿态误差，
Figure BDA0002806798130000093
表示二级滤波k时刻融合磁力计信息后的状态估计量，见(32)；
Figure BDA0002806798130000094
为矢量
Figure BDA0002806798130000095
的第1至第3个分量；
Figure BDA0002806798130000096
表示一级滤波k时刻融合加速度计信息后的姿态四元数，见(23)；
Figure BDA0002806798130000097
表示二级滤波k时刻融合磁力计信息后的姿态四元数。用
Figure BDA0002806798130000098
更新
Figure BDA0002806798130000099
并对状态矢量
Figure BDA00028067981300000910
中qξ置零；
步骤6：当GNSS速度正常情况下，融合GNSS速度信息，进行三级滤波；具体步骤如下：
步骤6-1：对于三级滤波，计算状态及其方差的一步预测：
Figure BDA00028067981300000911
Figure BDA00028067981300000912
其中，
Figure BDA00028067981300000913
表示k时刻融合GNSS速度信息后的状态一步预测，
Figure BDA00028067981300000914
表示k时刻融合磁力计信息后的状态估计量，见(32)；
Figure BDA00028067981300000915
和Pm,k分别为
Figure BDA00028067981300000916
和
Figure BDA00028067981300000917
的方差。
步骤6-2：利用GNSS速度建立三级滤波量测方程：
利用GNSS速度建立载体速度矢量对方程：
Figure BDA00028067981300000918
其中，
Figure BDA00028067981300000919
表示b系下的速度矢量估计，ve表示e系下的速度矢量，
Figure BDA00028067981300000920
表示由e系到b系的旋转矩阵，ε1表示噪声，b系下载体的速度可表示为
Figure BDA00028067981300000921
式(37)中，噪声ε1包含两类影响：一为车轮轨迹和车辆b系X轴的定义方向存在偏差；二为由路面引起车辆滑动或跳跃，导致在Y轴或Z轴存在速度；GNSS速度观测模型有：
Figure BDA00028067981300000922
其中，
Figure BDA00028067981300000923
表示GNSS速度观测，ε2表示噪声，将(39)带入(37)，得GNSS速度矢量对方程：
Figure BDA00028067981300000924
记
Figure BDA00028067981300000925
Figure BDA00028067981300000926
式(40)可写作
Figure BDA0002806798130000101
根据误差传播定律确定εs的方差Rs：
Figure BDA0002806798130000102
其中，R1和R2分别表示噪声ε1和ε2的方差；利用GNSS速度建立三级滤波量测方程
Figure BDA0002806798130000103
其中，观测信息为
Figure BDA0002806798130000104
观测矩阵为
Figure BDA0002806798130000105
zs,k表示k时刻的zs，见(41)；
Figure BDA0002806798130000106
为步骤5中融合磁力计信息后的姿态四元数，见(34)。
步骤6-3：建立三级滤波滚转/偏航-滚转约束方程：
利用Z轴角速率输出zg(3)(t)检测载体是否存在转弯：
Figure BDA0002806798130000107
由于GNSS速度无法测量载体滚转角，在三级滤波中，GNSS速度不对二级滤波的姿态滚转角修正；据此可建立滚转约束方程：
Figure BDA0002806798130000108
其中，矩阵下标(2,3)表示取矩阵第2行第3列的元素；(3,3)表示取矩阵第3行第3列的元素。
Figure BDA0002806798130000109
Figure BDA00028067981300001010
其中，
Figure BDA00028067981300001011
表示三级滤波中融合GNSS速度后的姿态四元数估计；
Figure BDA00028067981300001012
表示
Figure BDA00028067981300001013
与姿态真值间的误差四元数；
Figure BDA00028067981300001014
表示三级滤波中融合GNSS速度后的状态矢量估计；向量下标(1)，(2)，(3)，以及下文出现的(4)分别表示取向量的第1、第2、第3、第4个元素。
进一步整理，可将滚转约束方程改写成如下形式
Figure BDA00028067981300001015
其中，约束矩阵为
Figure BDA0002806798130000111
在车辆转弯时，GNSS速度方向与载体偏航角方向存在偏差；为抑制其对偏航角的影响，在转弯情况下建立偏航-滚转约束方程，约束矩阵为
Figure BDA0002806798130000112
步骤6-4：基于贝叶斯理论进行最优估计，得到含滚转/偏航-滚转约束的三级滤波状态估计值，完成姿态估计：
基于贝叶斯理论建立如下最优问题：
Figure BDA0002806798130000113
其中，Vk和
Figure BDA0002806798130000114
分别表示观测和预测的残差，
Figure BDA0002806798130000115
引入拉格朗日算子，
Figure BDA0002806798130000116
对其求一阶偏导数，有
Figure BDA0002806798130000121
其中，
Figure BDA0002806798130000122
表示求偏导数运算。最终得有无滚转约束的状态估计
Figure BDA0002806798130000123
和
Figure BDA0002806798130000124
间的关系
Figure BDA0002806798130000125
Figure BDA0002806798130000126
其中，
Figure BDA0002806798130000127
表示未进行滚转约束的滤波估计值，其方差为
Figure BDA0002806798130000128
表示进行滚转约束的滤波估计值，其方差为Ps,k；利用三级滤波的结果更新姿态四元数并做归一化处理：
Figure BDA0002806798130000129
其中，
Figure BDA00028067981300001210
表示三级滤波中融合GNSS速度后的姿态四元数估计；
Figure BDA00028067981300001211
表示三级滤波姿态误差四元数；
Figure BDA00028067981300001212
表示三级滤波中融合GNSS速度后的状态矢量估计；
Figure BDA00028067981300001213
为
Figure BDA00028067981300001214
的第1至第3个元素；
Figure BDA00028067981300001215
表示二级滤波中融合磁力计信息后的姿态四元数，见式(34)。

Claims (5)

1.一种附有GNSS速度矢量辅助的车载姿态估计方法，包括以下步骤：
步骤1：利用陀螺仪数据进行姿态推算；
步骤2：利用加速度计和陀螺仪对GNSS速度进行检测：
步骤2-1：利用长度为l的滑动递推窗口对GNSS速度进行p阶多项式拟合，进一步对其求导，获得运动加速度拟合多项式；对GNSS速度得到的运动加速度与陀螺仪和加速度计计算的运动加速度进行比较，检验GNSS速度是否异常；
步骤2-2：利用GNSS速度进行俯仰角求解，将其与陀螺仪获得的俯仰角进行比较，检验GNSS速度是否异常；
步骤3：利用GNSS速度对运动加速度进行补偿，进行陀螺仪/加速度计一级滤波；
取3维姿态误差qξ，陀螺漂移误差bg和加速度计零偏误差ba构建滤波状态矢量
Figure FDA0003564457890000011
并建立传感器漂移噪声模型：
Figure FDA0003564457890000012
Figure FDA0003564457890000013
其中，顶标·表示一阶导数运算，
Figure FDA0003564457890000014
表示陀螺漂移模型噪声，
Figure FDA0003564457890000015
表示噪声
Figure FDA0003564457890000016
服从0均值方差为
Figure FDA0003564457890000017
的高斯分布；
Figure FDA0003564457890000018
表示加速度计零偏模型噪声，
Figure FDA0003564457890000019
表示噪声
Figure FDA00035644578900000110
服从0均值方差为
Figure FDA00035644578900000111
的高斯分布；
记状态转移模型噪声
Figure FDA00035644578900000112
可建立系统状态模型：
Figure FDA00035644578900000113
其中，转移矩阵B与系统噪声方差Q分别为：
Figure FDA00035644578900000114
其中，Rg表示陀螺随机噪声的方差；
Figure FDA00035644578900000115
表示陀螺漂移模型噪声的方差；
Figure FDA00035644578900000116
表示加速度计零偏模型的噪声方差；[·×]表示反对称矩阵运算；进一步地，在系统状态模型的基础上，离散化的系统状态模型可写为：
Figure FDA0003564457890000021
其中，xk表示k时刻的状态矢量，
Figure FDA0003564457890000022
表示离散化的系统状态模型噪声，状态转移矩阵为Φk,k-1＝I9×9+BΔts；
利用加速度计建立量测模型：
Figure FDA0003564457890000023
其中，
Figure FDA0003564457890000024
Figure FDA0003564457890000025
表示k时刻的量测信息，Ha,k表示k时刻的加速度计量测模型矩阵，za,k表示k时刻的加速度计输出，
Figure FDA0003564457890000026
表示利用陀螺估计的姿态四元数，
Figure FDA0003564457890000027
表示k时刻的运动加速度估计；基于线性卡尔曼滤波进行状态估计，获得融合加速度计信息后的状态
Figure FDA0003564457890000028
及方差Pa,k；将一级滤波的结果更新至姿态四元数并做归一化处理：
Figure FDA0003564457890000029
Figure FDA00035644578900000210
其中，
Figure FDA00035644578900000211
表示一级滤波k时刻融合加速度计信息后的姿态误差，
Figure FDA00035644578900000212
表示一级滤波k时刻融合加速度计信息后的状态估计量，
Figure FDA00035644578900000213
表示取矢量
Figure FDA00035644578900000214
的第1至第3个分量；
Figure FDA00035644578900000215
表示k时刻利用陀螺估计的姿态四元数；
Figure FDA00035644578900000216
表示一级滤波k时刻融合加速度计信息后的姿态四元数；用
Figure FDA00035644578900000217
更新
Figure FDA00035644578900000218
并对状态矢量
Figure FDA00035644578900000219
中qξ置零；
步骤4：利用GNSS速度检测排除磁力计模型中的磁场干扰；
步骤5：进行二级滤波，融合磁力计数据；
进行二级滤波状态及其方差一步预测：
Figure FDA00035644578900000220
Figure FDA00035644578900000221
其中，
Figure FDA00035644578900000222
表示k时刻融合加速度计信息后的状态估计，在步骤3中获得，其方差为Pa,k；
Figure FDA00035644578900000223
表示k时刻融合磁力计信息后的状态一步预测，
Figure FDA0003564457890000031
表示状态
Figure FDA0003564457890000032
方差，Pa,k(1:3,1:3)表示取矩阵Pa,k第1至3行，1至3列的矩阵；
建立磁力计量测模型：
Figure FDA0003564457890000033
其中，zm表示磁力计输出，mn表示n系下的地磁参考矢量；εm为磁力计噪声，将模型记为
Figure FDA0003564457890000034
其中，量测信息
Figure FDA0003564457890000035
量测矩阵
Figure FDA0003564457890000036
融合加速度计后的姿态四元数
Figure FDA0003564457890000037
通过步骤3中(23)获得；为保证磁力计数据仅用于偏航角修正，而不影响俯仰和滚转，引入约束矩阵计算系统滤波增益Km,k：
Figure FDA0003564457890000038
其中，Rm表示磁力计的噪声方差，一步预测方差
Figure FDA0003564457890000039
由公式(8)获得；约束矩阵Λm,k为：
Figure FDA00035644578900000310
进行二级滤波观测修正：
Figure FDA00035644578900000311
Figure FDA00035644578900000312
其中，
Figure FDA00035644578900000313
表示k时刻融合磁力计信息后的状态估计，Pm,k表示
Figure FDA00035644578900000314
的方差；一步预测值
Figure FDA00035644578900000315
由(7)计算获得，增益Km,k利用(11)计算获得；利用二级滤波的结果更新姿态四元数并做归一化处理：
Figure FDA00035644578900000316
Figure FDA00035644578900000317
其中，
Figure FDA00035644578900000318
表示二级滤波k时刻融合磁力计信息后的姿态误差，
Figure FDA00035644578900000319
表示二级滤波k时刻融合磁力计信息后的状态估计量，见(13)；
Figure FDA00035644578900000320
为矢量
Figure FDA00035644578900000321
的第1至第3个分量；
Figure FDA00035644578900000322
表示一级滤波k时刻融合加速度计信息后的姿态四元数，见(6)；
Figure FDA00035644578900000323
表示二级滤波k时刻融合磁力计信息后的姿态四元数；用
Figure FDA00035644578900000324
更新
Figure FDA00035644578900000325
并对状态矢量
Figure FDA00035644578900000326
中qξ置零；
步骤6：进行三级滤波，融合GNSS速度信息；具体步骤如下：
步骤6-1：计算三级滤波状态及其方差的一步预测：
对于三级滤波，计算状态及其方差的一步预测：
Figure FDA0003564457890000041
Figure FDA0003564457890000042
其中，
Figure FDA0003564457890000043
表示k时刻融合GNSS速度信息后的状态一步预测，
Figure FDA0003564457890000044
表示k时刻融合磁力计信息后的状态估计量；
Figure FDA0003564457890000045
和Pm,k分别为
Figure FDA0003564457890000046
和
Figure FDA0003564457890000047
的方差；
步骤6-2：利用GNSS速度建立载体速度矢量对方程，进一步建立三级滤波量测方程；
利用GNSS速度建立载体速度矢量对方程：
Figure FDA0003564457890000048
其中，
Figure FDA0003564457890000049
表示b系下的速度矢量估计，ve表示e系下的速度矢量，
Figure FDA00035644578900000410
表示由e系到b系的旋转矩阵，ε1表示噪声，b系下载体的速度可表示为
Figure FDA00035644578900000411
式(37)中，噪声ε1包含两类影响：一为车轮轨迹和车辆b系X轴的定义方向存在偏差；二为由路面引起车辆滑动或跳跃，导致在Y轴或Z轴存在速度；GNSS速度观测模型有：
Figure FDA00035644578900000412
其中，
Figure FDA00035644578900000413
表示GNSS速度观测，ε2表示噪声，将(39)带入(37)，得GNSS速度矢量对方程：
Figure FDA00035644578900000414
记
Figure FDA00035644578900000415
式(40)可写作
Figure FDA00035644578900000416
根据误差传播定律确定εs的方差Rs：
Figure FDA00035644578900000417
其中，R1和R2分别表示噪声ε1和ε2的方差；利用GNSS速度建立三级滤波量测方程
Figure FDA00035644578900000418
其中，观测信息为
Figure FDA0003564457890000051
观测矩阵为
Figure FDA0003564457890000052
zs,k表示k时刻的zs；
Figure FDA0003564457890000053
为步骤5中融合磁力计信息后的姿态四元数；
步骤6-3：建立三级滤波滚转/偏航-滚转约束方程：
利用Z轴角速率输出zg(3)(t)检测载体是否存在转弯：
Figure FDA0003564457890000054
由于GNSS速度无法测量载体滚转角，在三级滤波中，GNSS速度不对二级滤波的姿态滚转角修正；据此可建立滚转约束方程：
Figure FDA0003564457890000055
其中，矩阵下标(2,3)表示取矩阵第2行第3列的元素；(3,3)表示取矩阵第3行第3列的元素；
Figure FDA0003564457890000056
Figure FDA0003564457890000057
其中，
Figure FDA0003564457890000058
表示三级滤波中融合GNSS速度后的姿态四元数估计；
Figure FDA0003564457890000059
表示
Figure FDA00035644578900000510
与姿态真值间的误差四元数；
Figure FDA00035644578900000511
表示三级滤波中融合GNSS速度后的状态矢量估计；向量下标(1)，(2)，(3)，以及下文出现的(4)分别表示取向量的第1、第2、第3、第4个元素；
将滚转约束方程改写成如下形式
Figure FDA00035644578900000512
其中，约束矩阵为
Figure FDA0003564457890000061
在车辆转弯时，GNSS速度方向与载体偏航角方向存在偏差；为抑制其对偏航角的影响，在转弯情况下建立偏航-滚转约束方程，约束矩阵为
Figure FDA0003564457890000062
步骤6-4：基于贝叶斯理论进行最优估计，得到含滚转/偏航-滚转约束的三级滤波状态估计值，完成姿态估计：
基于贝叶斯理论建立如下最优问题：
Figure FDA0003564457890000063
Figure FDA0003564457890000064
其中，Vk和
Figure FDA0003564457890000065
分别表示观测和预测的残差，
Figure FDA0003564457890000066
Figure FDA0003564457890000067
引入拉格朗日算子，
Figure FDA0003564457890000068
对其求一阶偏导数，有
Figure FDA0003564457890000071
其中，
Figure FDA0003564457890000072
表示求偏导数运算；最终得有无滚转约束的状态估计
Figure FDA0003564457890000073
和
Figure FDA0003564457890000074
间的关系
Figure FDA0003564457890000075
Figure FDA0003564457890000076
其中，
Figure FDA0003564457890000077
表示未进行滚转约束的滤波估计值，其方差为
Figure FDA0003564457890000078
Figure FDA0003564457890000079
表示进行滚转约束的滤波估计值，其方差为Ps,k；利用三级滤波的结果更新姿态四元数并做归一化处理：
Figure FDA00035644578900000710
Figure FDA00035644578900000711
其中，
Figure FDA00035644578900000712
表示三级滤波中融合GNSS速度后的姿态四元数估计；
Figure FDA00035644578900000713
表示三级滤波姿态误差四元数；
Figure FDA00035644578900000714
表示三级滤波中融合GNSS速度后的状态矢量估计；
Figure FDA00035644578900000715
为
Figure FDA00035644578900000716
的第1至第3个元素；
Figure FDA00035644578900000717
表示二级滤波中融合磁力计信息后的姿态四元数。
2.如权利要求1所述的一种附有GNSS速度矢量辅助的车载姿态估计方法，其特征在于，所述步骤1的具体方法为：
建立陀螺仪输出模型：
zg＝ω+bg+εg (16)
其中，zg表示陀螺输出，ω表示角速率，bg为陀螺漂移误差，εg为零均值高斯噪声；根据四元数微分方程进行姿态更新：
Figure FDA00035644578900000718
其中，q表示姿态四元数真值；顶标·表示微分运算；
Figure FDA00035644578900000719
表示四元数乘法；上标
Figure FDA00035644578900000721
表示矩阵转置；利用陀螺仪输出进行姿态更新将引入姿态误差，即有
Figure FDA00035644578900000720
其中，
Figure FDA0003564457890000081
表示姿态估计值，qe为姿态误差，可近似写为
Figure FDA0003564457890000082
其中，qξ表示四元数的第2至第4个分量，对于3维向量qξ，有状态微分方程：
Figure FDA0003564457890000083
3.如权利要求1所述的一种附有GNSS速度矢量辅助的车载姿态估计方法，其特征在于，所述步骤2-1的方法为：
利用长度为l的滑动递推窗口对GNSS速度进行p阶多项式拟合，然后对拟合的多项式进行求导，获得相应的运动加速度拟合多项式；对GNSS速度得到的运动加速度与陀螺仪和加速度计计算的运动加速度进行比较，检验GNSS速度是否异常；具体步骤如下：
给定滑动窗口长度l，则e系下的GNSS速度序列可表示为：
Figure FDA0003564457890000084
其中，e系表示WGS84坐标系，
Figure FDA0003564457890000085
表示k时刻的速度，βi为p阶回归模型的参数，Δts为GNSS采样间隔，ηi表示模型噪声，
Figure FDA0003564457890000086
Figure FDA0003564457890000087
其中，I3×3表示3阶单位阵，符号
Figure FDA00035644578900000811
表示克罗内克积运算；进一步可得最小二乘准则下的参数估计值
Figure FDA0003564457890000088
Figure FDA0003564457890000089
其中，rv,k-l:k表示从k-l时刻到k时刻的GNSS速度观测序列；
Figure FDA00035644578900000810
表示rv,k-l:k的方差矩阵；上标-1表示矩阵求逆运算；
利用回归模型计算k时刻的速度
Figure FDA0003564457890000091
Figure FDA0003564457890000092
其中，
Figure FDA0003564457890000093
对其求导得e系下的加速度为：
Figure FDA0003564457890000094
其中，
Figure FDA0003564457890000095
p表示速度序列回归模型的阶数；根据方差传递准则，其方差
Figure FDA0003564457890000096
为：
Figure FDA0003564457890000097
其中，
Figure FDA0003564457890000098
表示参数估计值
Figure FDA0003564457890000099
的方差；
对GNSS速度得到的运动加速度与陀螺仪和加速度计计算的运动加速度进行比较，检验GNSS速度是否异常：
利用GNSS速度计算b系下的运动加速度
Figure FDA00035644578900000910
Figure FDA00035644578900000911
其中，b系表示载体坐标系，角标n表示导航坐标系；
Figure FDA00035644578900000912
表示利用陀螺更新的姿态四元数，
Figure FDA00035644578900000913
表示将四元数转为n系到b系的捷联姿态矩阵；
Figure FDA00035644578900000914
表示e系到n系的姿态矩阵，
Figure FDA00035644578900000915
由式(25)计算获得；
加速度计输出模型为：
Figure FDA00035644578900000916
其中，za表示加速度计输出；ba为加速度计零偏误差；ab表示载体外部加速度，εa为加速度计噪声，g表示重力加速度；利用加速度计与陀螺仪可计算运动加速度
Figure FDA00035644578900000917
Figure FDA00035644578900000918
对两种方法计算的运动加速度进行比较，以检验GNSS速度是否有效：
Figure FDA00035644578900000919
其中，κv为经验阈值，其值与GNSS精度和加速度计的零偏稳定性有关；|| ||表示取模运算。
4.如权利要求1所述的一种附有GNSS速度矢量辅助的车载姿态估计方法，其特征在于，所述步骤2-2的方法为：
利用GNSS速度进行俯仰角求解，将其与陀螺计算的俯仰角进行比较，进一步检验GNSS速度是否异常；
两种方法计算俯仰角：
Figure FDA0003564457890000101
其中，θs表示利用GNSS速度计算的俯仰角，θa表示根据加速度计与陀螺仪计算的俯仰角，
Figure FDA0003564457890000102
Figure FDA0003564457890000103
分别表示GNSS速度在地向、北向和东向的分量；arctan表示反正切运算；arcsin表示反正弦运算；矩阵下标(1,3)表示取矩阵第1行第3列的元素；
检验GNSS速度是否有效：
Figure FDA0003564457890000104
其中，κθ为俯仰角阈值，||表示取绝对值。
5.如权利要求1所述的一种附有GNSS速度矢量辅助的车载姿态估计方法，其特征在于，所述步骤4的方法为：
利用GNSS速度计算载体偏航角：
Figure FDA0003564457890000105
磁力计计算的偏航角记为γm，设定阈值κγ，通过比较两种方法计算的偏航角，检验是否存在磁场干扰；
Figure FDA0003564457890000106
CN202011373852.7A 2020-11-30 2020-11-30 一种附有gnss速度矢量辅助的车载姿态估计方法 Active CN112683267B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011373852.7A CN112683267B (zh) 2020-11-30 2020-11-30 一种附有gnss速度矢量辅助的车载姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011373852.7A CN112683267B (zh) 2020-11-30 2020-11-30 一种附有gnss速度矢量辅助的车载姿态估计方法

Publications (2)

Publication Number Publication Date
CN112683267A CN112683267A (zh) 2021-04-20
CN112683267B true CN112683267B (zh) 2022-06-03

Family

ID=75446930

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011373852.7A Active CN112683267B (zh) 2020-11-30 2020-11-30 一种附有gnss速度矢量辅助的车载姿态估计方法

Country Status (1)

Country Link
CN (1) CN112683267B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113790720B (zh) * 2021-08-16 2023-08-15 北京自动化控制设备研究所 一种基于递推最小二乘的抗扰动粗对准方法
CN114413933A (zh) * 2022-01-17 2022-04-29 广东星舆科技有限公司 一种加速度计动态较准方法、系统及存储介质
CN114608570B (zh) * 2022-02-25 2023-06-30 电子科技大学 一种多模式自切换的管线仪自适应精密定位方法
CN117390347A (zh) * 2023-12-06 2024-01-12 无锡车联天下信息技术有限公司 一种基于运动估计漂移优化的汽车航向角解算方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1690067A1 (en) * 2003-12-05 2006-08-16 Honeywell International Inc. System and method for using multiple aiding sensors in a deeply integrated navigation system
CA2769085A1 (en) * 2011-02-28 2012-08-28 Research In Motion Limited Portable electronic device adapted to compensate for gyroscope bias
WO2014022664A2 (en) * 2012-08-02 2014-02-06 Memsic, Inc. Method and apparatus for data fusion of a three axis magnetometer and three axis accelerometer
WO2015057074A1 (en) * 2013-10-18 2015-04-23 Nederlandse Organisatie Voor Toegepast-Natuurwetenschappelijk Onderzoek Tno Adjusted navigation
CN109556632A (zh) * 2018-11-26 2019-04-02 北方工业大学 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN110017837A (zh) * 2019-04-26 2019-07-16 沈阳航空航天大学 一种姿态抗磁干扰的组合导航方法
CN110470294A (zh) * 2019-08-09 2019-11-19 西安电子科技大学 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法
CN110793515A (zh) * 2018-08-02 2020-02-14 哈尔滨工业大学 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109813308A (zh) * 2019-03-07 2019-05-28 京东方科技集团股份有限公司 姿态估计方法、装置及计算机可读存储介质
CN111076722B (zh) * 2019-11-18 2022-07-19 广州南方卫星导航仪器有限公司 基于自适应的四元数的姿态估计方法及装置

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1690067A1 (en) * 2003-12-05 2006-08-16 Honeywell International Inc. System and method for using multiple aiding sensors in a deeply integrated navigation system
CA2769085A1 (en) * 2011-02-28 2012-08-28 Research In Motion Limited Portable electronic device adapted to compensate for gyroscope bias
EP2520903A1 (en) * 2011-02-28 2012-11-07 Research In Motion Limited Portable electronic device adapted to compensate for gyroscope bias
WO2014022664A2 (en) * 2012-08-02 2014-02-06 Memsic, Inc. Method and apparatus for data fusion of a three axis magnetometer and three axis accelerometer
WO2015057074A1 (en) * 2013-10-18 2015-04-23 Nederlandse Organisatie Voor Toegepast-Natuurwetenschappelijk Onderzoek Tno Adjusted navigation
CN110793515A (zh) * 2018-08-02 2020-02-14 哈尔滨工业大学 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法
CN109556632A (zh) * 2018-11-26 2019-04-02 北方工业大学 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN110017837A (zh) * 2019-04-26 2019-07-16 沈阳航空航天大学 一种姿态抗磁干扰的组合导航方法
CN110470294A (zh) * 2019-08-09 2019-11-19 西安电子科技大学 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
"Attitude estimation using MARG sensors for unmanned aerial vehicles";Tan, Lining等;《Journal of Computational Methods in Sciences and Engineering》;20181231;第18卷(第4期);905-916 *
"Fast Complementary Filter for Attitude Estimation Using Low-Cost MARG Sensors";Wu, Jin等;《IEEE SENSORS JOURNAL》;20160915;第16卷(第18期);6997-7007 *
"GPS/Doppler导航随机模型的移动窗口实时估计算法";周泽波等;《测绘学报》;20110430;第40卷(第2期);220-225 *
"基于 MRPs/NPUPF 的地磁/加速度计测量的姿态估计新方法";郭庆等;《宇航学报》;20110228;第32卷(第2期);905-916 *

Also Published As

Publication number Publication date
CN112683267A (zh) 2021-04-20

Similar Documents

Publication Publication Date Title
CN112683267B (zh) 一种附有gnss速度矢量辅助的车载姿态估计方法
CN111426318B (zh) 基于四元数-扩展卡尔曼滤波的低成本ahrs航向角补偿方法
CN108180925B (zh) 一种里程计辅助车载动态对准方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
CN112629538B (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN101726295B (zh) 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN110702104B (zh) 一种基于车辆零速检测的惯性导航误差修正方法
CN108398128B (zh) 一种姿态角的融合解算方法和装置
CN110017837B (zh) 一种姿态抗磁干扰的组合导航方法
CN103727941B (zh) 基于载体系速度匹配的容积卡尔曼非线性组合导航方法
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN112504275B (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN106052685A (zh) 一种两级分离融合的姿态和航向估计方法
CN112683269B (zh) 一种附有运动加速度补偿的marg姿态计算方法
CN109959374B (zh) 一种行人惯性导航全时全程逆向平滑滤波方法
Kong et al. Performance improvement of visual-inertial navigation system by using polarized light compass
CN109387198B (zh) 一种基于序贯检测的惯性/视觉里程计组合导航方法
CN106595669B (zh) 一种旋转体姿态解算方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN104101345A (zh) 基于互补重构技术的多传感器姿态融合方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant