CN109254589B - 带可动相机探测器的姿态机动规划方法 - Google Patents

Abstract

本发明公开的带可动相机探测器的姿态机动规划方法，属于航天器姿态规划领域。本发明实现方法为：通过在轨道坐标系下建立探测器和可动相机组合体的姿态模型，在本体系下建立可动相机的姿态模型，基于上述模型处理可动相机的有界约束和指向约束，区别于传统的三轴稳定探测器姿态路径规划，将可动相机当做探测器的第n轴进行独立工作，通过随机运动规划RRT算法得到满足有界和指向约束的可动相机的姿态机动路径，再采用差分进化算法对上述路径优化处理，得到快速高效的可动相机的姿态路径。本发明具有速度快，效率高，安全性高，节省能源消耗和本体姿态机动灵活性高的优点，适用于带可动相机探测器姿态机动路径规划。

Description

带可动相机探测器的姿态机动规划方法

技术领域

本发明涉及可动相机的姿态建模与规划方法，适用于带可动相机探测器姿态机动过程，属于航天器姿态规划领域。

背景技术

探测器往往带有可动相机，以便于进行深空探测和导航等任务。探测器在执行任务时，要求可动相机进行多次姿态机动，故存在各种姿态几何约束，可动相机需要指向探测目标，同时可动相机作为光敏元件需要躲避强光天体。为了保证深空任务的持久运行，需要做好携带可动相机探测器的姿态规划。

在带可动相机探测器的姿态控制研究方面，把可动相机当作扰动，通过设计前馈反馈或者自适应控制器，来补偿探测器本体姿态运动，从而提高探测器的抗干扰性能。而在可动相机的姿态规划方面，都把可动相机看作本体系下的固定方向矢量，对三轴探测器整体进行姿态规划，常用方法有势能函数法、约束监测法、随机运动规划方法、遗传算法、粒子群算法等。例如随机运动规划RRT算法，就是一种高效地解决高维空间和复杂约束的路径规划算法，而差分进化算法则是一种基于群体的启发式搜索算法，可以用于全局优化路径。但基于三轴探测器整体姿态规划，这样可动相机不是真正意义上可动的。

实际深空探测任务中，为了避免大角度偏转和维持姿态稳定，可动相机常常独立于三轴本体作为单独的第四轴来进行工作，而且可动相机一般需要较快完成运动。这时就需要对带可动相机探测器进行姿态建模，并针对其第四轴可动的特性进行姿态规划。具有效率高，安全性高和灵活性强的优点。

发明内容

现有技术中带可动相机探测器都建立三轴稳定的姿态模型，把可动相机看作本体上的定矢量进行姿态建模和规划，并不符合探测器可动相机常常是相对于探测器本体转动进行工作的实际工程情况。本发明公开的带可动相机探测器的姿态机动规划方法要解决的技术问题是：建立探测器可动相机的四轴姿态模型，对可动相机和探测器的姿态动力学进行解耦，分别考虑可动相机和探测器所受约束，实现带可动相机探测器的姿态建模与姿态机动规划。本发明具有速度快，效率高，安全性高，节省能源消耗和本体姿态机动灵活性高的优点。

本发明的目的是通过下述技术方案实现的。

本发明公开的带可动相机探测器的姿态机动规划方法，通过在轨道坐标系下建立探测器和可动相机组合体的姿态模型，在本体系下建立可动相机的姿态模型，基于上述模型处理可动相机的有界约束和指向约束，区别于传统的三轴稳定探测器姿态路径规划，将可动相机当做探测器的第n轴进行独立工作，通过随机运动规划RRT算法得到满足有界和指向约束的可动相机的姿态机动路径，再采用差分进化算法对上述路径优化处理，得到快速高效的可动相机的姿态路径。该方法具有速度快，效率高，安全性高，节省能源消耗和本体姿态机动灵活性高的优点，适用于带可动相机探测器姿态机动路径规划。

本发明公开的带可动相机探测器的姿态机动规划方法，包括如下步骤：

步骤一：以轨道坐标系当作参考坐标系，在参考坐标系下建立探测器和可动相机的姿态模型。

以轨道坐标系当作参考坐标系，即当探测器的姿态角为零的时候，本体坐标系与轨道坐标系重合。此时探测器和可动相机组合体的角动量由两部分组成，再考虑动量矩定理，得出探测器和可动相机组合体姿态动力学方程：

其中，I_x和ω_x为可动相机相对探测器的转动惯量以及角速度，I是探测器和可动相机组合体的转动惯量，ω是探测器的绝对角速度，

是ω的斜矩阵，M_z是探测器本体的控制力矩。

组合体的运动学模型只考虑探测器本体的角速度，姿态参数用四元数表示：

步骤二：以探测器本体系为参考坐标系，在参考坐标系下建立可动相机的姿态模型。

取探测器本体系为参考坐标系，在参考坐标系中将可动相机当作探测器的第n轴，用四元数形式表示姿态，建立参考坐标系下可动相机的姿态模型，可动相机的姿态运动学和动力学的方程表示如下：

I_xω_x＝T-ω_x×I_xω_x (4)

其中：T为探测器对可动相机的控制力矩，即能够得到可动相机的姿态参数。

步骤三：基于步骤一建立的探测器和可动相机的姿态模型和步骤二建立的可动相机的姿态模型，建立可动相机相对探测器运动的有界和几何约束。

可动相机在相对探测器运动的时存在有界和几何约束，为了保持相机本身的稳定和防止过度运动带来较大干扰和器件损坏，可动相机在工作时，需保证可动相机的控制力矩u和角速度ω保持在预设范围以内。基于公式(3)和(4)的有界约束表示形式如公式(5)：

|u_i|≤u_max i＝1,2,3

|ω_i|≤ω_max i＝1,2,3 (5)

带可动相机的探测器在执行空间任务的时，存在各种各样的指向约束。现有技术把可动相机简化为固连在探测器本体上，并不符合探测器可动相机常常是相对于探测器本体转动进行工作的实际工程情况。为解决上述缺点，将可动相机和本体分离开，当作探测器的第n轴考虑。

可动相机的指向约束，避免强光天体进入可动相机光敏感元件的视场，r_x1表示可动相机的光敏元件在可动相机参考系下的方向矢量，r_I表示所述强光天体在惯性系下的方向矢量。要求强光天体方向矢量和可动相机光敏元件的视线方向夹角不能低于设定值，可动相机的指向约束表示形式如公式(6)：

C_xI＝C_xbC_bI (6)

其中，C_xb表示可动相机坐标系到探测器本体系下的姿态余弦矩阵，由q_x表示，C_bI表示探测器本体系到惯性系下的姿态余弦矩阵，由q表示，C_xI表示可动部件坐标系到惯性系下的姿态余弦矩阵，则转化成四元数形式由q_x和q同时表示，其中可动相机的约束由探测器本体角速度和可动相机角速度耦合表示。

上述方法处理指向约束，相比于三轴稳定探测器的指向约束，不需要探测器本体转动来规避约束，只用转动可动相机这一轴就可以规避指向约束。提高了探测器姿态机动的灵活性，能够简化本体姿态运动，节省姿态机动所需能源和燃料。

步骤四：基于步骤一建立的探测器和可动相机的姿态模型和步骤二建立的可动相机的姿态模型以及步骤三建立的可动相机有界和指向约束，通过随机运动规划算法得到满足有界和指向约束的可动相机的姿态路径，实现带可动相机探测器的姿态机动规划。

基于公式(1)和(2)建立的探测器和可动相机的姿态模型以及公式(3)和(4)建立的可动相机的姿态模型，由公式(5)和(6)进行约束判断，通过随机运动规划算法得到满足有界和指向约束的可动相机的姿态路径。建立的姿态模型包括四个矢量，所述随机运动规划算法将可动相机中探测器本体角速度和可动相机角速度解耦，得到可动相机的姿态路径。

所述随机运动规划算法为RRT算法，具体实现方法为：

首先初始化树Tr，设置初始节点和目标节点分别为初始姿态和最终姿态，以随机概率在搜索空间内生成随机扩展节点x_ran，再从树的所有节点中选择距离扩展节点最近的节点x_near，沿x_near向x_ran的延伸一个步长的距离，记新节点为x_new。在生成新节点x_new时，由公式(1)和(2)，得出探测器本体姿态四元数，由公式(3)和(4)，得出可动相机姿态四元数。然后再带入可动相机的约束公式(5)和(6)中，判断当前新节点的姿态参数和探测器本体的姿态参数是否违反可动相机的有界和指向约束，若都满足则成功扩展新节点。一直扩展新节点直到足够接近目标节点，得到从初始姿态到最终姿态的可行的可动相机姿态路径，即实现带可动相机探测器的姿态建模与姿态机动规划。

为提高所述携带可动相机探测器的姿态建模与规划方法的效率，还包括步骤五：基于步骤四得到的可动相机的姿态路径，采用优化处理，得到快速高效的可动相机的姿态路径。

步骤五所述优化处理方法包括蚁群算法，粒子群算法，差分进化算法等，优选差分进化算法。

对步骤四得到的可行路径，采用差分进化算法对路径进行优化处理。为了提高快速机动效率，设置时间适应度函数。

J_t＝t_f (7)

满足可动相机指向约束，设置指向二次型适应度函数：

初始种群为RRT算法得到的姿态机动路径，通过差分进化的交叉选择得到优化后的可动相机姿态路径，即能够得到带可动相机探测器的优化后的姿态机动路径。

有益效果：

1、本发明公开的带可动相机探测器的姿态机动规划方法，对探测器和可动相机组合体建模，将可动相机当做探测器第n轴考虑，对可用相机建模，实现了姿态规划中首次将可动相机与探测器本体分离，符合实际任务情况，进而得到更加准确的姿态模型。

2、本发明公开的带可动相机探测器的姿态机动规划方法，建立可动相机相对探测器运动的有界和几何约束，提高了探测器姿态机动的灵活性，可简化本体姿态运动，节省姿态机动所需能源和燃料

3、本发明公开的带可动相机探测器的姿态机动规划方法，使用RRT算法生成初始姿态机动路径，处理第n轴可动相机的有界和指向约束，得到分离后可动相机的姿态机动路径，姿态路径可以规避碰撞约束，安全可靠。

4、本发明公开的带可动相机探测器的姿态机动规划方法，用差分进化算法对可行路径进行优化，得到时间优化后可动相机的姿态机动路径，具有效率高，速度快，安全性高的优点。本发明在路径规划和航天器姿态规划领域中有很好的应用前景。

附图说明

图1是探测器可动相机所受指向约束示意图；

图2是探测器可动相机姿态机动路径求解算法总体流程图；

图3经过差分进化算法优化后探测器可动相机姿态路径示意图。

具体实施方式

为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。

为了验证方法的可行性，这里设置可动相机初始角速度为0，初始姿态四元数为[0.6469,0.0347,0.7224,0.2417]，光敏视场角为30度，给出四个强光天体，其本体系下方向矢量分别是：[0.1736,-0.9848,0]^T，[0.4924,0.8529,0.1736]^T，[0,0.5,0.8660]^T，[-0.7660,-0,0.6428]^T。

如图2所示，本实施例公开的带可动相机探测器姿态机动路径求解方法，包括如下步骤

步骤一：以轨道坐标系当作参考坐标系，在参考坐标系下建立探测器和可动相机的姿态模型。

以轨道坐标系当作参考坐标系，即当探测器的姿态角为零的时候，本体坐标系与轨道坐标系重合。此时探测器和可动相机组合体的角动量由两部分组成，再考虑动量矩定理，得出探测器和可动相机组合体姿态动力学方程：

其中，I_x和ω_x为可动相机相对探测器的转动惯量以及角速度，这里I_x设定为[0.1,0.1,0.1]，I为[1,1,1]表示探测器和可动相机组合体的转动惯量，ω是探测器的绝对角速度，

是ω的斜矩阵，M_z是探测器本体的控制力矩。

组合体的运动学模型只考虑探测器本体的角速度，姿态参数用四元数表示：

步骤二：以探测器本体系为参考坐标系，在参考坐标系下建立可动相机的姿态模型。

取探测器本体系为参考坐标系，在参考坐标系中将可动相机当作探测器的第n轴，用四元数形式表示姿态，建立参考坐标系下可动相机的姿态模型，可动相机的姿态运动学和动力学的方程表示如下：

I_xω_x＝T-ω_x×I_xω_x (4)

其中：T为探测器对可动相机的控制力矩，可动相机的初始姿态四元数q_x为[0.6469,0.0347,0.7224,0.2417]，可得到可动相机的姿态参数。

步骤三：基于步骤一建立的探测器和可动相机的姿态模型和步骤二建立的可动相机的姿态模型，建立可动相机相对探测器运动的有界和几何约束。

可动相机在相对探测器运动的时存在有界和几何约束，为了保持相机本身的稳定和防止过度运动带来较大干扰和器件损坏，可动相机在工作时，需保证可动相机的控制力矩u和角速度ω保持在预设范围以内。基于公式(3)和(4)的有界约束表示形式如公式(5)：

|u_i|≤u_max i＝1,2,3

|ω_i|≤ω_max i＝1,2,3 (5)

带可动相机的探测器在执行空间任务的时，存在各种各样的指向约束。现有技术把可动相机简化为固连在探测器本体上，并不符合探测器可动相机常常是相对于探测器本体转动进行工作的实际工程情况。为解决上述缺点，将可动相机和本体分离开，当作探测器的第n轴考虑。

可动相机的指向约束，避免强光天体进入可动相机光敏感元件的视场，如图1所示：r_x1表示可动相机的光敏元件在可动相机参考系下的方向矢量，r_I表示所述强光天体在惯性系下的方向矢量。要求强光天体方向矢量和可动相机光敏元件的视线方向夹角不能低于设定值，可动相机的指向约束表示形式如公式(6)：

C_xI＝C_xbC_bI (6)

其中，C_xb表示可动相机坐标系到探测器本体系下的姿态余弦矩阵，由q_x表示，C_bI表示探测器本体系到惯性系下的姿态余弦矩阵，由q表示，C_xI表示可动部件坐标系到惯性系下的姿态余弦矩阵，则转化成四元数形式由q_x和q同时表示，其中可动相机的约束由探测器本体角速度和可动相机角速度耦合表示，这里设定光敏视场角θ₁为30度，给出四个强光天体，其本体系下方向矢量分别是：[0.1736,-0.9848,0]^T，[0.4924,0.8529,0.1736]^T，[0,0.5,0.866]^T，[-0.766,-0,0.6428]^T。

上述方法处理指向约束，相比于三轴稳定探测器的指向约束，不需要探测器本体转动来规避约束，只用转动可动相机这一轴就可以规避指向约束。提高了探测器姿态机动的灵活性，可简化本体姿态运动，节省姿态机动所需能源和燃料。

步骤四：基于步骤一建立的探测器和可动相机的姿态模型和步骤二建立的可动相机的姿态模型以及步骤三建立的可动相机有界和指向约束，通过随机运动规划算法得到满足有界和指向约束的可动相机的姿态路径，实现带可动相机探测器的姿态机动规划。

基于公式(1)和(2)建立的探测器和可动相机的姿态模型以及公式(3)和(4)建立的可动相机的姿态模型，由公式(5)和(6)进行约束判断，通过随机运动规划算法得到满足有界和指向约束的可动相机的姿态路径。建立的姿态模型包括四个矢量，所述随机运动规划算法将可动相机中探测器本体角速度和可动相机角速度解耦，得到可动相机的姿态路径。

所述随机运动规划算法为RRT算法，具体实现方法为：

首先初始化树Tr，导入初始随机四元数，设置最大循环次数为10000，设置初始节点和目标节点分别为初始姿态和最终姿态，以随机概率在搜索空间内生成随机扩展节点x_ran，再从树的所有节点中选择距离扩展节点最近的节点x_near，沿x_near向x_ran的延伸一个步长的距离，记新节点为x_new。在生成新节点x_new时，由公式(1)和(2)，得出探测器本体姿态四元数，由公式(3)和(4)，得出可动相机姿态四元数。然后再带入可动相机的约束公式(5)和(6)中，判断当前新节点的姿态参数和探测器本体的姿态参数是否违反可动相机的有界和指向约束，若都满足则成功扩展新节点。一直扩展新节点直到足够接近目标节点，得到从初始姿态到最终姿态的可行的可动相机姿态路径，即实现带可动相机探测器的姿态建模与姿态机动规划。

为提高所述携带可动相机探测器的姿态建模与规划方法的效率，还包括步骤五：基于步骤四得到的可动相机的姿态机动路径，采用优化处理，得到快速高效的可动相机的姿态机动路径。

步骤五所述优化处理方法包括蚁群算法，粒子群算法，差分进化算法等，优选差分进化算法。

对步骤四得到的可行路径，采用差分进化算法对路径进行优化处理。为了提高快速机动效率，设置时间适应度函数。

J_t＝t_f (7)

满足可动相机指向约束，设置指向二次型适应度函数：

初始种群为RRT算法得到的姿态机动路径，取种群进化代数为500。然后对种群进行变异和交叉，对每一代种群进行适应度评价，为了快速机动，选择时间作为优化目标，同时考虑有界指向约束，一起构造适应度函数。每次选择最优适应度的种群作为当前种群，不断地迭代计算，保留并变异当前优良种群，即能够得到带可动相机探测器的优化后的姿态机动路径，如图3所示。

在图3中，圆形区域表示强光天体约束，点实线表示可动相机的姿态机动路径，可以看出可动相机能够成功地规避强光障碍区域，得到可动相机在本体系中安全快速的姿态机动路径。

以上所述的具体描述，对发明的目的、技术方案和有益效果进行进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.带可动相机探测器的姿态机动规划方法，其特征在于：包括如下步骤，

步骤一：以轨道坐标系当作参考坐标系，在参考坐标系下建立探测器和可动相机的姿态模型；

步骤二：以探测器本体系为参考坐标系，在参考坐标系下建立可动相机的姿态模型；

步骤三：基于步骤一建立的探测器和可动相机的姿态模型和步骤二建立的可动相机的姿态模型，建立可动相机相对探测器运动的有界和指向约束；

步骤四：基于步骤一建立的探测器和可动相机的姿态模型和步骤二建立的可动相机的姿态模型以及步骤三建立的可动相机有界和指向约束，通过随机运动规划算法得到满足有界和指向约束的可动相机的姿态路径，实现带可动相机探测器的姿态机动规划。

2.如权利要求1所述的带可动相机探测器的姿态机动规划方法，其特征在于：还包括步骤五：基于步骤四得到的可动相机的姿态路径，采用优化处理，得到可动相机的姿态路径。

3.如权利要求2所述的带可动相机探测器的姿态机动规划方法，其特征在于：步骤一具体实现方法为，

以轨道坐标系当作参考坐标系，即当探测器的姿态角为零的时候，探测器本体系与轨道坐标系重合；此时探测器和可动相机组合体的角动量由两部分组成，再考虑动量矩定理，得出探测器和可动相机组合体姿态动力学方程：

其中，I_x和ω_x为可动相机相对探测器的转动惯量以及角速度，I是探测器和可动相机组合体的转动惯量，ω是探测器的绝对角速度，

是ω的斜矩阵，M_z是探测器本体的控制力矩；

组合体的运动学模型只考虑探测器本体的角速度，姿态参数用四元数表示：

4.如权利要求3所述的带可动相机探测器的姿态机动规划方法，其特征在于：步骤二具体实现方法为，

取探测器本体系为参考坐标系，在参考坐标系中将可动相机当作探测器的第n轴，用四元数形式表示姿态，建立参考坐标系下可动相机的姿态模型，可动相机的姿态运动学和动力学的方程表示如下：

I_xω_x＝T-ω_x×I_xω_x (4)

其中，T为探测器对可动相机的控制力矩，即能够得到可动相机的姿态参数。

5.如权利要求4所述的带可动相机探测器的姿态机动规划方法，其特征在于：步骤三具体实现方法为，

可动相机在相对探测器运动的时存在有界和指向约束，为了保持相机本身的稳定和防止过度运动带来较大干扰和器件损坏，可动相机在工作时，需保证可动相机的控制力矩u和角速度ω保持在预设范围以内；基于公式(3)和(4)的有界约束表示形式如公式(5)：

|u_i|≤u_max i＝1，2，3

|ω_i|≤ω_max i＝1，2，3 (5)

带可动相机的探测器在执行空间任务的时候，存在各种各样的指向约束；将可动相机和本体分离开，当作探测器的第n轴考虑；

可动相机的指向约束，避免强光天体进入可动相机光敏感元件的视场，r_x1表示可动相机的光敏元件在可动相机参考系下的方向矢量，r_I表示所述强光天体在惯性系下的方向矢量；要求强光天体方向矢量和可动相机光敏元件的视线方向夹角不能低于设定值，可动相机的指向约束表示形式如公式(6)：

C_xI＝C_xbC_bI (6)

其中，C_xb表示可动相机坐标系到探测器本体系下的姿态余弦矩阵，由q_x表示，C_bI表示探测器本体系到惯性系下的姿态余弦矩阵，由q表示，C_xI表示可动部件坐标系到惯性系下的姿态余弦矩阵，则转化成四元数形式由q_x和q同时表示，其中可动相机的约束由探测器本体角速度和可动相机角速度耦合表示。

6.如权利要求5所述的带可动相机探测器的姿态机动规划方法，其特征在于：步骤四具体实现方法为，

基于公式(1)和(2)建立的探测器和可动相机的姿态模型以及公式(3)和(4)建立的可动相机的姿态模型，由公式(5)和(6)进行约束判断，通过随机运动规划算法得到满足有界和指向约束的可动相机的姿态路径；建立的姿态模型包括四个矢量，所述随机运动规划算法将可动相机中探测器本体角速度和可动相机角速度解耦，得到可动相机的姿态路径；

所述随机运动规划算法为RRT算法，具体实现方法为，

首先初始化树Tr，设置初始节点和目标节点分别为初始姿态和最终姿态，以随机概率在搜索空间内生成随机扩展节点x_ran，再从树的所有节点中选择距离扩展节点最近的节点x_near，沿x_near向x_ran的延伸一个步长的距离，记新节点为x_new；在生成新节点x_new时，由公式(1)和(2)，得出探测器本体姿态四元数，由公式(3)和(4)，得出可动相机姿态四元数；然后再带入可动相机的约束公式(5)和(6)中，判断当前新节点的姿态参数和探测器本体的姿态参数是否违反可动相机的有界和指向约束，若都满足则成功扩展新节点；一直扩展新节点直到足够接近目标节点，得到从初始姿态到最终姿态的可行的可动相机姿态路径，即实现带可动相机探测器的姿态建模与姿态机动规划。

7.如权利要求6所述的带可动相机探测器的姿态机动规划方法，其特征在于：步骤五所述优化处理方法包括蚁群算法，粒子群算法，差分进化算法。

8.如权利要求7所述的带可动相机探测器的姿态机动规划方法，其特征在于：对步骤四得到的可行路径，采用差分进化算法对路径进行优化处理；设置时间适应度函数；

J_t＝t_f (7)

满足可动相机指向约束，设置指向二次型适应度函数：

初始种群为RRT算法得到的姿态机动路径，通过差分进化的交叉选择得到优化后的可动相机姿态路径，即能够得到带可动相机探测器的优化后的姿态机动路径。

Images