飞思卡尔智能车赛道曲率计算

BIT_AETC HWQ 1/2 曾看到智能车制作论坛里很多人询问曲率的计算，今天在整理一个 的 时候，刚好又用到了曲率计算，我就解释下东北大学和上海大学摄像头组计算曲 率的方法（请参看第二届东北大学技术报告和第三届上海大学技术报告）。 曲率问题可以归结为已知三角形的三点坐标 1 1( , )A x y 、 2 2( , )B x y 、 3 3( , )C x y ， 求解三角形外接圆曲率。 学过高数的都应该知道向量的叉乘，定义为：两个向量进行叉乘得到的是一 个向量，方向垂直于这两个向量构成的平面（三个向量符合右手坐标系），大小 等于这两个向量组成的平行四边形的面积。 设两个向量为 ( , , )a a aa x y z=G 、 ( , , )b b bb x y z= G ，则 a a a b b b i j k a b x y z x y z × = GG G GG （1） 由叉乘的定义可知，求 ABCΔ 的面积，可以通过求解 2 AB AC×JJJG JJJG 来获得，将 上述叉乘应用于二维情况，即取 0z = ，可得 2 1 2 1 2 1 3 1 3 1 2 1 3 1 3 1 0 (( )( ) ( )( )) 0 i j k AB AC x x y y x x y y x x y y k x x y y × = − − = − − − − − − − GG G JJJG JJJG G （2） 所以 2 1 3 1 3 1 2 1(( )( ) ( )( )) 2 2ABC AB AC x x y y x x y ySΔ × − − − − −= = JJJG JJJG （3） 上式中，如果 ABC 三点是顺时针方向分布，则三角形面积为负值，逆时针 分布为正值（如果不需要符号，取下绝对值即可）。 A B C 面积的符号对于智能车其实还是挺有用的。如上图，如果曲线向右拐，算出 的面积是负的，如果曲线向左拐，算出的面积是正的，上述面积的正负反映了曲 线的方向。相比于海伦公式，用上述公式计算面积既可以减轻计算量，又可以反 映曲线的方向，一举两得。 有了上述公式，曲率可以现为三角形外接圆半径的倒数，从而可得曲率计 算公式： BIT_AETC HWQ 2/2 ABC4SK AB BC AC Δ= × × (4) 上式中要求出三边长，会用到求根公式，在单片机中开根号，那是很要命的。 如果精度要求不高，可以自已写一个简单的求根函数，东北大学给出的函数是 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); } 还有类似的求根算法可以看一下如下链接： http://hi.baidu.com/ssmsky_2006/blog/item/1e0e72fb40ade46d034f562f.html 注意：计算曲率的方法如果要提高精度，最好是能够将采到的畸形图像做一 下校正，以反映实际坐标。