(Bezier)貝塞爾曲在路徑規(guī)劃的運用
前言
之前被安排了活，一個局部區(qū)域機器運動控制的工作，大致是一個機器位于一個極限區(qū)域時候，機器要進入一個特殊的機制，使得機器可以安全的走出來。其中用到了bezier曲線進行優(yōu)化路徑，今天寫一下，正好也給大家分享一下工作和實踐的情況。
作者：良知猶存
轉(zhuǎn)載授權以及圍觀：歡迎關注微信公眾號：羽林君
或者添加作者個人微信：become_me
貝塞爾曲線基本介紹
線段都可以被拆分成兩個坐標的差來表示,如下面一階的貝塞爾曲線，P0到P1，可以用一個t進行拆分這段線,分別是線段 t(P0～P1)、線段 1-t(P0～P1)，P0和P1叫做， 這條條貝塞爾的兩個控制點，而貝塞爾曲線至少要有兩個控制點（就是下面的這條直線，一階貝塞爾曲線）。在貝塞爾曲線與控制點位置相關，這意味著在曲線生成過程中，我們可以通過調(diào)節(jié)控制點的位置，進而調(diào)整整個曲線。
貝塞爾的階數(shù)和次數(shù)是一樣的，二階貝塞爾，三個點，最高次數(shù)二次。例：二階貝塞爾：三個點，兩個線段，以所有等比的點組合成的曲線叫做二階貝塞爾曲線。
接下來給大家介紹一下貝塞爾曲線的推導工程，也比較簡單，并且網(wǎng)上的介紹也挺多的：
一階：
這里面有兩個控制點為 ，對應的曲線方程為：
t?[0,1] 這個方程可以理解為，從出發(fā)，朝著的方向前進的距離，從而得到了點B(t)的位置。t從0逐漸遞增到1，這個過程完成，就成了我們所看到的曲線。
另外，之所以是一階貝塞爾曲線是因為方程是關于t的一階多項式，多階也是一樣。
二階：有三個控制點，這里的 P0、P1、P2 分別稱之為控制點，曲線的產(chǎn)生完全與這三個點位置相關。
與一階有些區(qū)別就在于三個控制點形成兩個線段，每個線段上有一個點在運動，于是得到兩個點；再使用兩個點形成一個線段，這個線段上有一個點在運動，于是得到一個點；最后一個點的運動軌跡便構(gòu)成了二階貝塞爾曲線。
對應的曲線方程為：
這是一條迭代公式，每次迭代都會少掉一個“點”。
最后得：
三階：有四個控制點
設控制點為P0，P1，P2和P4，曲線方程為：
配圖這是matlab生成的gif動畫，大家想要的也可以找我，代碼私發(fā)給大家。
N階：
我們發(fā)現(xiàn)，實際上是每輪都是 n 個點，形成 n-1 條線段，每個線段上有一個點在運動，那么就只關注這 n-1 個點，循環(huán)往復。最終只剩一個點時，它的軌跡便是結(jié)果。
如此一來，你會發(fā)現(xiàn)貝塞爾曲線內(nèi)的遞歸結(jié)構(gòu)。實際上，上述介紹的分別是一階、二階、三階的貝塞爾曲線，貝塞爾曲線可以由階數(shù)遞歸定義。
N階貝塞爾曲線公式:
貝塞爾曲線應用
貝塞爾曲線在動畫中有應用，前端以及一些其他顯示要求；此外在路徑規(guī)劃過程中，也會使用貝塞爾曲線進行規(guī)劃好路徑再優(yōu)化，我就是使用了后者進行優(yōu)化規(guī)劃好的路徑，使得機器行走更加順暢，不過使用中大家需要按照機器實際相應來進行調(diào)整t的精度以及階數(shù)。
由于貝塞爾曲線本身的數(shù)學表達式便是一條遞歸式，所以決定采用遞歸的方式來實現(xiàn)。代碼如下，BezierCurve函數(shù)實現(xiàn)貝塞爾曲線迭代，UseBezierOptimizePath函數(shù)的第二個參數(shù)進行控制使用的階數(shù)，最后調(diào)用opencv實現(xiàn)可視化效果。
#include?
#include?
#include?
#include?
using?namespace?cv;
using?std::cout;
using?std::endl;
using?std::vector;
template?
T?BezierCurve(T?src)
{
????if?(src.size()?????????return?src;
????const?float?step?=?0.003;//1.0/step
????T?res;
????if?(src.size()?==?1)?{//遞歸結(jié)束條件
????????for?(float?t?=?0;?t?????????????res.push_back(src[0]);
????????return?res;
????}
????T?first_part{};
????T?second_part{};
????first_part.assign(src.begin(),?src.end()?-?1);
????second_part.assign(src.begin()?+?1,?src.end());
????T?pln1?=?BezierCurve(first_part);
????T?pln2?=?BezierCurve(second_part);
????for?(float?t?=?0;?t?????{
????????typename?T::iterator::value_type?temp{};
????????temp?+=?pln1[cvRound(1.0?/?step?*?t)]?*?(1.0?-?t)?;
????????temp?+=?pln2[cvRound(1.0?/?step?*?t)]?*?t;
????????res.emplace_back(temp);
????}
????return?res;
}
template?
T?UseBezierOptimizePath(T?path,uint8_t?order_number)
{
????if(path.size()?????????return?{};
????T?new_path{};
????for(uint8_t?i=0;i????{
????????T?tmp?=?BezierCurve(T(&path[i],&path[?i?+?order_number]));
????????new_path.insert(new_path.begin(),tmp.begin(),tmp.end());
????}
???
????return?new_path;
}
int?main(int?argc,?char?const*?argv[])
{
???while?(1)?{
????????cout<???????cout<???????cout<???????vector?path;
???????RNG?rng;
??????
???????for?(int?i?=?1;?i?<8;?i++)
???????????path.push_back(Point2f(i?*?800?/?8,?random()?%?800));//rng.uniform(0,800)));//cvRandInt(rng)?%?800));
???????Mat?img(900,?1200,?CV_8UC3);
???????img?=?0;
???????for(uint8_t?i?=0;i????????{
????????????cout<","<?????????line(img,Point(path[i].x,?path[i].y),Point(path[i+1].x,?path[i+1].y),?Scalar(255,?0,?0),?16,?LINE_AA,?0);
???????}
???????cout<????//????imshow("line",?img);
???????for?(int?i?=?0;?i????????????circle(img,?path[i],?3,?Scalar(0,?0,?255),?10);?//BGR
???
????//????vector?bezierPath?=?bezierCurve(path);
???????vector?bezierPath?=?UseBezierOptimizePath(path,4);
???????for?(int?i?=?0;?i?????????//????circle(img,?bezierPath[i],?3,?Scalar(0,?255,?255),?3);?//BGR
???????????img.at(cvRound(bezierPath[i].y),?cvRound(bezierPath[i].x))?=?{?0,?255,?255?};
????????//????printf("pose(%f?%f)\n",bezierPath[i].x,bezierPath[i].y);
????????????imshow("black",?img);
????????????//?waitKey(10);
???????}
???????if?(waitKey(0)?==?'q')
???????????break;
???}
???return?0;
}
顯示效果如下：
三階
四階
結(jié)語
這就是我自己的一些設不貝塞爾曲線的使用分享。如果大家有更好的想法和需求，也歡迎大家加我好友交流分享哈。
作者：良知猶存，白天努力工作，晚上原創(chuàng)公號號主。公眾號內(nèi)容除了技術還有些人生感悟，一個認真輸出內(nèi)容的職場老司機，也是一個技術之外豐富生活的人，攝影、音樂 and 籃球。關注我，與我一起同行。
???????????????? END ????????????????
推薦閱讀
【2】Linux開發(fā)coredump文件分析實戰(zhàn)分享
【3】CPU中的程序是怎么運行起來的 必讀
【4】cartographer環(huán)境建立以及建圖測試
本公眾號全部原創(chuàng)干貨已整理成一個目錄，回復[ 資源 ]即可獲得。