現(xiàn)：路徑規(guī)劃平滑軌跡實(shí)戰(zhàn)指南)
搞機(jī)器人路徑規(guī)劃的人大概率都有過(guò)這種經(jīng)歷全局規(guī)劃給出一條直線折線車走到拐點(diǎn)的時(shí)候不是急停就是急轉(zhuǎn)看著都心疼。更麻煩的是把軌跡交給運(yùn)動(dòng)控制器時(shí)速度和加速度在這里直接斷掉不加平滑根本跑不起來(lái)。我最早接觸貝塞爾曲線就是為了解決這類問(wèn)題。后來(lái)在避障小車、機(jī)械臂軌跡、噴涂路徑規(guī)劃這些場(chǎng)景里貝塞爾曲線幾乎成了默認(rèn)的平滑方案。這篇文章把貝塞爾曲線的原理、Matlab實(shí)現(xiàn)、路徑規(guī)劃里的實(shí)際應(yīng)用一次性講透代碼直接給可跑通版本。適合剛接觸路徑規(guī)劃的初學(xué)者也適合想快速在Matlab里出結(jié)果的工程師。1. 貝塞爾曲線的數(shù)學(xué)本質(zhì)與圖形直覺(jué)1.1 從一根橡皮筋說(shuō)起什么是貝塞爾曲線如果只給一個(gè)控制點(diǎn)貝塞爾曲線就是那個(gè)點(diǎn)本身。給兩個(gè)控制點(diǎn)它就是一條直線段。麻煩的是三個(gè)及以上的控制點(diǎn)這時(shí)候曲線怎么走你可以在心里想象一塊木板上釘了幾顆釘子然后把一根橡皮筋繞過(guò)這些釘子。橡皮筋不會(huì)緊貼每顆釘子而是被它們共同“拉”著形成一個(gè)光滑的弧線。貝塞爾曲線就是這根橡皮筋的數(shù)學(xué)表達(dá)。它最反直覺(jué)的地方在于除了第一個(gè)和最后一個(gè)控制點(diǎn)曲線上絕大多數(shù)點(diǎn)都不經(jīng)過(guò)中間控制點(diǎn)。中間控制點(diǎn)只是“吸引”曲線向自己的方向彎曲距離越近吸引越強(qiáng)。這跟很多初學(xué)者想的“用曲線把點(diǎn)連起來(lái)”完全不是一回事但恰恰是這個(gè)特性讓貝塞爾曲線在路徑平滑中特別有用——你不需要讓軌跡精確穿過(guò)每一個(gè)“想法點(diǎn)”只需要讓軌跡被這些點(diǎn)引導(dǎo)就能得到一條安全、平滑、可執(zhí)行的路徑。貝塞爾曲線的數(shù)學(xué)定義是用Bernstein基函數(shù)對(duì)控制點(diǎn)做加權(quán)平均階數(shù)等于控制點(diǎn)數(shù)量減一。如果有n1個(gè)控制點(diǎn)曲線方程長(zhǎng)這樣P(u) Σ (i0到n) B(i,n)(u) * P(i)u ∈ [0,1]其中B(i,n)(u) C(n,i) * u^i * (1-u)^(n-i)C(n,i)是組合數(shù)。這個(gè)式子看著唬人但拆開(kāi)看就是一個(gè)加權(quán)平均的推廣每個(gè)控制點(diǎn)乘以一個(gè)權(quán)重所有權(quán)重加起來(lái)恒等于1。u從0走到1的過(guò)程中權(quán)重分配不斷變化曲線就從起點(diǎn)平滑移動(dòng)到終點(diǎn)。1.2 德卡斯特里奧幾何構(gòu)造法公式歸公式真要直觀理解貝塞爾曲線的形狀變化我推薦德卡斯特里奧算法。這個(gè)算法把曲線生成變成了一組反復(fù)的線性插值手工都能算。以三個(gè)控制點(diǎn)P0、P1、P2為例生成一條二次貝塞爾曲線。先取參數(shù)u在線段P0-P1上按比例u找點(diǎn)A滿足A (1-u)P0 uP1在線段P1-P2上同樣找點(diǎn)B滿足B (1-u)P1 uP2。然后把A和B再按比例u連一次得到點(diǎn)C (1-u)A uB。讓u從0到1遍歷一遍點(diǎn)C的軌跡就是二次貝塞爾曲線。三個(gè)控制點(diǎn)做兩次線性插值四個(gè)控制點(diǎn)就做三次線性插值n1個(gè)控制點(diǎn)做n輪插值。這就是德卡斯特里奧算法的核心重復(fù)線性插值直到只剩一個(gè)點(diǎn)。這個(gè)算法最大的價(jià)值在于它的數(shù)值穩(wěn)定性。直接按Bernstein基函數(shù)公式計(jì)算當(dāng)階數(shù)比較高時(shí)u^i和(1-u)^(n-i)的數(shù)值可能過(guò)大或過(guò)小組合數(shù)也可能非常大計(jì)算機(jī)算起來(lái)容易丟精度。德卡斯特里奧算法只涉及乘法和加法避免了組合數(shù)的大數(shù)計(jì)算在工程里更穩(wěn)。我在教學(xué)和項(xiàng)目調(diào)試?yán)锪?xí)慣先用德卡斯特里奧算法的思路推導(dǎo)再用Bernstein公式做向量化實(shí)現(xiàn)這樣既理解幾何意義又拿到運(yùn)行效率。1.3 貝塞爾曲線的幾個(gè)手算樣例直接看一個(gè)具體例子。假設(shè)控制點(diǎn)P0(0,0)P1(2,4)P2(4,0)求這條二次貝塞爾曲線在u0.5處的點(diǎn)。第一步A 0.5P0 0.5P1 (1,2)。第二步B 0.5P1 0.5P2 (3,2)。第三步C 0.5A 0.5B (2,2)。所以曲線上u0.5處的點(diǎn)是(2,2)正好是拋物線頂點(diǎn)。這個(gè)結(jié)果也能從公式驗(yàn)證二次貝塞爾曲線公式展開(kāi)后是P(u) (1-u)2P0 2u(1-u)P1 u2P2帶u0.5進(jìn)去得到0.25P0 0.5P1 0.25P2 (2,2)。兩條路結(jié)果一致。這種手算的價(jià)值在于建立直覺(jué)參數(shù)u是比例的概念不是時(shí)間的概念。曲線上的點(diǎn)始終是對(duì)控制點(diǎn)的調(diào)和平均權(quán)重由當(dāng)前u決定。后面講路徑規(guī)劃中速度控制的時(shí)候這個(gè)區(qū)別會(huì)非常重要。2. 貝塞爾曲線在路徑規(guī)劃中的工程價(jià)值2.1 為什么不用折線和普通多項(xiàng)式擬合路徑規(guī)劃里最樸素的方案是折線連接。機(jī)器人先走到A點(diǎn)再走到B點(diǎn)再走到C點(diǎn)。缺點(diǎn)很明顯每個(gè)轉(zhuǎn)折點(diǎn)處方向突變?nèi)绻麢C(jī)器人是差速底盤這里必須減速到接近零再轉(zhuǎn)向效率極低如果是機(jī)械臂這里會(huì)帶來(lái)加速度沖擊機(jī)構(gòu)振動(dòng)大、壽命縮短。那用高次多項(xiàng)式擬合呢比如讓曲線精確經(jīng)過(guò)所有路徑點(diǎn)。問(wèn)題更麻煩。高次多項(xiàng)式對(duì)單個(gè)點(diǎn)的擾動(dòng)極其敏感一個(gè)噪聲點(diǎn)就能讓曲線在遠(yuǎn)離該點(diǎn)的地方劇烈振蕩這就是所謂的Runge現(xiàn)象?？刂泣c(diǎn)數(shù)量一多多項(xiàng)式階數(shù)水漲船高曲線形態(tài)基本不可控。貝塞爾曲線恰好在這兩者之間找到了平衡。它的階數(shù)雖然也是由控制點(diǎn)數(shù)量決定但曲線的形狀被凸包性質(zhì)緊緊約束住不會(huì)像高次多項(xiàng)式那樣失控。對(duì)于路徑規(guī)劃這種“我需要一條光滑且不會(huì)跑偏太遠(yuǎn)的曲線”的需求貝塞爾曲線是非常自然的候選。2.2 貝塞爾曲線的四個(gè)關(guān)鍵幾何性質(zhì)端點(diǎn)插值性。曲線的起點(diǎn)一定在第一個(gè)控制點(diǎn)P0上終點(diǎn)一定在最后一個(gè)控制點(diǎn)Pn上。這意味著路徑規(guī)劃的起止位置約束可以直接通過(guò)第一個(gè)和最后一個(gè)控制點(diǎn)精確滿足非常方便。端點(diǎn)切線方向。曲線在起點(diǎn)的切線方向由P0指向P1在終點(diǎn)的切線方向由P(n-1)指向Pn。這個(gè)性質(zhì)在泊車路徑規(guī)劃里是寶貝因?yàn)椴窜噷?duì)起點(diǎn)和終點(diǎn)的航向角有嚴(yán)格要求。只要控制點(diǎn)選在車輛朝向的延長(zhǎng)線上曲線出入方向就完全可控。凸包性。整條曲線完全落在所有控制點(diǎn)構(gòu)成的凸包內(nèi)。這個(gè)性質(zhì)在避障里極其重要只要把控制點(diǎn)選在障礙物之外曲線就絕對(duì)不可能穿過(guò)障礙物內(nèi)部。對(duì)于動(dòng)態(tài)避障小車規(guī)劃時(shí)只需要檢查凸包是否與障礙物碰撞計(jì)算量遠(yuǎn)小于逐點(diǎn)檢查。變差縮減性。曲線與任意直線的交點(diǎn)數(shù)量不超過(guò)控制點(diǎn)折線與該直線的交點(diǎn)數(shù)量。翻譯成人話就是曲線比控制點(diǎn)折線更“平穩(wěn)”不會(huì)出現(xiàn)比控制點(diǎn)更劇烈的擺動(dòng)。這個(gè)性質(zhì)讓貝塞爾曲線天然適合生成平滑軌跡不會(huì)自己“抽風(fēng)”。2.3 一個(gè)容易忽略的問(wèn)題參數(shù)速度不等于實(shí)際速度這是貝塞爾曲線在實(shí)際工程中最容易踩的坑。參數(shù)u從0均勻增長(zhǎng)到1對(duì)應(yīng)的曲線點(diǎn)并不是勻速運(yùn)動(dòng)的。因?yàn)榍€上點(diǎn)的實(shí)際運(yùn)動(dòng)速度是|dP/du| * (du/dt)而|dP/du|在曲線不同位置不恒定即使u是線性變化的實(shí)際速度也在改變。舉個(gè)例子四個(gè)控制點(diǎn)(0,0)、(0,10)、(10,10)、(10,0)構(gòu)成的三次貝塞爾曲線u從0到0.5時(shí)走過(guò)的弧長(zhǎng)和u從0.5到1時(shí)走過(guò)的弧長(zhǎng)并不相等。在噴涂路徑規(guī)劃里如果直接把u當(dāng)作時(shí)間噴槍在工件表面移動(dòng)速度就不均勻涂層厚度會(huì)忽厚忽薄在焊接里會(huì)導(dǎo)致焊縫質(zhì)量不一致。解決方案是對(duì)曲線做弧長(zhǎng)參數(shù)化先把曲線按弧長(zhǎng)離散采樣再建立弧長(zhǎng)s到參數(shù)u的映射關(guān)系最后按期望速度控制s的變化。細(xì)節(jié)我放到第4章講這里先記住結(jié)論u均勻不等于速度均勻涉及運(yùn)動(dòng)控制時(shí)必須顯式處理弧長(zhǎng)與速度的關(guān)系。3. Matlab實(shí)現(xiàn)與路徑規(guī)劃實(shí)戰(zhàn)3.1 核心函數(shù)基于Bernstein的向量化實(shí)現(xiàn)Matlab適合向量化編程。直接用for循環(huán)遍歷每個(gè)采樣點(diǎn)算基函數(shù)寫法直觀但效率差采樣點(diǎn)一多就慢。這里給出一個(gè)向量化實(shí)現(xiàn)核心是利用linspace生成u向量再用矩陣運(yùn)算同時(shí)計(jì)算所有采樣點(diǎn)。function curve bezier_curve(P, res) % 生成貝塞爾曲線 % 輸入: % P - 控制點(diǎn)矩陣大小為(n1) x dim每行一個(gè)控制點(diǎn) % res - 采樣點(diǎn)數(shù)即曲線上生成多少個(gè)點(diǎn) % 輸出: % curve - res x dim 的密集曲線點(diǎn)集 n size(P, 1) - 1; dim size(P, 2); u linspace(0, 1, res); curve zeros(res, dim); for i 0:n % Bernstein基函數(shù)向量化計(jì)算 coeff nchoosek(n, i) .* (u.^i) .* ((1 - u).^(n - i)); % 每個(gè)控制點(diǎn)按權(quán)重疊加 curve curve coeff * P(i 1, :); end endnchoosek(n, i)計(jì)算組合數(shù)。u是res×1的列向量u.^i得到每個(gè)采樣點(diǎn)的u冪次(1-u).^(n-i)得到另一部分兩者逐元素相乘后再乘以組合數(shù)得到res×1的權(quán)重向量。coeff * P(i1,:)是res×1乘1×dim的矩陣乘法結(jié)果是res×dim等價(jià)于把每個(gè)點(diǎn)都平移到控制點(diǎn)的位置并按權(quán)重縮放。實(shí)測(cè)下來(lái)100個(gè)控制點(diǎn)、5000個(gè)采樣點(diǎn)的情況下這個(gè)函數(shù)在普通筆記本上十幾毫秒跑完夠?qū)崟r(shí)使用的。如果控制點(diǎn)特別多可以用德卡斯特里奧算法的遞推實(shí)現(xiàn)避免組合數(shù)溢出但一般路徑規(guī)劃控制點(diǎn)不會(huì)超過(guò)十幾個(gè)這個(gè)版本夠用。3.2 完整示例小車平滑繞障路徑生成假設(shè)一臺(tái)小車要從起點(diǎn)(0,0)走到終點(diǎn)(10,0)中間遇到一個(gè)障礙物需要繞過(guò)去。傳統(tǒng)做法規(guī)劃出折線路徑(0,0)-(3,1)-(7,1)-(10,0)。轉(zhuǎn)折點(diǎn)處方向突變直接跟蹤很難受。用貝塞爾曲線把這條折線平滑一下% 控制點(diǎn): 起點(diǎn)、兩個(gè)繞障點(diǎn)、終點(diǎn) P [0, 0; 3, 1; 7, 1; 10, 0]; % 采樣生成曲線 res 200; curve bezier_curve(P, res); % 繪圖對(duì)比 figure; plot(P(:,1), P(:,2), k--o, LineWidth, 1.5); hold on; plot(curve(:,1), curve(:,2), b-, LineWidth, 2.5); legend(控制點(diǎn)折線, 貝塞爾曲線, Location, southeast); title(四控制點(diǎn)Bezier曲線路徑平滑); axis equal; grid on;跑完之后你會(huì)看到曲線從(0,0)出發(fā)平滑繞過(guò)中間兩個(gè)控制點(diǎn)附近最后精確落在(10,0)上。中間兩個(gè)控制點(diǎn)像磁極一樣把曲線“吸”過(guò)去但曲線并沒(méi)有真正觸碰到它們。這個(gè)例子里最核心的認(rèn)知是把路徑規(guī)劃的折點(diǎn)直接當(dāng)作控制點(diǎn)使用不一定能得到理想曲線。想讓曲線更貼近或更遠(yuǎn)離折線需要主動(dòng)調(diào)整控制點(diǎn)位置。工程上一般會(huì)從折線頂點(diǎn)沿角平分線方向偏移一定距離偏移量決定曲線的“切彎”程度。這個(gè)調(diào)參思路在第4章展開(kāi)。3.3 動(dòng)態(tài)避障場(chǎng)景的貝塞爾生成思路動(dòng)態(tài)避障小車的局部路徑規(guī)劃里貝塞爾曲線也能派上大用場(chǎng)。思路是在每個(gè)控制周期內(nèi)根據(jù)當(dāng)前車輛位置、目標(biāo)路徑上的前瞻點(diǎn)、障礙物位置動(dòng)態(tài)生成一組貝塞爾控制點(diǎn)實(shí)時(shí)計(jì)算局部避障軌跡。具體做法當(dāng)前車輛位置作為P0車輛航向方向前方1米處作為P1保證軌跡起點(diǎn)方向與當(dāng)前航向一致。目標(biāo)路徑上的一個(gè)前瞻點(diǎn)作為P3保證軌跡終點(diǎn)方向符合全局路徑趨勢(shì)。P2放在障礙物外側(cè)的安全距離上讓曲線被“推開(kāi)”遠(yuǎn)離障礙物。用三次貝塞爾曲線生成局部軌跡取未來(lái)1-2秒的點(diǎn)作為參考軌跡。每個(gè)控制周期都重新計(jì)算一遍控制點(diǎn)避障結(jié)束之后P2逐步回歸到方向線上曲線自然恢復(fù)為直線。這里面有個(gè)工程細(xì)節(jié)P2偏移方向要和障礙物位置實(shí)時(shí)聯(lián)動(dòng)。最簡(jiǎn)單的方法是把P2的坐標(biāo)寫成障礙物相對(duì)位置函數(shù)比如P2 目標(biāo)方向中點(diǎn) k * 障礙物單位方向向量k是距離系數(shù)。碰撞風(fēng)險(xiǎn)大就增大k風(fēng)險(xiǎn)小就減小k。這樣避障的“幅度”就有了連續(xù)性不會(huì)一幀猛拐、下一幀猛回。我在實(shí)測(cè)小車時(shí)用頻率是20Hz每次生成50個(gè)采樣點(diǎn)整體計(jì)算開(kāi)銷可以忽略不計(jì)。貝塞爾在這里最大的優(yōu)勢(shì)是計(jì)算輕、實(shí)時(shí)性強(qiáng)對(duì)嵌入式平臺(tái)很友好。4. 參數(shù)調(diào)優(yōu)與曲線質(zhì)量檢查4.1 控制點(diǎn)布局這五個(gè)經(jīng)驗(yàn)直接抄貝塞爾曲線能不能用得好90%靠控制點(diǎn)布局。這里整理幾條實(shí)際項(xiàng)目里反復(fù)驗(yàn)證過(guò)的經(jīng)驗(yàn)第一控制點(diǎn)數(shù)量寧少勿多。很多人習(xí)慣把路徑上的所有關(guān)鍵點(diǎn)都塞進(jìn)去當(dāng)控制點(diǎn)結(jié)果控制點(diǎn)多、階數(shù)高曲線開(kāi)始在個(gè)別區(qū)域扭曲調(diào)整一個(gè)點(diǎn)會(huì)波及整條曲線。路徑規(guī)劃里我用得最多的是三次貝塞爾即四個(gè)控制點(diǎn)一個(gè)起點(diǎn)、一個(gè)終點(diǎn)、兩個(gè)中間控制點(diǎn)大多數(shù)平滑需求都能滿足。點(diǎn)再多也建議拆成多段低階曲線而不是一段高階曲線。第二控制點(diǎn)間距和方向要符合直覺(jué)。曲線在起點(diǎn)處的方向完全由P0指向P1決定所以P1的方向決定了出口角度。想讓車頭朝向不發(fā)生突變P1必須選在車輛當(dāng)前朝向的延長(zhǎng)線上且與P0的距離應(yīng)根據(jù)期望的“入彎速度”來(lái)定距離太短曲線會(huì)拐得很急距離太長(zhǎng)曲線會(huì)偏離預(yù)期路徑很遠(yuǎn)。第三對(duì)稱性是釋放控制點(diǎn)位置的捷徑。對(duì)于繞障這種對(duì)稱場(chǎng)景讓P2和P3中間控制點(diǎn)關(guān)于中軸線對(duì)稱布置曲線對(duì)稱且沒(méi)有詭異扭轉(zhuǎn)。對(duì)稱結(jié)構(gòu)出問(wèn)題的概率遠(yuǎn)小于非對(duì)稱布局。第四請(qǐng)把凸包可視化出來(lái)?？刂泣c(diǎn)設(shè)好之后先用fill函數(shù)把控制點(diǎn)凸包畫(huà)出來(lái)再看凸包是否與障礙物重疊。如果凸包已經(jīng)碰到障礙物無(wú)論怎么微調(diào)曲線都沒(méi)用因?yàn)榍€保證落在凸包內(nèi)正確做法是調(diào)整控制點(diǎn)讓凸包離開(kāi)障礙物而不是在那里面試圖用“曲線細(xì)節(jié)”去找安全空間。第五曲線末端方向不能忘。終點(diǎn)的切線方向由P(n-1)指向Pn需要確保這個(gè)方向與后續(xù)路徑銜接方向一致否則即使位置連續(xù)方向不連續(xù)也會(huì)導(dǎo)致機(jī)器人到終點(diǎn)時(shí)突然轉(zhuǎn)向。泊車場(chǎng)景尤其敏感后面專門講。4.2 曲率檢查與最差點(diǎn)處理路徑規(guī)劃出來(lái)之后一個(gè)常規(guī)檢查項(xiàng)是曲線的最大曲率因?yàn)榍手苯訉?duì)應(yīng)轉(zhuǎn)向半徑而轉(zhuǎn)向半徑受底盤機(jī)械結(jié)構(gòu)限制。差速小車最小轉(zhuǎn)彎半徑可能很小但阿克曼底盤或麥克納姆輪底盤有明確限制。用離散點(diǎn)近似計(jì)算曲率的方法很簡(jiǎn)單function k curvature(curve) x curve(:,1); y curve(:,2); dx gradient(x); dy gradient(y); ddx gradient(dx); ddy gradient(dy); k abs(ddx .* dy - dx .* ddy) ./ (dx.^2 dy.^2).^1.5; end然后找最大曲率位置k curvature(curve); [max_k, idx] max(k); % 最大曲率點(diǎn)對(duì)應(yīng)的曲線位置 worst_point curve(idx, :);如果最大曲率超出底盤限制優(yōu)先調(diào)整離該點(diǎn)最近的控制點(diǎn)把那個(gè)控制點(diǎn)往外“拉”增大局部彎曲半徑。用手拖動(dòng)一個(gè)控制點(diǎn)并重新運(yùn)行檢查觀察最大曲率的變化這樣反復(fù)幾次就有手感了。另外離散求導(dǎo)對(duì)噪聲敏感密集采樣后如果曲率曲線毛刺明顯可以先對(duì)curve用smoothdata做一次平滑再做曲率計(jì)算。曲率分段不連續(xù)反而說(shuō)明曲線拼接點(diǎn)可能有連續(xù)性缺陷這是分段貝塞爾時(shí)需要重點(diǎn)檢查的項(xiàng)目。4.3 Matlab常見(jiàn)報(bào)錯(cuò)與排查筆記矩陣維度不匹配。這是初學(xué)者最常見(jiàn)的錯(cuò)。P寫成一行多列而不是多行兩列。報(bào)錯(cuò)通常出現(xiàn)在coeff * P(i1,:)這一句如果P是1×n的向量P(i1,:)取到的是單個(gè)標(biāo)量coeff是res×1的列向量?jī)烧邿o(wú)法直接相乘。解決辦法是用size函數(shù)顯式檢查P維度確保P是(n1)×2的矩陣。nchoosek數(shù)值溢出?？刂泣c(diǎn)數(shù)量在20以上時(shí)nchoosek(20,10)已經(jīng)是18萬(wàn)多再高階一些會(huì)變得巨大甚至超過(guò)double類型的精確表示范圍。路徑規(guī)劃控制點(diǎn)數(shù)在10個(gè)以上就應(yīng)該警惕解決辦法是改用德卡斯特里奧遞推算法或者把曲線拆成多段三次貝塞爾。曲線“穿墻”。很多人的第一反應(yīng)是我控制點(diǎn)明明沒(méi)靠近障礙物為什么曲線還是穿過(guò)障礙物回到凸包性質(zhì)控制點(diǎn)沒(méi)靠近障礙物但控制點(diǎn)連成的凸包可能覆蓋了障礙物區(qū)域。畫(huà)出凸包把凸包往外推曲線自然外移。曲線起點(diǎn)不貼合當(dāng)前位置。檢查你是否把第一個(gè)控制點(diǎn)精確設(shè)置為起點(diǎn)還是隨手填了個(gè)大概值。貝塞爾曲線精確穿過(guò)P0如果P0偏了起點(diǎn)就偏了。這在接管全局路徑時(shí)經(jīng)常發(fā)生車輛實(shí)際位置和路徑起點(diǎn)之間相差幾厘米導(dǎo)致起始段需要緊急修正。典型報(bào)錯(cuò)原因排查手段Matrix dimensions must agree控制點(diǎn)矩陣維度錯(cuò)誤size(P)應(yīng)輸出[n1, 2]曲線起點(diǎn)偏移P0坐標(biāo)未設(shè)置精確打印P(1,:)與真實(shí)起點(diǎn)對(duì)比曲線詭異扭曲控制點(diǎn)數(shù)量過(guò)多/階數(shù)過(guò)高拆成多段三次貝塞爾再拼接最大曲率超限中間控制點(diǎn)過(guò)于接近折角沿角平分線外移控制點(diǎn)5. 進(jìn)階方向多段貝塞爾拼接與工程延伸5.1 多段貝塞爾曲線的C1連續(xù)拼接單段貝塞爾曲線控制點(diǎn)一多局部調(diào)整變得困難現(xiàn)實(shí)中更常用多段三次貝塞爾拼接成一條長(zhǎng)軌跡。拼接最關(guān)鍵的是連續(xù)性。兩段三次貝塞爾曲線第一段控制點(diǎn)是A0、A1、A2、A3第二段控制點(diǎn)是B0、B1、B2、B3。要保證位置連續(xù)B0必須等于A3也就是第二段起點(diǎn)就是第一段終點(diǎn)。要保證切線方向連續(xù)A3和B1必須位于A2延長(zhǎng)線上且滿足B1 - A3 A3 - A2或者寫成 B1 2*A3 - A2。這個(gè)條件的幾何意義是第二段的第一個(gè)控制點(diǎn)B1是第一段倒數(shù)第二個(gè)控制點(diǎn)A2關(guān)于公共點(diǎn)A3的鏡像。這樣拼接點(diǎn)處的切線方向一致曲線看起來(lái)是一根光滑的線而不是折線。如果需要更平滑的曲率連續(xù)C2條件更苛刻一般推薦直接用B樣條或NURBS而不是強(qiáng)行拼貝塞爾。絕大多數(shù)移動(dòng)機(jī)器人路徑規(guī)劃到C1就夠了因?yàn)檐囕v運(yùn)動(dòng)學(xué)能容忍方向連續(xù)曲率跳變則可以通過(guò)速度規(guī)劃里的加速度限制來(lái)處理。5.2 泊車路徑和噴涂路徑里的貝塞爾應(yīng)用泊車路徑規(guī)劃對(duì)這個(gè)“C1連續(xù)”的需求特別突出。車輛從停車位駛出或者倒車入庫(kù)起點(diǎn)和終點(diǎn)都有明確的位姿約束。用分段貝塞爾做泊車路徑的思路是把整個(gè)倒車過(guò)程拆成幾段每段用三次貝塞爾段與段之間滿足C1連續(xù)。起點(diǎn)段的P1設(shè)置在車輛當(dāng)前航向延長(zhǎng)線上終點(diǎn)段的倒數(shù)第二個(gè)控制點(diǎn)設(shè)置在目標(biāo)車位朝向的反延長(zhǎng)線上。這樣車輛從出發(fā)到停車全過(guò)程航向角都是連續(xù)的不會(huì)出現(xiàn)在車位門口突然調(diào)頭的滑稽場(chǎng)面。噴涂路徑規(guī)劃的貝塞爾用法稍有不同。噴槍走的是覆蓋折返路徑在每個(gè)折返點(diǎn)需要平滑過(guò)渡。直接用貝塞爾在折返拐角處生成一個(gè)平滑的“U形”過(guò)渡段可以避免噴槍在角點(diǎn)處減速過(guò)度。過(guò)渡段的形狀直接影響噴涂均勻性所以必須結(jié)合前面講的弧長(zhǎng)參數(shù)化來(lái)設(shè)計(jì)速度曲線保證噴槍在過(guò)渡段和直線段的速度盡可能接近。把貝塞爾當(dāng)“拐角圓角器”用而不是把所有軌跡都替換掉是最省力的思路。5.3 什么時(shí)候該換B樣條或NURBS在寫這篇文章之前我需要誠(chéng)實(shí)說(shuō)一句貝塞爾不是萬(wàn)能的。當(dāng)你遇到以下場(chǎng)景認(rèn)真考慮換B樣條軌跡有幾十個(gè)甚至上百個(gè)需要精確經(jīng)過(guò)的位置點(diǎn)而你又不想把它們變成控制點(diǎn)。B樣條可以做到曲線不經(jīng)過(guò)控制點(diǎn)也能逼近形狀且支持局部修改改一個(gè)點(diǎn)不影響整條曲線。需要曲率連續(xù)的復(fù)雜路徑。三次B樣條天然支持C2連續(xù)拼接起來(lái)比多段貝塞爾省心得多。需要精確控制曲線對(duì)控制點(diǎn)折線的逼近程度。NURBS通過(guò)權(quán)因子可以調(diào)節(jié)“吸引力”大小貝塞爾做不到。不過(guò)B樣條和NURBS的代價(jià)是參數(shù)更多、實(shí)現(xiàn)更復(fù)雜、調(diào)參門檻更高。如果你的場(chǎng)景是“路徑點(diǎn)不多、平滑要求適中、計(jì)算平臺(tái)有限”用分段三次貝塞爾的性價(jià)比遠(yuǎn)高于上B樣條。我自己的經(jīng)驗(yàn)是80%的路徑規(guī)劃平滑需求四控制點(diǎn)三次貝塞爾就能解決剩下20%再考慮B樣條。從最簡(jiǎn)單的一段三次貝塞爾繞障到分段拼接長(zhǎng)軌跡再到配合速度規(guī)劃解決執(zhí)行問(wèn)題這條技術(shù)路線的每一步都不復(fù)雜但環(huán)環(huán)相扣。我在實(shí)際項(xiàng)目里最深的體會(huì)是貝塞爾曲線真正的門檻不在數(shù)學(xué)而在你有沒(méi)有把控制點(diǎn)當(dāng)成“力的方向”而不是“路徑的終點(diǎn)”來(lái)理解。搞明白這一點(diǎn)很多調(diào)參困惑都會(huì)迎刃而解。 貝塞爾曲線原理與Matlab實(shí)現(xiàn)：路徑規(guī)劃平滑軌跡實(shí)戰(zhàn)指南
搞機(jī)器人路徑規(guī)劃的人大概率都有過(guò)這種經(jīng)歷全局規(guī)劃給出一條直線折線車走到拐點(diǎn)的時(shí)候不是急停就是急轉(zhuǎn)看著都心疼。更麻煩的是把軌跡交給運(yùn)動(dòng)控制器時(shí)速度和加速度在這里直接斷掉不加平滑根本跑不起來(lái)。我最早接觸貝塞爾曲線就是為了解決這類問(wèn)題。后來(lái)在避障小車、機(jī)械臂軌跡、噴涂路徑規(guī)劃這些場(chǎng)景里貝塞爾曲線幾乎成了默認(rèn)的平滑方案。這篇文章把貝塞爾曲線的原理、Matlab實(shí)現(xiàn)、路徑規(guī)劃里的實(shí)際應(yīng)用一次性講透代碼直接給可跑通版本。適合剛接觸路徑規(guī)劃的初學(xué)者也適合想快速在Matlab里出結(jié)果的工程師。1. 貝塞爾曲線的數(shù)學(xué)本質(zhì)與圖形直覺(jué)1.1 從一根橡皮筋說(shuō)起什么是貝塞爾曲線如果只給一個(gè)控制點(diǎn)貝塞爾曲線就是那個(gè)點(diǎn)本身。給兩個(gè)控制點(diǎn)它就是一條直線段。麻煩的是三個(gè)及以上的控制點(diǎn)這時(shí)候曲線怎么走你可以在心里想象一塊木板上釘了幾顆釘子然后把一根橡皮筋繞過(guò)這些釘子。橡皮筋不會(huì)緊貼每顆釘子而是被它們共同“拉”著形成一個(gè)光滑的弧線。貝塞爾曲線就是這根橡皮筋的數(shù)學(xué)表達(dá)。它最反直覺(jué)的地方在于除了第一個(gè)和最后一個(gè)控制點(diǎn)曲線上絕大多數(shù)點(diǎn)都不經(jīng)過(guò)中間控制點(diǎn)。中間控制點(diǎn)只是“吸引”曲線向自己的方向彎曲距離越近吸引越強(qiáng)。這跟很多初學(xué)者想的“用曲線把點(diǎn)連起來(lái)”完全不是一回事但恰恰是這個(gè)特性讓貝塞爾曲線在路徑平滑中特別有用——你不需要讓軌跡精確穿過(guò)每一個(gè)“想法點(diǎn)”只需要讓軌跡被這些點(diǎn)引導(dǎo)就能得到一條安全、平滑、可執(zhí)行的路徑。貝塞爾曲線的數(shù)學(xué)定義是用Bernstein基函數(shù)對(duì)控制點(diǎn)做加權(quán)平均階數(shù)等于控制點(diǎn)數(shù)量減一。如果有n1個(gè)控制點(diǎn)曲線方程長(zhǎng)這樣P(u) Σ (i0到n) B(i,n)(u) * P(i)u ∈ [0,1]其中B(i,n)(u) C(n,i) * u^i * (1-u)^(n-i)C(n,i)是組合數(shù)。這個(gè)式子看著唬人但拆開(kāi)看就是一個(gè)加權(quán)平均的推廣每個(gè)控制點(diǎn)乘以一個(gè)權(quán)重所有權(quán)重加起來(lái)恒等于1。u從0走到1的過(guò)程中權(quán)重分配不斷變化曲線就從起點(diǎn)平滑移動(dòng)到終點(diǎn)。1.2 德卡斯特里奧幾何構(gòu)造法公式歸公式真要直觀理解貝塞爾曲線的形狀變化我推薦德卡斯特里奧算法。這個(gè)算法把曲線生成變成了一組反復(fù)的線性插值手工都能算。以三個(gè)控制點(diǎn)P0、P1、P2為例生成一條二次貝塞爾曲線。先取參數(shù)u在線段P0-P1上按比例u找點(diǎn)A滿足A (1-u)P0 uP1在線段P1-P2上同樣找點(diǎn)B滿足B (1-u)P1 uP2。然后把A和B再按比例u連一次得到點(diǎn)C (1-u)A uB。讓u從0到1遍歷一遍點(diǎn)C的軌跡就是二次貝塞爾曲線。三個(gè)控制點(diǎn)做兩次線性插值四個(gè)控制點(diǎn)就做三次線性插值n1個(gè)控制點(diǎn)做n輪插值。這就是德卡斯特里奧算法的核心重復(fù)線性插值直到只剩一個(gè)點(diǎn)。這個(gè)算法最大的價(jià)值在于它的數(shù)值穩(wěn)定性。直接按Bernstein基函數(shù)公式計(jì)算當(dāng)階數(shù)比較高時(shí)u^i和(1-u)^(n-i)的數(shù)值可能過(guò)大或過(guò)小組合數(shù)也可能非常大計(jì)算機(jī)算起來(lái)容易丟精度。德卡斯特里奧算法只涉及乘法和加法避免了組合數(shù)的大數(shù)計(jì)算在工程里更穩(wěn)。我在教學(xué)和項(xiàng)目調(diào)試?yán)锪?xí)慣先用德卡斯特里奧算法的思路推導(dǎo)再用Bernstein公式做向量化實(shí)現(xiàn)這樣既理解幾何意義又拿到運(yùn)行效率。1.3 貝塞爾曲線的幾個(gè)手算樣例直接看一個(gè)具體例子。假設(shè)控制點(diǎn)P0(0,0)P1(2,4)P2(4,0)求這條二次貝塞爾曲線在u0.5處的點(diǎn)。第一步A 0.5P0 0.5P1 (1,2)。第二步B 0.5P1 0.5P2 (3,2)。第三步C 0.5A 0.5B (2,2)。所以曲線上u0.5處的點(diǎn)是(2,2)正好是拋物線頂點(diǎn)。這個(gè)結(jié)果也能從公式驗(yàn)證二次貝塞爾曲線公式展開(kāi)后是P(u) (1-u)2P0 2u(1-u)P1 u2P2帶u0.5進(jìn)去得到0.25P0 0.5P1 0.25P2 (2,2)。兩條路結(jié)果一致。這種手算的價(jià)值在于建立直覺(jué)參數(shù)u是比例的概念不是時(shí)間的概念。曲線上的點(diǎn)始終是對(duì)控制點(diǎn)的調(diào)和平均權(quán)重由當(dāng)前u決定。后面講路徑規(guī)劃中速度控制的時(shí)候這個(gè)區(qū)別會(huì)非常重要。2. 貝塞爾曲線在路徑規(guī)劃中的工程價(jià)值2.1 為什么不用折線和普通多項(xiàng)式擬合路徑規(guī)劃里最樸素的方案是折線連接。機(jī)器人先走到A點(diǎn)再走到B點(diǎn)再走到C點(diǎn)。缺點(diǎn)很明顯每個(gè)轉(zhuǎn)折點(diǎn)處方向突變?nèi)绻麢C(jī)器人是差速底盤這里必須減速到接近零再轉(zhuǎn)向效率極低如果是機(jī)械臂這里會(huì)帶來(lái)加速度沖擊機(jī)構(gòu)振動(dòng)大、壽命縮短。那用高次多項(xiàng)式擬合呢比如讓曲線精確經(jīng)過(guò)所有路徑點(diǎn)。問(wèn)題更麻煩。高次多項(xiàng)式對(duì)單個(gè)點(diǎn)的擾動(dòng)極其敏感一個(gè)噪聲點(diǎn)就能讓曲線在遠(yuǎn)離該點(diǎn)的地方劇烈振蕩這就是所謂的Runge現(xiàn)象?？刂泣c(diǎn)數(shù)量一多多項(xiàng)式階數(shù)水漲船高曲線形態(tài)基本不可控。貝塞爾曲線恰好在這兩者之間找到了平衡。它的階數(shù)雖然也是由控制點(diǎn)數(shù)量決定但曲線的形狀被凸包性質(zhì)緊緊約束住不會(huì)像高次多項(xiàng)式那樣失控。對(duì)于路徑規(guī)劃這種“我需要一條光滑且不會(huì)跑偏太遠(yuǎn)的曲線”的需求貝塞爾曲線是非常自然的候選。2.2 貝塞爾曲線的四個(gè)關(guān)鍵幾何性質(zhì)端點(diǎn)插值性。曲線的起點(diǎn)一定在第一個(gè)控制點(diǎn)P0上終點(diǎn)一定在最后一個(gè)控制點(diǎn)Pn上。這意味著路徑規(guī)劃的起止位置約束可以直接通過(guò)第一個(gè)和最后一個(gè)控制點(diǎn)精確滿足非常方便。端點(diǎn)切線方向。曲線在起點(diǎn)的切線方向由P0指向P1在終點(diǎn)的切線方向由P(n-1)指向Pn。這個(gè)性質(zhì)在泊車路徑規(guī)劃里是寶貝因?yàn)椴窜噷?duì)起點(diǎn)和終點(diǎn)的航向角有嚴(yán)格要求。只要控制點(diǎn)選在車輛朝向的延長(zhǎng)線上曲線出入方向就完全可控。凸包性。整條曲線完全落在所有控制點(diǎn)構(gòu)成的凸包內(nèi)。這個(gè)性質(zhì)在避障里極其重要只要把控制點(diǎn)選在障礙物之外曲線就絕對(duì)不可能穿過(guò)障礙物內(nèi)部。對(duì)于動(dòng)態(tài)避障小車規(guī)劃時(shí)只需要檢查凸包是否與障礙物碰撞計(jì)算量遠(yuǎn)小于逐點(diǎn)檢查。變差縮減性。曲線與任意直線的交點(diǎn)數(shù)量不超過(guò)控制點(diǎn)折線與該直線的交點(diǎn)數(shù)量。翻譯成人話就是曲線比控制點(diǎn)折線更“平穩(wěn)”不會(huì)出現(xiàn)比控制點(diǎn)更劇烈的擺動(dòng)。這個(gè)性質(zhì)讓貝塞爾曲線天然適合生成平滑軌跡不會(huì)自己“抽風(fēng)”。2.3 一個(gè)容易忽略的問(wèn)題參數(shù)速度不等于實(shí)際速度這是貝塞爾曲線在實(shí)際工程中最容易踩的坑。參數(shù)u從0均勻增長(zhǎng)到1對(duì)應(yīng)的曲線點(diǎn)并不是勻速運(yùn)動(dòng)的。因?yàn)榍€上點(diǎn)的實(shí)際運(yùn)動(dòng)速度是|dP/du| * (du/dt)而|dP/du|在曲線不同位置不恒定即使u是線性變化的實(shí)際速度也在改變。舉個(gè)例子四個(gè)控制點(diǎn)(0,0)、(0,10)、(10,10)、(10,0)構(gòu)成的三次貝塞爾曲線u從0到0.5時(shí)走過(guò)的弧長(zhǎng)和u從0.5到1時(shí)走過(guò)的弧長(zhǎng)并不相等。在噴涂路徑規(guī)劃里如果直接把u當(dāng)作時(shí)間噴槍在工件表面移動(dòng)速度就不均勻涂層厚度會(huì)忽厚忽薄在焊接里會(huì)導(dǎo)致焊縫質(zhì)量不一致。解決方案是對(duì)曲線做弧長(zhǎng)參數(shù)化先把曲線按弧長(zhǎng)離散采樣再建立弧長(zhǎng)s到參數(shù)u的映射關(guān)系最后按期望速度控制s的變化。細(xì)節(jié)我放到第4章講這里先記住結(jié)論u均勻不等于速度均勻涉及運(yùn)動(dòng)控制時(shí)必須顯式處理弧長(zhǎng)與速度的關(guān)系。3. Matlab實(shí)現(xiàn)與路徑規(guī)劃實(shí)戰(zhàn)3.1 核心函數(shù)基于Bernstein的向量化實(shí)現(xiàn)Matlab適合向量化編程。直接用for循環(huán)遍歷每個(gè)采樣點(diǎn)算基函數(shù)寫法直觀但效率差采樣點(diǎn)一多就慢。這里給出一個(gè)向量化實(shí)現(xiàn)核心是利用linspace生成u向量再用矩陣運(yùn)算同時(shí)計(jì)算所有采樣點(diǎn)。function curve bezier_curve(P, res) % 生成貝塞爾曲線 % 輸入: % P - 控制點(diǎn)矩陣大小為(n1) x dim每行一個(gè)控制點(diǎn) % res - 采樣點(diǎn)數(shù)即曲線上生成多少個(gè)點(diǎn) % 輸出: % curve - res x dim 的密集曲線點(diǎn)集 n size(P, 1) - 1; dim size(P, 2); u linspace(0, 1, res); curve zeros(res, dim); for i 0:n % Bernstein基函數(shù)向量化計(jì)算 coeff nchoosek(n, i) .* (u.^i) .* ((1 - u).^(n - i)); % 每個(gè)控制點(diǎn)按權(quán)重疊加 curve curve coeff * P(i 1, :); end endnchoosek(n, i)計(jì)算組合數(shù)。u是res×1的列向量u.^i得到每個(gè)采樣點(diǎn)的u冪次(1-u).^(n-i)得到另一部分兩者逐元素相乘后再乘以組合數(shù)得到res×1的權(quán)重向量。coeff * P(i1,:)是res×1乘1×dim的矩陣乘法結(jié)果是res×dim等價(jià)于把每個(gè)點(diǎn)都平移到控制點(diǎn)的位置并按權(quán)重縮放。實(shí)測(cè)下來(lái)100個(gè)控制點(diǎn)、5000個(gè)采樣點(diǎn)的情況下這個(gè)函數(shù)在普通筆記本上十幾毫秒跑完夠?qū)崟r(shí)使用的。如果控制點(diǎn)特別多可以用德卡斯特里奧算法的遞推實(shí)現(xiàn)避免組合數(shù)溢出但一般路徑規(guī)劃控制點(diǎn)不會(huì)超過(guò)十幾個(gè)這個(gè)版本夠用。3.2 完整示例小車平滑繞障路徑生成假設(shè)一臺(tái)小車要從起點(diǎn)(0,0)走到終點(diǎn)(10,0)中間遇到一個(gè)障礙物需要繞過(guò)去。傳統(tǒng)做法規(guī)劃出折線路徑(0,0)-(3,1)-(7,1)-(10,0)。轉(zhuǎn)折點(diǎn)處方向突變直接跟蹤很難受。用貝塞爾曲線把這條折線平滑一下% 控制點(diǎn): 起點(diǎn)、兩個(gè)繞障點(diǎn)、終點(diǎn) P [0, 0; 3, 1; 7, 1; 10, 0]; % 采樣生成曲線 res 200; curve bezier_curve(P, res); % 繪圖對(duì)比 figure; plot(P(:,1), P(:,2), k--o, LineWidth, 1.5); hold on; plot(curve(:,1), curve(:,2), b-, LineWidth, 2.5); legend(控制點(diǎn)折線, 貝塞爾曲線, Location, southeast); title(四控制點(diǎn)Bezier曲線路徑平滑); axis equal; grid on;跑完之后你會(huì)看到曲線從(0,0)出發(fā)平滑繞過(guò)中間兩個(gè)控制點(diǎn)附近最后精確落在(10,0)上。中間兩個(gè)控制點(diǎn)像磁極一樣把曲線“吸”過(guò)去但曲線并沒(méi)有真正觸碰到它們。這個(gè)例子里最核心的認(rèn)知是把路徑規(guī)劃的折點(diǎn)直接當(dāng)作控制點(diǎn)使用不一定能得到理想曲線。想讓曲線更貼近或更遠(yuǎn)離折線需要主動(dòng)調(diào)整控制點(diǎn)位置。工程上一般會(huì)從折線頂點(diǎn)沿角平分線方向偏移一定距離偏移量決定曲線的“切彎”程度。這個(gè)調(diào)參思路在第4章展開(kāi)。3.3 動(dòng)態(tài)避障場(chǎng)景的貝塞爾生成思路動(dòng)態(tài)避障小車的局部路徑規(guī)劃里貝塞爾曲線也能派上大用場(chǎng)。思路是在每個(gè)控制周期內(nèi)根據(jù)當(dāng)前車輛位置、目標(biāo)路徑上的前瞻點(diǎn)、障礙物位置動(dòng)態(tài)生成一組貝塞爾控制點(diǎn)實(shí)時(shí)計(jì)算局部避障軌跡。具體做法當(dāng)前車輛位置作為P0車輛航向方向前方1米處作為P1保證軌跡起點(diǎn)方向與當(dāng)前航向一致。目標(biāo)路徑上的一個(gè)前瞻點(diǎn)作為P3保證軌跡終點(diǎn)方向符合全局路徑趨勢(shì)。P2放在障礙物外側(cè)的安全距離上讓曲線被“推開(kāi)”遠(yuǎn)離障礙物。用三次貝塞爾曲線生成局部軌跡取未來(lái)1-2秒的點(diǎn)作為參考軌跡。每個(gè)控制周期都重新計(jì)算一遍控制點(diǎn)避障結(jié)束之后P2逐步回歸到方向線上曲線自然恢復(fù)為直線。這里面有個(gè)工程細(xì)節(jié)P2偏移方向要和障礙物位置實(shí)時(shí)聯(lián)動(dòng)。最簡(jiǎn)單的方法是把P2的坐標(biāo)寫成障礙物相對(duì)位置函數(shù)比如P2 目標(biāo)方向中點(diǎn) k * 障礙物單位方向向量k是距離系數(shù)。碰撞風(fēng)險(xiǎn)大就增大k風(fēng)險(xiǎn)小就減小k。這樣避障的“幅度”就有了連續(xù)性不會(huì)一幀猛拐、下一幀猛回。我在實(shí)測(cè)小車時(shí)用頻率是20Hz每次生成50個(gè)采樣點(diǎn)整體計(jì)算開(kāi)銷可以忽略不計(jì)。貝塞爾在這里最大的優(yōu)勢(shì)是計(jì)算輕、實(shí)時(shí)性強(qiáng)對(duì)嵌入式平臺(tái)很友好。4. 參數(shù)調(diào)優(yōu)與曲線質(zhì)量檢查4.1 控制點(diǎn)布局這五個(gè)經(jīng)驗(yàn)直接抄貝塞爾曲線能不能用得好90%靠控制點(diǎn)布局。這里整理幾條實(shí)際項(xiàng)目里反復(fù)驗(yàn)證過(guò)的經(jīng)驗(yàn)第一控制點(diǎn)數(shù)量寧少勿多。很多人習(xí)慣把路徑上的所有關(guān)鍵點(diǎn)都塞進(jìn)去當(dāng)控制點(diǎn)結(jié)果控制點(diǎn)多、階數(shù)高曲線開(kāi)始在個(gè)別區(qū)域扭曲調(diào)整一個(gè)點(diǎn)會(huì)波及整條曲線。路徑規(guī)劃里我用得最多的是三次貝塞爾即四個(gè)控制點(diǎn)一個(gè)起點(diǎn)、一個(gè)終點(diǎn)、兩個(gè)中間控制點(diǎn)大多數(shù)平滑需求都能滿足。點(diǎn)再多也建議拆成多段低階曲線而不是一段高階曲線。第二控制點(diǎn)間距和方向要符合直覺(jué)。曲線在起點(diǎn)處的方向完全由P0指向P1決定所以P1的方向決定了出口角度。想讓車頭朝向不發(fā)生突變P1必須選在車輛當(dāng)前朝向的延長(zhǎng)線上且與P0的距離應(yīng)根據(jù)期望的“入彎速度”來(lái)定距離太短曲線會(huì)拐得很急距離太長(zhǎng)曲線會(huì)偏離預(yù)期路徑很遠(yuǎn)。第三對(duì)稱性是釋放控制點(diǎn)位置的捷徑。對(duì)于繞障這種對(duì)稱場(chǎng)景讓P2和P3中間控制點(diǎn)關(guān)于中軸線對(duì)稱布置曲線對(duì)稱且沒(méi)有詭異扭轉(zhuǎn)。對(duì)稱結(jié)構(gòu)出問(wèn)題的概率遠(yuǎn)小于非對(duì)稱布局。第四請(qǐng)把凸包可視化出來(lái)?？刂泣c(diǎn)設(shè)好之后先用fill函數(shù)把控制點(diǎn)凸包畫(huà)出來(lái)再看凸包是否與障礙物重疊。如果凸包已經(jīng)碰到障礙物無(wú)論怎么微調(diào)曲線都沒(méi)用因?yàn)榍€保證落在凸包內(nèi)正確做法是調(diào)整控制點(diǎn)讓凸包離開(kāi)障礙物而不是在那里面試圖用“曲線細(xì)節(jié)”去找安全空間。第五曲線末端方向不能忘。終點(diǎn)的切線方向由P(n-1)指向Pn需要確保這個(gè)方向與后續(xù)路徑銜接方向一致否則即使位置連續(xù)方向不連續(xù)也會(huì)導(dǎo)致機(jī)器人到終點(diǎn)時(shí)突然轉(zhuǎn)向。泊車場(chǎng)景尤其敏感后面專門講。4.2 曲率檢查與最差點(diǎn)處理路徑規(guī)劃出來(lái)之后一個(gè)常規(guī)檢查項(xiàng)是曲線的最大曲率因?yàn)榍手苯訉?duì)應(yīng)轉(zhuǎn)向半徑而轉(zhuǎn)向半徑受底盤機(jī)械結(jié)構(gòu)限制。差速小車最小轉(zhuǎn)彎半徑可能很小但阿克曼底盤或麥克納姆輪底盤有明確限制。用離散點(diǎn)近似計(jì)算曲率的方法很簡(jiǎn)單function k curvature(curve) x curve(:,1); y curve(:,2); dx gradient(x); dy gradient(y); ddx gradient(dx); ddy gradient(dy); k abs(ddx .* dy - dx .* ddy) ./ (dx.^2 dy.^2).^1.5; end然后找最大曲率位置k curvature(curve); [max_k, idx] max(k); % 最大曲率點(diǎn)對(duì)應(yīng)的曲線位置 worst_point curve(idx, :);如果最大曲率超出底盤限制優(yōu)先調(diào)整離該點(diǎn)最近的控制點(diǎn)把那個(gè)控制點(diǎn)往外“拉”增大局部彎曲半徑。用手拖動(dòng)一個(gè)控制點(diǎn)并重新運(yùn)行檢查觀察最大曲率的變化這樣反復(fù)幾次就有手感了。另外離散求導(dǎo)對(duì)噪聲敏感密集采樣后如果曲率曲線毛刺明顯可以先對(duì)curve用smoothdata做一次平滑再做曲率計(jì)算。曲率分段不連續(xù)反而說(shuō)明曲線拼接點(diǎn)可能有連續(xù)性缺陷這是分段貝塞爾時(shí)需要重點(diǎn)檢查的項(xiàng)目。4.3 Matlab常見(jiàn)報(bào)錯(cuò)與排查筆記矩陣維度不匹配。這是初學(xué)者最常見(jiàn)的錯(cuò)。P寫成一行多列而不是多行兩列。報(bào)錯(cuò)通常出現(xiàn)在coeff * P(i1,:)這一句如果P是1×n的向量P(i1,:)取到的是單個(gè)標(biāo)量coeff是res×1的列向量?jī)烧邿o(wú)法直接相乘。解決辦法是用size函數(shù)顯式檢查P維度確保P是(n1)×2的矩陣。nchoosek數(shù)值溢出?？刂泣c(diǎn)數(shù)量在20以上時(shí)nchoosek(20,10)已經(jīng)是18萬(wàn)多再高階一些會(huì)變得巨大甚至超過(guò)double類型的精確表示范圍。路徑規(guī)劃控制點(diǎn)數(shù)在10個(gè)以上就應(yīng)該警惕解決辦法是改用德卡斯特里奧遞推算法或者把曲線拆成多段三次貝塞爾。曲線“穿墻”。很多人的第一反應(yīng)是我控制點(diǎn)明明沒(méi)靠近障礙物為什么曲線還是穿過(guò)障礙物回到凸包性質(zhì)控制點(diǎn)沒(méi)靠近障礙物但控制點(diǎn)連成的凸包可能覆蓋了障礙物區(qū)域。畫(huà)出凸包把凸包往外推曲線自然外移。曲線起點(diǎn)不貼合當(dāng)前位置。檢查你是否把第一個(gè)控制點(diǎn)精確設(shè)置為起點(diǎn)還是隨手填了個(gè)大概值。貝塞爾曲線精確穿過(guò)P0如果P0偏了起點(diǎn)就偏了。這在接管全局路徑時(shí)經(jīng)常發(fā)生車輛實(shí)際位置和路徑起點(diǎn)之間相差幾厘米導(dǎo)致起始段需要緊急修正。典型報(bào)錯(cuò)原因排查手段Matrix dimensions must agree控制點(diǎn)矩陣維度錯(cuò)誤size(P)應(yīng)輸出[n1, 2]曲線起點(diǎn)偏移P0坐標(biāo)未設(shè)置精確打印P(1,:)與真實(shí)起點(diǎn)對(duì)比曲線詭異扭曲控制點(diǎn)數(shù)量過(guò)多/階數(shù)過(guò)高拆成多段三次貝塞爾再拼接最大曲率超限中間控制點(diǎn)過(guò)于接近折角沿角平分線外移控制點(diǎn)5. 進(jìn)階方向多段貝塞爾拼接與工程延伸5.1 多段貝塞爾曲線的C1連續(xù)拼接單段貝塞爾曲線控制點(diǎn)一多局部調(diào)整變得困難現(xiàn)實(shí)中更常用多段三次貝塞爾拼接成一條長(zhǎng)軌跡。拼接最關(guān)鍵的是連續(xù)性。兩段三次貝塞爾曲線第一段控制點(diǎn)是A0、A1、A2、A3第二段控制點(diǎn)是B0、B1、B2、B3。要保證位置連續(xù)B0必須等于A3也就是第二段起點(diǎn)就是第一段終點(diǎn)。要保證切線方向連續(xù)A3和B1必須位于A2延長(zhǎng)線上且滿足B1 - A3 A3 - A2或者寫成 B1 2*A3 - A2。這個(gè)條件的幾何意義是第二段的第一個(gè)控制點(diǎn)B1是第一段倒數(shù)第二個(gè)控制點(diǎn)A2關(guān)于公共點(diǎn)A3的鏡像。這樣拼接點(diǎn)處的切線方向一致曲線看起來(lái)是一根光滑的線而不是折線。如果需要更平滑的曲率連續(xù)C2條件更苛刻一般推薦直接用B樣條或NURBS而不是強(qiáng)行拼貝塞爾。絕大多數(shù)移動(dòng)機(jī)器人路徑規(guī)劃到C1就夠了因?yàn)檐囕v運(yùn)動(dòng)學(xué)能容忍方向連續(xù)曲率跳變則可以通過(guò)速度規(guī)劃里的加速度限制來(lái)處理。5.2 泊車路徑和噴涂路徑里的貝塞爾應(yīng)用泊車路徑規(guī)劃對(duì)這個(gè)“C1連續(xù)”的需求特別突出。車輛從停車位駛出或者倒車入庫(kù)起點(diǎn)和終點(diǎn)都有明確的位姿約束。用分段貝塞爾做泊車路徑的思路是把整個(gè)倒車過(guò)程拆成幾段每段用三次貝塞爾段與段之間滿足C1連續(xù)。起點(diǎn)段的P1設(shè)置在車輛當(dāng)前航向延長(zhǎng)線上終點(diǎn)段的倒數(shù)第二個(gè)控制點(diǎn)設(shè)置在目標(biāo)車位朝向的反延長(zhǎng)線上。這樣車輛從出發(fā)到停車全過(guò)程航向角都是連續(xù)的不會(huì)出現(xiàn)在車位門口突然調(diào)頭的滑稽場(chǎng)面。噴涂路徑規(guī)劃的貝塞爾用法稍有不同。噴槍走的是覆蓋折返路徑在每個(gè)折返點(diǎn)需要平滑過(guò)渡。直接用貝塞爾在折返拐角處生成一個(gè)平滑的“U形”過(guò)渡段可以避免噴槍在角點(diǎn)處減速過(guò)度。過(guò)渡段的形狀直接影響噴涂均勻性所以必須結(jié)合前面講的弧長(zhǎng)參數(shù)化來(lái)設(shè)計(jì)速度曲線保證噴槍在過(guò)渡段和直線段的速度盡可能接近。把貝塞爾當(dāng)“拐角圓角器”用而不是把所有軌跡都替換掉是最省力的思路。5.3 什么時(shí)候該換B樣條或NURBS在寫這篇文章之前我需要誠(chéng)實(shí)說(shuō)一句貝塞爾不是萬(wàn)能的。當(dāng)你遇到以下場(chǎng)景認(rèn)真考慮換B樣條軌跡有幾十個(gè)甚至上百個(gè)需要精確經(jīng)過(guò)的位置點(diǎn)而你又不想把它們變成控制點(diǎn)。B樣條可以做到曲線不經(jīng)過(guò)控制點(diǎn)也能逼近形狀且支持局部修改改一個(gè)點(diǎn)不影響整條曲線。需要曲率連續(xù)的復(fù)雜路徑。三次B樣條天然支持C2連續(xù)拼接起來(lái)比多段貝塞爾省心得多。需要精確控制曲線對(duì)控制點(diǎn)折線的逼近程度。NURBS通過(guò)權(quán)因子可以調(diào)節(jié)“吸引力”大小貝塞爾做不到。不過(guò)B樣條和NURBS的代價(jià)是參數(shù)更多、實(shí)現(xiàn)更復(fù)雜、調(diào)參門檻更高。如果你的場(chǎng)景是“路徑點(diǎn)不多、平滑要求適中、計(jì)算平臺(tái)有限”用分段三次貝塞爾的性價(jià)比遠(yuǎn)高于上B樣條。我自己的經(jīng)驗(yàn)是80%的路徑規(guī)劃平滑需求四控制點(diǎn)三次貝塞爾就能解決剩下20%再考慮B樣條。從最簡(jiǎn)單的一段三次貝塞爾繞障到分段拼接長(zhǎng)軌跡再到配合速度規(guī)劃解決執(zhí)行問(wèn)題這條技術(shù)路線的每一步都不復(fù)雜但環(huán)環(huán)相扣。我在實(shí)際項(xiàng)目里最深的體會(huì)是貝塞爾曲線真正的門檻不在數(shù)學(xué)而在你有沒(méi)有把控制點(diǎn)當(dāng)成“力的方向”而不是“路徑的終點(diǎn)”來(lái)理解。搞明白這一點(diǎn)很多調(diào)參困惑都會(huì)迎刃而解。