(舵机算法.docxVIP

根据前面介绍的CMOS图像传感器采集处理得到的只是40行中每一行的黑线坐标。并没有得出赛道的具体形状，到底是直道还是弯道，是大弯还是急弯，是偏向一个方向的弯道还是“S型”弯道，是“大S型”弯道还是“小S型”弯道。这就可以通过曲率的计算来得出。下面给出曲率的具体计算方法。首先，将扫描到的40行中每9行分为一组，而且彼此交叉，也就是说，在能扫描到的情况下，划分为从第0行到第8行，从第1行到第9行，从第2行到第10行，依此类推，最后从21行到第29行，一共可以划分为22段，分别计算每一段的曲率，以第一段为例，计算的方法为：将第i行的坐标定义为coordinate[i]，将CMOS图像传感器的中心坐标定义为middle，那么每一行相对于中心坐标的偏差就为ek[i]=coordinate[i]-middle (4.1)再乘以每一行各自的校正值emendation[i]，计算公式为ek[i]=ek[i]*emendation[i] (4.2)就得到了各行实际偏离中心位置的距离。然后用第0行的偏差加上第8行的偏差之和除以2再减去第4行的偏差，得出一个相对偏差ek_comparatively。ek_comparatively的计算公式为ek_comparatively＝(ek[8]+ek[0])/2-ek[4] (4.3)然后计算出该段曲线对应的直道，即将第0行的黑点与第8行的黑点直接相连的直线的长度计算出来，计算公式为distance_beeline=sqrt((ek[8]-ek[0])*(ek[8]-ek[0])+48*48) (4.4)最后将公式(4.3)除以公式(4.4)，得到最后曲线的弯曲度为curve_degree。计算公式为curve_degree=ek_comparatively/ distance_beeline (4.5)就得出了曲线的弯曲程度，然后根据每一段的弯曲程度，最终得出所扫描到的全部道路的形状。4.2.2 道路形状精确识别算法利用4.1.3中对道路弯曲程度的计算，根据扫描到的道路每段的弯曲程度，可以对道路形状进行较为精确的识别，在进行具体的道路识别之前，先给出几个比较模糊的概念，这是描述弯道的弯曲程度的，弯曲程度非常小，比较小，比较大，非常大。这几个概念在下面对于赛道具体识别时有提及。直道的识别 图4.2 直道通过4.2.1节中对道路弯曲程度的计算，我们可以非常精准的识别出直道，因为如果扫描到的道路为直道，在上面的计算中，在计算相对偏差时，ek_comparatively＝(ek[i+8]+ek[i])/2-ek[i+4]肯定会等于0，因为如果道路为直道，而且采样是根据等间隔采样，第i+4点正好是第i+8点和第i点的中心点，根据直线的特性，不管是横纵坐标，直线的中心点都应该等于起点和终点的一半。就算扫描到的图像存在误差，那么计算出的弯曲程度curve_degree也应该是一个很小的数，也就是说，如果赛道的每一段的弯曲程度curve_degree都非常小，那么该道路就一定是直道，反过来，只要其中有一段的弯曲程度curve_degree比较大，那么就可以判断这段道路不是直道，当然，如果有很多连续的段的弯曲程度curve_degree都非常小，那么也可以将连续很小的那一段判断为直道，赛道中直道的形状如图4.2所示。大弯的识别通过4.2.1节中对道路弯曲程度的计算，不但可以得出道路的弯曲程度，而且还可以得出道路弯曲的方向，因为在计算ek_comparatively＝(ek[i+8]+ek[i])/2-ek[i+4]时，如果道路弯曲的方向不一样，那么得到的ek_comparatively的就有正负号的区分，而curve_degree是在ek_comparatively的基础上除以了一个距离，距离必定为正，因此curve_degree也有正负之分，而curve_degree的正负就正好代表了道路弯曲的方向，如果道路在靠近车的部分为直道，而在远离车的部分向着一个方向弯曲，且弯曲程度curve_degree也比较小，那么就可以将这种道路判断为大弯。急弯的识别 图4.3 急弯与大弯基本相同，仍然是经过4.2.1节中的计算，得出道路中每段的弯曲程度curve_degree，如果靠近车的部分的道路的那么得到的弯曲程度curve_degree都非常小，而在远离车的部分道路得到的弯曲程度curve_degree都非常大，且正负号完全相同，也就是说赛道向同一个方向偏，那么就可以将这种道路判断为急弯，如果在长直道后面出现一个急弯，那么一定要精确识别，如果不能精确识别，及时偏转并减速，那么赛