[发明专利]一种并发建图定位与惯性导航的组合误差融合系统

权利要求书

1.一种并发建图定位与惯性导航的组合误差融合系统，其特征在于，包括运载体、惯性导航器件、并发建图定位装置和误差融合装置；所述惯性导航器件包括加速度计、陀螺仪和惯性导航驱动装置，所述并发建图定位装置包括激光雷达和环境地图构建装置；所述误差融合装置包括线性离散并发建图定位误差模块、惯性导航误差模块和加权融合模块；

在所述惯性导航器件中，所述加速度计获得运载体加速度，所述陀螺仪获得运载体角速度，并将加速度与角速度信号通过通讯连接传送至惯性导航误差模块和惯性导航驱动装置；所述惯性导航误差模块进行误差分析；所述惯性导航驱动装置处理得到位置、速度和角度控制参数，调整运载体姿态；

在所述并发建图定位装置中，所述激光雷达获得运载体与环境间距离和方位角，并将距离和方位角通过通讯连接，传送至线性离散并发建图定位误差模块和环境地图构件装置，分别进行误差分析和地图构建；

在所述误差融合装置中，所述线性离散并发建图定位误差模块和所述惯性导航误差模块分别将误差通过通讯连接传送至所述加权融合模块，所述加权融合模块分析获得姿态误差和地图误差，姿态误差通过通讯连接反馈至惯性导航驱动装置校正位置、速度和角度控制参数，地图误差通过通讯连接传送至环境地图构建装置校正环境地图。

2.根据权利要求1所述的并发建图定位与惯性导航的组合误差融合系统，其特征在于，在线性离散并发建图定位误差模块中，其采用的状态方程：

δx(k+1)＝F(k)δx(k)+G(k)w(k)，

δx(k)表示误差状态向量，运载体与地图误差状态表示为：

δx(k)＝[δx_v(k),δx_m(k)]^T，

运载体误差状态δx_v(k)在导航坐标系中表示为：

δx_v(k)＝[δpⁿ(k),δvⁿ(k),δψⁿ(k)]^T，

地图误差状态δx_m(k)在导航坐标系中表示为：

δpⁿ为位置误差，δVⁿ为速度误差，δψⁿ为姿态误差，F(k)表示系统转移矩阵，G(k)表示系统噪声输入矩阵，w(k)表示系统误差向量，代表器件本身误差，误差强度为Q(k)。

3.根据权利要求2所述的并发建图定位与惯性导航的组合误差融合系统，其特征在于，所述惯性导航误差模块中，所述惯性导航和静态环境模型为：

δpⁿ为位置误差，δVⁿ为速度误差，δψⁿ为姿态误差，f^b和ω^b分别为惯性导航器件测量所得的加速度和角速度，δf^b和δω^b表示惯性导航器件的测量误差，

由四元数组成的方向余弦矩阵进行表示，方向余弦矩阵表示为：

σ_δf和σ_δω分别表示加速度和角速度的噪声强度。