倒车轨迹理论实现方法

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

倒车轨迹在智能倒车领域内属于辅助倒车系统中的一种，虽然其还无法达到智能化倒车，但是其实用性和辅助性上对汽车智能化单元技术方面是一个有效的补充。

本文将基于使用为目的，从经验角度并结合基本数学推导分析倒车轨迹的原理、实现过程并给出实际使用过程中需要的操作点。

由于本文非侧重于数学理论，对部分数学细节在不影响实际结论情况下不做深入探讨。

一倒车轨迹的基本原理从日常经验可知，以自行车为例，如果前轮有一定转角，在维持转角不变状态和无轴向移动前提下自行车走过的路径将会以某个圆点为中心旋转，同样的状态也会出现在汽车上。

其走过路径如图1。

图中假设车轮不会出现轴向移动，故如果保持车轮转角不变的情况下，每个车轮只能沿着垂直其车轴的方向行进，这里取前后轮的轴心作为轨迹跟踪点（实际过程中两个前轮轴心不会出现平行），则轨迹应该是以前后轮轴向线的焦点为圆心的圆。

图中φ为为前轮同水平方向的夹角，记前后轮轴距为L，后轮轴长为W，后轮距离车尾的距离为D，从几何关系可知，后轮轴心的运动轨迹可以描述为以半径Lcot(φ)的圆周运动。

两个后轮的轨迹分别为Lcot(φ)-W/2和Lcot(φ)+W/2的圆。

这里的推导过程采用经验法结合几何推算，完全从数学角度的推算过程请参考资料1。

图中的x方向和y方向不同于一般习惯主要是考虑后面的视角变换。

从等式可以看到，当φ接近0度时候行进轨迹近似直线，接近90度时半径呈缩小趋势，符合我们日常经验值。

二视角转换从倒车公式推导出的路线图为行进路线的俯视图，实际显示给操作者的路线应该是从车内观察点观察到的轨迹，驾驶人员看到的运动轨迹实际为以车尾摄像头为中心点坐标的图像描述（图一中车尾位置的原点）。

将摄像头位置定为坐标零点，则轨迹上的任意点位置公式为：（x+Lcot(φ)）2+(y+D)2=(Lcot(φ))2（1）上面推导的轨迹仍然是基于俯视条件下的轨迹，看到的应该为处于一定视角观察的轨迹，故需要进行一定角度的转换才能切换到实际观察到的图像。

假设摄像头的可视角范围为2α，摄像头距离地面h，摄像头中心线同水平面的夹角为β，输出屏幕的高度为H，这里假设摄像头相对于屏幕为一个点，会造成实际计算结果的一定偏差，关于偏差的细节数学计算不属本文讨论的重点。

我们实际观察到的Yr为地面y在显示屏H上的投影，y方向的转换过程如图二：上图中为了便于计算，将经过摄像头采集后输出的显示器直接投影到同一个图中，这和实际输出没有区别。

上图中的虚线为水平线，点线为屏幕的中心点到摄像头的连线，β为中心线和水平线的夹角将上图进行简化可转为下列数学问题，等腰三角形中同顶点成已知角度所对应的边长，简化后的计算如图三。

Yr对应的角度为α+β-θ，其中θ=arctg(h/y)通过几何运算可得：三计算轨迹的条件从以上计算我们可以看到，需要计算出轨迹，必须提供以下参数：1摄像头的可视角范围2a2摄像头距离地面距离h3 摄像头中心线同水平面的夹角β4 输出屏幕的高度H和宽带W5汽车前后轮轴距L6汽车轴长W，后轮距离车尾的距离D7 前轮同水平方向的夹角φ以上参数中除转角φ外对于固定的车型和安装方式都已经固定，转角φ的获取可以有两种方式：1对于有方向盘角度信息的车型可以直接通过输出接口如can总线获取。

2外加角度传感器获取角度信息。

两种方式都需要对采集的信息作一定的校正。

四验证将上面公式的2和3带入1，可推导出显示屏幕上的实际轨迹图像。

我们用vc实现了过程模拟，程序流程图四，模拟结果如图五。

在真实的嵌入式平台上实现需要该平台支持图像叠加功能，目前许多多媒体soc芯片都能具备硬件α融合功能，可以方便的将描绘出的轨迹叠加到摄像头的输入信号上，对于角度信息的采集，则要求平台有can总线接口（部分车型可以输出角度信息）或外加角度传感器接口获取车轮转角。

五结论：从验证情况看，结果和经验吻合，证明算法的主要正确性。

一些细节信息需要针对具体的实现平台进行微调。

以上算法对于下列场景无法解决，如果路面有斜坡由于整个系统无法感知而导致叠加图像有误，当倒车速度过快（如漂移）会造成上述公式整体失效，但是对于普通用户场景-低速、路面相对平整还是主要场合，故将参数校准后会有较好的参考价值。

////////////////////////////////////////////////////////////////////////#include ""#include ""#include ""#ifdef _DEBUG#undef THIS_FILEstatic char THIS_FILE[]=__FILE__;#define new DEBUG_NEW#endif//////////////////////////////////////////////////////////////////////// Construction/Destruction////////////////////////////////////////////////////////////////////// CBackTrack::CBackTrack(){m_VBack = ;m_LRearAxle = 0;m_LRearWheel = 0;m_iTlong = 0;GetMarkXU();}CBackTrack::~CBackTrack(){}//////////////////////////////////////////////////////////////分步骤进行编程/*void CBackTrack::GetTfromView(){// float VBack = ;// 相机视野的最近点和最远点距离汽车后轴的距离，Ny要大于100，由于现在假定相机安装在后轴中心，故先付值100int Ny,Fy;Ny = 100;Fy = 330;// 中间变量float a,b;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)) + ;m_iFT = (int)(b * (float)(asin ( Fy / a)) + ;}//当这两项为零时，车辆没有倒车，因此可以只把后轮的延长线画出else{m_iNT = (int)(Ny/m_VBack + ;m_iFT = (int)(Fy/m_VBack + ;}// computer time jump stepm_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 = ;// 相机视野的最近点和最远点距离汽车后轴的距离，Ny要大于100，由于现在假定相机安装在后轴中心，故先付值100int Ny,Fy;Ny = 100;Fy = 380;// 中间变量// float a,b;int t;CPoint pl,pr;m_iNT = (int)(Ny/m_VBack + ;m_iFT = (int)(Fy/m_VBack + ;/*float a,b;a = m_LRearAxle / (float)tan (m_fAngle);b = m_LRearAxle / m_VBack / (float)sin (m_fAngle);do{m_fLX[0] = -(a - m_LRearWheel / 2) * (float)(cos(m_iNT / b)) + a;m_fLY[0] = (a- m_LRearWheel / 2) * (float)(sin(m_iNT / b));pl = m_pCali->GetUVfromXY( m_fLX[0], m_fLY[0]);m_fRX[0] = -(a+m_LRearWheel / 2) * (float)(cos(m_iNT / b)) + a;m_fRY[0] = (a+ m_LRearWheel / 2)* (float)(sin(m_iNT / b));pr = m_pCali->GetUVfromXY( m_fRX[0], m_fRY[0]);m_iNT ++;} while( 1 || 1 || 1 || 1);do{m_fLX[0] = -(a - m_LRearWheel / 2) * (float)(cos(m_iFT / b)) + a;m_fLY[0] = (a- m_LRearWheel / 2) * (float)(sin(m_iFT / b));pl = m_pCali->GetUVfromXY( m_fLX[0], m_fLY[0]);m_fRX[0] = -(a+m_LRearWheel / 2) * (float)(cos(m_iFT / b)) + a;m_fRY[0] = (a+ m_LRearWheel / 2)* (float)(sin(m_iFT / b));pr = m_pCali->GetUVfromXY( m_fRX[0], m_fRY[0]);m_iFT --;} while( > lWidth || > lHeight || > lWidth || > lHeight);*/// computer time jump stepm_step = (m_iFT - m_iNT) / 38;m_iTlong = 0;for( t = m_iNT; t m_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 back trackm_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 trackm_fRX [ l ] = - ( a + m_LRearWheel / 2) * (float)(cos ( b *t )) + a;m_fRY [ l] = ( a + m_LRearWheel / 2) * (float)(sin( b * t ));l++;}}/**************************************************车辆后轮延长线，无论方向盘转角等参数如何变，延长线上的点是不变的，因此可分开来写，尤其是当前轴中心点的转角为零时，在图像上只显示后轮延长线，因此需要单独写出来****************************************************/void CBackTrack::ComputerExtendXY(){int t;int l = 0;for(t = m_iNT ; t m_iFT ; t = t + m_step){// left rear wheel extendm_fLeX[ l ] = m_LRearWheel / 2;m_fLeY[ l ] = t*m_VBack;// right rear wheel extendm_fReX[ l ] = - m_LRearWheel / 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 imagep = m_pCali->GetUVfromXY( m_fLX[l], m_fLY[l]);m_iLU[l] = ;m_iLV[l] = ;//right rear wheel back track in imagep = m_pCali->GetUVfromXY( m_fRX[l], m_fRY[l]);m_iRU[l] = ;m_iRV[l] = ;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 imagep = m_pCali->GetUVfromXY( m_fLeX[l], m_fLeY[l]);m_iLeU[l] = ;m_iLeV[l] = ;// right rear wheel extend in imagep = m_pCali->GetUVfromXY( m_fReX[l], m_fReY[l]);m_iReU[l] = ;m_iReV[l] = ;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 *)lpDIBBits + lLineBytes * m_iLV[t] +m_iLU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * (m_iLV[t] -1) + m_iLU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * (m_iLV[t ]+1) + m_iLU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * m_iLV[t] +m_iLU[t] -1) = 255;*((unsigned char *)lpDIBBits + lLineBytes * m_iLV[t] +m_iLU[t] + 1) = 255;// right wheel track spot*((unsigned char *)lpDIBBits + lLineBytes * m_iRV[t] + m_iRU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * (m_iRV[t] -1) + m_iRU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * (m_iRV[t ]+1) + m_iRU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * m_iRV[t] +m_iRU[t] -1) = 255;*((unsigned char *)lpDIBBits + lLineBytes * m_iRV[t] +m_iRU[t] + 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);for( t = 0 ; t m_iTlong ; t ++){// left wheel track spot*((unsigned char *)lpDIBBits + lLineBytes * m_iLeV[t] +m_iLeU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * (m_iLeV[t] -1) + m_iLeU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * (m_iLeV[t ]+1) + m_iLeU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * m_iLeV[t] +m_iLeU[t] -1) = 255;*((unsigned char *)lpDIBBits + lLineBytes * m_iLeV[t] +m_iLeU[t] + 1) = 255;// right wheel track spot*((unsigned char *)lpDIBBits + lLineBytes * m_iReV[t] + m_iReU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * (m_iReV[t] -1) + m_iReU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * (m_iReV[t ]+1) + m_iReU[t]) = 255;*((unsigned char *)lpDIBBits + lLineBytes * m_iReV[t] +m_iReU[t] -1) = 255;*((unsigned char *)lpDIBBits + lLineBytes * m_iReV[t] +m_iReU[t] + 1) = 255;}if(bInverse)InverseBmp(lpDIBBits,lWidth,lHeight);}void CBackTrack::DrawBackTrack(unsigned char *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 trackk = (float) ( m_iLU [ t ] - m_iLU [ t + 1] ) / (float)( m_iLV [ 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 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--){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 + lLineBytes + U) = 255;*((unsigned char *)lpDIBBits + offset + U +1) = 255;}}}// draw right wheel back trackk = (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++){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 + offset + 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 && 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;}}}}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 extend trackif( 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 >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;}}}elsefor ( 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 *)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;}}}// draw right wheel extend trackif( 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 && 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 ] ) / (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 && 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;}}}}}bool CBackTrack::InitObject(CRearCalibration *pCali, float fAngle,float fAxleDist,float fWheelDist,int lWidth, int lHeight){if(!pCali->IfDoneCali()){return false;}m_LRearAxle = fAxleDist;m_LRearWheel = fWheelDist;m_pCali = pCali;m_fAngle = fAngle;GetTfromView(lWidth, lHeight);ComputerExtendXY();;GetExtendUV();ComputerXY();ComputerUV();return true;}void CBackTrack::InverseBmp(unsigned char *lpDIBBits, int lWidth, int lHeight){unsigned char *pData = new unsigned char[lWidth*lHeight];int i,j,off1=0,off2=(lHeight-1)*lWidth;for(i=0;i<lHeight;i++){for(j=0;j<lWidth;j++){pData[off2 + j] = lpDIBBits[off1 + j];}off1 += lWidth;off2 -= lWidth;}memcpy(lpDIBBits,pData,lWidth*lHeight);delete []pData;}/*08-01-09 彭海娟在辅助停车类中增加了一个私有成员数组m_U1[213]，m_U2[213]，用于存放两个标线在每行上的U值*//***************************************RelationYV()根据相机的理想小孔成像原理，计算标定板的路面坐标系Y与相应图像坐标系V之间的对应关系，假设Y与V成线性关系，并且假设每块的对应关系不同。