關於Quadratic Programming(QP)演算法和NMPC求解器(SQP)的研究

近期準備對QP、SQP進行深入總結和結尾，在對理論整理、測試和驗證的過程中，想到之前的工作和事，不由的想發表幾點想法。

一、QP問題分為好多種

1。無約束QP

2。等式約束QP

3。線性不等式約束

4。二次不等式約束QCQP

5。其他（整數或二值型別）

合理的選擇問題轉化形式，很重要。

二、不同型別的QP問題，耗時也不同

在QP最佳化求解中，最頭疼的應該就是無解和耗時。之前也有提過最佳化演算法分為問題轉化和問題求解，兩者決定了最終的耗時。

之前在MPC專案裡，使用了MPC的變形——增量MPC。根據線性系統的特性，透過對線性模型的兩個時刻相減就可以得到增量模型。它帶來的好處是可以透過限制增量值來約束原求解問題的變化率，從而減少相應的不等式約束的數量，達到減少耗時的目的。如果有過對QP耗時分析的話，就會明白減少約束數量對減少QP耗時的影響之大。

三、最佳化耗時的權衡

QP求解耗時主要有兩個——矩陣逆運算和約束，上述優化了後者，但是增量MPC的模型相對於原模型會增加，從而導致矩陣逆運算耗時的增加。

四、QCQP的應用

當前大多數開源庫都不支援二次約束二次規劃（QCQP）的問題求解，這不代表它沒有用途，在使用最佳化演算法求解繞障軌跡時可能會用到。Eigen的擴充套件庫有提供部分場景的偏導求解方法，可以用於QCQP的求解。

五、個人能寫出比開源更優秀的 QP求解庫？

在晉升第一次被問到時，我也曾質疑過自己的選擇，不過自己還是堅持下來。後來從寫QP求解，到耗時對比，再到擴充套件QP求解庫，還是很感動自己能堅持下來。

最後，還想說一點：術業有專攻，當下的osqp很流行，但是它肯定不是萬能的，它的耗時優勢大部分來源於qpoases。對於稀疏問題（規劃常見）的求解，它的耗時不一定是最少的。

對osqp和qpOASES的耗時對比如下圖

說明：

1。把原測試集中的所有不等式約束，統一轉化成Gx≤r。

2。耗時使用ctime計時。

3。osqp庫使用的是Eigen-osqp。

4。測試結果表明osqp計算是快，但是精度一般，並且，並不是osqp在所有用例中都快於qpOASES。

5。測試集覆蓋Hessian不正定的情況，實際上在規劃控制中，大部分場景都是保證Hessian是正定的（除了非線性規劃）。

6。後面有時間的話，構造或找些規控特定的測試集，進行針對性的耗時評估。

2022-02-23 補充自己寫的兩種演算法和主流的兩種演算法的計算結果。

早在2019年就開始研究nonlinear MPC，當時分了兩條路線：一條是使用開源庫，快速驗證；另一條是自己研究求解過程。

對於第一條線，迫於大環境方案和開發進度，自己一個人斷斷續續搞了一年多，最終在低速場景、低硬體平臺上跑起來了。當時，在模擬環境中完成初步調參，實車上再進行微調，在相同路徑的情況下，NMPC輸出的曲率相對於Lattice完美平滑；在初始位置存在橫向和角度偏差時，NMPC所允許的偏差遠遠大於Lattic（>67º和>1。8m，低速＜3m/s）。

第二條線，由於需要自己摸索，相對坎坷。至今雖搞清楚整個計算流程，使用開源QP或自己寫的QP進行測試，發現泛化能力極差。雖然可能與權重，引數相關，但是自己對細節的處理，肯定不如開源庫，比如非正定Hessian處理、偏導計算、積分操作、Objection的設定等等。一開始決定各個擊破，就先從偏導開始，準備使用專業的積分偏導庫——casadi。先對比積分和偏導的結果，發現自己寫的和casadi結果一模一樣，但是casadi的耗時卻是自己寫的10倍以上，官方提示生成c程式碼的話，可以提高4-10倍速度。在使用casadi的c++介面，實現程式碼生成時，要麼編譯提示該功能函式不支援程式碼生成，要麼提示該繼承類沒有該程式碼生成函式！！！搞了近一週，搞得頭疼。真是從入門到放棄！！

決定暫停這條路線了！

為了工作和生計，專心POMDP的決策開發了。

——2022-03-11

關於osqp精度的問題，在github上問了開發團隊，綜合來看，我的建議是開發初期不考慮耗時的情況下，先使用qpOASES驗證演算法開發和效能，後續再遷移到osqp。

——2022-03-25

相對於凸最佳化，Nonlinear Problem求解可算是複雜的多，面也廣的多。總結下來，自己也算是掌握了好幾種方法：¹半光滑牛頓法，²一般SQP演算法，³隨機分形搜尋。實際上主流求解非線性問題的方法就兩個：SQP和內點法（初步瞭解半光滑牛頓法和內點法是一類）。

對於第三種演算法：隨機分形搜尋演算法，當時研究和開發它的初衷是替換RRT*或A*，用於無引導線下的規劃演算法，他本質屬於全域性最佳化演算法，可以解決最佳化問題，不論是QP問題，還是非線性問題。實測中，只要迭代足夠，最終收斂於最優值。後來想想，它也可以解決多障礙物繞障的問題。由於時間有限，和C++標準庫的隨機函式有限，就不再專心優化了。

在SQP上，自己摸索和補充了好多，終究實現了全流程。在應用於路徑規劃時，發現耗時和收斂性都達不到預期，一開始以為偏導的近似存在偏差，可能影響收斂結果。於是苦心研究Eigen的偏導工具，放在SQP求解中沒發現效能提升！出於嚴謹性，對Eigen工具和近似求解進行例項對比，發現結果一模一樣，當維數增加到10以上時，eigen工具的耗時更少，除此之外，沒啥進展。

後來又抽時間查閱眾多論文，發現NMPC就是NLP的一個子集，不能用泛式求解方法求解NMPC問題。對於NMPC問題，還不算是高度非線性，不能就簡的使用近期求解，除了Hessian矩陣必須近似外，其他過程均需要手動求解，只有這樣才能保證實時性和可靠性。

接下來就是潛心研究和驗證了，等這塊完成，就徹底放棄路徑規劃的研究，專心決策演算法開發了，否則會感覺一件執意很久的事沒有善尾。