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

一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法
技术领域
1.本发明涉及无人驾驶、gps/bds定位导航、imu惯性测量单元、嵌入式设备等相关领域，具体涉及一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法。


背景技术：

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


技术实现要素：

7.针对上述的技术问题，本技术方案提供了一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法,采用了gps/bds双模式定位模块和imu惯性测量单元，定位时可见卫星数、定位的可靠性和精度有了很大提高，可有效的解决上述问题。
8.本发明通过以下技术方案实现：
9.一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法，收集无人配送车上的gps/bds接收机和惯性测量单元中的数据，将惯性测量单元搜集到的数据用sins解算出位置、速度、姿态角、姿态转移矩阵等信息以及gps/bds输出的位置、速度信息同时输入到ckf滤波器中；具体实施步骤如下：
10.步骤1：收集无人配送车上的gps/bds接收机和惯性测量单元中的数据，将imu搜集到的数据用sins解算出位置、速度、姿态的信息；
11.真实地理位置l,λ,h，真实的速度记作sins解算出来的位置信息ls,λs,hs,速度信息v
es
,v
ns
,v
us
，姿态角φ
es
,φ
ns
,φ
us
；gps/bds测量出的位置信息l
gb
,λ
gb
,h
gb
，速度信息v
egb
、v
ngb
、v
ugb
；
12.步骤2：建立sins和gps/dbs紧组合的系统状态误差方程；
13.步骤3：建立sins和gps/bds量测方程；
14.步骤4：将步骤2和步骤3所构建的状态误差方程和量测方程带入ckf算法，得出系统状态的最优估计值；
15.步骤5：设置阈值判断gps/bds有无数据信号；
16.当有信号时将测量出的定位结果对sins解算中存在的累计误差进行矫正，当gps/bds没有信号时，则用sins解算出来的定位结果对gps/bds进行补偿修正；
17.取e＝|z
k-x
k|k-1
|，其鉴别范围为：
18.其中zk为实际观测值，x
k|k-1
为状态预测值，th为误差阈值，将e与所设阈值进行比较；
19.步骤6：对ckf算法得出sins与gps/bds输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度信息，位置信息以及捷联姿态矩阵，以得到精确的导航信息。
20.进一步的，步骤2所述的建立sins和gps/dbs紧组合的系统状态误差方程，其具体操作方式如下：
21.步骤2.1：建立sins的状态误差方程；
22.记：姿态角误差为：φ＝[φ
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的状态方程为：xs(t)＝fs(t)xs(t)+gs(t)ws(t)；状态量取为：
[0024][0025]ws
(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系下三轴加速度的随机
误差为零均值白噪声过程，fb为加速度计的实际输出，分别为解算出的旋转角速度、地球旋转角速度和解算出的数学平台相对于地球的旋转角速度；rn、re分别为当地子午圈、卯酉圈曲率半径；
[0029]
步骤2.2：建立gps/bds状态误差方程；
[0030]
其状态误差方程可表示为：x
gb
(t)＝f
gb
(t)x
gb
(t)+g
gb
(t)w
gb
(t)；
[0031]
式中，
[0032]
δt
gps
、δt
bds
、δt
ru
分别为gps钟差、北斗钟差、等效距离误差；w
tu
、w
tru
为白噪声；
[0033]
步骤2.3：组合步骤2.1和步骤2.2得到的sins和gps/bds的状态误差方程，得到最终的系统状态误差方程：
[0034][0035]
即，
[0036]
进一步的，步骤3所述的建立sins和gps/bds量测方程，其具体操作方式如下：
[0037]
首先分别得到sins和gps/bds所对应的第i个卫星的伪距、伪距率的量测方程，然后进行组合得到最终的量测方程；
[0038]
步骤3.1：建立sins和gps/bds伪距量测方程：
[0039]
设：sins输出的位置信息为(xs,ys,zs),gps/bds的位置坐标为：(x
gb
,y
gb
,z
gb
)；第i颗卫星的位置为(xi,yi,zi)，通过伪距量测方程和泰勒展开式分析得出：sins解算位置位于第i个卫星的伪距通式为ρ
si
＝ri+e
i1
δx+e
i2
δy+e
i3
δz；
[0040]
同理，gps/bds的伪距通式为ρ
gb
＝r
i-δt
bds-δt
gps-v
ρ
；
[0041]
其中v
ρ
为量测噪声；
[0042]
由上述两式可以得到最终的伪距量测方程：z
ρs
(t)＝h
ρs
(t)x(t)+v
ρs
(t)；
[0043]
步骤3.2：建立sins和gps/bds伪距率量测方程：
[0044]
伪距率指的是，sins和gps/bds分别结算出来的位置与真实位置(x,y,z)与第i颗卫星之间距离的变化率，其公式分别为：
[0045][0046][0047]
则：伪距率量测方程为：
[0048]
步骤3.3：建立sins和gps/bds伪距、伪距率组合量测方程：
[0049]
将步骤3.1和3.2中的伪距量测方程和伪距率量测方程组合起来得到整体的量测方程；
[0050][0051]
即，z
ρ
(t)＝h
ρ
(t)x(t)+v
ρ
(t)。
[0052]
进一步的，步骤4所述的将步骤2和步骤3所构建的状态误差方程和量测方程带入ckf算法，得出系统状态的最优估计值，其具体操作方式如下：
[0053]
将步骤2.3和步骤3.3所构建的状态误差方程和量测方程带入ckf算法，得出系统状态的最优估计值；
[0054]
ckf滤波器中的系统状态误差方程和量测方程
[0055]
式中xk为17维状态向量,且各分量独立,服从高斯分布；zk为5维观测向量；ω
k-1
和vk分别为过程噪声和量测噪声，均值为0。
[0056]
进一步的，步骤5所述的设置阈值(th)判断gps/bds有无数据信号,其具体实现方法为：
[0057]
对得出的sins解算出来的位置信息ls,λs,hs,速度信息v
es
,v
ns
,v
us
，姿态φ
es
,φ
ns
,φ
us
对其矫正后为：
[0058][0059]
当gps/bds没有信号时，将sins矫正过的位置、速度信息对gps/bds的状态值进行修正；
[0060]
当gps/bds接收机有信号时，将测量出的位置信息l
gb
,λ
gb
,h
gb
,速度信息v
egb
、v
ngb
、v
ugb
对其矫正后为：
[0061][0062]
将矫正后的信息对sins进行修正，消除累计误差。
[0063]
有益效果
[0064]
本发明提出的一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位的方法，与现有技术相比较，其具有以下有益效果：
[0065]
(1)本技术方案采用了gps/bds双模式定位模块，定位时可见卫星数、定位的可靠性和精度有了很大提高。ckf滤波对处理非线性系统很友好，它采用的是三阶球面径向容积法则。可以有效地解决非线性系统的状态估计问题和发散问题以及保证数值的稳定性。设置阈值(th)判断gps/bds有无数据信号，当有信号时将测量出的定位结果对sins解算中存在的累计误差进行矫正，当gps/bds没有信号时，则用sins解算出来的定位结果对gps/bds进行补偿进一步提高系统模型的稳定性，解算出最优导航定位信息。
[0066]
(2)本技术方案采用了容积卡尔曼滤波(ckf)，对处理非线性系统很友好，可以有效地解决非线性系统的状态估计问题和发散问题以及保证数值的稳定性。
[0067]
(3)本技术方案设置阈值(th)判断gps/bds有无数据信号，当有信号时将测量出的定位结果对sins解算中存在的累计误差进行矫正，当gps/bds没有信号时，则用sins解算出来的定位结果对gps/bds进行补偿。
附图说明
[0068]
图1是本发明的整体方法流程示意框图。
[0069]
图2是本发明中方法的位置误差对比图。
[0070]
图3是本发明中方法的速度误差对比图。
具体实施方式
[0071]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。在不脱离本发明设计构思的前提下，本领域普通人员对本发明的技术方案做出的各种变型和改进，均应落入到本发明的保护范围。
[0072]
实施例1：
[0073]
如图1所示，一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法，收集无人配送车上的gps/bds接收机和惯性测量单元中的数据，将惯性测量单元搜集到的数据用sins解算出位置、速度、姿态角、姿态转移矩阵等信息以及gps/bds输出的位置、速度信息同时输入到ckf滤波器中；具体实施步骤如下：
[0074]
步骤1：收集无人配送车上的gps/bds接收机和惯性测量单元中的数据，将imu搜集到的数据用sins解算出位置、速度、姿态的信息；
[0075]
选取“东北天”地理坐标系作为导航坐标系n系，载体坐标系为b系，sins解算出来的数学平台记为n'系，三个欧拉角记作航向角ψ、俯仰角θ、横滚角γ，真实地理位置l,λ,h，真实的速度记作n系与b系之间的姿态矩阵记作n系与n'系之间的姿态矩阵记作b系与n'系之间的姿态矩阵记作
[0076]
sins解算出来的位置信息ls,λs,hs,速度信息v
es
,v
ns
,v
us
，姿态角φ
es
,φ
ns
,φ
us
；gps/bds测量出的位置信息l
gb
,λ
gb
,h
gb
，速度信息v
egb
、v
ngb
、v
ugb
。
[0077]
步骤2：建立sins和gps/dbs紧组合的系统状态误差方程；其具体操作方式如下：
[0078]
步骤2.1：建立sins的状态误差方程；
[0079]
记：姿态角误差为：φ＝[φ
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
为载体坐标系下加速度计的噪声；
[0080]
设：sins的状态方程为：xs(t)＝fs(t)xs(t)+gs(t)ws(t)；状态量取为：
[0081][0082]ws
(t)＝[w
gx
,w
gy
,w
gz
,w
ax
,w
ay
,w
az
]
t
；
[0083]
则：非线性误差模型如下：
[0084][0085]
其中，εb＝[b
gx b
gy b
gz
]为b系下三轴陀螺的常值误差，为b系下三轴陀螺的随机误差为零均值白噪声过程，为b系下三轴加速度常值误差，为n系下三轴加速度的随机误差为零均值白噪声过程，fb为加速度计的实际输出，分别为解算出的旋转角速度、地球旋转角速度和解算出的数学平台相对于地球的旋转角速度；rn、re分别为当地子午圈、卯酉圈曲率半径；
[0086]
步骤2.2：建立gps/bds状态误差方程；
[0087]
其状态误差方程可表示为：x
gb
(t)＝f
gb
(t)x
gb
(t)+g
gb
(t)w
gb
(t)；
[0088]
式中，
[0089]
δt
gps
、δt
bds
、δt
ru
分别为gps钟差、北斗钟差、等效距离误差；w
tu
、w
tru
为白噪声；
[0090]
步骤2.3：组合步骤2.1和步骤2.2得到的sins和gps/bds的状态误差方程，得到最终的系统状态误差方程：
[0091][0092]
即，
[0093]
步骤3：建立sins和gps/bds观测方程，其具体操作方式如下：
[0094]
首先分别得到sins和gps/bds所对应的第i个卫星的伪距、伪距率的测量方程，然后进行组合得到最终的量测方程；
[0095]
步骤3.1：建立sins和gps/bds伪距量测方程：
[0096]
设：sins输出的位置信息为(xs,ys,zs),gps/bds的位置坐标为：(x
gb
,y
gb
,z
gb
)；第i颗卫星的位置为(xi,yi,zi)，通过伪距量测方程和泰勒展开式分析得出：sins解算位置位于第i个卫星的伪距通式为ρ
si
＝ri+e
i1
δx+e
i2
δy+e
i3
δz；
[0097]
同理，gps/bds的伪距通式为ρ
gb
＝r
i-δt
bds-δt
gps-v
ρ
；
[0098]
其中v
ρ
为量测噪声；
[0099]
由上述两式可以得到最终的伪距量测方程：z
ρs
(t)＝h
ρs
(t)x(t)+v
ρs
(t)；
[0100]
步骤3.2：建立sins和gps/bds伪距率量测方程：
[0101]
伪距率指的是，sins和gps/bds分别结算出来的位置与真实位置(x,y,z)与第i颗卫星之间距离的变化率，其公式分别为：
[0102][0103][0104]
则：伪距率量测方程为：
[0105]
步骤3.3：建立sins和gps/bds伪距、伪距率组合量测方程：
[0106]
将步骤3.1和3.2中的伪距量测方程和伪距率量测方程组合起来得到整体的量测方程；
[0107][0108]
即，z
ρ
(t)＝h
ρ
(t)x(t)+v
ρ
(t)。
[0109]
步骤4：将步骤2和步骤3所构建的状态误差方程和量测方程带入ckf算法，得出系统状态的最优估计值；
[0110]
将步骤2.3和步骤3.3所构建的状态误差方程和量测方程带入ckf算法，得出系统状态的最优估计值；
[0111]
ckf滤波器中的系统状态误差方程和量测方程
[0112]
式中xk为17维状态向量,且各分量独立,服从高斯分布；zk为5维观测向量；ω
k-1
和vk分别为过程噪声和量测噪声，均值为0。
[0113]
步骤5：设置阈值判断gps/bds有无数据信号；
[0114]
当有信号时将测量出的定位结果对sins解算中存在的累计误差进行矫正，当gps/bds没有信号时，则用sins解算出来的定位结果对gps/bds进行补偿修正；
[0115]
取e＝|z
k-x
k|k-1
|，其鉴别范围为：
[0116]
其中zk为实际观测值，x
k|k-1
为状态预测值，th为误差阈值，将e与所设阈值进行比较；
[0117]
其具体实现方法为：
[0118]
对得出的sins解算出来的位置信息ls,λs,hs,速度信息v
es
,v
ns
,v
us
，姿态φ
es
,φ
ns
,φ
us
对其矫正后为：
[0119][0120]
当gps/bds没有信号时，将sins矫正过的位置、速度信息对gps/bds的状态值进行修正；
[0121]
当gps/bds接收机有信号时，将测量出的位置信息l
gb
,λ
gb
,h
gb
,速度信息v
egb
、v
ngb
、v
ugb
对其矫正后为：
[0122][0123]
将矫正后的信息对sins进行修正，消除累计误差。
[0124]
步骤6：对ckf算法得出sins与gps/bds输出的信息进行滤波、融合，得到导航参数的最优估计并修正速度信息，位置信息以及捷联姿态矩阵，以得到精确的导航信息。