方法示意圖： 控制率公式： 其中theta是當前航向角與路徑航向角之差，e為橫向誤差，v為車輛速度，lambda為控制參數。 算法步驟如下： 1. 根據當前定位結果找到路徑最鄰近點。 2. 計算該點與定位結果橫向誤差e與航線誤差theta。 3. 根據控制率公式計算出前輪 ...

首先要確定反饋量和控制量，這里反饋量用的是車和最近路徑點的橫向誤差，控制量用的是前輪轉角。 算法如下： 1. 根據當前定位結果找到路徑最鄰近點。 2. 計算該點與定位結果橫向誤差。 3. 以該誤差作為反饋測量值通過pid生成控制量即前輪轉角。 4. 將前輪轉角轉化為航向角，帶入運動模型 ...

最近在看二次規划方法，對於等式約束的二次規划問題，可以使用拉格朗日方法求解。 推導方法如《最優化理論與算法（第2版）》書上所述： 這里代碼如下（代碼中給了三個例子）： 結果如下： 圖中紅線為約束條件，曲面為待求解問題函數，紅點為問題的解，藍點為二次規划問題最小值 ...

可以根據狀態量(位置，速度，加速度)的起始和結束值列出6個方程，組成方程組解該問題。 1. 列出起始狀態： 2. 列出終止狀態： 3. 寫成矩陣形式： 求解c即可。 下面是從橫向-5米到5米的生成的路徑。 代碼如下： 結果如下： ...

二次規划： 目標函數是決策變量的二次函數，約束條件是線性函數。 二次規划標准模型： \[min\quad f=\frac{1}{2}X^THX+C^TX \] \[s.t.\begin{cases} \quad AX\leq b\\ Aeq\cdot X=beq\\ L ...

HSoptflow.m main.m 老外寫的函數，拿來研究研究。 ...

1.線性規划問題 如果目標函數和約束條件都是線性函數，則該模型稱為線性規划。 [x,f_opt,flag,c]=linprog(f,A,B,Aeq,Beq,xm,xM,x0,opt) 參數說明： X: 解 f_opt: 最優值 Flag：大於零表示求解成功，否則求解出問題 C ...

該方法也是一種路徑規划算法，不過障礙物過多的時候建立勢場可能比較耗時，而且容易陷入局部最優。 算法流程如下： 1. 對於柵格場景中每一個像素分別計算到終點的距離，距離越大，則對該像素賦值越大，結束得到引力場。 2. 對於柵格場景中每一個像素分別計算到所有障礙物的距離，距離越大，則對該像素賦值 ...