CN116700348B - 一种四旋翼飞行器视觉受限的视觉伺服控制方法及系统 - Google Patents

Abstract

本发明公开了一种四旋翼飞行器视觉受限的视觉伺服控制方法及系统，所述方法包括：建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程；对摄像设备的单个特征点透视投影模型进行求导简化处理，并基于求导简化结果与所述瞬时运动学方程结合；将单个特征点在虚拟图像二维平面中与摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理；对多特征点方程进行降维处理；进行控制成本函数构建处理；进行非线性优化处理，形成非线性优化问题方程；对非线性优化问题方程进行求解处理，并基于求解结果进行视觉伺服控制处理。在本发明实施例中，实现仅靠摄像设备和惯性测量即可实现四旋翼飞行器对地面运动物体的跟踪控制。

Description

一种四旋翼飞行器视觉受限的视觉伺服控制方法及系统

技术领域

本发明涉及飞行控制技术领域，尤其涉及一种四旋翼飞行器视觉受限的视觉伺服控制方法及系统。

背景技术

如今，多旋翼飞行器，尤其是四旋翼飞行器，由于具有很高的机动性，较低的购置和维护成本以及出色的垂直起降能力，即使在恶劣的环境下，也被认为是最受欢迎的自动驾驶空中平台。此外，导航、感知传感器和高性能电池技术的快速进展显著提高了四旋翼无人机的续航和载荷能力，使其成为各种任务的理想平台，例如搜索和救援、区域覆盖、监视、物体运输和智能农业。

尽管在自主多旋翼操作过程中使用了许多导航传感器(GPS，INS等)，但由于摄像机重量轻、体积小、无源、低功耗等有点，使得其在机器人运动控制中至关重要，以便能够精确监视和跟踪感兴趣的区域和目标。如果是无人飞行系统，例如四旋翼飞行器，则可以通过将摄像头传感器直接安装在机器人上来轻松实现，从而形成所谓的“眼在手”系统。传统的视觉伺服方法依赖于一组视觉特征的提取，跟踪和匹配，这些视觉特征被用作控制器的输入，以引导机器人完成相应的控制任务。

由于对视觉伺服方法和应用的研究的持续深入，图像处理的逐渐成熟，和各种控制方法的发展，视觉伺服控制在机器人系统的发展和应用等方面展现出了巨大的价值。关于视觉伺服控制，可以定义四个主要类别：i)基于位置的视觉伺服(PBVS)，其中所选用的控制误差定义在笛卡尔空间中。ii)基于图像的视觉伺服(IBVS)，其中控制误差函数在图像空间中定义；iii)2-1/2或混合视觉伺服，其中控制误差函数在笛卡尔和图像空间中部分定义；iv)直接视觉伺服，无需提取特定特征，而是在控制设计中利用完整图像。上述方法各有利弊，其功效在很大程度上取决于应用需求。但是，IBVS被认为对航空器应用特别有效，特别是对移动目标的跟踪，因为它具有更好的局部稳定性和收敛性，这是因为它具有针对相机校准缺陷和建模误差的固有鲁棒性。

但由于四旋翼无人机动力学与机载相机运动学的耦合关系以及机载相机的视角限制给基于IBVS的四旋翼无人机控制器设计带来了困难。此外，机载相机的校准以及目标特征的深度的精确性也制约着四旋翼无人机视觉伺服控制器的设计。

发明内容

本发明的目的在于克服现有技术的不足，本发明提供了一种四旋翼飞行器视觉受限的视觉伺服控制方法及系统，实现仅靠摄像设备和惯性测量即可实现四旋翼飞行器对地面运动物体的跟踪控制。

为了解决上述技术问题，本发明提供了一种四旋翼飞行器视觉受限的视觉伺服控制方法，所述方法包括：

基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程；

对所述摄像设备的单个特征点透视投影模型进行求导简化处理，并基于求导简化结果与所述瞬时运动学方程结合，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系；

将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理，获得多特征点方程；

采用动态模式分解对所述多特征点方程进行降维处理，获得视觉伺服控制的状态方程；

基于所述视觉伺服控制的状态方程进行控制成本函数构建处理，获得控制成本函数；

基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理，形成非线性优化问题方程；

基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理，并基于求解结果进行视觉伺服控制处理。

可选的，所述基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程，包括：

构建四旋翼飞行器动态模型中的惯性坐标系A＝{O_a,X_a,Y_a,Z_a}和机体坐标系B＝{O_b,X_b,Y_b,Z_b}以及摄像设备的虚拟二维平面投影坐标系C_p＝{V_p,U_p}，四旋翼飞行器的重心与机体坐标系的原点O_b重合；

假设η^a＝[η_xη_yη_zψ]^T表示四旋翼飞行器在惯性坐标系下的位置向量和航向角；(η_x0η_y0η_z0)表示重心在机体坐标系下的坐标；(f_x f_y f_z)表示摄像设备到四旋翼飞行器的重心的距离；

建立摄像设备与四旋翼飞行器的空间位置关系方程如下：

其中，s＝sin(·),c＝cos(·)；对方程(1)微分，同时引入四旋翼飞行器的机动性约束μ＝[μ_lμ_mμ_nω]^T，构建四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程如下：

其中，所述瞬时运动学方程表示四旋翼飞行器的可操作空间μ(t)到任务空间的线性映射；μ(t)表示四旋翼飞行器的机动性约束函数；ψ(t)表示航向角；J_a(ψ(t))表示映射矩阵。

可选的，所述瞬时运动学方程中的J_a(ψ(t))如下：

其中，ψ(t)表示航向角；ψ表示航向角；s＝sin(·),c＝cos(·)；J_a(.)表示映射矩阵。

可选的，所述对所述摄像设备的单个特征点透视投影模型进行求导简化处理，包括：

由摄像设备获得图像的第i个特征点在机载相机参考系C下表示为^Cξⁱ＝[^Cx^{i C}y^{i C}zⁱ]^T；第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标为^pξⁱ＝[^pu^{i p}vⁱ]^T；

则摄像设备的单特征点透视投影模型如下：

其中，f_c为摄像设备的焦距；

对所述单特征点透视投影模型进行求导简化处理，获得单个特征点在虚拟图像二维平面中与摄像设备的运动关系：

其中，^av_c(t)、^aω_c(t)分别表示摄像设备的速度和角速度；^cR_a表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵；为Jacobian图像矩阵，表达式如下：

其中，^puⁱ表示第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标^pξⁱ内的元素；^pvⁱ表示第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标^pξⁱ内的元素；^czⁱ表示表示摄像设备的图像平面的第i个特征点坐标深度。

可选的，所述基于求导简化结果与所述瞬时运动学方程结合，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系，包括：

将所述瞬时运动学方程代入求导简化结果中，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系如下：

其中，T_a表示四旋翼飞行器运动速度表换矩阵；^cR_a表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵。

可选的，所述多特征点方程如下：

其中：

其中，^pξⁱ＝[^pu^{i p}vⁱ]和^czⁱ分别表示摄像设备的图像平面的第i个特征点坐标及其深度；^cR_a表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵；T_a表示四旋翼飞行器运动速度表换矩阵；J_r(.)表示第r个特征点对应的Jacobian图像矩阵。

可选的，所述采用动态模式分解对所述多特征点方程进行降维处理，获得视觉伺服控制的状态方程，包括：

构建所述四旋翼飞行器的状态空间表达式如下：

其中，μ_ref＝[u_lref u_mref u_nref ω_ref]^T表示四旋翼飞行器的期望速度及期望航向角速度；A表示四旋翼飞行器的动态矩阵；B表示四旋翼飞行器的控制矩阵；μ(t)为四旋翼飞行器的可操作空间；

针对公式(9)的输入和四旋翼飞行器的状态进行s次的快照测量构成新的向量如下：

将公式(9)改写为：

对公式(10)改成为矩阵乘积形式如下：

采用动态模式分解对四旋翼飞行器的控制系统进行系统辨识，估计动态矩阵A和控制矩阵B，估计值如下：

通过系统辨识转换为：

将公式(8)和公式13进行结合，将状态变量重构为 得到方程如下：

令t为控制系统当前时间，T为预测范围，则公式(14)转化为控制系统预测模型：

其中，l表示预测步长；f_p(^pξ,^cz,ψ,μ)表示方程(8)的近似模型；f_μ(Φ,μ,μ_ref)表示方程(9)的近似模型；表示矩阵左奇异值向量矩阵；表示奇异值分解矩阵；表示的右奇异值向量矩阵；表示的右奇异值向量矩阵。

可选的，所述基于所述视觉伺服控制的状态方程进行控制成本函数构建处理，包括：

构建优化性能指标如下：

其中，图像特征点的代价：速度输入代价：μ_ref(l|t)；Q_p∈R^2r×2r>0,Q_u∈R^2m×2m>0为正定的权值函数；图像终端代价函数：

由此，控制成本函数如下：

其中，τ表示积分变量；表示成本函数。

可选的，所述基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理，形成非线性优化问题方程，包括：

令s_min、s_max分别表示跟踪目标特征点最小最大值，则视野约束：

令分别表示四旋翼飞行器运动速度最小最大值，引入控制速度约束：

则根据成本函数、视野约束和控制速度约束构建非线性优化问题方程如下：

其中，S表示视野约束量；U表示控制速度约束量。

另外，本发明还提供了一种四旋翼飞行器视觉受限的视觉伺服控制系统，所述系统包括：

建立模块：用于基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程；

结合模块：用于对所述摄像设备的单个特征点透视投影模型进行求导简化处理，并基于求导简化结果与所述瞬时运动学方程结合，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系；

第一构建模块：用于将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理，获得多特征点方程；

降维模块：用于采用动态模式分解对所述多特征点方程进行降维处理，获得视觉伺服控制的状态方程；

第二构建模块：用于基于所述视觉伺服控制的状态方程进行控制成本函数构建处理，获得控制成本函数；

非线性优化模块：用于基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理，形成非线性优化问题方程；

控制模块：用于基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理，并基于求解结果进行视觉伺服控制处理。

在本发明实施例中，通过本发明所提供的方法，实现在室内无GPS信号的环境下，仅依靠机载的摄像采集设备和惯性测量单元实现四旋翼对地面运动物体的跟踪控制。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见的，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是本发明实施例中的四旋翼飞行器视觉受限的视觉伺服控制方法的流程示意图；

图2是本发明实施例中的四旋翼飞行器视觉受限的视觉伺服控制系统的结构组成示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

实施例一，请参阅图1，图1是本发明实施例中的四旋翼飞行器视觉受限的视觉伺服控制方法的流程示意图。

如图1所示，一种四旋翼飞行器视觉受限的视觉伺服控制方法，所述方法包括：

S11：基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程；

在本发明具体实施过程中，所述基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程，包括：构建四旋翼飞行器动态模型中的惯性坐标系A＝{O_a,X_a,Y_a,Z_a}和机体坐标系B＝{O_b,X_b,Y_b,Z_b}以及摄像设备的虚拟二维平面投影坐标系C_p＝{V_p,U_p}，四旋翼飞行器的重心与机体坐标系的原点O_b重合；假设η^a＝[η_x η_y η_z ψ]^T表示四旋翼飞行器在惯性坐标系下的位置向量和航向角；(η_x0 η_y0 η_z0)表示重心在机体坐标系下的坐标；(f_x f_y f_z)表示摄像设备到四旋翼飞行器的重心的距离；

建立摄像设备与四旋翼飞行器的空间位置关系方程如下：

其中，s＝sin(·),c＝cos(·)；对方程(1)微分，同时引入四旋翼飞行器的机动性约束μ＝[μ_lμ_mμ_nω]^T，构建四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程如下：

其中，所述瞬时运动学方程表示四旋翼飞行器的可操作空间μ(t)到任务空间的线性映射；μ(t)表示四旋翼飞行器的机动性约束函数；ψ(t)表示航向角；J_a(ψ(t))表示映射矩阵。

进一步的，所述瞬时运动学方程中的J_a(ψ(t))如下：

其中，ψ(t)表示航向角；ψ表示航向角；s＝sin(·),c＝cos(·)；J_a(.)表示映射矩阵。

S12：对所述摄像设备的单个特征点透视投影模型进行求导简化处理，并基于求导简化结果与所述瞬时运动学方程结合，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系；

在本发明具体实施过程中，所述对所述摄像设备的单个特征点透视投影模型进行求导简化处理，包括：由摄像设备获得图像的第i个特征点在机载相机参考系C下表示为^Cξⁱ＝[^Cx^{i C}y^{i C}zⁱ]^T；第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标为^pξⁱ＝[^pu^{i p}vⁱ]^T；则摄像设备的单特征点透视投影模型如下：

其中，f_c为摄像设备的焦距；对所述单特征点透视投影模型进行求导简化处理，获得单个特征点在虚拟图像二维平面中与摄像设备的运动关系：

其中，^av_c(t)、^aω_c(t)分别表示摄像设备的速度和角速度；^cR_a表示惯性参考A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵；为Jacobian图像矩阵，表达式如下：

其中，^puⁱ表示第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标^pξⁱ内的元素；^pvⁱ表示第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标^pξⁱ内的元素；^czⁱ表示表示摄像设备的图像平面的第i个特征点坐标深度。

进一步的，所述基于求导简化结果与所述瞬时运动学方程结合，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系，包括：将所述瞬时运动学方程代入求导简化结果中，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系如下：

其中，T_a表示四旋翼飞行器运动速度表换矩阵；^cR_a表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵。

S13：将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理，获得多特征点方程；

在本发明具体实施过程中，所述多特征点方程如下：

其中：

其中，^pξⁱ＝[^pu^{i p}vⁱ]和^czⁱ分别表示摄像设备的图像平面的第i个特征点坐标及其深度；^cR_a表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵；T_a表示四旋翼飞行器运动速度表换矩阵；J_r(.)表示第r个特征点对应的Jacobian图像矩阵。

S14：采用动态模式分解对所述多特征点方程进行降维处理，获得视觉伺服控制的状态方程；

在本发明具体实施过程中，所述采用动态模式分解对所述多特征点方程进行降维处理，获得视觉伺服控制的状态方程，包括：构建所述四旋翼飞行器的状态空间表达式如下：

其中，μ_ref＝[u_lref u_mref u_nref ω_ref]^T表示四旋翼飞行器的期望速度及期望航向角速度；A表示四旋翼飞行器的动态矩阵；B表示四旋翼飞行器的控制矩阵；μ(t)为四旋翼飞行器的可操作空间；针对公式(9)的输入和四旋翼飞行器的状态进行s次的快照测量构成新的向量如下：

将公式(9)改写为：

对公式(10)改成为矩阵乘积形式如下：

采用动态模式分解对四旋翼飞行器的控制系统进行系统辨识，估计动态矩阵A和控制矩阵B，估计值如下：

通过系统辨识转换为：

将公式(8)和公式13进行结合，将状态变量重构为 得到方程如下：

令t为控制系统当前时间，T为预测范围，则公式(14)转化为控制系统预测模型：

其中，l表示预测步长；f_p(^pξ,^cz,ψ,μ)表示方程(8)的近似模型；f_μ(Φ,μ,μ_ref)表示方程(9)的近似模型；表示矩阵左奇异值向量矩阵；表示奇异值分解矩阵；表示的右奇异值向量矩阵；表示的右奇异值向量矩阵。

S15：基于所述视觉伺服控制的状态方程进行控制成本函数构建处理，获得控制成本函数；

在本发明具体实施过程中，所述基于所述视觉伺服控制的状态方程进行控制成本函数构建处理，包括：构建优化性能指标如下：

其中，图像特征点的代价：速度输入代价：μ_ref(l|t)；Q_p∈R^2r×2r>0,Q_u∈R^2m×2m>0为正定的权值函数；图像终端代价函数：

由此，控制成本函数如下：

其中，τ表示积分变量；表示成本函数。

S16：基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理，形成非线性优化问题方程；

在本发明具体实施过程中，所述基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理，形成非线性优化问题方程，包括：令s_min、s_max分别表示跟踪目标特征点最小最大值，则视野约束：

令分别表示四旋翼飞行器运动速度最小最大值，引入控制速度约束：

则根据成本函数、视野约束和控制速度约束构建非线性优化问题方程如下：

其中，S表示视野约束量；U表示控制速度约束量。

S17：基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理，并基于求解结果进行视觉伺服控制处理。

在本发明具体实施过程中，通过利用非线性最优化工具CasADI来对非线性优化问题方程中的P进行求解处理，即可得到最优化问题的求解结果，然后根据最优化问题的求解结果进行视觉伺服控制处理。

在本发明实施例中，通过本发明所提供的方法，实现在室内无GPS信号的环境下，仅依靠机载的摄像采集设备和惯性测量单元实现四旋翼对地面运动物体的跟踪控制。

实施例二，请参阅图2，图2是本发明实施例中的四旋翼飞行器视觉受限的视觉伺服控制系统的结构组成示意图。

如图2所示，一种四旋翼飞行器视觉受限的视觉伺服控制系统，所述系统包括：

建立模块21：用于基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程；

在本发明具体实施过程中，所述基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程，包括：构建四旋翼飞行器动态模型中的惯性坐标系A＝{O_a,X_a,Y_a,Z_a}和机体坐标系B＝{O_b,X_b,Y_b,Z_b}以及摄像设备的虚拟二维平面投影坐标系C_p＝{V_p,U_p}，四旋翼飞行器的重心与机体坐标系的原点O_b重合；假设η^a＝[η_x η_y η_z ψ]^T表示四旋翼飞行器在惯性坐标系下的位置向量和航向角；(η_x0 η_y0 η_z0)表示重心在机体坐标系下的坐标；(f_x f_y f_z)表示摄像设备到四旋翼飞行器的重心的距离；

建立摄像设备与四旋翼飞行器的空间位置关系方程如下：

其中，s＝sin(·),c＝cos(·)；对方程(1)微分，同时引入四旋翼飞行器的机动性约束μ＝[μ_lμ_mμ_nω]^T，构建四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程如下：

其中，所述瞬时运动学方程表示四旋翼飞行器的可操作空间μ(t)到任务空间的线性映射；μ(t)表示四旋翼飞行器的机动性约束函数；ψ(t)表示航向角；J_a(ψ(t))表示映射矩阵。

进一步的，所述瞬时运动学方程中的J_a(ψ(t))如下：

其中，ψ(t)表示航向角；ψ表示航向角；s＝sin(·),c＝cos(·)；J_a(.)表示映射矩阵。

结合模块22：用于对所述摄像设备的单个特征点透视投影模型进行求导简化处理，并基于求导简化结果与所述瞬时运动学方程结合，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系；

在本发明具体实施过程中，所述对所述摄像设备的单个特征点透视投影模型进行求导简化处理，包括：由摄像设备获得图像的第i个特征点在机载相机参考系C下表示为^Cξⁱ＝[^Cx^{i C}y^{i C}zⁱ]^T；第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标为^pξⁱ＝[^pu^{i p}vⁱ]^T；则摄像设备的单特征点透视投影模型如下：

其中，f_c为摄像设备的焦距；对所述单特征点透视投影模型进行求导简化处理，获得单个特征点在虚拟图像二维平面中与摄像设备的运动关系：

其中，^aν_c(t)、^aω_c(t)分别表示摄像设备的速度和角速度；^cR_a表示惯性参考A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵；为Jacobian图像矩阵，表达式如下：

其中，^puⁱ表示第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标^pξⁱ内的元素；^pvⁱ表示第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标^pξⁱ内的元素；^czⁱ表示表示摄像设备的图像平面的第i个特征点坐标深度。

进一步的，所述基于求导简化结果与所述瞬时运动学方程结合，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系，包括：将所述瞬时运动学方程代入求导简化结果中，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系如下：

其中，T_a表示四旋翼飞行器运动速度表换矩阵；^cR_a表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵。

第一构建模块23：用于将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理，获得多特征点方程；

在本发明具体实施过程中，所述多特征点方程如下：

其中：

其中，^pξⁱ＝[^pu^{i p}vⁱ]和^czⁱ分别表示摄像设备的图像平面的第i个特征点坐标及其深度；^cR_a表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵；T_a表示四旋翼飞行器运动速度表换矩阵；J_r(.)表示第r个特征点对应的Jacobian图像矩阵。

降维模块24：用于采用动态模式分解对所述多特征点方程进行降维处理，获得视觉伺服控制的状态方程；

在本发明具体实施过程中，所述采用动态模式分解对所述多特征点方程进行降维处理，获得视觉伺服控制的状态方程，包括：构建所述四旋翼飞行器的状态空间表达式如下：

其中，μ_ref＝[u^l _ref u_mref u_nref ω_ref]^T表示四旋翼飞行器的期望速度及期望航向角速度；A表示四旋翼飞行器的动态矩阵；B表示四旋翼飞行器的控制矩阵；μ(t)为四旋翼飞行器的可操作空间；针对公式(9)的输入和四旋翼飞行器的状态进行s次的快照测量构成新的向量如下：

将公式(9)改写为：

对公式(10)改成为矩阵乘积形式如下：

采用动态模式分解对四旋翼飞行器的控制系统进行系统辨识，估计动态矩阵A和控制矩阵B，估计值如下：

通过系统辨识转换为：

将公式(8)和公式13进行结合，将状态变量重构为 得到方程如下：

令t为控制系统当前时间，T为预测范围，则公式(14)转化为控制系统预测模型：

其中，l表示预测步长；f_p(^pξ,^cz,ψ,μ)表示方程(8)的近似模型；f_μ(Φ,μ,μ_ref)表示方程(9)的近似模型；表示矩阵左奇异值向量矩阵；表示奇异值分解矩阵；表示的右奇异值向量矩阵；表示的右奇异值向量矩阵。

第二构建模块25：用于基于所述视觉伺服控制的状态方程进行控制成本函数构建处理，获得控制成本函数；

在本发明具体实施过程中，所述基于所述视觉伺服控制的状态方程进行控制成本函数构建处理，包括：构建优化性能指标如下：

其中，图像特征点的代价：速度输入代价：μ_ref(l|t)；Q_p∈R^2r×2r>0,Q_u∈R^2m×2m>0为正定的权值函数；图像终端代价函数：

由此，控制成本函数如下：

其中，τ表示积分变量；表示成本函数。

非线性优化模块26：用于基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理，形成非线性优化问题方程；

在本发明具体实施过程中，所述基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理，形成非线性优化问题方程，包括：令s_min、s_max分别表示跟踪目标特征点最大最小值，则视野约束：

令分别表示四旋翼飞行器运动速度最小最大值，引入控制速度约束：

则根据成本函数、视野约束和控制速度约束构建非线性优化问题方程如下：

其中，S表示视野约束量；U表示控制速度约束量。

控制模块27：用于基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理，并基于求解结果进行视觉伺服控制处理。

在本发明具体实施过程中，通过利用非线性最优化工具CasADI来对非线性优化问题方程中的P进行求解处理，即可得到最优化问题的求解结果，然后根据最优化问题的求解结果进行视觉伺服控制处理。

在本发明实施例中，通过本发明所提供的方法，实现在室内无GPS信号的环境下，仅依靠机载的摄像采集设备和惯性测量单元实现四旋翼对地面运动物体的跟踪控制。

本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、磁盘或光盘等。

另外，以上对本发明实施例所提供的一种四旋翼飞行器视觉受限的视觉伺服控制方法及系统进行了详细介绍，本文中应采用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (8)

1.一种四旋翼飞行器视觉受限的视觉伺服控制方法，其特征在于，所述方法包括：

基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程；

对所述摄像设备的单个特征点透视投影模型进行求导简化处理，并基于求导简化结果与所述瞬时运动学方程结合，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系；

将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理，获得多特征点方程；

采用动态模式分解对所述多特征点方程进行降维处理，获得视觉伺服控制的状态方程；

基于所述视觉伺服控制的状态方程进行控制成本函数构建处理，获得控制成本函数；

基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理，形成非线性优化问题方程；

基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理，并基于求解结果进行视觉伺服控制处理；

其中，所述基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程，包括：

构建四旋翼飞行器动态模型中的惯性坐标系A＝{O_a,X_a,Y_a,Z_a}和机体坐标系B＝{O_b,X_b,Y_b,Z_b}，机载相机参考系C＝{O_c,X_c,Y_c,Z_c}摄像设备的虚拟二维平面投影坐标系C_p＝{V_p,U_p}，四旋翼飞行器的重心与机体坐标系的原点O_b重合；

假设η^a＝[η_x η_y η_z ψ]^T表示四旋翼飞行器在惯性坐标系下的位置向量和航向角；(η_x0η_y0 η_z0)表示重心在机体坐标系下的坐标；(f_x f_y f_z)表示摄像设备到四旋翼飞行器的重心的距离；

建立摄像设备与四旋翼飞行器的空间位置关系方程如下：

其中，s＝sin(·),c＝cos(·)；对方程(1)微分，同时引入四旋翼飞行器的机动性约束μ＝[μ_lμ_mμ_nω]^T，构建四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程如下：

其中，所述瞬时运动学方程中的J_a(ψ(t))如下：

所述瞬时运动学方程表示四旋翼飞行器的可操作空间μ(t)到任务空间的线性映射；μ(t)表示四旋翼飞行器的机动性约束函数；ψ表示航向角；ψ(t)表示航向角的时间函数；J_a(ψ(t))表示映射矩阵；

2.根据权利要求1所述的视觉伺服控制方法，其特征在于，所述对所述摄像设备的单个特征点透视投影模型进行求导简化处理，包括：

由摄像设备获得图像的第i个特征点在机载相机参考系C下表示为^Cξⁱ＝[^Cx^iCy^iCzⁱ]^T；第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标为^pξⁱ＝[^pu^ipvⁱ]^T；

则摄像设备的单特征点透视投影模型如下：

其中，f_c为摄像设备的焦距；

对所述单特征点透视投影模型进行求导简化处理，获得单个特征点在虚拟图像二维平面中与摄像设备的运动关系：

其中，^aν_c(t)、^aω_c(t)分别表示摄像设备的速度和角速度；^cR_a表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵；为Jacobian图像矩阵，表达式如下：

其中，^puⁱ表示第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标^pξⁱ内的元素；^pvⁱ表示第i个特征点在虚拟二维平面投影坐标系C_p下对应坐标^pξⁱ内的元素；^czⁱ表示表示摄像设备的图像平面的第i个特征点坐标深度。

3.根据权利要求1所述的视觉伺服控制方法，其特征在于，所述基于求导简化结果与所述瞬时运动学方程结合，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系，包括：

将所述瞬时运动学方程代入求导简化结果中，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系如下：

其中，T_a表示四旋翼飞行器运动速度表换矩阵；^cR_a表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵。

4.根据权利要求1所述的视觉伺服控制方法，其特征在于，所述多特征点方程如下：

其中：

其中，^pξⁱ＝[^pu^ipvⁱ]和^czⁱ分别表示摄像设备的图像平面的第i个特征点坐标及其深度；^cR_a表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系C_p的旋转矩阵；T_a表示四旋翼飞行器运动速度表换矩阵；J_r(.)表示第r个特征点对应的Jacobian图像矩阵。

5.根据权利要求1所述的视觉伺服控制方法，其特征在于，所述采用动态模式分解对所述多特征点方程进行降维处理，获得视觉伺服控制的状态方程，包括：

构建所述四旋翼飞行器的状态空间表达式如下：

其中，μ_ref＝[u^l _ref u_mref u_nref ω_ref]^T表示四旋翼飞行器的期望速度及期望航向角速度；A表示四旋翼飞行器的动态矩阵；B表示四旋翼飞行器的控制矩阵；

针对公式(9)的输入和四旋翼飞行器的状态进行s次的快照测量构成新的向量如下：

将公式(9)改写为：

对公式(10)改成为矩阵乘积形式如下：

采用动态模式分解对四旋翼飞行器的控制系统进行系统辨识，估计动态矩阵A和控制矩阵B，估计值如下：

通过系统辨识转换为：

将公式(8)和公式13进行结合，将状态变量重构为 得到方程如下：

令t为控制系统当前时间，T为预测范围，则公式(14)转化为控制系统预测模型：

其中，l表示预测步长；f_p(^pξ,^cz,ψ,μ)表示方程(8)的近似模型；f_μ(Φ,μ,μ_ref)表示方程(9)的近似模型；表示矩阵左奇异值向量矩阵；表示奇异值分解矩阵；表示的右奇异值向量矩阵；表示的右奇异值向量矩阵。

6.根据权利要求5所述的视觉伺服控制方法，其特征在于，所述基于所述视觉伺服控制的状态方程进行控制成本函数构建处理，包括：

构建优化性能指标如下：

其中，图像特征点的代价：速度输入代价：μ_ref(l|t)；Q_p∈R^2r×2r>0,Q_u∈R^2m×2m>0为正定的权值函数；图像终端代价函数：

由此，控制成本函数如下：

其中，τ表示积分变量；表示成本函数。

7.根据权利要求6所述的视觉伺服控制方法，其特征在于，所述基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理，形成非线性优化问题方程，包括：

令s_min、s_max分别表示跟踪目标特征点最小最大值，则视野约束：

令分别表示四旋翼飞行器运动速度最小最大值，引入控制速度约束：

则根据成本函数、视野约束和控制速度约束构建非线性优化问题方程如下：

其中，S表示视野约束量；U表示控制速度约束量。

8.一种采用了如权利要求1-7任何一项所述方法的四旋翼飞行器视觉受限的视觉伺服控制系统，其特征在于，所述系统包括：

建立模块：用于基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程；

结合模块：用于对所述摄像设备的单个特征点透视投影模型进行求导简化处理，并基于求导简化结果与所述瞬时运动学方程结合，获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系；

第一构建模块：用于将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理，获得多特征点方程；

降维模块：用于采用动态模式分解对所述多特征点方程进行降维处理，获得视觉伺服控制的状态方程；

第二构建模块：用于基于所述视觉伺服控制的状态方程进行控制成本函数构建处理，获得控制成本函数；

非线性优化模块：用于基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理，形成非线性优化问题方程；

控制模块：用于基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理，并基于求解结果进行视觉伺服控制处理。