一种基于机动能力辨识的抗干扰四元数姿态机动路径规划方法与流程

本发明涉及一种基于机动能力辨识的抗干扰四元数姿态机动路径规划方法，属于飞行器姿态控制技术领域。

背景技术：

关于基于四元数的姿态机动路径规划方法，目前多采用加速-匀速-减速姿态机动方法，事先设定机动过程的加速度和角速度，采用bang-cosst-bang(bcb)方案，将航天器姿态经理想“加速-匀速-减速”阶段的路径作为规划路径，bcb路径可使系统快速响应，但控制精度不高，且造成一定的机动能力浪费；已有方法提出基于抛物线型角加速度曲线的三段式机动路径规划算法，考虑了最大角速度和最大角加速度限制，但未考虑初始干扰的影响。

对于目前已有的四元数姿态机动路径规划方法，需要解决的问题有：(1)如何解决干扰造成的偏差过大、超调过大问题；(2)如何评估飞行器的机动能力，使得在不同的飞行时段、故障状态下能够高效率的进行姿态机动；(3)如何保证大角度机动过程的轨迹平滑。

技术实现要素：

本发明解决的技术问题为：提供一种基于机动能力辨识的抗干扰四元数姿态机动路径规划方法，是一种能够克服初始干扰，在线估计机动能力并进行机动路径规划的方法，解决干扰造成的偏差过大、超调过大问题；通过评估飞行器的机动能力，使得在不同的飞行时段、故障状态下能够高效率的进行姿态机动；同时保证大角度机动过程的轨迹平滑。

本发明解决的技术方案为：一种基于机动能力辨识的抗干扰四元数姿态机动路径规划方法，步骤如下：

(1)判断当前飞行器三轴角速度是否满足姿态稳定状态时角速度要求，若满足进行步骤(2)；若不满足保持当前四元数不变；

(2)设飞行器三轴角加速度为i＝x,y,z，设ki为飞行器执行机构的开关系数，b3i为当前轴i的力矩系数，ki表示开关特征量用当前轴i的力矩系数表示初始时刻力矩系数特征量的初值θ(0)，用三轴角加速度表示观测量y(k)；根据开关特征量力矩系数特征量θ(k)的初值θ(0)和观测量y(k)，递推确定力矩系数特征量

(3)根据步骤(2)确定的计算姿态机动过程的最大角加速度

(4)判断任务要求的姿态机动总时间t是否能够完成姿态机动，若能完成，则根据步骤(3)得到的最大角加速度、任务要求的姿态机动总时间t和姿态角度变化最大量θ，计算满足任务要求的最大角速度ωmax；否则，结束路径规划，无法完成任务；

(5)根据当前四元数qgb和目标四元数qgm，计算机动四元数qbm并进行符号归一化处理，再根据符号归一化处理后的qbm的矢量部分最大值|qmax|，计算三轴机动角加速度

(6)根据步骤(3)得到的步骤(4)得到的ωmax和步骤(5)得到的三轴机动角加速度计算加速时间t2、加速角θ2和匀速角δq0；

(7)根据步骤(6)得到的δq0的符号，生成相应角速度插值矩阵γ(ω)；

(8)根据步骤(7)角速度插值矩阵γ(ω)，生成当前程序四元数qcx(n)，完成路径规划。

步骤(1)判断当前飞行器三轴角速度是否满足姿态稳定状态时角速度要求，具体如下：

设姿态稳定状态时角速度为δ，若|ωi|≤δ则判定当前飞行器三轴角速度满足姿态稳定状态时角速度要求，否则判定不满足姿态稳定状态时角速度要求。

步骤(2)根据开关特征量力矩系数特征量θ(k)的初值θ(0)和观测量y(k)，递推确定力矩系数特征量具体如下：

采用如下递推公式计算

递推结果误差不大于0.01°/s。

其中，k＝1,2,3,…为递推的当前步数，k-1表示第k步的前一步；右上标t表示转置，不做特殊说明均为此意。

步骤(3)根据步骤(2)确定的计算姿态机动过程的最大角加速度具体为：

计算公式为：从步骤(2)递推确定的中取。

步骤(4)判断任务要求的姿态机动总时间t是否能够完成姿态机动，若能完成，则根据步骤(3)得到的最大角加速度、任务要求的姿态机动总时间t和姿态角度变化最大量θ，计算满足任务要求的最大角速度ωmax；否则，结束路径规划，具体如下：

设飞行任务要求的三轴姿态角变化量最大为θ，t为任务要求的姿态机动总时间，根据步骤(3)得到的计算

若则无法完成任务；

否则，

步骤(5)根据当前四元数qgb和目标四元数qgm，计算机动四元数qbm并进行符号归一化处理，再根据符号归一化处理后的qbm的矢量部分最大值|qmax|，计算三轴机动角加速度具体如下：

设飞行器三轴角速度为ωi，i＝x,y,z，姿态稳定状态时角速度为δ，当前姿态四元数为qgb＝[qgb0qgb1qgb2qgb3]^t，本体坐标系相对于基准坐标系的目标姿态四元数为qgm＝[qgm0qgm1qgm2qgm3]^t。

计算机动四元数，公式为：其中，qbm＝[qbm0qbm1qbm2qbm3]^t

用公式对计算得到的机动四元数进行符号归一化处理，即将的值赋给qbm，其中||||表示四元数的模，即

其中，为qgb的共轭四元数，为四元数乘法，sign为取正负号。根据公式计算|qmax|＝max{|qbm1|,|qbm2|,|qbm3|}，

步骤(6)根据步骤(3)得到的步骤(4)得到的ωmax和步骤(5)得到的三轴机动角加速度计算加速时间t2、加速角θ2和匀速角δq0，具体如下：

δq0＝arccos(qbm0)-θ2；

其中，

步骤(7)根据步骤(6)得到的δq0的符号，生成相应角速度插值矩阵γ(ω)，具体如下：

设机动起始时刻为tt0，结束时刻tk，t为当前时刻，t取值范围为大于等于tt0且小于等于tk，根据步骤(6)得到的δq0计算γ(ω)。

若δq0<0，

其中，否则，

其中，θy＝2arccosqbm0-2θ2。

步骤(8)根据步骤(7)角速度插值矩阵γ(ω)，生成当前程序四元数qcx(n)，完成路径规划，具体如下：

设δθ＝[δθ1δθ2δθ1]^t

δθ＝cz(γ(ω)，δtt0)·δt

(qcx(n)的初值qcx(0)为姿态机动刚开始时tt0的程序四元数)

其中，δtt0为机动过程已工作时间，δt为计算周期，cz(γ(ω),δtt0)为根据δtt0对γ(ω)线性插值过程。

x、y、z为飞行器本体坐标系的x轴、y轴、z轴，本体坐标系的原点为飞行器质心，x轴正方向为飞行器头部指向的方向，y轴在飞行面内与x轴垂直且指向上方，根据右手定则确定z轴正方向。

设飞行器执行机构的开关系数ki为0、1或-1，取0时表示当前轴(i轴)无力矩，取1时表示当前轴(i轴)有正力矩，取-1时表示当前轴(i轴)有负力矩。

当前轴的力矩系数b3i为正力矩除以当前轴i的惯量,i＝x,y,z。

用当前轴i的力矩系数表示初始时刻力矩系数特征量的初值θ(0)，具体为：力矩系数特征量

用三轴角加速度表示观测量

姿态机动过程为从当前姿态到目标姿态的过程。

飞行器上设有执行机构，执行机构为本体坐标系的x、y、z轴的姿态机动提供力矩。

本发明与现有技术相比的优点在于：

(1)本发明通过在线判断的角速度状态，解决干扰造成的偏差过大、超调过大问题，可有效抑制初始干扰影响；

(2)本发明通过在线辨识飞行器的实际动力学特性，可以更好的充分利用机动能力，实现快速大角度机动，避免机动能力的浪费；

(3)本发明通过角速度插值，进行姿态机动路径的在线规划，其路径平滑，目标计算精确，稳定性好；

(4)本发明通过在线辨识，评估飞行器的机动能力，使得在不同的飞行时段、故障状态下能够高效率的进行姿态机动；

(5)本发明可适用于飞行器空间任意角度的姿态调姿轨迹在线规划需求，具有一定的普适性。

附图说明

图1为本发明的规划方法流程图。

图2的(a)为滚动通道机动能力辨识示意图，(b)俯仰通道机动能力辨识示意图，(c)俯仰姿态角示意图，(d)偏航姿态角示意图，(e)滚动姿态角示意图，(f)滚动姿态角速度示意图，(g)偏航姿态角速度示意图，(h)俯仰姿态角速度示意图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步详细描述。

本发明一种基于机动能力辨识的抗干扰四元数姿态机动路径规划方法，动态调整机动开始时刻，消除干扰；对机动能力进行在线辨识，根据限制条件自主进行最优机动路径规划。可适用于飞行器空间任意角度的姿态调姿轨迹在线规划需求，具有一定的普适性。本发明可有效抑制干扰影响；根据飞行器实际动力学特性在线规划，充分利用机动能力，实现快速大角度机动；规划的姿态机动路径平滑，目标计算精确，稳定性好。且本发明解决干扰造成的偏差过大、超调过大问题；通过评估飞行器的机动能力，使得在不同的飞行时段、故障状态下能够高效率的进行姿态机动；保证大角度机动过程的轨迹平滑。

本发明涉及一种基于机动能力辨识的抗干扰四元数姿态机动路径规划方法，属于飞行器姿态控制技术领域。该方法能够克服初始干扰，在线估计机动能力并进行机动路径规划的方法，解决干扰造成的偏差过大、超调过大问题；通过评估飞行器的机动能力，使得在不同的飞行时段、故障状态下能够高效率的进行姿态机动；同时保证大角度机动过程的轨迹平滑。

本发明一种基于机动能力辨识的抗干扰四元数姿态机动路径规划方法，优选方案步骤如下，如图1所示：

(1)设姿态稳定状态时角速度为δ，若|ωi|≤δ则判定当前飞行器三轴角速度满足姿态稳定状态时角速度要求，否则判定不满足姿态稳定状态时角速度要求。若满足进行步骤(2)；若不满足保持当前四元数不变；

(2)设飞行器三轴角加速度为i＝x,y,z，设ki为飞行器执行机构的开关系数，b3i为当前轴i的力矩系数，ki表示开关特征量用当前轴i的力矩系数表示初始时刻力矩系数特征量的初值θ(0)，用三轴角加速度表示观测量y(k)；根据开关特征量力矩系数特征量θ(k)的初值θ(0)和观测量y(k)，递推确定力矩系数特征量

采用如下递推公式计算

递推结果误差不大于0.01°/s。

其中，k＝1,2,3,…为递推的当前步数，k-1表示第k步的前一步；右上标t表示转置，不做特殊说明均为此意。

(3)根据步骤(2)确定的计算姿态机动过程的最大角加速度计算公式为：从步骤(2)递推确定的中取；

(4)判断任务要求的姿态机动总时间t是否能够完成姿态机动，若则无法完成任务，结束路径规划；若能完成，则根据步骤(3)得到的最大角加速度、任务要求的姿态机动总时间t和姿态角度变化最大量θ，计算满足任务要求的最大角速度

(5)设飞行器三轴角速度为ωi，i＝x,y,z，姿态稳定状态时角速度为δ，当前四元数为qgb＝[qgb0qgb1qgb2qgb3]^t，本体坐标系相对于基准坐标系的目标四元数为qgm＝[qgm0qgm1qgm2qgm3]^t。

根据当前四元数qgb和目标四元数qgm，计算机动四元数qbm，公式为：

之后用公式进行符号归一化处理，再根据公式|qmax|＝max{|qbm1|,|qbm2|,|qbm3|}计算矢量部分最大值|qmax|，最后计算三轴机动角加速度

(6)根据步骤(3)得到的步骤(4)得到的ωmax和步骤(5)得到的三轴机动角加速度计算加速时间t2、加速角θ2和匀速角δq0，具体如下：

δq0＝arccos(qbm0)-θ2；

其中，

(7)根据步骤(6)得到的δq0的符号，生成相应角速度插值矩阵γ(ω)，具体如下：

设机动起始时刻为tt0，结束时刻tk，t为当前时刻，t取值范围为大于等于tt0且小于等于tk，根据步骤(6)得到的δq0计算γ(ω)。

若δq0<0，

其中，否则，

其中，θy＝2arccosqbm0-2θ2；

(8)根据步骤(7)角速度插值矩阵γ(ω)，生成当前程序四元数qcx(n)，完成路径规划，具体如下：

设δθ＝[δθ1δθ2δθ1]^t

δθ＝cz(γ(ω)，δtt0)·δt

(qcx(n)的初值qcx(0)为姿态机动刚开始时tt0的程序四元数)

其中，δtt0为机动过程已工作时间，δt为计算周期，cz(γ(ω),δtt0)为根据δtt0对γ(ω)线性插值过程。

本发明采用四元数作为姿态的表征工具，四元数无歧义点，可适用于飞行器空间任意角度的姿态调姿轨迹在线规划需求，具有一定的普适性；

先通过在线判断角速度状态，在初始角速度干扰较大时，优先保持程序四元数并消除角速度干扰，解决干扰造成的偏差过大、超调过大问题，可有效抑制初始干扰影响；

在飞行器整个飞行过程中，通过递推的方式，在线辨识飞行器的实际动力学特性，可以更好的充分利用机动能力，实现快速大角度机动，避免机动能力的浪费；

通过角速度插值，再积分递推程序四元数的方式，使得规划出的姿态机动路径平滑，目标计算精确，稳定性好。

应用本发明方法，优选的参数设置如下：

飞行器姿态机动任务，俯仰角从0°至160°，滚动角从0°至90°，x、y、z轴的转动惯量分别为3000kg·m²、4500kg·m²、4500kg·m²，执行机构提供的力矩分别为70n·m、35n·m、35n·m，机动任务时间不超过100s，如图2的(a)滚动通道机动能力辨识示意图，(b)俯仰通道机动能力辨识示意图，(c)俯仰姿态角示意图，(d)偏航姿态角示意图，(e)滚动姿态角示意图，(f)滚动姿态角速度示意图，(g)偏航姿态角速度示意图，(h)俯仰姿态角速度示意图所示。

仿真试验结果证明，本发明可有效解决干扰造成的偏差过大、超调过大问题；不同于现有方法采用人为设定机动能力，本方法采用递推在线评估飞行器实际动力学特性，充分利用了飞行器的机动能力，实现更快速的大角度机动；且规划的姿态机动路径平滑，目标计算精确，稳定性好。

本发明通过在线判断的角速度状态，解决干扰造成的偏差过大、超调过大问题，可有效抑制初始干扰影响；通过在线辨识飞行器的实际动力学特性，可以更好的充分利用机动能力，实现快速大角度机动，避免机动能力的浪费；通过角速度插值，进行姿态机动路径的在线规划，其路径平滑，目标计算精确，稳定性好；通过在线辨识，评估飞行器的机动能力，使得在不同的飞行时段、故障状态下能够高效率的进行姿态机动；可适用于飞行器空间任意角度的姿态调姿轨迹在线规划需求，具有一定的普适性。