CN116627154B - 基于位姿预测和轨迹优化的无人机引导降落方法及无人机 - Google Patents
基于位姿预测和轨迹优化的无人机引导降落方法及无人机 Download PDFInfo
- Publication number
- CN116627154B CN116627154B CN202310681898.2A CN202310681898A CN116627154B CN 116627154 B CN116627154 B CN 116627154B CN 202310681898 A CN202310681898 A CN 202310681898A CN 116627154 B CN116627154 B CN 116627154B
- Authority
- CN
- China
- Prior art keywords
- landing
- unmanned aerial
- aerial vehicle
- target
- state
- 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 63
- 238000005457 optimization Methods 0.000 title claims abstract description 35
- 238000001914 filtration Methods 0.000 claims abstract description 34
- 230000033001 locomotion Effects 0.000 claims abstract description 33
- 230000000007 visual effect Effects 0.000 claims abstract description 21
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000001514 detection method Methods 0.000 claims description 9
- 230000001133 acceleration Effects 0.000 claims description 8
- 239000003550 marker Substances 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 3
- 238000004458 analytical method Methods 0.000 description 4
- 238000004140 cleaning Methods 0.000 description 4
- 238000012544 monitoring process Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
- G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability
- G05D1/0825—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability using mathematical models
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
-
- 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
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Mathematical Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Physics (AREA)
- Mathematical Optimization (AREA)
- Algebra (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种基于位姿预测和轨迹优化的无人机引导降落方法及无人机,所述方法在近距离降落过程的具体步骤如下:当无人机在中距离跟踪过程中着降角度达到降落阈值,进入预着降状态;获取当前目标降落物的运动状态,通过卡尔曼滤波算法预测目标降落物运动状态;将预测的目标降落物的运动过程与设定的偏航运动角比较,判断目标降落物的运动状态是否稳定以及无人机是否进入着降状态;进入着降状态后,保持着降角度不变进行着降,根据当前的姿态和动力学结构构建初始降落轨迹;通过贝塞尔曲线优化初始降落曲线,找到最优降落轨迹;无人机基于最优降落轨迹降落到目标降落物上。与现有技术相比,本发明提升了多旋翼无人机在视觉引导降落上的稳定性和准确性。
Description
技术领域
本发明涉及无人机的导航和运动规划领域,尤其是涉及一种基于位姿预测和轨迹优化的四旋翼无人机视觉引导降落方法。
背景技术
随着科学技术的不断发展,多旋翼无人机已经广泛应用于农业、测绘、物流、消防、救援等领域,目前,随着无人机相关技术的不断升级和应用场景的不断扩展,无人机已经进入了快速发展期:传感器、控制系统、通信等技术不断升级,让实现更加复杂的任务成为可能。而无人机和无人车之间的协同作业可以实现更高效、更精准的任务完成。无人机可以提供高空视角和快速的移动能力,适用于需要大范围监测和快速响应的任务,如灾害监测、交通监管等;而无人车则可以提供更稳定、更精细的操作能力,适用于需要精细操作和长时间持续工作的任务,如物流配送、环境清理等。在协同作业中,无人机可以先进行快速的勘测和监测,将数据传输给无人车进行进一步的处理和操作。例如,在物流配送中,无人机可以先进行货物的快速运输和目标地点的勘测,将数据传输给无人车进行精细的操作和货物的最终配送。在环境清理中,无人机可以先进行空中监测和快速清理,将数据传输给无人车进行地面清理和处理。因此,让无人机能够平稳、安全的降落在无人车上实现车机协同作业就成为高质量完成任务的关键。
目前已经有一些关于无人机自主降落的研究,申请号为202011237125.8的专利,基于视觉锚点的无人机降落位姿滤波估计方法及系统,通过构建无人机在降落过程中的空间位姿构建卡尔曼滤波模型,但并没有对目标降落小车的空间位姿进行未来预测,当出现目标物短暂遮挡和运动误差时不能很好的降落到小车上,影响无人机的降落精度;申请号为202111452204.5的专利,设计了基于视觉的无人机降落方法,将无人机的降落过程分为返航、中高空、低空、近距离四部分,返航部分通过GPS信号引导,其余几个部分通过多个标识符的不同和KCF算法来实现无人机的跟踪和降落,但并没有对无人机的着降曲线进行优化处理。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于位姿预测和轨迹优化的无人机引导降落方法及无人机。
本发明的目的可以通过以下技术方案来实现:
作为本发明的第一方面,提供一种基于位姿预测和轨迹优化的无人机引导降落方法,所述方法基于无人机的飞行高度和与目标降落物的距离将无人机的引导降落分为远距离引导,中距离跟踪和近距离着降;
其中,所述近距离降落过程的具体步骤如下:
当无人机在中距离跟踪过程中着降角度达到降落阈值,无人机进入预着降状态;
无人机进入预着降状态,获取当前目标降落物的运动状态,通过卡尔曼滤波算法预测目标降落物运动状态;
将预测的目标降落物的运动过程与设定的偏航运动角比较进而判断目标降落物的运动状态是否稳定,并判断无人机是否能够进入着降状态;
进入着降状态后,保持着降角度不变并根据当前无人机的姿态和动力学结构构建初始降落轨迹;
在降落过程中通过贝塞尔曲线不断优化初始降落曲线,找到最优的降落轨迹;
无人机识别最终标志物,基于最优的降落轨迹降落到目标降落物上。
进一步的,所述远距离引导过程具体为:无人机获得目标降落物的相对位置,并根据定位导航系统导航飞行至目标物附近。
进一步的,所述中距离跟踪过程具体为:当无人机飞到目标物附近后,通过无人机的视觉相机捕捉目标物的图像信息,通过目标检测核相关滤波KCF算法对目标降落物进行框选跟踪。
进一步的,所述目标降落物设有用于检测识别的二维码;
当无人机的视觉相机检测到二维码信息后,将获取到的图像信息输入到目标的检测KCF算法中实现基于相机视觉的目标跟踪,具体步骤如下:
选择一个预先设计好的目标模板表示目标降落物,所述目标模板通过方向梯度直方图特征和颜色直方图特征构建;
在视觉相机的每一帧图像中,KCF算法将目标模板与当前帧的图像进行相关运算得到相应图,相应图中响应值最高的位置即为目标降落物的当前位置;
无人机实时监测并输出目标降落物的位置,无人机通过PID控制算法调整自身的飞行姿态和速度,实现对目标降落物的追踪。
进一步的,所述无人机在进行KCF算法框选追踪的过程中,不断计算并判断摄像头的视线角是否达到降落阈值;如果达到降落阈值,则进入预着降状态。
进一步的,当着降角度达到降落阈值后,无人机通过相机的多帧连续识别,结合卡尔曼滤波来判断目标降落物接下来的运动情况,并进一步判断无人机是否可以进入降落状态,通过卡尔曼滤波预测目标降落物到运动状态具体步骤如下:
根据目标降落物过程中的视觉锚点测量情况,构造目标降落物的位姿估计卡尔曼滤波模型,所述卡尔曼滤波模型包括系统状态预测方程和系统观测方程;
建立目标降落物的位置表示方程;
通过卡尔曼滤波模型估计目标降落物的未来运动轨迹。
进一步的,所述目标降落物的位置表示为线性方程:
X(k+1)=AX(k)+GW(k)
X(k+1)=AX(k)+GW(k)
Z(k+1)=HX(k+1)+V(k+1)
其中,k为离散时间;X(k)为k时刻系统状态;A为状态转移矩阵;Z(k)为k时刻对应状态的观测值;H为观测矩阵;G为噪声矩阵;W(k)为白噪声;V(k)观测噪声;
规定W(k)和V(k)的均值为0,Q、R为协方差矩阵;所述卡尔曼滤波器表述如下:
状态预测:
X(k+1|k)=AX(k|k)
协方差矩阵预测:
P(k+1|k)=AP(k|k)AT+GQGT
状态更新:
X(k+1|k+1)=X(k+1|k)+K(k+1)[Z(k+1)-HX(k+1|k)]
卡尔曼增益矩阵:
K(k+1)=P(k+1|k)HT[HP(k+1|k)HT+R]-1
协方差矩阵更新:
P(k+1|k+1)=[In-K(k+1)H]P(k+1|k)
其中,X(k+1|k)为k+1时刻的后验状态估计值,X(k|k)为k时刻的后验状态估计值,P(k+1|k)为k时刻的先验估计协方差(X(k+1|x)的协方差),P(k+1|k+1)为k+1时刻的后验估计协方差,P(k|k)为k时刻的后验估计协方差,K(k+1)为滤波增益矩阵也就是卡尔曼增益,Z(k+1)为k+1时刻对应状态的观测值,In为n阶单位矩阵。
进一步的,将得到的目标降落物的预测轨迹使用贝塞尔曲线进行动力学和运动学约束,得到无人机降落的视觉引导曲线,具体步骤如下:
根据得到的预测轨迹点建立贝塞尔曲线,在预测的轨迹中定义并选择起点和终点,设置控制点并建立贝塞尔曲线:
构建问题约束,包括连续性约束、端点约束和动力学约束;
使用贝塞尔曲线公式计算出曲线上的点的坐标;
通过软约束对贝塞尔曲线的进一步优化;
得到符合动力学约束的降落飞行曲线。
进一步的,所述通过软约束对贝塞尔曲线的进一步优化,构建优化目标函数如下:
J=λ1Js+λ2Jc+λ3Jd
其中,表示光滑代价,用minimum snap形式最小化加速度,fu(t)为参数化轨迹,x,y,z为三个维度上生成的轨迹点,T为轨迹生成周期,/>表示k阶导数;
表示碰撞代价,将对轨迹ds的积分使用每一小段的速度乘以时间dt来代替,c(p(t))表示沿着轨迹点的距离惩罚,c(p(Tk))表示每个事件段上的距离惩罚,v(t)表示x、y、z三个方向的速度合并后的无人机的真实速度,T/dt表示将时间T按dt时间间隔分割;
Jd,表示动力学代价,用于惩罚速度和加速度超出限制的部分;λ1、λ2、λ3,为权重优化系数。
作为本发明的第二方面,提供一种无人机,所述无人机在降落时执行如上任意一项所述的降落方法。
与现有技术相比,本发明具有以下有益效果:
1)本发明在传统的使用单目视觉进行追踪的基础上加入了卡尔曼滤波预测目标降落物的未来移动轨迹,对于目标降落物被一定程度上遮挡导致无法准确识别降落路径和轨迹的情况有很好的预测效果。
2)除此之外,增加了对角度约束下的降落曲线的贝塞尔优化,通过贝塞尔曲线的优化,使得下降曲线的抖动和震动减小,降落曲线的轨迹也更加符合无人机的安全性约束和动力学约束,更有利于视觉降落时的轨迹跟踪的实现。本发明能够提升多旋翼无人机在视觉引导降落上的稳定性和准确性。
附图说明
图1为本发明基于滤波估计算法的无人机视觉引导降落系统的流程图;
图2为车辆尾部设置的用于无人机识别的二维码阵列信息;
图3为卡尔曼滤波实现的小车轨迹预测与误差分析曲线(x位置信息);
图4为卡尔曼滤波实现的小车轨迹预测与误差分析曲线(y位置信息);
图5为卡尔曼滤波实现的小车轨迹预测与误差分析曲线(y误差信息);
图6为θ角度约束下的无人机远、中、近距离着降示意图;
图7为经过贝塞尔曲线优化后的无人机着降曲线。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施,给出了详细的实施方式和具体的操作过程,但本发明的保护范围不限于下述的实施例。
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于位姿预测与轨迹优化的无人机视觉引导降落方法,用以将执行完飞行任务的无人机精准降落到移动目标物上。
具体的,本发明设计了一种基于卡尔曼滤波算法的位姿预测方法以及基于贝塞尔曲线的轨迹优化算法,并基于前两种方法设计了一种基于角度约束的无人机自主降落方法,具体步骤如下:
S1:针对无人机的实际飞行高度和距离目标降落物的实际距离将无人机的引导降落分为远距离引导,中距离跟踪和近距离着降;
S2:远距离引导过程,无人机在返航时通过记载数传系统和通讯系统获得目标降落物的相对位置,并根据GPS导航系统导航飞行至目标物附近;
S3:中距离跟踪过程,当无人机飞到目标物附近后,通过无人机的单目视觉相机和双目视觉相机捕捉目标物的图像信息,通过无人机的目标检测KCF滤波算法对目标降落物进行框选跟踪;
S4:近距离降落过程,当无人机在跟踪过程中着降角度达到降落阈值时,无人机进入预着降状态;
S5:无人机进入预着降状态,通过无人机的单目视觉相机的多帧获取当前目标降落物的运动状态,通过卡尔曼滤波算法预测目标降落物运动状态;
S6:将预测的目标降落物的运动过程与设定的偏航运动角比较进而判断目标降落物的运动状态是否稳定,并判断无人机是否能够进入着降状态;
S7:进入着降状态后,通过单目视觉相机识别保持相机与目标降落物之间的着降角度不变,进而得到当前情况下的初始降落轨迹;
S8:在无人机的降落过程中通过贝塞尔曲线不断优化降落曲线,减少无人机在降落过程中调整姿态产生的震荡,找到最优的无人机降落轨迹;
S9:无人机识别最终标志物并实现在目标降落物上的精准降落。
进一步的,所述目标物为一般车辆,在车辆的尾部设有用于检测识别的二维码信息;
所述二维码信息是由多组二维码组成的二维码阵列,每个二维码呈白底黑码,每个二维码均具有自己独立的ID,包括4个较小的二维码A、B、C、D在阵列的四周,一个较大的二维码E在阵列的中心,一个较小的二维码F处于较大二维码的中心;
进一步的,所述目标降落小车用于无人机在远距离通过GPS定位系统在远距离跟踪;
所述二维码A、B、C、D用于无人机在中距离引导降落时使用,当无人机的单目相机检测到二维码信息后,将获取到的图像信息输入到目标的检测KCF算法中实现基于单目相机视觉的目标跟踪算法;
步骤一,无人机机载视觉选择一个预先设计好的目标模板来表示小车,通过HOG特征和颜色直方图等特征子来构建目标模板;
步骤二,在单目视觉相机的每一帧图像中,KCF算法将目标模板与当前帧的图像进行相关运算得到相应图,相应图中响应值最高的位置即为小车的当前位置;
步骤三,无人机根据记载模块实时监测并输出小车的位置,无人机通过PID控制算法调整自身的飞行姿态和速度,实现对小车的追踪。
进一步的,无人机在使用KCF算法框选追踪的过程中,会不断通过记载视觉导引信号的解算来判断摄像头的视线角是否达到降落阈值。如果达到降落阈值,则进入预着降状态;
进一步的,当着降角度达到降落阈值后,通过单目相机的多帧连续识别,结合卡尔曼滤波来判断小车接下来的运动情况,并进一步判断无人机是否可以进入降落状态,通过卡尔曼滤波预测目标降落小车到运动状态步骤:
步骤一,根据目标降落小车过程中的视觉锚点测量情况,构造目标降落小车的位姿估计卡尔曼滤波模型;其中模型包括系统状态预测方程和系统观测方程;
步骤二,识别视觉锚点的过程实际上是对单帧图像的处理,属于离散数据处理,可以将标识物识别过程看作线性离散控制,卡尔曼滤波能够通过递推有效的对其进行预测,这里我们将目标降落小车的位置表示为线性方程:
X(k+1)=AX(k)+GW(k)
X(k+1)=AX(k)+GW(k)
Z(k+1)=HX(k+1)+V(k+1)
其中,k为离散时间;X(k)为k时刻系统状态;A为状态转移矩阵;Z(k)为k时刻对应状态的观测值;H为观测矩阵;G为噪声矩阵;W(k)为白噪声;V(k)观测噪声。
步骤三,规定W(k)和V(k)的均值为0,协方差矩阵分别为Q、R。基于状态预测方程,通过上一时刻预测下一时刻系统状态,假定现在的系统状态为k,根据系统的模型,基于系统上一时刻状态预测出当前状态。
卡尔曼滤波器表述如下:
(1)状态预测:
X(k+1|k)=AX(k|k)
(2)协方差矩阵预测:
P(k+1|k)=AP(k|k)AT+GQGT
(3)状态更新:
X(k+1|k+1)=X(k+1|k)+K(k+1)[Z(k+1)-HX(k+1|k)]
(4)卡尔曼增益矩阵:
K(k+1)=P(k+1|k)HT[HP(k+1|k)HT+R]-1
(5)协方差矩阵更新:
P(k+1|k+1)=[In-K(k+1)H]P(k+1|k)
其中,In为n阶数单位矩阵,X(k+1|k)为k+1时刻的后验状态估计值,X(k|k)为k时刻的后验状态估计值,P(k+1|k)为k时刻的先验估计协方差(X(k+1|x)的协方差),P(k+1|k+1)为k+1时刻的后验估计协方差,P(k|k)为k时刻的后验估计协方差,K(k+1)为滤波增益矩阵也就是卡尔曼增益,Z(k+1)为k+1时刻对应状态的观测值。
步骤四,根据本文中小车的运动学方程可知,本专利中目标降落小车的状态向量为:
其中,x(k),y(k)表示目标降落小车在x,y方向上的位置信息。
由于视觉识别过程仅能观察到目标降落小车的位置信息,因此观测向量为:
Z(k)=[x(k) y(k)]T
则可知观测矩阵为:
步骤五,确定状态方程之后,将卡尔曼滤波用于估计目标降落小车的未来运动轨迹。
进一步的,根据得到的目标降落小车的预测轨迹,对角度约束下的无人机降落轨迹使用贝塞尔曲线进行动力学和运动学约束,得到适合无人机降落的视觉引导曲线,具体方式如下:
步骤一,无人机保持相机与目标降落物之间的着降角度不变,并根据与目标降落物之间的位姿偏差和距离得到预计降落轨迹曲线,然后根据得到的预测轨迹点建立贝塞尔曲线,在预测的轨迹中定义并选择起点和终点,其中每4个点设置一个控制点建立贝塞尔曲线;
其中,0<t<1,pi为贝塞尔曲线的参数
步骤二,构建问题约束,包括连续性约束、端点约束和动力学约束三部分,具体如下:
(1)端点约束:
(2)连续性约束:
(3)动力学约束:
其中,表示第j段第l阶曲线的第i个控制点的加速度,s表示控制点的位置矩阵,用于确定曲线上点的位置,d表示曲线的导数矩阵,n表示无人机的加速度限制,c表示曲线上控制点的曲率。
步骤三,使用上述贝塞尔曲线公式计算出曲线上的点的坐标;
步骤四,通过软约束将得到的贝塞尔曲线进行进一步优化,具体方式如下:
传统的硬约束优化如下:
等式约束:
gi(x)=ci
不等式约束:
hj(x)≥dj
其中,gi(x)是优化函数需要满足的边界条件,hj(x)是优化函数需要满足的区域条件。
但由于硬约束方法在基于飞行走廊的情况下生成的轨迹仍然存在较大的碰撞风险,同时对测量噪声也比较敏感,所以本发明采用软约束进行优化,入加权惩罚项:
minf(s)+λ1g(x)+λ2h(x)
其中,gi(x)是优化函数需要满足的边界条件,hj(x)是优化函数需要满足的区域条件,λ1、λ2表示权重优化系数。
建立优化问题:
参数化轨迹:
上式将带有多个节点的轨迹使用分段多项式进行描述,其中每一段轨迹使用m个时间间隔的n阶分段多项式函数表达,pnj为第j段曲线的第n个控制点。
构建目标函数:
J=λ1Js+λ2Jc+λ3Jd
其中,表示光滑代价,用minimum snap形式最小化加速度,fu(t)为参数化轨迹,x,y,z为三个维度上生成的轨迹点,T为轨迹生成周期,/>表示k阶导数。
表示碰撞代价,将对轨迹ds的积分使用每一小段的速度乘以时间dt来代替,c(p(t))表示沿着轨迹点的距离惩罚,c(p(Tk))表示每个事件段上的距离惩罚,v(t)表示x、y、z三个方向的速度合并后的无人机的真实速度即三轴速度平方和的算术平方根,T/dt表示将时间T按dt时间间隔分割。
Jd,表示动力学代价,惩罚速度和加速度超出限制的部分。
λ1、λ2、λ3,表示权重优化系数。
步骤五,通过贝塞尔曲线优化得到符合动力学约束的降落飞行曲线
进一步的,无人机通过角度约束、卡尔曼滤波和贝塞尔曲线优化后得到一条适合无人机进行着降的降落曲线。
与现有技术相比,本发明具有以下有益效果:
本发明在传统的使用单目视觉进行追踪的基础上加入了卡尔曼滤波预测目标降落物的未来移动轨迹,对于目标降落物被一定程度上遮挡导致无法准确识别降落路径和轨迹的情况有很好的预测效果;除此之外,增加了对角度约束下的降落曲线的贝塞尔优化,通过贝塞尔曲线的优化,使得下降曲线的抖动和震动减小,降落曲线的轨迹也更加符合无人机的安全性约束和动力学约束,更有利于视觉降落时的轨迹跟踪的实现。本发明对提升多旋翼无人机在视觉引导降落上的稳定性和准确性上具有重要的意义。
实施例2
作为本发明的另一种实施方式,本发明还提供一种多旋翼无人机,该无人机上搭载有单目视觉摄像机、机载执行器等设备。当无人机进行降落时,执行如上实施例所述的基于卡尔曼滤波位姿预测与贝塞尔曲线轨迹优化的无人机自主降落方法。
以上详细描述了本发明的较佳具体实施例。应当理解,本领域的普通技术人员无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此,凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案,皆应在由权利要求书所确定的保护范围内。
Claims (8)
1.一种基于位姿预测和轨迹优化的无人机引导降落方法,其特征在于,所述方法基于无人机的飞行高度和与目标降落物的距离将无人机的引导降落分为远距离引导,中距离跟踪和近距离着降;
其中,所述近距离着降过程的具体步骤如下:
当无人机在中距离跟踪过程中着降角度达到降落阈值,无人机进入预着降状态;
无人机进入预着降状态,获取当前目标降落物的运动状态,通过卡尔曼滤波算法预测目标降落物运动状态;
将预测的目标降落物的运动过程与设定的偏航运动角比较进而判断目标降落物的运动状态是否稳定,并判断无人机是否能够进入着降状态;
进入着降状态后,保持着降角度不变并根据当前无人机的姿态和动力学结构构建初始降落轨迹;
在降落过程中,将得到的目标降落物的预测轨迹使用贝塞尔曲线进行动力学和运动学约束,得到最优的降落轨迹,即无人机降落的视觉引导曲线,具体步骤如下:
根据得到的预测轨迹点建立贝塞尔曲线,在预测的轨迹中定义并选择起点和终点,设置控制点并建立贝塞尔曲线:
构建问题约束,包括连续性约束、端点约束和动力学约束;
使用贝塞尔曲线公式计算出曲线上的点的坐标;
通过软约束对贝塞尔曲线的进一步优化,所构建优化目标函数如下:
J=λ1Js+λ2Jc+λ3Jd
其中,表示光滑代价,用minimum snap形式最小化加速度,fu(t)为参数化轨迹,x,y,z为三个维度上生成的轨迹点,T为轨迹生成周期,/>表示k阶导数;/>表示碰撞代价,将对轨迹ds的积分使用每一小段的速度乘以时间dt来代替,c(p(t))表示沿着轨迹点的距离惩罚,c(p(Tk))表示每个事件段上的距离惩罚,v(t)表示x、y、z三个方向的速度合并后的无人机的真实速度,T/dt表示将时间T按dt时间间隔分割;Jd,表示动力学代价,用于惩罚速度和加速度超出限制的部分;λ1、λ2、λ3,为权重优化系数;
得到符合动力学约束的降落飞行曲线;
无人机识别最终标志物,基于最优的降落轨迹降落到目标降落物上。
2.根据权利要求1所述的一种基于位姿预测和轨迹优化的无人机引导降落方法,其特征在于,所述远距离引导过程具体为:无人机获得目标降落物的相对位置,并根据定位导航系统导航飞行至目标物附近。
3.根据权利要求1所述的一种基于位姿预测和轨迹优化的无人机引导降落方法,其特征在于,所述中距离跟踪过程具体为:当无人机飞到目标物附近后,通过无人机的视觉相机捕捉目标物的图像信息,通过目标检测核相关滤波KCF算法对目标降落物进行框选跟踪。
4.根据权利要求3所述的一种基于位姿预测和轨迹优化的无人机引导降落方法,其特征在于,所述目标降落物设有用于检测识别的二维码;
当无人机的视觉相机检测到二维码信息后,将获取到的图像信息输入到目标的检测KCF算法中实现基于相机视觉的目标跟踪,具体步骤如下:
选择一个预先设计好的目标模板表示目标降落物,所述目标模板通过方向梯度直方图特征和颜色直方图特征构建;
在视觉相机的每一帧图像中,KCF算法将目标模板与当前帧的图像进行相关运算得到相应图,相应图中响应值最高的位置即为目标降落物的当前位置;
无人机实时监测并输出目标降落物的位置,无人机通过PID控制算法调整自身的飞行姿态和速度,实现对目标降落物的追踪。
5.根据权利要求4所述的一种基于位姿预测和轨迹优化的无人机引导降落方法,其特征在于,所述无人机在进行KCF算法框选追踪的过程中,不断计算并判断摄像头的视线角是否达到降落阈值;如果达到降落阈值,则进入预着降状态。
6.根据权利要求1所述的一种基于位姿预测和轨迹优化的无人机引导降落方法,其特征在于,当着降角度达到降落阈值后,无人机通过相机的多帧连续识别,结合卡尔曼滤波来判断目标降落物接下来的运动情况,并进一步判断无人机是否可以进入降落状态,通过卡尔曼滤波预测目标降落物到运动状态具体步骤如下:
根据目标降落物过程中的视觉锚点测量情况,构造目标降落物的位姿估计卡尔曼滤波模型,所述卡尔曼滤波模型包括系统状态预测方程和系统观测方程;
建立目标降落物的位置表示方程;
通过卡尔曼滤波模型估计目标降落物的未来运动轨迹。
7.根据权利要求6所述的一种基于位姿预测和轨迹优化的无人机引导降落方法,其特征在于,所述目标降落物的位置表示为线性方程:
X(k+1)=AX(k)+GW(k)
Z(k+1)=HX(k+1)+V(k+1)
其中,k为离散时间;X(k)为k时刻系统状态;A为状态转移矩阵;Z(k)为k时刻对应状态的观测值;H为观测矩阵;G为噪声矩阵;W(k)为白噪声;V(k)观测噪声;
规定W(k)和V(k)的均值为0,Q、R为协方差矩阵;所述卡尔曼滤波模型表述如下:
状态预测:
X(k+1|k)=AX(k|k)
协方差矩阵预测:
P(k+1|k)=AP(k|k)AT+GQGT
状态更新:
X(k+1|k+1)=X(k+1|k)+K(k+1)[Z(k+1)-HX(k+1|k)]
卡尔曼增益矩阵:
K(k+1)=P(k+1|k)HT[HP(k+1|k)HT+R]-1
协方差矩阵更新:
P(k+1|k+1)=[In-K(k+1)H]P(k+1|k)
其中,X(k+1|k)为k+1时刻的后验状态估计值,X(k|k)为k时刻的后验状态估计值,P(k+1|k)为k时刻的先验估计协方差X(k+1|x)的协方差,P(k+1|k+1)为k+1时刻的后验估计协方差,P(k|k)为k时刻的后验估计协方差,K(k+1)为滤波增益矩阵也就是卡尔曼增益,Z(k+1)为k+1时刻对应状态的观测值,In为n阶单位矩阵。
8.一种无人机,其特征在于,所述无人机在降落时执行如权利要求1-7任意一项所述的降落方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310681898.2A CN116627154B (zh) | 2023-06-09 | 2023-06-09 | 基于位姿预测和轨迹优化的无人机引导降落方法及无人机 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310681898.2A CN116627154B (zh) | 2023-06-09 | 2023-06-09 | 基于位姿预测和轨迹优化的无人机引导降落方法及无人机 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN116627154A CN116627154A (zh) | 2023-08-22 |
CN116627154B true CN116627154B (zh) | 2024-04-30 |
Family
ID=87602527
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310681898.2A Active CN116627154B (zh) | 2023-06-09 | 2023-06-09 | 基于位姿预测和轨迹优化的无人机引导降落方法及无人机 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116627154B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117930664A (zh) * | 2024-03-21 | 2024-04-26 | 哈尔滨华拓导航技术有限公司 | 基于北斗rtk差分定位的无人机降落控制优化系统 |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108227751A (zh) * | 2018-01-29 | 2018-06-29 | 广州亿航智能技术有限公司 | 一种无人机的降落方法及系统 |
CN110456821A (zh) * | 2019-08-22 | 2019-11-15 | 安徽大学 | 基于动态触发机制的飞行器轨迹最优控制方法和系统 |
CN112504261A (zh) * | 2020-11-09 | 2021-03-16 | 中国人民解放军国防科技大学 | 一种基于视觉锚点的无人机降落位姿滤波估计方法及系统 |
CN112650301A (zh) * | 2021-01-11 | 2021-04-13 | 四川泓宝润业工程技术有限公司 | 一种引导无人机精准降落控制方法 |
CN113189875A (zh) * | 2021-04-29 | 2021-07-30 | 湖南大学 | 一种基于圆形特征的无人机在移动平台上鲁棒着陆方法 |
CN114200948A (zh) * | 2021-12-09 | 2022-03-18 | 中国人民解放军国防科技大学 | 一种基于视觉辅助的无人机自主着舰方法 |
CN114851227A (zh) * | 2022-06-22 | 2022-08-05 | 上海大学 | 一种基于机器视觉与触觉融合感知的装置 |
CN115113636A (zh) * | 2022-06-14 | 2022-09-27 | 海丰通航科技有限公司 | 控制航空器自主降落至舰船的方法、系统、存储介质及计算设备 |
CN115903879A (zh) * | 2022-08-01 | 2023-04-04 | 中国民航管理干部学院 | 基于地形数据插值技术的无人机航迹规划方法 |
CN115993772A (zh) * | 2022-11-22 | 2023-04-21 | 中国人民解放军91776部队 | 一种基于贝塞尔曲线的四阶段二维制导方法 |
CN116185049A (zh) * | 2023-03-01 | 2023-05-30 | 南京航天国器智能装备有限公司 | 基于视觉引导的无人直升机自主着舰方法 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2014169354A1 (en) * | 2013-04-16 | 2014-10-23 | Bae Systems Australia Limited | Landing system for an aircraft |
US20220415187A1 (en) * | 2019-12-04 | 2022-12-29 | Ge Aviation Systems Llc | Apparatus, system, and method of providing a trajectory planning engine for unmanned aircraft |
US20220017235A1 (en) * | 2020-02-19 | 2022-01-20 | The Texas A&M University System | Autonomous landing systems and methods for vertical landing aircraft |
-
2023
- 2023-06-09 CN CN202310681898.2A patent/CN116627154B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108227751A (zh) * | 2018-01-29 | 2018-06-29 | 广州亿航智能技术有限公司 | 一种无人机的降落方法及系统 |
CN110456821A (zh) * | 2019-08-22 | 2019-11-15 | 安徽大学 | 基于动态触发机制的飞行器轨迹最优控制方法和系统 |
CN112504261A (zh) * | 2020-11-09 | 2021-03-16 | 中国人民解放军国防科技大学 | 一种基于视觉锚点的无人机降落位姿滤波估计方法及系统 |
CN112650301A (zh) * | 2021-01-11 | 2021-04-13 | 四川泓宝润业工程技术有限公司 | 一种引导无人机精准降落控制方法 |
CN113189875A (zh) * | 2021-04-29 | 2021-07-30 | 湖南大学 | 一种基于圆形特征的无人机在移动平台上鲁棒着陆方法 |
CN114200948A (zh) * | 2021-12-09 | 2022-03-18 | 中国人民解放军国防科技大学 | 一种基于视觉辅助的无人机自主着舰方法 |
CN115113636A (zh) * | 2022-06-14 | 2022-09-27 | 海丰通航科技有限公司 | 控制航空器自主降落至舰船的方法、系统、存储介质及计算设备 |
CN114851227A (zh) * | 2022-06-22 | 2022-08-05 | 上海大学 | 一种基于机器视觉与触觉融合感知的装置 |
CN115903879A (zh) * | 2022-08-01 | 2023-04-04 | 中国民航管理干部学院 | 基于地形数据插值技术的无人机航迹规划方法 |
CN115993772A (zh) * | 2022-11-22 | 2023-04-21 | 中国人民解放军91776部队 | 一种基于贝塞尔曲线的四阶段二维制导方法 |
CN116185049A (zh) * | 2023-03-01 | 2023-05-30 | 南京航天国器智能装备有限公司 | 基于视觉引导的无人直升机自主着舰方法 |
Non-Patent Citations (5)
Title |
---|
a novel framework for autonomous landing of the quadrotor on the moving platform by onboard vision sensors;qianqian cao等;2921 3rd international conference on industrial artificial intelligence;全文 * |
non-linear model predictive control for autonomous landing of a uav on a moving platform;Beniamino Pozzan等;2022IEEE conference on control technology and applications;全文 * |
一种面向移动平台的无人机自主降落控制方法;张伟,等;计算机仿真(第02期);全文 * |
自主着陆/着舰技术综述;唐大全;毕波;王旭尚;李飞;沈宁;;中国惯性技术学报(第05期);全文 * |
飞翼无人机自主着舰控制技术的综述;王鑫;陈欣;李继广;;微型机与应用(第04期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN116627154A (zh) | 2023-08-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111932588B (zh) | 一种基于深度学习的机载无人机多目标跟踪系统的跟踪方法 | |
CN114048889B (zh) | 基于长短期记忆网络的飞行器轨迹预测的方法 | |
Tisdale et al. | Autonomous UAV path planning and estimation | |
Saripalli et al. | Landing on a moving target using an autonomous helicopter | |
Levine et al. | Information-rich path planning with general constraints using rapidly-exploring random trees | |
CN109755995A (zh) | 基于ros机器人操作系统的机器人自动充电对接方法 | |
CN105759829A (zh) | 基于激光雷达的微型无人机操控方法及系统 | |
He et al. | On the design and use of a micro air vehicle to track and avoid adversaries | |
CN115661204B (zh) | 一种无人机集群对运动目标的协同搜寻与跟踪定位方法 | |
CN116627154B (zh) | 基于位姿预测和轨迹优化的无人机引导降落方法及无人机 | |
CN116907282B (zh) | 基于人工智能算法的无人靶机超低空飞行控制方法 | |
Arreola et al. | Object recognition and tracking using Haar-like Features Cascade Classifiers: Application to a quad-rotor UAV | |
Pritzl et al. | Cooperative navigation and guidance of a micro-scale aerial vehicle by an accompanying UAV using 3D LiDAR relative localization | |
CN115328212A (zh) | 一种基于无人机吊舱的目标追踪方法及装置 | |
CN115903880A (zh) | 一种基于改进强化学习的无人机自主图像导航与避障方法 | |
CN114689030A (zh) | 一种基于机载视觉的无人机辅助定位方法及系统 | |
Kim et al. | A deep-learning-aided automatic vision-based control approach for autonomous drone racing in game of drones competition | |
CN114265425A (zh) | 一种多旋翼无人机编队防撞控制方法 | |
Saripalli | Vision-based autonomous landing of an helicopter on a moving target | |
Lee et al. | A deep reinforcement learning control strategy for vision-based ship landing of vertical flight aircraft | |
Rezende et al. | Autonomous system for a racing quadcopter | |
CN112947569A (zh) | 基于预设性能四旋翼无人机视觉伺服目标跟踪控制方法 | |
US11865978B2 (en) | Object tracking system including stereo camera assembly and methods of use | |
CN115755575A (zh) | 一种基于ros的双云台无人机自主降落方法 | |
CN112241180B (zh) | 一种无人机移动平台降落引导的视觉处理方法 |
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 |