1.翼伞系统避障航迹规划方法,其特征在于,包括:S1、建立风固定坐标系下的翼伞降阶质点模型:
其中(x,y,h)分别为翼伞系统在风固定坐标系中水平面x方向、y方向和垂直方向的位置分量,vs为翼伞系统水平方向速度,vz为垂直方向速度,ψ为航向角, 为航向角速率,u为与翼伞非对称下偏量对应的控制量;
建立翼伞飞行区域山峰障碍的模型:
其中h(x,y)为山峰障碍在水平面坐标(x,y)处的高度值;m为翼伞飞行区域山峰障碍的数量,hi、(x0i,y0i)分别为第i个山峰障碍的峰顶高度和中心坐标,xsi和ysi分别为第i个山峰障碍沿x轴和y轴方向的坡度相关量;
S2、确定翼伞系统在空投初始时刻t0的位置(x0,y0,h0)和航向角ψ0、翼伞系统在着陆时刻tf的期望位置(xf,yf,hf)和期望航向角ψf、翼伞操纵绳的最大下拉量umax;
S3、建立翼伞系统航迹规划的目标函数:J=f1J1+f2J2+f3J3+f4J4;
2 2
其中J1=(x(tf)-xf)+(y(tf)-yf) ,为着陆点水平位置误差指标;J2=cos(ψ(tf))+1,为着陆点航向误差指标; 为控制能耗指标; 为安全避障指标;f1、f2、f3和f4为加权因子;x(tf)、y(tf)、ψ(tf)分别为规划航迹上翼伞在着陆时刻的水平面x方向、y方向的坐标和航向角; 其中di(t)为t时刻翼伞距第i个山峰障碍表面的距离;Rsafe为预设的翼伞距山峰障碍表面的安全间距;
S4、将翼伞飞行时段[t0,tf]划分为n个相邻区间,每个子区间内u(t)取常数值以控制翼伞系统,即:其中: 将u(t)表示为序列[σk],k=1,…,n;
S5、求解翼伞飞行时段每个区间的最优控制量σk,使目标函数值J取最小值,得到最优控制量序列S6、根据最优控制量序列和翼伞的初始状态和速度,推导出规划的航迹path(t)=(x(t),y(t),h(t)),t∈[t0,tf];
其中: h(t)=h0+vzt,
2.根据权利要求1所述的翼伞系统避障航迹规划方法,其特征在于,翼伞飞行区域第i个山峰障碍沿x轴和y轴方向的坡度相同,xsi=ysi,第i座山峰障碍在高度H处的半径
3.根据权利要求1所述的翼伞系统避障航迹规划方法,其特征在于,所述步骤S5中采用梯度下降法求解翼伞飞行时段的最优控制量序列,具体包括:S51、设置学习率λ,容许误差e,迭代次数最大值L,控制量步长△σk;在每个子区间控制量的上下界内随机初始化控制量序列为 根据 计算目标函数的初始值J0;初始化迭代次数l=0,初始化最优目标函数值为正无穷大,初始化最优控制量序列 为 k为子区间序号,k=1,…,n;;
S52、对当前控制量序列做微调:
根据微调后的控制量序列 计算目标函数值J′l,计算当前迭代中的负梯度:更新每个子区间的控制量
如果 则令 如果 则令
根据更新后的控制量序列 计算目标函数值Jl+1;
如果Jl+1小于最优目标函数值,则将最优目标函数值更新为Jl+1,最优控制量序列 更新为S53、判断是否达到迭代结束条件,所述迭代结束条件为:当前迭代次数l≥L或目标函数值变化量|Jl+1-Jl|
4.根据权利要求3所述的翼伞系统避障航迹规划方法,其特征在于,根据控制量序列[σk]计算目标函数值J的步骤为:计算翼伞飞行时长:
根据质点模型得到规划航迹上翼伞系统着陆时刻的航向角和水平面位置:其中
计算着陆点水平位置误差指标J1:J1=(x(tf)-xf)2+(y(tf)-yf)2;
计算着陆点航向误差指标J2:J2=cos(ψ(tf))+1;
根据翼伞飞行时段[t0,tf]内翼伞的控制量u(t),计算控制能耗指标J3:计算翼伞在飞行时段内的安全避障指标J4:
其中 di(t)为t时刻翼伞距第i个山峰障碍表面的距离;Rsafe为预设的翼伞距山峰障碍表面的安全间距;
计算目标函数值J=f1J1+f2J2+f3J3+f4J4。
5.翼伞系统避障航迹规划系统,其特征在于,包括:翼伞降价质点模型建立模块(1),用于建立风固定坐标系下的翼伞降阶质点模型:其中(x,y,h)分别为翼伞系统在风固定坐标系中水平面x方向、y方向和垂直方向的位置分量,vs为翼伞系统水平方向速度,vz为垂直方向速度,ψ为航向角, 为航向角速率,u为与翼伞非对称下偏量对应的控制量;
山峰障碍模型建立模块(2),用于建立翼伞飞行区域山峰障碍模型:其中h(x,y)为山峰障碍在水平面坐标(x,y)处的山峰障碍高度值;m为翼伞飞行区域山峰障碍的数量,hi、(x0i,y0i)分别为第i个山峰障碍的峰顶高度和中心坐标,xsi和ysi分别为第i个山峰障碍沿x轴和y轴方向的坡度相关量;
翼伞系统初始状态和着陆期望状态确定模块(3),用于确定翼伞系统在空投初始时刻t0的位置(x0,y0,h0)和航向角ψ0、翼伞系统在着陆时刻tf的期望位置(xf,yf,hf)和期望航向角ψf、翼伞操纵绳的最大下拉量umax;
目标函数建立模块(4),用于建立翼伞系统航迹规划的目标函数:J=f1J1+f2J2+f3J3+f4J4;
其中J1=(x(tf)-xf)2+(y(tf)-yf)2,为着陆点水平位置误差指标;J2=cos(ψ(tf))+1,为着陆点航向误差指标; 为控制能耗指标; 为安全避障指标;f1、f2、f3和f4为加权因子;x(tf)、y(tf)、ψ(tf)分别为规划航迹上翼伞在着陆时刻的水平面x方向、y方向的坐标和航向角; 其中di(t)为t时刻翼伞距第i个山峰障碍表面的距离;Rsafe为预设的翼伞距山峰障碍表面的安全间距;
飞行时段区间划分模块(5),用于将翼伞飞行时段[t0,tf]划分为n个相邻区间,每个子区间内u(t)取常数值以控制翼伞系统,即:其中: 将u(t)表示为序列[σk],k=1,…,n;
最优控制量计算模块(6),用于求解翼伞飞行时段每个区间的最优控制量σk,使目标函数值J取最小值,得到最优控制量序列规划航迹推导模块(7),用于根据最优控制量序列和翼伞的初始状态和速度,推导出规划的航迹path(t)=(x(t),y(t),h(t)),t∈[t0,tf]。
6.根据权利要求5所述的翼伞系统避障航迹规划系统,其特征在于,所述最优控制量计算模块(6)采用梯度下降法求解翼伞飞行时段的最优控制量序列,具体包括:S51、设置学习率λ,容许误差e,迭代次数最大值L,控制量步长△σk;在每个子区间控制量的上下界内随机初始化控制量序列为 根据 计算目标函数的初始值J0;初始化迭代次数l=0,初始化最优目标函数值为正无穷大,初始化最优控制量序列 为 k为子区间序号,k=1,…,n;;
S52、对当前控制量序列做微调:
根据微调后的控制量序列 计算目标函数值J′l,计算当前迭代中的负梯度:更新每个子区间的控制量
如果 则令 如果 则令
根据更新后的控制量序列 计算目标函数值Jl+1;
如果Jl+1小于最优目标函数值,则将最优目标函数值更新为Jl+1,最优控制量序列 更新为S53、判断是否达到迭代结束条件,所述迭代结束条件为:当前迭代次数l≥L 或目标函数值变化量|Jl+1-Jl|
7.根据权利要求6所述的翼伞系统避障航迹规划系统,其特征在于,所述最优控制量计算模块(6)中根据控制量序列[σk]计算目标函数值J的步骤为:计算翼伞飞行时长:
根据质点模型得到规划航迹上翼伞系统着陆时刻的航向角和水平面位置:其中
2 2
计算着陆点水平位置误差指标J1:J1=(x(tf)-xf) +(y(tf)-yf) ;
计算着陆点航向误差指标J2:J2=cos(ψ(tf))+1;
根据翼伞飞行时段[t0,tf]内翼伞的控制量u(t),计算控制能耗指标J3:计算翼伞在飞行时段内的安全避障指标J4:
其中 di(t)为t时刻翼伞距第i个山峰障碍表面的距离;Rsafe为预设的翼伞距山峰障碍表面的安全间距;
计算目标函数值J=f1J1+f2J2+f3J3+f4J4。