CN100428100C - 深空探测器大角度机动路径自主生成方法 - Google Patents

Abstract

深空探测器大角度机动路径自主生成方法，涉及一种实现深空探测器自主生成大角度机动路径的方法。本发明要解决的技术问题是克服已有技术的困难，提供一种能够自主完成探测器大角度机动的方法，方法主要实现为：通过给定的探测器轨道信息，对环境进行判断并自动的生成障碍，基于这些障碍通过规划算法生成姿态机动的路径，这里，规划算法采用我们提出的改进的RRT算法，在随机树的扩展中通过单轴随机算法，使属于三维规划的大角度机动规划问题简化成为一个二维规划问题，并引入启发因子，加快算法的收敛。并利用前向搜索对规划出来的路径进行优化。本发明具有交互性强、直观、快捷等特点。

Description

深空探测器大角度机动路径自主生成方法

技术领域

本发明涉及一种实现深空探测器自主生成大角度机动路径的方法。

背景技术

深空探测是21世纪世界宇航大国关注的焦点之一，我国也在《中国航天》白皮书中提出，将“开展以月球探测为主的深空探测的预先研究”。由于探测目标距离远，飞行环境不确定等因素，深空探测器的控制和操作存在实时性和可靠性等方面的问题。应用自主技术是解决这些问题的一条有效途径，它通过在探测器上构造软件系统，完成探测器任务规划、活动分解执行、故障诊断、故障恢复等功能，使探测器的操作、控制形成真正的星上闭环。规划技术是实现自主探测器的关键技术之一。

探测器是深空探测任务中的主体，它由地球发射进入太空，实施月球探测、行星及其卫星探测和小行星与彗星探测。根据探测任务的不同，探测器可携带着陆器或探测车，并将其释放到星体表面进行近距离探测。深空探测中的探测目标与地球的距离十分遥远(现已达7.2×10⁹Km)，而且我们对深空环境还不十分了解，仅依靠以前的地面直接控制或遥控的方式很难完成对太阳系星体的探测活动。这就为深空探测任务提出了十分苛刻的要求，如操作费用、任务的可靠性、通讯网络和实时性的要求等。解决的办法就是采用人工智能中的规划调度技术、模式识别技术、故障诊断技术等，建立探测器上的自主控制系统。实现探测器自主性的核心是探测器自主任务规划技术，它根据地面的任务要求选择所要执行的活动，并给它们分配资源和时间，这些活动一旦执行便可以达到预期的目标。

探测器是一个复杂的个体，要完成自主的任务规划需要考虑到探测器的各种活动、约束，以及探测器自身的复杂属性，需要有很强的启发信息。正是基于这种思想，提出一种基于空间简化的深空大角度机动自主规划器，在考虑探测器的约束的前提下进行探测器姿态大角度机动的路径规划和优化。

在已有的大角度机动规划技术方案中，在先技术[1](参见G.Singh，G.Macala，E.Wong，and R.Rasmussen.A Constraint Monitor Algorithm for the Cassini Spacecraft.InProceedings of the AIAA Guidance，Navigation，and Control Conference，272-282，1997)中的Cassini探测器采用的是一种被动模式，即利用约束管理器，通过实时检查来确保姿态机动指令的可行性。但这是一种局部的被动判断，不能从整个机动过程中考虑规划。

在先技术[2](参见C.McInnes.Large Angle Slew Maneuver with AutonomousSun Vector Avoidance.AIAA J.on Guidance，Control and Dynamics，17(4)：875-877，1994)中通过定义势能函数的方法，该方法的主要缺点是容易陷入局部最小，而且对于复杂问题很难定义势能函数。

在先技术[3](参见E.Frazzoli，M.A.Dahleh and E.Feron.A RandomizedAttitude Slew Planning Algorithm for Autonomous Spacecraft.AIAA Guidance，Navigation and Control Conf.，AIAA，2001-4155)中结合概率路径图法(PRM，Probabilistic RoadMap)和快速扩展树法(RRT，Rapidly-Exploring Random Trees)，把自主探测器大角度机动当作一个三维规划问题。在RRT树的扩展中，随机变量为探测器姿态的俯仰、偏航、滚转三个角，该方法能完成大角度机动规划，但由于其还是一个三维规划问题，求解过程的搜索空间很大。

并且在先技术[2]和在先技术[3]中，都只是针对障碍完成路径规划，要完成星上的自主大角度机动规划，还需要解决自主的生成规划问题。

发明内容

本发明要解决的技术问题是克服上述已有技术的困难，提供一种能够自主完成探测器大角度机动的方法，该实现方法主要为：通过给定的探测器轨道信息，对环境进行判断并自动的生成障碍，基于这些障碍通过规划算法生成姿态机动的路径，这里，规划算法采用我们提出的改进的快速扩展树法算法，在随机树的扩展中通过单轴随机算法，使属于三维规划的大角度机动规划问题简化成为一个二维规划问题，并引入启发因子，加快算法的收敛。并利用前向搜索对规划出来的路径进行优化。

本发明的深空探测器大角度机动路径自主生成方法按照如下步骤进行：

一、障碍自主生成：需要的初始参数：(1)用户提供的探测器载荷安装；(2)在给定的时刻和探测器当前的位置。(3)大角度机动的初始、终止姿态。

1)、根据探测器载荷的安装，确定相关矢量，限制相关矢量R_i与圆锥体的轴线方向R_c的夹角满足下述条件：R_i·R_c＜cosα_min或R_i·R_c＞cosα_max；

2)、根据给定时刻，探测器自主生成地球-天线障碍区；

3)、根据探测器当前位置，确定太阳-导航相机障碍区、太阳-星敏感器障碍区；

4、根据给定时刻和探测器当前位置，自主确定需要考虑的大型行星，并确定相应的星敏感器-行星障碍区，其中当前行星在星敏感器中的成像大小满足下述条件：L＝R/D_sp-planet，R为行星的半径，D_sp-planet为探测器与行星的距离；

二、大角度机动规划：

(一)、利用单轴随机扩展的快速扩展树法规划算法规划姿态规划路径，具体方法如下：

1)、规划空间：

采用Euler轴角或四元数表示探测器的姿态；

2)、姿态制导律：

为满足约束条件，一次完整的大角度机动要分成多次小的大角度机动，定义小的机动间的状态为节点，假定节点处探测器的姿态角速率为零，且节点间的姿态运动为固定Euler轴旋转，则相邻节点间的姿态制导律为： $q_{i+1}=q_i\⊗q_{\mathrm{ei}},$ 其中：q_i为第i个节点对应的姿态四元数，q_ei为第i个节点与第i+1个节点间的偏差四元数，对应两节点间的固定Euler轴角旋转；

3)、二维的概率启发因子：

在随机生成节点的过程中，引入启发因子会加快算法的收敛，假设S、T分别为平面上的两点，两者中点为C，则随机生成点R可取概率为：

其中：A为常数，Dis(R，C)表示R、C之间的距离；

4)、单轴随机扩展算法：

(1)随机生成两个角α1、β1，求出其在单位球面上对应的一点Rrz(x_rz，y_rz，z_rz)，令Rrz为随机Euler轴角Rr时探测器体坐标系的Z轴指向矢量；

(2)按照二维的概率启发因子决定此点是否可取，如果可取则继续下一步骤，否则返回步骤(1)；

(3)在已有的Euler轴角中，找出Z轴指向矢量与Rrz夹角最小的Euler轴角Rn，夹角为θ；

(4)如果θ＞0.2弧度，返回步骤(1)；如果θ≤0.2弧度，继续下一步骤；

(5)Rn的X轴指向矢量为Rnx(x_nx，y_nx，z_nx)，Rn的Y轴指向矢量为Rny(x_ny，y_ny，z_ny)，如果z_nx≥z_ny，则执行步骤(6)，否则执行步骤(7)；

(6)通过下式求解Rr的X轴指向矢量Rrx(x_rx，y_rx，z_rx)：

$\left\{\begin{array}{c}{z}_{\mathrm{rx}}=z_{\mathrm{nx}}\\ x_{\mathrm{rx}}\·x_{\mathrm{rz}}+y_{\mathrm{rx}}\·y_{\mathrm{rz}}+z_{\mathrm{rx}}{z}_{\mathrm{rz}}=0\\ x_{\mathrm{rx}}\·x_{\mathrm{rx}}+y_{\mathrm{rx}}\·y_{\mathrm{rx}}+z_{\mathrm{rx}}\·z_{\mathrm{rx}}=1\end{array}\right.;$

(7)通过下式求解Rr的Y轴指向矢量Rry(x_ry，y_ry，z_ry)：

$\left\{\begin{array}{c}{z}_{\mathrm{ry}}=z_{\mathrm{ny}}\\ x_{\mathrm{ry}}\·x_{\mathrm{rz}}+y_{\mathrm{ry}}\·y_{\mathrm{rz}}+z_{\mathrm{ry}}\·z_{\mathrm{rz}}=0\\ x_{\mathrm{ry}}\·x_{\mathrm{ry}}+y_{\mathrm{ry}}\·y_{\mathrm{ry}}+z_{\mathrm{ry}}\·z_{\mathrm{ry}}=1\end{array}\right.;$

(8)由Rrz(x_rz，y_rz，z_rz)、Rrx(x_rx，y_rx，z_rx)或Rry(x_ry，y_ry，z_ry)即可确定此次随机生成的Euler轴角；

(9)判断生成的Euler轴角是否满足约束条件，如果不满足约束条件，返回步骤(1)；

5)、利用上述单轴随机扩展算法，得到改进的快速扩展树法规划算法如下：

(1)把起始姿态加入路径列表；

(2)判断路径列表中的最后一个点是否能够直接到达终止姿态，如果可以，结束，否则继续；

(3)利用单轴随机扩展算法生成一个随机姿态；

(4)在当前路径中找到离随机姿态最近的姿态；

(5)判断随机姿态和最近姿态之间的路径是否通过障碍区，如果是，则返回步骤(3)，否则，建立两者的联系；

(6)把随机点加入路径列表末尾，跳转至步骤(2)；

(二)、利用前向搜索对路径优化；

(三)、最终生成可行大角度机动规划路径。

本发明所述方法的基本原理：

在探测器大角度机动过程中，探测器姿态必须满足敏感仪器、保持通信链路等的约束。而这些约束是变化的，如对星敏感器来说，如果某个大行星的在其上的成像很小，则可以不考虑其影响，这样，我们就需要根据当前的时间和探测器所处的位置来确定当前的障碍，另外行星都是在运动的，所以其障碍在不同的时刻也是不同的，我们根据其成像大小来判断其是否需要考虑，从而能够自主的生成障碍。

在机动过程中，把探测器视作一个刚体，则其姿态为在三维空间的旋转。其规划问题构造空间为三维。用Euler轴角或四元数表示探测器的姿态。探测器的姿态有三个随机参数，即这是一个三维空间运动规划问题，而规划问题的搜索空间与规划空间的维数成指数增长。为了减少求解过程的搜索空间则可以减少规划问题的维数。基于这种目的，在每一步搜索我们采用一种单轴随机的算法，把此规划问题简化成一个二维空间的规划问题，具体办法是先随机生成两个变量，其对应探测器姿态的一个轴的指向矢量，而再充分利用临近点的信息得到另一轴的信息，从而确定当前随机的姿态。算法中只需要两个随机变量，即规划空间为二维，减少了规划问题构造空间的维数，从而减少问题求解的搜索空间，并在单轴随机过程中引入启发因子，加快算法的收敛。应用上述单轴随机算法进行随机扩展，直到找到可行的路径，并利用前向搜索的方式对路径进行优化。

这样，在给定时刻和给定探测器的位置的情况下，我们就能够自主的生成当前的障碍，并在这些障碍的情况下，进行探测器的大角度机动规划，最终形成一条可行的规划路径。

本发明能够自主的生成姿态机动障碍，并自主进行姿态机动规划生成大角度机动路径，并具有交互性强、直观、快捷等特点。在先技术[1]，先技术[2]，先技术[3]的不同之处在于，本发明的新颖之处在于能够自主的生成当前的姿态障碍，并能自主的避开障碍生成路径。从而实现了星上自主大角度机动，避免了传统的通过地面进行规划，然后给探测器上传机动指令，或探测器进行被动的检测地面上传指令是否可行方式。规划器可以作为深空探测器自主任务规划系统的一部分，也可以作为地面探测器姿态大角度机动路径规划的工具，形成地面上传的机动指令。

具体实施方式

具体实施方式一：本实施方式按照如下步骤实现深空探测器自主生成大角度机动路径：

一、障碍自主生成

大角度机动中要考虑装置、载荷等对外部环境的限制，主要的外部环境约束可以分成两类——指向约束和积分约束，指向约束要求探测器的某一矢量方向不能进入或离开某一区域，如果忽略机动过程中约束的变化，这类约束的障碍区域是一个圆锥体，我们只需要限制相关的矢量R_i与圆锥体的轴线方向R_c的夹角就行，可表示为：

R_i·R_c＜cosα_min或R_i·R_c＞cosα_max。

积分约束是一种时变的约束，如散热面暴露在太阳光下的时间不能太长，具体由下式决定：

${\&Integral;}_0^{t}g(\mathrm{\&theta;},d)\mathrm{d\&tau;}<H$

其中：H为一常值，即探测器最大能够承受的辐射量；g(θ，d)为辐射强度函数，与受照射面与太阳光矢量的夹角、探测器与太阳的距离等相关。

在这些约束中，指向约束是一个跟探测器轨道相关的参数，其可能考虑的对象有水星、金星、地球、火星、木星、土星，但探测器在不同的位置会有些不同，即有时只需要考虑其中的某几颗，我们是通过当前行星在星敏感器中的成像大小来决定的，为了简便，我们定义简单的成像大小为L，具体如下式：

L＝R/D_sp-planet

其中：R为行星的半径(公里)，D_sp-planet为探测器与其的距离(天文单位)。

当某行星的L大于1000时，就认为其为需要考虑的障碍。

二、大角度机动规划

上述已经自主的生成障碍，下一步的工作就是在这些障碍存在的情况下，自主的规划出一条可行的机动路径。基于对规划问题的规划空间、约束的描述以及机动姿态的制导律，本实施方式引入改进的RRT算法进行大角度机动规划，对RRT算法改进的具体步骤为：应用单轴随机算法生成探测器姿态的一个轴，然后利用临近点的信息确定当前的随机姿态。其中单轴随机算法中还引入了启发因子。

(一)规划空间

自主探测器大角度机动问题是一个含动态约束的动力学规划问题，通过简化，我们可以把问题变成一个运动学规划问题，而其也是一个PSPACE难题。算法的复杂度与问题的维数成指数增长。在机动过程中，把探测器视作一个刚体，则其姿态为在三维空间的旋转，即此规划问题的构造空间的维数为3。这里，我们用Euler轴角或四元数表示探测器的姿态。

(二)姿态制导律

为满足约束条件，一次完整的大角度机动可能要分成多次小的大角度机动，我们称小的机动间的状态为节点，为了问题的简化，我们假定节点处探测器的姿态角速率为零，且节点间的姿态运动为固定Euler轴旋转，可知相邻节点间的姿态制导律为：

$q_{i+1}=q_i\&CircleTimes;q_{\mathrm{ei}}$

其中：q_i为第i个节点对应的姿态四元数

q_ei为第i个节点与第i+1个节点间的偏差四元数，对应两节点间的固定Euler轴角旋转。

(三)单轴随机扩展的RRT规划算法

对于姿态大角度机动规划问题，采用三轴RRT方法，每一次树的扩展需要三个随机量，我们应用单轴随机扩展算法，即每次扩展只需要两个随机变量。而单轴随机时又采用二维的概率启发因子。

1、二维的概率启发因子

在随机生成节点的过程中，引入启发因子会加快算法的收敛，这里我们以简单的势能函数作为启发因子，用于表示接受此次随机生成的节点的概率，假设S、T分别为平面上的两点，两者中点为C，则随机生成点R可取概率为：

其中：A为常数，根据障碍选取，路径中的障碍越多，A值越大，一般取0.4；Dis(R，C)表示R、C之间的距离；

2、单轴随机扩展算法

在大角度机动规划中，此时的构造空间维数为3，我们应用单轴随机的方法生成节点，则问题简化为一个构造空间维数为2，具体方法如下：

(1)随机生成两个角α1、β1，求出其在单位球面上对应的一点Rrz(x_rz，y_rz，z_rz)，令Rrz为随机Euler轴角Rr时探测器体坐标系的Z轴指向矢量；

(2)按照二维的概率启发因子决定此点是否可取，如果可取则继续下一步骤，否则返回步骤(1)；

(3)在已有的Euler轴角中，找出Z轴指向矢量与Rrz夹角最小的Euler轴角Rn，夹角为θ；

(4)如果θ＞0.2弧度，返回步骤(1)；如果θ≤0.2弧度，继续下一步骤；

(5)得到Rn的X轴指向矢量Rnx(x_nx，y_nx，z_nx)，Rn的Y轴指向矢量Rny(x_ny，y_ny，z_ny)，如果z_nx≥z_ny，则执行步骤(6)，否则执行步骤(7)；

(6)通过下式求解Rr的X轴指向矢量Rrx(x_rx，y_rx，z_rx)：

$\left\{\begin{array}{c}{z}_{\mathrm{rx}}=z_{\mathrm{nx}}\\ x_{\mathrm{rx}}\&CenterDot;x_{\mathrm{rz}}+y_{\mathrm{rx}}\&CenterDot;y_{\mathrm{rz}}+z_{\mathrm{rx}}{z}_{\mathrm{rz}}=0\\ x_{\mathrm{rx}}\&CenterDot;x_{\mathrm{rx}}+y_{\mathrm{rx}}\&CenterDot;y_{\mathrm{rx}}+z_{\mathrm{rx}}\&CenterDot;z_{\mathrm{rx}}=1\end{array}\right.;$

(7)通过下式求解Rr的Y轴指向矢量Rry(x_ry，y_ry，z_ry)：

$\left\{\begin{array}{c}{z}_{\mathrm{ry}}=z_{\mathrm{ny}}\\ x_{\mathrm{ry}}\&CenterDot;x_{\mathrm{rz}}+y_{\mathrm{ry}}\&CenterDot;y_{\mathrm{rz}}+z_{\mathrm{ry}}\&CenterDot;z_{\mathrm{rz}}=0\\ x_{\mathrm{ry}}\&CenterDot;x_{\mathrm{ry}}+y_{\mathrm{ry}}\&CenterDot;y_{\mathrm{ry}}+z_{\mathrm{ry}}\&CenterDot;z_{\mathrm{ry}}=1\end{array}\right.;$

(8)由Rrz(x_rz，y_rz，z_rz)、Rrx(x_rx，y_rx，z_rx)或Rry(x_ry，y_ry，z_ry)即可确定此次随机生成的Euler轴角。

(9)判断生成的Euler轴角是否满足约束条件，如果否，返回步骤(1)。

可以看出，在算法中随机Euler轴角仅一个轴是随机生成的，再利用此轴临近Euler轴角的信息，即可确定当前随机生成的Euler轴角，从而减少了对随机Euler轴角另一轴的搜索。

随机生成节点算法保证了路径的起点和终点不在障碍区，但其并不能保证路径上所有点均不在障碍区，因此还需要检测路径是否通过了障碍区，我们可以采用取样检测的方法，只需要检测路径中的取样点，而点的几何约束检测可利用前面的约束表达式。

3、单轴随机扩展的RRT规划算法

利用上面的随机算法，可以得到改进的RRT规划算法如下：

(1)把起始姿态加入路径列表；

(2)判断路径列表中的最后一个点是否能够直接到达终止姿态，如果可以，结束，否则继续；

(3)利用单轴随机扩展算法生成一个随机姿态；

(4)在当前路径中找到离随机姿态最近的姿态，

(5)判断随机姿态和最近姿态之间的路径是否通过障碍区，如果是，则返回3，否则，建立两者的联系；

(6)把随机点加入路径列表末尾；跳转至2；

H-RRT(N_s，N_t)

  {

    N_ini＝N_s；R.add(N_s)；//初始化，起点加入路径序列

    while！Arrive(N_ini，N_t)；

    {

    do {

       N_rand＝RandNode()；

       N_near＝LeastD(N_rand，R)；

       }while！AvoidRouter(N_rand，N_near)；

    R.add(N_rand)；

    N_ini＝N_rand；//更新路径最后一个点

    }

  R.add(N_t)；

}。

其中：Arrive(N_ini，N_t)用于判断从N_ini是否能够直接到达N_t；RandNode()为根据单轴随机扩展算法生成一个节点。LeastD(N_rand，R)，找到已有节点集合中离N_rand最近的节点，AvoidRouter(N_rand，N_near)检测N_rand、N_near之间的路径是否在障碍区。

算法中需要用到两个状态间的距离信息，而在非平面规划过程中，没有直观的距离，因此需要通过定义一个变量来表示距离，称之为伪距离，Frazzoli等人是通过求解最优控制问题，从而利用代价函数作为伪距离。我们这里通过两个Euler轴角之间的机动时间当作伪距离，机动时间估计方法可以参照Clementine上采用的方法。

(四)路径优化及积分约束

由随机生成树构成的路径往往需要优化，才能形成最优或次优的路径，优化的准则是整个机动时间最短。

路径优化采用前向搜索的方法，如下：以起点为初始点，依次搜索，找到其能直接到达的最远节点，条件是他们之间直接到达的路径能够满足路径检测。删除中间节点，连接起点和最远点。然后以最远点为起点，进行同样的操作。如此反复搜索，直到起点为路径终点的前一个点。这样，依次连接所有被选中的起点，加上路径终点构成最终的优化路径，具体如下：

Optimal(R)

{

for i＝0→size(R)-2//size(R)初始路径R的节点数；

   {tag＝i+1；//初始化下一个连接点的编号tag；

     for j＝size(R)-1→i+1

        {//依次搜索

        If AvoidRouter(R[i]，R[j])；{tag＝j；L_Node＝R[j]；}

        }

     Result_R.add(L_Node)；//加入连接的节点；

     i＝tag；

   }

}。

积分约束是在路径生成之后进行检测，如果不满足则需要重新规划，这里把路径按节点分成离散的段，则积分约束变成下式：

$\underset{i=0}{\overset{n-2}{\mathrm{\&Sigma;}}}(g\left(i\right)+g(i+1))\mathrm{\&Delta;t}/2\&le;H$

其中：Δt为从i点机动到i+1点的时间；n为路径被离散成的点数，包含起点和终点。

Claims (1)

1、深空探测器大角度机动路径自主生成方法，其特征在于所述方法按照如下步骤进行：

一、障碍自主生成：

1)、根据探测器载荷的安装，确定相关矢量，限制相关矢量R_i与圆锥体的轴线方向R_c的夹角满足下述条件：R_i·R_c＜cosα_min或R_i·R_c＞cosα_max；

2)、根据给定时刻，探测器自主生成地球-天线障碍区；

3)、根据探测器当前位置，确定太阳-导航相机障碍区、太阳-星敏感器障碍区；

4)、根据给定时刻和探测器当前位置，自主确定需要考虑的大型行星，并确定相应的星敏感器-行星障碍区，其中当前行星在星敏感器中的成像大小满足下述条件：L＝R/D_sp-planet，R为行星的半径，D_sp-planet为探测器与行星的距离；

二、大角度机动规划：

(一)、利用单轴随机扩展的快速扩展树法规划算法规划姿态规划路径，具体方法如下：

1)、规划空间：

采用Euler轴角或四元数表示探测器的姿态；

2)、姿态制导律：

为满足约束条件，一次完整的大角度机动要分成多次小的大角度机动，定义小的机动间的状态为节点，假定节点处探测器的姿态角速率为零，且节点间的姿态运动为固定Euler轴旋转，则相邻节点间的姿态制导律为： $q_{i+1}=q_i\&CircleTimes;q_{\mathrm{ei}},$ 其中：q_i为第i个节点对应的姿态四元数，q_ei为第i个节点与第i+1个节点间的偏差四元数，对应两节点间的固定Euler轴角旋转；

3)、二维的概率启发因子：

在随机生成节点的过程中，引入启发因子会加快算法的收敛，假设S、T分别为平面上的两点，两者中点为C，则随机生成点R可取概率为：

其中：A为常数，Dis(R，C)表示R、C之间的距离；

4)、单轴随机扩展算法：

(1)随机生成两个角α1、β1，求出其在单位球面上对应的一点Rrz(x_rz，y_rz，z_rz)，令Rrz为随机Euler轴角Rr时探测器体坐标系的Z轴指向矢量；

(2)按照二维的概率启发因子决定此点是否可取，如果可取则继续下一步骤，否则返回步骤(1)；

(3)在已有的Euler轴角中，找出Z轴指向矢量与Rrz夹角最小的Euler轴角Rn，夹角为θ；

(4)如果θ＞0.2弧度，返回步骤(1)；如果θ≤0.2弧度，继续下一步骤；

(5)Rn的X轴指向矢量为Rnx(x_nx，y_nx，z_nx)，Rn的Y轴指向矢量为Rny(x_ny，y_ny，z_ny)，如果z_nx≥z_ny，则执行步骤(6)，否则执行步骤(7)；

(6)通过下式求解Rr的X轴指向矢量Rrx(x_rx，y_rx，z_rx)：

$\left\{\begin{array}{c}{z}_{\mathrm{rx}}=z_{\mathrm{nx}}\\ x_{\mathrm{rx}}\&CenterDot;x_{\mathrm{rz}}+y_{\mathrm{rx}}\&CenterDot;y_{\mathrm{rz}}+z_{\mathrm{rx}}\&CenterDot;z_{\mathrm{rz}}=0\\ x_{\mathrm{rx}}\&CenterDot;x_{\mathrm{rx}}+y_{\mathrm{rx}}\&CenterDot;y_{\mathrm{rx}}+z_{\mathrm{rx}}\&CenterDot;z_{\mathrm{rx}}=1\end{array}\right.;$

(7)通过下式求解Rr的Y轴指向矢量Rry(x_ry，y_ry，z_ry)：

$\left\{\begin{array}{c}{z}_{\mathrm{ry}}=z_{\mathrm{ny}}\\ x_{\mathrm{ry}}\&CenterDot;x_{\mathrm{rz}}+y_{\mathrm{ry}}\&CenterDot;y_{\mathrm{rz}}+z_{\mathrm{ry}}\&CenterDot;z_{\mathrm{rz}}=0\\ x_{\mathrm{ry}}\&CenterDot;x_{\mathrm{ry}}+y_{\mathrm{ry}}\&CenterDot;y_{\mathrm{ry}}+z_{\mathrm{ry}}\&CenterDot;z_{\mathrm{ry}}=1\end{array}\right.;$

(8)由Rrz(x_rz，y_rz，z_rz)、Rrx(x_rx，y_rx，z_rx)或Rry(x_ry，y_ry，z_ry)即可确定此次随机生成的Euler轴角；

(9)判断生成的Euler轴角是否满足约束条件，如果不满足约束条件，返回步骤(1)；

5)、利用上述单轴随机扩展算法，得到改进的快速扩展树法规划算法如下：

(1)把起始姿态加入路径列表；

(2)判断路径列表中的最后一个点是否能够直接到达终止姿态，如果可以，结束，否则继续；

(3)利用单轴随机扩展算法生成一个随机姿态；

(4)在当前路径中找到离随机姿态最近的姿态；

(5)判断随机姿态和最近姿态之间的路径是否通过障碍区，如果是，则返回步骤(3)，否则，建立两者的联系；

(6)把随机点加入路径列表末尾，跳转至步骤(2)；

(二)、利用前向搜索对路径优化；

(三)、最终生成可行大角度机动规划路径。