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

权利要求书

1.一种基于B样条函数的卫星路径规划和预测控制的跟踪方法，其特征在于，包括以下步骤：

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

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

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

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

所述步骤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 i＝0,1,…N_p-1

x_e(k+N_p)∈X_f i＝0,1,…N_p-1；

所述步骤S4中设计边规划边跟踪的动态路径规划及跟踪控制算法，具体步骤为：

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