[发明专利]面向三维空间的组合定位方法及其系统

权利要求书

1.一种面向三维空间的组合定位方法，其特征在于在惯性测量单元进行位置积分的过程中，

当定位基站获取定位信息后，执行基于卡尔曼滤波算法的预测环节和更新环节，具体步骤为：

使用误差量作为滤波器的状态量，令误差量为位置误差、速度误差、姿态误差、加速度计零值漂移及陀螺仪零值漂移，表示为：

则状态转移方程为

对误差方程求导得：

所述预测环节为：

在量测方程中，为了实现卡尔曼滤波算法的非线性处理，需要对量测方程进行一阶梯度近似，则量测误差量关于状态误差量的偏导数为

所述更新环节为：

K＝PH^T(HPH^T+V)^-1

P←(I-KH)P

其中，K为卡尔曼增益，V为量测噪声，P为量测噪声的协方差，H为测量系统的参数，Q为系统噪声的协方差，F为状态转移矩阵，不带前缀的量代表实际量，其下缀里面的“k”或“t”代表该量是第k或第t时刻的实际量；带前缀“^”的量代表预测量；带前缀“～”的量代表与预测量对应的偏差值；

当收到所述定位基站的数据时，将所述惯性测量单元输出的位置信息和所述定位基站提供的位置信息进行融合，利用所述更新环节提供的概率最优模型对所述惯性测量单元的当前位置进行矫正。

2.如权利要求1所述的组合定位方法，其特征在于在使用卡尔曼滤波算法的过程中，对状态量进行一阶近似。

3.如权利要求1所述的组合定位方法，其特征在于：

对所述惯性测量单元相对虚拟坐标系的姿态进行参数标定，实现外参估计。

4.如权利要求3所述的组合定位方法，其特征在于通过如下步骤实现外参估计：

在定位基站被放置并保持静止的情况下，在虚拟坐标系上三个轴上分别安装加速度计，用于感知重力在三个轴方向的分解力大小；

根据所述重力的分解情况，获得静止状态下虚拟坐标系相对大地坐标系之间的相对姿态。

5.如权利要求1所述的组合定位方法，其特征在于：

所述定位基站从多个视角对平面标定板进行投影成像，获取所述定位基站相对于所述平面标定板的姿态矩阵；

解算包括姿态参数的方程组，得到内参模型和外参模型的所有参数值。

6.一种面向三维空间的组合定位系统，包括惯性测量单元和三维空间定位设备；所述三维空间定位设备中包括定位基站，所述定位基站中的两个激光发射源发射激光束分别照射到一字镜上，形成两个垂直的激光平面；电机转动带动所述一字镜进行匀速旋转运动，在三维空间内形成不断旋转的激光平面；其特征在于：

在惯性测量单元进行位置积分的过程中，当收到定位基站的数据时，将所述惯性测量单元输出的位置信息和所述定位基站提供的位置信息进行融合，利用权利要求1～5中任意一项所提供的概率最优模型对所述惯性测量单元的当前位置进行矫正。

7.如权利要求6所述的组合定位系统，其特征在于：

所述概率最优模型是基于卡尔曼滤波算法的预测环节和更新环节得到的。