一种具有末端约束的姿态机动路径规划方法与流程

本发明涉及一种具有末端约束的姿态机动路径规划方法，属于卫星姿态(指向)机动路径规划技术领域。

背景技术：

一种机动角度不确定且存在末端约束条件的姿态机动/指向路径规划问题，包括如下三种类型：

(1)在惯性定向模式中，姿控系统需对整星进行惯性定向(即，指定某一个时刻卫星光轴指向某一天区进行载荷定标成像)。由于从当前卫星姿态机动到目标姿态(载荷开机时刻的姿态)对应的欧拉转角与卫星所处的轨道位置相关，为了兼顾开机时刻并把开机前的等待时间最小化，以降低由于非正常对地姿态产生的外扰角动量积累，必须动态确定机动起始时刻并计算该时刻对应的欧拉转角。此问题可归结为机动角度不确定而末端角速度为零的机动路径规划问题；

(2)在陀螺标定模式中，姿控系统相对惯性空间完成三轴姿态机动后，整星相对于轨道坐标系主要在俯仰轴上存在一个较大的误差角，并在消除此误差角后建立轨道角速度，恢复正常对地姿态。由于轨道系相对惯性空间存在一个轨道角速度，因而机动过程中误差角会不断扩大。此问题可归结为机动角度不确定而末端角速度不为零的机动路径规划问题；

(3)在卫星进入地面站测控弧段时，数传天线必须尽快对地面站进行定向并下传载荷数据。在捕获过程中，数传天线两轴机动转角与卫星在轨道上的实时位置相关，同时在完成对地面站捕获后须维持一个时变角速度以实现对地面站的定向跟踪。此问题可归结为机动角度不确定而末端角速度为时变的机动路径规划问题。

综上所述，以前的路径方法只能解决机动角度确定而且末端角度是零这种情况，也就是rest-to-rest的情况。本发明的目的在于解决一种具有末端角速度约束的姿态路径规划(rest-to-move)问题，这种角速度约束可以是零、非零常值、非零时变值三种情况，而且要机动多大的角度事先是不确定的。

技术实现要素：

本发明的技术解决问题是：克服现有技术的不足，提出了一种机动角度不确定且存在末端角速度约束的姿态机动/指向路径动态规划方法，采用预估-校正思想，通过迭代计算多次预估并校正机动角度，确保机动结束后满足姿态角速度约束。同时由于充分利用了系统机动最大角加速度和角速度并考虑了执行机构的响应特性，因而本发明保证了机动/指向全过程的快速性和平稳性要求。

本发明的技术解决方案是：

提供一种具有末端约束的姿态机动路径规划方法，步骤如下：

(1)建立卫星观测地面目标在时刻t的状态约束，并根据状态约束计算获得机动结束时姿态角的值χ(tend)和姿态角速度ω(tend)；

(2)根据执行机构的输出能力，设定最大角速度ωmax和最大角加速度amax，按机动过程相对时刻tm时间对机动过程进行规划，规划后的角加速度如下为ar＝ar(tm)，其中包含ar＝0时的滑行时间tacc2；

(3)根据步骤(2)规划的角加速度ar，获得各时刻规划角速度ωr＝ωr(tm)和规划姿态角χr＝χr(tm)，并求出规划结束tend时刻的姿态角χr(tend)；

(4)计算姿态角χr(tend)和实际值χ(tend)的偏差δχ，确定滑行时间tacc2：

tacc2＝δχ/ωmax

(5)判断δχ是否满足精度要求，如果满足则停止迭代，将tacc2作为最终获得的滑行时间；否则返回步骤(2)更新tacc2，重新规划姿态机动路径。

优选的，地面目标在时刻t的状态约束为：

χ＝χ(t)

ω＝ω(t)

其中χ为姿态角，ω为姿态角速度。

优选的，机动过程相对时刻tm＝t–t0，t0为机动起始时刻。

优选的，δχ的精度要求为δχ<1/10000。

优选的，步骤(2)中对机动过程进行规划，采用加加速度为正弦波、加速度为正弦波或加速度为三角波轨迹规划方法。

优选的，地面目标在时刻t的状态约束为：

u0为卫星轨道初始位置，ω0为轨道角速度，卫星在各轨道幅角u＝u0+ω0t，δu、δr分别为卫星相对地面站的角距、距离，β为卫星天线对地面站的俯仰角，ωβ为俯仰角速度；re为地球平均半径，rs为卫星轨道半径，δg为地面站地理纬度，μ为地面站相对卫星的仰角。

优选的，设定最大角加速度为amax，tm＝t–t0为路径规划相对时间，t0为机动起始时刻，机动路径分为7个加加速阶段，加速度ar＝ar(tm)如下述公式所示：

①加加速段0≤tm≤tsinacc，

ar＝amaxsin(2πftm)

②匀加速段tsinacc<tm≤tsinacc+tconacc1

ar＝amax

③减加速段tsinacc+tconacc1<tm≤tacc1

ar＝amax–amaxsin[2πf(tm–tsinacc–tconacc1)]

④滑行段tacc1<tm≤tacc1+tacc2

ar＝0

⑤反向加加速段tacc1+tacc2<tm≤tacc1+tacc2+tsinacc

ar＝–amaxsin[2πf(tm–tacc1–tacc2)]

⑥反向匀加速段tacc1+tacc2+tsinacc<tm≤tacc1+tacc2+tsinacc+tconacc3

ar＝–amax

⑦反向减加速段tacc1+tacc2+tsinacc+tconacc3<tm≤tacc1+tacc2+tacc3

ar＝–amax+amaxsin[2πf(tm–tacc1–tacc2–tsinacc–tconacc3)]

其中tsinacc为加加速时间，由执行机构的响应特性确定；f为加速度或加加速度的频率；tconacc1为加速段的匀加速时间，tconacc1＝ωmax/amax–tsinacc，式中ωmax为最大规划角速度；tacc1为总的加速时间；tconacc3为减速段的匀减速时间，tconacc3＝(ωmax–ωtar)/amax–tsinacc，式中ωtar＝ωβ(t0+tacc1+tacc2+tacc3)为目标角速度；加速段时长tacc1＝2tsinacc+tconacc1和减速段时长tacc3＝2tsinacc+tconacc3；滑行段时长tacc2为待求量，初值设为0。

优选的，设初始条件ωr(0)≠0、χr(0)≠0，沿机动时间tm∈[0,tacc1+tacc2+tacc3]的积分结果如下：

①0≤tm≤tsinacc

②tsinacc<tm≤tsinacc+tconacc1

③tsinacc+tconacc1<tm≤tacc1

④tacc1<tm≤tacc1+tacc2

⑤tacc1+tacc2<tm≤tacc1+tacc2+tsinacc

⑥tacc1+tacc2+tsinacc<tm≤tacc1+tacc2+tsinacc+tconacc3

⑦tacc1+tacc2+tsinacc+tconacc3<tm≤tacc1+tacc2+tacc3

求得机动完成后的姿态角χr(t0+tacc1+tacc2+tacc3)和机动结束时刻t＝tend＝t0+tacc1+tacc2+tacc3。

优选的，δχ＝χr(t0+tacc1+tacc2+tacc3)–β(t0+tacc1+tacc2+tacc3)。

优选的，如果加加速度为正弦波，则f＝0.25/tsinacc为正弦波的频率，如果加速度为三角波，f为三角波的频率，如果加速度为正弦波，f为正弦波的频率。

本发明与现有技术相比具有如下优点：

(1)本发明通过有限次迭代快速确定保证末端状态的规划结果，相比考虑所有工况而设定最大机动时间的传统方法，可大幅提高机动效率。

(2)本发明的方法实现了对一类末端角速度时变(rest-to-move)的姿态机动问题的全过程轨迹规划，充分利用了执行机构的输出能力，克服了仅通过角速度和角加速度等限幅措施进行短期规划带来的冲击影响，可大幅提高指向控制平稳性和快速性。

(3)本发明的方法针对执行机构响应特性设计加速及加加速过程，一方面考虑了驱动机构的输出能力，另一方面又照顾到扰动补偿机构(如动量轮)的响应特性，更有利于实现对天线等运动附件作大角度指向机动时进行精确的前馈力矩补偿。

(4)本发明采用“预估-校正”的机动规划迭代方法，从而实现对目标状态的动态规划。相比传统静态规划方法，采用本发明的方法可实现对从任意初始状态到具有动态末端约束的姿态(指向)的全路径规划，并且充分利用姿态或指向执行机构的输出能力，满足快速性和平滑性的要求。

附图说明

图1为姿态机动路径规划方法流程图；

图2为卫星数传天线俯仰轴运动学简化模型；

图3(a)为卫星天线对地面站的俯仰角曲线，图3(b)为卫星天线对地面站的角速度曲线；

图4为地面站对卫星的仰角曲线；

图5为轨迹规划的时间点示意图；

图6为天线俯仰角指向、角速度、角加速度路径规划图；

图7为天线俯仰角、角速度、角加速度指向局部路径规划图；

图8为天线指向误差及迭代结果图。

具体实施方式

本发明提供一种具有末端约束的姿态机动路径规划方法，结合图1该方法的步骤包括：

(1)建立状态约束

建立卫星观测地面目标姿态在时刻t的状态约束：

χ＝χ(t)

ω＝ω(t)

其中χ为姿态(指向)角，ω为姿态(指向)角速度。

并根据状态约束计算获得机动结束时姿态角的值χ(tend)。

(2)机动过程规划

根据执行机构的输出能力，设定最大角速度ωmax和最大角加速度amax，t0为机动起始时刻，tm＝t–t0为机动过程相对时刻。选取一种适当的路径规划方法，如加加速度为正弦波、加速度为正弦波、加速度为三角波等轨迹规划方法，按tm时间对机动过程进行规划，规划后的角加速度如下式所示：

ar＝ar(tm)

其中规划过程包含ar＝0时的滑行时间tacc2。

(3)预估机动结束时的角度和时间

根据步骤(2)规划的角加速度ar，对机动相对时间tm进行积分，获得各时刻规划角速度ωr＝ωr(tm)和规划姿态角χr＝χr(tm)，从而求出规划结束时刻tend时的姿态角χr(tend).

(4)对规划结果进行校正

根据步骤(3)得到的预估值χr(tend)和实际值χ(tend)的偏差：

δχ＝χr(tend)-χ(tend)

确定新的滑行时间

tacc2＝δχ/ωmax

(5)迭代

重复上述步骤(2)～(4)，经过若干次“预估-校正”迭代，误差角δχ将趋于零，阈值例如可以设定为1/10000，最终获得滑行时间tacc2的稳定值。

实施例

本发明具体方法如下：

(1)建立状态约束

不考虑轨道进动，卫星数传天线俯仰轴运动学简化模型如图2所示。设u0为卫星轨道初始位置，ω0为轨道角速度，通过解三角形可计算卫星在各轨道幅角u＝u0+ω0t下，卫星相对地面站的角距δu、距离δr，卫星天线对地面站的俯仰角β和俯仰角速度ωβ(忽略偏航和滚控角速度)，以及地面站对卫星的地平仰角μ如下述公式所示：

式中，re＝6378km为地球平均半径，rs为卫星轨道半径，δg为地面站地理纬度。卫星飞行高度为500km，则轨道半径rs＝re+500＝6878km，卫星轨道角速度ω0＝0.0635°/s。设地面站地理纬度δg＝30°n，海拔0km，则天线相对地面站的俯仰角β及其角速度ωβ，以及地面站相对卫星的仰角μ的时间曲线如图3、4所示。同时，计算获得β(tend)。

(2)机动过程规划

根据执行机构的响应特性选取一种适当的路径规划方法，如加加速度为正弦波、加速度为正弦波、加速度为三角波等轨迹规划方法，对机动过程进行规划。下面根据加加速曲线为正弦波的方法进行路径规划，设最大角加速度为amax，tm＝t–t0为路径规划相对时间(t0为机动起始时刻)，则7个加加速阶段的加速度ar＝ar(tm)如下述公式所示：

①加加速段0≤tm≤tsinacc，

ar＝amaxsin(2πftm)

②匀加速段tsinacc<tm≤tsinacc+tconacc1

ar＝amax

③减加速段tsinacc+tconacc1<tm≤tacc1

ar＝amax–amaxsin[2πf(tm–tsinacc–tconacc1)]

④滑行段tacc1<tm≤tacc1+tacc2

ar＝0

⑤反向加加速段tacc1+tacc2<tm≤tacc1+tacc2+tsinacc

ar＝–amaxsin[2πf(tm–tacc1–tacc2)]

⑥反向匀加速段tacc1+tacc2+tsinacc<tm≤tacc1+tacc2+tsinacc+tconacc3

ar＝–amax

⑦反向减加速段tacc1+tacc2+tsinacc+tconacc3<tm≤tacc1+tacc2+tacc3

ar＝–amax+amaxsin[2πf(tm–tacc1–tacc2–tsinacc–tconacc3)]

开始机动之前或机动到位之后ar＝0。其中tsinacc为加加速时间，由执行机构的响应特性确定；如果加加速度为正弦波，f＝0.25/tsinacc为正弦波的频率，如果加速度为三角波，f为三角波的频率，加速度为正弦波，f为正弦波的频率；tconacc1为加速段的匀加速时间，tconacc1＝ωmax/amax–tsinacc，式中ωmax为最大规划角速度；tacc1为总的加速时间；tconacc3为减速段的匀减速时间，tconacc3＝(ωmax–ωtar)/amax–tsinacc，式中ωtar＝ωβ(t0+tacc1+tacc2+tacc3)为目标角速度。上述各时间确定后可以获得加速段时长tacc1＝2tsinacc+tconacc1和减速段时长tacc3＝2tsinacc+tconacc3；滑行段时长tacc2为待求量，初值可设为0。各时间段的定义如图5所示。

(3)预估机动结束时的角度和时间

根据步骤(2)规划的角加速度，通过积分法可以获得各时刻规划角速度ωr＝ωr(tm)和规划姿态角χr＝χr(tm)。设初始条件ωr(0)≠0、χr(0)≠0，沿机动时间tm∈[0,tacc1+tacc2+tacc3]的积分结果如下各公式所示：

①0≤tm≤tsinacc

②tsinacc<tm≤tsinacc+tconacc1

③tsinacc+tconacc1<tm≤tacc1

④tacc1<tm≤tacc1+tacc2

⑤tacc1+tacc2<tm≤tacc1+tacc2+tsinacc

⑥tacc1+tacc2+tsinacc<tm≤tacc1+tacc2+tsinacc+tconacc3

⑦tacc1+tacc2+tsinacc+tconacc3<tm≤tacc1+tacc2+tacc3

上一段结束时的角速度、姿态角作为下一段的初值，根据上述公式可求出机动完成后的姿态角χr(t0+tacc1+tacc2+tacc3)和机动结束时刻t＝tend＝t0+tacc1+tacc2+tacc3。

(4)对规划结果进行校正

根据预估值χr(t0+tacc1+tacc2+tacc3)和实际值β(t0+tacc1+tacc2+tacc3)的偏差进行校正，即

δ＝χr(t0+tacc1+tacc2+tacc3)–β(t0+tacc1+tacc2+tacc3)

从而确定新的滑行时间tacc2＝δ/ωmax，以及机动结束时刻t＝t0+tacc1+tacc2+tacc3，进而转步骤(2)重新规划机动路径。

(5)迭代

重复上述步骤(2)～(4)，经过若干次“预估-校正”迭代，误差角δ将趋于零，最终获得滑行时间tacc2的稳定值。根据上述步骤，设卫星进入测控弧段(μ>0)时天线开始进行指向控制，即起始时刻t0＝598.72s对地面站进行捕获。天线初始为静止状态，按最大角速度ωmax＝2.5°/s，最大角加速度amax＝0.2°/s²，加加速时间tsinacc＝10s对指向目标进行路径规划。参见图8，经过3次预估-校正迭代，滑行时间tacc2收敛到4.6354s，相对误差小于0.0001。由迭代结果获得的规划轨迹，天线经过不到50秒实现对地面站捕获，终端误差角小于0.0001°，误差角速度小于1e-6°/s，天线平稳转入对地面站的跟踪阶段。

由图6可以看出本规划使卫星天线完成了从初始位置出发，到达天线应该指向的地面站位置，这个位置在规划前是不确定的，而且达到这个位置后满足天线指向的角速度约束条件，这样就保证了后面可以平滑地转入对天线的持续跟踪。图7是图6的局部放大，可以看出天线指向规划完成时的角度、角速度、角加速度的末端状态。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。