CN116774589A - 鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法 - Google Patents

Abstract

本发明提供了一种鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，包括：获取图像数据并提取图像特征，获取无人机姿态信息，整合得到总体系统模型；设置高增益状态观测器和深度估计器对无人机运动速度和特征深度进行估计；构建性能约束和紧状态约束，基于性能约束和紧状态约束设计鲁棒非线性模型预测控制视觉伺服控制器，使得无人机在随机扰动下能够对地面目标进行追踪，并且整个追踪过程满足预先构建的性能约束本发明解决了传统控制器在时变、欠驱动、状态耦合的非线性系统中的适用性较差、无法对系统状态量和输入量进行约束的问题，实现了无人机平滑追踪地面目标并保持目标在相机视野范围内。

Description

鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法

技术领域

本发明涉及无人机目标追踪技术领域，特别涉及一种鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法。

背景技术

近年来，四旋翼无人机在各种应用领域中得到了广泛的应用。在农业领域四旋翼无人机可以进行精准的作物喷洒和监测，提高农作物的产量和质量；在军用方面，四旋翼无人机可以用于战场侦察、目标追踪、空中侦察和打击等任务，具有重要的作用；在消防领域，四旋翼无人机可以用于高空灭火和火场侦查，提高灭火效率和救援速度；在安保领域，四旋翼无人机可以用于边境巡查和监测等任务，提高边境安全水平。这些应用场景都需要四旋翼无人机具有较高的自主飞行和导航能力，而视觉伺服技术正是实现这一目标的重要手段。

视觉伺服技术是指通过摄像头获取实时图像，将图像信息处理成控制指令，从而实现四旋翼无人机的自主飞行、导航和任务执行等功能。相比于传统的惯性导航系统，视觉伺服技术具有更高的精度和鲁棒性，能够适应更加复杂和多变的环境，实现更加精准和可靠的控制。因此，视觉伺服技术在无人机的控制和应用中发挥着不可替代的作用。

四旋翼无人机在视觉伺服目标追踪领域一直是研究的热点之一。随着机载计算机的计算能力和传感器的不断提升，四旋翼无人机在目标追踪和识别方面的能力也得到了大幅提高。目标追踪技术在军事、安防、交通等领域都有广泛应用，可以有效提高自主飞行器的应用效果和实用价值。

然而，在实际应用中，四旋翼无人机在视觉伺服目标追踪方面仍然存在一些挑战和难点。首先，目标在追踪过程中可能会出现光照变化、遮挡和运动模糊等问题，影响追踪效果。其次，由于视觉传感器的局限性，目标在不同距离和角度下的特征信息可能会发生变化，导致追踪算法需要针对不同情况进行调整。此外，飞行平台本身的姿态变化和动态控制也会影响追踪算法的实时性和鲁棒性。因此，如何提高视觉伺服目标追踪的鲁棒性、实时性和精度，仍然是当前研究的难点和挑战。

发明内容

本发明的目的是：针对上述背景技术中存在的不足，提供一种视觉伺服目标追踪控制方法，以解决传统视觉伺服控制器在时变、欠驱动、状态耦合的非线性系统中的适用性较差、无法对系统状态量和输入量进行约束的问题。

为了达到上述目的，本发明提供了一种鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，包括如下步骤：

一种鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，其特征在于，包括如下步骤：

S1，获取图像数据并提取图像特征，获取无人机姿态信息，分别得到图像动力学模型和无人机动力学模型，将图像动力学模型和无人机动力学模型整合得到总体系统模型；

S2，设置高增益观测器，计算无人机运动速度，同时设置深度估计器对总体系统模型中特征深度进行估计；

S3，根据无人机目标追踪过程中无人机运动速度约束、特征状态约束和控制量约束条件,设置基于鲁棒非线性模型预测控制的无人机视觉伺服目标追踪控制器，控制器的控制量为无人机的线速度和角速度期望指令，经过控制器的指令转换模块转换成期望姿态和油门指令，控制无人机运动进行目标追踪，追踪过程满足预先构件的约束。

进一步地，其特征在于，S1中设定相机坐标系中的一某个三维特征点对应在二维图像平面坐标为/>根据单目相机投影模型/>与/>的关系如下：

其中，λ为相机的焦距；

设定被追踪物体由n个特征点组成，其在虚拟图像平面上的坐标为 图像矩/>中心矩/>其中/> m₀₀为零阶矩，m₁₀、m₀₁均为一阶矩；所选图像矩特征的具体表达式如下：

其中，a＝μ₂₀+μ₀₂；

得到图像动力模型：

其中，为无人机在虚拟坐标系/>中的线速度，/>为偏航角速度，/>是特征点深度，q＝[q_x,q_y,q_z,q_ψ]^T为图像矩特征；

无人机动力学模型为：

其中，f＝[f_x,f_y,f_z]^T，f_x,f_y,f为虚拟平面中三个方向的加速度；

将图像动力学模型和无人机动力学模型整合到一起，得到总体系统模型：

其中，

进一步地，深度估计器具体形式为：

其中：

为特征点深度的估计值。

进一步地，高增益观测器的具体形式为：

其中，为状态η的估计值，/>为特征点深度的估计值，y＝[q₁,q₂,q₃]^T为相机获取的图像特征，/>1≤i≤3，H_i是观测器增益矩阵，ε是高增益观测器参数，/>是方程/>的两个正根，为观测器非线性项。

进一步地，观测器非线性项中

进一步地，S3中在构建约束时，针对总体系统模型并考虑有界扰动：

其中，表示噪声信号的上确界，/>是紧集，是二次连续可微的非线性函数；对应名义系统为：

定义实际系统状态误差名义系统状态误差/> 为名义系统的状态，控制器的代价函数如下：

其中，矩阵Q、P、R是正定矩阵，T是预测步长，是根据名义系统在初始时刻为t_k时系统预测状态误差，/>在初始时刻为t_k时预测控制输入，s∈[t_k,t_k+T]；控制器进行优化写成如下形式：

其中，V_T表示优化问题的目标函数，对上述问题求解得到/>J^*(x(t_k))，其中，/>是预测步长内的最优控制序列，/>是输入为最优控制/>时的最优状态轨迹，J^*(x(t_k))为代价函数最优值；/>是紧状态约束，通过下面两个式子求解：

其中代表集合/>和集合/>的庞特里亚金差，/>是根据噪声信息设置的状态边界。

本发明还提供了一种计算机可读介质，其上存储有计算机程序，该程序被处理器执行时实现如前所述的鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法。

本发明的上述方案有如下的有益效果：

本发明提供的鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，利用了图像矩设计能够反映平移和旋转的图像特征，提高了追踪过程中特征点的鲁棒性；通过高增益观测器解决了加速度计噪声带来的速度信号估计不准确问题；根据系统状态方程信息，通过特征深度估计器估计目标深度信息；通过基于鲁棒非线性模型预测控制的无人机视觉伺服目标追踪控制器，解决了传统控制器在时变、欠驱动、状态耦合的非线性系统中的适用性较差、无法对系统状态量和输入量进行约束的问题，实现了无人机平滑追踪地面目标并保持目标在相机视野范围内；

本发明的其它有益效果将在随后的具体实施方式部分予以详细说明。

附图说明

图1为本发明的框架示意图；

图2为本发明中基于图像的视觉伺服无人机目标追踪示意图；

图3为本发明实施例中的实验结果图，其中(a)为图像特征误差，(b)为特征点运动轨迹，(c)为控制量，(d)为无人机运动速度；

图4为本发明的无人机三维轨迹示意图。

具体实施方式

以下通过特定的具体实例说明本公开的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本公开的其他优点与功效。显然，所描述的实施例仅仅是本公开一部分实施例，而不是全部的实施例。本公开还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本公开的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本公开中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。

需要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本公开，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目个方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。

还需要说明的是，以下实施例中所提供的图示仅以示意方式说明本公开的基本构想，图式中仅显示与本公开中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践所述方面。

请参阅图1、图2，本发明的实施例提供了一种鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，包括如下步骤：

S1，基于单目相机获取图像数据，利用虚拟相机建立解耦的图像动力学模型，以消除四旋翼无人机在追踪过程中横滚角和俯仰角带来的影响。

在该步骤中，假设相机坐标系中的一某个三维特征点/>对应在二维图像平面坐标为/>根据单目相机投影模型/>与/>的关系如下：

其中，λ为相机的焦距，其为相机的内部参数在相机出厂时就已经标定好。

假设被追踪物体由n个特征点组成，其在虚拟图像平面上的坐标为 图像矩/>中心矩/>其中/> m₀₀为零阶矩，m₁₀、m₀₁均为一阶矩。所选图像矩特征的具体表达式如下：

其中，a＝μ₂₀+μ₀₂。

得到图像动力学模型：

其中，为无人机在虚拟坐标系/>中的线速度，/>为偏航角速度，/>是特征点深度，需要通过在线深度估计器对其进行在线估计，q＝[q_x,q_y,q_z,q_ψ]^T为图像矩特征。

利用惯性单元获得无人机姿态信息，从而得到无人机动力学模型为：

其中，f_x,f_y,f为虚拟平面中三个方向的加速度。

最终将图像动力学模型和无人机动力学模型整合到一起，得到总体系统模型：

其中，

S2，设置高增益观测器，计算无人机运动速度。

在该步骤中，由于总体系统模型中存在时变且无法使用单目相机直接测量的深度和无人机运动速度，因此还设置深度估计器配合高增益状态观测器对总体系统模型中特征深度和无人机运动速度进行估计，解决了因加速度计噪声大带来的速度信号估计不准确问题，使视觉伺服无人机目标追踪系统利用单目相机就可以获取目标深度信息，

其中，深度估计器具体形式为：

其中：

高增益观测器的具体形式为：

其中，为状态η的估计值，/>为特征点深度的估计值，y＝[q₁,q₂,q₃]^T为相机获取的图像特征，/>1≤i≤3，H_i是观测器增益矩阵，ε是高增益观测器参数，需要足够小，/>是方程/>的两个正根，为观测器非线性项，且：

高增益观测器对无人机运动速度的估计值可以为后续制器提供近似的状态信息。

S3，根据无人机目标追踪过程中无人机运动速度约束、特征状态约束和控制量约束条件,设置基于鲁棒非线性模型预测控制的无人机视觉伺服目标追踪控制器，控制器的控制量为无人机的线速度和角速度期望指令，经过控制器的指令转换模块转换成期望姿态和油门指令，控制无人机运动进行目标追踪。

在构建约束时，针对总体系统模型并考虑有界扰动：

其中，表示噪声信号的上确界，/>是紧集，是二次连续可微的非线性函数。对应名义系统为：

定义实际系统状态误差名义系统状态误差/> 为名义系统的状态，控制器的代价函数如下：

其中，矩阵Q、P、R是正定矩阵，T是预测步长，是根据名义系统在初始时刻为t_k时系统预测状态误差，/>在初始时刻为t_k时预测控制输入，是待优化变量，s∈[t_k,t_k+T]。最终整体优化可以写成如下形式：

其中，V_T表示优化问题的目标函数，对上述问题求解可以得到/> J^*(x(t_k))，其中，/>是预测步长内的最优控制序列，/>是输入为最优控制/>时的最优状态轨迹，J^*(x(t_k))为代价函数最优值。/>是紧状态约束，可通过下面两个式子求解：

其中代表集合/>和集合/>的庞特里亚金差，/>是根据噪声信息设置的状态边界。设置紧状态约束的目的是增强系统的鲁棒性，如果/>则/>可以通过离线方式设置紧状态约束/>与传统MPC控制方法相比，紧状态约束的引入并没有增加算法的计算规模。

通过控制器实现了无人机较为平滑地追踪地面目标,且能有效保证目标始终处于相机的视野范围内。即使系统存在外部扰动，仍能满足预先设计的性能约束。

为了进一步证明本方案的有效性，本实施例中基于自行设计的实验平台进行实验，实验目标是利用图像作为反馈数据控制无人机悬停在地面物体正上方1m处。控制器参数如下：期望特征q_r＝[0,0,1,0]^T，采样时间δ＝0.1s，预测周期T＝0.3s，Q＝diag[1,1,1,1,0.1,0.1,0.1]^T，R＝[0.1,0.1,0.1,0.1]^T，ε＝0.1，里普利兹常数L＝1.06，状态约束x_max＝[5,5,5,3.14,1,1,1]^T，x_min＝-x_max；控制量约束u_max＝[1,1,1,0.1]^T，u_min＝-u_max。姿态环采用Pixhawk6C默认的串级PID控制器,控制周期为0.01s。在实验开始之前，将无人机放置在期望位置，测量出a^*＝5×10^-8，将无人机放置在初始位置，测量出特征点深度初始值Z(0)＝1.53m。

实验结果如图3所示。图3(a)展示了视觉伺服任务过程中图像矩误差的收敛过程，从局部放大图中可以清晰看到所有图像矩误差最终进入到预先设计的控制不变集Ω(ε)中，并保持稳定。图3(b)展示了视觉伺服任务过程中特征点在二维图像平面的运动轨迹，可以看到特征点的运动轨迹近似一条直线，最终收敛到预先设计的终端集合中。从图3(c)和图3(d)可以看出，视觉伺服任务过程中控制输入和无人机的运动速度始终满足控制输入约束和速度约束/>整个过程中无人机的三维运动轨迹如图4曲线所示。实际飞行实验无人机会在期望位置产生小幅度抖动，这是由于实验之前并不知道干扰ω的上确界。另一个原因是图像数据、无人机状态数据通过wifi发送到地面站产生了延迟，导致控制指令无法及时地作用到无人机。尽管如此，无人机最终仍能在满足约束地前提下实现对地面目标地稳定追踪，验证了本方案的有效性。

基于同一发明构思，本实施例还提供了一种计算机可读介质，其上存储有计算机程序，该程序被处理器执行时实现前述的鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法。

该计算机可读介质包括但不限于任何类型的盘(包括软盘、硬盘、光盘、CD-ROM、和磁光盘)、ROM、RAM、EPROM(Erasable Programmable Read-Only Memory，可擦写可编程只读存储器)、EEPROM、闪存、磁性卡片或光线卡片。也就是说，可读介质包括由设备(例如计算机)以能够读的形式存储或传输信息的任何介质。

本实施例提供的计算机可读存储介质，与前述的方法具有相同的发明构思及相同的有益效果，在此不再赘述。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，其特征在于，包括如下步骤：

S1，获取图像数据并提取图像特征，获取无人机姿态信息，分别得到图像动力学模型和无人机动力学模型，将图像动力学模型和无人机动力学模型整合得到总体系统模型；

S2，设置高增益观测器，计算无人机运动速度，同时设置深度估计器对总体系统模型中特征深度进行估计；

S3，根据无人机目标追踪过程中无人机运动速度约束、特征状态约束和控制量约束条件,设置基于鲁棒非线性模型预测控制的无人机视觉伺服目标追踪控制器，控制器的控制量为无人机的线速度和角速度期望指令，经过控制器的指令转换模块转换成期望姿态和油门指令，控制无人机运动进行目标追踪，追踪过程满足预先构件的约束。

2.根据权利要求1所述的鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，其特征在于，S1中设定相机坐标系中的一某个三维特征点/>对应在二维图像平面坐标为/>根据单目相机投影模型/>与/>的关系如下：

其中，λ为相机的焦距；

设定被追踪物体由n个特征点组成，其在虚拟图像平面上的坐标为 图像矩/>中心矩/>其中/> m₀₀为零阶矩，m₁₀、m₀₁均为一阶矩；所选图像矩特征的具体表达式如下：

其中，a＝μ₂₀+μ₀₂；

得到图像动力模型：

其中，为无人机在虚拟坐标系/>中的线速度，/>为偏航角速度，/>是特征点深度，q＝[q_x,q_y,q_z,q_ψ]^T为图像矩特征；

无人机动力学模型为：

其中，f＝[f_x,f_y,f_z]^T，f_x,f_y,f为虚拟平面中三个方向的加速度；

将图像动力学模型和无人机动力学模型整合到一起，得到总体系统模型：

其中，x＝[x₁,x₂,x₃,x₄,x₅,x₆,x₇]^T＝[q,V^ν]^T，

3.根据权利要求2所述的鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，其特征在于，深度估计器具体形式为：

其中：

为特征点深度的估计值。

4.根据权利要求3所述的鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，其特征在于，高增益观测器的具体形式为：

其中，为状态η的估计值，/>为特征点深度的估计值，y＝[q₁,q₂,q₃]^T为相机获取的图像特征，/>1≤i≤3，H_i是观测器增益矩阵，ε是高增益观测器参数，/>是方程/>的两个正根，/>为观测器非线性项。

5.根据权利要求4所述的鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，其特征在于，观测器非线性项中

6.根据权利要求5所述的鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法，其特征在于，S3中在构建约束时，针对总体系统模型并考虑有界扰动：

其中，表示噪声信号的上确界，/>是紧集，f:是二次连续可微的非线性函数；对应名义系统为：

定义实际系统状态误差名义系统状态误差/>为名义系统的状态，控制器的代价函数如下：

其中，矩阵Q、P、R是正定矩阵，T是预测步长，是根据名义系统在初始时刻为t_k时系统预测状态误差，/>在初始时刻为t_k时预测控制输入，s∈[t_k,t_k+T]；控制器进行优化写成如下形式：

其中，V_T表示优化问题的目标函数，对上述问题求解得到/>J^*(x(t_k))，其中，/>是预测步长内的最优控制序列，/>是输入为最优控制/>时的最优状态轨迹，J^*(x(t_k))为代价函数最优值；/>是紧状态约束，通过下面两个式子求解：

其中代表集合/>和集合/>的庞特里亚金差，/>是根据噪声信息设置的状态边界。

7.一种计算机可读介质，其上存储有计算机程序，该程序被处理器执行时实现如权利要求1-6任意一项所述的鲁棒非线性模型预测控制的视觉伺服目标追踪控制方法。