1. 賽題回顧與核心挑戰(zhàn)解析2022年的高教社杯全國(guó)大學(xué)生數(shù)學(xué)建模競(jìng)賽簡(jiǎn)稱“國(guó)賽”B題題目是“無(wú)人機(jī)遂行編隊(duì)飛行中的純方位無(wú)源定位”。這個(gè)題目一出來(lái)當(dāng)時(shí)就在我們建模圈子里引起了不小的討論。它不像一些純數(shù)據(jù)處理的題目更像是一個(gè)從實(shí)際工程問(wèn)題中抽象出來(lái)的、帶有強(qiáng)烈物理背景和軍事應(yīng)用色彩的模型。簡(jiǎn)單來(lái)說(shuō)就是給你一群無(wú)人機(jī)它們只知道彼此之間的方向角方位信息不知道距離然后要求你設(shè)計(jì)算法讓它們能僅憑這些“純方位”信息確定出自己在空間中的準(zhǔn)確位置并最終形成一個(gè)穩(wěn)定的編隊(duì)。這題的難點(diǎn)非常突出。第一信息極度匱乏。傳統(tǒng)的定位無(wú)論是GPS還是多基站測(cè)距距離信息是關(guān)鍵。而這里只有角度相當(dāng)于你蒙著眼睛只靠聽聲音判斷同伴在哪個(gè)方向但要你說(shuō)出他離你具體多遠(yuǎn)這本身就極具挑戰(zhàn)性。第二模型的高度非線性。方位角θ, φ與空間直角坐標(biāo)x, y, z之間的關(guān)系是通過(guò)反三角函數(shù)arctan聯(lián)系的這直接導(dǎo)致了觀測(cè)方程是非線性的。處理非線性系統(tǒng)無(wú)論是狀態(tài)估計(jì)還是優(yōu)化求解復(fù)雜度都遠(yuǎn)高于線性系統(tǒng)。第三編隊(duì)控制的耦合性。定位不是為了定位而定位最終目的是為了控制無(wú)人機(jī)飛到指定位置形成編隊(duì)。這里就存在一個(gè)“雞生蛋還是蛋生雞”的問(wèn)題準(zhǔn)確的定位需要好的隊(duì)形幾何而好的隊(duì)形又依賴于準(zhǔn)確的定位。如何設(shè)計(jì)一個(gè)能同時(shí)處理“估計(jì)”定位和“控制”編隊(duì)的協(xié)同算法是問(wèn)題的核心。所以評(píng)價(jià)這個(gè)題目不能只看最后誰(shuí)做出來(lái)了更要看解題團(tuán)隊(duì)是如何拆解這些核心挑戰(zhàn)的。它考察的不僅僅是數(shù)學(xué)公式的推導(dǎo)能力更是將復(fù)雜工程問(wèn)題轉(zhuǎn)化為可計(jì)算模型并設(shè)計(jì)穩(wěn)健求解策略的系統(tǒng)性思維。2. 主流解題思路的拆解與對(duì)比面對(duì)這樣一個(gè)硬核題目當(dāng)年各參賽隊(duì)伍主要演化出了幾條技術(shù)路徑。我們可以把它們看作應(yīng)對(duì)同一問(wèn)題的不同“哲學(xué)”。2.1 思路一分步求解先定位后控制這是最直觀、也最符合人類思維習(xí)慣的思路。既然問(wèn)題分兩步定位和編隊(duì)那就先解決定位再解決控制。定位階段核心是建立非線性方程組。對(duì)于第i架無(wú)人機(jī)它觀測(cè)到第j架無(wú)人機(jī)的方位角俯仰和偏航可以寫成關(guān)于兩者坐標(biāo)xi, yi, zi和xj, yj, zj的函數(shù)。所有無(wú)人機(jī)之間的觀測(cè)就構(gòu)成了一個(gè)龐大的非線性方程組系統(tǒng)。求解方法主要有非線性最小二乘法將定位問(wèn)題轉(zhuǎn)化為一個(gè)優(yōu)化問(wèn)題即尋找所有無(wú)人機(jī)的位置使得根據(jù)這些位置計(jì)算出的理論方位角與實(shí)際觀測(cè)到的方位角之間的誤差平方和最小。常用算法有Levenberg-MarquardtL-M算法它能較好地處理非線性最小二乘問(wèn)題兼具梯度下降和高斯-牛頓法的優(yōu)點(diǎn)對(duì)初始值比較魯棒。極大似然估計(jì)在假設(shè)觀測(cè)噪聲服從高斯分布的前提下最大化觀測(cè)數(shù)據(jù)的似然函數(shù)其形式與非線性最小二乘本質(zhì)相同?？刂齐A段在獲得估計(jì)出的無(wú)人機(jī)位置后設(shè)計(jì)控制律使其飛向編隊(duì)目標(biāo)位置。常用方法包括基于比例-積分-微分PID的控制、領(lǐng)航-跟隨法指定一架或幾架為領(lǐng)航者其余跟隨其相對(duì)位置、或者基于一致性協(xié)議的控制讓所有無(wú)人機(jī)的位置和速度趨于一致。注意這個(gè)思路的致命弱點(diǎn)在于“誤差傳遞”。定位階段的任何誤差無(wú)論是算法收斂到局部最優(yōu)解還是觀測(cè)噪聲被放大都會(huì)毫無(wú)保留地傳遞給控制階段。控制算法再精巧也是基于錯(cuò)誤的位置信息進(jìn)行決策可能導(dǎo)致編隊(duì)發(fā)散或振蕩。這就好比用一把刻度不準(zhǔn)的尺子去裁布無(wú)論裁縫手藝多好衣服也很難合身。2.2 思路二協(xié)同定位與控制的集成設(shè)計(jì)更高級(jí)的思路是打破“先定位后控制”的藩籬將兩者視為一個(gè)整體進(jìn)行協(xié)同設(shè)計(jì)。這類方法通常借助狀態(tài)空間模型和現(xiàn)代控制理論。模型構(gòu)建將每架無(wú)人機(jī)的狀態(tài)位置、速度定義為狀態(tài)變量將控制指令推力、姿態(tài)定義為輸入將觀測(cè)到的方位角定義為輸出。這樣整個(gè)無(wú)人機(jī)編隊(duì)系統(tǒng)就可以用一個(gè)大型的、非線性的狀態(tài)空間方程來(lái)描述。算法核心——擴(kuò)展卡爾曼濾波EKF是處理非線性系統(tǒng)狀態(tài)估計(jì)的經(jīng)典工具。它的思想是在每一個(gè)時(shí)間步對(duì)非線性模型在當(dāng)前估計(jì)值處進(jìn)行一階泰勒展開將其線性化然后應(yīng)用標(biāo)準(zhǔn)卡爾曼濾波的預(yù)測(cè)和更新步驟。在B題場(chǎng)景下EKF可以同時(shí)完成兩件事狀態(tài)估計(jì)定位利用當(dāng)前的方位角觀測(cè)更新對(duì)所有無(wú)人機(jī)位置和速度的估計(jì)。最優(yōu)控制結(jié)合估計(jì)出的狀態(tài)和編隊(duì)目標(biāo)通過(guò)線性二次型調(diào)節(jié)器等設(shè)計(jì)控制律產(chǎn)生最優(yōu)控制指令。優(yōu)勢(shì)EKF框架實(shí)現(xiàn)了定位與控制的閉環(huán)?？刂苿?dòng)作會(huì)影響無(wú)人機(jī)狀態(tài)進(jìn)而影響后續(xù)的觀測(cè)而更準(zhǔn)確的觀測(cè)又會(huì)改善狀態(tài)估計(jì)從而產(chǎn)生更好的控制。這種反饋機(jī)制使得系統(tǒng)對(duì)噪聲和初始誤差有一定的抑制能力。2.3 思路三基于圖優(yōu)化與因子圖的高級(jí)方法這是當(dāng)時(shí)一些頂尖隊(duì)伍可能嘗試的、更為前沿的思路靈感來(lái)源于機(jī)器人領(lǐng)域的同步定位與地圖構(gòu)建。圖模型表示將整個(gè)問(wèn)題建模為一個(gè)圖。圖中的節(jié)點(diǎn)Node代表需要估計(jì)的無(wú)人機(jī)位姿位置和姿態(tài)邊Edge代表約束。約束主要來(lái)自兩類一是觀測(cè)約束一架無(wú)人機(jī)觀測(cè)到另一架產(chǎn)生的方位角測(cè)量二是控制約束無(wú)人機(jī)自身的運(yùn)動(dòng)模型。因子圖優(yōu)化因子圖是表達(dá)這種聯(lián)合概率估計(jì)問(wèn)題的直觀工具。每個(gè)約束對(duì)應(yīng)一個(gè)因子Factor它定義了相關(guān)聯(lián)的節(jié)點(diǎn)變量取某個(gè)值時(shí)該約束被滿足的概率或代價(jià)。整個(gè)定位與編隊(duì)問(wèn)題就轉(zhuǎn)化為尋找一組節(jié)點(diǎn)變量的值使得所有因子的聯(lián)合概率最大或總代價(jià)最小。這通常通過(guò)高斯-牛頓法或列文伯格-馬夸爾特法等非線性優(yōu)化算法求解。優(yōu)勢(shì)與挑戰(zhàn)圖優(yōu)化方法理論上非常強(qiáng)大能夠一次性優(yōu)化所有時(shí)刻的所有狀態(tài)稱為“全平滑”精度最高。但它計(jì)算量巨大對(duì)實(shí)時(shí)性要求高的編隊(duì)控制可能構(gòu)成挑戰(zhàn)。在國(guó)賽有限的時(shí)間內(nèi)完整實(shí)現(xiàn)并調(diào)試好一個(gè)因子圖優(yōu)化框架對(duì)參賽者的編程和理論功底都是極大的考驗(yàn)。3. 關(guān)鍵模型細(xì)節(jié)與算法實(shí)現(xiàn)中的“坑”無(wú)論選擇哪條思路在具體實(shí)現(xiàn)時(shí)都會(huì)遇到幾個(gè)共性的、也是決定成敗的關(guān)鍵細(xì)節(jié)。3.1 觀測(cè)模型的精確建立方位角如何用數(shù)學(xué)公式精確表示這是所有工作的基石。假設(shè)無(wú)人機(jī)i在位置p_i (x_i, y_i, z_i)觀測(cè)到無(wú)人機(jī)j在位置p_j。那么從i指向j的向量是v_ij p_j - p_i。偏航角方位角α這是向量v_ij在水平面XY平面投影與正北方向通常取Y軸正方向的夾角。計(jì)算公式為α atan2(x_j - x_i, y_j - y_i)。這里必須使用atan2函數(shù)而非atan因?yàn)閍tan2能正確處理四個(gè)象限給出(-π, π]范圍內(nèi)的唯一角度。俯仰角β這是向量v_ij與水平面XY平面的夾角。計(jì)算公式為β arcsin((z_j - z_i) / ||v_ij||)其中||v_ij||是向量的模長(zhǎng)即距離?？狱c(diǎn)一奇異性。當(dāng)兩架無(wú)人機(jī)處于同一垂直線上時(shí)即x_i x_j且y_i y_j偏航角α的定義失效分母為零。在仿真或?qū)嶋H中需要加入微小擾動(dòng)或設(shè)計(jì)邏輯處理這種極端情況?？狱c(diǎn)二模長(zhǎng)未知導(dǎo)致的非線性。注意俯仰角公式中包含了距離||v_ij||。這正是“純方位無(wú)源”的核心困難我們觀測(cè)到β但公式中卻包含我們不知道的距離信息。這直接導(dǎo)致了觀測(cè)方程關(guān)于位置坐標(biāo)是非線性的。3.2 初始值的選取決定優(yōu)化算法的生死非線性優(yōu)化算法如L-M、EKF嚴(yán)重依賴于初始猜測(cè)值。一個(gè)糟糕的初值很可能導(dǎo)致算法收斂到錯(cuò)誤的局部極小點(diǎn)甚至發(fā)散。隨機(jī)初始化最簡(jiǎn)單但也最危險(xiǎn)。在三維空間中隨機(jī)給無(wú)人機(jī)分配初始坐標(biāo)大概率會(huì)導(dǎo)致優(yōu)化失敗?；诰嚯x的粗略估計(jì)雖然不知道精確距離但可以假設(shè)一個(gè)初始距離例如所有無(wú)人機(jī)初始都在一個(gè)半徑為R的球面上。結(jié)合觀測(cè)到的角度可以反算出一個(gè)粗略的初始坐標(biāo)。這個(gè)估計(jì)即使誤差很大也通常比完全隨機(jī)要好因?yàn)樗辽俦３至藷o(wú)人機(jī)之間的相對(duì)幾何關(guān)系。逐步初始化先選擇兩架無(wú)人機(jī)固定它們的相對(duì)位置例如設(shè)定距離為D根據(jù)它們相互觀測(cè)的角度確定一條基線。然后以這兩架為參考利用第三架無(wú)人機(jī)對(duì)前兩架的觀測(cè)通過(guò)三角測(cè)量初步確定第三架的位置以此類推逐步初始化整個(gè)編隊(duì)。這種方法更穩(wěn)健但實(shí)現(xiàn)稍復(fù)雜。個(gè)人心得在實(shí)際編程中我會(huì)先用一個(gè)簡(jiǎn)單的線性化方法比如忽略距離假設(shè)所有無(wú)人機(jī)都在單位距離上生成一套初始解作為“熱身”。然后用這套解作為正式非線性優(yōu)化的初值。同時(shí)一定要加入蒙特卡洛多次運(yùn)行的策略用不同的隨機(jī)種子生成多組初始值分別進(jìn)行優(yōu)化最后選擇目標(biāo)函數(shù)值重投影誤差最小的那組結(jié)果作為最終解。這能極大提高找到全局最優(yōu)解或一個(gè)足夠好的局部最優(yōu)解的概率。3.3 編隊(duì)隊(duì)形的描述與生成題目要求形成特定編隊(duì)如錐形、圓形。如何用數(shù)學(xué)描述一個(gè)“隊(duì)形”相對(duì)位置描述法這是最常用的方法。指定一個(gè)編隊(duì)幾何中心或領(lǐng)航無(wú)人機(jī)編號(hào)0的位置p_0。對(duì)于編隊(duì)中其他任意無(wú)人機(jī)i定義它相對(duì)于中心或領(lǐng)航者的期望位置偏移d_i。那么它的目標(biāo)位置就是p_0 d_i。d_i的集合就定義了整個(gè)隊(duì)形。例如一個(gè)水平圓形編隊(duì)若半徑為R包含N架無(wú)人機(jī)則第i架i0,...,N-1的相對(duì)偏移可以是(R * cos(2πi/N), R * sin(2πi/N), 0)?；趫D剛性的描述更理論化的方法是使用圖剛性理論。將編隊(duì)視為一個(gè)圖期望的隊(duì)形由圖中特定邊長(zhǎng)的約束來(lái)定義例如“無(wú)人機(jī)1和2之間的距離應(yīng)保持為10米”。控制的目標(biāo)就是使實(shí)際距離收斂到這些期望值。這種方法更適用于只有相對(duì)距離測(cè)量的場(chǎng)景在純方位問(wèn)題中應(yīng)用較少。實(shí)現(xiàn)細(xì)節(jié)在控制算法中我們需要計(jì)算當(dāng)前位置到目標(biāo)位置的誤差。如果使用分步法這個(gè)誤差直接就是(p_target - p_estimated)。如果使用EKF等集成方法目標(biāo)位置會(huì)作為期望狀態(tài)融入控制器設(shè)計(jì)中。這里的一個(gè)小技巧是在仿真開始時(shí)可以讓編隊(duì)中心p_0緩慢移動(dòng)或保持靜止觀察無(wú)人機(jī)是否能從隨機(jī)初始位置匯聚并保持隊(duì)形這可以很好地測(cè)試控制算法的收斂性。4. 仿真驗(yàn)證與結(jié)果分析的藝術(shù)數(shù)學(xué)建模競(jìng)賽的論文光有模型和算法不夠必須有令人信服的仿真實(shí)驗(yàn)和結(jié)果分析。對(duì)于B題仿真設(shè)計(jì)尤其重要。4.1 仿真環(huán)境搭建通常使用MATLAB或Python進(jìn)行仿真。需要模擬的關(guān)鍵環(huán)節(jié)包括無(wú)人機(jī)動(dòng)力學(xué)模型一個(gè)簡(jiǎn)化的二階積分器模型通常就足夠了即控制輸入直接對(duì)應(yīng)加速度。p u。更真實(shí)的模型可以考慮一階動(dòng)力學(xué)速度控制甚至加上簡(jiǎn)單的空氣動(dòng)力學(xué)效應(yīng)但非必需。觀測(cè)噪聲模型方位角測(cè)量不可能完美。需要添加噪聲最常用的是高斯白噪聲。例如α_measured α_true w_α其中w_α ~ N(0, σ_α^2)。噪聲標(biāo)準(zhǔn)差σ的大小直接決定了問(wèn)題的難度。在結(jié)果分析中研究算法在不同噪聲水平下的表現(xiàn)是必須的?？刂浦芷谂c觀測(cè)周期設(shè)定仿真的時(shí)間步長(zhǎng)?？刂浦芷诤陀^測(cè)周期可以相同也可以不同例如控制頻率高于觀測(cè)頻率。這會(huì)影響EKF等算法的設(shè)計(jì)。4.2 評(píng)價(jià)指標(biāo)的設(shè)計(jì)如何定量評(píng)價(jià)你的算法好壞不能只說(shuō)“形成了編隊(duì)”。需要設(shè)計(jì)可量化的指標(biāo)定位精度均方根誤差。在所有時(shí)間步計(jì)算所有無(wú)人機(jī)估計(jì)位置與其真實(shí)位置在仿真中真實(shí)位置是已知的之間的歐氏距離的均方根值。RMSE_pos sqrt( mean( ||p_estimated - p_true||^2 ) )。編隊(duì)收斂誤差當(dāng)系統(tǒng)穩(wěn)定后計(jì)算無(wú)人機(jī)實(shí)際位置與期望編隊(duì)位置之間的誤差的均方根值。這衡量了控制性能。收斂時(shí)間從隨機(jī)初始狀態(tài)到編隊(duì)誤差穩(wěn)定在某個(gè)閾值如5%的編隊(duì)尺寸以內(nèi)所需的時(shí)間。算法魯棒性改變初始位置、改變觀測(cè)噪聲強(qiáng)度、甚至隨機(jī)“丟失”部分無(wú)人機(jī)的觀測(cè)數(shù)據(jù)模擬通信中斷觀察上述指標(biāo)的變化。一個(gè)健壯的算法應(yīng)該對(duì)這些擾動(dòng)不敏感。4.3 可視化讓結(jié)果自己說(shuō)話在論文中一圖勝千言。必須精心設(shè)計(jì)圖表。三維軌跡圖展示所有無(wú)人機(jī)從初始散亂狀態(tài)逐漸匯聚并形成目標(biāo)隊(duì)形如錐形的完整三維運(yùn)動(dòng)軌跡。用不同顏色區(qū)分不同無(wú)人機(jī)用時(shí)間序列或箭頭表示運(yùn)動(dòng)方向。誤差收斂曲線繪制定位RMSE和編隊(duì)誤差隨時(shí)間變化的曲線。曲線應(yīng)平滑下降并最終趨于穩(wěn)定值?？梢栽谕粡垐D上用不同線型展示不同噪聲水平下的曲線進(jìn)行對(duì)比。隊(duì)形保持動(dòng)畫如果可能在論文中附上關(guān)鍵幀截圖甚至提供動(dòng)畫的鏈接如上傳至視頻網(wǎng)站。這能最直觀地展示算法效果。敏感性分析圖用柱狀圖或折線圖展示定位精度、收斂時(shí)間等關(guān)鍵指標(biāo)隨噪聲強(qiáng)度、無(wú)人機(jī)數(shù)量等參數(shù)變化的趨勢(shì)。4.4 一份優(yōu)秀的摘要應(yīng)該包含什么對(duì)于B題摘要必須精煉地概括你的整體思路、模型亮點(diǎn)、關(guān)鍵算法和核心結(jié)果。第一句直擊問(wèn)題本質(zhì)。“針對(duì)無(wú)人機(jī)純方位無(wú)源定位與編隊(duì)控制的強(qiáng)非線性、強(qiáng)耦合難題本文提出了……”思路概括用一兩句話說(shuō)明你是分步走還是協(xié)同設(shè)計(jì)用了什么核心模型如非線性最小二乘、EKF、因子圖。創(chuàng)新點(diǎn)/亮點(diǎn)簡(jiǎn)要提及你解決關(guān)鍵難點(diǎn)的獨(dú)特方法。例如“針對(duì)初始值敏感問(wèn)題提出了基于序貫三角測(cè)量的魯棒初始化策略”“為提升抗噪性能設(shè)計(jì)了基于漸消因子的自適應(yīng)EKF”。核心結(jié)果給出最重要的量化指標(biāo)。“仿真表明在方位角測(cè)量噪聲標(biāo)準(zhǔn)差為3度的條件下最終定位精度達(dá)到0.15米編隊(duì)形成時(shí)間小于30秒且對(duì)部分觀測(cè)丟失具有魯棒性。”最后總結(jié)方法的價(jià)值。“本文方法為解決僅有角度信息的協(xié)同定位與編隊(duì)問(wèn)題提供了一種有效方案?！?. 從解題者視角看賽題價(jià)值與備賽啟示回過(guò)頭看2022年國(guó)賽B題是一道非常經(jīng)典的“好題”。它的“好”體現(xiàn)在以下幾個(gè)方面問(wèn)題背景前沿且實(shí)際源自多智能體協(xié)同、自主導(dǎo)航等前沿領(lǐng)域有明確的工程應(yīng)用價(jià)值如無(wú)人機(jī)集群、水下機(jī)器人編隊(duì)不是憑空捏造的數(shù)學(xué)游戲。模型層次豐富問(wèn)題本身可以從淺入深地理解。新手可以嘗試建立非線性方程組并用優(yōu)化工具箱求解進(jìn)階者可以嘗試實(shí)現(xiàn)EKF高手則可以挑戰(zhàn)圖優(yōu)化等更現(xiàn)代的框架。不同水平的隊(duì)伍都能找到切入點(diǎn)但深度天差地別。綜合考察能力強(qiáng)它不僅僅考察數(shù)學(xué)幾何、優(yōu)化、概率論還考察控制理論、算法實(shí)現(xiàn)編程、仿真設(shè)計(jì)和結(jié)果分析是對(duì)學(xué)生綜合科研素養(yǎng)的一次全面檢驗(yàn)。區(qū)分度極高由于難點(diǎn)明確優(yōu)秀的隊(duì)伍可以通過(guò)在初始值處理、噪聲抑制、控制耦合等細(xì)節(jié)上的精巧設(shè)計(jì)顯著拉開與一般隊(duì)伍的差距。對(duì)于后來(lái)者從這道題中可以汲取的備賽經(jīng)驗(yàn)是夯實(shí)基礎(chǔ)非線性最小二乘、卡爾曼濾波及其擴(kuò)展形式、多智能體協(xié)同控制的基本原理必須了然于胸。不能只停留在知道名詞要能推導(dǎo)、能編程實(shí)現(xiàn)。建立工具箱思維平時(shí)積累常用算法的代碼模塊如L-M算法、EKF的預(yù)測(cè)更新步驟、PID控制器等。比賽時(shí)可以直接調(diào)用或快速修改節(jié)省大量時(shí)間。重視仿真與可視化結(jié)果分析部分往往是論文的“門面”。熟練掌握MATLAB或Python的繪圖功能尤其是三維動(dòng)態(tài)軌跡的繪制。學(xué)會(huì)設(shè)計(jì)有說(shuō)服力的對(duì)比實(shí)驗(yàn)和敏感性分析。培養(yǎng)系統(tǒng)思維不要孤立地看待問(wèn)題中的各個(gè)部分。像B題必須思考定位誤差如何影響控制控制動(dòng)作又如何反作用于定位精度。建立整個(gè)系統(tǒng)的閉環(huán)思維模型是解決這類耦合問(wèn)題的關(guān)鍵。這道題也反映了一個(gè)趨勢(shì)數(shù)學(xué)建模競(jìng)賽越來(lái)越傾向于選擇這種“硬核”的、跨學(xué)科的、帶有明確應(yīng)用背景的題目。它要求參賽者不僅要有扎實(shí)的數(shù)學(xué)功底還要具備一定的工程思維和解決復(fù)雜系統(tǒng)問(wèn)題的能力。能夠成功挑戰(zhàn)此類題目的隊(duì)伍其收獲絕不僅僅是一張獎(jiǎng)狀更是一段接近真實(shí)科研過(guò)程的寶貴歷練。