CN114740893A - 一种固定翼无人机多次测量定位地面目标地理位置方法 - Google Patents

Abstract

本发明公开了一种固定翼无人机多次测量定位地面目标地理位置方法。固定翼无人机以匀速定高的状态执行期望航线任务；通过固定翼无人机上的云台相机拍摄图像，通过图像实时监测，若目标出现在图像中且不位于图像中心区域中，进入调整状态；当目标出现在图像中且位于图像中心区域中，进入定位状态，根据目标中心点相对于图像中心之间的偏差控制固定翼无人机上的云台相机进行偏转；偏转处理后，云台相机的光轴指向目标，根据云台相机相对于与固定翼无人机在俯仰、偏航两个方向上的夹角，根据这两个角度、固定翼无人机的地理信息和高度信息，结算处理获得目标相对于固定翼无人机的地理位置信息。本发明方法具有原理简单，固定翼无人机飞行任务简单，对户外地面目标多次定位等优点。

Description

一种固定翼无人机多次测量定位地面目标地理位置方法

技术领域

本发明属于固定翼无人机控制技术领域的一种固定翼无人机定位方法，尤其涉及一种固定翼无人机多次测量定位地面目标地理位置方法。

背景技术

在传统的定位方法中，常运用于四旋翼借助机载云台相机结合图像信息对室内目标进行定位，但不适用户外高空长时间对目标的定位。对目标的定位仍然是固定翼无人机最主要的功能之一，利用目标的特征点信息，结合机载传感器的内参和外参，解算目标在空间的位置。由于参数的标定环节极易产生误差，固定翼无人机又处于高速运动状态，导致定位效果较差；加之特征点的信息独立于控制环节之外，无法保证目标始终位于云台相机视野之内，使固定翼无人机无法实现长久鲁棒的跟踪。

发明内容

为解决上述问题，本发明的目的是提供一种固定翼无人机多次测量定位地面目标地理位置方法，利用自身地理位置信息结算地面目标的经纬度信息。

本发明方法具有原理简单，固定翼无人机飞行任务简单，对户外地面目标多次定位等优点。

本发明的技术方案为：

1)固定翼无人机执行期望航线任务，在目标所在区域中飞行，固定翼无人机保持以最小速度匀速定高以期望航线进行飞行，在飞行过程中控制固定翼无人机上安装的云台相机相对于固定翼无人机以固定的初始姿态保持；

2)通过固定翼无人机上安装的云台相机拍摄图像，通过图像实时监测：

若目标未出现在图像中，则不进行处理，继续飞行直到目标出现在图像中；

若目标出现在图像中且位于图像中心区域中，则进行下一步骤；

若目标出现在图像中且不位于图像中心区域中，则控制固定翼无人机进入调整状态S_t1，直到目标出现在图像中且位于图像中心区域中；

所述的目标位于图像中心区域中是以目标的质心是否位于图像中心区域中进行判断。

所述的图像中心区域是以图像中心为原点、以完整图像宽度的四分之三为宽度、以完整图像高度的四分之三为高度的矩形区域。

3)当目标出现在图像中且位于图像中心区域中，控制固定翼无人机进入定位状态S_t2，根据目标相对于图像中心之间的偏差，控制固定翼无人机上的云台相机基于偏转策略和所设模糊控制器进行偏转；

目标中心的坐标为s，与图像中心坐标s*存在偏差，

4)按照步骤3)偏转处理后，目标位于在图像中心区域，云台相机的光轴指向目标，根据云台相机相对于与固定翼无人机在俯仰、偏航两个方向上的夹角，结合固定翼无人机的地理位置信息采用最小二乘法处理获得目标的地理位置信息。

具体实施中，利用GPS设备得到固定翼无人机的地理位置信息。

所述步骤1)中，所述的期望航线由直线和圆弧构成，所述的期望航线是由众多航点组成的折线构成，其中一部分航点预先设置为期望航点。

在这样的期望航线中能有效测定目标的位置信息。

所述步骤1)中，固定翼无人机水平飞行，控制云台相机倾斜朝下朝向地面，云台相机朝向和固定翼无人机飞行的水平面之间呈45度的角度差。

所述的调整状态S_t1中，控制固定翼无人机仅以圆弧航线进行飞行，根据期望目标相对于固定翼无人机方向，控制固定翼无人机以偏航角速率w实时飞行。

通过进入调整状态S_t1能够有效避免目标远离图像中心区域。

所述的调整状态S_t1，固定翼无人机以速度v、半径为r、保持高度h的圆弧进行飞行，且不改变云台相机相对于固定翼无人机的姿态，按照以下公式处理获得固定翼无人机的偏航角速率ω_uav为：

d＝L*sinη₁

其中，d表示固定翼无人机当前位置距离期望航线中最接近的期望航点的水平距离，L表示固定翼无人机的当前位置与期望航线中最接近的期望航点的距离，η₁表示期望偏航角η投影到水平面上的分量，r表示圆弧航线的半径，v_uav表示固定翼无人机的速度，h表示固定翼无人机的飞行高度。

表示固定翼无人机当前位置距离期望航线中最接近的期望航点的水平距离的变化率。

偏航角速率ω_uav是以绕圆弧航线的圆心的角速率。

具体实施，以固定翼无人机当前位置为圆点建立笛卡尔坐标系，将当前位置与最接近的期望航点之间的连线以分量η₁角度分解为水平方向和垂直方向，得到当前位置距离最接近的期望航点的水平距离d，进而再处理获得偏航角速率。

所述的定位状态S_t2中，控制固定翼无人机沿着调整状态S_t1中的圆弧航线的切线以直线航线定高匀速飞行，按照以下公式获得云台相机云台的偏转速度，控制云台相机云台相对于固定翼无人机的姿态进行偏转：

θ＝(w_x，w_y)

其中，θ表示云台相机云台的偏转速度，w_x表示云台相机云台的偏转速度在俯仰方向的分量，w_y表示云台相机云台的偏转速度在偏航方向的分量，

为图像雅可比矩阵，λ_a表示模糊调整增益，s(t)表示t时刻下的目标质心在图像中的坐标，s^*表示图像中心的坐标。

偏转控制是在建立图像坐标系后，根据目标位置s(t)与图像中心s^*的相对位置关系得出偏转方向，利用目标质心的坐标与图像中心坐标之间的位置偏差利用模糊控制器进行设计控制，使目标快速收敛位于到图像中心。

偏转控制是在建立图像坐标系后，获得目标在图像中的位置与图像中心位置的相对关系，由于云台相机的偏航和俯仰运动是相对独立的，再根据偏差ΔX和ΔY的方向性，确定偏航和俯仰的运动方向。

记录当前云台相机相对于固定翼无人机的姿态作为当前姿态，根据当前姿态相比初始姿态在俯仰、偏航两个方向上的夹角，并带入以下公式计算获得在无人机地理坐标系的水平方向上，目标与固定翼无人机的距离A：

其中，(x_i，y_i)表示在不同位置多次测量到同一个目标时无人机的地理坐标，Δ_i是固定翼无人机视线光轴指向目标时的测量误差，p_i表示云台相机在偏航方向上与固定翼无人机的夹角，q_i表示云台相机在俯仰方向上与固定翼无人机的夹角，i表示固定翼无人机在第i个地理位置测量到同一个目标的序号，i＝1，2，3...，h表示固定翼无人机的飞行高度；

当A不等于R，则是目标未出现在固定翼无人机的相机图像范围内的情况，直到目标出现在固定翼无人机的相机图像范围内再处理。

当A＝R，则认为固定翼无人机的地理坐标是落在在以目标(X，Y)为圆心、半径为R的圆上，R表示期望圆半径，建立以下公式利用最小二乘法求解处理获得目标在地面上的位置(X，Y)：

δ_i＝(x_i-X)²+(y_i-Y)²-R²

其中，δ_i表示无人机在第i个地理位置与目标在地面上的位置(X，Y)的距离平方与拟合圆半径平方的差；

利用最小二乘法当δ_i取极小值时得到目标在地面上的位置(X，Y)，按照以下公式转换到大地坐标系下获得经纬度：

其中，L_{固定翼无人机}、B_{固定翼无人机}分别是固定翼无人机的纬度、经度，L_目标、B_目标分别是目标的纬度、经度，RE表示地球的曲率。

具体实施中，在不同位置多次测量同一个目标，将云台相机的光轴投影到水平方向，投影相交于目标点，利用多次测量的数据进行输入处理，采用最小二乘法进行求解处理获得目标在无人机地理坐标系的坐标(X，Y)，最后转化到大地坐标系中，得到目标的坐标(L_目标，B_目标)。

本发明根据图像的变化速率分别对云台相机、固定翼无人机进行控制，可以对地面目标进行多次测量，利用最小二乘法处理数据，提高定位精度。

本发明利用视觉伺服和模糊控制控制云台，使目标始终保持在图像中心范围，结合飞机位置参数和云台指向目标角度参数，计算目标在大地地理坐标系的经纬度。

本发明由于采用以上技术方案，使其与现有技术相比具有以下的优点和积极效果：

本发明构建直线和圆弧组成的航线任务，简化固定翼无人机定位的航线任务，相比理想圆的定位航向任务，分别控制固定翼无人机和云台相机，减少整个定位任务过程中的控制量，提高目标锁定效率；

本发明利用视觉伺服结合模糊控制调整来快速控制云台，通过在不同位置利用多次测量的云台相机光轴与目标交会时，固定翼无人机和目标的相对位置关系，最小二乘法结算目标的地理位置信息，提高定位的精确度。

附图说明

图1为固定翼无人机航线任务图；

图2为云台相机图像分区图；

图3为结算偏航角速率w原理图

图4为固定翼无人机多次测量光轴交会定位目标原理图；

图5为任务流程图。

具体实施方式

下面结合说明书附图，对本发明提出的一种固定翼无人机多次测量定位地面目标地理位置方法进一步详细说明。

如图5所示，本发明的实施例及其实施过程如下：

设计规划一种固定翼无人机多次测量定位地面目标地理位置的期望航线任务如图1，航线任务由圆弧相切于直线构成，能有效观测到目标信息，通过GPS、气压计等传感器获取纬经度信息(L_{固定翼无人机}，B_{固定翼无人机})和高度信息h，固定翼无人机保持以最小速度匀速定高飞行，在俯仰方向上云台相机与固定翼无人机保持45度的角度差，搜寻地面目标。建立以云台相机采集的图像大小800*600像素的图像坐标系，其图像中心坐标p₀(x₀，y₀)＝(400，300)，横轴l最大值为800，纵轴w最大值为600。

具体实施中，构建图像坐标系、云台相机坐标系、机体坐标系和固定翼无人机地理坐标系的系统坐标系，云台相机图像坐标系是以云台相机视野图像左上顶点作为原点，其中心位置即云台相机瞄准线所在位置p₀(x₀，y₀)＝(400，300)，x轴和y轴分别为横轴和纵轴，固定翼无人机云台坐标系定义的原点以图像中心一致位于云台相机光轴，y轴沿着云台相机光轴朝向上，z轴垂直云台相机光轴，x轴以右手准则构建，则云台中心和图像中心一致，则认为目标的坐标在云台坐标系中与在相机图像坐标保持一致，表示为(x₀，y₀，0)。目标锁定时，以云台相机平行于固定翼无人机机头时俯仰角为0°，前倾为正，后倾为负；以镜头光轴指向固定翼无人机机头时为0°，顺时针为正，逆时针为负，云台镜头的俯仰角为α，偏航角为β，此时有云台坐标系g到固定翼无人机机体坐标系b的关系

可得到目标相对于无人机在无人机机体坐标系的坐标[X_b Y_b Z_b]：

固定翼无人机地理坐标系n到无人机体坐标系b的转换矩阵

如下：

其中

θ，γ是固定翼无人机的航向角、俯仰角以及横滚角。

在无人机地理坐标系下，目标坐标表示为：

[X Y Z]表示目标在地理坐标系中坐标的具体数值，

表示固定翼无人机地理坐标系到机体坐标系的转换矩阵，[X_b Y_b Z_b]表示目标相对于无人机在无人机机体坐标系的位置。

具体实施中，目标偏离图像中心程度较重的评判标准是如图2，将以图像中心构建宽为3/4l，长为3/4w的矩形区域范围，出现在这四个区域以外的阴影区域认为图像偏离程度较重。图像中I II III IV分别代表图像区域的四个象限。偏转策略如下：当目标在I区域时，说明固定翼无人机运动期望位置在中心区域的右上方；当目标位于II区域时，说明固定翼无人机运动期望位置位于中心区域的左上方；当目标处于III区域时，表明固定翼无人机运动期望位置在中心区域的左下方；当目标处于IV区域时，说明固定翼无人机运动期望位置处于中心区域的右下方。目标出现在云台相机图像中，但偏离图像中心程度较重，固定翼无人机期望运动为圆弧，固定翼无人机往期望参考点进行偏转，读取此时固定翼无人机与云台相机的垂直分量上的夹角q_i，得到期望圆的半径：

固定翼无人机当前位置与期望航点的距离为L，期望偏航角为η，设η由η₁，η₂，η₃三个合角形成，如图3，按照以下公式处理获得固定翼无人机的偏航角速率ω_uav为：

d＝L*sinη₁

其中，d表示固定翼无人机当前位置距离期望航线中最接近的期望航点的水平距离，L表示固定翼无人机的当前位置与期望航线中最接近的期望航点的距离，η₁表示期望偏航角η投影到水平面上的分量，r表示圆弧航线的半径，v_uav表示固定翼无人机的速度，h表示固定翼无人机的飞行高度。

表示固定翼无人机当前位置距离期望航线中最接近的期望航点的水平距离的变化率。

具体实施中，整个过程进入定位状态S_t2，目标中心出现在图像中心分区，目标当前状态的像素坐标设定为s(t)，与期望在图像中心坐标s^*存在偏差，则偏差可表示为：

e(t)＝s(t)-s^*

其中，e(t)表示目标当前状态像素坐标与图像中心坐标的偏差；

相关目标中心点的变化速度和控制云台在俯仰和偏航变化速度

的变换关系为：

其中，

表示云台在俯仰和偏航的变化速度，v表示目标中心点在x轴方向和y轴方向的变化速度，L_s表示云台在俯仰和偏航上的变换速度和目标中心点变化之间的转换关系；

偏差e(t)关于时间的导数和相关目标中心点速度之间的关系为：

其中，

表示表示目标当前状态像素坐标与图像中心坐标的偏差关于时间的导数，L_e表示偏差的导数与相关目标中心点变化速度的转换关系；

得出云台的速度控制方案可以表示为：

θ＝(w_x，w_y)

其中θ＝(w_x，w_y)，w_x表示控制云台在俯仰方向的速度，w_y表示控制云台在偏航方向的速度，

表示为L_e的近似值。

因为实际情况中不能直接得到L_e的准确值。设计一个基于模糊规则的增益调节控制器，主要考虑误差||e||(目标中心点当前像素坐标与图像中心像素坐标之间的差值)和d||e||/dt(像素误差的微分)的变化。||e||、d||e||/dt和λ的模糊描述有三种分别为S(小)，M(中)和B(大)。模糊逻辑的推理方法为mamdani推理法，输出函数为高斯隶属函数，回归类型为maximum，去模糊化使用的是面积的重心(CoA)，表示为输出隶属函数u_i(λ_a)的重心的加权平均值。设w_i为第i条规则隶属函数的加权因子：

w_i＝max((u_i ^||e||(||e||)，u_k(d||e||/dt))

其中，(u_i ^||e||(||e||)，u_k(d||e||/dt)分别表示误差||e||和d||e||/dt的隶属度函数，u_i(λ_a)表示为模糊输出隶属函数，增益λ_a通过模糊规则来调整，并通过限制所有输入和输出以获得有限λ_a。e表示目标中心点当前像素坐标与图像中心像素坐标的偏差，||e||表示目标中心点当前像素坐标与图像中心像素坐标之间的差值，u_i(λ_a)表示误差||e||在第i个的规则下、增益为λ_a的模糊输出隶属函数；u_k表示在误差的微分在第k条规则下的的输出隶属函数；d||e||/dt表示对像素误差的微分；λ_a表示增益；w_i表示第i条规则隶属函数的加权因子，max()表示取其中的最大值；

||e||、d||e||/dt为模糊逻辑的输入，需归一化处理，通过模糊控制使像素误差不断减小并最终收敛到期望位置。云台相机锁定目标，目标出现中心区域，读取云台相机与固定翼无人机在偏航和俯仰上的夹角。

具体实施中，在不同地理位置，目标出现在图像中心，云台相机视线光轴指向目标，获取固定翼无人机的地理位置信息、记录当前云台相机相对于固定翼无人机的姿态作为当前姿态，根据当前姿态相比初始姿态在俯仰、偏航两个方向上的夹角，并带入以下式子：

其中，(x_i，y_i)表示在不同位置多次测量到同一个目标时无人机的地理坐标，Δ_i是固定翼无人机视线光轴指向目标时的测量误差，p_i表示云台相机在偏航方向上与固定翼无人机的夹角，q_i表示云台相机在俯仰方向上与固定翼无人机的夹角，i表示固定翼无人机在第i个地理位置测量到同一个目标的序号，i＝1，2，3...，h表示固定翼无人机的飞行高度；

当A不等于R，则是目标未出现在固定翼无人机的相机图像范围内的情况，直到目标出现在固定翼无人机的相机图像范围内再处理。

当A＝R，认为固定翼无人机的地理坐标是落在以目标(X，Y)为圆心，半径为R的圆上，R表示期望圆半径，p_i表示云台在偏航方向上与固定翼无人机的夹角，q_i表示云台在俯仰上与固定翼无人机的夹角。Δ_i是固定翼无人机视线光轴指向目标时的测量误差，如图4所示，将各点的固定翼无人机地理位置(x_i，y_i)带入：

(x_i-X)²+(y_i-Y)²＝R²

δ_i＝(x_i-X)²+(y_i-Y)²-R²＝x_i ²+y_i ²+ax_i+by_i+c

其中，δ_i表示无人机在第i个地理位置与目标地理位置(X，Y)的距离平方与拟合圆半径平方的差。

当δ_i的值趋近0时，则拟合目标的地理位置越准确。令Q(a，b，c)＝∑δ_i ²，对abc三个参数进行求导，满足以下情况时：

拟合出目标(X，Y)的地理位置的值：

转换成在大地坐标系中目标坐标表示为：

其中，L_{固定翼无人机}、B_{固定翼无人机}分别是固定翼无人机的纬度、经度，L_目标、B_目标分别是目标的纬度、经度，RE表示地球的曲率。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式。即使对本发明作出各种变化，倘若这些变化属于本发明权利要求及其等同技术的范围之内，则仍落入在本发明的保护范围之中。

Claims (7)

1.一种固定翼无人机多次测量定位地面目标地理位置方法，其特征在于，方法包括以下步骤：

1)固定翼无人机执行期望航线任务，在目标所在区域中飞行，固定翼无人机保持以最小速度匀速定高以期望航线进行飞行，在飞行过程中控制固定翼无人机上安装的云台相机相对于固定翼无人机以固定的姿态保持；

2)通过固定翼无人机上安装的云台相机拍摄图像，通过图像实时监测：

若目标出现在图像中且位于图像中心区域中，则进行下一步骤；

若目标出现在图像中且不位于图像中心区域中，则控制固定翼无人机进入调整状态S_t1，直到目标出现在图像中且位于图像中心区域中；

3)控制固定翼无人机进入定位状态S_t2，根据目标相对于图像中心之间的偏差，控制固定翼无人机上的云台相机进行偏转；

4)按照步骤3)偏转处理后，云台相机的光轴指向目标，根据云台相机相对于与固定翼无人机在俯仰、偏航两个方向上的夹角，结合固定翼无人机的地理位置信息处理获得目标的地理位置信息。

2.根据权利要求1所述的一种固定翼无人机多次测量定位地面目标地理位置方法，其特征为：

所述步骤1)中，所述的期望航线由直线和圆弧构成，所述的期望航线是由众多航点组成的折线构成。

3.根据权利要求1所述的一种固定翼无人机多次测量定位地面目标地理位置方法，其特征为：

所述步骤1)中，固定翼无人机水平飞行，控制云台相机倾斜朝下朝向地面，云台相机朝向和固定翼无人机飞行的水平面之间呈45度的角度差。

4.根据权利要求1所述的一种固定翼无人机多次测量定位地面目标地理位置方法，其特征为：

所述的调整状态S_t1中，控制固定翼无人机仅以圆弧航线进行飞行，根据期望目标相对于固定翼无人机方向，控制固定翼无人机以偏航角速率w实时飞行。

5.根据权利要求4所述的一种固定翼无人机多次测量定位地面目标地理位置方法，其特征为：

所述的调整状态S_t1，固定翼无人机以速度v、半径为r、保持高度h的圆弧进行飞行，且不改变云台相机相对于固定翼无人机的姿态，按照以下公式处理获得固定翼无人机的偏航角速率ω_uav为：

d＝L*sinη₁

其中，d表示固定翼无人机当前位置距离期望航线中最接近的期望航点的水平距离，L表示固定翼无人机的当前位置与期望航线中最接近的期望航点的距离，η₁表示期望偏航角η投影到水平面上的分量，r表示圆弧航线的半径，v_uav表示固定翼无人机的速度，h表示固定翼无人机的飞行高度，

表示固定翼无人机当前位置距离期望航线中最接近的期望航点的水平距离的变化率。

6.根据权利要求1所述的一种固定翼无人机多次测量定位地面目标地理位置方法，其特征为：

所述的定位状态S_t2中，控制固定翼无人机沿着调整状态S_t1中的圆弧航线的切线以直线航线定高匀速飞行，按照以下公式获得云台相机云台的偏转速度，控制云台相机云台相对于固定翼无人机的姿态进行偏转：

θ＝(w_x，w_y)

其中，θ表示云台相机云台的偏转速度，w_x表示云台相机云台的偏转速度在俯仰方向的分量，w_y表示云台相机云台的偏转速度在偏航方向的分量，

为图像雅可比矩阵，λ_a表示模糊调整增益，s(t)表示t时刻下的目标质心在图像中的坐标，s^*表示图像中心的坐标。

7.根据权利要求1所述的一种固定翼无人机多次测量定位地面目标地理位置方法，其特征为：

记录当前云台相机相对于固定翼无人机的姿态作为当前姿态，根据当前姿态相比初始姿态在俯仰、偏航两个方向上的夹角，并带入以下公式计算获得在无人机地理坐标系的水平方向上，目标与固定翼无人机的距离A：

其中，(x_i,y_i)表示在不同位置多次测量到同一个目标时无人机的地理坐标,Δ_i是固定翼无人机视线光轴指向目标时的测量误差，p_i表示云台相机在偏航方向上与固定翼无人机的夹角，q_i表示云台相机在俯仰方向上与固定翼无人机的夹角，i表示固定翼无人机在第i个地理位置测量到同一个目标的序号，i＝1,2,3…，h表示固定翼无人机的飞行高度；

当A＝R，则认为固定翼无人机的地理坐标是落在在以目标(X,Y)为圆心、半径为R的圆上，R表示期望圆半径，建立以下公式利用最小二乘法求解处理获得目标在地面上的位置(X，Y)：

δ_i＝(x_i-X)²+(y_i-Y)²-R²

其中，δ_i表示无人机在第i个地理位置与目标在地面上的位置(X，Y)的距离平方与拟合圆半径平方的差；

利用最小二乘法当δ_i取极小值时得到目标在地面上的位置(X，Y)，按照以下公式转换到大地坐标系下获得经纬度：

其中，L_{固定翼无人机}、B_{固定翼无人机}分别是固定翼无人机的纬度、经度，L_目标、B_目标分别是目标的纬度、经度，RE表示地球的曲率。

Images