时变指向约束下姿态参数空间离散化路径规划方法.pdf

第 45 卷第 3 期 2024 年 3 月宇 航 学 报Journal of AstronauticsNo.32024MarchVol.45时变指向约束下姿态参数空间离散化路径规划方法朱哲1，2，徐瑞1，2，李朝玉1，2，朱圣英1，2，龙嘉腾1，2，朱雷3（1.北京理工大学深空探测技术研究所，北京 100081；2.深空自主导航与控制工信部重点实验室（北京理工大学），北京 100081；3.上海航天控制技术研究所，上海 201109）摘要：为满足深空探测器在复杂动态环境约束下的姿态机动任务需求，解决时变指向约束下姿态路径规划求解困难、路径优化性能差的问题，提出了一种基于修正罗德里格斯参数（MRP）和动态路径搜索的姿态参数空间离散化路径规划方法。通过MRP空间的笛卡尔网格划分和非奇异空间构建，实现航天器姿态的参数空间离散化和指向约束表征。考虑指向约束的时变动态特性，在三维动态空间中搜索初始到目标姿态的机动路径，并引入路径节点的时间特性，在约束冲突时对路径进行实时修正，设计非奇异空间时变约束路径搜索算法，生成执行路径节点序列。进一步，对生成的路径节点进行插值拟合，基于逆动力学方法计算角速度和控制力矩，完成姿态机动轨迹规划。仿真结果验证了该方法的有效性，可为深空探测器在时变指向约束下规划出姿态机动执行路径。关键词：航天器姿态规划；姿态参数离散化；时变指向约束；路径搜索算法中图分类号：V448.22+.2 文献标识码：A 文章编号：1000-1328（2024）03-0352-14 DOI：10.3873/j.issn.1000-1328.2024.03.003Parameter Space Discretization Method for Attitude Motion Planning under Time-variant Pointing ConstraintsZHU Zhe1，2，XU Rui1，2，LI Zhaoyu1，2，ZHU Shengying1，2，LONG Jiateng1，2，ZHU Lei3（1.Institute of Deep Space Exploration Technology，Beijing Institute of Technology，Beijing 100081，China；2.Key Laboratory of Autonomous Navigation and Control for Deep Space Exploration（Beijing Institute of Technology），Ministry of Industry and Information Technology，Beijing，100081，China；3.Shanghai Aerospace Control Technology Research Institute，Shanghai 201109，China）Abstract:In order to meet the requirement of attitude maneuvering tasks of the deep space probe under the complex dynamic environment constraints，and solve the problems of difficult attitude path planning and poor path optimization performance under time-varying pointing constraints，an attitude parameter space discretization path planning method based on modified rodrigues parameters（MRP）and dynamic path searching is proposed.Through the Cartesian grid division and non-singular map construction of the MRP space，the discretization of the parameter space of the spacecraft attitude and the characterization of the pointing constraints are realized.Considering the dynamic characteristics of time-varying pointing constraints，the path searching algorithm is used to search the maneuvering path from the initial attitude to the target attitude in the dynamic 3D map，the time characteristics of the path nodes are introduced，and the path is corrected in real time when the constraints conflict.A nonsingular space path searching algorithm under time-variant constraints is designed，and the sequence of path nodes for execution is generated.Furthermore，interpolation path fitting is performed on the path nodes generated by the path search algorithm，the angular velocity and control torque are calculated based on the inverse dynamics method，and the attitude maneuver trajectory planning is completed.The simulation results verify the effectiveness of the method proposed in this paper.The attitude maneuvering for execution of deep space probe under the time-varying orientation constraints can be obtained.收稿日期：2023-09-19；修回日期：2023-12-26基金项目：国家重点研发计划资助（2019YFA0706500）；国家自然科学基金（61976020）；民用航天预研项目（KJSP2020020302）352第 3 期朱哲等：时变指向约束下姿态参数空间离散化路径规划方法Key words:Spacecraft attitude planning；Attitude parameter discretization；Time-variant pointing constraint；Path searching algorithm0引言航天器在姿态机动时，其本体或可动部件的姿态受到指向约束，这类约束缩小了航天器姿态机动的可行域，被称作姿态指向约束。航天器姿态违反姿态指向约束时，会出现强光天体进入相机视场、天线偏离通信目标等问题，航天器的导航定姿、科学观测或通信传输能力将遭到损坏，影响任务正常执行。姿态指向约束包括禁忌指向约束和强制指向约束1。进一步地，随着深空探测任务的发展，航天器在姿态机动任务中可能同时面临多种姿态指向约束。例如在以OSIRIS-REx为代表的小行星采样返回任务中，探测器在对自旋小行星进行附着抵近时，需要同时规避太阳强光干扰并对小行星地貌进行观测2-4。前者是传统静态禁忌指向约束；而后者在快速自旋小行星的条件下，对探测器提出了强制指向约束，且约束方向矢量在日心轨道惯性系下是动态变化的，形成的指向变化角不可忽视，构成了动态时变的强制指向约束。另外，探测器在行星探测任务中，惯性系下部分天体，例如行星的卫星，相对于探测器有较高动态的相对指向变化。当该天体具有高反射率时，例如卡西尼任务中探测器飞掠经过土卫二时，会对光学敏感器（如导航相机、星敏感器）等构成强光干扰5-7，该类天体对探测器造成了时变禁忌指向约束。又例如，在类似罗塞塔号的彗星探测任务中，探测器在彗星抵近观测时受到彗尾和彗星气体喷流对敏感器和载荷视场的影响，需执行一定的姿态机动进行规避8-9。由于彗星相关地质活动具有不确定和动态特性，这类深空环境对探测器载荷指向轴构成了一类动态的禁忌指向约束。这类新型复杂任务场景使得深空探测器面临与以往任务不同的、随时间变化的姿态指向约束，该约束使姿态机动可行域成为一个动态空间，探测器需要在姿态参数的动态空间中满足姿态对准或跟踪的机动需求。而传统姿态路径规划大多只能使用静态空间的路径搜索算法10-12处理静态指向约束。时变指向约束带来的新问题对姿态路径规划方法的应用提出了挑战。针对复杂多约束下航天器的姿态机动规划问题，姿态规划方法需要统一考虑指向约束和动力学约束等多种约束，综合求解得到姿态机动路径（姿态参数序列）和包括角速度和控制力矩序列在内的姿态机动轨迹。目前，求多约束下无目标优化航天器的姿态机动离散可行解主要采用包括势函数法、随机规划算法、空间离散化路径搜索算法在内的启发式约束评价规避算法。其中势函数法适合处理简单指向约束的情况13。随机规划方法能高效适应航天器的各种复杂约束情况，He等14使用RRT-GoalBias算法生成了时变指向约束下的姿态机动路径，并提出姿态随动球面来表示姿态机动和指向约束变化的动态过程。但其随机扩展过程生成的路径具有不确定性，路径不具有全局优化性。空间离散化路径搜索算法因具备较强的几何约束处理能力和较为完备的全局优化性能，在时变指向约束下的姿态规划问题中具有较高研究价值。空间离散化路径搜索算法首先对姿态空间或姿态参数空间进行投影离散化，然后采用图搜索算法求解得到满足指向约束的姿态机动路径。图搜索算法以已知信息的姿态和约束空间为基础，通过构建图搜索寻找到达目标的机动路径。首先对姿态空间或者姿态参数空间进行投影离散网格划分，然后求解得到满足指向约束的姿态机动路径。常见的图搜索算法包括 A*搜索算法，其最初由 Hart等15发表。通过构造启发式函数，A*算法可以获得较好的寻的性能。Stentz16在此基础上提出 D*和Focused D*算法，在处理动态插入障碍的地图路径搜索方面获得了较好效果。近年来图搜索算法被应用在姿态机动路径的规划问题求解上，Kjellberg等17通过二十面体方法将单位天球离散化为描述某个本体固联轴方向的像素图，而后利用A*搜索算法得到满足单轴禁忌约束的姿态机动路径。在此基础上，Kjellberg等18又进一步扩展算法的通用性，将二十面体方法应用于姿态四元数表示方式形成的超球面空间的离散化当中，实现了求解复杂多轴指向约束下的姿态机动路径。Tanygin19采用最小失真变换将完整的三轴姿态表示投影到一个三维空间，并将该投影空间在三维笛卡尔网格上离散化，而后利用图搜索算法得到满足复杂多轴约束的353宇航学报第 45 卷姿态机动路径。进一步地，Tanygin20又将该算法由求解单一目标点问题扩展到求解多个目标点构成的连续体问题。Calaon等21针对航天器敏感器对准导航天体和载荷规避强光天体的需求，将路径搜索算法应用于修正罗德里格斯参数的三维网格中，生成满足指向约束的离散参考路径，进而采用B-样条曲线拟合路径离散点，满足姿态运动学的同时能够优化控制力矩代价。进一步地，Calaon等22将拟合曲线扩展到非均匀有理B样条曲线（NURBS），从而得到整体控制力矩代价更优的姿态轨迹。空间离散化路径搜索算法可以有效地处理多轴复杂指向约束，并通过启发式评价函数最小化从起点到终点的路径上所有节点的总路径代价。但该算法本身适用于几何类型约束，生成姿态路径离散节点，无法处理角速度和控制力矩有界约束以及姿态动力学约束，动力学类型的约束需要在路径拟合和轨迹规划算法中考虑。为了使图搜索算法生成的路径满足动力学约束，引入轨迹规划算法生成含有角速度和控制力矩的完整姿态机动规划序列。以多项式方法23-24为例，采用一定阶数的多项式来表示航天器姿态机动过程中的姿态变化曲线，然后结合逆动力学方法25求解角速度和控制力矩。通过优化多项式系数，实现匹配边界条件。本文提出了一种姿态参数空间离散化路径规划方法，通过姿态参数空间离散化、动态空间路径搜索、逆动力学轨迹规划3部分实现完整的姿态机动规划。首先对基于修正罗德里格斯参数的空间离散化方法进行研究，实现姿态空间离散化的空间构建和指向约束表达。在此基础上，设计时变指向约束下的姿态路径搜索方法，设计非奇异空间搜索代价函数，通过引入节点时间特性处理时变指向约束，实现姿态路径生成。进一步采用多项式路径拟合，用逆动力学法计算姿态机动的角速度和控制力矩轨迹。最后数值仿真展示了不同组合的时变指向约束下，采用空间离散化方法规划的姿态机动路径的效果，验证了时变指向约束下路径规划的可行解。1姿态参数空间离散化对于姿态参数空间离散化方法，需要针对不同的姿态描述方式，即姿态描述参数，选择不同的三维空间离散化方法，并实现姿态机动的指向约束表示。本节对修正罗德里格斯参数（MRP）描述方式进行了参数空间离散化处理，从而构建姿态空间的三维离散网格，结合离散空间的指向约束表达，得到姿态机动的路径规划空间，为空间离散化路径搜索算法设计提供了基础。1.1姿态描述参数航天器姿态采用本体坐标系与惯性坐标系之间的相对转动关系来描述，刚体航天器姿态可以采用四元数和修正罗德里格斯参数进行描述。1）四元数采用单位四元数描述航天器的姿态：q=cos(/2)evsin(/2)=q0qTvT（1）式中：ev表示沿欧拉轴的特征向量；表示欧拉旋转角，0，180。由于 q=1（表示欧几里得范数），四元数的4个参数中只有3个独立，四元数空间事实上是一个四维超球面。2）修正罗德里格斯参数修正罗德里格斯参数（MRP）用于表示SO（3）空间中的刚体旋转姿态，是三维向量表示的姿态参数，可由四元数导出：=qv1+q0=evtan(/4)（2）式中：0，360)相较于四元数空间，对MRP参数空间进行离散化处理好处在于，MRP的3个参数相互独立，因此构成一个无限大的三维空间，R3内任意点均表示一个有效的三轴姿态指向（但一个三轴姿态并不唯一对应于R3内某一个点）。但与此同时，等于360时MRP发生奇异，为了避免该奇异性，在MRP的“无限空间”中划出一定范围的集合，为后续路径搜索提供一个非奇异空间。由于对MRP表示的每个旋转=(ev，)，存在一个对应的影集（Shadow set）=(ev，)，=-2，表示相同的姿态26。对于单个对应的，下文称为节点对应的影节点。对于的分量：i=eitan(/4)=eisin()/21+cos()/2=eisin()/2-1+cos()/2-=-i2，i=1，2，3（3）式（3）成立时，2组MRP参数和取值不同，354第 3 期朱哲等：时变指向约束下姿态参数空间离散化路径规划方法是参数空间中不同的点，但表示相同的姿态，当 1时 1。如果因为指向约束的时变，由过去时刻姿态组成的已执行路径在当前时刻与约束发生冲突，或未来预期执行的路径在当前时刻与约束发生冲突，都不能被证明会影响航天器实时姿态机动的执行，因而无需修正现有路径。因此，在时变指向约束下的姿态路径规划当中，需要将节点放在时间线上，每个节点具有自身唯一对应的时刻，即航天器姿态机动至该状态的时刻。判断当前节点到下一节点的扩展是否与该时刻的指向约束冲突，冲突则调用MODIFY_COST（）修正路径。值得注意的是，这一路径上的动态修正是预先规划的，建立在已知时变函数的动态约束条件上，而非执行过程中的实时修正。根据上述不足，本文对时变指向约束下的路径搜索算法进行改进，提出了非奇异空间时变约束路径搜索算法。2.2非奇异空间时变约束路径搜索算法在 Focused D*算法应用中，路径代价函数是基于MRP空间中节点间的欧几里得距离d(1，2)计算的。由于MRP节点存在影节点，节点之间的距离计算应当选取grid(i)和grid(i)到另一节点的欧氏距离的最小值，即：d(1，2)=mind(1，2)，d(1，2)，d(1，2)（9）如此则路径搜索算法可以在MRP非奇异空间中向各个方向均等地扩展，确保规划出总代价函数最优的全局路径的合理性。前序代价函数h(i)定义如下：h(i)=i=0n-1d(i，i+1)（10）后序代价函数g(i)定义如下：g(i)=d(i，G)（11）当姿态机动欧拉旋转角大于180时，借助MRP影节点的连续路径效果如下。由式（9），在=1的球面表面节点与其影节点的距离d=0。当路径扩展出单位球面，而通过影节点从球面的中心对称一侧进入时，d=0，说明路径是连续的。在时变指向约束条件下，离散化空间和指向约束构成的路径搜索空间需要随节点在初始规划路径上的运动动态更新。由路径搜索算法生成的含N+1个节点q0，q1，qN的姿态机动路径被分为357宇航学报第 45 卷N段，需对每一段路径的边界节点赋予时刻特性。由于姿态总机动时间T未知，对T的估计可以通过假设一个期望的平均机动角速度，结合式（4）、（5）得到：T 4L（12）式中：L=i=1nd(i，i-1)表示MRP空间的姿态机动路径总长度。通过估计的总机动时间和期望平均角速度，可将路径上节点对应时刻t0，t1，tN设为与节点路径长度成比例关系的数值，即：ti-ti-1=d(i-i-1)LT，i=1，N（13）节点在路径上对应时刻的时变指向约束为rI(ti)，k(ti)，i=0，N，k表示第k个指向约束的视场角。将时变指向约束引入路径搜索算法的动态环境更新中，可采取两种方式：1）沿总路径代价最小的最优路径转动过程中算法存储的map空间动态更新，这种方法需要在循环中重复离散化的空间构建，用时较长；2）在节点遇到障碍的判断函数 isCollision（X）中加入对时变指向约束的判断，需要将MRP笛卡尔网格节点坐标转换为姿态参数，进而利用式（7）、（8）对指向约束特征轴方向矢量rgj（j个指向约束）进行判断。采用两种方式的非奇异空间时变约束路径搜索算法结构中如图7所示：1.01.01.00.50.50000.50.51.01.01.0321禁忌约束1禁忌约束2起点终点图6静态约束下借助MRP影节点的连续路径Fig.6Continuous path under static constraints by MRP switching姿态参数空间离散化当前时刻指向约束下三维地图构建当前节点对应时刻 ti是否首次路径搜索否否否否是是是是定义期望平均角速度 时变指向约束下路径搜索算法路径节点与指向约束冲突沿规划路径机动，更新当前节点及对应时间当前节点到达终点第一种时变约束引入方式第二种时变约束引入方式路径节点转换为姿态指向且是否同时满足禁忌和强制指向约束MODIFY_COST()修正最优路径PROCESS_STATE()寻找最优路径生成可执行姿态路径多项式插值拟合连续路径逆动力学求解角速度和控制力矩轨迹完整的姿态规划解图7非奇异空间时变约束路径搜索算法流程图Fig.7Flowchart of nonsingular time-variant path searching algorithm358第 3 期朱哲等：时变指向约束下姿态参数空间离散化路径规划方法3逆动力学方法求解姿态连续轨迹对于路径搜索生成的离散节点序列，需要采用插值拟合曲线将离散路径节点串起来，从而生成一条航天器能够跟踪执行的姿态机动轨迹。本节首先给出四元数表示下的姿态逆运动学和动力学方程。进一步地，由于路径搜索的MRP节点序列可以简单地转换为四元数节点序列，对路径节点序列采用四元数多项式插值进行拟合。生成的四元数时间拟合曲线便可使用逆动力学方法计算角速度和控制力矩曲线，得到完整的姿态机动轨迹。3.1姿态逆运动学和逆动力学刚体航天器本体坐标系下的姿态动力学方程为：J =T-(J)（14）式中：J是惯量矩阵，=x，y，zT是角速度矢量，T是控制力矩。采用单位四元数q=qe0，qTevT描述惯性系下的航天器姿态，设=0，x，y，zT，姿态运动学方程如下：q=12q =120-x-y-zx0z-yy-z0 xzy-x0 qe0qev（15）式中：运算符表示四元数乘法。设q*是q的共轭四元数，逆运动学方程如下25：=2q*q（16）对上式求微分得到：=0，x，y，zT=2q*q+122qI（17）式中：qI=1，0，0，0T。结合式（14）的动力学方程和式（17），可以得到用于计算控制力矩T的逆动力学方程：T=J vec(2q q)+vec(2q q)(J vec(2q q)（18）式中：vec()表示四元数的向量部分。式（16）和（18）说明可以由q(t)推导(t)和T(t)。3.2基于四元数多项式插值的路径拟合采用基于四元数多项式插值的逆动力学方法27，生成满足离散路径点的姿态机动连续轨迹，包括姿态角速度和控制力矩。对于路径搜索算法生成的含有N+1个节点0，1，N的姿态机动路径，使用式（2）的转换关系得到q0，q1，qN的四元数路径。路径分为N段，在每段路径的时间 ti-1，ti内，对四元数4个分量的分别采用三次多项式插值，即分段三次样条插值，满足边界条件q0，q1，qN。插值多项式如下：qi，j(t)=k=03ai，j，k(t-ti-1)kqi，j(t)=k=13kai，j，k(t-ti-1)k-1qi，j(t)=k=23k(k-1)ai，j，k(t-ti-1)k-2，i=1，N，j=0，1，2，3（19）式中：qi，j(t)即四元数的插值函数，下标i表示第i个分段，下标j表示四元数的第j个分量。总共有4N个三次插值多项式，有16N个多项式系数需求解。姿态机动路径节点和边界条件提供了等数量的约束方程。其中边界条件q0，q1，qN和初始及末端角速度为零提供了8N+8个约束方程：qi，j(ti-1)=qi-1，j，qi，j(ti)=qi，jq1，j(t0)=0，qN，j(tN)=0（20）式中：qi，j是qi第j个分量。由分段多项式的分段连续条件，除去总路径的两个端点，中间N-1个节点处角速度和角加速度，即qi，j(t)的一阶和二阶导数相等，这提供了余下的8(N-1)个约束方程：qi，j(ti)=qi+1，j(ti)qi，j(ti)=qi+1，j(ti)，i=1，N-1，j=0，1，2，3（21）进一步地，姿态机动轨迹的四元数需要满足归一化条件，即q(t)=1。归一化插值函数qi，j(t)表示为：qi，j(t)=qi，j(t)k=03q2i，k(t)=qi，j(t)S，i=1，N，j=0，1，2，3（22）qi，j(t)的值S在1左右浮动，因此qi，j(t)的一阶和二阶导表示如下：359宇航学报第 45 卷 qi，j=qi，jS-qi，jSS2S=k=03qi，kqi，kS qi，j=qi，jS-2qi，jS-qi，jSS2+2qi，jS2S3S=k=03(q2i，k+qi，kqi，k)S-k=03(qi，kqi，k)SS2（23）综上所述，通过计算多项式全部系数，可以得到q(t)，q(t)，q(t)的分段数值表达式，角速度和控制力矩T可以由式（16）和（18）计算。特殊地，当离散节点序列类似图8，通过MRP影节点保证路径连续到达目标节点时，难以直接进行插值拟合。可以先将节点序列映射为影节点序列，与原影节点分段相连接，构成连续路径。进而可以采用本节方法插值拟合生成解析轨迹，得到角速度和控制力矩时间曲线。4数值仿真为了验证所提出的姿态参数空间离散化路径规划方法的可行性，本节进行了数值仿真。考虑了深空探测器在外部动态指向约束下，从初始到目标姿态的重新定向姿态机动过程。首先，对多个时变禁忌指向约束下的姿态机动路径规划进行仿真，对比生成路径与传统方法结果的更优性。然后，同时考虑禁忌约束和时变强制约束，给出这一条件下的姿态规划结果。4.1动态禁忌指向约束下姿态规划对姿态路径规划的参数空间离散化方法进行仿真，考虑深空探测器在本体系z轴方向上安装一个光学敏感器，其单位方向矢量用rB表示；需要在姿态机动过程中规避两个明亮天体，在惯性系下的单位方向矢量用rF1，rF2表示；要求rB与rF1，rF2之间的最小夹角为F1，F2；考虑指向约束的时变条件，对rF1，rF2有旋转角速度F1，F2。环绕器转动惯量为J，初始姿态为q0，目标姿态为qf；初始和目标角速度符合0，f符合双零约束。在路径规划和轨迹规划的参数定义方面，空间离散化的精细度定义为N，期望平均角速度为，t是路径拟合时间步长。具体数值如表1所示。仿 真 环 境 为 Matlab R2021b，计 算 机 主 频 3.20 GHz，内存16 G。在时变指向约束下，采用基于MRP空间离散化的姿态路径搜索算法，在表1仿真条件下对问题求解。姿态参数空间离散化方法在MRP空间的规划路径如图8所示，算法在单位天球惯性系上的姿态指向路径如图9所示，该路径是探测器在算法的规划结果不断修正过程中实际执行的节点路径。表1禁忌指向约束仿真条件Table 1Simulation parameters under forbidden constraints变量q0qf0fF1F2rF1rF2F1F1rBJN t值1，0，0，0T0.826 5，-0.091 3，-0.547 9，-0.091 3T0，0，0T0，0，0T10150.340 7，-0.029 8，0.939 7T0.664 5，-0.241 8，0.707 1T0，0，-0.01T rad/s0，0，-0.01T rad/s0，0，1Tdiag(22.010 9，8.923 3，42.832 0)kg m2600.03 rad/s0.01 s1.01.01.00.50.50.50000.50.50.51.01.01.0321起点终点0.10.10000.10.10.40.2禁忌约束2禁忌约束1图8姿态参数空间离散化方法在MRP空间的规划路径Fig.8Path planned by attitude parameter space discretization path planning method in MRP space1.01.01.00.20.40.60.80.50.50000.50.51.01.0zyx终点起点禁忌约束1禁忌约束2图9敏感器光轴在单位天球惯性系上的指向路径（图中所示为初始指向约束）Fig.9Path of sensor axis on unit celestial sphere in inertial frame（initial pointing constraints displayed）360第 3 期朱哲等：时变指向约束下姿态参数空间离散化路径规划方法图8展示了MRP空间中成功规避两个禁忌约束的实际运动节点组成的机动路径。由于禁忌约束的方向矢量随时间在惯性系下摆动变化，图9展示了与图8对应的在单位天球惯性系下，敏感器指向实际执行的机动路径。该实际机动路径与每一次路径搜索算法规划的路径都不同，这是因为每当原先规划路径与该时刻时变指向约束冲突时，算法都对路径进行动态修正，生成从当前点到目标的新路径。规划路径动态修正中的变化展示在图10中（选取了3次修正路径），三角形符号表示当前时刻姿态机动的敏感器指向，即路径上的当前姿态节点；指向约束随时间从图中左侧摆向右侧。从起点到终点的路径经过算法修正的次数实际上等于时变指向约束与当前节点沿先前路径扩展发生冲突的次数。通过多次路径修正，最终确定一条与图9所示相同的、探测器可实际执行规避时变约束的机动路径。图 11给出了 MRP和单位四元数及角速度、控制力矩随时间的变化轨迹。MRP和四元数之间可以相互转换，其时间曲线说明姿态机动过程无退绕。角速度时间曲线显示角速度的模在期望值 =0.03 rad/s附近保持小幅波动。单轴控制力矩最大值不超过1 N m。其中，控制力矩时间曲线在起始和末端的较大幅值对应角速度在起始和末端的加速和减速，从而满足角速度的双零约束。在4050 s时间段控制力矩的快速增加造成了角速度分量的较大变化，角速度发生转向，对应了此时路径对于禁忌约束2的“应急”修正。与文献 14 中时变指向约束下基于改进 RRT的姿态规划方法进行对比，由于RRT方法规划结果具有随机不确定性，取多次重复规划结果，将本文方法与其进行比较。在相同仿真条件下，将上述结果的规划路径长度与RRT方法的40次规划结果进行比较，如图12所示。其中姿态机动路径的长度计算方式如下18：=arccos(2(qm qn)2-1)（24）式（24）计算得到两姿态节点对应姿态qm，qn之间的路径长度（rad）。由此可以计算整条四元数姿态路径的机动“路程长度”，即路径长度。图 12显示，本文的路径搜索算法基于Focused D*算法进行改进，规划路径的评价函数与路径长度相关，因此具有较好的路径长度优化效果，与文献 14 中RRT方法的结果相比，路径长度大大缩短。图13直观展示了天球坐标系下本文方法与随机选取的文献 14 中RRT方法生成的2条路径的对比，RRT方法结果因1.01.01.00.20.40.60.80.50.50000.50.51.01.0zyx终点起点禁忌约束2禁忌约束1图10单位天球惯性系上不同时刻下算法修正的三维指向路径（指向约束随时间在图中从左向右摆动）Fig.103D pointing path on unit sphere in inertial frame at different time（as time-variant pointing constraints swinging from left to right）0.100102030405060700.10.20.3MRPs213|(a)MRP-时间曲线1.000.50102030405060700.5q(b)四元数-时间曲线0.040.0201020304050607000.020.04/(rads1)(c)角速度-时间曲线1.51.00102030405060700.500.51.0力矩/(Nm)(d)控制力矩-时间曲线时间/s时间/s时间/s时间/sMRPs2MRPs1MRPs3q2q1q3q4|q|T2T1T3图11MRP、单位四元数及角速度、控制力矩随时间的变化轨迹Fig.11Trajectory of attitude（MRP and quaternion），rate and control torque over time361宇航学报第 45 卷其随机不确定性，对约束的规避方向可能不同，路径的长度分布差异较大。需要说明的是，图13反映了单位天球上的敏感器指向路径，该路径长度与式（24）计算的四元数路径长度不同：前者只能反映敏感器方向矢量在天球上划过的角度，后者则是探测器三轴姿态在三轴姿态机动中的路径长度度量。图13中两条RRT方法路径由式（24）计算，长度分别为5.085 6和7.140 8，作为对比，本文方法生成路径的长度为1.925 3.4.2动态强制指向约束下姿态规划进一步地，考虑小行星探测器在附着过程中对自旋小行星进行定向观测的时变强制指向约束条件，定义附着相机安装在本体系z轴方向上。在姿态机动过程中同时规避2个明亮天体，构成静态的禁忌约束，方向矢量为rF1，rF2，视场角设置与前一节相同。在惯性系下的强制约束方向矢量用rM1表示，要求rB与rM1之间的最小夹角为M1，rM1有旋转角速度M1。对应M1的取值大小，设置期望平均角速度。定义新的初始与目标姿态q0，qf，仿真条件设置如表2。其中强制指向约束角速度M1参考小行星2016HO3自旋周期0.467 h，角速度大小设为0.2/s，约等于0.003 5 rad/s.在上述条件下，姿态参数空间离散化方法在MRP空间和惯性系单位天球上，探测器实际机动路径如图14，15所示。由于强制约束要求三角形符号表示的当前姿态节点始终在图15中浅蓝色强制约束范围内，当前节点到目标姿态节点G的路径由于超出强制约束边界而无法规划，需要设置一个临时目标节点G。将临时目标节点设为：位于当前时刻指向约束的可行域内，到目标姿态节点的距离（用式（9）计算）最小的节点，该节点在强制约束边界上。采用分段式规划，分别规划一条满足强制约束和禁忌约束的、从当前节点i到临时目标节点G的路径，和一条只需满足禁忌约束的、从临时目标节点G到目标姿态节点G的路径。两段路径相连接即从起始到目标姿态，且满足当前时刻强制约束和禁忌约束的机动路径。当前节点扩展与时变强制约束冲突时，如果G G，则选取新的临时目标节点G，修正i到G的路径，并修正G到G的路径；如果G=G，直接修正i到G的路径。最终得到一条每一时刻姿态节点都满足时变强制指向约束的探测器可执行路径，即图14、15所示路径。1.01.01.00.20.40.60.80.50.50000.50.51.01.0zyx本文方法RRT1RRT2终点起点禁忌约束1禁忌约束2图13敏感器光轴在单位天球惯性系上的指向路径（本文方法与RRT方法结果对比）Fig.13Path of sensor axis on unit celestial sphere in inertial flame（results compared with RRT）表2加入强制指向约束的仿真条件Table 2Simulation parameters introducing mandatory constraints变量q0qfM1rM1M1rF1rF2 值1，0，0，0T0.801 8，0.180 2，-0.540 5，0.180 2T200，0，1T-0.003 5，0，0T rad/s0.330 4，0.088 5，0.939 7T0.696 4，-0.122 8，0.707 1T0.004 rad/sRRT空间离散化方法812610510152025303540420路径长度/rad时间/s图12空间离散化方法规划路径长度与RRT方法的40次重复仿真结果对比Fig.12Path length comparison between space discretization method and RRT for 40 times1.01.01.01.00.50.50.20.20.10.1000.500000.50.50.51.01.00.10.10.40.2321禁忌约束1禁忌约束2起点终点图14MRP空间中的规划路径（只画出静态指向约束）Fig.14Planned path in MRP space（static constraints displayed）362第 3 期朱哲等：时变指向约束下姿态参数空间离散化路径规划方法MRP和单位四元数及角速度、控制力矩随时间的变化轨迹如图16所示。角速度时间曲线显示，角速度的模在期望值 =0.004 rad/s附近保持小幅波动。单轴控制力矩最大值不超过0.02 N m。由此可知，与前一节角速度期望值 =0.03 rad/s时单轴控制力矩最大值不超过1 N m相比，角速度期望值越大，控制力矩幅值的峰值越大，通过设置期望角速度可以使控制力矩满足姿态机动的有界约束。将强制约束下的规划结果同样与RRT方法进行对比，取多次重复规划结果。在相同仿真条件下，将上述结果的规划路径长度与RRT方法的40次规划结果进行比较，如图17所示。812610510152025303540420路径长度/rad时间/sRRT空间离散化方法图17空间离散化方