[发明专利]一种用于锚栓图像检测的无人机控制方法和系统

权利要求书

1.一种用于锚栓图像检测的无人机控制方法，其特征在于，包括：

根据目标锚栓的初始坐标位置、无人机及搭载摄像机的坐标和角度，建立无人机自主定位模型；

根据所述无人机自主定位模型，实时控制所述无人机向所述目标锚栓飞行；

在所述无人机的飞行过程中，根据所述无人机和所述目标锚栓的坐标位置，实时校正所述摄像机的拍摄角度；

当所述无人机飞行至所述目标锚栓对应的拍摄区域时，根据所述摄像机的拍摄角度，控制所述摄像机获取所述目标锚栓的图像；

所述根据无人机和目标锚栓的坐标位置，实时校正所述摄像机的拍摄角度的步骤，包括：

根据无人机的初始坐标位置建立误差方程，使用所述误差方程建立无人机的旋转矩阵；

使用所述旋转矩阵的系数阵和常数阵，构成无人机坐标位置和姿态角的法方程式；

根据最小二乘法求解所述法方程式，计算所述无人机的坐标位置和摄像机的拍摄角度的校正值；

具体为：

无人机的坐标位置即无人机的位置点坐标(X_u、Y_u、Z_u)；摄像机的拍摄角度即摄像机的姿态角：偏航角、俯仰角和横滚角：ω、ξ和κ；

首先通过共线方程求解无人机位置信息，共线方程如下：

式中，(Fx)⁰和(Fy)⁰依次是Fx和Fy的初值；和分别是Fx和Fy对无人机位置信息即摄像机的位姿信息中各元素的偏导数；ΔX_u、ΔY_u、ΔZ_u、Δω、Δξ和Δκ分别是无人机的位置点坐标X_u、Y_u、Z_u以及摄像机的姿态角ω、ξ和κ的初值的增量；

对上式等号两边同时乘以并进行变换得到下式：

式中，Vx和Vy为残差；a_ij为各个变量前的系数；ΔX_u、ΔY_u、ΔZ_u、Δω、Δξ和Δκ为待求值，lx和ly是像点分别在x轴和y轴的观测值和计算值之差；该方程式即共线条件方程线性化公式，即误差方程；用于调节无人机位置点和摄像机姿态角的误差；

已知每个地面目标点均按照上述公式列出误差方程组的两个误差方程，那么n个目标点即可列出2n个方程，用矩阵形式简单表示，即旋转矩阵如下：

V＝AX-L；其中，

X＝[ΔX_u ΔY_u ΔZ_u Δω Δξ Δκ]^T；

求解上述2n个方程构成的方程组，即可得到无人机位置点坐标及摄像机姿态角的增量；

在求得姿态角的增量后，计算ω、ξ和κ分别对应的方向余弦：cosω、cosξ及cosκ；将该方向余弦的偏导数代入求无人机坐标点和对ω、ξ和κ的偏导数的共线方程，能够将上述共线方程简化为：

便于求解无人机坐标点和对ω⁰、ξ⁰和κ的偏导数；

然后，逐点计算每个地面目标锚栓点对应的像素点坐标值(x_ic、y_ic)，以及每个误差方程的系数及常数项；

在计算得到每个误差方程的系数及常数项后，计算上述旋转矩阵的系数阵A^TA和常数阵A^TL，使用上述系数阵A^TA和常数阵A^TL构成法方程式；

因为旋转矩阵：V＝AX-L是一个由2n个方程组成的误差方程组，且其方程的总数多于待求未知数的个数，因此采用最小二乘法求解；

按最小二乘原理，若使得各个误差方程式的残差平方和最小，则待求参数的最佳估计值满足：

V^TV＝min，将原本复杂的问题转化为V^TV对待求值的极值求解问题；

使用V^TV分别对ΔX_u、ΔY_u、ΔZ_u、Δω、Δξ和Δκ求极值；

由于V^TV＝(AX-L)^T(AX-L)＝X^TA^TAX-2X^TA^TL+L^TL，则整理后得到：

A^TAX＝A^TL；

将N＝A^TA表示方程组的系数阵，将等式两边同乘以N^-1则求出X；得到：

X＝N^-1A^TL，该式即ΔX_u、ΔY_u、ΔZ_u、Δω、Δξ和Δκ的解；上述求解过程需要迭代求解，将无人机坐标点和摄像机姿态角的各元素的校正值与指定线差比较，直至比较结果收敛。