arduplane自主著陸的縱向控制

1、Arduplane3.4.0自主著陸縱向控制鞠涵 2022-6-226/22/20222自主著陸過(guò)程無(wú)人機(jī)著陸過(guò)程示意各階段需求各階段需求進(jìn)場(chǎng)平飛段：保持進(jìn)場(chǎng)高度與姿態(tài)穩(wěn)定下滑引導(dǎo)段：高精度高度、航跡跟蹤，保持下沉率，消耗勢(shì)能末端拉起段：滿足接地下沉率、俯仰角、速度指標(biāo)減速滑跑段：盡快減速，糾正側(cè)向偏移6/22/20223自主著陸控制模塊（參考代碼arduplane v3.4.0）各控制模塊控制?？刂颇K塊功能目標(biāo)功能目標(biāo)arduplanearduplane控制器控制器傳感器傳感器（精度）精度）制導(dǎo)制導(dǎo)解算算航向差和側(cè)偏距，給出滾轉(zhuǎn)角、高度指令A(yù)P_L1_CONTROLsetup_glide_

2、slope陀螺儀、磁羅盤、GPS（2.5m CEP）速度速度控速度V,V，達(dá)到目標(biāo)速度AP_TECS 空速傳感器（5m/s）,GPS,加速度計(jì)（16384LSB/g）縱向縱向控、q、H、H,增穩(wěn)，滿足高度、下沉率指令A(yù)P_TECSGPS、空速器、陀螺儀、激光測(cè)距器（0.025m）橫向橫向控p、?，橫向增穩(wěn)、水平保持，ROLL_CONTROLLER陀螺儀（8.75mdps）航向航向控制Ny，r，消除側(cè)滑，協(xié)調(diào)轉(zhuǎn)彎YAW_CONTROLLER陀螺儀滑跑滑跑航向航向控、r保證地面滑跑穩(wěn)定GROUND_STEERING陀螺儀、磁羅盤6/22/20224arduplane自主著陸軌跡規(guī)劃Prev_WP_

3、locnext_WP_locSink_height total_distanceSink_height =(prev_WP_loc.alt - next_WP_loc.alt)= 30-0=30mtotal_distance = get_distance(prev_WP_loc, next_WP_loc) =300m （水平距離）groundspeed = ahrs.groundspeed() =12m/s （水平速度）sink_time = total_distance / groundspeed=25ssink_rate = sink_height / sink_time=1.2m/s6/

4、22/20225arduplane自主著陸軌跡規(guī)劃aim_height = aparm.land_flare_sec *sink_rate=1.5*1.2=1.8m如果未設(shè)flare_sec,取aim_height =land_flare_alt若設(shè)有flare_sec ， 拉起高度有上限：aim_height2*land_flare_altflare_time=aim_height / SpdHgt_Controller-get_land_sinkrate() =1.8/0.25=7.2sflare_distance = groundspeed * flare_time=12*7.2=86.

5、4m設(shè)定flare_distance上限不大于total_distance的一半aim_heightflare_distance6/22/20226自主著陸軌跡規(guī)劃location_update(loc, land_bearing_cd*0.01f, -flare_distance)loc.alt += aim_height*100=1.8mland_slope = (sink_height - aim_height) / total_distanceland_projection=500mlocation_update(loc, land_bearing_cd*0.01f, land_pro

6、jection) loc.alt -= land_slope * land_projection * 100 locPrev_WP_locland_slopeland_projection6/22/20227自主著陸軌跡規(guī)劃target_altitude.offset_cm = loc.alt - prev_WP_loc.alt;float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc); 該點(diǎn)所處已經(jīng)在水平路線完成的百分比set_target_altitude_proportion(loc,

7、1.0f - land_proportion);得到該點(diǎn)目標(biāo)高度Prev_WP_loclocland_approachflareland_point6/22/20228自主著陸軌跡規(guī)劃優(yōu)點(diǎn)：該法將初始拉飄點(diǎn)后移，使緩和下滑引導(dǎo)段與末端拉起段的下沉率指令變化。安全性增加，對(duì)氣壓高度計(jì)、GPS不準(zhǔn)的狀況有利。缺點(diǎn)：著陸點(diǎn)易偏離預(yù)設(shè)點(diǎn)Prev_WP_locland_approachflareland_point6/22/20229自主著陸軌跡規(guī)劃Prev_WP_locland_approachflareland_point根據(jù)上述數(shù)據(jù)，實(shí)際尺寸下，著陸點(diǎn)距預(yù)定點(diǎn)后移23m6/22/202210軌跡規(guī)

8、劃關(guān)鍵參數(shù)參數(shù)名稱參數(shù)名稱作用作用物理意義物理意義TECS_LAND_ARSPD目標(biāo)空速下滑引導(dǎo)段與末端拉起段共用。應(yīng)選擇一個(gè)高于失速速度的值，但是又足夠低，這樣飛機(jī)才有能力降低高度并以一個(gè)合理的距離著陸LAND_FLARE_SEC預(yù)設(shè)拉平飄落時(shí)間根據(jù)實(shí)際下沉率預(yù)計(jì)過(guò)LAND_FLARE_SEC時(shí)間后會(huì)觸地，則結(jié)束下滑引導(dǎo)進(jìn)入拉平飄落階段。LAND_SINK預(yù)設(shè)下沉率在末端拉起段希望達(dá)到的下沉率。LAND_FLARE_ALT備用拉平高度優(yōu)先級(jí)低于基于LAND_FLARE_SEC計(jì)算得到的高度，詳見(jiàn)前面ppt的解釋著陸點(diǎn)及倒數(shù)第二點(diǎn)的坐標(biāo)初步給出進(jìn)場(chǎng)著陸距離、下沉率需注意與目標(biāo)速度、最終下沉率等

9、參數(shù)協(xié)調(diào)6/22/202211自主著陸的總能量控制TECS（Total Energy Control System）：從控制飛機(jī)總能量的變化與分配出發(fā),實(shí)現(xiàn)對(duì)縱向飛行軌跡控制與速度控制的解耦1 。機(jī)動(dòng)飛行過(guò)程中，總能量的變化主要是由推力的變化來(lái)控制的,升降舵的偏轉(zhuǎn)主要引起飛機(jī)俯仰力矩的變化,改變姿態(tài),對(duì)升力和阻力的影響小.在推力不變時(shí),它僅改變總能量在動(dòng)能和勢(shì)能之間的分配關(guān)系2?？偰芰靠刂频幕驹?用推力控制總能量的變化率,用升降舵構(gòu)成的俯仰姿態(tài)回路控制總能量的分配。6/22/202212自主著陸的總能量控制-油門通道開(kāi)環(huán)前饋+PID控制參數(shù)名稱參數(shù)名稱作用作用物理意義物理意義THR_DAM

10、P油門阻尼相當(dāng)于STE_err的D項(xiàng)，有利于減小速度、高度的顫動(dòng)timeConst時(shí)間常數(shù)通常放在分母，越小響應(yīng)越快rollComp滾轉(zhuǎn)補(bǔ)償補(bǔ)償滾轉(zhuǎn)時(shí)額外阻力帶來(lái)的能量損失ARSPD_FBW_MAX最大速度至少最小速度1.5倍ARSPD_FBW_MIN最小速度建議1.2倍失速速度之上關(guān)鍵參數(shù)6/22/202213自主著陸的總能量分配-升降舵通道考慮了飛行速度對(duì)舵效的影響、過(guò)載限制、滾轉(zhuǎn)角補(bǔ)償、拉平飄落之前對(duì)最小俯仰角的過(guò)渡等等6/22/202214自主著陸的總能量分配-升降舵通道參數(shù)名稱參數(shù)名稱作用作用物理意義物理意義LAND_SPDWGT升降舵速度權(quán)重范圍02，1為中性值，越大越優(yōu)先考慮空速

11、。如果想要精確的控制著陸位置，設(shè)置一個(gè)較低的值，這種情況下，飛機(jī)依然會(huì)通過(guò)油門努力控制著陸速度，但是不會(huì)試圖通過(guò)俯仰角的變化控制空速。LAND_PITCH_CD著陸最小俯仰角應(yīng)用于末端拉起段，用來(lái)防止機(jī)頭過(guò)低，導(dǎo)致前起或者螺旋槳損壞。LAND_PITCH_MAX著陸最大俯仰角用于末端拉起段，取值不大于10度，比一般飛行情況限制要嚴(yán)格，因?yàn)槔鸲嗡俣鹊?，更易失速PTCH_DAMP俯仰阻尼相當(dāng)于高度指令D項(xiàng)，有利于減小俯仰角振蕩。也可以防止拉飄發(fā)生時(shí)俯仰角的突然變化。LAND_DAMP拉平俯仰阻尼拉平段以控制下沉率為目的，此時(shí)其相當(dāng)于下沉率P項(xiàng)，取值應(yīng)比PTCH_DAMP大6/22/202215參考文獻(xiàn)1 Faleiro LF, LambregtsAA. Analysis and tuningof a-Total Ene