CN115933736A - 一种航迹快速规划精准控制飞行器的方法 - Google Patents

一种航迹快速规划精准控制飞行器的方法 Download PDF

Info

Publication number
CN115933736A
CN115933736A CN202211570165.3A CN202211570165A CN115933736A CN 115933736 A CN115933736 A CN 115933736A CN 202211570165 A CN202211570165 A CN 202211570165A CN 115933736 A CN115933736 A CN 115933736A
Authority
CN
China
Prior art keywords
node
aircraft
point
vector
coordinate system
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.)
Pending
Application number
CN202211570165.3A
Other languages
English (en)
Inventor
吕云海
刘艳玺
朱英杰
邢雅彤
王银忠
高帅
郭雅欣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Dongying Power Industry Bureau Of State Grid Shandong Electric Power Co
Original Assignee
Dongying Power Industry Bureau Of State Grid Shandong Electric Power Co
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Dongying Power Industry Bureau Of State Grid Shandong Electric Power Co filed Critical Dongying Power Industry Bureau Of State Grid Shandong Electric Power Co
Priority to CN202211570165.3A priority Critical patent/CN115933736A/zh
Publication of CN115933736A publication Critical patent/CN115933736A/zh
Pending legal-status Critical Current

Links

Images

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种航迹快速规划精准控制飞行器的方法，建立了完整的仅存在定位信息限制的飞行器航迹路线模型，确定飞行器的可行集、提出信息素的更新法则、启发函数，得出基于最大优化在飞行器即时转弯中的航迹长度和校正次数得解。考虑飞行器在转弯时会受到结构和控制系统的限制，无法完成即时转弯，并存在一转弯半径，继而在飞行器航迹校正中引入切线圆轨迹模型，在保证相邻节点间最小化航迹路线长度的情况下以最小化校正次数，得到在飞行器即时转弯中成功以最短距离到达终点的方法。

Description

一种航迹快速规划精准控制飞行器的方法
技术领域
本发明涉及飞行器航迹规划技术领域，具体涉及一种航迹快速规划精准控制飞行器的方法。
背景技术
智能飞行器因具有起降简单、操作灵活、自动控制智能化、系统配置多样化等特点，在农业灌溉、地理测绘、气象预报、安全报警以及军事作战等生产生活多个领域有着广泛的应用。智能飞行器在现实生活中扮演着重要的角色，未来的发展方向也更加趋于智能化、自动化。
但是，这类飞行器由于自身结构的限制，更多依靠人力对其飞行轨迹进行操纵控制，无法自发性根据定位系统对自身进行全方位的精准定位，当误差积累到一定程度可能产生严重后果。因此，飞行器能否依据已知飞行的位置信息对航迹作出快速而准确的规划是飞行器脱离人力控制、迈向智能化的一项重点任务，也是国内外着力攻克的难点之一，更是智能飞行器能被准确控制的基本保障。
发明内容
本发明为了克服以上技术的不足，提供了一种实现飞行器在已知全部位置坐标等信息的前提下，在最短时间内自行完成航迹规划、位置坐标校正等任务的方法，该方法脱离人为控制，以实现智能化、自动化、快速化航迹规划功能。
本发明克服其技术问题所采用的技术方案是：
一种航迹快速规划精准控制飞行器的方法，其特征在于，包括如下步骤：
a)依照获取的各类地理定位信息使飞行器从A地到B地进行航迹规划；
b)计算得到航迹模型
Figure BDA0003982194980000011
得到最优航迹；
c)计算得到航迹模型
Figure BDA0003982194980000012
得到飞行器转弯受结构和控制系统的限制下的航迹规划。
进一步的，步骤a)包括如下步骤：
a-1)飞行器从A地到B地飞行时，飞行器飞行过程中实时定位约束为：εh＜θ且εv＜θ，其中εh为水平误差，εv为垂直误差；
a-2)如果飞行器从A地到B地的路径为直线，则无需校正，如果飞行器从A地到B地的路径不为直线，则飞行器采取直线行驶到最近一个校正点的方式校正飞行器误差，飞行器从从A地到B地的路径由多个节点连成的线段组成，将飞行器的路径简化为一个有向图，每一个校正点均为有向图中的一个点，ωij为有向图中是否存在一条由点i到点j的线路，当ωij＝0时表示路径规划中飞行器不可由点i飞向点j，当ωij＝1时表示路径规划中飞行器可由点i飞向点j；
a-3)飞行器从节点i到节点j时向量ij与单位向量iB的夹角θ满足
Figure BDA0003982194980000021
飞行器满足约束条件为：
Figure BDA0003982194980000022
其中
Figure BDA0003982194980000023
为飞行器在节点i处的水平误差，
Figure BDA0003982194980000024
为飞行器在节点i处的垂直误差，Lij为节点i与节点j之间的距离，
Figure BDA0003982194980000025
Xi为节点i在三维坐标系中X轴坐标，Xj为节点j在三维坐标系中X轴坐标，Yi为节点i在三维坐标系中Y轴坐标，Yj为节点j在三维坐标系中Y轴坐标，Zi为节点i在三维坐标系中Z轴坐标，Zj为节点j在三维坐标系中Z轴坐标，δ为飞行器产生系统误差的比例系数，α1为飞行中垂直方向误差的上限值，α2为飞行中垂直方向误差的下限值，β1为飞行中水平方向误差的上限值，β2为飞行中水平方向误差的下限值；
a-4)对有向图上的任意一节点i，在
Figure BDA0003982194980000026
且
Figure BDA0003982194980000027
时取到所有最大可能的点，所有最大可能的点构成最大可能到达点集合J，集合J满足约束条件为：
Figure BDA0003982194980000031
进一步的，步骤b)包括如下步骤：
b-1)通过公式
Figure BDA0003982194980000032
建立航迹模型
Figure BDA0003982194980000033
其中Lmin为飞行器从A地到B地的最短路径，V为权重系数，Ω为飞行器航迹，
Figure BDA0003982194980000034
n为经过的校正点的数量，nmin为需要校正点的最少个数，
Figure BDA0003982194980000035
d为飞行器由某一点到任意一个校正点不经过校正可飞行的最大距离，
Figure BDA0003982194980000036
N为所有节点的总数，
Figure BDA0003982194980000037
dij为飞行器由节点i到节点j在不经过校正点可飞行的最大距离，航迹模型
Figure BDA0003982194980000038
满足约束条件为：
Figure BDA0003982194980000039
Figure BDA00039821949800000310
为飞行器在节点j处的水平误差，
Figure BDA00039821949800000311
为飞行器在节点j处的垂直误差，
Figure BDA00039821949800000312
为飞行器在节点i到节点j方向上的垂直误差，
Figure BDA00039821949800000313
为飞行器在节点i到节点j方向上的水平误差；
b-2)当飞行器从校正点i到达下一个校正点j时通过
Figure BDA00039821949800000314
确定集合J，在距终点B点从0到
Figure BDA0003982194980000041
的范围内，
Figure BDA0003982194980000042
时，集合J中有一个点j′，LiB校正点i与点B之间的距离，
Figure BDA0003982194980000043
XB为点B在三维坐标系中X轴坐标，YB为点B在三维坐标系中Y轴坐标，ZB为点B在三维坐标系中Z轴坐标，点j′为使{Lij+LiB}最小的点；
b-3)飞行器从起点A飞到终点B得到一条航迹，通过公式τij(t+1)＝(1-ρ)τij(t)+Δτij(t)计算第t轮飞行器到达终点时，更新信息素浓度后的集合J中个可行点之间路径的信息素浓度τij(t+1)，式中ρ为信息素更新系数，τij(t)为飞行器在第t-1轮遍历集合J中所含可行点之间路径的信息素浓度，该信息素浓度表示飞行器成功到达终点时此轮遍历路径上节点i和节点j之间路径的遍历次数，Δτij(t)为信息素增量，
Figure BDA0003982194980000044
m为飞行器的总个数，
Figure BDA0003982194980000045
Lk(t)为经过t轮信息素更新后，各节点间距离之和，nk(t)为经过t轮信息素更新后，飞行器经过的校正点的数量，将信息素浓度τij(t+1)最高的航迹作为最优航迹；
b-4)通过公式
Figure BDA0003982194980000046
计算得到启发式函数Hij(t)，式中LjB校正点j与点B之间的距离，
Figure BDA0003982194980000047
Xj为校正点j在三维坐标系中X轴坐标，Yj为校正点j在三维坐标系中Y轴坐标，Zj为校正点j在三维坐标系中Z轴坐标，
Figure BDA0003982194980000051
为威胁代价函数，式中f为可行点阈值，nj为飞行器从节点i到节点j后，设在节点j的可行点个数，
Figure BDA0003982194980000052
b-5)通过公式
Figure BDA0003982194980000053
计算得到第q个飞行器经过t轮遍历后从节点i到节点j的概率
Figure BDA0003982194980000054
进一步的，步骤c)包括如下步骤：
c-1)飞行器到达节点i时的前进方向为向量v1的方向，将飞行器经过节点i且方向为v1的直线固定，利用节点j的坐标在三维空间中确定一平面ijv1，以节点i为切点，以v1为切线，在平面ijv1内做靠近节点i一侧的半径为飞行器转弯半径R的圆o，以节点j向圆o做切线，切点为q，得到飞行器最短路径为i→q→j，节点i与节点j之间的直线与前进方向v1之间的夹角为γ；
c-2)当
Figure BDA0003982194980000055
时，通过公式
Figure BDA0003982194980000056
计算得到圆o的圆心到节点j的距离Loj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ2，
Figure BDA0003982194980000057
通过公式
Figure BDA0003982194980000058
计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ1，通过公式
Figure BDA0003982194980000059
计算得到飞行器的最小路径；
c-3)当
Figure BDA0003982194980000061
时，通过公式
Figure BDA0003982194980000062
计算得到圆o的圆心到节点j的距离Loj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ2，
Figure BDA0003982194980000063
通过公式
Figure BDA0003982194980000064
计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ1，通过公式
Figure BDA0003982194980000065
计算得到飞行器的最小路径；
c-4)v1＝(a1,b1,c1)，a1为向量v1在三维坐标系中X轴坐标，b1为向量v1在三维坐标系中Y轴坐标，c1为向量v1在三维坐标系中Z轴坐标，节点j到圆o的切线方向为向量v2的方向，v2＝(a2,b2,c2)，a2为向量v2在三维坐标系中X轴坐标，b2为向量v2在三维坐标系中Y轴坐标，c2为向量v2在三维坐标系中Z轴坐标，节点i到节点j的直线方向为向量vij的方向，vij＝(aij,bij,cij)，aij为向量vij在三维坐标系中X轴坐标，bij为向量vij在三维坐标系中Y轴坐标，cij为向量vij在三维坐标系中Z轴坐标，向量v1、向量v2、向量vij共面，行列式
Figure BDA0003982194980000066
通过公式
Figure BDA0003982194980000067
计算得到向量v2；
c-5)计算得到节点i与节点j之间的距离Lij后通过公式
Figure BDA0003982194980000068
建立航迹模型
Figure BDA0003982194980000069
航迹模型
Figure BDA00039821949800000610
满足约束条件为：
Figure BDA0003982194980000071
通过对航迹模型
Figure BDA0003982194980000072
的求解，得到飞行器在已转转弯半径R从节点i到节点j的最短路径。本发明的有益效果是：过递进计算方法，首先建立了完整的仅存在定位信息限制的飞行器航迹路线模型，确定飞行器的可行集、提出信息素的更新法则、启发函数的设计方案，在此基础上得出基于最大优化在飞行器即时转弯中的航迹长度和校正次数得解。之后考虑飞行器在转弯时会受到结构和控制系统的限制，无法完成即时转弯，并存在一转弯半径，继而在飞行器航迹校正中引入切线圆轨迹模型，在保证相邻节点间最小化航迹路线长度的情况下以最小化校正次数，得到在飞行器即时转弯中成功以最短距离到达终点的方法。
通过如上计算设计的控制器，可成功实现飞行器在已知全部位置坐标等信息的前提下，在最短时间内自行完成航迹规划、位置坐标校正等任务的功能。
附图说明
图1为飞行器可行路径图；
图2为飞行器在i点的航向与j点为锐角的最优航迹；
图3为飞行器在i点的航向与j点为钝角的最优轨迹。
具体实施方式
下面结合附图1、附图2、附图3对本发明做进一步说明。
本发明的航迹快速规划精准控制飞行器的方法，对现实环境的抽象化模型处理：1.所有位置的定位信息是已知的且固定不变的，不受天气等不可控因素干扰。飞行器在飞行途径每一个位置坐标点时会对垂直方向信息和水平方向信息进行校正，已改变航向，确保按照最优航线航行。这些需要校正的点我们称之为校正点，并在水平和垂直方向拆分为水平校正点和垂直校正点。飞行器每飞行1m，由于自身定位仪器精度不足，在校正过程中垂直误差和水平误差将各增加Δθ个专用单位(以下简称单位)。到达终点时垂直误差和水平误差均应小于θ个单位，根据现实情况，可假设当垂直误差和水平误差均小于θ单位时，飞行器仍能够按照规划路径飞行。
2.飞行器从任意一点A到点B飞行是一个连续过程。
3.想情况下不考虑飞机的功耗问题。
具体的，包括如下步骤：
a)依照获取的各类地理定位信息使飞行器从A地到B地进行航迹规划，飞行器在前进时方向可以任意改变。
b)计算得到航迹模型
Figure BDA0003982194980000081
得到最优航迹。
c)计算得到航迹模型
Figure BDA0003982194980000082
得到飞行器转弯受结构和控制系统的限制下的航迹规划，飞行器的最小转弯半径是已知的。
实施例1：
进一步的，步骤a)包括如下步骤：
1.设置飞行器飞行过程中实时定位约束：
飞行器的垂直误差和水平误差均应小于θ个单位，可以按照规划路径正常飞行，并且飞行器到达终点时垂直误差和水平误差均应小于θ个单位，对定位误差有如下约束：εh＜θ且εv＜θ，其中εh为水平误差，εv为垂直误差。
2.简化飞行器飞行路径：
若从任意一点到另一点的路径为直线，无需校正；否则，校正飞行器误差。飞行器采取直线驶向最近一个校正点的方式。将所经过的校正点作为节点，飞行器从A点到B点的整个路径可以看作由多个节点连城的线段所组成。将飞行器的路径简化为一个有向图，每一个校正点都为路径中的一个点，用ωij判断飞行器能否从点i飞向点j并在点j校正(即有向图中是否存在一条由i到j的线路)，当ωij＝0时表示路径规划中飞行器不可由点i飞向点j，当ωij＝1时表示路径规划中飞行器可由点i飞向点j。
3.引入飞行器在校正点处误差校正约束和飞行方向约束：
飞行器从节点i能否驶到节点j，与i点、j点之间的距离Lij和飞行器飞行中自身产生的误差有关。规定向量ij与单位向量iB的夹角θ必须小于90度，即
Figure BDA0003982194980000091
即ij·iB＞0。飞行器满足约束条件为：
Figure BDA0003982194980000092
其中
Figure BDA0003982194980000093
为飞行器在节点i处的水平误差，
Figure BDA0003982194980000094
为飞行器在节点i处的垂直误差，Lij为节点i与节点j之间的距离，
Figure BDA0003982194980000095
Xi为节点i在三维坐标系中X轴坐标，Xj为节点j在三维坐标系中X轴坐标，Yi为节点i在三维坐标系中Y轴坐标，Yj为节点j在三维坐标系中Y轴坐标，Zi为节点i在三维坐标系中Z轴坐标，Zj为节点j在三维坐标系中Z轴坐标，δ为飞行器产生系统误差的比例系数，由飞行器硬件决定，α1为飞行中垂直方向误差的上限值，α2为飞行中垂直方向误差的下限值，大于此误差飞行器将报错不可维持继续飞行，β1为飞行中水平方向误差的上限值，β2为飞行中水平方向误差的下限值，大于此误差飞行器将报错不可维持继续飞行。
4.确定所有可行点范围约束：
利用上面的约束条件，对有向图上的任意一节点i，在
Figure BDA0003982194980000096
且
Figure BDA0003982194980000097
时取到所有最大可能的点，所有最大可能的点构成最大可能到达点集合J，集合J满足约束条件为：
Figure BDA0003982194980000098
利用该约束条件作出节点i与最大可能到达点集合J的连接情况，确定所有可行点的范围。
实施例2：
步骤b)包括如下步骤：
把飞行器经过校正区域校正次数降到最少，即求min(L,n)，其中L为航迹长度，n为经过的校正点的数量，V为引入的权重系数，具体步骤为：
1.对目标进行归一化处理：
飞行器从出发点A到终点B的最短路径为Lmin，需要校正点的最少个数为nmin，Lmin用LAB代替，即令LAB＝Lmin，令
Figure BDA0003982194980000101
d为飞行器由某一点到任意一个校正点不经过校正可飞行的最大距离。取
Figure BDA0003982194980000102
将目标函数可以改写为：
Figure BDA0003982194980000103
得到航迹模型
Figure BDA0003982194980000104
Figure BDA0003982194980000105
其中Ω为飞行器航迹，
Figure BDA0003982194980000106
N为所有节点的总数，
Figure BDA0003982194980000107
dij为飞行器由节点i到节点j在不经过校正点可飞行的最大距离，航迹模型
Figure BDA0003982194980000108
满足约束条件为：
Figure BDA0003982194980000109
Figure BDA00039821949800001010
为飞行器在节点j处的水平误差，
Figure BDA00039821949800001011
为飞行器在节点j处的垂直误差，
Figure BDA00039821949800001012
为飞行器在节点i到节点j方向上的垂直误差，
Figure BDA00039821949800001013
为飞行器在节点i到节点j方向上的水平误差。
2.根据航迹模型
Figure BDA00039821949800001014
得到如下求解算法：
(1)确定校正点i的下一可达点集合J(可行集)：当飞行器到达下一个校正点j时通过
Figure BDA0003982194980000111
确定集合J。终点处对于误差的要求与水平校正点、垂直校正点处不同，在靠近终点时采取不同设计：因为终点处对于误差的要求为θ，在距终点B点从0到
Figure BDA0003982194980000112
的范围内，飞行器只需要分别进行一次水平校正和垂直校正即可到达终点，即当
Figure BDA0003982194980000113
时，可行集J中只有一个点j′，其中j′为上述使{Lij+LiB}最小的点，得出LiB两种不同范围下的可行集，LiB校正点i与点B之间的距离，
Figure BDA0003982194980000114
XB为点B在三维坐标系中X轴坐标，YB为点B在三维坐标系中Y轴坐标，ZB为点B在三维坐标系中Z轴坐标。
(2)更新信息素并记录信息素浓度：
在三维空间内进行路径规划，采用全局信息素更新，即一个飞行器从起点A飞到终点B得到一条航迹，以该航迹的长度和经过校正区域校正的次数作为评价来判断信息素的多少，然后从集合航迹中选择最优航迹，对该航迹上各点的信息素进行更新，完成第t次循环，飞行器在第t轮遍历集合J所含可行点的过程中，成功到达终点，此轮遍历路径上任意两点i和j之间路径的遍历次数的多少就是信息素的浓度。更新法则如下：
飞行器从起点A飞到终点B得到一条航迹，通过公式τij(t+1)＝(1-ρ)τij(t)+Δτij(t)计算第t轮飞行器到达终点时，更新信息素浓度后的集合J中个可行点之间路径的信息素浓度τij(t+1)，式中ρ为信息素更新系数，τij(t)为飞行器在第t-1轮遍历集合J中所含可行点之间路径的信息素浓度，该信息素浓度表示飞行器成功到达终点时此轮遍历路径上节点i和节点j之间路径的遍历次数，Δτij(t)为信息素增量，
Figure BDA0003982194980000115
m为飞行器的总个数，
Figure BDA0003982194980000121
Lk(t)为经过t轮信息素更新后，各节点间距离之和，nk(t)为经过t轮信息素更新后，飞行器经过的校正点的数量，将信息素浓度τij(t+1)最高的航迹作为最优航迹。
(3)设计启发式函数，启发式函数Hij(t)表示在t时刻，飞行器从节点i飞向终点B的期望度，其如附图1所示。
启发式函数第一项选择最短路径，由节点i到终点B的最短路径为LiB，当经过节点j时最短路径为LiB+Lij，以
Figure BDA0003982194980000122
作为启发函数的第一项。启发函数的第二项引入校正点数量，从节点i到终点B出发，校正点数用
Figure BDA0003982194980000123
近似，从节点i到节点j再到终点B，所需要的校正点数用
Figure BDA0003982194980000124
近似，以
Figure BDA0003982194980000125
作为启发函数的第二项，得到启发式函数
Figure BDA0003982194980000126
引入威胁代价函数，消除无法到达终点的情况，令j≠B，从节点i到节点j后，设在节点j的可行点个数为nj，可行点阈值为f，威胁代价函数表示为：
Figure BDA0003982194980000127
当f＝1时，可表示为：
Figure BDA0003982194980000131
最终得到引述威胁代价函数后的启发式函数为：
Figure BDA0003982194980000132
其中min(nj,1)的取值为0或1。
(4)计算转移概率：
具体的，通过公式
Figure BDA0003982194980000133
计算得到第q个飞行器经过t轮遍历后从节点i到节点j的概率
Figure BDA0003982194980000134
实施例3：
步骤c)包括如下步骤：
用Lij来表示从节点节点i到节点j飞行器的轨迹长度，从节点i到节点j的轨迹不再是一条直线，引入CLC路径规划模式来计算和优化Lij。这里C代表圆，L代表直线，并对飞行器在到达点的前进方向没有要求。
飞行器在起点i的方向固定，到达节点j的方向任意，如附图2所示和附图3所示，飞行器到达节点i时的前进方向为向量v1的方向，将飞行器经过节点i且方向为v1的直线固定，利用节点j的坐标在三维空间中确定一平面ijv1，以节点i为切点，以v1为切线，在平面ijv1内做靠近节点i一侧的半径为飞行器转弯半径R的圆o，以节点j向圆o做切线，切点为q，得到飞行器最短路径为i→q→j，节点i与节点j之间的直线与前进方向v1之间的夹角为γ；
当
Figure BDA0003982194980000135
时，由余弦定理，通过公式
Figure BDA0003982194980000136
计算得到圆o的圆心到节点j的距离Loj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ2，
Figure BDA0003982194980000141
通过公式
Figure BDA0003982194980000142
计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ1，通过公式
Figure BDA0003982194980000143
计算得到飞行器的最小路径。
c-3)当
Figure BDA0003982194980000144
时，通过公式
Figure BDA0003982194980000145
计算得到圆o的圆心到节点j的距离Loj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ2，
Figure BDA0003982194980000146
通过公式
Figure BDA0003982194980000147
计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ1，通过公式
Figure BDA0003982194980000148
计算得到飞行器的最小路径；
下面要确定向量v2的方向：
具体的，v1＝(a1,b1,c1)，a1为向量v1在三维坐标系中X轴坐标，b1为向量v1在三维坐标系中Y轴坐标，c1为向量v1在三维坐标系中Z轴坐标，节点j到圆o的切线方向为向量v2的方向，v2＝(a2,b2,c2)，a2为向量v2在三维坐标系中X轴坐标，b2为向量v2在三维坐标系中Y轴坐标，c2为向量v2在三维坐标系中Z轴坐标，节点i到节点j的直线方向为向量vij的方向，vij＝(aij,bij,cij)，aij为向量vij在三维坐标系中X轴坐标，bij为向量vij在三维坐标系中Y轴坐标，cij为向量vij在三维坐标系中Z轴坐标，向量v1、向量v2、向量vij共面，向量v2可由二者线性表示，且有行列式
Figure BDA0003982194980000151
通过公式
Figure BDA0003982194980000152
计算得到向量v2；
c-5)计算得到节点i与节点j之间的距离Lij后通过公式
Figure BDA0003982194980000153
建立航迹模型
Figure BDA0003982194980000154
航迹模型
Figure BDA0003982194980000155
满足约束条件为：
Figure BDA0003982194980000156
通过对航迹模型
Figure BDA0003982194980000157
的求解，即可实现飞行器在已知转弯半径(即飞行器最小转弯半径为R)，从一个点i(起点或校正点)飞向另一个点j(校正点或终点)的最优航迹规划功能，并且点i到点j为最短路径。。
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种航迹快速规划精准控制飞行器的方法，其特征在于，包括如下步骤：
a)依照获取的各类地理定位信息使飞行器从A地到B地进行航迹规划；
b)计算得到航迹模型
Figure FDA0003982194970000011
得到最优航迹；
c)计算得到航迹模型
Figure FDA0003982194970000012
得到飞行器转弯受结构和控制系统的限制下的航迹规划。
2.根据权利要求1所述的航迹快速规划精准控制飞行器的方法，其特征在于，步骤a)包括如下步骤：
a-1)飞行器从A地到B地飞行时，飞行器飞行过程中实时定位约束为：εh＜θ且εv＜θ，其中εh为水平误差，εv为垂直误差；
a-2)如果飞行器从A地到B地的路径为直线，则无需校正，如果飞行器从A地到B地的路径不为直线，则飞行器采取直线行驶到最近一个校正点的方式校正飞行器误差，飞行器从从A地到B地的路径由多个节点连成的线段组成，将飞行器的路径简化为一个有向图，每一个校正点均为有向图中的一个点，ωij为有向图中是否存在一条由点i到点j的线路，当ωij＝0时表示路径规划中飞行器不可由点i飞向点j，当ωij＝1时表示路径规划中飞行器可由点i飞向点j；
a-3)飞行器从节点i到节点j时向量ij与单位向量iB的夹角θ满足
Figure FDA0003982194970000013
飞行器满足约束条件为：
Figure FDA0003982194970000014
其中
Figure FDA0003982194970000015
为飞行器在节点i处的水平误差，
Figure FDA0003982194970000016
为飞行器在节点i处的垂直误差，Lij为节点i与节点j之间的距离，
Figure FDA0003982194970000017
Xi为节点i在三维坐标系中X轴坐标，Xj为节点j在三维坐标系中X轴坐标，Yi为节点i在三维坐标系中Y轴坐标，Yj为节点j在三维坐标系中Y轴坐标，Zi为节点i在三维坐标系中Z轴坐标，Zj为节点j在三维坐标系中Z轴坐标，δ为飞行器产生系统误差的比例系数，α1为飞行中垂直方向误差的上限值，α2为飞行中垂直方向误差的下限值，β1为飞行中水平方向误差的上限值，β2为飞行中水平方向误差的下限值；
a-4)对有向图上的任意一节点i，在
Figure FDA0003982194970000021
且
Figure FDA0003982194970000022
时取到所有最大可能的点，所有最大可能的点构成最大可能到达点集合J，集合J满足约束条件为：
Figure FDA0003982194970000023
3.根据权利要求2所述的航迹快速规划精准控制飞行器的方法，其特征在于，步骤b)包括如下步骤：
b-1)通过公式
Figure FDA0003982194970000024
建立航迹模型
Figure FDA0003982194970000025
其中Lmin为飞行器从A地到B地的最短路径，V为权重系数，Ω为飞行器航迹，
Figure FDA0003982194970000026
n为经过的校正点的数量，nmin为需要校正点的最少个数，
Figure FDA0003982194970000027
d为飞行器由某一点到任意一个校正点不经过校正可飞行的最大距离，
Figure FDA0003982194970000028
N为所有节点的总数，
Figure FDA0003982194970000029
dij为飞行器由节点i到节点j在不经过校正点可飞行的最大距离，航迹模型
Figure FDA00039821949700000210
满足约束条件为：
Figure FDA0003982194970000031
Figure FDA0003982194970000032
为飞行器在节点j处的水平误差，
Figure FDA0003982194970000033
为飞行器在节点j处的垂直误差，
Figure FDA0003982194970000034
为飞行器在节点i到节点j方向上的垂直误差，
Figure FDA0003982194970000035
为飞行器在节点i到节点j方向上的水平误差；
b-2)当飞行器从校正点i到达下一个校正点j时通过
Figure FDA0003982194970000036
确定集合J，在距终点B点从0到
Figure FDA0003982194970000037
的范围内，
Figure FDA0003982194970000038
时，集合J中有一个点j′，LiB校正点i与点B之间的距离，
Figure FDA0003982194970000039
XB为点B在三维坐标系中X轴坐标，YB为点B在三维坐标系中Y轴坐标，ZB为点B在三维坐标系中Z轴坐标，点j′为使{Lij+LiB}最小的点；
b-3)飞行器从起点A飞到终点B得到一条航迹，通过公式τij(t+1)＝(1-ρ)τij(t)+Δτij(t)计算第t轮飞行器到达终点时，更新信息素浓度后的集合J中个可行点之间路径的信息素浓度τij(t+1)，式中ρ为信息素更新系数，τij(t)为飞行器在第t-1轮遍历集合J中所含可行点之间路径的信息素浓度，该信息素浓度表示飞行器成功到达终点时此轮遍历路径上节点i和节点j之间路径的遍历次数，Δτij(t)为信息素增量，
Figure FDA00039821949700000310
m为飞行器的总个数，
Figure FDA00039821949700000311
Lk(t)为经过t轮信息素更新后，各节点间距离之和，nk(t)为经过t轮信息素更新后，飞行器经过的校正点的数量，将信息素浓度τij(t+1)最高的航迹作为最优航迹；
b-4)通过公式
Figure FDA0003982194970000041
计算得到启发式函数Hij(t)，式中LjB校正点j与点B之间的距离，
Figure FDA0003982194970000042
Xj为校正点j在三维坐标系中X轴坐标，Yj为校正点j在三维坐标系中Y轴坐标，Zj为校正点j在三维坐标系中Z轴坐标，
Figure FDA0003982194970000043
为威胁代价函数，式中f为可行点阈值，nj为飞行器从节点i到节点j后，设在节点j的可行点个数，
Figure FDA0003982194970000044
b-5)通过公式
Figure FDA0003982194970000045
计算得到第q个飞行器经过t轮遍历后从节点i到节点j的概率
Figure FDA0003982194970000046
4.根据权利要求3所述的航迹快速规划精准控制飞行器的方法，其特征在于，步骤c)包括如下步骤：
c-1)飞行器到达节点i时的前进方向为向量v1的方向，将飞行器经过节点i且方向为v1的直线固定，利用节点j的坐标在三维空间中确定一平面ijv1，以节点i为切点，以v1为切线，在平面ijv1内做靠近节点i一侧的半径为飞行器转弯半径R的圆o，以节点j向圆o做切线，切点为q，得到飞行器最短路径为i→q→j，节点i与节点j之间的直线与前进方向v1之间的夹角为γ；
c-2)当
Figure FDA0003982194970000051
时，通过公式
Figure FDA0003982194970000052
计算得到圆o的圆心到节点j的距离Loj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ2，
Figure FDA0003982194970000053
通过公式
Figure FDA0003982194970000054
计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ1，通过公式
Figure FDA0003982194970000055
计算得到飞行器的最小路径；
c-3)当
Figure FDA0003982194970000056
时，通过公式
Figure FDA0003982194970000057
计算得到圆o的圆心到节点j的距离Loj，节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点j的直线之间的夹角为γ2，
Figure FDA0003982194970000058
通过公式
Figure FDA0003982194970000059
计算得到节点j到圆o的切线的切点到圆o的圆心的直线与圆o的圆心到节点i的直线之间的转弯角γ1，通过公式
Figure FDA00039821949700000510
计算得到飞行器的最小路径；
c-4)v1＝(a1,b1,c1)，a1为向量v1在三维坐标系中X轴坐标，b1为向量v1在三维坐标系中Y轴坐标，c1为向量v1在三维坐标系中Z轴坐标，节点j到圆o的切线方向为向量v2的方向，v2＝(a2,b2,c2)，a2为向量v2在三维坐标系中X轴坐标，b2为向量v2在三维坐标系中Y轴坐标，c2为向量v2在三维坐标系中Z轴坐标，节点i到节点j的直线方向为向量vij的方向，vij＝(aij,bij,cij)，aij为向量vij在三维坐标系中X轴坐标，bij为向量vij在三维坐标系中Y轴坐标，cij为向量vij在三维坐标系中Z轴坐标，向量v1、向量v2、向量vij共面，行列式
Figure FDA0003982194970000061
通过公式
Figure FDA0003982194970000062
计算得到向量v2；
c-5)计算得到节点i与节点j之间的距离Lij后通过公式
Figure FDA0003982194970000063
建立航迹模型
Figure FDA0003982194970000064
航迹模型
Figure FDA0003982194970000065
满足约束条件为：
Figure FDA0003982194970000066
通过对航迹模型
Figure FDA0003982194970000067
的求解，得到飞行器在已转转弯半径R从节点i到节点j的最短路径。
CN202211570165.3A 2022-12-06 2022-12-06 一种航迹快速规划精准控制飞行器的方法 Pending CN115933736A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211570165.3A CN115933736A (zh) 2022-12-06 2022-12-06 一种航迹快速规划精准控制飞行器的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211570165.3A CN115933736A (zh) 2022-12-06 2022-12-06 一种航迹快速规划精准控制飞行器的方法

Publications (1)

Publication Number Publication Date
CN115933736A true CN115933736A (zh) 2023-04-07

Family

ID=86653593

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211570165.3A Pending CN115933736A (zh) 2022-12-06 2022-12-06 一种航迹快速规划精准控制飞行器的方法

Country Status (1)

Country Link
CN (1) CN115933736A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116534278A (zh) * 2023-07-07 2023-08-04 四川腾盾科技有限公司 一种低速无人机验证最小盘旋半径指标的试飞规划方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116534278A (zh) * 2023-07-07 2023-08-04 四川腾盾科技有限公司 一种低速无人机验证最小盘旋半径指标的试飞规划方法
CN116534278B (zh) * 2023-07-07 2023-11-07 四川腾盾科技有限公司 一种低速无人机验证最小盘旋半径指标的试飞规划方法

Similar Documents

Publication Publication Date Title
CN110715664B (zh) 多约束条件下智能无人飞行器航迹快速规划方法
CN111006693B (zh) 智能飞行器航迹规划系统及其方法
CN111595343B (zh) 一种基于定位误差校正的无人机航迹规划方法
CN111580556B (zh) 一种时空约束下多无人机协同路径规划与制导方法
CN111650958B (zh) 一种固定翼无人机起飞段切入航路点的在线路径规划方法
US7228227B2 (en) Bezier curve flightpath guidance using moving waypoints
CN111207752A (zh) 一种基于动态切点调整的无人机航迹规划方法
CN103092211B (zh) 一种基于无线电和激光引导的无人机应急着陆方法
CN103676944A (zh) 基于Dubins路径和稀疏A*搜索的无人机航迹规划方法
CN106094877B (zh) 一种无人机着陆导航系统及控制方法
CN107943049A (zh) 一种无人车控制方法及无人割草车
CN112729308B (zh) 多约束条件下无人机快速航迹规划方法
CN115933736A (zh) 一种航迹快速规划精准控制飞行器的方法
CN105892487B (zh) 一种无人机8字形航迹控制方法
CN103700286A (zh) 一种舰载无人机自动着舰引导方法
CN110308740A (zh) 一种面向移动目标追踪的无人机群动态任务分配方法
CN110609556A (zh) 一种基于los导航法的多无人艇协同控制方法
CN111897362B (zh) 一种复杂环境下的翼伞组合式航迹规划方法
CN113126644B (zh) 基于自适应视线法的无人机三维航迹跟踪方法
CN111273686A (zh) 一种三维环境下多无人机同时到达指定地的路径规划方法
CN114003059B (zh) 运动学约束条件下基于深度强化学习的uav路径规划方法
CN110794874B (zh) 一种定位误差约束下的飞行器航迹快速规划方法
CN109991993A (zh) 基于rtk差分定位的天地双飞控系统及飞控
WO2022099803A1 (zh) 返航控制方法、装置、电子设备和存储介质
CN110488875A (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