CN112129291B - 一种基于Bezier曲线的固定翼无人机航迹优化方法 - Google Patents
一种基于Bezier曲线的固定翼无人机航迹优化方法 Download PDFInfo
- Publication number
- CN112129291B CN112129291B CN202010869502.3A CN202010869502A CN112129291B CN 112129291 B CN112129291 B CN 112129291B CN 202010869502 A CN202010869502 A CN 202010869502A CN 112129291 B CN112129291 B CN 112129291B
- Authority
- CN
- China
- Prior art keywords
- distance
- bezier
- transition
- flight
- vector
- 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
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
本发明公开了一种基于Bezier曲线的固定翼无人机航迹优化方法。和基于常曲率圆弧的Dubins曲线航迹优化算法相比,该方法可以通过改变Bezier控制点从几何上调整航迹形状,并且具有参数化的表达式,在选取控制点时加入无人机安全曲率约束,使航迹满足曲率约束的同时保持航迹曲率的连续性。另外设计了二平分角法,减小了控制点求取的复杂度,具有极高的工程实用价值。
Description
技术领域
本发明涉及一种无人机航迹优化的算法设计,优化后的航迹满足无人机机动性约束,为航迹跟踪提供了有效的参考航迹,具体涉及基于Bezier曲线的航迹优化技术,属于航空器航迹优化技术领域。
背景技术
目前,通过航点规划器生成的目标航点序列,两两直线相连可以得到由直线段组成的多边形航迹。受动力学限制,无人机无法在两条直线航段之间完成急转弯,必须进行平滑过渡飞行。航迹优化技术解决了航迹的可飞性问题,优化后的航迹满足连续性和安全性约束。
大部分的固定翼无人机均采用标准圆弧进行航段之间的过渡转弯,得到Dubins目标航迹。该方式可利用Dubins曲线常曲率圆弧来约束无人机协调转弯时的滚转角,使航迹满足无人机机动约束。其优点是计算简单,设计过程简便,工程上易于实现,缺点是Dubins航迹在直线段与圆弧段过渡时,航迹曲率变化不连续。曲率的变化与侧向加速度制导指令直接相关,在航段切换时侧向加速度指令会发生突变。由于无人机指令响应速度有限,实际侧向加速度无法骤变,因而会产生较大的航迹跟踪误差。Bezier曲线可以通过改变控制点从几何上调整航迹形状,并且具有参数化的表达式,在选取控制点时加入无人机安全曲率约束,使航迹满足曲率约束的同时保持航迹曲率的连续性,本发明正是在此背景下产生的。
发明内容
发明目的:本发明的目的在于实现目标航点序列的航迹优化,使航迹满足无人机安全曲率约束的同时,航迹曲率连续,航段平缓光滑过渡,为无人机提供高精度的航迹跟踪参考。
技术方案:
一种基于Bezier曲线的固定翼无人机航迹优化方法,所述航迹是一组包含(n+1)个航点的目标航点序列,(n+1)个航点构成n个Bezier航段,第i个航点Wi和第i+1个航点Wi+1构成第i个航段,i=0,1,…,n;
该航迹优化方法根据不同的航段过渡策略,求出满足无人机安全曲率约束的过渡距离,得到各个Bezier控制点,最终确定Bezier航迹曲线,实现航迹优化,具体为:
1)采用提前转弯法作为航段过渡策略
①定距离法:通过第i和(i+1)个航段之间的夹角和给定的无人机安全曲率约束,求出Wi和Wi+1之间的安全过渡距离将dκ(i)作为Wi和Wi+1之间的实际过渡距离dfit(i);其中γ是向量和的夹角,c1=7.2364, κsafe为无人机的安全曲率;
②交互距离法:
a)定义约束因子factor为dW(n-1)、 这四个约束项参数中的最小值;其中,dW(n-1)为Wn-1和Wn之间的距离,dκ(n-2)为Wn-2和Wn-1之间的安全过渡距离,dleft(n-2)为Wn-2和Wn-1之间的距离裕度,dκ(n-3)为Wn-3和Wn-2之间的安全过渡距离,dleft(n-3)为Wn-3和Wn-2之间的距离裕度;
b)如果约束因子factor为dW(n-1)、两项其中之一,则Wn-2和Wn-1之间的实际过渡距离dfit(n-2)=factor;如果约束因子factor为 两项其中之一,则dfit(n-2)=min(dW(n-1),(dW(n-2)-factor)),dW(n-2)为Wn-2和Wn-1之间的距离;W0和W1之间的实际过渡距离dfit(0)=min[dW(0),(dW(1)-dfit(1)];
③采用①或②中方法求得实际过渡距离后,利用两条3次Bezier曲线进行Wi到Wi+2之间过渡航迹的设计:
第一条3次Bezier曲线的Bezier控制点B0-B3的坐标关系如下:
第二条3次Bezier曲线的Bezier控制点E0-E3的坐标关系如下:
2)采用过点跟踪法作为航段过渡策略
利用两条3次Bezier曲线进行Wi到Wi+2之间过渡航迹的设计:
第一条3次Bezier曲线的Bezier控制点B0-B3的坐标关系如下:
第二条3次Bezier曲线的Bezier控制点E0-E3的坐标关系如下:
进一步,dleft(i)与dfit(i)和dW(i)之间的关系,其数学表达式为:
有益效果:
1、本发明的一种基于Bezier曲线的固定翼无人机航迹优化方法,克服了常规Dubins航迹直线段和圆弧航迹曲率不连续的问题,使航段之间的飞行过渡平缓,为航迹跟踪提高精度的参考轨迹;
2、本发明的一种基于Bezier曲线的固定翼无人机航迹优化方法,通过几何构造方法,得到Bezier控制点,避免了传统Bezier控制点的复杂求解过程。实际工程应用中,只需要存储控制点坐标,利用控制点和Bezier参数表达式可以实现航迹的描述,算法时间复杂度和控制复杂度较低,航迹优化效率较高;
3、本发明的一种基于Bezier曲线的固定翼无人机航迹优化方法,适用于多种类型的固定翼无人机的航迹优化,是一种通用的航迹优化方法。在已知无人机安全曲率约束的前提下,利用曲率约束与过渡距离模型,根据不同过渡策略,可快速实现固定航点的优化。
附图说明
图1是两条3次Bezier航段G2连续的几何关系图;
图2是提前转弯法示意图;
图3是过点跟踪法示意图;
图4是基于曲率约束的Bezier航迹示意图;
图5是定距离法航迹优化示意图;
图6是交互距离算法伪代码图;
图7是交互距离法航迹优化示意图;
图8是Bezier过点跟踪航迹优化示意图;
图9是固定距离航迹优化航迹曲线示意图;
图10是固定距离航迹优化航迹曲率变化示意图;
图11是交互距离航迹优化航迹曲线示意图;
图12是交互距离航迹优化航迹曲率变化示意图;
图13是过点跟踪航迹优化航迹曲线示意图;
图14是过点跟踪航迹优化航迹曲率变化示意图。
具体实施方式
下面详细描述本发明的实施方式,所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的,仅用于解释本发明,而不能解释为对本发明的限制。
基于Bezier曲线的固定翼无人机航迹优化方法,具体设计步骤如下:
步骤1,设计3次Bezier组合航迹p(u),其数学表达式为:
式中,bj称为控制顶点或Bezier控制点,将它们依次连接可构成该曲线的控制多边形,Bj,3(u)为3次伯恩斯坦基函数,其数学表达式为:
如图1所示,当i=1时,两条3次Bezier航段在公共连接点p0(1)=p1(0)=b3处的G1连续性(切线方向相同)条件,可用Bezier控制点向量进行表示,其数学表达式为:
从上式可以看出,三顶点b2、b3、b4共线且顺序排列,它们的连线是公共连接点处的公切线。在要求公共连接点b3处G2连续(曲率连续)时,用κ0(1)表示连接点的左曲率,用κ1(0)表示连接点的右曲率,3次Bezier曲线在公共连接点b3处的曲率可用控制点向量表示,其数学表达式为:
步骤2中针对不同飞行任务的需求,设计了两种航段过渡策略:提前转弯法和过点跟踪法。提前转弯法是最常用的航段过渡方法,如图2所示,利用一条与两直线航段相切的圆弧r1作为过渡航段,得到圆弧过渡航迹,在还未到达航点O时提前转弯,完成航段过渡;过点跟踪法则要求无人机必须到达指定航点,如图3所示,利用三个半径相等的标准圆弧r1、r2、r3进行组合,得到一条通过中间航点O的过渡航迹,当对任务执行要求较高,必须到达任务航点时,可使用该策略。
步骤3中,利用两条3次Bezier曲线进行过渡航迹的设计,如图4所示。第一段航迹B0-B3Bezier控制点的坐标关系如下:
第二段曲线E3-E0控制点的坐标关系如下:
其中,系数为c1=7.2364,hb=he=c3d,gb=ge=c2c3d,γ是向量和的夹角,Ψ是向量和的夹角,Ψ=Υ/2,d是B0和W2之间的长度或者E0和W2之间的长度,称为过渡距离。为向量的单位向量,为向量的单位向量,为向量的单位向量。是一个连接航点W1和W3的向量,Dy是的Y轴分量。d是需要设计的过渡距离参数。如图4所示,在过渡航段中航迹曲率先增大后减小,并在两个Bezier航迹的连接点处达到最大值。如果无人机的安全曲率κsafe已知,连接点处的曲率为κpath,是Bezier航迹的曲率上界,当κpath≤κsafe时,过渡航段满足无人机曲率约束。过渡距离与无人机安全曲率约束的模型,其数学表达式为:
当κpath=κsafe时,可以求出满足无人机曲率约束的最小安全过渡距离参数dκ:
步骤4中,通过设计过渡距离算法来确定航迹优化曲线。根据步骤2中,不同的航段过渡策略,设计提前转弯法和过点跟踪法。一组包含(n+1)个航点Wi(i=0,1,…,n)的目标航点序列,构成n个Bezier航段,将每个航段中第一个航点的序号设为航段id编号,这些航段id编号从0到(n-1),为了方便后续算法的描述与设计,现定义如下四种参数:
dW(i):称为航段长度。表示两相邻航点Wi和Wi+1之间的距离;
dκ(i):称为安全过渡距离。表示达到无人机安全转弯曲率时的航段i的过渡距离;
dfit(i):称为实际过渡距离。表示在航迹优化中选取的过渡距离;
dleft(i):称为距离裕度。在航段i中,除了过渡距离的消耗外,剩下的距离作为该航段的距离裕度。dleft(i)与dfit(i)和dW(i)之间的关系,其数学表达式为:
根据提前转弯策略设计定距离法和交互距离法。定距离法是通过两航段之间的夹角和给定的无人机安全曲率约束,求出安全过渡距离dκ(i),将它作为实际过渡距离,其数学表达式为:
dfit(i)=dκ(i) (i=0,1…n-1)
如图5所示,由7个目标航点构成的多边形航迹中,通过定距离法确定了过渡距离。可以看出首段航迹和末端航迹只有一个过渡距离,中间航段则有两个过渡距离。定距离法形式简单,可以根据过渡距离快速确定一条满足无人机安全曲率约束的航迹曲线。
交互距离法在设计过渡距离dfit(i)时考虑连续三个航段的最大曲率约束长度dκ(i),设计约束因子,使优化后的航迹在满足无人机安全曲率约束的同时,距离裕度最小。具体的算法伪代码图6所示。从最后一个航段开始,向前递推设计过渡距离dfit,首先设计dfit(n-2),它大致可分为3个步骤:
b)判断约束因子factor所取的最小值是前面两项还是后面两项,如果为前两项其中之一,dfit(n-2)=factor;
c)如果约束因子factor为后两项其中之一,在最终过渡距离的选取时加入距离裕度检查,设计为dfit(n-2)=min(dW(n-1),(dW(n-2)-factor))。
当所处航段不是末航段时,同样采用上面的步骤进行过渡距离的设计,其中唯一的区别是第一个约束项dW(k)被(dW(k)-dfit(k))取代,可算出所有中间航段的过渡距离。首航段的过渡距离dfit(0)设计为dfit(0)=min[dW(0),(dW(1)-dfit(1)]。图7中显示了在同样的目标航点下,利用交互式构造算法进行航迹优化的示意图,与采用折半式构造算法相比(图5),整个航迹的距离裕度更小,其中航段W1W2、W2W3和W3W4距离裕度为零,航段长度得到了充分的利用。
过点跟踪在到达目标航点后再进行航向调整,飞往下一个目标航点。利用Bezier曲线进行过点跟踪算法设计如图8所示,将航点W2沿向量扩展到Q点,γ′是向量和向量之间的夹角,dQ是向量的模长,也是需要设计的过渡距离,新的航迹同样由两条直线航迹和一段过渡曲线航迹构成。根据正弦定理,过渡距离dQ的数学表达式为:
过渡距离dQ利用过渡距离与无人机安全曲率约束的模型,其数学表达式为:
其中D为航段W2W3的长度。联立以上两式得到如下方程:
γ′为唯一的未知参数,可以通过求解方程确定,根据γ′可求出过渡距离dQ,进而确定Bezier过点跟踪航迹的控制点,通过参数Bezier公式进行航迹的描述。
本发明的一种基于Bezier曲线的固定翼无人机航迹优化方法。以无人机安全曲率κsafe=5×10-4m-1为例,根据过点跟踪策略,利用定距离法进行航迹优化的效果如图9-图10所示,利用交互距离进行航迹优化的效果如图11-图12所示;采用过点跟踪策略进行航迹优化的效果如图13-图14所示。通过与传统的Dubins航迹优化相比,在确保航迹满足无人机机动约束的同时,航迹曲率依然保持连续性,为无人机跟踪提供了精确的航迹参考。
以上所述仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。
Claims (2)
1.一种基于Bezier曲线的固定翼无人机航迹优化方法,其特征在于,所述航迹是一组包含(n+1)个航点的目标航点序列,(n+1)个航点构成n个Bezier航段,第i个航点Wi和第i+1个航点Wi+1构成第i个航段,i=0,1,…,n;
该航迹优化方法根据不同的航段过渡策略,求出满足无人机安全曲率约束的过渡距离,得到各个Bezier控制点,最终确定Bezier航迹曲线,实现航迹优化,具体为:
1)采用提前转弯法作为航段过渡策略
①定距离法:通过第i和(i+1)个航段之间的夹角和给定的无人机安全曲率约束,求出Wi和Wi+1之间的安全过渡距离将dκ(i)作为Wi和Wi+1之间的实际过渡距离dfit(i);其中Υ是向量和的夹角,c1=7.2364, κsafe为无人机的安全曲率;
②交互距离法:
a)定义约束因子factor为dW(n-1)、 这四个约束项参数中的最小值;其中,dW(n-1)为Wn-1和Wn之间的距离,dκ(n-2)为Wn-2和Wn-1之间的安全过渡距离,dleft(n-2)为Wn-2和Wn-1之间的距离裕度,dκ(n-3)为Wn-3和Wn-2之间的安全过渡距离,dleft(n-3)为Wn-3和Wn-2之间的距离裕度;
b)如果约束因子factor为dW(n-1)、两项其中之一,则Wn-2和Wn-1之间的实际过渡距离dfit(n-2)=factor;如果约束因子factor为 两项其中之一,则dfit(n-2)=min(dW(n-1),(dW(n-2)-factor)),dW(n-2)为Wn-2和Wn-1之间的距离;W0和W1之间的实际过渡距离dfit(0)=min[dW(0),(dW(1)-dfit(1)];
③采用①或②中方法求得实际过渡距离后,利用两条3次Bezier曲线进行Wi到Wi+2之间过渡航迹的设计:
第一条3次Bezier曲线的Bezier控制点B0-B3的坐标关系如下:
第二条3次Bezier曲线的Bezier控制点E0-E3的坐标关系如下:
2)采用过点跟踪法作为航段过渡策略
利用两条3次Bezier曲线进行Wi到Wi+2之间过渡航迹的设计:
第一条3次Bezier曲线的Bezier控制点B0-B3的坐标关系如下:
第二条3次Bezier曲线的Bezier控制点E0-E3的坐标关系如下:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010869502.3A CN112129291B (zh) | 2020-08-26 | 2020-08-26 | 一种基于Bezier曲线的固定翼无人机航迹优化方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010869502.3A CN112129291B (zh) | 2020-08-26 | 2020-08-26 | 一种基于Bezier曲线的固定翼无人机航迹优化方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112129291A CN112129291A (zh) | 2020-12-25 |
CN112129291B true CN112129291B (zh) | 2022-04-01 |
Family
ID=73848522
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010869502.3A Active CN112129291B (zh) | 2020-08-26 | 2020-08-26 | 一种基于Bezier曲线的固定翼无人机航迹优化方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112129291B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113467498B (zh) * | 2021-07-14 | 2022-07-01 | 西北工业大学 | 一种基于Bezier-凸优化的运载火箭上升段轨迹规划方法 |
Family Cites Families (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7228227B2 (en) * | 2004-07-07 | 2007-06-05 | The Boeing Company | Bezier curve flightpath guidance using moving waypoints |
CN103149937B (zh) * | 2013-02-26 | 2015-10-21 | 北京航空航天大学 | 一种基于曲率补偿的横侧向曲线航迹跟踪方法 |
US9785146B2 (en) * | 2016-01-26 | 2017-10-10 | Northrop Grumman Systems Corporation | Maneuver planning with higher order rational Bezier curves |
CN108204814B (zh) * | 2016-12-20 | 2021-06-22 | 南京理工大学 | 无人机三维场景路径导航平台及其三维改进路径规划方法 |
CN107168305B (zh) * | 2017-04-01 | 2020-03-17 | 西安交通大学 | 路口场景下基于Bezier和VFH的无人车轨迹规划方法 |
CN107608372B (zh) * | 2017-08-14 | 2020-12-04 | 广西师范大学 | 一种基于改进rrt算法与改进ph曲线相结合的多无人机协同航迹规划方法 |
CN108153328B (zh) * | 2017-12-18 | 2020-04-14 | 北京理工大学 | 一种基于分段贝塞尔曲线的多导弹协同航迹规划方法 |
CN108398960B (zh) * | 2018-03-02 | 2021-01-26 | 南京航空航天大学 | 一种改进APF与分段Bezier相结合的多无人机协同目标追踪方法 |
CN109839953A (zh) * | 2019-02-19 | 2019-06-04 | 上海交通大学 | 基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法 |
CN110297500A (zh) * | 2019-06-28 | 2019-10-01 | 天津大学 | 一种给定多航路点下无人机的连续航迹规划方法 |
-
2020
- 2020-08-26 CN CN202010869502.3A patent/CN112129291B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN112129291A (zh) | 2020-12-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107702716B (zh) | 一种无人驾驶路径规划方法、系统和装置 | |
US7228227B2 (en) | Bezier curve flightpath guidance using moving waypoints | |
CN112269965B (zh) | 一种非完整约束条件下的连续曲率路径优化方法 | |
CN109664884B (zh) | 一种可变车速下的可拓自适应车道保持控制方法 | |
CN109947123A (zh) | 一种基于视线导引律的无人机路径跟踪与自主避障方法 | |
CN111258231A (zh) | 仿猛禽视觉导航的自主空中加油对接半物理系统及其方法 | |
CN108459602B (zh) | 多障碍复杂环境下欠驱动无人艇的自主靠泊方法 | |
CN112129291B (zh) | 一种基于Bezier曲线的固定翼无人机航迹优化方法 | |
CN110750850A (zh) | 强约束复杂任务条件下的三维剖面优化设计方法、系统及介质 | |
Lee et al. | RRT-based path planning for fixed-wing UAVs with arrival time and approach direction constraints | |
Zhang et al. | Smooth path and velocity planning under 3D path constraints for car-like vehicles | |
CN110928314B (zh) | 一种基于轨迹预测的纯跟踪模型改进算法 | |
Cao et al. | Implementing B-spline path planning method based on roundabout geometry elements | |
CN113835442B (zh) | 高超声速滑翔飞行器线性伪谱再入制导方法和系统 | |
CN109606364B (zh) | 一种分层式自学习可拓神经网络车道保持控制方法 | |
Silva et al. | Quadrotor thrust vectoring control with time and jerk optimal trajectory planning in constant wind fields | |
CN113093804B (zh) | 基于反演滑模控制的无人艇编队控制方法和控制系统 | |
Jazar | Mathematical theory of autodriver for autonomous vehicles | |
Lorenz et al. | A decoupled approach for trajectory generation for an unmanned rotorcraft | |
Sudhakara et al. | Optimal trajectory planning based on bidirectional spline-RRT* for wheeled mobile robot | |
Girbés et al. | Continuous-curvature control of mobile robots with constrained kinematics | |
CN111290406A (zh) | 一种路径规划的方法、机器人及存储介质 | |
CN109062213A (zh) | 一种基于修正比例导引的智能车自动驾驶的方法 | |
Piscini et al. | Experimental comparison of trajectory control and planning algorithms for autonomous vehicles | |
Takano et al. | Posture-to-posture trajectory planning for an autonomous car using triple clothoid segments |
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 |