CN116700348B - 一种四旋翼飞行器视觉受限的视觉伺服控制方法及*** - Google Patents
一种四旋翼飞行器视觉受限的视觉伺服控制方法及*** Download PDFInfo
- Publication number
- CN116700348B CN116700348B CN202310855217.XA CN202310855217A CN116700348B CN 116700348 B CN116700348 B CN 116700348B CN 202310855217 A CN202310855217 A CN 202310855217A CN 116700348 B CN116700348 B CN 116700348B
- Authority
- CN
- China
- Prior art keywords
- equation
- quadrotor
- feature point
- control
- image
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 75
- 230000000007 visual effect Effects 0.000 title claims abstract description 65
- 238000005457 optimization Methods 0.000 claims abstract description 54
- 238000012545 processing Methods 0.000 claims abstract description 45
- 238000010276 construction Methods 0.000 claims abstract description 29
- 230000009467 reduction Effects 0.000 claims abstract description 14
- 238000005259 measurement Methods 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 67
- 230000008569 process Effects 0.000 claims description 24
- 238000000354 decomposition reaction Methods 0.000 claims description 18
- 230000005484 gravity Effects 0.000 claims description 12
- 238000013507 mapping Methods 0.000 claims description 11
- 238000009795 derivation Methods 0.000 claims description 8
- 230000008859 change Effects 0.000 claims description 4
- 238000003384 imaging method Methods 0.000 claims description 4
- 229940050561 matrix product Drugs 0.000 claims description 4
- 230000006870 function Effects 0.000 description 40
- 238000013461 design Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Image Processing (AREA)
Abstract
本发明公开了一种四旋翼飞行器视觉受限的视觉伺服控制方法及***,所述方法包括:建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程;对摄像设备的单个特征点透视投影模型进行求导简化处理,并基于求导简化结果与所述瞬时运动学方程结合;将单个特征点在虚拟图像二维平面中与摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理;对多特征点方程进行降维处理;进行控制成本函数构建处理;进行非线性优化处理,形成非线性优化问题方程;对非线性优化问题方程进行求解处理,并基于求解结果进行视觉伺服控制处理。在本发明实施例中,实现仅靠摄像设备和惯性测量即可实现四旋翼飞行器对地面运动物体的跟踪控制。
Description
技术领域
本发明涉及飞行控制技术领域,尤其涉及一种四旋翼飞行器视觉受限的视觉伺服控制方法及***。
背景技术
如今,多旋翼飞行器,尤其是四旋翼飞行器,由于具有很高的机动性,较低的购置和维护成本以及出色的垂直起降能力,即使在恶劣的环境下,也被认为是最受欢迎的自动驾驶空中平台。此外,导航、感知传感器和高性能电池技术的快速进展显著提高了四旋翼无人机的续航和载荷能力,使其成为各种任务的理想平台,例如搜索和救援、区域覆盖、监视、物体运输和智能农业。
尽管在自主多旋翼操作过程中使用了许多导航传感器(GPS,INS等),但由于摄像机重量轻、体积小、无源、低功耗等有点,使得其在机器人运动控制中至关重要,以便能够精确监视和跟踪感兴趣的区域和目标。如果是无人飞行***,例如四旋翼飞行器,则可以通过将摄像头传感器直接安装在机器人上来轻松实现,从而形成所谓的“眼在手”***。传统的视觉伺服方法依赖于一组视觉特征的提取,跟踪和匹配,这些视觉特征被用作控制器的输入,以引导机器人完成相应的控制任务。
由于对视觉伺服方法和应用的研究的持续深入,图像处理的逐渐成熟,和各种控制方法的发展,视觉伺服控制在机器人***的发展和应用等方面展现出了巨大的价值。关于视觉伺服控制,可以定义四个主要类别:i)基于位置的视觉伺服(PBVS),其中所选用的控制误差定义在笛卡尔空间中。ii)基于图像的视觉伺服(IBVS),其中控制误差函数在图像空间中定义;iii)2-1/2或混合视觉伺服,其中控制误差函数在笛卡尔和图像空间中部分定义;iv)直接视觉伺服,无需提取特定特征,而是在控制设计中利用完整图像。上述方法各有利弊,其功效在很大程度上取决于应用需求。但是,IBVS被认为对航空器应用特别有效,特别是对移动目标的跟踪,因为它具有更好的局部稳定性和收敛性,这是因为它具有针对相机校准缺陷和建模误差的固有鲁棒性。
但由于四旋翼无人机动力学与机载相机运动学的耦合关系以及机载相机的视角限制给基于IBVS的四旋翼无人机控制器设计带来了困难。此外,机载相机的校准以及目标特征的深度的精确性也制约着四旋翼无人机视觉伺服控制器的设计。
发明内容
本发明的目的在于克服现有技术的不足,本发明提供了一种四旋翼飞行器视觉受限的视觉伺服控制方法及***,实现仅靠摄像设备和惯性测量即可实现四旋翼飞行器对地面运动物体的跟踪控制。
为了解决上述技术问题,本发明提供了一种四旋翼飞行器视觉受限的视觉伺服控制方法,所述方法包括:
基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程;
对所述摄像设备的单个特征点透视投影模型进行求导简化处理,并基于求导简化结果与所述瞬时运动学方程结合,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系;
将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理,获得多特征点方程;
采用动态模式分解对所述多特征点方程进行降维处理,获得视觉伺服控制的状态方程;
基于所述视觉伺服控制的状态方程进行控制成本函数构建处理,获得控制成本函数;
基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理,形成非线性优化问题方程;
基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理,并基于求解结果进行视觉伺服控制处理。
可选的,所述基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程,包括:
构建四旋翼飞行器动态模型中的惯性坐标系A={Oa,Xa,Ya,Za}和机体坐标系B={Ob,Xb,Yb,Zb}以及摄像设备的虚拟二维平面投影坐标系Cp={Vp,Up},四旋翼飞行器的重心与机体坐标系的原点Ob重合;
假设ηa=[ηxηyηzψ]T表示四旋翼飞行器在惯性坐标系下的位置向量和航向角;(ηx0ηy0ηz0)表示重心在机体坐标系下的坐标;(fx fy fz)表示摄像设备到四旋翼飞行器的重心的距离;
建立摄像设备与四旋翼飞行器的空间位置关系方程如下:
其中,s=sin(·),c=cos(·);对方程(1)微分,同时引入四旋翼飞行器的机动性约束μ=[μlμmμnω]T,构建四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程如下:
其中,所述瞬时运动学方程表示四旋翼飞行器的可操作空间μ(t)到任务空间的线性映射;μ(t)表示四旋翼飞行器的机动性约束函数;ψ(t)表示航向角;Ja(ψ(t))表示映射矩阵。
可选的,所述瞬时运动学方程中的Ja(ψ(t))如下:
其中,ψ(t)表示航向角;ψ表示航向角;s=sin(·),c=cos(·);Ja(.)表示映射矩阵。
可选的,所述对所述摄像设备的单个特征点透视投影模型进行求导简化处理,包括:
由摄像设备获得图像的第i个特征点在机载相机参考系C下表示为Cξi=[Cxi Cyi Czi]T;第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标为pξi=[pui pvi]T;
则摄像设备的单特征点透视投影模型如下:
其中,fc为摄像设备的焦距;
对所述单特征点透视投影模型进行求导简化处理,获得单个特征点在虚拟图像二维平面中与摄像设备的运动关系:
其中,avc(t)、aωc(t)分别表示摄像设备的速度和角速度;cRa表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵;为Jacobian图像矩阵,表达式如下:
其中,pui表示第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标pξi内的元素;pvi表示第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标pξi内的元素;czi表示表示摄像设备的图像平面的第i个特征点坐标深度。
可选的,所述基于求导简化结果与所述瞬时运动学方程结合,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系,包括:
将所述瞬时运动学方程代入求导简化结果中,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系如下:
其中,Ta表示四旋翼飞行器运动速度表换矩阵;cRa表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵。
可选的,所述多特征点方程如下:
其中:
其中,pξi=[pui pvi]和czi分别表示摄像设备的图像平面的第i个特征点坐标及其深度;cRa表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵;Ta表示四旋翼飞行器运动速度表换矩阵;Jr(.)表示第r个特征点对应的Jacobian图像矩阵。
可选的,所述采用动态模式分解对所述多特征点方程进行降维处理,获得视觉伺服控制的状态方程,包括:
构建所述四旋翼飞行器的状态空间表达式如下:
其中,μref=[ulref umref unref ωref]T表示四旋翼飞行器的期望速度及期望航向角速度;A表示四旋翼飞行器的动态矩阵;B表示四旋翼飞行器的控制矩阵;μ(t)为四旋翼飞行器的可操作空间;
针对公式(9)的输入和四旋翼飞行器的状态进行s次的快照测量构成新的向量如下:
将公式(9)改写为:
对公式(10)改成为矩阵乘积形式如下:
采用动态模式分解对四旋翼飞行器的控制***进行***辨识,估计动态矩阵A和控制矩阵B,估计值如下:
通过***辨识转换为:
将公式(8)和公式13进行结合,将状态变量重构为 得到方程如下:
令t为控制***当前时间,T为预测范围,则公式(14)转化为控制***预测模型:
其中,l表示预测步长;fp(pξ,cz,ψ,μ)表示方程(8)的近似模型;fμ(Φ,μ,μref)表示方程(9)的近似模型;表示矩阵/>左奇异值向量矩阵;/>表示奇异值分解矩阵;/>表示/>的右奇异值向量矩阵;/>表示/>的右奇异值向量矩阵。
可选的,所述基于所述视觉伺服控制的状态方程进行控制成本函数构建处理,包括:
构建优化性能指标如下:
其中,图像特征点的代价:速度输入代价:μref(l|t);Qp∈R2r×2r>0,Qu∈R2m×2m>0为正定的权值函数;图像终端代价函数:
由此,控制成本函数如下:
其中,τ表示积分变量;表示成本函数。
可选的,所述基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理,形成非线性优化问题方程,包括:
令smin、smax分别表示跟踪目标特征点最小最大值,则视野约束:
令分别表示四旋翼飞行器运动速度最小最大值,引入控制速度约束:
则根据成本函数、视野约束和控制速度约束构建非线性优化问题方程如下:
其中,S表示视野约束量;U表示控制速度约束量。
另外,本发明还提供了一种四旋翼飞行器视觉受限的视觉伺服控制***,所述***包括:
建立模块:用于基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程;
结合模块:用于对所述摄像设备的单个特征点透视投影模型进行求导简化处理,并基于求导简化结果与所述瞬时运动学方程结合,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系;
第一构建模块:用于将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理,获得多特征点方程;
降维模块:用于采用动态模式分解对所述多特征点方程进行降维处理,获得视觉伺服控制的状态方程;
第二构建模块:用于基于所述视觉伺服控制的状态方程进行控制成本函数构建处理,获得控制成本函数;
非线性优化模块:用于基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理,形成非线性优化问题方程;
控制模块:用于基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理,并基于求解结果进行视觉伺服控制处理。
在本发明实施例中,通过本发明所提供的方法,实现在室内无GPS信号的环境下,仅依靠机载的摄像采集设备和惯性测量单元实现四旋翼对地面运动物体的跟踪控制。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见的,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其它的附图。
图1是本发明实施例中的四旋翼飞行器视觉受限的视觉伺服控制方法的流程示意图;
图2是本发明实施例中的四旋翼飞行器视觉受限的视觉伺服控制***的结构组成示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
实施例一,请参阅图1,图1是本发明实施例中的四旋翼飞行器视觉受限的视觉伺服控制方法的流程示意图。
如图1所示,一种四旋翼飞行器视觉受限的视觉伺服控制方法,所述方法包括:
S11:基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程;
在本发明具体实施过程中,所述基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程,包括:构建四旋翼飞行器动态模型中的惯性坐标系A={Oa,Xa,Ya,Za}和机体坐标系B={Ob,Xb,Yb,Zb}以及摄像设备的虚拟二维平面投影坐标系Cp={Vp,Up},四旋翼飞行器的重心与机体坐标系的原点Ob重合;假设ηa=[ηx ηy ηz ψ]T表示四旋翼飞行器在惯性坐标系下的位置向量和航向角;(ηx0 ηy0 ηz0)表示重心在机体坐标系下的坐标;(fx fy fz)表示摄像设备到四旋翼飞行器的重心的距离;
建立摄像设备与四旋翼飞行器的空间位置关系方程如下:
其中,s=sin(·),c=cos(·);对方程(1)微分,同时引入四旋翼飞行器的机动性约束μ=[μlμmμnω]T,构建四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程如下:
其中,所述瞬时运动学方程表示四旋翼飞行器的可操作空间μ(t)到任务空间的线性映射;μ(t)表示四旋翼飞行器的机动性约束函数;ψ(t)表示航向角;Ja(ψ(t))表示映射矩阵。
进一步的,所述瞬时运动学方程中的Ja(ψ(t))如下:
其中,ψ(t)表示航向角;ψ表示航向角;s=sin(·),c=cos(·);Ja(.)表示映射矩阵。
S12:对所述摄像设备的单个特征点透视投影模型进行求导简化处理,并基于求导简化结果与所述瞬时运动学方程结合,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系;
在本发明具体实施过程中,所述对所述摄像设备的单个特征点透视投影模型进行求导简化处理,包括:由摄像设备获得图像的第i个特征点在机载相机参考系C下表示为Cξi=[Cxi Cyi Czi]T;第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标为pξi=[pui pvi]T;则摄像设备的单特征点透视投影模型如下:
其中,fc为摄像设备的焦距;对所述单特征点透视投影模型进行求导简化处理,获得单个特征点在虚拟图像二维平面中与摄像设备的运动关系:
其中,avc(t)、aωc(t)分别表示摄像设备的速度和角速度;cRa表示惯性参考A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵;为Jacobian图像矩阵,表达式如下:
其中,pui表示第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标pξi内的元素;pvi表示第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标pξi内的元素;czi表示表示摄像设备的图像平面的第i个特征点坐标深度。
进一步的,所述基于求导简化结果与所述瞬时运动学方程结合,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系,包括:将所述瞬时运动学方程代入求导简化结果中,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系如下:
其中,Ta表示四旋翼飞行器运动速度表换矩阵;cRa表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵。
S13:将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理,获得多特征点方程;
在本发明具体实施过程中,所述多特征点方程如下:
其中:
其中,pξi=[pui pvi]和czi分别表示摄像设备的图像平面的第i个特征点坐标及其深度;cRa表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵;Ta表示四旋翼飞行器运动速度表换矩阵;Jr(.)表示第r个特征点对应的Jacobian图像矩阵。
S14:采用动态模式分解对所述多特征点方程进行降维处理,获得视觉伺服控制的状态方程;
在本发明具体实施过程中,所述采用动态模式分解对所述多特征点方程进行降维处理,获得视觉伺服控制的状态方程,包括:构建所述四旋翼飞行器的状态空间表达式如下:
其中,μref=[ulref umref unref ωref]T表示四旋翼飞行器的期望速度及期望航向角速度;A表示四旋翼飞行器的动态矩阵;B表示四旋翼飞行器的控制矩阵;μ(t)为四旋翼飞行器的可操作空间;针对公式(9)的输入和四旋翼飞行器的状态进行s次的快照测量构成新的向量如下:
将公式(9)改写为:
对公式(10)改成为矩阵乘积形式如下:
采用动态模式分解对四旋翼飞行器的控制***进行***辨识,估计动态矩阵A和控制矩阵B,估计值如下:
通过***辨识转换为:
将公式(8)和公式13进行结合,将状态变量重构为 得到方程如下:
令t为控制***当前时间,T为预测范围,则公式(14)转化为控制***预测模型:
其中,l表示预测步长;fp(pξ,cz,ψ,μ)表示方程(8)的近似模型;fμ(Φ,μ,μref)表示方程(9)的近似模型;表示矩阵/>左奇异值向量矩阵;/>表示奇异值分解矩阵;/>表示/>的右奇异值向量矩阵;/>表示/>的右奇异值向量矩阵。
S15:基于所述视觉伺服控制的状态方程进行控制成本函数构建处理,获得控制成本函数;
在本发明具体实施过程中,所述基于所述视觉伺服控制的状态方程进行控制成本函数构建处理,包括:构建优化性能指标如下:
其中,图像特征点的代价:速度输入代价:μref(l|t);Qp∈R2r×2r>0,Qu∈R2m×2m>0为正定的权值函数;图像终端代价函数:
由此,控制成本函数如下:
其中,τ表示积分变量;表示成本函数。
S16:基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理,形成非线性优化问题方程;
在本发明具体实施过程中,所述基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理,形成非线性优化问题方程,包括:令smin、smax分别表示跟踪目标特征点最小最大值,则视野约束:
令分别表示四旋翼飞行器运动速度最小最大值,引入控制速度约束:
则根据成本函数、视野约束和控制速度约束构建非线性优化问题方程如下:
其中,S表示视野约束量;U表示控制速度约束量。
S17:基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理,并基于求解结果进行视觉伺服控制处理。
在本发明具体实施过程中,通过利用非线性最优化工具CasADI来对非线性优化问题方程中的P进行求解处理,即可得到最优化问题的求解结果,然后根据最优化问题的求解结果进行视觉伺服控制处理。
在本发明实施例中,通过本发明所提供的方法,实现在室内无GPS信号的环境下,仅依靠机载的摄像采集设备和惯性测量单元实现四旋翼对地面运动物体的跟踪控制。
实施例二,请参阅图2,图2是本发明实施例中的四旋翼飞行器视觉受限的视觉伺服控制***的结构组成示意图。
如图2所示,一种四旋翼飞行器视觉受限的视觉伺服控制***,所述***包括:
建立模块21:用于基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程;
在本发明具体实施过程中,所述基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程,包括:构建四旋翼飞行器动态模型中的惯性坐标系A={Oa,Xa,Ya,Za}和机体坐标系B={Ob,Xb,Yb,Zb}以及摄像设备的虚拟二维平面投影坐标系Cp={Vp,Up},四旋翼飞行器的重心与机体坐标系的原点Ob重合;假设ηa=[ηx ηy ηz ψ]T表示四旋翼飞行器在惯性坐标系下的位置向量和航向角;(ηx0 ηy0 ηz0)表示重心在机体坐标系下的坐标;(fx fy fz)表示摄像设备到四旋翼飞行器的重心的距离;
建立摄像设备与四旋翼飞行器的空间位置关系方程如下:
其中,s=sin(·),c=cos(·);对方程(1)微分,同时引入四旋翼飞行器的机动性约束μ=[μlμmμnω]T,构建四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程如下:
其中,所述瞬时运动学方程表示四旋翼飞行器的可操作空间μ(t)到任务空间的线性映射;μ(t)表示四旋翼飞行器的机动性约束函数;ψ(t)表示航向角;Ja(ψ(t))表示映射矩阵。/>
进一步的,所述瞬时运动学方程中的Ja(ψ(t))如下:
其中,ψ(t)表示航向角;ψ表示航向角;s=sin(·),c=cos(·);Ja(.)表示映射矩阵。
结合模块22:用于对所述摄像设备的单个特征点透视投影模型进行求导简化处理,并基于求导简化结果与所述瞬时运动学方程结合,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系;
在本发明具体实施过程中,所述对所述摄像设备的单个特征点透视投影模型进行求导简化处理,包括:由摄像设备获得图像的第i个特征点在机载相机参考系C下表示为Cξi=[Cxi Cyi Czi]T;第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标为pξi=[pui pvi]T;则摄像设备的单特征点透视投影模型如下:
其中,fc为摄像设备的焦距;对所述单特征点透视投影模型进行求导简化处理,获得单个特征点在虚拟图像二维平面中与摄像设备的运动关系:
其中,aνc(t)、aωc(t)分别表示摄像设备的速度和角速度;cRa表示惯性参考A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵;为Jacobian图像矩阵,表达式如下:
其中,pui表示第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标pξi内的元素;pvi表示第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标pξi内的元素;czi表示表示摄像设备的图像平面的第i个特征点坐标深度。
进一步的,所述基于求导简化结果与所述瞬时运动学方程结合,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系,包括:将所述瞬时运动学方程代入求导简化结果中,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系如下:
其中,Ta表示四旋翼飞行器运动速度表换矩阵;cRa表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵。
第一构建模块23:用于将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理,获得多特征点方程;
在本发明具体实施过程中,所述多特征点方程如下:
其中:
其中,pξi=[pui pvi]和czi分别表示摄像设备的图像平面的第i个特征点坐标及其深度;cRa表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵;Ta表示四旋翼飞行器运动速度表换矩阵;Jr(.)表示第r个特征点对应的Jacobian图像矩阵。
降维模块24:用于采用动态模式分解对所述多特征点方程进行降维处理,获得视觉伺服控制的状态方程;
在本发明具体实施过程中,所述采用动态模式分解对所述多特征点方程进行降维处理,获得视觉伺服控制的状态方程,包括:构建所述四旋翼飞行器的状态空间表达式如下:
其中,μref=[ul ref umref unref ωref]T表示四旋翼飞行器的期望速度及期望航向角速度;A表示四旋翼飞行器的动态矩阵;B表示四旋翼飞行器的控制矩阵;μ(t)为四旋翼飞行器的可操作空间;针对公式(9)的输入和四旋翼飞行器的状态进行s次的快照测量构成新的向量如下:
将公式(9)改写为:
对公式(10)改成为矩阵乘积形式如下:
采用动态模式分解对四旋翼飞行器的控制***进行***辨识,估计动态矩阵A和控制矩阵B,估计值如下:
通过***辨识转换为:
将公式(8)和公式13进行结合,将状态变量重构为 得到方程如下:
令t为控制***当前时间,T为预测范围,则公式(14)转化为控制***预测模型:
其中,l表示预测步长;fp(pξ,cz,ψ,μ)表示方程(8)的近似模型;fμ(Φ,μ,μref)表示方程(9)的近似模型;表示矩阵/>左奇异值向量矩阵;/>表示奇异值分解矩阵;/>表示/>的右奇异值向量矩阵;/>表示/>的右奇异值向量矩阵。
第二构建模块25:用于基于所述视觉伺服控制的状态方程进行控制成本函数构建处理,获得控制成本函数;
在本发明具体实施过程中,所述基于所述视觉伺服控制的状态方程进行控制成本函数构建处理,包括:构建优化性能指标如下:
其中,图像特征点的代价:速度输入代价:μref(l|t);Qp∈R2r×2r>0,Qu∈R2m×2m>0为正定的权值函数;图像终端代价函数:
由此,控制成本函数如下:
其中,τ表示积分变量;表示成本函数。
非线性优化模块26:用于基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理,形成非线性优化问题方程;
在本发明具体实施过程中,所述基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理,形成非线性优化问题方程,包括:令smin、smax分别表示跟踪目标特征点最大最小值,则视野约束:
令分别表示四旋翼飞行器运动速度最小最大值,引入控制速度约束:
则根据成本函数、视野约束和控制速度约束构建非线性优化问题方程如下:
其中,S表示视野约束量;U表示控制速度约束量。
控制模块27:用于基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理,并基于求解结果进行视觉伺服控制处理。
在本发明具体实施过程中,通过利用非线性最优化工具CasADI来对非线性优化问题方程中的P进行求解处理,即可得到最优化问题的求解结果,然后根据最优化问题的求解结果进行视觉伺服控制处理。
在本发明实施例中,通过本发明所提供的方法,实现在室内无GPS信号的环境下,仅依靠机载的摄像采集设备和惯性测量单元实现四旋翼对地面运动物体的跟踪控制。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成,该程序可以存储于一计算机可读存储介质中,存储介质可以包括:只读存储器(ROM,Read Only Memory)、随机存取存储器(RAM,RandomAccess Memory)、磁盘或光盘等。
另外,以上对本发明实施例所提供的一种四旋翼飞行器视觉受限的视觉伺服控制方法及***进行了详细介绍,本文中应采用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处,综上所述,本说明书内容不应理解为对本发明的限制。
Claims (8)
1.一种四旋翼飞行器视觉受限的视觉伺服控制方法,其特征在于,所述方法包括:
基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程;
对所述摄像设备的单个特征点透视投影模型进行求导简化处理,并基于求导简化结果与所述瞬时运动学方程结合,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系;
将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理,获得多特征点方程;
采用动态模式分解对所述多特征点方程进行降维处理,获得视觉伺服控制的状态方程;
基于所述视觉伺服控制的状态方程进行控制成本函数构建处理,获得控制成本函数;
基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理,形成非线性优化问题方程;
基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理,并基于求解结果进行视觉伺服控制处理;
其中,所述基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程,包括:
构建四旋翼飞行器动态模型中的惯性坐标系A={Oa,Xa,Ya,Za}和机体坐标系B={Ob,Xb,Yb,Zb},机载相机参考系C={Oc,Xc,Yc,Zc}摄像设备的虚拟二维平面投影坐标系Cp={Vp,Up},四旋翼飞行器的重心与机体坐标系的原点Ob重合;
假设ηa=[ηx ηy ηz ψ]T表示四旋翼飞行器在惯性坐标系下的位置向量和航向角;(ηx0ηy0 ηz0)表示重心在机体坐标系下的坐标;(fx fy fz)表示摄像设备到四旋翼飞行器的重心的距离;
建立摄像设备与四旋翼飞行器的空间位置关系方程如下:
其中,s=sin(·),c=cos(·);对方程(1)微分,同时引入四旋翼飞行器的机动性约束μ=[μlμmμnω]T,构建四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程如下:
其中,所述瞬时运动学方程中的Ja(ψ(t))如下:
所述瞬时运动学方程表示四旋翼飞行器的可操作空间μ(t)到任务空间的线性映射;μ(t)表示四旋翼飞行器的机动性约束函数;ψ表示航向角;ψ(t)表示航向角的时间函数;Ja(ψ(t))表示映射矩阵;
2.根据权利要求1所述的视觉伺服控制方法,其特征在于,所述对所述摄像设备的单个特征点透视投影模型进行求导简化处理,包括:
由摄像设备获得图像的第i个特征点在机载相机参考系C下表示为Cξi=[CxiCyiCzi]T;第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标为pξi=[puipvi]T;
则摄像设备的单特征点透视投影模型如下:
其中,fc为摄像设备的焦距;
对所述单特征点透视投影模型进行求导简化处理,获得单个特征点在虚拟图像二维平面中与摄像设备的运动关系:
其中,aνc(t)、aωc(t)分别表示摄像设备的速度和角速度;cRa表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵;为Jacobian图像矩阵,表达式如下:
其中,pui表示第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标pξi内的元素;pvi表示第i个特征点在虚拟二维平面投影坐标系Cp下对应坐标pξi内的元素;czi表示表示摄像设备的图像平面的第i个特征点坐标深度。
3.根据权利要求1所述的视觉伺服控制方法,其特征在于,所述基于求导简化结果与所述瞬时运动学方程结合,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系,包括:
将所述瞬时运动学方程代入求导简化结果中,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系如下:
其中,Ta表示四旋翼飞行器运动速度表换矩阵;cRa表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵。
4.根据权利要求1所述的视觉伺服控制方法,其特征在于,所述多特征点方程如下:
其中:
其中,pξi=[puipvi]和czi分别表示摄像设备的图像平面的第i个特征点坐标及其深度;cRa表示惯性坐标系A到摄像设备的虚拟二维平面投影坐标系Cp的旋转矩阵;Ta表示四旋翼飞行器运动速度表换矩阵;Jr(.)表示第r个特征点对应的Jacobian图像矩阵。
5.根据权利要求1所述的视觉伺服控制方法,其特征在于,所述采用动态模式分解对所述多特征点方程进行降维处理,获得视觉伺服控制的状态方程,包括:
构建所述四旋翼飞行器的状态空间表达式如下:
其中,μref=[ul ref umref unref ωref]T表示四旋翼飞行器的期望速度及期望航向角速度;A表示四旋翼飞行器的动态矩阵;B表示四旋翼飞行器的控制矩阵;
针对公式(9)的输入和四旋翼飞行器的状态进行s次的快照测量构成新的向量如下:
将公式(9)改写为:
对公式(10)改成为矩阵乘积形式如下:
采用动态模式分解对四旋翼飞行器的控制***进行***辨识,估计动态矩阵A和控制矩阵B,估计值如下:
通过***辨识转换为:
将公式(8)和公式13进行结合,将状态变量重构为 得到方程如下:
令t为控制***当前时间,T为预测范围,则公式(14)转化为控制***预测模型:
其中,l表示预测步长;fp(pξ,cz,ψ,μ)表示方程(8)的近似模型;fμ(Φ,μ,μref)表示方程(9)的近似模型;表示矩阵/>左奇异值向量矩阵;/>表示奇异值分解矩阵;/>表示/>的右奇异值向量矩阵;/>表示/>的右奇异值向量矩阵。
6.根据权利要求5所述的视觉伺服控制方法,其特征在于,所述基于所述视觉伺服控制的状态方程进行控制成本函数构建处理,包括:
构建优化性能指标如下:
其中,图像特征点的代价:速度输入代价:μref(l|t);Qp∈R2r×2r>0,Qu∈R2m×2m>0为正定的权值函数;图像终端代价函数:
由此,控制成本函数如下:
其中,τ表示积分变量;表示成本函数。
7.根据权利要求6所述的视觉伺服控制方法,其特征在于,所述基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理,形成非线性优化问题方程,包括:
令smin、smax分别表示跟踪目标特征点最小最大值,则视野约束:
令分别表示四旋翼飞行器运动速度最小最大值,引入控制速度约束:
则根据成本函数、视野约束和控制速度约束构建非线性优化问题方程如下:
其中,S表示视野约束量;U表示控制速度约束量。
8.一种采用了如权利要求1-7任何一项所述方法的四旋翼飞行器视觉受限的视觉伺服控制***,其特征在于,所述***包括:
建立模块:用于基于设置在四旋翼飞行器底部的摄像设备采集的图像信息建立四旋翼飞行器的可操作空间到任务空间的瞬时运动学方程;
结合模块:用于对所述摄像设备的单个特征点透视投影模型进行求导简化处理,并基于求导简化结果与所述瞬时运动学方程结合,获得单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系;
第一构建模块:用于将单个特征点在虚拟图像二维平面中与所述摄像设备的运动关系应用到跟踪目标的多个特征点中进行多特征点方程的构建处理,获得多特征点方程;
降维模块:用于采用动态模式分解对所述多特征点方程进行降维处理,获得视觉伺服控制的状态方程;
第二构建模块:用于基于所述视觉伺服控制的状态方程进行控制成本函数构建处理,获得控制成本函数;
非线性优化模块:用于基于所述控制成本函数和所述状态方程引入约束条件进行非线性优化处理,形成非线性优化问题方程;
控制模块:用于基于非线性最优化工具CasADI对所述非线性优化问题方程进行求解处理,并基于求解结果进行视觉伺服控制处理。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310855217.XA CN116700348B (zh) | 2023-07-12 | 2023-07-12 | 一种四旋翼飞行器视觉受限的视觉伺服控制方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310855217.XA CN116700348B (zh) | 2023-07-12 | 2023-07-12 | 一种四旋翼飞行器视觉受限的视觉伺服控制方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116700348A CN116700348A (zh) | 2023-09-05 |
CN116700348B true CN116700348B (zh) | 2024-03-19 |
Family
ID=87834133
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310855217.XA Active CN116700348B (zh) | 2023-07-12 | 2023-07-12 | 一种四旋翼飞行器视觉受限的视觉伺服控制方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116700348B (zh) |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2010043370A1 (en) * | 2008-10-13 | 2010-04-22 | Universite De Mediterranee | Steering aid method and system for landing on a target of a moving platform, and a 3 d vehicle equipped as such |
CN102190081A (zh) * | 2010-03-04 | 2011-09-21 | 南京航空航天大学 | 基于视觉的飞艇定点鲁棒控制方法 |
CN106363646A (zh) * | 2016-05-25 | 2017-02-01 | 上海铸天智能科技有限公司 | 基于视觉伺服控制的多旋翼和机载机械臂联合位姿控制法 |
KR101791166B1 (ko) * | 2016-05-31 | 2017-10-27 | 아주대학교산학협력단 | 오브젝트의 공간 정보 추정 장치 및 방법 |
CN109143855A (zh) * | 2018-07-31 | 2019-01-04 | 西北工业大学 | 一种基于模糊sarsa学习的旋翼无人机视觉伺服控制方法 |
CN110231828A (zh) * | 2019-05-31 | 2019-09-13 | 燕山大学 | 基于nftsm的四旋翼无人机视觉伺服控制方法 |
CN111923049A (zh) * | 2020-08-21 | 2020-11-13 | 福州大学 | 基于球面模型的飞行机械臂视觉伺服与多任务控制方法 |
CN112947569A (zh) * | 2021-03-09 | 2021-06-11 | 中南大学 | 基于预设性能四旋翼无人机视觉伺服目标跟踪控制方法 |
CN113138608A (zh) * | 2021-05-10 | 2021-07-20 | 新疆大学 | 一种使用扰动观测器和非线性速度观测器的四旋翼无人机视觉伺服控制方法 |
-
2023
- 2023-07-12 CN CN202310855217.XA patent/CN116700348B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2010043370A1 (en) * | 2008-10-13 | 2010-04-22 | Universite De Mediterranee | Steering aid method and system for landing on a target of a moving platform, and a 3 d vehicle equipped as such |
CN102190081A (zh) * | 2010-03-04 | 2011-09-21 | 南京航空航天大学 | 基于视觉的飞艇定点鲁棒控制方法 |
CN106363646A (zh) * | 2016-05-25 | 2017-02-01 | 上海铸天智能科技有限公司 | 基于视觉伺服控制的多旋翼和机载机械臂联合位姿控制法 |
KR101791166B1 (ko) * | 2016-05-31 | 2017-10-27 | 아주대학교산학협력단 | 오브젝트의 공간 정보 추정 장치 및 방법 |
CN109143855A (zh) * | 2018-07-31 | 2019-01-04 | 西北工业大学 | 一种基于模糊sarsa学习的旋翼无人机视觉伺服控制方法 |
CN110231828A (zh) * | 2019-05-31 | 2019-09-13 | 燕山大学 | 基于nftsm的四旋翼无人机视觉伺服控制方法 |
CN111923049A (zh) * | 2020-08-21 | 2020-11-13 | 福州大学 | 基于球面模型的飞行机械臂视觉伺服与多任务控制方法 |
CN112947569A (zh) * | 2021-03-09 | 2021-06-11 | 中南大学 | 基于预设性能四旋翼无人机视觉伺服目标跟踪控制方法 |
CN113138608A (zh) * | 2021-05-10 | 2021-07-20 | 新疆大学 | 一种使用扰动观测器和非线性速度观测器的四旋翼无人机视觉伺服控制方法 |
Non-Patent Citations (1)
Title |
---|
Autonomous Landing on Ground Target of UAV by Using Image-Based Visual Servo Control;Zhang, YW.etc;36th Chinese Control Conference (CCC);第11204-11209页 * |
Also Published As
Publication number | Publication date |
---|---|
CN116700348A (zh) | 2023-09-05 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Bircher et al. | Three-dimensional coverage path planning via viewpoint resampling and tour optimization for aerial robots | |
Paris et al. | Dynamic landing of an autonomous quadrotor on a moving platform in turbulent wind conditions | |
TWI827649B (zh) | 用於vslam比例估計的設備、系統和方法 | |
Barber et al. | Vision-based target geo-location using a fixed-wing miniature air vehicle | |
Meier et al. | PIXHAWK: A micro aerial vehicle design for autonomous flight using onboard computer vision | |
Martínez et al. | On-board and ground visual pose estimation techniques for UAV control | |
Azrad et al. | Visual servoing of quadrotor micro-air vehicle using color-based tracking algorithm | |
Rady et al. | A hybrid localization approach for UAV in GPS denied areas | |
CN114111818B (zh) | 一种通用视觉slam方法 | |
Sanfourche et al. | Perception for UAV: Vision-Based Navigation and Environment Modeling. | |
CN112947569B (zh) | 基于预设性能四旋翼无人机视觉伺服目标跟踪控制方法 | |
CN110967017A (zh) | 一种用于双移动机器人刚体协作搬运的协同定位方法 | |
CN116578035A (zh) | 基于数字孪生技术的旋翼无人机自主降落控制*** | |
Zhang et al. | Aerial and ground-based collaborative mapping: an experimental study | |
Conte et al. | High accuracy ground target geo-location using autonomous micro aerial vehicle platforms | |
Andersen et al. | Improving MAV pose estimation using visual information | |
CN116700348B (zh) | 一种四旋翼飞行器视觉受限的视觉伺服控制方法及*** | |
CN116989772A (zh) | 一种空地多模态多智能体协同定位与建图方法 | |
Denuelle et al. | Biologically-inspired visual stabilization of a rotorcraft UAV in unknown outdoor environments | |
Leishman et al. | Utilizing an improved rotorcraft dynamic model in state estimation | |
MacArthur et al. | Unmanned ground vehicle state estimation using an unmanned air vehicle | |
CN107161350B (zh) | 事故处置飞行器自主续航方法及其*** | |
Biswas et al. | Goal-aware Navigation of Quadrotor UAV for Infrastructure Inspection | |
Duan et al. | Integrated localization system for autonomous unmanned aerial vehicle formation flight | |
Ajmera et al. | Autonomous visual tracking and landing of a quadrotor on a moving platform |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |