一种基于B样条函数的卫星路径规划和预测控制的跟踪方法

技术领域

本发明涉及一种基于B样条函数的卫星路径规划和预测控制的跟踪方法，主要应用于航天器路径规划及跟踪控制，属于航天器控制技术领域。

背景技术

空间站作为一种长时间在轨运行，为人类提供开发太空资源，观测太空环境的载人航天器，已经吸引了众多国内外研究人员进行科学研究。随着空间站的研究与发展，其长期在轨稳定运行离不开航天器的监视与侦查，如美国的AERCam微小卫星，对空间站进行外部检查；以及空间站舱外监视器Inspector，对空间站进行舱外维修与检测。除此之外，航天员出舱维护与管理也是空间站稳定运行必不可少的保证，然而，航天员出舱活动风险大、费用高，针对此问题，各国相继研制开发了代替航天员舱外活动的微小卫星，同时其可以完成各种各样复杂的空间任务。

微小卫星体积小，质量轻，成本低，在完成很多复杂的空间任务上发挥着越来越大的作用，如深空探测、悬停绕飞、检测侦查、维修管理等空间任务。随着科学技术的进步与发展，空间站中的卫星功能将会更加强大，应用越来越灵活，作用也会随之更加广泛，因此，在空间站上释放卫星的需求将会增加。目前，国际空间站上已经安装有专门释放卫星的装置，如国际空间站上的日本实验模块小卫星轨道释放器和NanoRacks立方体卫星释放器。

空间站中释放出的卫星需要考虑其对空间站产生的碰撞，对空间碎片等障碍物的碰撞，以及卫星相互之间的碰撞威胁，因此在空间站释放卫星的路径规划问题中，躲避障碍物是不可避免的难题及任务之一。同时，在规划的路径基础上，卫星需要进行实时在线跟踪，满足空间站复杂环境下的各种约束，以及具有良好的跟踪效果。

发明内容

针对现有技术中存在的问题，本发明提出一种基于B样条函数的卫星路径规划和预测控制的跟踪方法，利用B样条函数进行路径规划，保证曲率最小使得卫星路径尽可能的光滑连续，同时设计躲避障碍物的安全通道，以防止卫星与空间障碍物发生碰撞，并通过投影的方法得到参考状态量及参考输入量；除此之外，基于设计好的路径结合模型预测控制进行轨迹跟踪，达到燃料最优的优化性能并满足输入约束，同时建立反馈控制器，终端约束惩罚项以及终端约束集，保证了闭环系统的稳定性，使得卫星具有良好的跟踪效果，此外，由于卫星传感器探测范围的限制，整个算法过程为边路径规划边进行轨迹跟踪，增加了工程实用性。

根据本发明的一方面，提供了一种基于B样条函数的卫星路径规划和预测控制的跟踪方法，包括以下步骤：

S1：建立卫星相对空间站运动的轨道模型和误差模型；

S2：基于规划的卫星路径尽可能光滑且能够躲避空间障碍物的要求，设计卫星躲避障碍物的安全通道约束，以及基于B样条函数，设计路径规划方法，得到卫星参考路径，然后利用投影的方法解算出卫星参考路径的参考状态量及参考输入量；

S3：基于步骤S1建立的卫星的误差模型以及步骤S3得到的卫星参考路径，建立基于模型预测控制的跟踪控制器(MPC控制器)；

S4：基于步骤S3得到的卫星参考路径和步骤S4建立的跟踪控制器，设计边规划边跟踪的动态路径规划及MPC跟踪控制器，使得卫星安全抵达目标位置。

进一步地，所述步骤S1中的卫星相对空间站运动的轨道模型如下：

其中，m是卫星质量，w₀是卫星轨道角速度，满足μ是地球的重力常数，R₀是卫星与地球中心的距离，z轴负方向指向地球半径，y轴指向卫星轨道半径，x轴在轨道平面内与y轴垂直，p_x,p_y,p_z为卫星在相对坐标系中的位置，u_x,u_y,u_z为卫星推进器的推力。

卫星参考路径的参考状态量x_R及参考输入量u_R分别为:

其中，分别为卫星在x,y,z轴的参考位置，为卫星在x,y,z轴的参考速度，为卫星在x,y,z轴的参考输入量。

则状态误差x_e及输入误差u_e为：

根据卫星相对空间站运动的轨道模型可得：

则可得到卫星轨道运动的误差模型：

其中：

v_x,v_y,v_z分别为卫星在x,y,z轴的速度，为卫星在x,y,z轴的速度误差，为卫星在x,y,z轴的输入误差，为卫星在x,y,z轴的位置误差，x_e(t)为卫星的状态误差向量，u_e(t)为卫星的输入误差向量。

进一步地，所述步骤S2中设计的路径规划方法，规划目标为曲率最小且路径光滑，同时满足卫星躲避障碍物的安全通道约束，该路径规划方法为：

针对卫星路径规划过程中要求得到的轨迹尽可能的平滑且曲率连续，设计B样条算法对路径进行规划，将基于B样条函数的路径规划方法扩展到三维空间，获得卫星的参考轨迹，B样条函数表示为：

其中，λ_i(i＝0,1,...,n)为控制节点，n为控制节点总共个数，为B样条函数的基函数，阶次为k_c，是由一个非减的序列决定的，该序列第一个后最后一个节点重复k_c+1次，表示为：

其中，d在这里表示由节点矢量的非递减参数的序列。

因此，基函数的递推公式为：

其中，是为节点矢量的非递减参数，为第i个节点的矢量参数。

为了保证所设计的路径尽可能的光滑且曲率连续，所设计的路径规划方法的目标函数为：

其中，K(τ)为B样条函数的曲率，表示为：

其中，τ是样条函数的参数，r＝(C_x(λ,τ),C_y(λ,τ),C_z(λ,τ))是样条函数的参数方程，C_x(λ,τ),C_y(λ,τ),C_z(λ,τ)为样条函数在x,y,z轴的分量，进一步写为：

其中，v(τ)表示样条函数的速度，a(τ)表示样条函数的加速度。

为了使所设计的路径躲避空间障碍物，设计安全通道约束使得路径始终在安全通道内，该安全避障约束为：

其中，是安全通道，视为无障碍物区域，该安全通道约束具体可表示为：

其中，是组成安全通道的每个圆域，O_r是安全通道的中心轴线，R_d是每个圆域的半径，是样条函数在第r个圆域的函数值，

所设计的路径规划问题为：

minF(λ,τ)

通过求解上述路径规划问题，得到卫星参考路径，为了进行跟踪控制，需要得到参考状态量及参考输入量，利用投影的方法，结合卫星参考路径解算出跟踪控制中的参考状态量及参考输入量，其中，参考位置可表示为与时间相关的函数：

其中，ρ是一个大于零的常值，是样条函数在x-y平面上的投影，只与相关，

则参考速度为：

根据卫星相对空间站运动的轨道模型以及误差模型求出卫星的参考输入量：

进一步地，所述步骤S3中建立基于模型预测控制的跟踪控制器的具体过程如下：

首先，将卫星的误差模型离散化为：

其中，是初始状态，A_k,B_k为：

其中，T_s为采样周期，

为了简化计算，在预测时域N_p内，卫星的预测模型为：

其中，以及：

基于模型预测控制的轨迹跟踪控制器的目标函数为：

其中，Q,R为正定矩阵，x_e(k)为卫星的状态误差，u_e(k)为卫星的输入误差，k为当前时刻。

进一步地，所述步骤S3中的跟踪控制器包含反馈控制器、终端惩罚项、终端约束集的三个设计，分别为：

(1)反馈控制器

为了保证闭环系统的稳定性，引入反馈控制器，即：

u_e(k+i/k)＝Kx_e(k+i/k)(i＝N_p,N_p+1,…)

其中，K是反馈控制器的增益矩阵；

(2)终端惩罚项

考虑目标函数使用有限步长的预测窗N_p代替无限窗，在目标函数中引入终端项h(x_e(k+N_p))＝x_e(k+N_p)^TSx_e(k+N_p)确保目标函数不变性，此时目标函数为：

其中，S是终端惩罚项的权值矩阵，且满足：

S-(A+BK)^TS(A+BK)＝Q+K^TRK；

(3)终端约束集

通过上述(1)中的反馈控制器，设计终端约束集X_f，使得：

其中，U是输入约束集，满足U＝{u_e|u_min≤u_e(k)≤u_max}，则终端约束集为：

X_f＝{x_e:u_min≤K(A+BK)^lx_e≤u_max}(l＝N_p,N_p+1,…∞)

引入上述三个设计步骤后，跟踪问题表示为：

u_e(k+i)∈U>p-1

x_e(k+N_p)∈X_f>p-1

进一步地，所述步骤S4中设计边规划边跟踪的动态路径规划及跟踪控制算法，具体步骤为：考虑卫星传感器探测范围有限，只能得到局部信息，由此根据卫星传感器探测到的局部信息进行路径规划，得到参考路径，然后进行基于预测控制的轨迹跟踪，当跟踪到预测点不够时，开始进行下一步的路径规划及轨迹跟踪，依次向前进行，直到卫星安全到达目标位置。

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

(1)与一般的卫星路径规划问题相比，本发明将基于样条函数的路径规划方法扩展到三维空间中，建立了三维空间下曲率最小的优化目标，使得所规划的路径尽可能的光滑且满足曲率连续，实用性更强。

(2)与现有的路径规划中的避障约束相比，针对空间站的复杂结构，设计了一种躲避障碍物的安全通道约束，确保规划路径始终在安全通道内，使得卫星能够躲避空间站中的障碍物以及避免与其他卫星或空间碎片发生碰撞。

(3)本发明在解算卫星的参考状态量及参考输入量时，采用投影的方法，将其分解在二维平面中从而求解相应的状态量，进一步根据卫星模型得到参考输入量，为接下来的跟踪控制提供参考值。

(4)与现有的模型预测控制相比，本发明建立的终端约束集，考虑了终端状态的输入满足输入约束的要求，提供了终端状态的可行域，并结合终端惩罚项以及反馈控制器保证了闭环系统的稳定性；

(5)本发明考虑卫星探测范围受限的情况，设计了一种动态路径规划及跟踪控制算法，进行边规划边跟踪的动态控制，使得卫星能安全抵达目标位置完成空间任务，具有强实时性、高可靠性的优点。

附图说明

图1为一种基于B样条函数的卫星路径规划和预测控制的跟踪方法的设计流程框图。

图2为所设计的动态路径规划和跟踪控制的整体实施方案图。

图3为所设计的卫星避障的安全通道示意图。

具体实施方式

如图1所示，本发明基于B样条函数的卫星路径规划和预测控制的跟踪方法的步骤为：首先建立卫星相对空间站运动的轨道模型以及误差模型；其次设计一种躲避障碍物的安全通道约束(详细示意图见图3，圆框组成安全通道，中间实线表示满足避障约束的一条可行路径)，结合B样条函数进行路径规划；然后利用投影的方法解算出参考路径的参考状态量及参考输入量；再次设计基于模型预测控制的轨迹跟踪控制器对规划路径进行实时跟踪控制；最后根据所设计的动态路径规划及实时跟踪的算法，使卫星安全到达目标位置，其整体实施方案如图2所示。具体实施步骤如下：

第一步，建立卫星相对空间站运动的轨道模型以及误差模型：

其中，m是卫星质量，w₀是卫星轨道角速度，满足μ是地球的重力常数，R₀是卫星与地球中心的距离，z轴负方向指向地球半径，y轴指向卫星轨道半径，x轴在轨道平面内与y轴垂直，p_x,p_y,p_z为卫星在相对坐标系中的位置，u_x,u_y,u_z为卫星推进器的推力。仿真中，选取m＝10kg，μ＝3.986×10¹⁴m/s²,R₀＝6.778×10⁶m。

卫星参考路径的参考状态量x_R及参考输入量u_R分别为:

其中，分别为卫星在x,y,z轴的参考位置，为卫星在x,y,z轴的参考速度，为卫星在x,y,z轴的参考输入量。

则状态误差x_e及输入误差u_e为：

根据卫星相对空间站运动的轨道模型可得：

则可得到轨道运动的状态误差模型：

v_x,v_y,v_z分别为卫星在x,y,z轴的速度，为卫星在x,y,z轴的速度误差，为卫星在x,y,z轴的输入误差，为卫星在x,y,z轴的位置误差，x_e(t)为卫星的状态误差向量，u_e(t)为卫星的输入误差向量。

第二步，根据权利要求1所述的卫星规划的路径尽可能光滑且能够躲避空间障碍物的要求，结合B样条函数设计路径规划的方法：

针对卫星路径规划过程中要求得到的轨迹尽可能的平滑且曲率连续，设计B样条算法对路径进行规划。B样条函数具有几何不变性，局部性和连续性等优点，经常用来进行路径规划。利用基于B样条函数的方法生成参考路径时，既要保证曲率的连续性，又要躲避障碍物。考虑工程实用性，将基于样条函数的路径规划方法扩展到三维空间，获得卫星的参考轨迹。

首先，B样条函数可表示为：

其中，λ_i(i＝0,1,...,n)为控制节点，n为控制节点总共个数，为B样条函数的基函数，阶次为k_c(仿真中，选取k_c＝3)，是由一个非减的序列决定的，该序列第一个后最后一个节点重复k_c+1次，可表示为：

其中，d在这里表示由节点矢量的非递减参数的序列。

因此，基函数的递推公式为：

其中，是为节点矢量的非递减参数，为第i个节点的矢量参数。

为了保证所设计的路径尽可能的光滑且曲率连续，所设计的路径规划方法的目标函数为：

其中，K(τ)为B样条函数的曲率，表示为：

其中，τ是样条函数的参数，r＝(C_x(λ,τ),C_y(λ,τ),C_z(λ,τ))是样条函数的参数方程，C_x(λ,τ),C_y(λ,τ),C_z(λ,τ)为样条函数在x,y,z轴的分量，

可进一步写为：

其中，

v(τ)表示样条函数的速度，a(τ)表示样条函数的加速度。

为了使所设计的路径躲避空间障碍物，设计安全通道约束使得路径始终在安全通道内，从而达到避免与障碍物发生碰撞的目的，该避障约束为：

其中，是安全通道，视为无障碍物区域，该安全通道是由弯曲的圆柱组成，给卫星提供安全可行的飞行区域，该通道约束具体可表示为：

其中，是组成安全通道的每个圆域，O_r是安全通道的中心轴线，R_d是每个圆域的半径，是样条函数在第r个圆域的函数值。仿真中，选取R_d＝6m。

所设计的路径规划问题为：

minF(λ,τ)

通过求解上述规划问题，得到卫星的参考路径，为了进行跟踪控制，需要得到参考状态量及参考输入量，通过利用投影的方法，结合卫星的参考路径解算出跟踪控制中的参考状态量及参考输入量，其中，参考位置可表示为与时间相关的函数：

其中，ρ是一个大于零的常值，是样条函数在x-y平面上的投影，只与相关。仿真中，选取ρ＝1。

因此，参考速度为：

根据卫星的轨道相对运动模型求出卫星的参考输入量：

第三步，根据权利要求1所述的基于卫星的误差模型，设计基于模型预测控制的轨迹跟踪方法，包含反馈控制器、终端惩罚项、终端约束集保证闭环系统的稳定性。具体步骤为：

为了确保模型预测控制的实施，卫星的状态误差模型离散化为：

其中，是初始状态，A_k,B_k为：

其中，T_s为采样周期，仿真中，T_s＝0.1s。

为了简化计算，在预测时域N_p内，卫星的预测模型为：

其中，以及:

仿真中，选取N_p＝8。

所述基于模型预测控制的轨迹跟踪控制方法的目标函数为：

其中，Q,R为正定矩阵。仿真中，选取Q＝diag(0.5,0.5,0.5,0.5,0.5,0.5),R＝diag(0.01,0.01,0.01)。

由于该目标函数的预测窗为无限长，确保了目标函数的有效性和稳定性，然而实际中无法达到预测无限步的理想状态，因此需要采用有限步长的预测窗来构建目标函数，为了确保目标函数的完整性，以及闭环系统的稳定性，设计以下三个方案：

(1)反馈控制器

为了保证闭环系统的稳定性，引入反馈控制器，即：

u_e(k+i/k)＝Kx_e(k+i/k)(i＝N_p,N_p+1,…)

其中，K是增益矩阵。

当前k时刻的优化结果为则下一时刻进行优化时的初始值为：

仿真中，选取K＝diag(-0.01,-0.01,-0.01,-0.5,-0.5,-0.5)。

(2)终端惩罚项

考虑目标函数使用有限步长的预测窗N_p代替无限窗，可在目标函数中引入终端项h(x_e(k+N_p))＝x_e(k+N_p)^TSx_e(k+N_p)确保目标函数不变性，此时目标函数为：

其中，S是终端惩罚项的权值矩阵。终端惩罚项的权值矩阵可设计为：

S-(A+BK)^TS(A+BK)＝Q+K^TRK

仿真中，可计算得：

证明如下：选取有限窗代替无限窗时，引入终端惩罚项，保证目标函数不变性，即有：

证明当终端惩罚项的权值矩阵满足S-(A+BK)^TS(A+BK)＝Q+K^TRK时，上式成立：首先，权值矩阵关系式同时乘以x_e(k)^T和x_e(k)，有:

选取x_e(k+1)＝Ax_e(k)+Bu_e(k)，u_e(k)＝Kx_e(k),则有：

当k＝0,1,2,…时：

由于(A+BK)稳定，则则：

因此：

可以得出：

证毕。

(3)终端约束集

通过上述(1)中的反馈控制器，设计终端约束集X_f，使得：

其中，U是输入约束集，满足U＝{u_e|u_min≤u_e(k)≤u_max}，则终端约束集为：

X_f＝{x_e:u_min≤K(A+BK)^lx_e≤u_max}(l＝N_p,N_p+1,…∞)

仿真中，选取u_max＝-u_min＝0.9N。

引入上述三个设计步骤后跟踪问题可表示为：

u_e(k+i)∈U>p-1

x_e(k+N_p)∈X_f>p-1

仿真中，选取初始状态为x＝[22.75,33.26,89.28,0,0,0]^T，选取目标状态为x_f＝[59.56,40.68,96.49,0,0,0]^T。

第四步，考虑卫星传感器探测范围有限，设计一种边规划边跟踪的动态路径规划及跟踪控制算法，具体步骤为：考虑卫星传感器探测范围有限，只能得到局部信息，由此根据探测到的局部信息进行路径规划，得到参考路径，然后进行基于预测控制的轨迹跟踪，当跟踪到预测点不够时，开始进行下一步的路径规划及跟踪，依次向前进行，直到卫星安全到达目标位置。

本发明针对空间站中卫星的路径规划和轨迹跟踪问题，设计了一种动态的边规划边跟踪的实时控制算法，通过设计卫星避障的安全通道约束以及基于样条函数的路径规划方法，获得卫星安全有效的参考路径，通过投影的方法解算出参考状态量及参考输入量，通过模型预测控制的方法进行实时跟踪，实现了燃料最优且满足输入约束，设计了反馈控制器，终端惩罚项，终端约束集确保了闭环系统的稳定性和可行性。

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