Webots機器人系統搭建課程介紹四構建機械臂控制器一Ubuntu虛擬機系統二Webots仿真軟件安裝三搭建三自由度機械臂五控制器優化與其他功能探索Ubuntu虛擬機系統VMplayerUbuntu20系統，密碼123已安裝必要軟件1）提供配置好的虛擬環境：Ubuntu虛擬機系統2）若安裝ubunt20.04非虛擬機或者自己配置虛擬機，需安裝如下安裝包：Ubuntu-20.04Webots-2023b）CmakeVscodeRos-noetics（推薦，機器人操作系統使用）Webots仿真軟件安裝推薦方案：Debian軟件包方式安裝：下載sudoaptinstall./webots_2023a-rev2_amd64.deb或者：sudogdebiwebots_2023a-rev2_amd64.debtips:

Linux下安裝完成webot軟件后需要自行下載assest文件，并將其解壓到:~/.cache/Cyberbotics/Webots/assets/搭建三自由度機械臂Webots介紹《機器人學導論》中三自由度機械臂經典模型為例進行建模控制學習搭建三自由度機械臂搭建三自由度機械臂創建工程：搭建三自由度機械臂仿真環境搭建：搭建三自由度機械臂創建機器人節點：搭建三自由度機械臂創建機器人底座搭建三自由度機械臂創建機械臂搭建三自由度機械臂創建機械臂搭建三自由度機械臂創建機械臂創建連桿的虛擬驅動電機創建第一個連桿搭建三自由度機械臂創建機械臂創建連桿組碰撞屬性設置關節軸方向與位置構建兩節機械臂通過復制創建第二個hingeJoint搭建三自由度機械臂創建機械臂搭建三自由度機械臂創建機械臂構建第三級機械臂構建機械臂控制器Cmake控制器編譯配置圖構建機械臂控制器構建機械臂控制器構建機械臂控制器控制代碼構建機械臂控制器鍵盤識別代碼構建機械臂控制器在當前目錄下創建build文件夾并打開terminal終端，通過cmake..配置，make-j編譯：構建機械臂控制器編譯生成圖構建機械臂控制器選擇編譯后控制器進行仿真補充直接打開webots工程文件在vscode中編輯代碼時有時會發現頭文件或函數行下面有紅色波浪線，表示vscode找不到文件或者函數的位置，可在vscode的插件模塊中搜索c/c++extension，點擊install安裝構建機械臂控制器在當前目錄下創建build文件夾并打開terminal終端，通過cmake..配置，make-j編譯：單腿機器人及基本運動控制方法課程介紹四搭建單腿機器人模型一單腿運動控制簡介二單腿運動學推導三基本運動控制方法五構建單腿控制器腿足機器人控制技術本節針對單腿系統介紹運動學、逆運動學、靜力學等基礎知識，以及基于這些知識的位置控制、PD控制、阻抗控制、虛擬模型控制等內容，建立起腿足關節到肢體控制的基本能力，并基于此掌握、應用并創新一些新的腿足控制。摘要關節控制肢體控制機器人控制電動氣動液壓旋轉關節直線關節腿式臂式位置控制阻抗控制力柔順控制模型控制無模型控制耦合關節逆動力學控制強化學習控制MPC控制2.1單腿運動學建模

四足機器人抽取一個單腿進行研究2.1單腿運動學建模2.1單腿運動學建模2.1單腿運動學建模L1L2L12xz2.1單腿運動學建模θ2θ1θauxL02.1單腿運動學建模2.1單腿運動學建模2.1單腿運動學建模2.2單腿運動控制方法2.2單腿運動控制方法2.2單腿運動控制方法2.2單腿運動控制方法2.3仿真實驗在Webots中搭建2自由度的單腿模型，為簡化控制與前面運動學用的模型相比去掉了側擺關節，按照順序以此實現位置控制、PD控制、位置阻抗和虛擬模型力控制實驗。仿真中潛在的有兩個導軌，水平方向x上導軌上安裝有伺服電機，維持x軸的位置；豎直導軌電機設置最大輸出力為0，可豎直方向上自由運動。模型搭建2.3仿真實驗2.3仿真實驗創建機器人軀干創建機器人hip關節2.3仿真實驗創建機器人knee關節及連桿創建機器人足底2.3仿真實驗程序框架主函數框架2.3仿真實驗2.3仿真實驗2.3仿真實驗2.3仿真實驗2.3仿真實驗四、思考與作業4.1作業：仿真中將機器人抬高0.3m，自由落地后采集足端力，分別繪制出四種方法最優參數下的足地沖擊力圖。4.2思考與探索：(1)通過本章所學控制方法，設計運動軌跡，實現仿真中機器人連續跳躍。(2)在(1)基礎上研究最小化足地沖擊力方法。2.4思考作業擴展URDF單腿機器人及基本運動控制方法腿足穩定運動控制中一個經典且被廣泛應用的方法就是Raibert在1985年寫的《leggedrobotthatbalance》中的“三段式”運動規劃與控制方法；本次課嘗試在webots中搭建書中第6章的單腿機器人跳躍模型，構建3D空間全向跳躍的控制器，深入學習書中理論并通過仿真實踐加深腿足機器人控制的理解；1.引言2.1“三段式”控制(1)Hopping：控制系統在調節機器跳躍的高度的同時實現周期循環跳躍運動。hopping是一種由身體質量、彈性腿和重力組成的振蕩運動。在腿部支撐過程中，軀干受到彈性腿的彈力能實現騰空飛行，在飛行過程中，機器人系統沿著彈道軌跡運動。在每次支撐期間，控制系統通過支腿提供垂直推力，以維持腿部振蕩并調節其振幅。(2)ForwardSpeed:控制系統的第二部分用于調節向前運行速度和加速度。這是通過在每個循環的騰空飛行時間內，將腿移動到相對于身體的一個適當的向前位置來完成的。在落地時，腳相對于身體的位置對隨后的支撐過程有很大的影響，導致機器人的加減速。后面的學習我們會知道，相對軀干的落足位置可以實現當前機器人勻速運動、加速或者減速運動。(3)Posture。控制系統的第三部分是穩定機體的俯仰角(二維平面來說，對三維空間來說就包含俯仰角和橫滾角)，使機體保持直立。如果腳和地面之間有良好的力傳遞關系，那么身體和腿之間圍繞Hip關節施加的扭矩會加速身體繞其俯仰軸運動。在每次支撐，一個執行器控制Hip關節，即可使身體恢復到直立姿勢。

這個過程總結簡化為將運動分解為(a)上下彈跳運動、(b)前進速度和(c)身體姿勢的控制。將控件劃分為這三個部分使運行更容易理解，并導致相當簡單的控制系統。軀干平衡的控制率為Hopping是一個有規律的周期運動控制，每個周期中的運動都可以歸為不同階段或者狀態，下面引入狀態機概念，狀態機調控著hoping過程的每個階段，每個階段的切換是根據當前感知數據，根據關鍵事件標志切換的。例如hopping可以劃分為：Compression、Thrust、Flight、Landing等階段，當彈簧腿的速度從負變為正即>0，則狀態機從Compression切換為Thrust，這個狀態下要采取的控制為:開始蹬腿并調整軀干姿態，完整的hopping狀態機如圖2.1狀態機中性點對稱軌跡

非對稱軌跡對于每一個向前的速度，有一個獨特的腳的位置，導致向前加速度為0，即不加速也不減速，維持恒定速度，我們稱它為中性點，記作xfo。對于Hopping無向前運動，中性點位于身體正下方，而對于非零向前速度，中性點位于身體前進方向的前方，且跑得越快，中性點越靠前2.1中性點機器人運動速度估計，其思路就是機器人腿部處于支撐相時，其足端與地面緊密接觸，假設其不會滑動，則機器人機身的運動速度與腿部的運動速度即為大小相同方向相反的量，腿上的速度可以通過運動學變換得到，變換中通過腿部安裝的關節位置傳感器測量映射為腿足末端運動速度，即可得到機器人機身的速度估計：2.1速度估計在計算前足位時，考慮了兩個因素，用實際測得的運動速度來近似地確定中性點的位置，利用前向速度與期望速度的誤差計算距離中性點的偏移量，使系統加減速，中性點和偏移量結合起來指定控制系統如何放置腳，上述表示為：2.1落足點控制根據運動學計算期望的hip角度由此可以實現軀干姿態控制

2.1姿態控制壓縮彈簧單腿簡化控制量圖2.1簡化替代模型2.1Hopping運動規劃2.1Hopping能量分析2.1Hopping能量分析2.2仿真算法2.2仿真算法3仿真實驗Webots中建立的單腿機器人模型，最上方為軀干，下面緊接著為Hip_a/a關節，負責機器人腿部左右運動，繼而為Hip_f/e關節，負責機器人腿部的前后運動，這兩個關節均為旋轉關節，在機器人支撐相時這兩個關節負責調整機器人的姿態平衡，在擺動相時調整落足位置。再下方為Knee關節，此為直線運動關節，可以上下移動，Knee_Spring關節設置為被動關節，即關節處的電機最大輸出力為0，區別于其他關節，該關節的彈性系數springConstant設置為1000，阻尼系數設置為20，即彈簧阻尼關節，模擬實際的彈簧腿3仿真實驗3仿真實驗3仿真實驗3仿真實驗編譯好代碼后進行webots仿真，選擇我們的3D_single_leg_controller控制器，點擊realtime仿真按扭，機器人開始自己建立跳躍震蕩運動，默認的參數會使得機器人正常跳躍起來，當參數不合適時候，可能無法建立有效震蕩，從而機器人會傾倒。機器人正常跳躍起來后通過鍵盤的w-s-a-d按鍵進行前-后-左-右速度調節，機器人會追蹤給定速度。調整如下參數進行仿真驗證，加深算法掌握程度：周期震蕩參數修改其中的參數*1.5，仿真驗證其正常震蕩跳躍的參數。姿態調整參數3仿真實驗修改其中RPY_Angle[0]

/

2.0和-phi_td/3.0參數，仿真驗證參數對姿態穩定性的影響。支撐擺動相參數修改以上參數，仿真驗證對機器人穩定性的影響。機器人狀體信息輸出打印足底力信息、姿態角度信息，繪制機器人完整運動的曲線圖，通過GPS信息采集機器人運動速度真值，繪制速度跟蹤曲線圖，查看機器人運動效果。3仿真實驗4思考作業4.1作業：調出機器人運動速度最快（前進方向和左右方向速度矢量疊加）的穩定運動參數，仿真并繪制機器人關節層位置、速度、扭矩以及工作空間層足端位置、足底力、運動速度跟隨曲線圖。4.2思考與探索：如何實現單腿機器人軀干變化或者負載變化下的參數自適應跳躍運動控制。四足機器人全身位姿控制四足機器人足端保持不動，實現軀干位置和姿態的運動，運動效果看似跳舞，其軀干也是實際場景中云臺效果位姿全身控制分析

位姿全身控制分析

位姿全身控制分析Webots單元模塊搭建urdf編輯Webots文件轉譯（urdf2webots）Webots模型文件調整Webots四足仿真模型模型文件標準形狀

仿真模型構建URDF是XML，它是一種文本格式。因此，您可以在任何文本編輯器中創建文件。建議使用具有良好XML支持的IDE。URDF描述方法/urdf/XML/linkurdf中的link標簽用于描述機器人某個部件(也即剛體部分)的外觀和物理屬性，比如:機器人底座、輪子、激光雷達、攝像頭...每一個部件都對應一個link,在link標簽內，可以設計該部件的形狀、尺寸、顏色、慣性矩陣、碰撞參數等一系列屬性URDF描述方法/urdf/XML/linkCollision

--->描述連桿的質量、質心位置和中心慣性屬性<inertial>

這個link關于質心坐標系的轉動慣量ixx,iyy,izz和ixy,ixz,iyz，<Origin>描述連桿質心相對于連桿坐標系的位置和方向。屬性1:xyz=x偏移y便宜z偏移屬性2:rpy=x翻滾y俯仰z偏航(單位是弧度)<mass>該link的質量urdf中的link標簽用于描述機器人某個部件(也即剛體部分)的外觀和物理屬性，比如:機器人底座、輪子、激光雷達、攝像頭...每一個部件都對應一個link,在link標簽內，可以設計該部件的形狀、尺寸、顏色、慣性矩陣、碰撞參數等一系列屬性URDF描述方法/urdf/XML/linkurdf中的link標簽用于描述機器人某個部件(也即剛體部分)的外觀和物理屬性，比如:機器人底座、輪子、激光雷達、攝像頭...每一個部件都對應一個link,在link標簽內，可以設計該部件的形狀、尺寸、顏色、慣性矩陣、碰撞參數等一系列屬性visual--->描述外觀(對應的數據是可視的)geometry設置連桿的形狀標簽1:box(盒狀)屬性:size=長(x)寬(y)高(z)標簽2:cylinder(圓柱)屬性:radius=半徑length=高度標簽3:sphere(球體)屬性:radius=半徑標簽4:mesh(為連桿添加皮膚)屬性:filename=資源路徑(格式:package://<packagename>/<path>/文件)origin設置偏移量與傾斜弧度屬性1:xyz=x偏移y便宜z偏移屬性2:rpy=x翻滾y俯仰z偏航(單位是弧度)metrial設置材料屬性(顏色)屬性:name標簽:color屬性:rgba=紅綠藍權重值與透明度URDF描述方法/urdf/XML/linkCollision

--->描述碰撞屬性geometry設置連桿的形狀標簽1:box(盒狀)屬性:size=長(x)寬(y)高(z)標簽2:cylinder(圓柱)屬性:radius=半徑length=高度標簽3:sphere(球體)屬性:radius=半徑標簽4:mesh(為連桿添加皮膚)屬性:filename=資源路徑(格式:package://<packagename>/<path>/文件)origin設置偏移量與傾斜弧度屬性1:xyz=x偏移y便宜z偏移屬性2:rpy=x翻滾y俯仰z偏航(單位是弧度)Collision碰撞屬性。Collision可能與Vision屬性不同，例如，通常使用更簡單的碰撞模型來減少計算時間。注意:同一link可以存在多個<collision>標簽實例，它們定義的幾何形狀的結合形成了link的碰撞。URDF描述方法Joint單元描述關節的運動學和動力學，也規定了關節的安全限位。URDF描述方法name(必要)指定關節名字type(必要)指定關節的類型，可以是以下其中一種:revolute-一種沿軸旋轉的鉸鏈關節，其范圍由上限和下限指定。continuous

-連續的鉸鏈關節，繞軸旋轉，沒有上限和下限。prismatic

-沿軸滑動的滑動關節，其范圍由上限值和下限值指定。fixed-這不是一個真正的關節，因為它不能移動。所有的自由度都是鎖定的。這種類型的接頭不需要指定其他屬性，如axis、dynmaics等;floating

-這個關節允許所有6個自由度的運動。planar

-這個關節允許在垂直于軸的平面上運動。URDF描述方法<origin>子連桿在父連桿坐標中的位置屬性1:xyz=x偏移y便宜z偏移屬性2:rpy=x翻滾y俯仰z偏航(單位是弧度)<parent>父link名稱<child>子link名稱<axis>(默認為(1,0,0))在關節框架中指定的關節旋轉/平移軸<calibration>(可選)關節的參考位置，用于校準關節的絕對位置。rising(可選)當關節向正方向移動時，這個參考位置將觸發上升沿。falling(可選)當關節向正方向移動時，這個參考位置將觸發下降沿。<dynamics>指定關節物理屬性的元素damping(可選)

關節阻尼值(對于移動關節，單位[N?s/m]，對于旋轉關節，單位為[N?m?s/rad])。friction(可選)關節靜摩擦值(移動關節[N]為單位，轉動關節[N?m]為單位)。<limit>一個元素可以包含以下屬性:lower(可選，默認為0)一個指定關節下限的屬性upper(可選，默認為0)指定關節上限的屬性effort(必要)關節的最大輸出力velocity(必要)關節最大關節速度URDF描述方法URDF描述方法實際用四足機器人的urdf模型文件截取URDF描述方法3.程序解析程序代碼中首先看初始變量定義：3.程序解析3.程序解析3.程序解析3.仿真效果四、思考與作業4.1思考與探索：仿真環境中搭建了一個隨機運動的云臺，將四足機器人放置在該云臺上，通過本章節學習到的知識，設計機器人控制器，使得機器人在云臺上保持姿態穩定（roll和pitch角盡量維持在0）的時間越長越好。提示：機器人不進行任何控制時，隨著云臺隨機的轉動，機器人會因摩擦力不夠而滑落或翻滾；控制器設計中姿態調整策略可以想象人或者動物在上下坡面時的姿態，即使當前沒有太多力學理論知識支撐，但從自然界中生物的行為建立仿生運動策略也是一種很好的科研方式。

四足機器人位置規劃方法常見步態運動四足機器人典型步態（1）

步行步態(Walk)四足機器人在步行步態中，至少會有三條腿處于支撐相，四條腿按照左前、右后、左后、右前的順序依次進入擺動相，如此往復。其在控制過程中，每次只有一條腿抬起，在下一條腿抬起之時擺動腿轉換為支持腿。步行步態相較于其他步態速度較低，穩定性較高。（2）

對角小跑步態(Trot)對角小跑步態是一種處于對角線上兩只腳同時運行的步態，將對角線上的兩條腿看作一組。對角小跑步態相較于其他步態可以減少一定的能量損耗，而且軀干姿態角不會出現特別劇烈的變化，因此對角小跑步態是最常用于四足機器人的步態。對角小跑步態的占空比為0.5，左前，右前，右后，左后的相位差分別為0.5，0，0.5，0。四足機器人典型步態（3）

溜蹄步態(Pace)溜蹄步態將左右兩側四條腿看為兩組，同側腿作為一組相位差為0，異側腿之間相位差為0.5。溜蹄步態相較于對角小跑步態，運動過程中有明顯的單側力使得四足機器人的起伏較大，穩定性低。因此，溜蹄步態在四足機器人中并不常用。四足機器人典型步態（4）跳躍步態(Bound)跳躍步態將前側兩條腿和后側兩條腿分別看為兩組，每一組的兩條腿運動規律基本相同。跳躍步態存在四足騰空的現象，屬于四足機器人的高動態運動步態，其前后起伏較大，因此其穩定性較差。四足機器人典型步態（5）四足跳躍步態(Pronk)四足跳躍步態將四條腿看為一組，四條腿的運動規律基本一致，也屬于四足機器人的高動態步態。四足機器人典型步態（6）

飛奔步態(Gallop)飛奔步態是四足動物運動速度最高的步態，至多只有兩只腿同時作為支撐腿，存在單腿支撐和四腿同時處于擺動相的現象，屬于不規則步態。四足機器人典型步態步態軌跡設計人走路時足端軌跡？通過研究發現哺乳動物足端的軌跡基本都是圓滑的弧，將其分解為x軸方向和z軸方向的運動軌跡如圖所示足端軌跡圖5.12（b）足端分解圖四足機器人步態解析四足機器人步態軌跡規劃四足機器人步態軌跡規劃四足機器人步態軌跡控制仿真模型構建四足機器人步態仿真Webots中建立的四足機器人模型與上一章節使用的機器人模型一樣，所以不進行過多建模內容的介紹。需要主要一點的在虛擬機中環境中仿真受到顯卡等資源進行仿真畫面卡斷嚴重時候，可切換為簡化模型的四足機器人，同時本章節介紹的控制方法并不需要過高的控制頻率，提高仿真的間隔也是可以有效提升仿真速度，比如頻率設置timestep為5，即對應于真實四足機器人中控制頻率200Hz的實際情況，在一般性能的電腦虛擬機中就可實現連續運動仿真。四足機器人步態程序解析四足機器人步態程序解析四、思考與作業4.1作業：根據提供的仿真demo中trot步態，修改為pace、bound等步態，并通過軀干姿態穩定性、運動速度、足底力等進行步態特點分析。4.2思考與探索：本節機器人步態運動中僅有前后運動，自行探索如何實現機器人的自轉運動。足底力計算R=Rotxyz('x',θ0)*Rotxyz('y',θ1)*Rotxyz('y',θ2)ans=

[c1*c2-s1*s2,

0,

c1*s2+c2*s1][c1*s0*s2+c2*s0*s1,

c0,

s0*s1*s2-c1*c2*s0][-c0*c1*s2-c0*c2*s1,

s0,

c0*c1*c2-c0*s1*s2]實際足底力=R*[fxfyfz]‘四足機器人SLIP模型控制壓縮彈簧單腿簡化控制量圖軀干姿態控制落足點控制跳躍高度控制中性點對稱軌跡非對稱軌跡上一章節介紹基于足端位置軌跡規劃的控制方法，這種控制方法簡單但機器人運動魯棒性不足，且無法對外力擾動做出調整，從而無法實現類似BigDog“踹不倒”的炫酷效果。雖然波士頓動力2010年前后就展示了四足機器人跑、跳、踹不倒等高動態特性，但作者開始讀研時國內并沒有類似的腿足控制技術，故作者讀研期間領到的第一個科研任務就是賦予四足機器人一定擾動適應能力，實現類似“踹不倒”的展示效果。本章內容以作者2016年前后針對山東大學機器人研究中心研制的SCalf-II四足機器人控制技術為引導，開始介紹基于簡化動力學模型的四足機器人控制方法，賦予四足機器人一定的柔順特性、外力擾動適應特性，實現具有一定擾動適應能力、實用的穩定對角步態控制。摘要（1）

姿態穩定性分析由于Trot步態是一種動步態，在支撐相，處于對角線上的兩只腿同時支撐軀干運動，在Trot步態中機器人軀干很容易繞其對角線旋轉，因此必須對軀干姿態進行控制。為簡化四足運動的控制算法，我們引入虛擬腿的概念。由于Trot步態以對角線上的兩條腿兩兩為一組，每組腿的運動形式相同，其髖關節作用于軀干的力也類似，因此對角線上兩條腿的運動可以等效成為一條虛擬的腿。虛擬Hip位置：質心位置：二次導數→加速度（為例）：假定足與地面之間沒有相對滑動，腿部質量為零，所有的質量均集中于軀干上，建立系統的動力學模型如下：Trot步態控制器四足機器人對角步態控制器的控制框圖仿真驗證Webots中建立的四足機器人模型如圖所示，包含一個完整模型和一個簡化模型，這里機器人以MIT的mini-cheetah尺寸參數基本一致的SDUQuad-24為模型，對應優寶特同類產品yobogo。機器人模型構建與之前章節一致，在此不在贅述，綜合考慮保真度和仿真速度條件下time_step設置為5（ms）。思考與作業4.1驗證性實驗基于上述機器人控制方式，嘗試控制機器人：全向運動，通過記錄GPS位置或者直接繪制軀干運動軌跡查看跟隨效果；修改SLIP控制參數，試驗與側向沖擊相關參數，并調出最優效果；

4.2自行探索實驗通過調參和模式調整實現仿真中四足機器人1.2m/s以上快速運動上下10°以上斜坡（自己搭建）5cm樓梯地形記錄實驗數據，繪制數據圖證明上述實驗結果。

4.3思考與探索：本節學習的SLIP控制方法優缺點，是否可應用于雙足機器人運動控制？四足機器人MPC控制MPC原理本章介紹基于模型預測的腿足機器人運動控制方法，這是一種當前綜合控制效果很好的算法，對比上一章節的VM優化方法，MPC方法在復雜地形適應上更有優勢。這里介紹的MPC是基于MIT于2018年IROS論文開源的方法，雖然這套方法距今（2023年）已經過去4年了，但仍然算是當前最主流方法，尤其是配合后面章節的WBC算法，是腿足機器人控制領域一顆耀眼的明珠。下面介紹的MPC方法保留原版MIT狀態機框架和核心運動控制算法，舍棄掉MIT自己搭建動力學仿真平臺、仿真與控制器間共享內存等繁瑣的內容，將精力全部放在機器人算法學習上，比起直接學習MIT的論文與閱讀代碼效率更高，更適合初學者入門。

（1）簡化機器人動力學建模機器人的動力學模型是十分復雜、非線性的，而模型預測控制由于需要迭代求解最優，所以很耗時，這就是之前為什么MPC方法沒有真正應用在腿足機器人運動控制上的原因。我們所說的模型預測控制根據模型是線性或者非線性可以區分為LinearMPC和NolinearMPC，相對于NMPC高維復雜的狀態模型來說LMPC當前已經有很好的求解方案，且根據線性系統理論我們可以將LMPC方法轉換為QP形式，進而通過qpOASES等開源庫求解，這是求解速度最快也是最常用的方式，當然也有直接進行LMPC求解的庫。腿足機器人運動控制器需要嚴格的高頻伺服控制周期，而MPC求解速度一般遠低于伺服周期，所以如何更快的求解出MPC是一個核心問題。為了加快MPC求解速度，這里介紹的方法采用了如下策略：首先，機器人模型簡化為質心集中于軀干，腿部無質量的單剛體模型，便于動力學更新；其次，

將非線性動力學模型進行線性化，構建線性狀態表達，從而將問題轉換為凸優化形式。以上兩個簡化可實現MPC求解速度在30ms