一种基于CKF算法的GPS/BDS和SINS融合的无人配送车导航定位方法技术

本发明专利技术公开了一种基于CKF算法的GPS/BDS和SINS融合的无人配送车导航定位方法，先收集无人配送车上的GPS/BDS接收机和惯性测量单元中的数据，将IMU搜集到的数据用SINS解算出位置、速度、姿态的信息；再建立SINS和GPS/DBS紧组合的系统误差状态方程；然后建立SINS和GPS/BDS量测方程；将构建的系统状态误差方程和SINS和GPS/BDS量测方程带入CKF算法，得出系统状态的最优估计值；设置阈值判断GPS/BDS有无数据信号，当有信号时将测量出的定位结果对SINS解算中存在的累计误差进行矫正，当没有信号时，则用SINS解算出来的定位结果对GPS/BDS进行补偿修正；最后对CKF算法得出SINS与GPS/BDS输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度信息，位置信息以及捷联姿态矩阵，以得到精确的导航信息。以得到精确的导航信息。以得到精确的导航信息。

【技术实现步骤摘要】
一种基于CKF算法的GPS/BDS和SINS融合的无人配送车导航定位方法


[0001]本专利技术涉及无人驾驶、GPS/BDS定位导航、IMU惯性测量单元、嵌入式设备等相关领域，具体涉及一种基于CKF算法的GPS/BDS和SINS融合的无人配送车导航定位方法。

技术介绍

[0002]专利号为CN1034968843A的中国专利公开了一种GPS/SINS超紧组合导航系统自适应混合滤波方法。该方法涉及一种GPS/SINS超紧组合导航系统自适应混合滤波方法,该方法将SINS数据和相应的GPS数据组合在一起，采用自适应卡尔曼滤波方法建立GPS/SINS组合导航模型并得出得到系统状态的最优估计值和估计误差方阵，利用有效估计值对SINS输出的导航定位信息进行输出矫正，使用所得到的估计误差方阵，分析出系统的各个状态的可观测度信息，并将它作为反馈因子与系统获得的最优估计值的乘积作为反馈量，对GPS和SINS的参数进行反馈校正。
[0003]该方法使用传统的卡尔曼滤波算法存在很大的局限性，在非线性的场景中并不能达到最优的估计效果，且GPS易受随机误差的干扰，不能提供稳定精确的定位信号，与SINS数据融合定位，数据维度增加传统的卡尔曼滤波器很难达到最优解，从而不能得到精确的导航定位。其主要缺点：
[0004](1)使用GPS单个定位系统所提供的可见卫星数量少，定位精度不高，卫星信号几何结构差，不能提供很好的实时定位导航。
[0005](2)自适应卡尔曼滤波法在状态量的维数过多时，存在发散问题，导致所求出来的最优估计值达不到最优，从而导致系统模型不够准确。
[0006](3)GPS抗干扰能力弱，当收到外部环境，特别时没有视距的情况，GPS数据会出现无信号的情况，不能提供导航定位信息，在SINS解算比较依赖前一时刻的数据值，进而出现IMU计算出累计误差，这很难消除。

技术实现思路

[0007]针对上述的技术问题，本技术方案提供了一种基于CKF算法的GPS/BDS和SINS融合的无人配送车导航定位方法,采用了GPS/BDS双模式定位模块和IMU惯性测量单元，定位时可见卫星数、定位的可靠性和精度有了很大提高，可有效的解决上述问题。
[0008]本专利技术通过以下技术方案实现：
[0009]一种基于CKF算法的GPS/BDS和SINS融合的无人配送车导航定位方法，收集无人配送车上的GPS/BDS接收机和惯性测量单元中的数据，将惯性测量单元搜集到的数据用SINS解算出位置、速度、姿态角、姿态转移矩阵等信息以及GPS/BDS输出的位置、速度信息同时输入到CKF滤波器中；具体实施步骤如下：
[0010]步骤1：收集无人配送车上的GPS/BDS接收机和惯性测量单元中的数据，将IMU搜集到的数据用SINS解算出位置、速度、姿态的信息；
[0011]真实地理位置L,λ,H，真实的速度记作SINS解算出来的位置信息L
S
,λ
S
,H
S
,速度信息v
es
,v
ns
,v
us
，姿态角φ
es
,φ
ns
,φ
us
；GPS/BDS测量出的位置信息L
GB
,λ
GB
,H
GB
，速度信息v
eGB
、v
nGB
、v
uGB
；
[0012]步骤2：建立SINS和GPS/DBS紧组合的系统状态误差方程；
[0013]步骤3：建立SINS和GPS/BDS量测方程；
[0014]步骤4：将步骤2和步骤3所构建的状态误差方程和量测方程带入CKF算法，得出系统状态的最优估计值；
[0015]步骤5：设置阈值判断GPS/BDS有无数据信号；
[0016]当有信号时将测量出的定位结果对SINS解算中存在的累计误差进行矫正，当GPS/BDS没有信号时，则用SINS解算出来的定位结果对GPS/BDS进行补偿修正；
[0017]取e＝|z
k
‑
x
k|k
‑1|，其鉴别范围为：
[0018]其中z
k
为实际观测值，x
k|k
‑1为状态预测值，TH为误差阈值，将e与所设阈值进行比较；
[0019]步骤6：对CKF算法得出SINS与GPS/BDS输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度信息，位置信息以及捷联姿态矩阵，以得到精确的导航信息。
[0020]进一步的，步骤2所述的建立SINS和GPS/DBS紧组合的系统状态误差方程，其具体操作方式如下：
[0021]步骤2.1：建立SINS的状态误差方程；
[0022]记：姿态角误差为：φ＝[φ
u φ
e φ
n
]T
；速度误差为：位置误差为：δP＝[δL δλ δH]T
；b
gx
、b
gy
、b
gz
为三轴陀螺的常值误差；b
ax
、b
ay
、b
az
三轴加速度的随机误差；w
gx
、w
gy
、w
gz
为载体坐标系下陀螺的噪声；w
ax
、w
ay
、w
az
为载体坐标系下加速度计的噪声；
[0023]设：SINS的状态方程为：X
s
(t)＝F
s
(t)X
s
(t)+G
s
(t)W
s
(t)；状态量取为：
[0024][0025]W
s
(t)＝[w
gx
,w
gy
,w
gz
,w
ax
,w
ay
,w
az
]T
；
[0026]则：非线性误差模型如下：
[0027][0028]其中，ε
b
＝[b
gx b
gy b
gz
]为b系下三轴陀螺的常值误差，为b系下三轴陀螺的随机误差为零均值白噪声过程，为b系下三轴加速度常值误差，为n系下三轴加速度的随机
误差为零均值白噪声过程，f
b
为加速度计的实际输出，分别为解算出的旋转角速度、地球旋转角速度和解算出的数学平台相对于地球的旋转角速度；R
N
、R
E
分别为当地子午圈、卯酉圈曲率半径；
[0029]步骤2.2：建立GPS/BDS状态误差方程；
[0030]其状态误差方程可表示为：X
GB
(t)＝F
GB
(t)X
GB
(t)+G
GB...

【技术保护点】

【技术特征摘要】
1.一种基于CKF算法的GPS/BDS和SINS融合的无人配送车导航定位方法，收集无人配送车上的GPS/BDS接收机和惯性测量单元中的数据，将惯性测量单元搜集到的数据用SINS解算出位置、速度、姿态角、姿态转移矩阵等信息以及GPS/BDS输出的位置、速度信息同时输入到CKF滤波器中；具体实施步骤如下：步骤1：收集无人配送车上的GPS/BDS接收机和惯性测量单元中的数据，将IMU搜集到的数据用SINS解算出位置、速度、姿态的信息；真实地理位置L,λ,H，真实的速度记作SINS解算出来的位置信息L
S
,λ
S
,H
S
,速度信息v
es
,v
ns
,v
us
，姿态角φ
es
,φ
ns
,φ
us
；GPS/BDS测量出的位置信息L
GB
,λ
GB
,H
GB
，速度信息v
eGB
、v
nGB
、v
uGB
；步骤2：建立SINS和GPS/DBS紧组合的系统状态误差方程；步骤3：建立SINS和GPS/BDS量测方程；步骤4：将步骤2和步骤3所构建的状态误差方程和量测方程带入CKF算法，得出系统状态的最优估计值；步骤5：设置阈值判断GPS/BDS有无数据信号；当有信号时将测量出的定位结果对SINS解算中存在的累计误差进行矫正，当GPS/BDS没有信号时，则用SINS解算出来的定位结果对GPS/BDS进行补偿修正；取e＝|z
k
‑
x
k|k
‑1|，其鉴别范围为：其中z
k
为实际观测值，x
k|k
‑1为状态预测值，TH为误差阈值，将e与所设阈值进行比较；步骤6：对CKF算法得出SINS与GPS/BDS输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度信息，位置信息以及捷联姿态矩阵，以得到精确的导航信息。2.根据权利要求1所述的一种基于CKF算法的GPS/BDS和SINS融合的无人配送车导航定位方法，其特征在于：步骤2所述的建立SINS和GPS/DBS紧组合的系统状态误差方程，其具体操作方式如下：步骤2.1：建立SINS的状态误差方程；记：姿态角误差为：φ＝[φ
u φ
e φ
n
]
T
；速度误差为：位置误差为：δP＝[δL δλ δH]
T
；b
gx
、b
gy
、b
gz
为三轴陀螺的常值误差；b
ax
、b
ay
、b
az
三轴加速度的随机误差；w
gx
、w
gy
、w
gz
为载体坐标系下陀螺的噪声；w
ax
、w
ay
、w
az
为载体坐标系下加速度计的噪声；设：SINS的状态方程为：X
s
(t)＝F
s
(t)X
s
(t)+G
s
(t)W
s
(t)；状态量取为：W
s
(t)＝[w
gx
,w
gy
,w
gz
,w
ax
,w
ay
,w
az
]
T
；则：非线性误差模型如下：
其中，ε
b
＝[b
gx b
gy b
gz
]为b系下三轴陀螺的常值误差，为b系下三轴陀螺的随机误差为零均值白噪声过程，
▽
b
为b系下三轴加速度常值误差，为n系下三轴加速度的随机误差为零均值白噪声过程，f
b
为加速度计的实际输出，分别为解算出的旋转角速度、地球旋转角速度和解算出的数学平台相对于地球的旋转角速度；R
N
、R
E
分别为当地子午圈、卯酉圈曲率半径；步骤2.2：建立GPS/BDS状态误差方程；其状态误差方程可表示为：X
GB
(t)＝F

【专利技术属性】
技术研发人员：张青春，周玲，吴峥，王刚，孟凡东，文张源，张洪源，
申请(专利权)人：淮阴工学院，
类型：发明
国别省市：