[发明专利]一种智能导航方法、装置和设备

权利要求书

1.一种智能导航方法，其特征在于，所述方法包括：

S1：采集环境图像并获取所述环境图像中的原始视觉特征；

S2：将无线测量获取的距离角度信息作为参考信息对所述原始视觉特征进行筛选得到目标视觉特征；

S3：基于IMU测量利用所述目标视觉特征初始化环境中无线信号源的位置以及无线测量的偏置；

S4：利用无线测量的测量结果、IMU测量的测量结果和所述目标视觉特征估计运动状态信息，所述运动状态信息包括：状态变量、视觉特征点的深度和无线测量的偏置，所述状态变量包括：三维位置、三维速度和三维姿态；

S5：根据所述运动状态信息和环境中的视觉特征对应的三维信息进行导航；

所述S2包括：

S201：测量与信号源之间的距离和角度；

S202：获取搭载的IMU测量的加速度和角速度；

S203：利用IMU预积分技术将IMU测量数据与无线测量对齐；

S204：将无线测量的距离通过余弦定理进行降噪处理，以抵消无线测量的偏置：

S205：采用卡尔曼滤波将无线测量的距离和角度与IMU的短时积分进行融合，以获得相邻无线测量信息之间的相对位移与旋转；

S206：基于相邻无线测量信息之间的相对位移与旋转，将第k帧图像中的视觉特征通过对极几何重投影到第k+1帧，以形成一组无线匹配的特征点，每个特征点都与光学匹配的特征点相关联；最后，根据无线匹配和光学匹配特征点之间的距离进行排序，排序最高的特征点对应最小距离；

S207：将原始视觉特征点按特征距离的升序排列，每个原始视觉特征点映射一个归一化距离对应的分数，定义阈值∈并选择分数小于∈的原始视觉特征点作为目标视觉特征点参与后续的状态估计；

所述S3包括：

S301：初始化运动状态并通过无线测距确定无线信号源的位置；初始化操作仅在执行导航任务前执行一次；初始化向量定义为：

其中，和为第j个关键帧时的位置和速度，是初始时刻相机参考系的重力，s是单目视觉测量的尺度信息，n是一组数据中关键帧的数量，n为大于等于4的正整数；

S302：初始化后将利用更新的传感器测量信息估计和更新MAV的状态，运行过程中无线信号源对应的无线信道会随着环境变化，导致无线测量的距离偏置发生变化，以此来修正距离测量。

2.如权利要求1所述的智能导航方法，其特征在于，所述步骤S203包括：利用IMU预积分技术将IMU测量数据与无线测量对齐；

IMU预积分技术为：

其中，表示智能导航设备在t时刻的加速度测量值，表示智能导航设备在t时刻的角速度测量值，表示四元数乘法操作符，(·)ⁱ表示IMU参考系下的测量，是当接收到第(k+1)个无线测量时相对于收到第k个无线测量时的IMU参考系，表示设备直接测量的量或者可以由测量的量直接估计出的量。

3.如权利要求1所述的智能导航方法，其特征在于，所述步骤S301包括：

单目视觉运动恢复结构算法SfM提供像平面坐标的无尺度位置以及相机旋转其中，将初始相机参考系设为SfM的参考系；表示第j个关键帧时相对于初始相机参考系的相机位姿；

通过手动测量IMU和相机之间的外部参数以将位姿在相机和IMU参考系之间进行转换；二者关系为：且是的旋转矩阵；

将相对位移和旋转替换为基于单目视觉的无尺度位姿，根据运动获取测量模型如下：是加性高斯噪声；求解下列最小二乘问题得到X₀：F为参与计算的数据组中所有的帧图像；

获取一组位置数据并通过解下列二次规划问题(QP)问题来计算无线节点的位置：

表示第j帧图像时设备与第l个无线信号源的距离；是无线测量参考系到IMU参考系之间的相对位移。

4.如权利要求3所述的智能导航方法，其特征在于，所述步骤S4包括：

基于图优化的滑动窗口估计器建立三种传感器的测量模型，通过最小化三种传感器的测量残差；通过初始化设备状态以及无线信号源位置驱动优化问题的求解，实时计算得出所述运动状态信息，所述运动状态信息还包括位置、速度、姿态、无线测量的偏置以及视觉特征点的深度。