[发明专利]一种用于GNSS拒止环境的无人机全局定位方法

权利要求书

1.一种用于GNSS拒止环境的无人机全局定位方法，其特征在于，包括以下步骤：

(1)、首先进行初始化，输入无人机起飞经纬度(lon_takeoff，lat_takeoff)、目标点的WGS84坐标，然后根据磁力计输出的磁场矢量计算航向角，获取稳定的航向角yaw_mag；

|yaw_current-yaw_last|≤θ (1)

其中，yaw_current为当前循环计算出的航向角，yaw_last为上一个循环计算出的航向角，θ为单位为弧度制的阈值，循环计算，直到满足公式(1)的条件，获得磁力计航向角yaw_mag＝yaw_current；

然后，无人机起飞；

(2)、基于机载摄像头视觉信息计算无人机局部位置、姿态角

2.1)、首先，机载摄像头采集当前图像数据，通过FAST(Features from AcceleratedSegment Test)角点检测算法获得图像关键点，基于BRIEF(Binary RobustIndependentElementary Features)描述子对图像关键点进行描述，得到特征点；

2.2)、然后，计算上一帧图像与当前帧图像的特征点匹配关系，进而求得相机坐标系下第k帧与第k+1帧之间的旋转矩阵平移向量

构建基础矩阵E：

点点为特征点匹配点对i，i＝1，2，...，8的两个匹配特征点，点位于第k帧、点位于第k+1帧，分别表示为其中，为第k帧上点的像素横坐标、像素纵坐标，为第k+1帧上点的像素横坐标、像素纵坐标；其在归一化平面上的坐标分别为：

基于八点法，求解以下公式：

得到基础矩阵E；

基于奇异值分解，通过基础矩阵E求得第k帧与第k+1帧之间的旋转矩阵平移向量

2.3)、然后，根据旋转矩阵平移向量计算所有特征点的深度即求解以下公式：

得到第k帧点第k+1帧点的深度N为特征点匹配点对的数量；

2.4)、最后，计算无人机旋转矩阵平移向量

对于k+1帧点根据以下公式得到世界坐标系下的非齐次坐标

其中：

式中，为第0帧到第k+1帧的旋转矩阵，为第0帧到第k+1帧的平移向量，则点在世界坐标系下的齐次坐标

定义从相机坐标系到世界坐标系的旋转矩阵

平移向量

基于奇异值分解，通过以下公式求解旋转矩阵平移向量

其中，平移向量表示基于机载摄像头视觉信息计算出的无人机局部位置，旋转矩阵为基于机载摄像头视觉信息计算出的无人机局部姿态角；

(3)、基于惯性测量单元的加速度、角速度计算无人机局部位置、姿态角

3.1)、通过惯性测量单元积分来计算第k+1帧的位置、速度：

其中，为世界坐标系下第k+1帧的位置，为世界坐标系下第k帧的位置，为世界坐标系下第k帧的速度，为世界坐标系下第k+1帧的速度，Δt_k为第k帧与第k+1帧之间的时间间隔，为惯性测量单元在世界坐标系下的旋转矩阵，为惯性测量单元加速度计输出的加速度信息，g^w＝[0，0，g]^T为重力加速度；

3.2)、计算无人机四元数：

其中，为世界坐标系下第k+1帧的四元数，为世界坐标系下第k帧的四元数，为惯性测量单元输出的角速度信息，为的反对称矩阵；

其中，表示无人机局部位置，表示局部姿态角；

(4)、融合基于机载摄像头视觉信息与惯性测量单元的加速度、角速度计算的位置、姿态角

4.1)、首先将基于机载摄像头视觉信息得到平移向量与基于惯性测量单元得到的平移向量融合，得到融合的局部平移向量p_local：

其中，α₁、β₁为权重标量，融合的局部平移向量p_local表示融合后的位置；

4.2)、然后将旋转矩阵转换为欧拉角：

将基于无人机四元数转换为欧拉角：

4.3)、将机载摄像头欧拉角与惯性测量单元欧拉角融合，得到融合后的欧拉角[pitch_local，roll_local，yaw_local]^T：

其中，α₂、β₂为权重标量，融合后的欧拉角表示融合后的姿态角；

(5)、基于气压计读取无人机当前气压压力，计算当前海拔高度h_abs

其中，P₀为标准大气压强，取值101.325kPa，P为无人机实际测量大气压，单位为kPa，T为实际测量温度，单位为℃；

(6)、全局位置、姿态角融合估计

6.1)、将融合后的姿态角转换为地理坐标系下，获得全局姿态角[pitch,roll，yaw]^T：

6.2)、将融合的局部平移向量p_local从右前上坐标系转换为ENU坐标系位置[e，n，u]^T：

其中，ENU坐标系下的坐标原点为无人机起飞位置；

6.3)、将ENU坐标系下的无人机位置[e，n，u]^T转换为ECEF坐标系下的无人机位置[x_ecef，y_ecef，z_ecef]^T：

6.4)、将ECEF坐标系下的无人机位置[x_ecef，y_ecef，z_ecef]^T转换到WGS84坐标系下：

其中，alt、lat、lon分别为高度、纬度、经度，e为椭球偏心率：

C为基准椭球体的曲率半径：

a为地球长半轴长度，b为地球短半轴长度，ξ为高程异常值，为似大地水准面与大地高程参考椭球面的高程之差；

α₃、β₃为权重标量，h_abs0为无人机起飞位置海拔高度；

(7)、将全局位置、姿态角发送到飞控系统

全局位姿的纬度lat、经度lon、高度alt、俯仰角pitch、横滚角roll、航向角yaw以及速度发送到飞控系统，实现自定位控制。