[发明专利]用于输电塔场景下的自主巡检无人机的高精度定位方法及其系统

权利要求书

1.一种用于输电塔场景下的自主巡检无人机的高精度定位方法，包括以下步骤：

S1：提取巡检无人机惯性导航装置中加速度计和陀螺仪中的数据，计算出无人机在输电塔场景下的绝对姿态与位置；

S2：利用巡检无人机上的视觉传感器对输电塔周围环境进行数据采集，根据采集到的图像，计算相邻两帧图像中像素的位移，得到无人机的相对位姿变化，通过时间和像素的梯度分析得到无人机光流的运动情况，进行辅助导航定位，具体步骤包括：

S201：根据无人机巡检过程中采集到相邻两帧图像的光强连续情况，利用如下式的光流方程，求得图像中像素在x轴上的运动速度，记为图像中像素在y轴上的运动速度，记为图像在该点x方向处的梯度记为ψ_x；图像在该点y方向处的梯度记为ψ_y；图像灰度对时间的变化量记为ψ_t：

S202：针对无人机拍摄的第k-1时刻至第k时刻的图像，按照步骤S201计算第k时刻图像各像素的运动速度(u，v)，对图像建立关于(u，v)的方向直方图，选取直方图的最大值所代表的方向为第k-1时刻至第k时刻的图像像素运动方向，则其反方向为无人机第k-1时刻至第k时刻的运动方向；

S203：结合S1中计算的第k-1时刻无人机速度大小与步骤S202计算的k-1至k时刻的运动方向，构造无人机第k-1时刻至k时刻的速度v_vk-1；

S204：根据无人机第k-1时刻的速度v_vk-1，结合无人机第k-1时刻至第k时刻的运动方向，计算出无人机于k时刻的位置P_vk＝P_vk-1+v_vk-1*T₂；式中T₂为相机的采样周期，P_vk、P_vk-1分别为采用光流法计算的无人机k-1时刻与k时刻的位置；

S205：计算未知误差e＝||P_ik-P_vk||₂，若则由P_sk＝aP_ik+bP_vk，对无人机位置进行更新，否则不对无人机位置进行更新；式中P_sk为通过光流运动计算出的k时刻无人机位置，P_ik为惯性导航系统计算得到的第k时刻无人机的位置，P_vk为采用光流法计算得到的无人机位置，为位置误差阈值，a,b分别为加权系数；

S3：根据步骤S1得到的无人机绝对姿态与位置以及步骤S2得到的相对位姿变化数据，结合采集到所述图像的特征点信息，计算无人机位姿，使无人机获得精确的位姿信息，具体步骤包括：

S301：将所述视觉传感器采集到第1帧与第2帧图像的ORB特征点进行特征匹配后，求解得到无人机前两帧图像的初始位姿转换；

S302：根据S301得到的初始位姿转换关系，依照图像的特征匹配情况，计算出第1帧与第2帧图像中每一个特征点的深度；根据第1帧图像特征点像素坐标与深度信息即可求出第1帧图像特征点对应的空间点坐标P，构建一个初步的空间点点云；

S303：从第2帧图像以后的每一帧图像，将其与还原出的空间点进行匹配，根据匹配结果计算出该帧的位姿，而后构建最小二乘问题优化该帧位姿；对第k帧图像中未与空间点匹配的特征点，将其与第k-1帧图像中的ORB特征点进行特征匹配，还原出对应的空间点，并增加至步骤S302中的空间点点云中；

S304：构建卡尔曼滤波系统，以惯导联合光流法计算出的无人机位姿为系统状态量，步骤S303得到的无人机位姿为系统观测量定义系统模型为：

则通过卡尔曼滤波系统实时更新，得到高精度的无人机位姿；模型中f为采用步骤S1和S2方法计算的由k-1时刻无人机位姿X_k-1以及视觉传感器输入数据u映射到k时刻无人机位姿X_k的映射关系，u为视觉传感器输入数据，分别为零均值高斯分布。