前言

之前被安排了活，一個(gè)局部區(qū)域機(jī)器運(yùn)動(dòng)控制的工作，大致是一個(gè)機(jī)器位于一個(gè)極限區(qū)域時(shí)候，機(jī)器要進(jìn)入一個(gè)特殊的機(jī)制，使得機(jī)器可以安全的走出來(lái)。其中用到了bezier曲線進(jìn)行優(yōu)化路徑，今天寫(xiě)一下，正好也給大家分享一下工作和實(shí)踐的情況。

作者：良知猶存

轉(zhuǎn)載授權(quán)以及圍觀：歡迎關(guān)注微信公眾號(hào)：羽林君

或者添加作者個(gè)人微信：become_me

貝塞爾曲線基本介紹

線段都可以被拆分成兩個(gè)坐標(biāo)的差來(lái)表示,如下面一階的貝塞爾曲線，P0到P1，可以用一個(gè)t進(jìn)行拆分這段線,分別是線段 t(P0～P1)、線段 1-t(P0～P1)，P0和P1叫做， 這條條貝塞爾的兩個(gè)控制點(diǎn)，而貝塞爾曲線至少要有兩個(gè)控制點(diǎn)（就是下面的這條直線，一階貝塞爾曲線）。在貝塞爾曲線與控制點(diǎn)位置相關(guān)，這意味著在曲線生成過(guò)程中，我們可以通過(guò)調(diào)節(jié)控制點(diǎn)的位置，進(jìn)而調(diào)整整個(gè)曲線。

貝塞爾的階數(shù)和次數(shù)是一樣的，二階貝塞爾，三個(gè)點(diǎn)，最高次數(shù)二次。例：二階貝塞爾：三個(gè)點(diǎn)，兩個(gè)線段，以所有等比的點(diǎn)組合成的曲線叫做二階貝塞爾曲線。

接下來(lái)給大家介紹一下貝塞爾曲線的推導(dǎo)工程，也比較簡(jiǎn)單，并且網(wǎng)上的介紹也挺多的：

一階：

這里面有兩個(gè)控制點(diǎn)為 ，對(duì)應(yīng)的曲線方程為：

t?[0,1] 這個(gè)方程可以理解為，從出發(fā)，朝著的方向前進(jìn)的距離，從而得到了點(diǎn)B(t)的位置。t從0逐漸遞增到1，這個(gè)過(guò)程完成，就成了我們所看到的曲線。

另外，之所以是一階貝塞爾曲線是因?yàn)榉匠淌顷P(guān)于t的一階多項(xiàng)式，多階也是一樣。

二階：有三個(gè)控制點(diǎn)，這里的 P0、P1、P2 分別稱之為控制點(diǎn)，曲線的產(chǎn)生完全與這三個(gè)點(diǎn)位置相關(guān)。

與一階有些區(qū)別就在于三個(gè)控制點(diǎn)形成兩個(gè)線段，每個(gè)線段上有一個(gè)點(diǎn)在運(yùn)動(dòng)，于是得到兩個(gè)點(diǎn)；再使用兩個(gè)點(diǎn)形成一個(gè)線段，這個(gè)線段上有一個(gè)點(diǎn)在運(yùn)動(dòng)，于是得到一個(gè)點(diǎn)；最后一個(gè)點(diǎn)的運(yùn)動(dòng)軌跡便構(gòu)成了二階貝塞爾曲線。

對(duì)應(yīng)的曲線方程為：

這是一條迭代公式，每次迭代都會(huì)少掉一個(gè)“點(diǎn)”。

最后得：

三階：有四個(gè)控制點(diǎn)

設(shè)控制點(diǎn)為P0，P1，P2和P4，曲線方程為：

配圖這是matlab生成的gif動(dòng)畫(huà)，大家想要的也可以找我，代碼私發(fā)給大家。

N階：

我們發(fā)現(xiàn)，實(shí)際上是每輪都是 n 個(gè)點(diǎn)，形成 n-1 條線段，每個(gè)線段上有一個(gè)點(diǎn)在運(yùn)動(dòng)，那么就只關(guān)注這 n-1 個(gè)點(diǎn)，循環(huán)往復(fù)。最終只剩一個(gè)點(diǎn)時(shí)，它的軌跡便是結(jié)果。

如此一來(lái)，你會(huì)發(fā)現(xiàn)貝塞爾曲線內(nèi)的遞歸結(jié)構(gòu)。實(shí)際上，上述介紹的分別是一階、二階、三階的貝塞爾曲線，貝塞爾曲線可以由階數(shù)遞歸定義。

N階貝塞爾曲線公式:

貝塞爾曲線應(yīng)用

貝塞爾曲線在動(dòng)畫(huà)中有應(yīng)用，前端以及一些其他顯示要求；此外在路徑規(guī)劃過(guò)程中，也會(huì)使用貝塞爾曲線進(jìn)行規(guī)劃好路徑再優(yōu)化，我就是使用了后者進(jìn)行優(yōu)化規(guī)劃好的路徑，使得機(jī)器行走更加順暢，不過(guò)使用中大家需要按照機(jī)器實(shí)際相應(yīng)來(lái)進(jìn)行調(diào)整t的精度以及階數(shù)。

由于貝塞爾曲線本身的數(shù)學(xué)表達(dá)式便是一條遞歸式，所以決定采用遞歸的方式來(lái)實(shí)現(xiàn)。代碼如下，BezierCurve函數(shù)實(shí)現(xiàn)貝塞爾曲線迭代，UseBezierOptimizePath函數(shù)的第二個(gè)參數(shù)進(jìn)行控制使用的階數(shù)，最后調(diào)用opencv實(shí)現(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é)語(yǔ)

這就是我自己的一些設(shè)不貝塞爾曲線的使用分享。如果大家有更好的想法和需求，也歡迎大家加我好友交流分享哈。

作者：良知猶存，白天努力工作，晚上原創(chuàng)公號(hào)號(hào)主。公眾號(hào)內(nèi)容除了技術(shù)還有些人生感悟，一個(gè)認(rèn)真輸出內(nèi)容的職場(chǎng)老司機(jī)，也是一個(gè)技術(shù)之外豐富生活的人，攝影、音樂(lè) and 籃球。關(guān)注我，與我一起同行。

                              ????????????????  END  ????????????????