倒车轨迹理论实现方法

1、倒车轨迹理论实现方法                   帅文  王文梁关键字：倒车轨迹  视角转换 前言：倒车轨迹是近两年部分国产汽车导航设备上新出现的一个功能，其借助方向盘转角信息将汽车可能的后退路线叠加到后视摄像头的输出上并标注出距离，以直观形象化的形式协助驾驶人员调整选择倒车路线，减少驾驶人员特别是新手的误判断，对使用者是一个不错的实用功能。倒车轨迹

2、在智能倒车领域内属于辅助倒车系统中的一种，虽然其还无法达到智能化倒车，但是其实用性和辅助性上对汽车智能化单元技术方面是一个有效的补充。本文将基于使用为目的，从经验角度并结合基本数学推导分析倒车轨迹的原理、实现过程并给出实际使用过程中需要的操作点。由于本文非侧重于数学理论，对部分数学细节在不影响实际结论情况下不做深入探讨。一 倒车轨迹的基本原理  从日常经验可知，以自行车为例，如果前轮有一定转角，在维持转角不变状态和无轴向移动前提下自行车走过的路径将会以某个圆点为中心旋转，同样的状态也会出现在汽车上。其走过路径如图1。图中假设车轮不会出现轴向移动，故如果保持车轮转角

3、不变的情况下，每个车轮只能沿着垂直其车轴的方向行进，这里取前后轮的轴心作为轨迹跟踪点（实际过程中两个前轮轴心不会出现平行），则轨迹应该是以前后轮轴向线的焦点为圆心的圆。图中为为前轮同水平方向的夹角，记前后轮轴距为L，后轮轴长为W，后轮距离车尾的距离为D，从几何关系可知，后轮轴心的运动轨迹可以描述为以半径Lcot()的圆周运动。两个后轮的轨迹分别为Lcot()-W/2和Lcot()+W/2的圆。这里的推导过程采用经验法结合几何推算，完全从数学角度的推算过程请参考资料1。图中的x方向和y方向不同于一般习惯主要是考虑后面的视角变换。从等式可以看到，当接近0度时候行进轨迹近似直线，接近90度时半径呈缩

4、小趋势，符合我们日常经验值。二 视角转换   从倒车公式推导出的路线图为行进路线的俯视图，实际显示给操作者的路线应该是从车内观察点观察到的轨迹，驾驶人员看到的运动轨迹实际为以车尾摄像头为中心点坐标的图像描述（图一中车尾位置的原点）。将摄像头位置定为坐标零点，则轨迹上的任意点位置公式为：（x+Lcot()）2+(y+D)2=(Lcot()2     （1）上面推导的轨迹仍然是基于俯视条件下的轨迹，看到的应该为处于一定视角观察的轨迹，故需要进行一定角度的转换才能切换到实际观察到的图像。假设摄像头的可视角范围为

5、2，摄像头距离地面h，摄像头中心线同水平面的夹角为，输出屏幕的高度为H，这里假设摄像头相对于屏幕为一个点，会造成实际计算结果的一定偏差，关于偏差的细节数学计算不属本文讨论的重点。我们实际观察到的Yr为地面y在显示屏H上的投影，y方向的转换过程如图二：上图中为了便于计算，将经过摄像头采集后输出的显示器直接投影到同一个图中，这和实际输出没有区别。上图中的虚线为水平线，点线为屏幕的中心点到摄像头的连线，为中心线和水平线的夹角将上图进行简化可转为下列数学问题，等腰三角形中同顶点成已知角度所对应的边长，简化后的计算如图三。Yr对应的角度为+-，其中=arctg(h/y) 通过几何运算可得：三&

6、#160;计算轨迹的条件从以上计算我们可以看到，需要计算出轨迹，必须提供以下参数：1 摄像头的可视角范围2a2 摄像头距离地面距离h3 摄像头中心线同水平面的夹角4 输出屏幕的高度H和宽带W5 汽车前后轮轴距L6 汽车轴长W，后轮距离车尾的距离D7 前轮同水平方向的夹角以上参数中除转角外对于固定的车型和安装方式都已经固定，转角的获取可以有两种方式：1 对于有方向盘角度信息的车型可以直接通过输出接口如can总线获取。2外加角度传感器获取角度信息。两种方式都需要对采集的信息作一定的校正。 四 验证  将上面公

7、式的2和3带入1，可推导出显示屏幕上的实际轨迹图像。我们用vc实现了过程模拟，程序流程图四，模拟结果如图五。在真实的嵌入式平台上实现需要该平台支持图像叠加功能，目前许多多媒体soc芯片都能具备硬件融合功能，可以方便的将描绘出的轨迹叠加到摄像头的输入信号上，对于角度信息的采集，则要求平台有can总线接口（部分车型可以输出角度信息）或外加角度传感器接口获取车轮转角。五  结论：从验证情况看，结果和经验吻合，证明算法的主要正确性。一些细节信息需要针对具体的实现平台进行微调。以上算法对于下列场景无法解决，如果路面有斜坡由于整个系统无法感知而导致叠加图像有误，当倒车速度过快（如漂移）

8、会造成上述公式整体失效，但是对于普通用户场景-低速、路面相对平整还是主要场合，故将参数校准后会有较好的参考价值。/ BackTrack.h: interface for the CBackTrack class. / / #if !defined(AFX_BACKTRACK_H_EAF4E3A8_3691_44DA_A864_72658056A1D9_INCLUDED_) #define AFX_BACKTRACK_H_EAF4E3A8_3691_44DA_A864_72658056A1D9_INCLUDED_ #if _MSC_VER > 1000 #pragma once #endi

9、f / _MSC_VER > 1000 #include "RearCalibration.h" /* struct Mypoint int x; int y; ; typedef struct Mypoint MyPoint; */ class CBackTrack public: bool InitObject(CRearCalibration *pCali, float fAngle,float fAxleDist,float fWheelDist,int lWidth, int lHeight); void DrawExtendSpot(unsigned ch

10、ar *lpDIBBits, int lWidth, int lHeight, bool bInverse=false); void DrawExtendTrack(unsigned char *lpDIBBits, int lWidth, int lHeight); void DrawBackSpot(unsigned char *lpDIBBits, int lWidth, int lHeight); void DrawBackTrack(unsigned char *lpDIBBits, int lWidth, int lHeight, bool bInverse=false); /*

11、void Way2_Back_Track_Draw(char *lpDIBBits, int lWidth, int lHeight); void Back_Track_Draw(char *lpDIBBits, int lWidth, int lHeight); void Calibration_Track_Draw(CRearCalibration *pCali,char *lpDIBBits, int lWidth, int lHeight); void GetBackTrackDraw(CRearCalibration *pCali,char *lpDIBBits, int lWidt

12、h, int lHeight, float fAngle); */ void DrawTrack(unsigned char *lpDIBBits, int lWidth, int lHeight); CBackTrack(); virtual CBackTrack(); private: void InverseBmp(unsigned char *lpDIBBits, int lWidth, int lHeight); CRearCalibration *m_pCali; float m_fAngle; float m_VBack; int m_iTlong; int m_step; vo

13、id GetExtendUV(); void ComputerExtendXY(); void GetTfromView(int lWidth, int lHeight); void ComputerXY(); void ComputerUV(); CPoint GetUfromVofExtend(short v); CPoint GetUfromV(short v); void GetMarkXU(); short RelationYV(short v); int m_iNT; int m_iFT; float m_fLX40; float m_fLY40; float m_fRX40; f

14、loat m_fRY40; float m_fLeX40; float m_fLeY40; float m_fReX40; float m_fReY40; int m_iLU40; int m_iLV40; int m_iRU40; int m_iRV40; int m_iLeU40; int m_iLeV40; int m_iReU40; int m_iReV40; float m_LRearAxle; float m_LRearWheel; int m_U1288,m_U2288; ; #endif / !defined(AFX_BACKTRACK_H_EAF4E3A8_3691_44DA

15、_A864_72658056A1D9_INCLUDED_) / BackTrack.cpp: implementation of the CBackTrack class. / / #include "stdafx.h" #include "BackTrack.h" #include "math.h" #ifdef _DEBUG #undef THIS_FILE static char THIS_FILE=_FILE_; #define new DEBUG_NEW #endif / / Construction/Destruction

16、 / CBackTrack:CBackTrack() m_VBack = 0.1f; m_LRearAxle = 0; m_LRearWheel = 0; m_iTlong = 0; GetMarkXU(); CBackTrack:CBackTrack() / /分步骤进行编程 /* void CBackTrack:GetTfromView() /float VBack = 138.9; / 相机视野的最近点和最远点距离汽车后轴的距离，Ny要大于100，由于现在假定相机安装在后轴中心，故先付值100 int Ny,Fy; Ny = 100; Fy = 330; / 中间变量 float a,b

17、; int t; /判断分母中为零的项，a不可能为零，因为前轴中心的转向角永远小于50度 if( sin(m_fAngle) != 0 ) a = m_LRearAxle / (float)(tan (m_fAngle) - m_LRearWheel / 2; b = m_LRearAxle / m_VBack / (float)(sin (m_fAngle); m_iNT = (int)(b * (float)(asin ( Ny / a) + 0.5f); m_iFT = (int)(b * (float)(asin ( Fy / a) + 0.5f); /当这两项为零时，车辆没有倒车，因

18、此可以只把后轮的延长线画出 else m_iNT = (int)(Ny/m_VBack + 0.5f); m_iFT = (int)(Fy/m_VBack + 0.5f); / computer time jump step m_step = (m_iFT - m_iNT) / 30; m_iTlong = 0; for( t = m_iNT; t m_iFT; t = t + m_step) m_iTlong + ; */ void CBackTrack:GetTfromView(int lWidth, int lHeight) /float VBack = 138.9; / 相机视野的最近

19、点和最远点距离汽车后轴的距离，Ny要大于100，由于现在假定相机安装在后轴中心，故先付值100 int Ny,Fy; Ny = 100; Fy = 380; / 中间变量 /float a,b; int t; CPoint pl,pr; m_iNT = (int)(Ny/m_VBack + 0.5f); m_iFT = (int)(Fy/m_VBack + 0.5f); /* float a,b; a = m_LRearAxle / (float)tan (m_fAngle); b = m_LRearAxle / m_VBack / (float)sin (m_fAngle); do m_fL

20、X0 = -(a - m_LRearWheel / 2) * (float)(cos(m_iNT / b) + a; m_fLY0 = (a- m_LRearWheel / 2) * (float)(sin(m_iNT / b); pl = m_pCali->GetUVfromXY( m_fLX0, m_fLY0); m_fRX0 = -(a+m_LRearWheel / 2) * (float)(cos(m_iNT / b) + a; m_fRY0 = (a+ m_LRearWheel / 2)* (float)(sin(m_iNT / b); pr = m_pCali->Get

21、UVfromXY( m_fRX0, m_fRY0); m_iNT +; while( pl.x 1 | pl.y 1 | pr.x 1 | pr.y 1); do m_fLX0 = -(a - m_LRearWheel / 2) * (float)(cos(m_iFT / b) + a; m_fLY0 = (a- m_LRearWheel / 2) * (float)(sin(m_iFT / b); pl = m_pCali->GetUVfromXY( m_fLX0, m_fLY0); m_fRX0 = -(a+m_LRearWheel / 2) * (float)(cos(m_iFT

22、/ b) + a; m_fRY0 = (a+ m_LRearWheel / 2)* (float)(sin(m_iFT / b); pr = m_pCali->GetUVfromXY( m_fRX0, m_fRY0); m_iFT -; while( pl.x > lWidth | pl.y > lHeight | pr.x > lWidth | pr.y > lHeight); */ / computer time jump step m_step = (m_iFT - m_iNT) / 38; m_iTlong = 0; for( t = m_iNT; t m

23、_iFT; t = t + m_step) m_iTlong + ; /* ComputerXY() 计算倒车轨迹点 参数是方向盘转角，此处暂为前轴中心点的转角 */ void CBackTrack:ComputerXY() float a,b; a = m_LRearAxle / (float)(tan (m_fAngle); b = m_VBack * (float)(sin (m_fAngle) / m_LRearAxle; int t; int l = 0; for ( t = m_iNT ; t m_iFT ; t = t + m_step) / left rear wheel ba

24、ck track m_fLX l = - ( a - m_LRearWheel / 2) * (float)(cos ( b * t ) + a; m_fLY l = ( a - m_LRearWheel / 2) * (float)(sin( b * t ); / right rear wheel back track m_fRX l = - ( a + m_LRearWheel / 2) * (float)(cos ( b * t ) + a; m_fRY l = ( a + m_LRearWheel / 2) * (float)(sin( b * t ); l+; /* 车辆后轮延长线，

25、无论方向盘转角等参数如何变， 延长线上的点是不变的，因此可分开来写，尤其是当前轴 中心点的转角为零时，在图像上只显示后轮延长线，因此需要 单独写出来 */ void CBackTrack:ComputerExtendXY() int t; int l = 0; for(t = m_iNT ; t m_iFT ; t = t + m_step) / left rear wheel extend m_fLeX l = m_LRearWheel / 2; m_fLeY l = t*m_VBack; / right rear wheel extend m_fReX l = - m_LRearWheel

26、 / 2; m_fReY l = m_fLeY l ; l+; void CBackTrack:ComputerUV() int t; int l = 0; CPoint p; for( t = m_iNT ; t m_iFT ; t = t + m_step) / left rear wheel back track in image p = m_pCali->GetUVfromXY( m_fLXl, m_fLYl); m_iLUl = p.x; m_iLVl = p.y; /right rear wheel back track in image p = m_pCali->Ge

27、tUVfromXY( m_fRXl, m_fRYl); m_iRUl = p.x; m_iRVl = p.y; l+; void CBackTrack:GetExtendUV() int t; int l = 0; CPoint p; for( t = m_iNT ; t m_iFT ; t = t + m_step) / left rear wheel extend in image p = m_pCali->GetUVfromXY( m_fLeXl, m_fLeYl); m_iLeUl = p.x; m_iLeVl = p.y; / right rear wheel extend i

28、n image p = m_pCali->GetUVfromXY( m_fReXl, m_fReYl); m_iReUl = p.x; m_iReVl = p.y; l+; void CBackTrack:DrawBackSpot(unsigned char *lpDIBBits, int lWidth, int lHeight) int t ; long lLineBytes = WIDTHBYTES(lWidth * 8); for( t = 0 ; t m_iTlong ; t +) / left wheel track spot *(unsigned char *)lpDIBBi

29、ts + lLineBytes * m_iLVt + m_iLUt) = 255; *(unsigned char *)lpDIBBits + lLineBytes * (m_iLVt -1) + m_iLUt) = 255; *(unsigned char *)lpDIBBits + lLineBytes * (m_iLVt +1) + m_iLUt) = 255; *(unsigned char *)lpDIBBits + lLineBytes * m_iLVt + m_iLUt -1) = 255; *(unsigned char *)lpDIBBits + lLineBytes * m

30、_iLVt + m_iLUt + 1) = 255; / right wheel track spot *(unsigned char *)lpDIBBits + lLineBytes * m_iRVt + m_iRUt) = 255; *(unsigned char *)lpDIBBits + lLineBytes * (m_iRVt -1) + m_iRUt) = 255; *(unsigned char *)lpDIBBits + lLineBytes * (m_iRVt +1) + m_iRUt) = 255; *(unsigned char *)lpDIBBits + lLineBy

31、tes * m_iRVt + m_iRUt -1) = 255; *(unsigned char *)lpDIBBits + lLineBytes * m_iRVt + m_iRUt + 1) = 255; void CBackTrack:DrawExtendSpot(unsigned char *lpDIBBits, int lWidth, int lHeight, bool bInverse) if(bInverse) InverseBmp(lpDIBBits,lWidth,lHeight); int t ; long lLineBytes = WIDTHBYTES(lWidth * 8)

32、; for( t = 0 ; t m_iTlong ; t +) / left wheel track spot *(unsigned char *)lpDIBBits + lLineBytes * m_iLeVt + m_iLeUt) = 255; *(unsigned char *)lpDIBBits + lLineBytes * (m_iLeVt -1) + m_iLeUt) = 255; *(unsigned char *)lpDIBBits + lLineBytes * (m_iLeVt +1) + m_iLeUt) = 255; *(unsigned char *)lpDIBBit

33、s + lLineBytes * m_iLeVt + m_iLeUt -1) = 255; *(unsigned char *)lpDIBBits + lLineBytes * m_iLeVt + m_iLeUt + 1) = 255; / right wheel track spot *(unsigned char *)lpDIBBits + lLineBytes * m_iReVt + m_iReUt) = 255; *(unsigned char *)lpDIBBits + lLineBytes * (m_iReVt -1) + m_iReUt) = 255; *(unsigned ch

34、ar *)lpDIBBits + lLineBytes * (m_iReVt +1) + m_iReUt) = 255; *(unsigned char *)lpDIBBits + lLineBytes * m_iReVt + m_iReUt -1) = 255; *(unsigned char *)lpDIBBits + lLineBytes * m_iReVt + m_iReUt + 1) = 255; if(bInverse) InverseBmp(lpDIBBits,lWidth,lHeight); void CBackTrack:DrawBackTrack(unsigned char

35、 *lpDIBBits, int lWidth, int lHeight, bool bInverse) if(bInverse) InverseBmp(lpDIBBits,lWidth,lHeight); int t,tj ; float k ; int U,offset; long lLineBytes = WIDTHBYTES(lWidth * 8); for( t = 0 ; t m_iTlong - 1 ; t +) / draw left wheel back track k = (float) ( m_iLU t - m_iLU t + 1 ) / (float) ( m_iLV

36、 t - m_iLV t + 1 ); if( m_iLV t m_iLV t + 1 ) for ( tj = m_iLV t ; tj m_iLV t + 1 ; tj+) offset = lLineBytes * tj; U = (int) (m_iLU t - k * (m_iLV t - tj); if( U lWidth - 1 && tj lHeight - 1 && tj >1 && U >1 ) *(unsigned char *)lpDIBBits + offset + U) = 255; *(unsigned

37、char *)lpDIBBits + offset - lLineBytes + U) = 255; *(unsigned char *)lpDIBBits + offset + lLineBytes + U) = 255; *(unsigned char *)lpDIBBits + offset + U +1) = 255; *(unsigned char *)lpDIBBits + offset + U -1) = 255; else if( m_iLV t > m_iLV t + 1 ) for ( tj = m_iLV t ; tj > m_iLV t + 1 ; tj-)

38、 offset = lLineBytes * tj; U = (int) (m_iLU t - k * (m_iLV t - tj); if( U lWidth - 1 && tj lHeight - 1 && tj >1 && U >1 ) *(unsigned char *)lpDIBBits + offset + U) = 255; *(unsigned char *)lpDIBBits + offset - lLineBytes + U) = 255; *(unsigned char *)lpDIBBits + offset

39、+ lLineBytes + U) = 255; *(unsigned char *)lpDIBBits + offset + U +1) = 255; *(unsigned char *)lpDIBBits + offset + U -1) = 255; / draw right wheel back track k = (float) ( m_iRU t - m_iRU t + 1 ) / (float) ( m_iRV t - m_iRV t + 1 ); if( m_iRV t m_iRV t + 1 ) for ( tj = m_iRV t ; tj m_iRV t + 1 ; tj

40、+) offset = lLineBytes * tj; U = (int) (m_iRU t - k * (m_iRV t - tj); if( U lWidth - 1 && tj lHeight - 1 && tj >1 && U >1 ) *(unsigned char *)lpDIBBits + offset + U) = 255; *(unsigned char *)lpDIBBits + offset - lLineBytes + U) = 255; *(unsigned char *)lpDIBBits + offse

41、t + lLineBytes + U) = 255; *(unsigned char *)lpDIBBits + offset + U +1) = 255; *(unsigned char *)lpDIBBits + offset + U -1) = 255; else if( m_iRV t > m_iRV t + 1 ) for ( tj = m_iRV t ; tj > m_iRV t + 1 ; tj-) offset = lLineBytes * tj; U = (int) (m_iRU t - k * (m_iRV t - tj); if( U lWidth - 1 &

42、amp;& tj lHeight - 1 && tj >1 && U >1 ) *(unsigned char *)lpDIBBits + offset + U) = 255; *(unsigned char *)lpDIBBits + offset - lLineBytes + U) = 255; *(unsigned char *)lpDIBBits + offset + lLineBytes + U) = 255; *(unsigned char *)lpDIBBits + offset + U +1) = 255; *(unsigne

43、d char *)lpDIBBits + offset + U -1) = 255; if(bInverse) InverseBmp(lpDIBBits,lWidth,lHeight); void CBackTrack:DrawExtendTrack(unsigned char *lpDIBBits, int lWidth, int lHeight) int t ,tj; float k ; int V; long lLineBytes = WIDTHBYTES(lWidth * 8); for( t = 0 ; t m_iTlong - 1 ; t +) / draw left wheel

44、extend track if( m_iLeU t > m_iLeU t + 1 ) for ( tj = m_iLeU t ; tj > m_iLeU t + 1 ; tj-) k = (float) ( m_iLeV t - m_iLeV t + 1 ) / (float) ( m_iLeU t - m_iLeU t + 1 ); V = (int) ( k * tj + m_iLeV t - k * m_iLeU t ); if( tj lWidth - 1 && V lHeight - 1 && tj >1 && V &

45、gt;1 ) *(unsigned char *)lpDIBBits + lLineBytes * V + tj) = 255; *(unsigned char *)lpDIBBits + lLineBytes * ( V - 1 ) + tj) = 255; *(unsigned char *)lpDIBBits + lLineBytes * (V + 1) + tj) = 255; *(unsigned char *)lpDIBBits + lLineBytes * V + tj +1) = 255; *(unsigned char *)lpDIBBits + lLineBytes * V

46、 + tj -1) = 255; else for ( tj = m_iLeU t ; tj m_iLeU t + 1 ; tj+) k = (float) ( m_iLeV t - m_iLeV t + 1 ) / (float) ( m_iLeU t - m_iLeU t + 1 ); V = (int) ( k * tj + m_iLeV t - k * m_iLeU t ); if( tj lWidth - 1 && V lHeight - 1 && tj >1 && V >1 ) *(unsigned char *)lpDI

47、BBits + lLineBytes * V + tj) = 255; *(unsigned char *)lpDIBBits + lLineBytes * ( V - 1 ) + tj) = 255; *(unsigned char *)lpDIBBits + lLineBytes * (V + 1) + tj) = 255; *(unsigned char *)lpDIBBits + lLineBytes * V + tj +1) = 255; *(unsigned char *)lpDIBBits + lLineBytes * V + tj -1) = 255; / draw right

48、 wheel extend track if( m_iReU t > m_iReU t + 1 ) for ( tj = m_iReU t ; tj > m_iReU t + 1 ; tj-) k = (float) ( m_iReV t - m_iReV t + 1 ) / (float) ( m_iReU t - m_iReU t + 1 ); V = (int) ( k * tj + m_iReV t - k * m_iReU t ); if( tj lWidth - 1 && V lHeight - 1 && tj >1 &&a

49、mp; V >1 ) *(unsigned char *)lpDIBBits + lLineBytes * V + tj) = 255; *(unsigned char *)lpDIBBits + lLineBytes * ( V - 1 ) + tj) = 255; *(unsigned char *)lpDIBBits + lLineBytes * (V + 1) + tj) = 255; *(unsigned char *)lpDIBBits + lLineBytes * V + tj +1) = 255; *(unsigned char *)lpDIBBits + lLineBytes * V + tj -1) = 255; else for ( tj = m_iReU t ; tj m_iReU t + 1 ; tj+) k = (float) ( m_iReV t - m_iReV t + 1