CN106155081A - 一种旋翼无人机大范围目标监测和精确定位方法 - Google Patents

Abstract

本发明公开一种旋翼无人机大范围目标监测和精确定位方法，利用搭载在无人机上的3个摄像机拍摄图像，并将图像回传到地面站，选择一个摄像机拍摄的图像，并对应选取图像中标志物；然后对该标志物进行视觉识别定位，并结合相机的视角约束，生成有效的航迹实现对无人机的航迹规划；在此航迹下，对已选定的标志物进行多点图像测量，运用图像处理和线性回归的方法计算航向偏差；令无人机恢复到平飞航迹，选择摄像机视野里的任一静止或运动目标，实现目标的精确定位；与现有技术相比，该方法的监测范围大，更容易捕获标志物，并通过有效的航迹规划，更精确地计算航向偏差，提高目标定位精度。

Description

一种旋翼无人机大范围目标监测和精确定位方法

技术领域

本发明属于视觉测量与无人机飞行控制领域，具体涉及一种旋翼无人机大范围目标监测和精确定位方法。

背景技术

现今世界的无人机技术已日趋成熟，性能也日臻完美。对于机载摄像机的应用也极为普遍，因为图像摄取是最直观有效的侦查、监视、追踪手段。在民用中，载有摄像机的小型无人机可以完成勘探、跟踪、搜索等任务，例如：对地震、洪灾等自然灾害地区进行灾情评估、地貌勘察等。随着无人机的快速发展，其应用领域也自然会从军事领域迅速地向民用领域蔓延。目前，基于视觉的无人机目标定位已成为国内外无人机研究领域的热点问题。

基于视觉的无人机目标定位方法，是在获取无人机自身GPS位置、高程、摄像机内外参数等前提下，利用摄像机的测距模型，求取目标在大地坐标系下的绝对坐标。现有成型的视觉定位系统大多面向军用，因其依赖于配套的高精度传感器，例如，INS惯导系统等，该模型可获得很高的定位精度。旋翼无人机具有成本低、可定点悬停和垂直起降等优势，在民用领域得到越来越广的应用。考虑传感器成本、体积等限制，旋翼无人机一般配备低精度AHRS惯性测量系统等传感器。而利用旋翼无人机进行视觉定位，由于AHRS系统本身存在很大的航向偏差，该模型不能直接使用，需对航向偏差进行有效估计。而现有技术仅利用单一摄像机寻找标志物，然后采取对同一标志物的多点图像测量计算航向偏差，主要存在以下缺陷：1、由于视场范围有限，发现满足条件的标志物比较困难，耗时时间长；2、没有考虑对无人机的航迹进行规划，难以精确地计算航向偏差，导致目标的定位精度不高。

发明内容

有鉴于此，本发明提供了一种旋翼无人机大范围目标监测和精确定位方法，能够精确地计算AHRS系统的航向偏差，然后进行校正，精确定位。

实现本发明的技术方案如下：

一种旋翼无人机大范围目标监测和精确定位方法，包括以下步骤：

步骤一、利用搭载在无人机上的3个摄像机拍摄图像并且回传到地面站，其中3个摄像机分别为1个前视摄像机和2个侧视摄像机；

步骤二、从回传的图像中选择具有清晰轮廓的静态物体作为标志物，并将拍摄该图像的摄像机作为选定摄像机；若仅有一个摄像机拍摄到标志物，则选择该摄像机作为选定摄像机，若有两个以上摄像机拍摄到标志物，则优先选择前视摄像机作为选定摄像机；

步骤三、若选定摄像机为前视摄像机，则设定无人机的航迹为平飞，并执行步骤五；若选定摄像机为任一侧视摄像机，则预估标志物相对于无人机的位置，结合标志物相对于无人机的位置、选定摄像机的视角约束、航向偏差的估计精度以及实时性要求，生成无人机的航迹；

步骤四、地面站发送控制命令至无人机，使无人机按照生成的航迹飞行；

步骤五、在无人机沿航迹飞行过程中，选定摄像机对标志物进行多点视觉测量，运用图像处理和线性回归的方法计算无人机的航向偏差；

步骤六、若无人机的航迹非平飞，则将无人机由航迹飞行状态切换到平飞状态，选择任一摄像机视野中的任一目标，利用航向偏差得到无人机的真实航向，进而实现对目标的精确定位。

进一步地，步骤三中预估标志物相对于无人机的位置具体过程为：

对标志物进行识别，然后根据标志物的几何中心在图像中的像素位置(x_f,y_f)，计算标志物在选定摄像机坐标系{C}下的坐标p_c＝[x_c y_c z_c]^T，再将p_c转化为无人机的机体坐标系{B}下的坐标p_b＝[x_b y_b z_b]^T，即标志物相对于无人机的位置；其中，选定摄像机坐标系{C}的x,y,z分别指向摄像机的前方，右方和下方；机体坐标系{B}的x,y,z分别指向无人机的前方，右方和下方。

进一步地，步骤三中生成无人机的航迹具体计算过程为：

若选定摄像机为左视摄像机，计算过程如下：

(1)设置选定摄像机的视角约束为FOV，为确保标志物落在选定摄像机的图像视野内，无人机需限定其飞行范围M：

$\begin{array}{c}{x}_0-\frac{h}{\sin \left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\≤M_x\≤x_0+\frac{h}{\sin \left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\\ y_0-\left(\frac{h}{\tan \left(\mathrm{\β}\right)}-\frac{h}{\tan \left(\mathrm{\β}+\frac{FOV}{2}\right)}\right)\≤M_y\≤y_0+\left(\frac{h}{\tan \left(\mathrm{\β}-\frac{FOV}{2}\right)}-\frac{h}{\tan \left(\mathrm{\β}\right)}\right)\end{array}---\left(4\right)$

无人机在任一位置都具有一定的拍摄覆盖范围S：

$\begin{array}{c}{x}_0-\frac{h}{sin\left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\≤S_x\≤x_0+\frac{h}{sin\left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\\ y_0-\frac{h}{tan(\mathrm{\β}-\frac{FOV}{2})}\≤S_y\≤y_0-\frac{h}{tan(\mathrm{\β}+\frac{FOV}{2})}\end{array}---\left(5\right)$

其中，x₀，y₀为无人机在惯性参考坐标系{I}下的水平方向位置，β为选定摄像机的俯视角，h为无人机与标志物的相对高度，M_x和M_y分别为无人机在惯性参考坐标系{I}下飞行范围的横、纵坐标；S_x和S_y分别为无人机在惯性参考坐标系{I}下任一位置拍摄范围的横、纵坐标；

(2)控制无人机在原位置作角度为α的自旋；

$\mathrm{\α}=arctan(-\frac{x_b}{y_b})---\left(6\right)$

若α≤0，则逆时针旋转|α|角度，若α＞0，则顺时针旋转|α|；

(3)根据标志物与覆盖范围右边界的距离若Δd≥0，则无人机向右平飞距离为s₁的航程，否则，无人机向左平飞距离为s₂的航程；

$\begin{array}{c}{s}_1=\left|\mathrm{\Δ}d\right|+k_1\\ s_2=\left|\mathrm{\Δ}d\right|+k_2\end{array}---\left(7\right)$

其中，为无人机的覆盖范围S的右边界；k₁设置为GPS的水平定位误差δ，k₂＝-δ；

(4)根据航向偏差的估计精度要求，选择不同的航迹，航向偏差的估计精度从高到低所对应的航迹分别为：绕椭圆飞,绕圆周飞和平飞；若选择绕圆周飞航迹，其半径为R，或选择绕椭圆飞航迹，其短半轴为a，长半轴为b；对于绕圆周或绕椭圆飞行的航迹，飞行航迹的弧度rad为：1.5π≤rad≤2π；若d≥0，若Δd＜0，

若选定摄像机为右视摄像机，计算过程如下：

(1)设置选定摄像机的视角约束为FOV，为确保标志物落在选定摄像机的图像视野内，无人机需限定其飞行范围M：

$\begin{array}{c}{x}_0-\frac{h}{sin\left(\mathrm{\β}\right)}\·tan\left(\frac{FOV}{2}\right)\≤M_x\≤x_0+\frac{h}{sin\left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\\ y_0-\left(\frac{h}{\tan \left(\mathrm{\β}-\frac{FOV}{2}\right)}-\frac{h}{\tan \left(\mathrm{\β}\right)}\right)\≤M_y\≤y_0+\left(\frac{h}{\tan \left(\mathrm{\β}\right)}-\frac{h}{\tan \left(\mathrm{\β}+\frac{FOV}{2}\right)}\right)\end{array}---\left(15\right)$

无人机在任一位置都具有一定的拍摄覆盖范围S：

$\begin{array}{c}{x}_0-\frac{h}{\sin \left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\≤S_x\≤x_0+\frac{h}{\sin \left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\\ y_0+\frac{h}{\tan \left(\mathrm{\β}+\frac{FOV}{2}\right)}\≤S_y\≤y_0+\frac{h}{\tan \left(\mathrm{\β}-\frac{FOV}{2}\right)}\end{array}---\left(16\right)$

(2)控制无人机在原位置作角度为α的自旋；

$\mathrm{\α}=arctan(-\frac{x_b}{y_b})---\left(17\right)$

若α≤0，则逆时针旋转|α|角度，若α＞0，则顺时针旋转|α|；

(3)根据标志物与覆盖范围左边界的距离若Δd≥0，则无人机向右平飞距离为s₁的航程，否则，无人机向左平飞距离为s₂的航程；

$\begin{array}{c}{s}_1=\left|\mathrm{\Δ}d\right|-k_1\\ s_2=\left|\mathrm{\Δ}d\right|-k_2\end{array}---\left(18\right)$

其中，为无人机的覆盖范围S的右边界；k₁设置为GPS的水平定位误差δ，k₂＝-δ；

(4)根据航向偏差的估计精度要求，选择不同的航迹，航向偏差的估计精度从高到低所对应的航迹分别为：绕椭圆飞,绕圆周飞和平飞；若选择绕圆周飞航迹，其半径为R，或选择绕椭圆飞航迹，其短半轴为a，长半轴为b；

对于绕圆周或绕椭圆飞行的航迹，飞行航迹的弧度rad为：1.5π≤rad≤2π；若Δd≥0，若Δd＜0，

进一步地，预估标志物相对于无人机的位置具体计算如下：

利用摄像机的针孔模型得到：

$\left[\begin{array}{c}{x}_f\\ y_f\end{array}\right]=\frac{f}{x_c}\left[\begin{array}{c}{y}_c\\ z_c\end{array}\right]---\left(1\right)$

无人机与标志物的相对高度为：

联立(1)和(2)得到p_c＝[x_c y_c z_c]^T，

标志物相对于无人机的位置为

$p_b=C_{cam}^b{p}_c=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]p_c---\left(3\right)$

其中，f为选定摄像机的内参数焦距；θ_c表示选定摄像机坐标系{C}相对于惯性参考坐标系{I}的横滚角、俯仰角；ψ、θ、φ表示选定摄像机坐标系{C}相对于机体坐标系{B}的方位角、俯仰角、横滚角。

有益效果：

本发明所提供的方法采用搭载在旋翼无人机上的3个摄像机拍摄不同方位的图像，监测范围大，更容易地人为捕获具有清晰轮廓的静态物体作为标志物；若选定摄像机为任一侧视摄像机，预估标志物相对于无人机的位置，生成无人机的航迹，在无人机沿此航迹飞行过程中，选定摄像机对标志物进行多点视觉测量，能明显提高航向偏差的估计精度；然后校正航向，实现对目标的精确定位。

附图说明

图1为本发明的旋翼无人机目标定位系统结构图；

图2为本发明所提供方法的流程图；

图3为本发明的3个摄像机拍摄覆盖范围效果图；

图4为本发明所使用无人机与标志物的相对高度的示意图；

图5为本发明的无人机飞行范围和拍摄覆盖范围示意图；

图6为本发明所使用的摄像机测距模型示意图；

图7为本发明所提供方法的目标定位效果图；其中，7-a表示无航迹规划的定位效果，7-b，7-c，7-d分别表示平飞航迹规划、圆周航迹规划、椭圆航迹规划的定位效果。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

搭建如下实验平台对本发明的有效性进行验证。使用一架T650四旋翼无人机，一台笔记本作为地面站，对无人机进行控制，系统结构如图1所示。

对于无人机，机上带有GPS定位系统，AHRS惯性测量系统，高程计，无线图像传输模块和无线数据收发模块，使用APM飞控工作在自稳模式来保证无人机的稳定飞行。在无人机的机头位置安装前视摄像机，在左翼、右翼位置分别安装左视摄像机和右视摄像机，3个摄像机的俯视角β均为45°。其中，摄像机用于采集图像，并通过无线图像传输模块回传图像到地面站，而无人机的位置、姿态和高程信息通过无线数据收发模块传输到地面站。

地面站以计算机为主体，运行无人机视觉定位、航迹生成和控制算法，使用USB接口连接无线数据收发模块，实现无人机与地面站的相互通信。

基于该实验平台，如图2所示，一种旋翼无人机大范围目标监测和精确定位方法，包括以下步骤：

步骤一、系统启动后，利用搭载在无人机上的3个摄像机拍摄图像并且回传到地面站，其中3个摄像机分别为1个前视摄像机和2个侧视摄像机，拍摄覆盖范围如图3所示；

步骤二、从回传的图像中选择具有清晰轮廓的静态物体作为标志物，并将拍摄该图像的摄像机作为选定摄像机；若仅有一个摄像机拍摄到标志物，则选择该摄像机作为选定摄像机，若有两个以上摄像机拍摄到标志物，则优先选择前视摄像机作为选定摄像机；若拍摄到标志物的为两个侧视摄像机，则任选一个侧视摄像机作为选定摄像机。

步骤三、若选定摄像机为前视摄像机，则设定无人机的航迹为平飞，并执行步骤五；若选定摄像机为任一侧视摄像机，则预估标志物相对于无人机的位置，结合标志物相对于无人机的位置、选定摄像机的视角约束、航向偏差的估计精度以及实时性要求，生成无人机的航迹；

运用SIFT算法对标志物进行识别，得到m个特征点P₁,P₂...P_m-1,P_m并进行存储作为模板，然后取这些特征点的几何中心P_g(g≤m)代表标志物在图像中的像素位置,记为(x_f,y_f)，再计算标志物在选定摄像机坐标系{C}下的坐标p_c＝[x_c y_c z_c]^T，最后将p_c转化为无人机的机体坐标系{B}下的坐标p_b＝[x_b y_b z_b]^T，即相对于无人机的位置；其中，选定摄像机坐标系{C}的x,y,z分别指向摄像机的前方，右方和下方；机体坐标系{B}的x,y,z分别指向无人机的前方，右方和下方。

预估标志物相对于无人机的位置具体计算如下：

利用摄像机的针孔模型得到：

$\left[\begin{array}{c}{x}_f\\ y_f\end{array}\right]=\frac{f}{x_c}\left[\begin{array}{c}{y}_c\\ z_c\end{array}\right]---\left(1\right)$

如图4所示，无人机与标志物的相对高度为：

联立(1)和(2)得到p_c＝[x_c y_c z_c]^T，

标志物相对于无人机的位置为

$p_b=C_{cam}^b{p}_c=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]p_c---\left(3\right)$

其中，f为选定摄像机的内参数焦距；θ_c表示选定摄像机坐标系{C}相对于惯性参考坐标系{I}的横滚角、俯仰角；ψ、θ、φ表示选定摄像机坐标系{C}相对于机体坐标系{B}的方位角、俯仰角、横滚角。

以下计算以左视摄像机为例计算无人机的航迹：

(1)设置选定摄像机的视角约束FOV为30°，为确保标志物落在选定摄像机的图像视野内，如图5所示，图中①，②，③表示不同航向飞行，无人机需限定其飞行范围M：

无人机在任一位置都具有一定的拍摄覆盖范围S：

其中，x₀，y₀为无人机在惯性参考坐标系{I}下的水平方向位置，β为选定摄像机的俯视角，M_x和M_y分别为无人机在惯性参考坐标系{I}下飞行范围的横、纵坐标；S_x和S_y分别为无人机在惯性参考坐标系{I}下任一位置拍摄范围的横、纵坐标；

(2)控制无人机在原位置作角度为α的自旋；

$\mathrm{\α}=arctan(-\frac{x_b}{y_b})---\left(6\right)$

若α≤0，则逆时针旋转|α|角度，若α＞0，则顺时针旋转|α|；

(3)根据标志物与覆盖范围右边界的距离若Δd≥0，则无人机向右平飞距离为s₁的航程，否则，无人机向左平飞距离为s₂的航程；

$\begin{array}{c}{s}_1=\left|\mathrm{\Δ}d\right|+k_1\\ s_2=\left|\mathrm{\Δ}d\right|+k_2\end{array}---\left(7\right)$

其中，为无人机的覆盖范围S的右边界；k₁设置为GPS的水平定位误差δ，k₂＝-δ；

(4)根据航向偏差的估计精度要求，选择不同的航迹，航向偏差的估计精度从高到低所对应的航迹分别为：绕椭圆飞,绕圆周飞和平飞；若选择绕圆周飞航迹，其半径为R，或选择绕椭圆飞航迹，其短半轴为a，长半轴为b；

对于绕圆周或绕椭圆飞行的航迹，飞行航迹的弧度rad为：1.5π≤rad≤2π；若Δd≥0，若Δd＜0，

步骤四、地面站发送控制命令至无人机，使无人机按照生成的航迹飞行；

步骤五、在无人机沿航迹飞行过程中，选定摄像机对标志物进行多点视觉测量，运用图像处理和线性回归的方法计算无人机的航向偏差；具体地，采用SIFT算法对当前图像进行特征提取，然后利用步骤三中模板对特征点进行匹配，得到w组匹配点P₁,P₂...P_w-1,P_w，最后取这些匹配点的几何中心P_s(s≤w)代表标志物在图像中的像素位置,记为(x_f,y_f)，如图6所示，[x₀ y₀ z₀]^T，[x_i y_i z_i]^T分别表示无人机、标志物在惯性参考坐标系{I}的坐标，摄像机的测距模型为

$\left(\begin{array}{c}{x}_i\\ y_i\end{array}\right)=\left(\begin{array}{c}{x}_0\\ y_0\end{array}\right)+(z_0-z_i)\frac{1}{(0,0,1)C_b^n\left(\begin{array}{c}{x}_f\\ y_f\\ f\end{array}\right)}\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\end{array}\right]C_b^n\left(\begin{array}{c}{x}_f\\ y_f\\ f\end{array}\right)---\left(8\right)$

姿态矩阵为

$C_b^n=\left[\begin{array}{ccc}{\mathrm{cos\ψ}}_n{\mathrm{cos\θ}}_n& {\mathrm{cos\ψ}}_n{\mathrm{sin\θ}}_n{\mathrm{sin\φ}}_n-{\mathrm{sin\ψ}}_n{\mathrm{cos\φ}}_n& {\mathrm{sin\ψ}}_n{\mathrm{sin\φ}}_n+{\mathrm{cos\ψ}}_n{\mathrm{sin\θ}}_n{\mathrm{cos\φ}}_n\\ {\mathrm{sin\ψ}}_n{\mathrm{cos\θ}}_n& {\mathrm{cos\ψ}}_n{\mathrm{cos\φ}}_n+{\mathrm{sin\ψ}}_n{\mathrm{sin\θ}}_n{\mathrm{sin\φ}}_n& {\mathrm{sin\ψ}}_n{\mathrm{sin\θ}}_n{\mathrm{cos\φ}}_n-{\mathrm{cos\ψ}}_n{\mathrm{sin\φ}}_n\\ -{\mathrm{sin\θ}}_n& {\mathrm{sin\φ}}_n{\mathrm{cos\θ}}_n& {\mathrm{cos\φ}}_n{\mathrm{cos\θ}}_n\end{array}\right]$

其中，相对高度h＝z₀-z_i，(ψ_n,θ_n,φ_n)表示无人机的航向角、俯仰角、横滚角，俯仰角θ、横滚角φ的测量精度高，其误差忽略不计，而航向角ψ的测量存在较大的航向偏差δψ，需要通过线性回归方法进行计算。

设参数θ＝[θ₁,θ₂]^T，θ₁＝[x_i,y_i]^T，θ₂＝δψ，y₁＝[x₀,y₀,z₀,x_f,y_f]^T，量测方程为

z₁＝y₁+v₁,v₁～N(0,R₁) (9)

$C_{bc}^n\≈C_b^n+{\mathrm{\δC}}_b^n({\mathrm{\θ}}_2+v_2),v_2~N(0,R_2)---\left(10\right)$

其中v₁，v₂为测量噪声，R₁，R₂为实对称正定阵。则式(8)变形为

${\mathrm{\θ}}_1=f(z_1-v_1,C_{bc}^n-{\mathrm{\δC}}_b^n({\mathrm{\θ}}_2+v_2\left)\right)---\left(11\right)$

运用泰勒展开，式(11)变为

$\begin{array}{c}f(z_1-v_1,C_{bc}^n-{\mathrm{\δC}}_b^n({\mathrm{\θ}}_2+v_2\left)\right)\\ \≈f\left(z_1,C_{bc}^n\right)-\frac{\∂f}{\∂y_1}{|}_{z_1,C_{bc}^n}.v_1-\frac{\∂f}{\∂{\mathrm{\θ}}_2}{|}_{z_1,C_{bc}^n}.v_2-\frac{\∂f}{\∂{\mathrm{\θ}}_2}{|}_{z_1,C_{bc}^n}.{\mathrm{\θ}}_2\end{array}---\left(12\right)$

由式(11)和式(12)，得

$f(z_1,C_{bc}^n)\≈{\mathrm{\θ}}_1+\frac{\∂f}{\∂{\mathrm{\θ}}_2}{|}_{z_1,C_{bc}^n}.{\mathrm{\θ}}_2+\frac{\∂f}{\∂y_1}{|}_{z_1,C_{bc}^n}.v_1+\frac{\∂f}{\∂{\mathrm{\θ}}_2}{|}_{z_1,C_{bc}^n}.v_2---\left(13\right)$

通过对同一标志物的多点视觉测量，通过这些测量值得到如下线性回归方程，

$\left(\begin{array}{c}f(z_{1_1},C_{{\mathrm{bc}}_1}^n)\\ .\\ .\\ f(z_{1_N},C_{{\mathrm{bc}}_N}^n)\end{array}\right)=\left[\begin{array}{cc}{I}_{2,}& \frac{\∂f}{\∂{\mathrm{\θ}}_2}{|}_{z_{1_1},C_{{\mathrm{bc}}_1}^n}\\ .& .\\ .& .\\ I_{2,}& \frac{\∂f}{\∂{\mathrm{\θ}}_2}{|}_{z_{1_N},C_{{\mathrm{bc}}_N}^n}\end{array}\right]\mathrm{\θ}+V---\left(14\right)$

其中，噪声为

V～N(0,R)

协方差矩阵为

$R=diag\left({\left\{\left(\frac{\∂f}{\∂y_1}{|}_{z_{1_k},C_{{\mathrm{bc}}_k}^n}\right)R_1{\left(\frac{\∂f}{\∂y_1}{|}_{z_{1_k},C_{{\mathrm{bc}}_k}^n}\right)}^T+\left(\frac{\∂f}{\∂{\mathrm{\θ}}_2}{|}_{z_{1_k},C_{{\mathrm{bc}}_k}^n}\right)R_2{\left(\frac{\∂f}{\∂{\mathrm{\θ}}_2}{|}_{z_{1_k},C_{{\mathrm{bc}}_k}^n}\right)}^T\right\}}_{k=1}^N\right)$

通过式(14)可求解航向偏差δψ。

步骤六、若无人机的航迹非平飞，则将无人机由航迹飞行状态切换到平飞状态，选择任一摄像机视野中的任一目标，利用所计算的航向偏差得到无人机的真实航向，进而实现对目标的精确定位。

以选定摄像机为左视摄像机举例，且在如下条件下：δ＝5m，rad＝1.5π，δψ＝30deg，h＝45m，本发明所提供方法的效果如表1，如图7所示。

表1 不同航迹规划下的结果比较

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种旋翼无人机大范围目标监测和精确定位方法，其特征在于，包括以下步骤：

步骤一、利用搭载在无人机上的3个摄像机拍摄图像并且回传到地面站，其中3个摄像机分别为1个前视摄像机和2个侧视摄像机；

步骤二、从回传的图像中选择具有清晰轮廓的静态物体作为标志物，并将拍摄该图像的摄像机作为选定摄像机；若仅有一个摄像机拍摄到标志物，则选择该摄像机作为选定摄像机，若有两个以上摄像机拍摄到标志物，则优先选择前视摄像机作为选定摄像机；

步骤三、若选定摄像机为前视摄像机，则设定无人机的航迹为平飞，并执行步骤五；若选定摄像机为任一侧视摄像机，则预估标志物相对于无人机的位置，结合标志物相对于无人机的位置、选定摄像机的视角约束、航向偏差的估计精度以及实时性要求，生成无人机的航迹；

步骤四、地面站发送控制命令至无人机，使无人机按照生成的航迹飞行；

步骤五、在无人机沿航迹飞行过程中，选定摄像机对标志物进行多点视觉测量，运用图像处理和线性回归的方法计算无人机的航向偏差；

步骤六、若无人机的航迹非平飞，则将无人机由航迹飞行状态切换到平飞状态，选择任一摄像机视野中的任一目标，利用航向偏差得到无人机的真实航向，进而实现对目标的精确定位。

2.如权利要求1所述的一种旋翼无人机大范围目标监测和精确定位方法，其特征在于，步骤三中预估标志物相对于无人机的位置具体过程为：

对标志物进行识别，然后根据标志物的几何中心在图像中的像素位置(x_f,y_f)，计算标志物在选定摄像机坐标系{C}下的坐标p_c＝[x_c y_c z_c]^T，再将p_c转化为无人机的机体坐标系{B}下的坐标p_b＝[x_b y_b z_b]^T，即标志物相对于无人机的位置；其中，选定摄像机坐标系{C}的x,y,z分别指向摄像机的前方，右方和下方；机体坐标系{B}的x,y,z分别指向无人机的前方，右方和下方。

3.如权利要求1所述的一种旋翼无人机大范围目标监测和精确定位方法，其特征在于，步骤三中生成无人机的航迹具体计算过程为：

若选定摄像机为左视摄像机，计算过程如下：

(1)设置选定摄像机的视角约束为FOV，为确保标志物落在选定摄像机的图像视野内，无人机需限定其飞行范围M：

$\begin{array}{c}{x}_0-\frac{h}{\sin \left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\≤M_x\≤x_0+\frac{h}{\sin \left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\\ y_0-\left(\frac{h}{\tan \left(\mathrm{\β}\right)}-\frac{h}{\tan \left(\mathrm{\β}+\frac{FOV}{2}\right)}\right)\≤M_y\≤y_0+\left(\frac{h}{\tan \left(\mathrm{\β}-\frac{FOV}{2}\right)}-\frac{h}{\tan \left(\mathrm{\β}\right)}\right)\end{array}---\left(4\right)$

无人机在任一位置都具有一定的拍摄覆盖范围S：

$\begin{array}{c}{x}_0-\frac{h}{sin\left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\≤S_x\≤x_0+\frac{h}{sin\left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\\ y_0-\frac{h}{tan(\mathrm{\β}-\frac{FOV}{2})}\≤S_y\≤y_0-\frac{h}{tan(\mathrm{\β}+\frac{FOV}{2})}\end{array}---\left(5\right)$

其中，x₀，y₀为无人机在惯性参考坐标系{I}下的水平方向位置，β为选定摄像机的俯视角，h为无人机与标志物的相对高度，M_x和M_y分别为无人机在惯性参考坐标系{I}下飞行范围的横、纵坐标；S_x和S_y分别为无人机在惯性参考坐标系{I}下任一位置拍摄范围的横、纵坐标；

(2)控制无人机在原位置作角度为α的自旋；

$\mathrm{\α}=arctan(-\frac{x_b}{y_b})---\left(6\right)$

若α≤0，则逆时针旋转|α|角度，若α＞0，则顺时针旋转|α|；

(3)根据标志物与覆盖范围右边界的距离若Δd≥0，则无人机向右平飞距离为s₁的航程，否则，无人机向左平飞距离为s₂的航程；

$\begin{array}{c}{s}_1=\left|\mathrm{\Δ}d\right|+k_1\\ s_2=\left|\mathrm{\Δ}d\right|+k_2\end{array}---\left(7\right)$

其中，为无人机的覆盖范围S的右边界；k₁设置为GPS的水平定位误差δ，k₂＝-δ；

(4)根据航向偏差的估计精度要求，选择不同的航迹，航向偏差的估计精度从高到低所对应的航迹分别为：绕椭圆飞,绕圆周飞和平飞；若选择绕圆周飞航迹，其半径为R，或选择绕椭圆飞航迹，其短半轴为a，长半轴为b；

对于绕圆周或绕椭圆飞行的航迹，飞行航迹的弧度rad为：1.5π≤rad≤2π；若Δd≥0，若Δd＜0，

若选定摄像机为右视摄像机，计算过程如下：

(1)设置选定摄像机的视角约束为FOV，为确保标志物落在选定摄像机的图像视野内，无人机需限定其飞行范围M：

$\begin{array}{c}{x}_0-\frac{h}{sin\left(\mathrm{\β}\right)}\·tan\left(\frac{FOV}{2}\right)\≤M_x\≤x_0+\frac{h}{sin\left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\\ y_0-\left(\frac{h}{\tan \left(\mathrm{\β}-\frac{FOV}{2}\right)}-\frac{h}{\tan \left(\mathrm{\β}\right)}\right)\≤M_y\≤y_0+\left(\frac{h}{\tan \left(\mathrm{\β}\right)}-\frac{h}{\tan \left(\mathrm{\β}+\frac{FOV}{2}\right)}\right)\end{array}---\left(15\right)$

无人机在任一位置都具有一定的拍摄覆盖范围S：

$\begin{array}{c}{x}_0-\frac{h}{\sin \left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\≤S_x\≤x_0+\frac{h}{\sin \left(\mathrm{\β}\right)}\·\tan \left(\frac{FOV}{2}\right)\\ y_0+\frac{h}{\tan \left(\mathrm{\β}+\frac{FOV}{2}\right)}\≤S_y\≤y_0+\frac{h}{\tan \left(\mathrm{\β}-\frac{FOV}{2}\right)}\end{array}---\left(16\right)$

(2)控制无人机在原位置作角度为α的自旋；

$\mathrm{\α}=arctan(-\frac{x_b}{y_b})---\left(17\right)$

若α≤0，则逆时针旋转|α|角度，若α＞0，则顺时针旋转|α|；

(3)根据标志物与覆盖范围左边界的距离若Δd≥0，则无人机向右平飞距离为s₁的航程，否则，无人机向左平飞距离为s₂的航程；

$\begin{array}{c}{s}_1=\left|\mathrm{\Δ}d\right|-k_1\\ s_2=\left|\mathrm{\Δ}d\right|-k_2\end{array}---\left(18\right)$

其中，为无人机的覆盖范围S的右边界；k₁设置为GPS的水平定位误差δ，k₂＝-δ；

(4)根据航向偏差的估计精度要求，选择不同的航迹，航向偏差的估计精度从高到低所对应的航迹分别为：绕椭圆飞,绕圆周飞和平飞；若选择绕圆周飞航迹，其半径为R，或选择绕椭圆飞航迹，其短半轴为a，长半轴为b；

对于绕圆周或绕椭圆飞行的航迹，飞行航迹的弧度rad为：1.5π≤rad≤2π；若Δd≥0，若Δd＜0，

4.如权利要求2所述的一种旋翼无人机大范围目标监测和精确定位方法，其特征在于，预估标志物相对于无人机的位置具体计算如下：

利用摄像机的针孔模型得到：

$\left[\begin{array}{c}{x}_f\\ y_f\end{array}\right]=\frac{f}{x_c}\left[\begin{array}{c}{y}_c\\ z_c\end{array}\right]---\left(1\right)$

无人机与标志物的相对高度为：

联立(1)和(2)得到p_c＝[x_c y_c z_c]^T，

标志物相对于无人机的位置为

$p_b=C_{cam}^b{p}_c=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}-\sin \mathrm{\ψ}\cos \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\φ}-\cos \mathrm{\ψ}\sin \mathrm{\φ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]p_c---\left(3\right)$

其中，f为选定摄像机的内参数焦距；θ_c表示选定摄像机坐标系{C}相对于惯性参考坐标系{I}的横滚角、俯仰角；ψ、θ、φ表示选定摄像机坐标系{C}相对于机体坐标系{B}的方位角、俯仰角、横滚角。