CN105353772A

CN105353772A - 一种无人机机动目标定位跟踪中的视觉伺服控制方法

Info

Publication number: CN105353772A
Application number: CN201510785505.8A
Authority: CN
Inventors: 王立鹏; 刘培强; 周英伟; 韩松; 樊鹏辉; 王小刚; 杨超峰
Original assignee: China Academy of Aerospace Electronics Technology Co Ltd
Current assignee: Aerospace Age Feihong Technology Co., Ltd.
Priority date: 2015-11-16
Filing date: 2015-11-16
Publication date: 2016-02-24
Anticipated expiration: 2035-11-16
Also published as: CN105353772B

Abstract

本发明属于无人机飞行控制与机器视觉交叉融合的技术领域，具体涉及一种无人机机动目标定位跟踪中的视觉伺服控制方法。所述方法建立大地坐标系、机体坐标系、摄像机坐标系、图像坐标系、机体大地过渡坐标系，通过上述建立的坐标系之间的关系，根据目标的成像序列，进行目标的定位及目标跟踪的姿态角给定值和航线跟踪的姿态角给定值的计算，完成视觉伺服控制。本发明仅需使用单一固定相机对目标成像，通过控制无人机位姿，实现对机动目标的高精度连续定位跟踪，该方法无需跟踪云台和激光测距设备，有效地降低了载荷的体积和成本，提高了侦察的隐蔽性。

Description

一种无人机机动目标定位跟踪中的视觉伺服控制方法

技术领域

本发明属于无人机飞行控制与机器视觉交叉融合的技术领域，具体涉及一种无人机机动目标定位跟踪中的视觉伺服控制方法。

背景技术

与有人机相比，无人机具有体积小、造价低、机动灵活、使用方便和对环境条件要求较低等诸多优点。从无人机诞生之日起，它就随着科学技术水平的不断提高而不断进步，并已逐步被广泛应用于军用、民用、警用等众多领域，所执行的任务包括：目标侦察、跟踪监视、目标打击、毁伤评估、抢险救灾、人员搜救、地形勘察等。无人机在人们的日常生活中的功能、价值已崭露头角；在现代化战争中的作用、地位正逐步提高，已成为不可或缺的战术武器，凭借战场零伤亡的独特优势，其成为未来战场的重要航空武器已毋庸置疑。因此近年来，国内外无人机技术的发展势头十分迅猛。

机动目标的定位和跟踪是无人机侦察跟踪的重要功能之一。目标定位就是通过无人机飞行参数和载荷信息，计算出目标点在大地坐标系下的坐标值；目标跟踪则是根据定位信息，通过设计控制策略将机动目标始终锁定在无人机载荷视场之中，并计算得到目标的运动信息。利用图像数据进行目标识别的技术已经日趋成熟，而如何利用目标识别的定位结果设计伺服控制策略进行有效的目标跟踪仍是国内外无人机研究的热点和难点。

由于小型和微小型无人机载荷能力有限，侦察任务中一般使用无云台摄像机进行成像侦察，摄像机本身没有跟踪能力，若要实现目标跟踪功能，需要利用图像信息设计控制算法，控制无人机的位置和姿态将目标锁定在图像中心区域，实现基于视觉伺服的机动目标的定位和跟踪。

本发明方法可在军事行动中，实现目标精确定位、监视跟踪和精确打击等任务；在多机协作中，有利于协同完成任务；在灾难救援中，实现伤员快速救治等。因此，机动目标精确定位跟踪有着重要的研究意义和广阔的应用前景。

发明内容

本发明提出了一种无人机机动目标定位跟踪中的视觉伺服控制方法，通过对无人机进行视觉伺服控制，将目标锁定在摄像机视场之中，达到精确目标定位和跟踪效果。

本发明的具体技术方案如下：第一步、确定无人机、GPS、惯性测量单元、摄像机等设备状态，确定摄像机内参数和安装角度；定义光轴上单位向量i，由坐标系关系得到单位向量在大地坐标系{G}下的表示^GP_i，由飞机高度和共线成像原理求的光轴与地面交点A在{G}下的表示^GP_A，进而求得A在机体坐标系下的表示^BP_A；

第二步、根据侦察成像选择目标点，由目标点t在图像中的坐标值(^Ix_t,^Iy_t)和摄像机内参数，求出目标点在摄像机坐标系{C}下的坐标值^CP_t，结合摄像机安装位置、角度、飞机当前经度、纬度、高度和姿态角，求出目标点在大地坐标系{G}的坐标^GP_t，再由地球模型解算得到目标点经纬度，实现目标精确定位；

第三步、结合上次目标定位结果，根据时间间隔、飞机的飞行信息，得到目标当前的运动状态，并估计机动目标下一时刻位置

第四步、以图像中心^IP_A为机动目标在图像中的期望位置^IP_tr，则目标在{G}下的期望位置^GP_tr为^GP_A，根据误差Δ^GP_t＝^GP_tr-^GP_t方程求出姿态角给定值θ_t、φ_t和ψ_t；

第五步、由飞行航迹中飞机期望位置和飞机当前位置^GP_BO的误差Δ^GP_BO，根据横侧向控制律和纵向控制律分别求出姿态角给定值φ_r和θ_r。结合任务权重和第四步中姿态角给定值，得到各姿态角总给定量，以此伺服控制无人机各相关舵面，实现目标定位跟踪过程中稳定裕度最大化的视觉伺服控制。

有益效果：本发明仅需使用单一固定相机对目标成像，通过控制无人机位姿，实现对机动目标的高精度连续定位跟踪，该方法无需跟踪云台和激光测距设备，有效地降低了载荷的体积和成本，提高了侦察的隐蔽性。该方法普遍适用于无人机的目标定位跟踪***，尤其是载有固定摄像机的小型无人机和云台运动失效的无人机***。该方法对无人机(和其他类似执行机构)高精度目标定位跟踪具有重要的实际意义和有广阔的应用前景，对定位新方法的开辟有一定的引导作用。

附图说明

图1是无人机目标定位跟踪方法涉及的装置连接示意图；

图2是机动目标定位跟踪过程中所用各坐标系；

图3是地球模型下经纬度与坐标系表示关系示意图；

图4是无人机航迹飞行下动目标定位跟踪过程示意图。

具体实施方式

下面结合附图，对本发明提出的一种基于视觉伺服的无人机机动目标定位跟踪方法具体实施方式进行详细说明。

1无人机平台及坐标系定义

以侦察用无人机***为例，本发明所涉及无人机平台包括飞行器子***、飞行控制计算机、图像处理计算机、摄像机等部分，飞行器子***包含有惯性测量单元和GPS等设备，用来测量无人机姿态和位置。主要装置连接示意图如图1所示。

机动目标定位跟踪过程中所用各坐标系如图2所示，各坐标系定义如下：

大地坐标系{G}：以无人机开始进行目标定位跟踪时刻，重心在地面的投影作为全局固定不变的大地坐标系原点，X轴平行于经线，Y轴平行于纬线，Z轴垂直于XOY平面指向地心。

机体坐标系{B}：以无人机重心为原点，固连在无人机上，X轴在无人机对称平面内平行于设计轴线指向机头，Y轴垂直于无人机对称平面指向机体右侧，Z轴在对称平面内垂直于X轴指向机身下方。

摄像机坐标系{C}：以摄像机光心为原点，X轴与Y轴分别平行于图像的纵向和横向直线，Z轴为摄像机光轴，与图像平面垂直。定义摄像机光轴与{B}的XOY平面夹角为高低角α，光轴在平面之上为正；光轴与{B}的XOZ平面夹角为方位角β，光轴在平面右侧为正。

图像坐标系{I}：以图像左上角点为原点二维坐标系，X轴平行于图像最左侧纵向像素连线，Y轴平行于横向像素连线，坐标值x和y分别表示像素的行数和列数。

机体大地过渡坐标系{GB}：以无人机重心为原点，X、Y、Z各轴平行于大地坐标系{G}。

无人机侦察飞行过程中，任意时刻机体坐标系{B}相对于大地坐标系{G}的位姿表示，由飞机的经度、纬度、高度和姿态角唯一决定。根据大地坐标系{G}和机体坐标系{B}的经度、纬度和高度，由图3所示地球模型可解得{B}原点在{G}下的坐标^GP_BO＝(^Gx_BO,^Gy_BO,^Gz_BO)^T，结合机体的俯仰角θ、偏航角ψ、滚转角φ可得{B}在{G}下的表示和{B}在{G}下的表示

T_{B}^{G} = [\begin{matrix} c θ c ψ & s θ c ψ s φ - s ψ c φ & s θ c ψ c φ + s ψ s φ & {x_{G}}_{B O} \\ c θ s ψ & s θ s ψ s φ + c ψ c φ & s θ s ψ c φ - c ψ s φ & {y_{G}}_{B O} \\ - s θ & c θ s φ & c θ c φ & {z_{G}}_{B O} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (1)

T_{G}^{B} = [\begin{matrix} c θ c ψ & c θ s ψ & - s θ & - {x_{G}}_{B O} \\ s θ c ψ s φ - s ψ c φ & s θ s ψ s φ + c ψ c φ & c θ s φ & - {y_{G}}_{B O} \\ s θ c ψ c φ + s ψ s φ & s θ s ψ c φ - c ψ s φ & c θ c φ & - {z_{G}}_{B O} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (2)

由坐标系定义可以得出，{B}在{GB}下的表示和{B}在{GB}下的表示为：

T_{B}^{G B} = [\begin{matrix} c θ c ψ & s θ c ψ s φ - s ψ c φ & s θ c ψ c φ + s ψ s φ & 0 \\ c θ s ψ & s θ c ψ s φ + c ψ c φ & s θ c ψ c φ - c ψ s φ & 0 \\ - s θ & c θ s φ & c θ c φ & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (3)

T_{G B}^{B} = [\begin{matrix} c θ c ψ & c θ s ψ & - s θ & 0 \\ s θ c ψ s φ - s ψ c φ & s θ s ψ s φ + c ψ c φ & c θ s φ & 0 \\ s θ c ψ c φ + s ψ s φ & s θ s ψ c φ - c ψ s φ & c θ c φ & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (4)

摄像机坐标系{C}相对于{B}的位姿由摄像机固定位置^BP_CO＝(^Bx_CO,^By_CO,^Bz_CO)^T、当前的高低角α和方位角β决定，当安装角度固定后，{C}在{B}下的表示固定不变：

T_{C}^{B} = [\begin{matrix} c α c β & - s β & s α c β & {x_{B}}_{C O} \\ c α s β & c β & s α s β & {y_{B}}_{C O} \\ - s α & 0 & c α & {z_{B}}_{C O} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (5)

图像坐标系与摄像机坐标系关系由摄像机内参数确定，摄像机内参数包括图像中心坐标^IP_Icent＝(^Ix_Icent,^Iy_Icent,1)^T、焦距f、比例因子λ等。由中心透视投影模型可知，图像中心像素坐标在摄像机坐标系{C}下坐标表示为^CP_Icent＝(0,0,f)^T，则{I}在{C}下的表示为：

T_{I}^{C} = [\begin{matrix} λ & 0 & 0 & - λ^{I} x_{I c e n t} \\ 0 & λ & 0 & - λ^{I} y_{I c e n t} \\ 0 & 0 & 0 & f \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (6)

根据上述各坐标系之间的关系，可以方便地求出某一坐标系下任意点在其他坐标系的坐标表示。

2机动目标精确定位

当无人机飞至侦察区域后，打开载荷摄像机开始进行侦察，记光轴与地面的交点为A。根据上文所述各坐标系之间的关系，定义摄像机光轴线上单位向量i，则其在摄像机坐标系{C}下的齐次坐标为^CP_i＝(0,0,1,1)^T，根据摄像机的位姿由公式(5)，可得机体坐标系{B}下的表示^BP_i：

{P_{B}}_{i} = T_{C}^{B} \cdot {P_{C}}_{i} - - - (7)

根据飞机的位姿由公式(1)，求得大地坐标系{G}下的表示^GP_i：

{P_{G}}_{i} = T_{B}^{G} \cdot {P_{B}}_{i} = T_{B}^{G} \cdot T_{C}^{B} \cdot {P_{C}}_{i} - - - (8)

根据透视成像原理和成像三角形关系，由飞机高度计算得到光轴与地面交点A在{G}下的坐标^GP_A。再根据公式(2)求出^BP_A，后面计算中使用。(由地球模型和飞机当前经纬度，由^BP_A可得A点经度和纬度。大地坐标系的原点可以根据此方法取某一时刻平台信息计算得到，也可取已知点。)

无人机发现目标后，根据侦察任务选择感兴趣目标点，由所选目标点t在图像坐标系{I}下的齐次坐标^IP_t＝(^Ix_t,^Iy_t,1,1)^T，根据公式(6)得出^CP_t，继而可依次求出^BP_t、^GP_t：

{P_{G}}_{t} = T_{B}^{G} \cdot {P_{B}}_{t} = T_{B}^{G} \cdot T_{C}^{B} \cdot {P_{C}}_{t} = T_{B}^{G} \cdot T_{C}^{B} \cdot T_{I}^{C} \cdot {P_{I}}_{t} - - - (9)

由地球模型和经纬度关系，求出目标点t的经纬高信息，实现目标精确定位。对于机动目标来说，该方法可以根据两次定位状态估计目标运动信息。通常图像处理计算机处理速度非常快，根据两次目标定位的时间间隔、飞机的飞行信息，得到目标当前的运动状态，并估计机动目标下一时刻位置从而在机动目标打击中提高***命中率。

3无人机视觉伺服控制下的机动目标跟踪

无人机自主侦察飞行过程中，希望保持目标点在图像的中心位置，以获得最大的稳定裕度。对于有云台的侦察载荷来说，当给出无人机侦察航线或航迹规划后，跟踪定位过程可以依靠云台的伺服控制锁定目标在图像中心位置。当载荷云台运动失效或没有云台时，为实现稳定裕度最大化，可以通过控制无人机姿态来实现，具体如下：

根据式(7)和(8)的推导过程，结合式(3)和前面所求^BP_A，可分别求得^GP_t和^GBP_tr＝^GP_A，则误差为Δ^GP_t＝^GP_tr-^GP_t，由此方程组求出姿态角给定值θ_t、φ_t和ψ_t；

由飞行航迹中飞机期望位置和飞机当前位置^GP_BO的误差根据横侧向控制律求出滚转角给定值φ_r

φ_{r} = K_{φ} ({\tilde{y}}_{G}_{B O} - {y_{G}}_{B O}) + {K_{d φ}}^{G} {\overset{\cdot}{y}}_{B O} - - - (10)

根据纵向控制律分别求出俯仰角给定值θ_r

θ_{r} = K_{θ} ({\tilde{z}}_{G}_{B O} - {z_{G}}_{B O}) - - - (11)

结合任务权重λ_m分配目标跟踪和航线跟踪的姿态角给定值，得到总输出

θ＝λ_mθ_t+(1-λ_m)φ_r

φ＝λ_mφ_t+(1-λ_m)φ_r(12)

ψ＝ψ_t

以此作为飞行控制的内环给定，伺服控制无人机各相关舵面，实现目标定位跟踪过程中稳定裕度最大化的视觉伺服控制。

Claims

1.一种无人机机动目标定位跟踪中的视觉伺服控制方法，其特征在于，所述方法应用无人机进行视觉伺服控制，将目标锁定在摄像机视场之中。

2.根据权利要求1所述的一种无人机机动目标定位跟踪中的视觉伺服控制方法，其特征在于，所述方法建立大地坐标系、机体坐标系、摄像机坐标系、图像坐标系、机体大地过渡坐标系，通过上述建立的坐标系之间的关系，进行目标的定位及目标跟踪的姿态角给定值和航线跟踪的姿态角给定值的计算，完成视觉伺服控制。

3.根据权利要求2所述的一种无人机机动目标定位跟踪中的视觉伺服控制方法，其特征在于，所述方法包括以下步骤：

A)确定无人机上定位装置、惯性测量单元及摄像机的设备状态及设备参数，所述摄像机的设备参数包括摄像机内参数及安装角度，根据摄像机内参数及安装角度确定光轴指向，由各坐标系间相互关系确定光轴与地面交点在无人机机体坐标系上的位置表示；

B)根据摄像机的侦察成像，选择目标点，确定目标点在摄像机坐标系中的坐标位置，再结合摄像机的设备参数得出目标点在地球模型上的定位位置；

C)根据定位结果，结合无人机的飞行信息，得到目标点当前的运动状态，并预估目标点下一刻的位置；

D)将图像中心作为目标点的期望位置，根据误差计算出目标跟踪的姿态角给定值；

E)由无人机航迹中飞机期望位置和飞机当前位置的误差，根据横侧向控制律和纵向控制律分别求出航线跟踪的姿态角给定值，根据目标跟踪的姿态角给定值及航线跟踪的姿态角给定值，得到各姿态角总给定量，以此伺服控制无人机的相关舵面，实现目标定位跟踪过程中稳定裕度最大化的视觉伺服控制。

4.根据权利要求3所述的一种无人机机动目标定位跟踪中的视觉伺服控制方法，其特征在于，所述大地坐标系{G}：以无人机开始进行目标定位跟踪时刻，重心在地面的投影作为全局固定不变的大地坐标系原点，X轴平行于经线，Y轴平行于纬线，Z轴垂直于XOY平面指向地心；

机体坐标系{B}：以无人机重心为原点，固连在无人机上，X轴在无人机对称平面内平行于设计轴线指向机头，Y轴垂直于无人机对称平面指向机体右侧，Z轴在对称平面内垂直于X轴指向机身下方；

摄像机坐标系{C}：以摄像机光心为原点，X轴与Y轴分别平行于图像的纵向和横向直线，Z轴为摄像机光轴，与图像平面垂直；定义摄像机光轴与{B}的XOY平面夹角为高低角α，光轴在平面之上为正；光轴与{B}的XOZ平面夹角为方位角β，光轴在平面右侧为正；

图像坐标系{I}：以图像左上角点为原点二维坐标系，X轴平行于图像最左侧纵向像素连线，Y轴平行于横向像素连线，坐标值x和y分别表示像素的行数和列数；

5.根据权利要求4所述的一种无人机机动目标定位跟踪中的视觉伺服控制方法，其特征在于，所述坐标系之间的关系包括：

根据定位信息和地球模型得到{B}原点在{G}下的坐标^GP_BO＝(^Gx_BO,^Gy_BO,^Gz_BO)^T，结合机体的俯仰角θ、偏航角ψ、滚转角φ得到{B}在{G}下的表示和{B}在{G}下的表示

T_{B}^{G} = [\begin{matrix} c θ c ψ & s θ c ψ s φ - c ψ c φ & s θ c ψ c φ + s ψ s φ & {x_{G}}_{B O} \\ c θ s ψ & s θ s ψ s φ + c ψ c φ & s θ s ψ c φ - c ψ s φ & {y_{G}}_{B O} \\ - s θ & c θ s ψ & c θ c φ & {z_{G}}_{B O} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (1)

T_{G}^{B} = [\begin{matrix} c θ c ψ & c θ s ψ & - s θ & - {x_{G}}_{B O} \\ s θ c ψ s φ - c ψ c φ & s θ s ψ s φ + c ψ c φ & c θ s φ & - {y_{G}}_{B O} \\ s θ c ψ c φ + s ψ s φ & s θ s ψ c φ - c ψ s φ & c θ c φ & - {z_{G}}_{B O} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (2)

由坐标系定义得出，{B}在{GB}下的表示和{B}在{GB}下的表示为：

T_{B}^{G B} = [\begin{matrix} c θ c ψ & s θ c ψ s φ - c ψ c φ & s θ c ψ c φ + s ψ s φ & 0 \\ c θ s ψ & s θ s ψ s φ + c ψ c φ & s θ s ψ c φ - c ψ s φ & 0 \\ - s θ & c θ s φ & c θ c φ & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (3)

T_{G B}^{B} = [\begin{matrix} c θ c ψ & c θ s ψ & - s θ & 0 \\ s θ c ψ s φ - c ψ c φ & s θ s ψ s φ + c ψ c φ & c θ s φ & 0 \\ s θ c ψ c φ + s ψ s φ & s θ s ψ c φ - c ψ s φ & c θ c φ & 0 \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (4)

摄像机坐标系{C}相对于{B}的位姿由摄像机固定位置^BP_CO＝(^Bx_CO,^By_CO,^Bz_CO)^T、高低角α和方位角β决定，{C}在{B}下的表示为：

T_{C}^{B} = [\begin{matrix} c α c β & - s β & s α c β & {x_{B}}_{C O} \\ c α s β & c β & s α s β & {y_{B}}_{C O} \\ - s α & 0 & c α & {z_{B}}_{C O} \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (5)

图像坐标系与摄像机坐标系关系由摄像机内参数确定，摄像机内参数包括图像中心坐标^IP_Icent＝(^Ix_Icent,^Iy_Icent,1)^T、焦距f、比例因子λ；由中心透视投影模型可知，图像中心像素坐标在摄像机坐标系{C}下坐标表示为^CP_Icent＝(0,0,f)^T，则{I}在{C}下的表示为：

T_{I}^{C} = [\begin{matrix} λ & 0 & 0 & - λ^{I} x_{I c e n t} \\ 0 & λ & 0 & - λ^{I} y_{I c e n t} \\ 0 & 0 & 0 & f \\ 0 & 0 & 0 & 1 \end{matrix}] - - - (6) .

6.根据权利要求5所述的一种无人机机动目标定位跟踪中的视觉伺服控制方法，其特征在于，所述摄像机的光轴与地面的交点为A，摄像机光轴线上的单位向量为i，i在摄像机坐标系的齐次坐标系为^CP_i＝(0,0,1,1)^T，根据摄像机的位姿由公式(5)，可得机体坐标系{B}下的表示^BP_i：

{P_{B}}_{i} = T_{C}^{B} \cdot {P_{C}}_{i} - - - (7)

{P_{G}}_{i} = T_{B}^{G} \cdot {P_{B}}_{i} = T_{B}^{G} \cdot T_{C}^{B} \cdot {P_{C}}_{i} - - - (8)

根据透视成像原理和成像三角形关系，由飞机高度计算得到光轴与地面交点A在{G}下的坐标^GP_A，再根据公式(2)求出^BP_A；

无人机发现目标后，由所选目标点t在图像坐标系{I}下的齐次坐标^IP_t＝(^Ix_t,^Iy_t,1,1)^T，根据公式(6)得出^CP_t，继而可依次求出^BP_t、^GP_t：

{P_{G}}_{t} = T_{B}^{G} \cdot {P_{B}}_{t} = T_{B}^{G} \cdot T_{C}^{B} \cdot {P_{C}}_{t} = T_{B}^{G} \cdot T_{C}^{B} \cdot T_{I}^{G} \cdot {P_{I}}_{t} - - - (9) .

7.根据权利要求6所述的一种无人机机动目标定位跟踪中的视觉伺服控制方法，其特征在于，根据式(7)和(8)，结合式(3)和所求得的^BP_A，分别求得^GP_t和^GBP_tr＝^GP_A，则误差为Δ^GP_t＝^GP_tr-^GP_t，由此方程组求出姿态角给定值θ_t、φ_t和ψ_t；

φ_{r} = K_{φ} ({\tilde{y}}_{G}_{B O} - {y_{G}}_{B O}) + K_{d φ} {\overset{\cdot}{y}}_{G}_{B O} - - - (10)

根据纵向控制律分别求出俯仰角给定值θ_r

θ_{r} = K_{θ} ({\tilde{z}}_{G}_{B O} - {z_{G}}_{B O}) - - - (11)

θ＝λ_mθ_t+(1-λ_m)φ_r

φ＝λ_mφ_t+(1-λ_m)φ_r(12)

ψ＝ψ_t

以此作为飞行控制的内环给定，伺服控制无人机各舵面，实现目标定位跟踪过程中的视觉伺服控制。