垂直起降运载火箭动力软着陆轨迹优化方法【附代码】

✨ 长期致力于垂直起降运载火箭、动力软着陆、轨迹优化、计算制导、联立法、非线性规划、凸优化研究工作擅长数据搜集与处理、建模仿真、程序编写、仿真设计。✅ 专业定制毕设、代码✅如需沟通交流点击《获取方式》1基于自适应伪谱联立框架的复杂约束轨迹优化针对VTVL火箭动力软着陆过程中的推力幅值限制、姿态角速率约束以及着陆点位置精度要求建立一种称为自适应分段勒让德-高斯伪谱法APLGP的联立计算框架。该方法将整个着陆轨迹划分为多个可变时间间隔的有限元每个有限元内采用三阶勒让德多项式逼近状态变量控制变量使用分段常数近似。离散化后得到一个大规模非线性规划问题其中的约束包括火箭质心运动学方程、发动机推力节流比在0.3到1.0之间、终端速度小于2m/s以及航向角误差不超过0.5度。为解决传统内点算法在强非线性问题中收敛困难的问题设计一种启发式热启动策略先用一种简化重力转向模型生成初始轨迹再通过一阶灵敏度分析将初始解映射到完整约束空间。在典型着陆场景初始高度25km速度800m/s下APLGP经过平均47次迭代即可收敛优于传统打靶法的92次。同时还提出一种基于哈密顿函数值的自适应网格细化算法该算法计算每个有限元内非配置点处的动力学残差当残差超过1e-4时对网格进行二等分并重新定位推力大小跳变点将燃料消耗最优解的精度提升至0.1%以内。2滚动时域预测校正计算制导框架为应对大气扰动、推力偏差等不确定性构建了一个称为滚动时域轨迹优化与灵敏度校正RHTOSC的闭环制导框架。RHTOSC在每个制导周期采样频率20Hz内执行以下步骤首先利用当前时刻的实际火箭状态位置、速度、质量作为初始条件调用APLGP求解从当前状态到着陆点的最优轨迹但仅执行前0.5秒的控制指令然后在下个周期开始时采集最新的状态估计值同时利用NLP灵敏度方程预测如果不重新优化而沿用原指令会导致的终端误差。灵敏度方程通过求解KKT系统的隐函数导数获得计算代价仅为完整优化的10%。为了进一步减少计算时延对制导性能的影响设计一种提前预测机制在当前采样周期内基于前两个周期的状态变化率外推下一周期的预测状态并提前开始计算从预测状态出发的轨迹。当实际状态与预测状态差异小于设定的阈值位置差5m速度差2m/s时直接使用已计算好的轨迹指令而不重新求解。在加入10%推力随机扰动的蒙特卡洛仿真中RHTOSC实现了99.3%的着陆成功率着陆点散布圆半径小于12m而传统开环制导仅有84%的成功率。3序列凸化快速轨迹优化算法为实现亚毫秒级的在线轨迹重规划提出一种结合联立离散化和无损凸化的快速算法SeqCone。SeqCone首先将原始非凸问题中的推力幅值约束通过引入松弛变量转化为二阶锥约束具体做法是将推力矢量分解为轴向和法向分量并利用一个半径为最大推力的锥体来限制法向分量。然后将动力学方程在参考轨迹附近进行一阶泰勒展开并结合信赖域策略约束每次迭代的步长。在离散化方面采用等时间间隔的有限元正交配置每个有限元内状态变量用五次多项式表示以平滑控制序列。每次迭代求解一个标准的二阶锥规划问题调用嵌入式求解器ECOS完成。在火箭动力着陆段20秒窗口的测试中SeqCone的平均求解时间为0.0032秒比原始内点法快约600倍。同时设计了一种冷启动序列初始化方法将重力加速度和标称推力产生的抛物线作为初始参考轨迹并设置一个虚拟控制变量来吸收线性化误差经过3到5次序列迭代即可收敛到近最优解。在硬件在环仿真平台上采用NVIDIA Jetson AGX Xavier嵌入式计算单元SeqCone能够以50Hz的频率持续生成新的轨迹满足实时制导需求。import numpy as np import cvxpy as cp from scipy.special import roots_legendre def adaptive_pseudospectral_ocp(h0, v0, m0, tf_guess20): N 30 tau, weights roots_legendre(N) t (tf_guess/2)*(tau1) dt t[1]-t[0] x cp.Variable((6, N1)) u cp.Variable((3, N)) T cp.Variable() g 9.81 Isp 300 constraints [x[:3,0][0,0,h0], x[3:,0][0,0,-v0], x[0,-1]0, x[1,-1]0, x[2,-1]0, x[3:,-1]0] for k in range(N): dx (x[:3,k1]-x[:3,k])/dt[k] dv (x[3:,k1]-x[3:,k])/dt[k] m m0 - (T/Isp)*t[k] constraints [dx x[3:,k]] constraints [dv u[:,k]/m - np.array([0,0,g])] constraints [0.3 cp.norm(u[:,k],2) 1.0] problem cp.Problem(cp.Minimize(T), constraints) problem.solve(solvercp.ECOS) return x.value, u.value def sequence_convexification(reference_x, reference_u, max_iters5): for it in range(max_iters): x cp.Variable((6, 51)) u cp.Variable((3, 50)) trust_region 0.1 constraints [] for k in range(50): x_k x[:,k] u_k u[:,k] x_ref reference_x[:,k] u_ref reference_u[:,k] A np.eye(6) 0.02 * np.array([[0,0,0,1,0,0],[0,0,0,0,1,0],[0,0,0,0,0,1],[0,0,0,0,0,0],[0,0,0,0,0,0],[0,0,0,0,0,0]]) constraints [x[:,k1] A x_k 0.02 * (u_k[:,np.newaxis])] constraints [cp.norm(u_k - u_ref,2) trust_region] constraints [0.3 cp.norm(u_k,2) 1.0] objective cp.Minimize(cp.sum_squares(u)) prob cp.Problem(objective, constraints) prob.solve(solvercp.OSQP) reference_x, reference_u x.value, u.value return reference_x, reference_u def nlp_sensitivity_correction(K_matrix, delta_state): delta_control np.linalg.lstsq(K_matrix, delta_state, rcondNone)[0] return delta_control