真空迭代制導(dǎo)定點

2023-08-04 14:52 作者:咆哮鼠啊啊啊 0人讀過 | 我要投稿

前言

介紹一下我多次探索迭代制導(dǎo)，終于成功復(fù)現(xiàn)的一個真空單級著陸的方案。這篇文章不會詳細(xì)介紹具體的數(shù)學(xué)物理原理，主要分享一下制導(dǎo)的計算流程，不保證正確，如果時間和水平允許也許會寫一個簡單的原理分析，歡迎指正

物理量

常量（初次運(yùn)行時讀取/設(shè)定/計算）

???? $%5Cmu$ ：單位 $%5Crm%7Bm%5E3%2Fs%5E2%7D$ ，星球引力常量，等于萬有引力常數(shù)乘星球質(zhì)量（ $%5Cmu%3DGM$ ）

???? $T$ ：單位 $%5Crm%7BN%7D$ ，引擎最大節(jié)流閥推力

???? $V_e$ ：單位 $%5Crm%7Bm%2Fs%7D$ ，引擎噴氣速度，數(shù)值等于比沖秒數(shù)乘9.81

???? $%5Cdot%7Bm%7D$ ：單位 $%5Crm%7Bkg%2Fs%7D$ ，引擎最大節(jié)流閥的質(zhì)量消耗速率（ $%5Cdot%7Bm%7D%3DT%2FV_e$ ）

???? $%5Cvec%7BR%7D_f%2CX_f%2CY_f%2CZ_f$ ：單位 $%5Crm%7Bm%7D$ ，設(shè)定的制導(dǎo)終端位置矢量和三分量

???? $%5Cvec%7BV%7D_f%2CV_%7Bfx%7D%2CV_%7Bfy%7D%2CV_%7Bfz%7D$ ：單位 $%5Crm%7Bm%2Fs%7D$ ，設(shè)定的制導(dǎo)終端速度矢量和三分量

???? $t_p$ ：單位 $%5Crm%7Bs%7D$ ，制導(dǎo)周期，每個制導(dǎo)周期開始時刷新一次制導(dǎo)參數(shù)，通常是迭代周期 $dt$ 的整數(shù)倍

???? $dt$ ：單位 $%5Crm%7Bs%7D$ ，迭代周期，每個迭代周期刷新一次控制量

狀態(tài)參數(shù)（每個迭代周期讀取一次）

???? $m$ ：單位 $%5Crm%7Bkg%7D$ ，瞬時質(zhì)量

???? $%5Cvec%7BR%7D%2CX%2CY%2CZ$ ：單位 $%5Crm%7Bm%7D$ ，瞬時位置矢量和三分量

???? $%5Cvec%7BV%7D%2CV_%7Bx%7D%2CV_%7By%7D%2CV_%7Bz%7D$ ：單位 $%5Crm%7Bm%2Fs%7D$ ，瞬時速度矢量和三分量

每個迭代周期更新一次的參數(shù)

中間參數(shù)：

???? $t_%7Bgo%7D$ ：總剩余飛行時間

???? $t$ ：制導(dǎo)周期內(nèi)的已飛行時間

???? $%5Cvec%7Bg%7D%2C%5Cvec%7Bg%7D_f$ ：當(dāng)前、終端引力加速度矢量和三分量（ $%5Cvec%7Bg%7D%3D-%5Cfrac%7B%5Cmu%5Cvec%7BR%7D%7D%7B%7C%7C%5Cvec%7BR%7D%7C%7C%5E3%7D$ ， $%5Cvec%7Bg%7D_f$ 同理）

???? $%5Cvec%7Bg%7D_m%2Cg_%7Bmx%7D%2Cg_%7Bmy%7D%2Cg_%7Bmz%7D$ ：平均引力加速度矢量和三分量（ $%5Cvec%7Bg%7D_m%3D%5Cfrac%7B1%7D%7B2%7D(%5Cvec%7Bg%7D_f%2B%5Cvec%7Bg%7D)$ ）

???? $%5CDelta%20%5Cvec%7BV%7D%2C%5CDelta%20V_x%2C%5CDelta%20V_y%2C%5CDelta%20V_z$ ：推力產(chǎn)生的總速度增量矢量和三分量

???? $%5Cvec%7Bu%7D%2Cu_x%2Cu_y%2Cu_z$ ：引擎產(chǎn)生的加速度矢量和三分量

控制參數(shù)：

???? $%5Cvarphi%2C%5Cpsi$ ：姿態(tài)角

???? $k$ ：節(jié)流閥

每個制導(dǎo)周期更新一次的參數(shù)

中間參數(shù)：

???? $F0%2CF1%2CF2%2CF3$ ：推力積分

制導(dǎo)參數(shù)：

???? $k_%7B%5Cvarphi1%7D%2Ck_%7B%5Cvarphi2%7D%2Ck_%7B%5Cpsi1%7D%2Ck_%7B%5Cpsi2%7D$ ：制導(dǎo)率參數(shù)

???? $%5Cvarphi_v%2C%5Cpsi_v$ ：僅滿足速度約束的姿態(tài)角

???? $%5Cvarphi_c%2C%5Cpsi_c$ ：僅滿足速度約束和YZ方向位置約束的姿態(tài)角

坐標(biāo)系與姿態(tài)角

這里默認(rèn)著陸點在軌道平面內(nèi)。坐標(biāo)系原點O在星球中心，OY指向終端位置；OX在軌道平面垂直于OY，與飛船速度同向；OZ垂直軌道平面

這里默認(rèn)火箭受到的推力方向和火箭指向方向相同。姿態(tài)角 $%5Cpsi$ 是火箭方向矢量與其在XY平面的投影的夾角，從XY平面向Z軸正半軸方向為負(fù)；姿態(tài)角 $%5Cvarphi$ 是火箭方向矢量在XY平面的投影與X軸正半軸的夾角，從X軸正半軸向Y軸正半軸方向為正。左右手系都可以用這套規(guī)則。 $%5Cvarphi%2C%5Cpsi$ 與習(xí)慣意義上的俯仰和偏航有區(qū)別，注意區(qū)分

制導(dǎo)流程

一、調(diào)整軌道，讓軌道足夠低，且軌道平面離著陸點足夠近，選擇合適的點火時機(jī)

二、初始化全部常量（星體參數(shù)、引擎參數(shù)、著陸點參數(shù)），進(jìn)入迭代循環(huán)

三、迭代

????1. 讀取火箭狀態(tài)量參數(shù)（位置、速度、質(zhì)量）

????2. 估計全程平均引力加速度 $%5Cvec%7Bg%7D_m%3D%5Cfrac%7B1%7D%7B2%7D(%5Cvec%7Bg%7D_f%2B%5Cvec%7Bg%7D)$

????3. 預(yù)估一個 $t_%7Bgo%7D$ ，使用以下兩個公式迭代至收斂，得到剩余飛行時間 $t_%7Bgo%7D$ 和推力產(chǎn)生的總速度增量矢量 $%5CDelta%20%5Cvec%7BV%7D$

???? $%5Cbegin%7Balign%7D%0A%5CDelta%5Cvec%7BV%7D%20%26%3D%20%5Cvec%7BV%7D_f-%5Cvec%7BV%7D-%5Cvec%7Bg%7D_mt_%7Bgo%7D%20%5C%5C%0At_%7Bgo%7D%20%26%3D%20%5Cfrac%7Bm%7D%7B%5Cdot%7Bm%7D%7D%5B1-%5Cexp(-%5Cfrac%7B%7C%7C%5CDelta%20%5Cvec%7BV%7D%7C%7C%7D%7BV_e%7D)%5D%0A%5Cend%7Balign%7D$

????4. 根據(jù) $t_%7Bgo%7D$ 、速度誤差 $%5CDelta%5Cvec%7BV%7D$ 、位置誤差 $%5Cvec%7BR%7D_f-%5Cvec%7BR%7D$ 判斷是否結(jié)束迭代

????5. 如果是新制導(dǎo)周期內(nèi)的第一次迭代，刷新制導(dǎo)參數(shù)，否則跳過以下刷新制導(dǎo)參數(shù)的步驟，使用所在制導(dǎo)周期第一次迭代計算的制導(dǎo)參數(shù)

????????5.1 計算推力積分

$%5Cbegin%7Balign%7D%0AF0%20%26%3D%20V_e%20%5Cln%20%5Cfrac%7B%5Ctau%7D%7B%5Ctau-t_%7Bgo%7D%7D%20%5C%5C%0AF1%20%26%3D%20%5Ctau%20F_0%20-%20V_e%20t_%7Bgo%7D%20%5C%5C%0AF2%20%26%3D%20F0%20t_%7Bgo%7D%20-%20F1%20%5C%5C%0AF3%20%26%3D%20F2%20%5Ctau%20-%20%5Cfrac%7B1%7D%7B2%7DV_e%20t_%7Bgo%7D%5E2%0A%5Cend%7Balign%7D$

????????5.2 計算 $%5Cvarphi_v%2C%5Cpsi_v$

$%5Cbegin%7Balign%7D%0A%5Cvarphi_v%20%26%3D%20%5Carctan%5Cfrac%7B%5CDelta%20V_y%7D%7B%5CDelta%20V_x%7D%20%5C%5C%0A%5Cpsi_v%20%26%3D%20-%5Carcsin%5Cfrac%7B%5CDelta%20V_z%7D%7B%7C%7C%5CDelta%5Cvec%7BV%7D%7C%7C%7D%0A%5Cend%7Balign%7D$

????????5.3 計算 $k_%7B%5Cvarphi1%7D%2Ck_%7B%5Cvarphi2%7D%2Ck_%7B%5Cpsi1%7D%2Ck_%7B%5Cpsi2%7D$

$%5Cbegin%7Balign%7D%0Ak_%7B%5Cvarphi1%7D%20%26%3D%20%5Cfrac%7BY_f-F2%5Csin(%5Cvarphi_v)%5Ccos(%5Cpsi_v)-1%2F2g_%7Bmy%7Dt_%7Bgo%7D%5E2-V_yt_%7Bgo%7D-Y%7D%7B(-F2%2B%5Cfrac%7BF3F0%7D%7BF1%7D)%5Ccos(%5Cvarphi_v)%7D%20%5C%5C%0Ak_%7B%5Cvarphi2%7D%20%26%3D%20k_%7B%5Cvarphi1%7D%5Cfrac%7BF0%7D%7BF1%7D%20%5C%5C%0Ak_%7B%5Cpsi1%7D%20%26%3D%20%5Cfrac%7BZ_f%2BF2%5Csin(%5Cpsi_v)-1%2F2g_%7Bmz%7Dt_%7Bgo%7D%5E2-V_zt_%7Bgo%7D-Z%7D%7B(F2-%5Cfrac%7BF3F0%7D%7BF1%7D)%5Ccos(%5Cpsi_v)%7D%20%5C%5C%0Ak_%7B%5Cpsi2%7D%20%26%3D%20k_%7B%5Cpsi1%7D%5Cfrac%7BF0%7D%7BF1%7D%0A%5Cend%7Balign%7D$

????6. 計算 $%5Cvarphi_c%2C%5Cpsi_c$

$%5Cbegin%7Balign%7D%0A%5Cvarphi_c%20%3D%20%5Cvarphi_v-k_%7B%5Cvarphi1%7D%2Bk_%7B%5Cvarphi2%7Dt%20%5C%5C%0A%5Cpsi_c%20%3D%20%5Cpsi_v-k_%7B%5Cpsi1%7D%2Bk_%7B%5Cpsi2%7Dt%0A%5Cend%7Balign%7D$

????7. 計算 $%5Cvec%7Bu%7D$

$%5Cbegin%7Balign%7D%0Au_x%20%26%3D%202(X_f-X-V_%7Bx%7Dt_%7Bgo%7D-%5Cfrac%7B1%7D%7B2%7Dg_%7Bmx%7Dt_%7Bgo%7D%5E2)%2Ft_%7Bgo%7D%5E2%20%5C%5C%0Au_y%20%26%20%3D%5Cfrac%7BT%7D%7Bm%7D%5Csin(%5Cvarphi_c)%5Ccos(%5Cpsi_c)%20%5C%5C%20%0Au_z%20%26%3D%20-%5Cfrac%7BT%7D%7Bm%7D%5Csin(%5Cpsi_c)%0A%5Cend%7Balign%7D$

????8. 計算控制參數(shù)

$%5Cbegin%7Balign%7D%0A%5Cvarphi%20%26%3D%20%5Carctan%5Cfrac%7Bu_y%7D%7Bu_x%7D%20%5C%5C%0A%5Cpsi%20%26%3D%20-%5Carcsin%5Cfrac%7Bu_z%7D%7B%7C%7C%5Cvec%7Bu%7D%7C%7C%7D%20%5C%5C%0Ak%20%26%3D%20%5Cfrac%7Bm%7C%7C%5Cvec%7Bu%7D%7C%7C%7D%7BT%7D%0A%5Cend%7Balign%7D$

四、結(jié)束制導(dǎo)進(jìn)入下一階段

已知問題

這里沒有考慮自轉(zhuǎn)，常量中的落點信息應(yīng)該設(shè)定落點的經(jīng)緯度和海拔高度，在估算 $t_%7Bgo%7D$ 之后計算落點坐標(biāo)，如有必要還需要根據(jù)新計算的落點坐標(biāo)重新建立坐標(biāo)系

X方向使用勻加速模型控制落點的X位置，很大程度上破壞了最優(yōu)化問題求解得到的好結(jié)果

理論上 $k_%7B%5Cvarphi1%7D%2Ck_%7B%5Cvarphi2%7D%2Ck_%7B%5Cpsi1%7D%2Ck_%7B%5Cpsi2%7D$ 都是rad或rad/s單位的小量，但如果軌道太高或者點火時間不在窗口期，可能出現(xiàn) $k_%7B%5Cvarphi1%7D%2Ck_%7B%5Cpsi1%7D$ 比 $%5Cpi$ 還大的情況