CN114296471B - 一种基于全捷联下视相机的无人机精准着陆控制方法 - Google Patents

Abstract

本发明涉及一种基于全捷联下视相机的高空旋翼无人机精准着陆控制方法。采用与机体下部完全刚性固联的低成本相机，将拍摄的目标图像根据共线原理和视觉光流等技术估算出无人机与目标的相对位置和相对速度信息，导引无人机到目标上方悬停，然后控制无人机下降精准着陆到目标点。本发明设计的方法在精准着陆过程中不依赖卫星导航系统定位数据，只通过机载相机测量的无人机与目标的相对位置信息和无人机自身的速度与姿态进行控制精准着陆，使得卫星导航定位偏差不进入控制回路，提高了系统的抗干扰鲁棒性。

Description

一种基于全捷联下视相机的无人机精准着陆控制方法

技术领域

本发明属于无人机导航、定位与控制的技术领域，具体涉及一种基于全捷联下视相机的高空旋翼无人机精准着陆控制方法。

背景技术

旋翼型无人机具有低成本、可垂直起降、悬停、结构简单和开发周期短等优点，能够完成很多有人机无法完成的任务，尤其近年来结合机载视觉识别和人工智能等新兴技术，在军事和民用领域获得了广泛的使用。然而，随着各种不同用途无人机不断涌现，民事领域的无人机回收事故也不断出现；同时在军事领域，也需要无人机具有从高空对地精确打击和精准着陆到目标的能力。因此，研究并解决低成本旋翼型无人机的安全自主精准着陆问题具有重大的应用前景。

旋翼型无人机目前主要依靠卫星导航定位，而在无人机着陆过程中，由于卫星导航系统(GPS)定位误差比较大，且无人机在近地着陆过程中容易受到地效气流和风等环境因素影响，如果不加其它信息如人为控制，很难实现精准着陆。由于光学传感器能够探测周围环境，结合旋翼型无人机可定点悬停的特点，通过机载光学成像相机等传感器采集周围的图像，将摄像信息进行信息分析处理，根据目标图像的特征，得到每个特征点在图像中的对应点，经过数字图像处理后可以得到无人机自身相对目标信息，因此采用视觉导航定位技术等手段可以使无人机具有目标相对定位和自主导航能力。

目前常用的旋翼型无人机精准着陆方法主要分两种：一是基于差分卫星定位(RTK)技术，通过RTK基站给无人机发送其在大地坐标系下的精确位置坐标；一种是基于吊舱云台摄像机的图像识别技术，使无人机的机载图像识别设备识别地面目标，通过控制吊舱云台和无人机，获得目标与无人机的相对位置。其中，基于RTK基站的定位方式过于依赖基站，在不适合基站安装的着陆区域就不能使用，如起伏的山区或敌方区域；基于吊舱云台摄像机的图像定位方式由于需要安装吊舱云台，大幅增加了无人机的成本，同时吊舱云台的机械结构精密复杂，在某些军事领域需要高空抛撒启控的军用无人机，初始抛撒时的冲击过载会破坏云台机械结构。因此，如何设计一种不依赖复杂机械结构，低成本、可靠的旋翼型无人机的精准着陆控制方法需要进一步研究。

发明内容

针对旋翼型无人机从高空下落精准着陆到指定目标点的任务应用问题，本发明的目的是提供一种基于全捷联下视相机的高空旋翼无人机精准着陆控制方法。

实现本发明目的所采用的技术方案具体如下：一种基于全捷联下视相机的无人机精准着陆控制方法，包括以下步骤：

S1、高空基于视觉及卫星导航系统进行目标定位；

S12、根据相机视场角和相机方形像素阵列的宽度计算相机焦距；

S13、根据相机焦距和目标在像素阵列上的位置获取相机坐标系中像素位置到坐标原点的距离长度；

S14、由像素位置到坐标原点的距离长度和目标在像素阵列的上的位置得到目标在相机坐标系中的单位坐标向量；

S15、根据目标在相机坐标系中的单位坐标向量，并结合卡尔曼滤波算法估算目标到无人机的相对距离；

S16、通过坐标转换得到目标的地理位置信息与目标到无人机的相对距离之间的关系，并结合卫星导航系统测量得到的无人机位置信息量以及目标在相机坐标系中的单位坐标向量，计算目标的地理位置信息；

S2、高空阶段基于目标位置信息进行无人机控制；

当获得目标位置信息后，可采多环反馈跟踪的控制算法控制无人机飞向目标，具体控制算法如下：

S21、.采用比例积分PI控制器，根据控制器参数和无人机与目标间的距离得到参考速度；

S22、采用比例P控制器，根据控制器参数和参考速度得到参考加速度；

S23、根据参考加速度指令计算所需控制力，再结合目前姿态计算得到姿态角指令及油门控制量；

S24、采用双闭环PID控制方案，根据设计的PID控制器参数、姿态角指令、和当下姿态角速度，计算得到电机电调控制量；

S3、低空阶段基于目标位置信息进行无人机控制；

当无人机下降到超声波测距仪的有效精度的测量范围内，在此低空阶段运用光流技术获得无人机相对目标的速度估计，包括如下步骤：

S31、根据相机连续拍摄时由时间索引的图像序列从一个时间到下一个时间，相机相对惯性坐标系经历旋转和平移，将目标三维位置根据坐标系转换关系转到相机坐标系，计算得到相机平面的目标特征像素点的相对速度的运动公式；

S32、采用传统的计算光流的差分方法既计算特征像素点在单位时间的运动速度来获得目标特征像素点的相对速度；

S33、基于步骤S32计算得出的目标特征像素点的相对速度，采用目标标识点保持在视场预先设定圆区域内，不断下降高度，同时以目标特征像素点的相对速度和为零的制导控制策略来导引无人机至目标上方。

进一步地，所述旋翼无人机的动力学方程为：

其中，各个状态含义如下

位置状态：P_fs＝[X,Y,Z]^T

姿态状态：Θ_fs＝[φ,θ,ψ]，滚转角φ，偏航角ψ，俯仰角θ，滚转角速度偏航角速度/>俯仰角速度/>

转动惯量：滚转轴转动惯量I_x，偏航轴转动惯量I_y，俯仰轴转动惯量I_z

空气阻力关系系数：K₁，K₂，K₃

虚拟控制量：U₁，U₂，U₃，U₄

惯性系到机体系旋转矩阵：

其中位置信息、姿态角和姿态角速度由机载卫星导航设备及惯组测量，其它信息通过预先测量获得：

进一步地，所述位置控制器控制参考速度的计算公式为：

V_cX＝K_PX(X_c-X)+K_IX∫(X_c-X)dt

V_cY＝K_Y(Y_c-Y)+K_IY∫(Y_c-Y)dt

V_cZ＝K_Z(Z_c-Z)+K_IZ∫(Z_c-Z)dt

其中，K_PX，K_IX，K_Y，K_IY，K_Z和K_IZ为控制器参数，速度指令限幅值为 |V_cX|≤V_maxXm/s，|V_cY|≤V_maxYm/s，|V_cZ|≤V_maxZm/s，其中V_maxX，V_maxY， V_maxZ是预先设置的；

无人机到达指令位置坐标(X_c,Y_c,Z_c)，无人机当前位置坐标(X,Y,Z)；

进一步地，根据所述速度控制器参数为K_VPx，K_VPy和K_VPz得到参考加速度指令如下：

a_xc＝K_VPx(V_cX-V_X)

a_yc＝K_VPy(V_cY-V_Y)

a_zc＝K_VPz(V_cZ-V_Z)

其中，V_X、V_Y和V_Z为当前无人机速度。

进一步地，所述姿态角指令计算方法如下：

无人机内部需要的控制力指令如下

u_1x＝a_xcm

u_1y＝a_ycm

u_1z＝(a_zc-g)m

为使跟踪加速度指令位置，设所需指令姿态角为φ_c，θ_c，油门控制量为U₁，由关系式

u_1x＝-(sinθcosφcosψ+sinφsinψ)U₁

u_1y＝-(sinθcosφsinψ-sinφcosψ)U₁

u_1z＝-(cosφcosθ)U₁

及由于上公式可变为：

由u_1z＝-(cosφ_c cosθ_c)U₁，可得则

计算上式得：

俯仰角指令：

滚转角指令：

纵向油门控制量U₁：

进一步地，所述电机电调控制量计算方法如下：

设PID控制器参数为K_Pφwai，K_Iφwai，K_Pφnei，K_Pθwai，K_Iθwai， K_Pθnei，K_Pψwai，K_Iψwai和K_Pψnei，电机电调控制量如下:

ΔPWM_{φ_c}＝K_Pφnei(K_Pφwai(φ_c-φ)+K_Iφwai∫(φ_c-φ)dt-w_x)

ΔPWM_{θ_c}＝K_Pθnei(K_Pθwai(θ_c-θ)+K_Iθwai∫(θ_c-θ)dt-w_y)

ΔPWM_{ψ_c}＝K_Pψnei(K_Pψwai(ψ_c-ψ)+K_Iψwai∫(ψ_c-ψ)dt-w_z)

ΔPWM_{f_c}＝U₁

其中，滚转角指令φ_c，俯仰角指令θ_c，偏航角指令ψ_c和油门PWM指令U₁由姿态角指令提供。

进一步地，所述目标三维位置根据坐标系转换关系转到相机坐标系的相机平面上的位置投影方程如下：

其中，(P_x,P_y)是目标在相机像素阵列上的位置，P_f为相机焦距；

相机相对惯性坐标系平移T(T_x，T_y，T_z)，为目标对象在相机坐标系中的坐标向量，L为目标像素点到拍摄目标的实际距离长度；

惯性系到机体系旋转矩阵：

当考虑短时间内，如图像序列两帧之间的时间间隔内，旋转运动满足小角度假设，由于相机系统与旋翼无人机具有相同的运动，如图2所示，相机平面的目标特征像素点的相对速度的运动公式如下：

是滚转角速度，/>是偏航角速度，/>是俯仰角速度。

进一步地，所述目标相对速度的光流技术算法为：

假设目标特征像素点在视场内运动时，在相同光照条件下，它的亮度或颜色等特征I(P_x(t),P_y(t),t)将保持不变，则特征I(P_x(t),P_y(t),t)对时间的导数为零：

其中，利用光流算法是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系，从而计算出相邻帧之间物体的瞬时速度信息的一种方法，即可以采用传统的计算光流的差分方法既计算特征像素点在单位时间的运动速度来获得目标特征像素点的相对速度 P_Vx和P_Vy，通过光流算法计算的像素点相对速度后，带入到前一个公式

其中V_Z用超声波传感器测量得到，可以解算飞行器的速度V_X、V_Y。

进一步地，所述，当目标标识点超出在视场预先设定圆区域，通过控制无人机移动，使目标像素点相对朝设定的圆区域移动，即设置一定的无人机飞行速度，使目标特征像素点的相对速度P_Vx和P_Vy向相机成像平面中心移动至视场预先设定圆区域。

进一步地，低空阶段制导控制系统所需的导航信息可由加速度计、气压计和卫星导航系统计算，相机距目标的距离L由机载超声波测距仪测量得到。

进一步地，低空阶段设计速度控制器，采用比例PI控制器，控制器参数为 K_VPx，K_VIx，K_VPy，K_VIy，K_VPz和K_VIz，得到参考加速度指令如下：

a_xc＝K_VPx(V_cX-V_X)+K_VIx∫(V_cX-V_X)dt

a_yc＝K_VPy(V_cY-V_Y)+K_VIy∫(V_cY-V_Y)dt

a_zc＝K_VPz(V_cZ-V_Z)+K_VIz∫(V_cZ-V_Z)dt

其它的制导控制系统的控制结构和控制参数与高空阶段基于目标位置信息进行无人机控制的算法相同。

与现有技术相比，本发明采用与机体下部完全刚性固联的低成本相机，根据拍摄的目标图像利用共线原理和视觉光流等技术估算出无人机与目标的相对位置和相对速度信息，导引无人机到目标上方悬停，然后控制无人机下降精准着陆到目标点。

本发明设计的方法可以估算出待着陆目标与无人机的相对距离，进而可以估算待着陆目标的绝对位置定位信息，为目标环境态势感知提供了必要的分析数据，提高了系统的扩展性。

本发明设计的方法在精准着陆过程中不依赖卫星导航系统定位数据，只通过机载相机测量的无人机与目标的相对位置信息和无人机自身的速度与姿态进行控制精准着陆，使得卫星导航定位偏差不进入控制回路，提高了系统的抗干扰鲁棒性。

总的来说，本发明设计的方法在精准着陆过程中只使用与无人机机体下部完全刚性固联的机载相机，在保证着陆精度的同时避免了使用复杂机械结构的吊舱云台，能够很好的满足微小型无人机低成本、小型化和机体抗过载冲击能力要求，降低了无人机成本，提高了系统的可靠性和准确性。

附图说明

图1为本发明无人机、下视相机和目标之间的关系图；

图2为本发明相机坐标系与目标相对运动成像关系图；

图3为本发明基于目标位置信息的无人机控制算法示意图；

图4为本发明基于速度信息的无人机控制算法示意图。

具体实施方式

针对旋翼型无人机从高空下落精准着陆到指定目标点的任务应用问题，本发明的目的是提供一种基于全捷联下视相机的高空旋翼无人机精准着陆控制方法。

实现本发明目的所采用的技术方案具体如下：

一种基于全捷联下视相机的高空旋翼无人机精准着陆控制方法，如图1所示，导航传感器包括机载惯性传感器(陀螺仪、加速度计)、磁力计、气压高度计、卫星导航定位接收机、固定在机体底部的捷联下视相机和超声波测距仪。低成本的机载惯性器件和磁强计分别测量机体坐标系下的加速度、姿态角度、姿态角速度和磁场强度信息；卫星导航系统测量机体的当前经纬度和相对地面导航坐标系的速度信息；采用与机体下部完全刚性固联的低成本相机，将拍摄的目标图像根据共线原理和视觉光流等技术估算出无人机与目标的相对位置和相对速度信息，导引无人机到目标上方悬停，然后控制无人机下降精准着陆到目标点。

更近一步地，本发明设计的方法可以估算得待着陆目标与无人机的相对距离，进而可以估算待着陆目标的绝对位置定位信息，为目标环境态势感知提供了必要的分析数据，提高了系统的扩展性。

更近一步地，本发明设计的方法在精准着陆过程中不依赖卫星导航系统定位数据，只通过机载相机测量的无人机与目标的相对位置信息和无人机自身的速度与姿态进行控制精准着陆，使得卫星导航定位偏差不进入控制回路，提高了系统的抗干扰鲁棒性。

更近一步地，本发明设计的方法在精准着陆过程中只使用与无人机机体下部完全刚性固联的机载相机，在保证着陆精度的同时避免了使用复杂机械结构的吊舱云台，能够很好的满足微小型无人机低成本、小型化和机体抗过载冲击能力要求，提高系统的可靠性和准确性。

具体实施方式

下面结合附图和具体实施方案对本发明作进一步详细说明。以下实施例仅是说明性的，并不构成对本发明的限制。

本发明为使旋翼型无人机最终精准着陆到指定目标点，从无人机从高空定位目标到最终精准着陆目标的全过程需经历如下两个阶段：

高空基于视觉及卫星导航系统的目标定位及下落阶段；

低空基于视觉及超声波测距仪的相对目标速度控制的末段精准着陆阶段；

本发明提出的高空低成本旋翼无人机、下视相机和目标之间的关系图如图1 所示。受限于高空投放无人机的高过载的投放环境和低成本考虑，本方案中采用基于无人机机体下部完全刚性固联的捷联机载相机进行目标识别和导航制导。方案成功的关键在于为制导控制系统提供目标信息，基于此目标，根据不同导航定位传感器的测量精度，将导航阶段分为高空依赖卫星系统和低空依赖超声波测距仪定位的两阶段策略，本发明由于使用了可抗冲击过载环境的与机体固定连接摄像机，不同于传统使用吊舱云台摄像机时需要复杂的机械结构、坐标系转换和目标识别定位算法，提出了将拍摄的目标图像根据共线原理和视觉光流等技术估计出目标的相对位置和相对速度信息的算法，具体步骤如下：

(1)高空基于视觉及卫星导航系统的目标定位算法

当无人机依靠自身惯性导航等设备(陀螺仪、加速度计和磁力计)获得相对惯性系的姿态信息在高空稳定悬停后，等待机载的卫星导航成功接收到定位信号后，设备无人机的三维世界位置和平移速度可由卫星导航定位系统估计。为了成功定位目标位置信息，采用如下算法策略：

由于着陆区域特点，可以在水平地面假设下，将该卫星信号稳定的接收点设置为导航惯性系Oⁱ(oxⁱyⁱzⁱ)原点，全捷联相机相对机体以一定安装角完全刚性固定安装在机体下部，无人机的高度为h，机体坐标系O^b(ox^by^bz^b)、全捷联下视相机的相机坐标系O^c(ox^cy^cz^c)和目标的关系如图1所示，其中相机光轴与机体/>轴重合。

相机坐标系O^c(ox^cy^cz^c)的几何图如图2所示，相机坐标系中的拍摄目标向量由表示，目标在像平面的投影用/>表示，设相机的视场角为η，相机的方形像素阵列的宽度M已知，相机像素为N×N，则可以计算相机的焦距 P_f如下式：

拍摄目标在像素阵列的投影位置在相机坐标系中表示为(P_x,P_y,P_f)，其中 (P_x,P_y)是目标在像素阵列上的位置，相机坐标原点到像素位置(P_x,P_y)的距离长度P_L表示如下式：

设像素点(P_x,P_y)到拍摄目标的实际距离长度为L，可得到如下三角相似关系式：

可知目标对象在相机坐标系中的坐标向量表示如下：

令在惯性坐标系中目标位置矢量为旋翼无人机的位置矢量为/>当将无人机作为一个质点考虑时，无人机到目标的距离就是机载相机到目标的距离L表示如下：

其中表示目标位置与无人机位置向量差的转置。

相机坐标系到机体坐标系的转移矩阵为根据相机相对无人机机体的安装角位置确定；机体坐标系到惯性坐标系的转移矩阵为/>由机载惯性导航装置测量机体相对惯性坐标系的姿态信息(滚转角φ，俯仰角θ，偏航角ψ)确定，由图1的几何关系知，定义向量/>如下：

只要知道相对距离L的值就可以测量出目标的地理位置向量相对距离的 L的值可由下式测量：

其中无人机自身相对惯性系的位置信息向量和速度信息向量/>可以由卫星导航系统测量，针对地面固定目标速度即位置导数/>和相对距离的导数/>有如下式：

由于卫星和机载惯性导航装置测量存在噪声，同时风等外界干扰也会影响测量值，为了有效减小测量误差对目标位置估计的影响，可以将的公式作为状态方程，L的公式作为测量方程，采用基于扩展卡尔曼(EKF)等滤波算法来估计相对距离L，然后就可以根据公式/>获得相对准确的目标位置/>将目标位置作为无人机位置目标控制指令导引无人机飞向目标。

(2)高空基于目标位置信息的旋翼无人机控制算法

当在高空基于获得目标位置信息后，可采用如图3所示的多环反馈跟踪的控制算法控制无人机飞向目标，具体控制算法如下：

旋翼无人机的动力学方程：

其中，各个状态含义如下，其中位置信息、姿态角和姿态角速度由机载卫星导航设备及惯组测量，其它如转动惯量等信息预先测量：

位置状态：P_fs＝[X,Y,Z]^T

姿态状态：Θ_fs＝[φ,θ,ψ]，滚转角φ，偏航角ψ，俯仰角θ，滚转角速度偏航角速度/>俯仰角速度/>

转动惯量：滚转轴转动惯量I_x，偏航轴转动惯量I_y，俯仰轴转动惯量I_z。

空气阻力关系系数：K₁，K₂，K₃

虚拟控制量：U₁，U₂，U₃，U₄

惯性系到机体系旋转矩阵：

2)位置控制器设计

为使旋翼无人机从当前位置坐标(X,Y,Z)到达指令位置(X_c,Y_c,Z_c)，即： X→X_c，Y→Y_c，Z→Z_c

设计位置控制器，比例积分PI控制器，控制器参数为K_PX，K_IX，K_Y，K_IY， K_Z和K_IZ。

V_cX＝K_PX(X_c-X)+K_IX∫(X_c-X)dt

V_cY＝K_Y(Y_c-Y)+K_IY∫(Y_c-Y)dt

V_cZ＝K_Z(Z_c-Z)+K_IZ∫(Z_c-Z)dt

其中，速度指令限幅值为|V_cX|≤V_maxX m/s，|V_cY|≤V_maxYm/s， |V_cZ|≤V_maxZm/s，其中V_maxX，V_maxY，V_maxZ是预先设置的。

设计速度控制器，采用比例P控制器，控制器参数为K_VPx，K_VPy和K_VPz，得到参考加速度指令如下：

a_xc＝K_VPx(V_cX-V_X)

a_yc＝K_VPy(V_cY-V_Y)

a_zc＝K_VPz(V_cZ-V_Z)

其中，V_X、V_Y和V_Z为当前无人机速度。

3)根据加速度指令得到姿态角指令

无人机内部需要的控制力指令如下：

u_1x＝a_xcm

u_1y＝a_ycm

u_1z＝(a_zc-g)m

为使跟踪加速度指令位置，设所需指令姿态角为φ_c，θ_c，由关系式

u_1x＝-(sinθcosφcosψ+sinφsinψ)U₁

u_1y＝-(sinθcosφsinψ-sinφcosψ)U₁

u_1z＝-(cosφcosθ)U₁

由于上公式变为

由u_1z＝-(cosφ_c cosθ_c)U₁，可得则

上式子中第一行，第二行得：

俯仰角指令：

滚转角指令：

纵向油门控制量U₁如下：

4)姿态环控制器设计

采用双闭环PID控制方案，待设计的PID控制器参数为K_Pφwai，K_Iφwai， K_Pφnei，K_Pθwai，K_Iθwai，K_Pθnei，K_Pψwai，K_Iψwai和K_Pψnei，电机电调控制量如下:

ΔPWM_{φ_c}＝K_Pφnei(K_Pφwai(φ_c-φ)+K_Iφwai∫(φ_c-φ)dt-w_x)

ΔPWM_{θ_c}＝K_Pθnei(K_Pθwai(θ_c-θ)+K_Iθwai∫(θ_c-θ)dt-w_y)

ΔPWM_{ψ_c}＝K_Pψnei(K_Pψwai(ψ_c-ψ)+K_Iψwai∫(ψ_c-ψ)dt-w_z)

ΔPWM_{f_c}＝U_{f_c}

其中，滚转角指令φ_c，俯仰角指令θ_c，偏航角指令ψ_c和油门PWM指令U_{f_c}由位置控制环提供。

(3)低空基于视觉及超声波测距仪的相对目标速度估计与制导控制策略

当无人机下降到机载低成本超声波测距仪的有效精度的测量范围内如距地面6米左右时，由于卫星系统定位位置精度(水平定位精度10米内)不再满足末端精确着陆要求，然而此时机载超声波测距仪和机载下视相机此时可以获得较为精确的有效信号，在此低空阶段运用光流技术获得无人机相对目标的速度估计，根据此信息可设计末制导控制算法。

基于机载嵌入式处理器计算能力等考虑，复杂的视觉里程算法在本项目的低成本旋翼无人机上并不适用，这是由于复杂的视觉里程算法需要大量的板载计算资源，成本较高，同时在末段着陆阶段，无人机已经距离目标较近(6米高度左右)，在低空末段着陆阶段无人机下降到据目标这个过程中，由于光流是对未知相机与目标相对运动的一个较好的近似，本方案采用传统的光流技术来估计目标相对速度，具体算法如下：

相机与目标相对速度的运动场和相机的光流具有如下定义：

运动场：三维相对速度矢量在二维图像平面上的投影。

光流：图像中观察到的目标特征点的亮度模式(或颜色等特征)的二维位移。

运动场是最终的期望估计目标，光流可以根据图像视觉处理算法直接在图像序列中得到。

1)目标特征像素点的相对速度的运动学公式

由于考虑旋翼无人机不断在空中运动，并且捷联下视相机坐标系与无人机机体固联，根据相机连续拍摄时由时间索引的图像(帧)序列从一个时间t到下一个时间t+1，相机相对惯性坐标系经历旋转(滚转、俯仰、偏航)和平移T(T_x， T_y，T_z)，目标三维位置根据坐标系转换关系转到相机坐标系的相机平面上的位置投影方程如下：

当考虑短时间内，如图像序列两帧之间的时间间隔内，旋转运动满足小角度假设，由于相机系统与旋翼无人机具有相同的运动，如图2所示，相机平面的目标特征像素点的相对速度的运动公式如下：

2)目标特征像素点的相对速度的光流技术算法

假设目标特征像素点在视场内运动时，在相同光照条件下，它的亮度或颜色等特征I(P_x(t),P_y(t),t)将保持不变，则特征I(P_x(t),P_y(t),t)对时间的导数为零：

利用光流算法是利用图像序列中像素在时间域上的变化以及相邻帧之间的相关性来找到上一帧跟当前帧之间存在的对应关系，从而计算出相邻帧之间物体的瞬时速度信息的一种方法，即可以采用传统的计算光流的差分方法计算特征像素点在单位时间的运动速度来获得目标特征像素点的相对速度P_Vx和P_Vy。通过光流算法计算的像素点相对速度后，带入到前一个公式

其中Vz用超声波传感器测量得到，可以解算飞行器相对目标的速度V_X、 V_Y(在目标静止的条件下相对目标的速度即为无人机本身飞行速度)，该飞行器相对目标速度即用来控制飞行器运动，精确着陆到目标点。

3)基于目标特征像素点的相对速度的制导控制算法

当旋翼无人机在低空末制导阶段从距目标6米左右逐步下降高度至着陆到目标这个过程中，可以采用目标标识点在视场预先设定圆区域内，不断下降高度，同时以目标特征像素点的相对速度P_Vx和P_Vy为零的制导控制策略来导引无人机至目标上方，当目标标识点超出在视场预先设定圆区域，设置目标特征像素点的相对速度P_Vx和P_Vy向相机成像平面中心移动至视场预先设定圆区域，可采用如图4所示的控制算法，制导控制系统所需的导航信息如无人机的垂向速度V_z可由加速度计、气压计和卫星导航系统计算，相机据目标的距离L由机载超声波测距仪测量得到，同时，可以根据光流算法估算得到无人机的水平速度V_x和V_y。

设计速度控制器，采用比例PI控制器，控制器参数为K_VPx，K_VIx，K_VPy，K_VIy， K_VPz和K_VIz，得到参考加速度指令如下：

a_yc＝K_VPy(V_cY-V_Y)+K_VIy∫(V_cY-V_Y)dt

a_xc＝K_VPx(V_cX-V_X)+K_VIx∫(V_cX-V_X)dt

a_zc＝K_VPz(V_cZ-V_Z)+K_VIz∫(V_cZ-V_Z)dt

其它的制导控制系统的控制结构和控制参数可以参考基于目标位置信息的旋翼无人机控制算法。

综上所述，本发明设计一种不依赖复杂机械结构，低成本和可靠的旋翼型无人机的精准着陆控制方法。

本发明不仅局限于上述具体实施方式，本领域一般技术人员根据本发明公开的内容，可以采用其它多种具体实施方式实施本发明，因此，凡是采用本发明的设计结构和思路，做一些简单的变化或更改的设计，都落入本发明保护的范围。

Claims (3)

1.一种基于全捷联下视相机的无人机精准着陆控制方法，其特征在于，导航传感器包括机载惯性传感器、磁力计、气压高度计、卫星导航定位接收机、超声波测距仪和固定在机体底部的捷联下视相机；所述方法应用于无人机从高空下落精准着陆到指定目标点；

包括以下步骤：

S1、高空基于视觉及卫星导航系统进行目标定位；

采用基于无人机机体下部完全刚性固联的捷联机载相机进行目标识别和导航制导；

S12、根据相机视场角和相机方形像素阵列的宽度计算相机焦距；

S13、根据相机焦距和目标在像素阵列上的位置获取相机坐标系中像素位置到坐标原点的距离长度；

S14、由像素位置到坐标原点的距离长度和目标在像素阵列的上的位置得到目标在相机坐标系中的单位坐标向量；

S15、根据目标在相机坐标系中的单位坐标向量，并结合卡尔曼滤波算法估算目标到无人机的相对距离；

S16、通过坐标转换得到目标的地理位置信息与目标到无人机的相对距离之间的关系，并结合卫星导航系统测量得到的无人机位置信息量以及目标在相机坐标系中的单位坐标向量，计算目标的地理位置信息；

S2、高空阶段基于目标位置信息进行无人机控制；

当获得目标位置信息后，可采多环反馈跟踪的控制算法控制无人机飞向目标，具体控制算法如下：

旋翼无人机的动力学方程为：

其中，各个状态含义如下:

位置状态：P_fs＝[X,Y,Z]^T

姿态状态：Θ_fs＝[φ,θ,ψ]，滚转角φ，偏航角ψ，俯仰角θ，滚转角速度偏航角速度俯仰角速度/>

转动惯量：滚转轴转动惯量I_x，偏航轴转动惯量I_y，俯仰轴转动惯量I_z

空气阻力关系系数：K₁，K₂，K₃

虚拟控制量：U₁，U₂，U₃，U₄

惯性系到机体系旋转矩阵：

其中位置信息、姿态角和姿态角速度由机载卫星导航设备及惯组测量，其它信息通过预先测量获得；

S21、设计位置控制器，采用比例积分PI控制器，根据控制器参数和无人机与目标间的距离得到参考速度；

所述位置控制器控制参考速度的计算公式为：

V_cX＝K_PX(X_c-X)+K_IX∫(X_c-X)dt

V_cY＝K_Y(Y_c-Y)+K_IY∫(Y_c-Y)dt

V_cZ＝K_Z(Z_c-Z)+K_IZ∫(Z_c-Z)dt

其中，K_PX，K_IX，K_Y，K_IY，K_Z和K_IZ为控制器参数，速度指令限幅值为|V_cX|≤V_maxXm/s，|V_cY|≤V_maxYm/s，|V_cZ|≤V_maxZm/s，其中V_maxX，V_maxYV_maxZ是预先设置的；

无人机到达指令位置坐标(X_c,Y_c,Z_c)，无人机当前位置坐标(X,Y,Z)

S22、设计速度控制器，采用比例P控制器，根据控制器参数和参考速度得到参考加速度；

根据所述速度控制器参数为K_VPx，K_VPy和K_VPz得到参考加速度指令如下：

a_xc＝K_VPx(V_cX-V_X)

a_yc＝K_VPy(V_cY-V_Y)

a_zc＝K_VPz(V_cZ-V_Z)

S23、根据参考加速度指令计算所需控制力，再结合目前姿态计算得到姿态角指令及油门控制量；

所述姿态角指令计算方法如下：

无人机内部需要的控制力指令如下

u_1x＝a_xcm

u_1y＝a_ycm

u_1z＝(a_zc-g)m

为使跟踪加速度指令位置，设所需指令姿态角为φ_c，θ_c，油门控制量为U₁，由关系式

u_1x＝-(sinθcosφcosψ+sinφsinψ)U₁

u_1y＝-(sinθcosφsinψ-sinφcosψ)U₁

u_1z＝-(cosφcosθ)U₁

及由于上公式可变为：

由u_1z＝-(cosφ_c cosθ_c)U₁，可得则

计算上式得：

俯仰角指令：

滚转角指令：

纵向油门控制量U₁：

S24、设计姿态环控制器，采用双闭环PID控制方案，根据设计的PID控制器参数、姿态角指令、和当下姿态角速度，计算得到电机电调控制量；

所述电机电调控制量计算方法如下：

设PID控制器参数为K_Pφwai，K_Iφwai，K_Pφnei，K_Pθwai，K_Iθwai，K_Pθnei，K_Pψwai，K_Iψwai和K_Pψnei，电机电调控制量如下:

ΔPWM_{φ_c}＝K_Pφnei(K_Pφwai(φ_c-φ)+K_Iφwai∫(φ_c-φ)dt-w_x)

ΔPWM_{θ_c}＝K_Pθnei(K_Pθwai(θ_c-θ)+K_Iθwai∫(θ_c-θ)dt-w_y)

ΔPWM_{ψ_c}＝K_Pψnei(K_Pψwai(ψ_c-ψ)+K_Iψwai∫(ψ_c-ψ)dt-w_z)

ΔPWM_{f_c}＝U₁

其中，滚转角指令φ_c，俯仰角指令θ_c，偏航角指令ψ_c和油门PWM指令U₁由姿态角指令提供；

S3、低空阶段基于目标位置信息进行无人机控制；

当无人机下降到超声波测距仪的有效精度的测量范围内，在此低空阶段运用光流技术获得无人机相对目标的速度估计，包括如下步骤：

S31、根据相机连续拍摄时由时间索引的图像序列从一个时间到下一个时间，相机相对惯性坐标系经历旋转和平移，将目标三维位置根据坐标系转换关系转到相机坐标系，计算得到相机平面的目标特征像素点的相对速度的运动公式；

S32、采用传统的计算光流的差分方法既计算特征像素点在单位时间的运动速度来获得目标特征像素点的相对速度；

所述目标相对速度的光流技术算法为：

假设目标特征像素点在视场内运动时，在相同光照条件下，它的亮度或颜色等特征I(P_x(t),P_y(t),t)将保持不变，则特征I(P_x(t),P_y(t),t)对时间的导数为零：

采用传统的计算光流的差分方法计算特征像素点在单位时间的运动速度来获得目标特征像素点的相对速度P_Vx和P_Vy，通过光流算法计算的像素点相对速度后，带入到前一个公式

其中V_Z用超声波传感器测量得到，可以解算飞行器的速度V_X、V_Y；

S33、基于步骤S32计算得出的目标特征像素点的相对速度，采用目标标识点保持在视场预先设定圆区域内，不断下降高度，同时以目标特征像素点的相对速度和为零的制导控制策略来导引无人机至目标上方；

当所述目标标识点超出在视场预先设定圆区域，通过控制无人机移动，使目标像素点相对朝设定的圆区域移动；

低空阶段制导控制系统所需的导航信息可由加速度计、气压计和卫星导航系统计算，相机距目标的距离L由机载超声波测距仪测量得到。

2.根据权利要求1所述的一种基于全捷联下视相机的无人机精准着陆控制方法，其特征在于，所述目标三维位置根据坐标系转换关系转到相机坐标系的相机平面上的位置投影方程如下：

其中，(P_x,P_y)是目标在相机像素阵列上的位置，P_f为相机焦距；

相机相对惯性坐标系平移T(T_x，T_y，T_z)，为目标对象在相机坐标系中的坐标向量，L为目标像素点到拍摄目标的实际距离长度；

惯性系到机体系旋转矩阵：

当考虑短时间内，图像序列两帧之间的时间间隔内，旋转运动满足小角度假设，由于相机系统与旋翼无人机具有相同的运动，相机平面的目标特征像素点的相对速度的运动公式如下：

是滚转角速度，/>是偏航角速度，/>是俯仰角速度。

3.根据权利要求1所述的一种基于全捷联下视相机的无人机精准着陆控制方法，其特征在于，低空阶段设计速度控制器，采用比例PI控制器，控制器参数为K_VPx，K_VIx，K_VPy，K_VIy，K_VPz和K_VIz，得到参考加速度指令如下：

a_xc＝K_VPx(V_cX-V_X)+K_VIx∫(V_cX-V_X)dt

a_yc＝K_VPy(V_cY-V_Y)+K_VIy∫(V_cY-V_Y)dt

a_zc＝K_VPz(V_cZ-V_Z)+K_VIz∫(V_cZ-V_Z)dt

其它的制导控制系统的控制结构和控制参数与高空阶段基于目标位置信息进行无人机控制的算法相同。