曲率

彭岸輝

從理論上講，相對于偏差量，曲率是智能車更好的一個控制變量。但由于路徑檢測單元的局限性，很難計算出非常精確的曲率。

注意：計算曲率的方法如果要提高精度，最好是能夠將采到的畸形圖像做一下校正，以反映實際坐標。

曲率問題可以歸結為已知三角形的三點坐標?A(?x1,?y1?)、B(?x2?,?y2?)、C?(?x3?,?y3?)，求解三角形外接圓曲率。

學過高數的都應該知道向量的叉乘，定義為：兩個向量進行叉乘得到的是一個向量，方向垂直于這兩個向量構成的平面(三個向量符合右手坐標系)，大小等于這兩個向量組成的平行四邊形的面積。

設兩個向量為?a

=?(?xa?,?ya?,?za?)?、?b

=?(?xb?,?yb?,?zb?)則

由叉乘的定義可知，求?ΔABC的面積，可以通過求解來獲得，將上述叉乘公式應用于二維情

況，即取?z?=?0，可得

所以

上式中，如果?ABC三點是順時針方向分布，則三角形面積為負值，逆時針分布為正值(如果不需要符號，取下絕對值即可)。

圖示為：

面積的符號對于智能車其實還是挺有用的。如上圖，如果曲線向右拐，算出的面積是負的，如果曲線向左拐，算出的面積是正的，上述面積的正負反映了曲線的方向。相比于海倫公式，用上述公式計算面積既可以減輕計算量，又可以反映曲線的方向，一舉兩得。有了上述公式，曲率可以表現為三角形外接圓半徑的倒數，從而可得曲率計算公式：

上式中要求出三邊長，會用到求根公式，在單片機中開根號，那是很要命的。如果精度要求不高，可以自已寫一個簡單的求根函數，東北大學的技術報告給出的求根函數是

unsigned?int?m_sqrt(unsigned?int?x)

{

uchar?ans=0,p=0x80;

while(p!=0)

{

ans+=p;

if(ans*ans>x)

ans-=p;

p=(uchar)(p/2);

}

return(ans);

}

曲率計算的參考代碼：

void?qulv()

{

int?x1,x2,x3,y1,y2,y3;

unsigned?int?length1,length2,length3;

int?S;

y1=5;

y2=10;

y3=15;

x1=mid_line[2];

x2=mid_line[1];

x3=mid_line[0];

S=abs((y2-y1)*(x3-x1)-(y3-y1)*(x2-x1));

length1=sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));

length2=sqrt((x3-x1)*(x3-x1)+(y3-y1)*(y3-y1));

length3=sqrt((x2-x3)*(x2-x3)+(y2-y3)*(y2-y3));

if(S<1)

S=1;

K=abs((length1*length2*length3)/S);//曲率的倒數，即曲率半徑

Center_Average=mid(Center[0],Center[1],Center[2]);

大家具體可以看下往屆的技術報告。這里只是講解下這三種常用的方法，其他的方法就不再訴說了。

PS：限于作者的水平，對于文檔中存在的不嚴謹甚至錯誤，希望各位能夠予以指教，

本人聯系郵箱：860038779@qq.com。

總結

以上是生活随笔為你收集整理的智能车改舵机中值步骤_飞思卡尔智能车摄像头组新手指南（10）--控制算法进阶篇之舵机3...的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。