CN117647933A - 一种面向精度提升的轨迹规划方法 - Google Patents
一种面向精度提升的轨迹规划方法 Download PDFInfo
- Publication number
- CN117647933A CN117647933A CN202410113561.6A CN202410113561A CN117647933A CN 117647933 A CN117647933 A CN 117647933A CN 202410113561 A CN202410113561 A CN 202410113561A CN 117647933 A CN117647933 A CN 117647933A
- Authority
- CN
- China
- Prior art keywords
- model
- error
- flight control
- matrix
- aircraft
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 230000006872 improvement Effects 0.000 title claims abstract description 11
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 claims abstract description 62
- 238000010206 sensitivity analysis Methods 0.000 claims abstract description 11
- 230000006870 function Effects 0.000 claims description 71
- 239000011159 matrix material Substances 0.000 claims description 60
- 230000001133 acceleration Effects 0.000 claims description 23
- 230000008569 process Effects 0.000 claims description 18
- 230000007613 environmental effect Effects 0.000 claims description 12
- 239000013598 vector Substances 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 5
- 238000009434 installation Methods 0.000 claims description 5
- 230000001629 suppression Effects 0.000 claims description 5
- 230000000694 effects Effects 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 4
- 230000003190 augmentative effect Effects 0.000 claims description 3
- 230000004907 flux Effects 0.000 claims description 3
- 238000005457 optimization Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 2
- 238000010276 construction Methods 0.000 claims 1
- 230000008685 targeting Effects 0.000 abstract description 4
- 238000005516 engineering process Methods 0.000 description 6
- 238000011161 development Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 230000008451 emotion Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000000116 mitigating effect Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000035945 sensitivity 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
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Navigation (AREA)
Abstract
本发明公开了一种面向精度提升的轨迹规划方法，包括：构建飞行器动力学模型与惯性导航误差传播模型；联合飞行器动力学模型、惯性导航误差传播模型与路径约束构建第一轨迹规划模型；基于路径嵌入将路径约束转化为端点性能指标，得到第二轨迹规划模型；通过灵敏度分析计算目标函数对飞行控制变量的变分，构建第二轨迹规划模型的子模型；对飞行控制变量进行迭代求解，对飞行器进行飞行控制。本发明应用于轨迹规划领域，通过路径嵌入将路径约束转化为端点性能指标，减小储存量，再通过灵敏度分析计算目标函数对飞行控制变量的变分，实现飞行控制变量进行迭代求解，避免了传统打靶法精度不高的问题。
Description
技术领域
本发明涉及轨迹规划技术领域，具体是一种面向精度提升的轨迹规划方法。
背景技术
惯性导航技术因为其能够独立自主的获得高频率的姿态、速度和位置信息，在现代导航技术中发挥着不可或缺的基础性作用，惯性导航的指示精度是制导精度的主要制约因素。
作为一个积分推算系统，惯性导航系统的指示误差（指示值和真值的偏差）来源于陀螺和加计等惯性仪表的输出误差。传统上，研制和使用部门主要从改进工艺和标定补偿等方面减少惯性仪表的输出误差从而提高惯性导航的指示精度。然而，随着弹载惯性器件的进步，传统方案的发展潜力变得越来越小。另一方面，作为广泛的共识，惯性仪表的输出误差和其输入（即角速度和加速度）的时间历程密切相关，但是利用这一特性进行导航指示精度提升的研究却方兴未艾。
发明内容
针对上述现有技术中的不足，本发明供一种面向精度提升的轨迹规划方法，能够有效地抑制导航误差，从而提升飞行器制导的精度。
为实现上述目的，本发明提供一种面向精度提升的轨迹规划方法，包括如下步骤：
步骤1，构建飞行器动力学模型与惯性导航误差传播模型；
步骤2，联合所述飞行器动力学模型、所述惯性导航误差传播模型与路径约束，以最小化落点误差协方差的2范数为目标函数，构建考虑误差抑制的第一轨迹规划模型；
步骤3，在所述第一轨迹规划模型的基础上，基于路径嵌入将所有的路径约束转化为端点性能指标，得到第二轨迹规划模型；
步骤4，在所述第二轨迹规划模型的基础上，通过灵敏度分析计算目标函数对飞行控制变量的变分，构建所述第二轨迹规划模型的子模型；
步骤5，基于所述第二轨迹规划模型的子模型对飞行器的飞行控制变量进行迭代求解，并根据得到飞行控制变量对飞行器进行飞行控制，即完成飞行器的轨迹规划。
在其中一个实施例，步骤1中，所述惯性导航误差传播模型的构建过程为：
构建带有标度系数正负不对称项和二次项的陀螺误差模型，为：
；
其中，为测量坐标系的输入角速度，/>为陀螺组合件的输出角速度，/>为陀螺组合件的零偏，/>为陀螺组合件的标度系数误差和安装误差构成的矩阵，/>为由于陀螺标度系数正负不对称引起的误差项，/>为由于陀螺标度系数二次非线性变化引起的误差系数，/>为陀螺噪声；
将陀螺误差模型转换为矩阵形式，为：
；
其中，为相关对角化函数构成的输入角速度对角化函数矩阵，/>为陀螺误差项矩阵，/>为陀螺误差；
构建不考虑杆臂效应的加速度计误差模型，为：
；
其中，是测量坐标系上的加速度输入，/>是加速度计组合件的输出，/>为加速度计组合件的零偏，/>为加速度计组合件的标度系数误差和安装误差构成的矩阵，/>为由于加速度计标度系数正负不对称引起的误差项，/>为由于加速度计标度系数二次非线性变化引起的误差系数，/>为加速度噪声；
将加速度计误差模型转换为矩阵形式，为：
；
其中，为相关对角化函数构成的输入加速度对角化函数矩阵，/>为加速度计误差项矩阵，/>为加速度计误差；
联合矩阵形式的陀螺误差模型与加速度计误差模型，得到惯性导航误差传播模型，为：
；
其中，为姿态主旋转矢量的计算值和真值的误差，/>为速度的计算值和真值的误差，/>为地心距的计算值和真值的误差，/>为姿态对陀螺的环境函数矩阵，/>为速度对陀螺的环境函数矩阵，/>为速度对加速度的环境函数矩阵，/>为位置对陀螺的环境函数矩阵，/>为位置对加速度的环境函数矩阵；
记、/>、/>，则最终的惯性导航误差传播模型为：
；
；
其中，为位置环境函数矩阵，/>为惯导误差系数，/>为速度环境函数矩阵，/>为飞行器的状态变量，/>为飞行器的飞行控制变量，/>表示/>为与/>、/>相关的函数。
在其中一个实施例，所述目标函数为：
；
；
其中，表示目标函数，/>为落点误差协方差，/>为惯导误差系数/>的协方差矩阵，/>为矩阵的转置。
在其中一个实施例，步骤2中，所述第一轨迹规划模型为：
；
；
；
；
其中，表示飞行器动力学模型，/>表示所有的路径约束；所述路径约束包括：
热流约束，为：，其中，/>为飞行器的热流，/>为热流系数，/>为大气密度，/>为飞行器的速度，/>为允许的最大热流密度；
动压约束，为：，其中，/>为飞行器的动压，/>为动压最大值；
过载约束，为：，其中，/>为飞行器的过载，/>为允许的最大过载，/>为升力加速度，/>为阻力加速度；
禁飞区约束，为：，其中，/>为圆形禁飞区圆心的经纬度，/>为飞行器的经纬度，/>为禁飞区半径。
在其中一个实施例，步骤3中，所述基于路径嵌入将所有的路径约束转化为端点性能指标的过程为：
采用障碍函数代替路径约束，将第一轨迹规划模型转换障碍函数形式，为：
；
；
；
；
其中，为障碍函数形式下的目标函数，/>为性能指标，为起始时间，/>为终点时间，/>为松弛变量，/>为积分变量；
将拉格朗日形式的性能指标通过微分改写成/>形式，将路径约束处理成微分方程，得到第二轨迹规划模型，为：
；
；
；
；
；
其中，表示松弛的路径约束的负对数。
在其中一个实施例，步骤4中，所述通过灵敏度分析计算目标函数对飞行控制变量的变分，构建所述第二轨迹规划模型的子模型的过程具体为：
获取目标函数对飞行控制变量的变分，为：
；
其中，为目标函数/>关于飞行控制变量/>的变分，/>为/>关于飞行控制变量的变分，/>为飞行控制变量/>的变分，/>为性能指标/>关于飞行控制变量/>的变分；
、/>服从如下变分方程：
；
；
；
其中，为状态变量/>关于飞行控制变量/>的变分，/>、/>、分别为/>、/>、/>对/>的偏导数，/>、、/>分别为/>、/>、/>对/>的偏导数；
变分系统在0时刻均为0，即：
；
；
；
在目标函数对飞行控制变量的变分的基础上，联合第二轨迹规划模型，得到第二轨迹规划模型的子模型为：
；
；
；
；
；
其中，为第二轨迹规划模型的子模型的目标函数，/>、/>为算法参数，/>为拉格朗日乘子，/>为飞行控制变量的控制增量，/>为软信赖域项，为用于使松弛变量/>收敛为0的增广拉格朗日项。
在其中一个实施例，步骤5中，基于所述第二轨迹规划模型的子模型对飞行器的飞行控制变量进行迭代求解的过程为：
步骤5.1，对于给定的飞行控制变量，根据积分变分系统得到的/>、/>；
步骤5.2，求解无约束优化问题，得到控制增量/>与松弛变量/>；
步骤5.3，将飞行控制变量更新为，并将拉格朗日乘子更新为；
步骤5.4，重复步骤5.2至步骤5.3直到松弛变量收敛为0，即可输出最终的飞行控制变量/>。
与现有技术相比，本发明具有如下有益技术效果：
1.本发明在轨迹规划的过程中，先是通过路径嵌入的技术将所有的路径约束转化为端点性能指标，因此在积分变分过程中无需存储中间时刻的变量，从而所需的储存量很小，这使得能够在普通计算机上处理很大规模的最优控制问题；
2.本发明通过灵敏度分析计算目标函数对飞行控制变量的变分，从而实现飞行控制变量进行迭代求解，进而有效地避免了传统打靶法采用数值差分精度不高的问题。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
图1为本发明实施例中面向精度提升的轨迹规划方法的流程图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明，本发明实施例中所有方向性指示（诸如上、下、左、右、前、后……）仅用于解释在某一特定姿态（如附图所示）下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本发明中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
在本发明中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
本实施例公开了一种面向精度提升的轨迹规划方法，飞行器从当前位置出发到达目标位置的轨迹有无数条，由于飞行轨迹确定了飞行器体坐标上的角速度和比力输入，通过精心的选择飞行轨迹可以减少导航误差对惯导器件误差的灵敏度，从而达到提升精度的目的。参考图1，本实施例中面向精度提升的轨迹规划方法主要包括如下步骤：
步骤1，构建飞行器动力学模型与惯性导航误差传播模型；
步骤2，联合所述飞行器动力学模型、所述惯性导航误差传播模型与路径约束，以最小化落点误差协方差的2范数为目标函数，构建考虑误差抑制的第一轨迹规划模型；
步骤3，在所述第一轨迹规划模型的基础上，基于路径嵌入将所有的路径约束转化为端点性能指标，得到第二轨迹规划模型；
步骤4，在所述第二轨迹规划模型的基础上，通过灵敏度分析计算目标函数对飞行控制变量的变分，构建所述第二轨迹规划模型的子模型；
步骤5，基于所述第二轨迹规划模型的子模型对飞行器的飞行控制变量进行迭代求解，并根据得到飞行控制变量对飞行器进行飞行控制，即完成飞行器的轨迹规划。
本实施例中的轨迹规划方法主要针对于滑翔飞行器，考虑到滑翔飞行器具有较大的机动变轨能力，本实施例中的轨迹规划方法主要考虑在助推滑翔飞行器的滑翔段应用基于飞行轨迹的误差抑制。因此在步骤1中所构建的飞行器动力学模型为：
；
；
；
；
；
；
其中，为地心距，/>为飞行器的速度，/>为速度倾角，/>为阻力加速度，/>为重力加速度，/>为升力加速度，/>为偏航角，即飞行速度的在水平面的投影和坐标北向的夹角；为纬度，/>为经度。
进一步地：
；
；
；
其中，为地球引力参数，/>为飞行器的质量，/>为阻力系数，/>为攻角，/>为马赫数，/>为大气密度，/>为气动参考面积。
为了描述方便，在本实施例中用作为飞行器的状态变量，用作为飞行器的飞行控制变量，并将飞行器动力学模型的简记为：
；
其中，表示飞行器动力学模型的简记。
在步骤1的具体实施过程中，惯性导航误差传播模型主要由惯导系统中陀螺误差模型与加速度计误差模型构成。
本实施例中，在陀螺误差模型中考虑带有标度系数正负不对称项和二次项，即陀螺误差模型为：
；
其中，为测量坐标系的输入角速度，/>为陀螺组合件的输出角速度，/>为陀螺噪声；
为陀螺组合件的零偏，表示为：
；
其中，、/>、/>分别为陀螺组合件在/>方向上的零偏；
为陀螺组合件的标度系数误差和安装误差构成的矩阵，表示为：
；
其中，为陀螺比例系数矩阵，/>为陀螺安装矩阵，/>为单位矩阵，为各陀螺的比例系数误差，/>，/>为各陀螺的安装误差，/>为/>，/>为/>，/>为/>，/>为/>，/>为/>，/>为；
为由于陀螺标度系数正负不对称引起的误差项，表示为：
；
其中，为各个陀螺标度系数正负不对称引起的误差项；
为由于陀螺标度系数二次非线性变化引起的误差系数，表示为：
；
其中，为各个陀螺标度系数二次非线性变化引起的误差系数。
在具体实施过程中，陀螺误差模型可表示为矩阵形式，为：
；
为相关对角化函数构成的输入角速度对角化函数矩阵，表示为：
；
相关对角化函数具体为：
；
；
；
其中，为/>在坐标轴上的3个分量，/>为矩阵的转置；
为陀螺误差项矩阵，表示为：
；
分别为矩阵/>的非零元素安装先横后列的顺序压平为向量，具体为：
；
；
；
本实施例中的加速度计误差模型不考虑杆臂效应，即加速度计误差模型为：
；
其中，是测量坐标系上的加速度输入，/>是加速度计组合件的输出，/>为加速度噪声；
为加速度计组合件的零偏，表示为：
；
其中，分别为加速度计组合件在/>方向上的零偏；
为加速度计组合件的标度系数误差和安装误差构成的矩阵，表示为：
；
其中，为加速度计比例系数矩阵，/>为加速度计安装矩阵，/>为各个加速度计比例系数误差，/>为加速度计安装误差，/>为/>，/>为/>，/>为/>；
为由于加速度计标度系数正负不对称引起的误差项，表示为：
；
其中，为各个加速度标度系数正负不对称引起的误差项；
为由于加速度计标度系数二次非线性变化引起的误差系数，表示为：
；
其中，为各个加速度计标度系数二次非线性变化引起的误差系数。
在具体实施过程中，加速度计误差模型可表示为矩阵形式，为：
；
；
其中，为/>去掉2，3，6列得到的矩阵，/>为相关对角化函数构成的输入加速度对角化函数矩阵；
相关对角化函数具体为：
；
；
；
其中，为/>在坐标轴上的3个分量；/>
为加速度计误差项矩阵，表示为：/>；
分别为矩阵/>的非零元素安装先横后列的顺序压平为向量，具体为：
；
；
；
根据陀螺误差模型与加速度计误差模型，可得到：
；
；
其中，表示陀螺误差，/>表示加速度计误差；
于是对应的惯性导航误差传播模型为：
；
其中，为姿态主旋转矢量/>的计算值和真值的误差，/>为速度/>的计算值和真值的误差，/>为地心距/>的计算值和真值的误差，/>为姿态对陀螺的环境函数矩阵，/>为速度对陀螺的环境函数矩阵，/>为速度对加速度的环境函数矩阵，/>为位置对陀螺的环境函数矩阵，/>为位置对加速度的环境函数矩阵。
；
；
；
；
；
其中，为从测量坐标系到惯性系的方向余弦，/>为时间，/>为积分变量，/>为矩阵指数，/>为地球自转角速度。/>
本实施例中为了简洁起见，记、/>、/>，则最终的惯性导航误差传播模型为：
；
；
其中，为位置环境函数矩阵，/>为惯导误差系数，/>为速度环境函数矩阵，表示/>为与/>、/>相关的函数。
考虑为惯导误差系数/>的协方差矩阵，根据概率理论落点误差的协方差/>为：
；
则步骤2中的目标函数为：
；
本实施例中，步骤2中的路径约束包括热流约束、动压约束、过载约束与禁飞区约束，具体地：
热流约束为，其中，/>为飞行器的热流，/>为热流系数，/>为允许的最大热流密度；
动压约束为，其中，/>为飞行器的动压，/>为动压最大值；
过载约束为，其中，/>为飞行器的过载，/>为允许的最大过载；
对于某个以经纬度为中心圆形禁飞区，禁飞区约束为，其中，/>为禁飞区半径。
在具体实施过程中，将所有路径约束记为，则考虑误差抑制的第一轨迹规划模型可表示为：
；
；
；
；
由于考虑误差抑制的轨迹规划模型含有矩阵微分方程，第一轨迹规划模型的求解具有很高的维度，而基于状态离散的配点类方法在这样的问题求解上会直接出现内存瓶颈，为此本实施例选用打靶法，并做了如下改进：
1、通过路径嵌入的技术，将所有的路径约束转化为端点性能指标，减少储存量；
2、通过灵敏度分析精确计算目标函数对飞行控制变量的变分，避免使用数值差分精度不高的问题。
在具体实施过程中，通过路径嵌入的技术，将所有的路径约束转化为端点性能指标具体为：
首先，采用障碍函数代替路径约束，将第一轨迹规划模型转换障碍函数形式，为：
；
；
；
；
其中，为障碍函数形式下的目标函数，/>为性能指标(即惩罚项)，/>为起始时间，/>为终点时间，/>为松弛变量；
然后，将拉格朗日形式的性能指标通过微分改写成梅耶尔(Mayer)形式，这一将路径约束处理成微分方程的做法即为“路径嵌入”，可以节省储存量，即最终的第二轨迹规划模型转换为：
；
；
；
；
；
其中，表示松弛的路径约束的负对数。
步骤4中，通过灵敏度分析计算目标函数对飞行控制变量的变分，构建所述第二轨迹规划模型的子模型的过程具体为：
首先，获取目标函数对飞行控制变量的变分，为：
；
其中，为目标函数/>关于飞行控制变量/>的变分，/>为/>关于飞行控制变量的变分，/>为飞行控制变量/>的变分，/>为性能指标/>关于飞行控制变量/>的变分；
、/>服从如下变分方程：
；
；
；
其中，为状态变量/>关于飞行控制变量/>的变分，/>、/>、分别为/>、/>、/>对/>的偏导数，/>、/>、/>分别为/>、/>、/>对/>的偏导数；
变分系统在0时刻均为0，即：
；
；
；
在目标函数对飞行控制变量的变分的基础上，联合第二轨迹规划模型，得到第二轨迹规划模型的子模型为：
；
；
；
；
；
其中，为第二轨迹规划模型的子模型的目标函数，/>、/>为算法参数，/>为拉格朗日乘子，/>为飞行控制变量的控制增量，/>为软信赖域项，为用于使松弛变量/>收敛为0的增广拉格朗日项，从而替代约束/>。
因此步骤5中，在第二轨迹规划模型的子模型的基础上，对飞行器的飞行控制变量进行迭代求解，实现飞行器的轨迹规划的过程具体为：
步骤5.1，对于给定的飞行控制变量，根据积分变分系统得到的/>、/>；
步骤5.2，求解无约束优化问题，得到控制增量/>与松弛变量/>；
步骤5.3，将飞行控制变量更新为，并将拉格朗日乘子更新为；
步骤5.4，重复步骤5.2至步骤5.3直到松弛变量收敛为0，即可输出最终的飞行控制变量/>，并根据最终的飞行控制变量/>完成飞行器的轨迹规划。
本实施例中面向精度提升的轨迹规划方法在积分过程中无需存储中间时刻的变量，从而所需的储存量很小，这使得能够在普通计算机上处理很大规模的最优控制问题。另外，使用灵敏度分析精确计算变分，避免了传统打靶法采用数值差分精度不高的问题。
以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。
Claims (7)
1.一种面向精度提升的轨迹规划方法，其特征在于，包括如下步骤：
步骤1，构建飞行器动力学模型与惯性导航误差传播模型；
步骤2，联合所述飞行器动力学模型、所述惯性导航误差传播模型与路径约束，以最小化落点误差协方差的2范数为目标函数，构建考虑误差抑制的第一轨迹规划模型；
步骤3，在所述第一轨迹规划模型的基础上，基于路径嵌入将所有的路径约束转化为端点性能指标，得到第二轨迹规划模型；
步骤4，在所述第二轨迹规划模型的基础上，通过灵敏度分析计算目标函数对飞行控制变量的变分，构建所述第二轨迹规划模型的子模型；
步骤5，基于所述第二轨迹规划模型的子模型对飞行器的飞行控制变量进行迭代求解，并根据得到飞行控制变量对飞行器进行飞行控制，即完成飞行器的轨迹规划。
2.根据权利要求1所述的面向精度提升的轨迹规划方法，其特征在于，步骤1中，所述惯性导航误差传播模型的构建过程为：
构建带有标度系数正负不对称项和二次项的陀螺误差模型，为：
；
其中，为测量坐标系的输入角速度，/>为陀螺组合件的输出角速度，/>为陀螺组合件的零偏，/>为陀螺组合件的标度系数误差和安装误差构成的矩阵，/>为由于陀螺标度系数正负不对称引起的误差项，/>为由于陀螺标度系数二次非线性变化引起的误差系数，/>为陀螺噪声；
将陀螺误差模型转换为矩阵形式，为：
；
其中，为相关对角化函数构成的输入角速度对角化函数矩阵，/>为陀螺误差项矩阵，/>为陀螺误差；
构建不考虑杆臂效应的加速度计误差模型，为：
；
其中，是测量坐标系上的加速度输入，/>是加速度计组合件的输出，/>为加速度计组合件的零偏，/>为加速度计组合件的标度系数误差和安装误差构成的矩阵，/>为由于加速度计标度系数正负不对称引起的误差项，/>为由于加速度计标度系数二次非线性变化引起的误差系数，/>为加速度噪声；
将加速度计误差模型转换为矩阵形式，为：
；
其中，为相关对角化函数构成的输入加速度对角化函数矩阵，/>为加速度计误差项矩阵，/>为加速度计误差；
联合矩阵形式的陀螺误差模型与加速度计误差模型，得到惯性导航误差传播模型，为：
；
其中，为姿态主旋转矢量的计算值和真值的误差，/>为速度的计算值和真值的误差，/>为地心距的计算值和真值的误差，/>为姿态对陀螺的环境函数矩阵，/>为速度对陀螺的环境函数矩阵，/>为速度对加速度的环境函数矩阵，/>为位置对陀螺的环境函数矩阵，/>为位置对加速度的环境函数矩阵；
记、/>、/>，则最终的惯性导航误差传播模型为：
；
；
其中，为位置环境函数矩阵，/>为惯导误差系数，/>为速度环境函数矩阵，/>为飞行器的状态变量，/>为飞行器的飞行控制变量，/>表示/>为与/>、/>相关的函数。
3.根据权利要求2所述的面向精度提升的轨迹规划方法，其特征在于，所述目标函数为：
；
；
其中，表示目标函数，/>为落点误差协方差，/>为惯导误差系数/>的协方差矩阵，/>为矩阵的转置。
4.根据权利要求3所述的面向精度提升的轨迹规划方法，其特征在于，步骤2中，所述第一轨迹规划模型为：
；
；
；
；
其中，表示飞行器动力学模型，/>表示所有的路径约束；
所述路径约束包括：
热流约束，为：，其中，/>为飞行器的热流，/>为热流系数，/>为大气密度，/>为飞行器的速度，/>为允许的最大热流密度；
动压约束，为：，其中，/>为飞行器的动压，/>为动压最大值；
过载约束，为：，其中，/>为飞行器的过载，/>为允许的最大过载，/>为升力加速度，/>为阻力加速度；
禁飞区约束，为：，其中，/>为圆形禁飞区圆心的经纬度，/>为飞行器的经纬度，/>为禁飞区半径。
5.根据权利要求4所述的面向精度提升的轨迹规划方法，其特征在于，步骤3中，所述基于路径嵌入将所有的路径约束转化为端点性能指标的过程为：
采用障碍函数代替路径约束，将第一轨迹规划模型转换障碍函数形式，为：
；
；
；
；
其中，为障碍函数形式下的目标函数，/>为性能指标，/>为起始时间，/>为终点时间，/>为松弛变量，/>为积分变量；
将拉格朗日形式的性能指标通过微分改写成Mayer形式，将路径约束处理成微分方程，得到第二轨迹规划模型，为：
；
；
；
；
；
其中，表示松弛的路径约束的负对数。
6.根据权利要求5所述的面向精度提升的轨迹规划方法，其特征在于，步骤4中，所述通过灵敏度分析计算目标函数对飞行控制变量的变分，构建所述第二轨迹规划模型的子模型的过程具体为：
获取目标函数对飞行控制变量的变分，为：
；
其中，为目标函数/>关于飞行控制变量/>的变分，/>为/>关于飞行控制变量/>的变分，/>为飞行控制变量/>的变分，/>为性能指标/>关于飞行控制变量/>的变分；
、/>服从如下变分方程：
；
；
；
其中，为状态变量/>关于飞行控制变量/>的变分，/>、/>、分别为/>、/>、/>对/>的偏导数，/>、、/>分别为/>、/>、/>对/>的偏导数；
变分系统在0时刻均为0，即：
；
；
；
在目标函数对飞行控制变量的变分的基础上，联合第二轨迹规划模型，得到第二轨迹规划模型的子模型为：
；
；
；
；
；
其中，为第二轨迹规划模型的子模型的目标函数，/>、/>为算法参数，/>为拉格朗日乘子，/>为飞行控制变量的控制增量，/>为软信赖域项，/>为用于使松弛变量/>收敛为0的增广拉格朗日项。
7.根据权利要求6所述的面向精度提升的轨迹规划方法，其特征在于，步骤5中，基于所述第二轨迹规划模型的子模型对飞行器的飞行控制变量进行迭代求解的过程为：
步骤5.1，对于给定的飞行控制变量，根据积分变分系统得到的/>、/>；
步骤5.2，求解无约束优化问题，得到控制增量/>与松弛变量/>；
步骤5.3，将飞行控制变量更新为，并将拉格朗日乘子更新为；
步骤5.4，重复步骤5.2至步骤5.3直到松弛变量收敛为0，即可输出最终的飞行控制变量/>。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410113561.6A CN117647933B (zh) | 2024-01-26 | 2024-01-26 | 一种面向精度提升的轨迹规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410113561.6A CN117647933B (zh) | 2024-01-26 | 2024-01-26 | 一种面向精度提升的轨迹规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117647933A true CN117647933A (zh) | 2024-03-05 |
| CN117647933B CN117647933B (zh) | 2024-03-29 |
Family
ID=90049825
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410113561.6A Active CN117647933B (zh) | 2024-01-26 | 2024-01-26 | 一种面向精度提升的轨迹规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117647933B (zh) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104035335A (zh) * | 2014-05-27 | 2014-09-10 | 北京航空航天大学 | 基于高精度纵、横程解析预测方法的平稳滑翔再入制导律 |
| CN109491246A (zh) * | 2018-11-19 | 2019-03-19 | 北京航天自动控制研究所 | 一种基于数值优化算法的自适应救援轨迹规划方法 |
| CN111897214A (zh) * | 2020-06-24 | 2020-11-06 | 哈尔滨工业大学 | 一种基于序列凸优化的高超声速飞行器轨迹规划方法 |
| JP2022073731A (ja) * | 2020-11-02 | 2022-05-17 | 東京計器株式会社 | 追尾制御装置 |
| CN114895563A (zh) * | 2022-05-17 | 2022-08-12 | 重庆大学 | 基于强化学习的新型智能协作配送机器人系统 |
| CN114995125A (zh) * | 2022-05-07 | 2022-09-02 | 北京控制工程研究所 | 一种月面飞跃轨迹生成方法 |
| CN115755598A (zh) * | 2022-10-31 | 2023-03-07 | 北京控制工程研究所 | 一种智能航天器集群分布式模型预测路径规划方法 |
| CN117215197A (zh) * | 2023-10-23 | 2023-12-12 | 南开大学 | 四旋翼飞行器在线轨迹规划方法、系统、电子设备及介质 |
-
2024
- 2024-01-26 CN CN202410113561.6A patent/CN117647933B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104035335A (zh) * | 2014-05-27 | 2014-09-10 | 北京航空航天大学 | 基于高精度纵、横程解析预测方法的平稳滑翔再入制导律 |
| CN109491246A (zh) * | 2018-11-19 | 2019-03-19 | 北京航天自动控制研究所 | 一种基于数值优化算法的自适应救援轨迹规划方法 |
| CN111897214A (zh) * | 2020-06-24 | 2020-11-06 | 哈尔滨工业大学 | 一种基于序列凸优化的高超声速飞行器轨迹规划方法 |
| JP2022073731A (ja) * | 2020-11-02 | 2022-05-17 | 東京計器株式会社 | 追尾制御装置 |
| CN114995125A (zh) * | 2022-05-07 | 2022-09-02 | 北京控制工程研究所 | 一种月面飞跃轨迹生成方法 |
| CN114895563A (zh) * | 2022-05-17 | 2022-08-12 | 重庆大学 | 基于强化学习的新型智能协作配送机器人系统 |
| CN115755598A (zh) * | 2022-10-31 | 2023-03-07 | 北京控制工程研究所 | 一种智能航天器集群分布式模型预测路径规划方法 |
| CN117215197A (zh) * | 2023-10-23 | 2023-12-12 | 南开大学 | 四旋翼飞行器在线轨迹规划方法、系统、电子设备及介质 |
Non-Patent Citations (1)
| Title |
|---|
| 孙小东: "基于参数辨识的滑翔弹道在线规划方法", 工程科技Ⅱ辑, 15 February 2023 (2023-02-15) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117647933B (zh) | 2024-03-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109556632B (zh) | 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法 | |
| Fontanella et al. | MEMS gyros temperature calibration through artificial neural networks | |
| Huang et al. | Kalman-filtering-based in-motion coarse alignment for odometer-aided SINS | |
| CN109596018B (zh) | 基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法 | |
| JP5569681B2 (ja) | 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法 | |
| CN109001787A (zh) | 一种姿态角解算与定位的方法及其融合传感器 | |
| CN108827310A (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
| CN111551175B (zh) | 一种航姿参考系统的互补滤波姿态解算方法 | |
| CN102692225A (zh) | 一种用于低成本小型无人机的姿态航向参考系统 | |
| Youn et al. | Combined quaternion-based error state Kalman filtering and smooth variable structure filtering for robust attitude estimation | |
| CN105203098A (zh) | 基于九轴mems传感器的农业机械全姿态角更新方法 | |
| CN110672092B (zh) | 一种降低固定翼无人机平台磁干扰的航迹生成方法 | |
| CN110015446B (zh) | 一种半解析的火星进入制导方法 | |
| de Celis et al. | Hybridized attitude determination techniques to improve ballistic projectile navigation, guidance and control | |
| CN113670314B (zh) | 基于pi自适应两级卡尔曼滤波的无人机姿态估计方法 | |
| CN109000639B (zh) | 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置 | |
| Long et al. | Orientation estimation algorithm applied to high-spin projectiles | |
| CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
| Whittaker et al. | Inertial navigation employing common frame error representations | |
| CN103954288B (zh) | 一种卫星姿态确定系统精度响应关系确定方法 | |
| Chou et al. | Two‐step optimal filter design for the low‐cost attitude and heading reference systems | |
| Whittaker et al. | Linearized analysis of inertial navigation employing common frame error representations | |
| CN117647933B (zh) | 一种面向精度提升的轨迹规划方法 | |
| Guo et al. | Analysis and design of an attitude calculation algorithm based on elman neural network for SINS | |
| Condomines | Nonlinear Kalman Filter for Multi-Sensor Navigation of Unmanned Aerial Vehicles: Application to Guidance and Navigation of Unmanned Aerial Vehicles Flying in a Complex Environment |
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 |