[发明专利]基于蓝牙AOA和IMU融合的载体姿态零速校正系统

权利要求书

1.基于蓝牙AOA和IMU融合的载体姿态零速校正系统，其特征在于，包括IMU模块、IMU位置计算模块、蓝牙AOA定位模块、Kalman融合定位输出模块、载体静态检测模块、载体姿态校正模块；

所述IMU模块安装于载体目标上，用于采集载体运动过程中的三轴加速度数据与三轴角速度数据；

所述IMU位置计算模块与IMU模块连接，用于接收三轴加速度数据与三轴角速度数据，并进行积分运算，得到载体目标的姿态数据与定位估计值；

所述蓝牙AOA定位模块包括固定在载体目标上的发射节点和固定在空间已知位置上的两个基站接收节点，以及蓝牙AOA位置计算模块；所述发射节点用于向空间中发射蓝牙信号数据，所述基站接收节点用于采样所述发射节点发射出的蓝牙信号数据获得蓝牙信号采样数据，并将蓝牙信号采样数据发送到所述蓝牙AOA位置计算模块；所述蓝牙AOA位置计算模块对蓝牙信号采样数据进行运算处理，得到载体目标的定位测量值；

所述Kalman融合定位输出模块接收IMU位置计算模块输出的定位估计值和蓝牙AOA位置计算模块输出的定位测量值，通过卡尔曼滤波算法进行数据融合，得到载体目标的融合定位估计值，并将融合结果传递给IMU位置计算模块；

所述载体静态检测模块接收IMU模块的三轴加速度数据和三轴角速度数据，以及Kalman融合定位输出模块输出的融合定位估计值；通过对接收数据进行处理，用于确定载体目标的运动状态，并将运动状态数据传递给载体姿态校正模块；

所述载体姿态校正模块接收IMU模块的三轴加速度数据、载体静态检测模块输出的运动状态数据以及Kalman融合定位输出模块输出的融合定位估计值；通过对接收数据进行处理，计算出载体目标在零速状态下的姿态数据，并将计算结果传递给IMU位置计算模块。

2.根据权利要求1所述的基于蓝牙AOA和IMU融合的载体姿态零速校正系统，其特征在于，所述蓝牙AOA定位模块中的发射节点发射包含CTE在内的蓝牙信号，接收节点对发射节点发射的信号进行采样处理。

3.根据权利要求2所述的基于蓝牙AOA和IMU融合的载体姿态零速校正系统，其特征在于，所述接收节点对发射节点发射的信号进行采样处理的流程包括：接收节点首先利用阵列天线对蓝牙信号进行采集，将其转换为I/Q数据，再利用I/Q数据计算相邻阵列天线间的相位差，最后根据相位差数据，结合天线阵列与蓝牙信号传播方向间的几何关系，计算出蓝牙信号到达天线的角度数据。

4.根据权利要求1所述的基于蓝牙AOA和IMU融合的载体姿态零速校正系统，其特征在于，所述Kalman融合定位输出模块将IMU位置计算模块输出的定位估计值作为卡尔曼滤波的先验信息，蓝牙AOA位置计算模块输出的定位测量值作为卡尔曼滤波的观测信息，通过卡尔曼滤波融合计算，最终得到载体目标的融合定位估计值。

5.根据权利要求1所述的基于蓝牙AOA和IMU融合的载体姿态零速校正系统，其特征在于，所述载体静态检测模块将三轴加速度数据与三轴角速度数据进行滑动滤波，再将滤波结果与融合定位估计值一起进行阈值检测，通过检测结果确定载体目标的运动状态。

6.根据权利要求1所述的基于蓝牙AOA和IMU融合的载体姿态零速校正系统，其特征在于，所述载体姿态校正模块对接收数据进行处理，通过确定IMU的三轴旋转顺序与对应欧拉角，确定IMU的姿态矩阵表达式；在静止状态下，利用IMU三轴加速度测量值与姿态矩阵表达式，计算出载体的姿态参数；同时对载体零速时刻前n点融合定位估计值进行处理，确定载体的当前姿态偏航角，并与姿态参数一起完成载体目标的姿态更新。