最小二乘法在飞思卡尔智能车路径搜索中的应用

1、什么是最小二乘法

最小二乘法就是要使得观测点和估计点的距离的平方达到最小，我们可以使用一些已知的离散的点，拟合出一条与这些离散点最为接近的曲线，从而可以分析出这些离散点的走向趋势。

如图所示是一个线性拟合，红色的线代表观测点与估计点的距离，使得这些距离的平方和最小这样就是最小二乘法。

2、最小二乘法的推导过程（学霸请忽略，学渣直接看推导结果）

3、如何将最小二乘法应用到飞思卡尔智能车的路径搜索上

在智能车比赛中对时间的实时性要求比较高，如果对摄像头采集到的一整幅图像进行完整的扫描需要耗费非常大的时间和资源，因此对于图像的识别算法至关重要，如何用尽可能少的时间和资源从图像中提取出有效的路径信息成为一个难点。我路径搜索算法如下：

1、图像底部开始，前面十行采用从中间往两边查找左右边线，找不到的用图像的左右边缘点代替

2、使用左右边线之和除以二作为路径中线

3、超过十行的，先使用搜索出的前十行的左右边线进行最小二乘法拟合出曲线的轨迹方程算出接下来的点可能出现的位置

4、在拟合出来的位置的左右 n 个点的范围内寻找目标点，找不到的使用出来的点代替

5、使用左右中线之和除以二作为路径中线

具体代码如下（无删减，自行理解这看）：

/* image_control.c */#include "image_control.h"
#include "main.h"#define  IMG_BLACK       0
#define IMG_WHITE       255
#define FIND_CENTER     0
#define FIND_LEFT       1
#define FIND_RIGHT      2
#define CENTER_POINT    IMAGE_W/2int16 centerLine[IMAGE_H+1] = {0};           // 最后一个元素用来记录转向点对应的行数
int16 leftLine[IMAGE_H] = {0};
int16 rightLine[IMAGE_H] = {0};static uint8 leftFindFlag;              // 用来标记左黑线是否找到
static uint8 rightFindFlag;             // 用来标记右黑线是否找到static int16 leftCount;
static int16 rightCount;
static int16 findLine;int16 createPoint(int type, int line);int16 *findCenterLine(uint8 (* image)[IMAGE_W])
{int8 temp, tmp;// 前十行从中间往两边查找for(findLine = IMAGE_H-1; findLine > IMAGE_H-11; findLine--){leftFindFlag = 0;rightFindFlag = 0;for(temp = 0; temp < CENTER_POINT; temp++){// 寻找左黑线if(leftFindFlag == 0&& image[findLine][CENTER_POINT-temp-1] == IMG_BLACK&& image[findLine][CENTER_POINT-temp] == IMG_WHITE&& image[findLine][CENTER_POINT-temp+1] == IMG_WHITE&& image[findLine][CENTER_POINT-temp+2] == IMG_WHITE){leftLine[findLine] = CENTER_POINT-temp-1;leftFindFlag = 1;}// 寻找右黑线if(rightFindFlag == 0&& image[findLine][CENTER_POINT+temp] == IMG_BLACK&& image[findLine][CENTER_POINT+temp-1] == IMG_WHITE&& image[findLine][CENTER_POINT+temp-2] == IMG_WHITE&& image[findLine][CENTER_POINT+temp-3] == IMG_WHITE){rightLine[findLine] = CENTER_POINT+temp;rightFindFlag = 1;}if(leftFindFlag == 1 && rightFindFlag == 1)break;}// 对未找到的黑线进行补全if(leftFindFlag == 0)leftLine[findLine] = 0;if(rightFindFlag == 0)rightLine[findLine] = IMAGE_W-1;// 对中线进行赋值centerLine[findLine] = (leftLine[findLine]+rightLine[findLine])/2;}// 十行后根据前面行位置查找黑线for(findLine = IMAGE_H-11; findLine >= 0; findLine--){leftFindFlag = 0;rightFindFlag = 0;// 预测下一行黑线位置leftCount = createPoint(FIND_LEFT, findLine);rightCount = createPoint(FIND_RIGHT, findLine);//leftCount = (2 * leftLine[findLine+1] - leftLine[findLine+2]);//rightCount = (2 * rightLine[findLine+1] - rightLine[findLine+2]);/* 在预测点的左右 FIND_COUNT 个点查找黑线位置 */// 寻找左黑线for(temp = 0; temp < FIND_COUNT*2+1; temp++){if(leftFindFlag != 0)break;else if((leftCount-temp+FIND_COUNT)+3 > IMAGE_W-1)continue;else if((leftCount-temp+FIND_COUNT) < 0)break;else if(image[findLine][leftCount-temp+FIND_COUNT] == IMG_BLACK&& image[findLine][leftCount-temp+FIND_COUNT+1] == IMG_WHITE&& image[findLine][leftCount-temp+FIND_COUNT+2] == IMG_WHITE&& image[findLine][leftCount-temp+FIND_COUNT+3] == IMG_WHITE){leftLine[findLine] = leftCount-temp+FIND_COUNT;leftFindFlag = 1;}}// 寻找右黑线for(temp = 0; temp < FIND_COUNT*2+1; temp++){if(rightFindFlag != 0)break;else if((rightCount+temp-FIND_COUNT)-3 < 0)continue;else if(rightCount+temp-FIND_COUNT > IMAGE_W-1)break;else if(image[findLine][rightCount+temp-FIND_COUNT] == IMG_BLACK&& image[findLine][rightCount+temp-FIND_COUNT-1] == IMG_WHITE&& image[findLine][rightCount+temp-FIND_COUNT-2] == IMG_WHITE&& image[findLine][rightCount+temp-FIND_COUNT-3] == IMG_WHITE){rightLine[findLine] = rightCount+temp-FIND_COUNT;rightFindFlag = 1;}}// 补全未找到的左右黑线if(leftFindFlag == 0)leftLine[findLine] = leftCount;if(rightFindFlag == 0)rightLine[findLine] = rightCount;/* 查找中线 */
#if (FIND_TYPE == TYPE_1)             // 补全未找到的左右黑线// if(leftFindFlag == 0)//   leftLine[findLine] = leftCount;// if(rightFindFlag == 0)//   rightLine[findLine] = rightCount;/* 对中线进行赋值 */centerLine[findLine] = (leftLine[findLine]+rightLine[findLine])/2;if(centerLine[findLine] <= 0){centerLine[findLine] = 0;break;}else if(centerLine[findLine] >= IMAGE_W-1){centerLine[findLine] = IMAGE_W-1;break;}
#else       // 补全未找到的左右黑线// if(leftFindFlag == 0)//   leftLine[findLine] = (leftCount < 0) ? 0 : ((leftCount > IMAGE_W-1) ? IMAGE_W-1 : leftCount);// if(rightFindFlag == 0)//   rightLine[findLine] = (rightCount < 0) ? 0 : ((rightCount > IMAGE_W-1) ? IMAGE_W-1 : rightCount);/* 对中线进行赋值 */// 左右黑线都存在则取左右黑线中值作为黑线值if(leftLine[findLine] > 0 && rightLine[findLine] < IMAGE_W-1)centerLine[findLine] = (leftLine[findLine]+rightLine[findLine])/2;// 左黑线超出范围else if(leftLine[findLine] <= 0 && rightLine[findLine] < IMAGE_W-1){// 根据右黑线的偏移量来确定中线temp = centerLine[findLine+1] + (rightLine[findLine] - rightLine[findLine+1]);// 根据最小二乘法补全中线//temp = createPoint(FIND_CENTER, findLine);if(temp <= 0){// 中线超出范围则跳出循环，记录该行为转向行centerLine[findLine] = 0;break;}elsecenterLine[findLine] = temp;}// 右黑线超出范围else if(leftLine[findLine] > 0 && rightLine[findLine] >= IMAGE_W-1){// 根据左黑线的偏移量来确定中线temp = centerLine[findLine+1] + (leftLine[findLine] - leftLine[findLine+1]);// 根据最小二乘法补全中线//temp = createPoint(FIND_CENTER, findLine);if(temp >= IMAGE_W-1){// 中线超出范围则跳出循环，记录该行为转向行centerLine[findLine] = IMAGE_W-1;break;}elsecenterLine[findLine] = temp;}// 左右黑线都超出范围else{// 根据最小二乘法补全中线temp = createPoint(FIND_CENTER, findLine);// 根据中线偏移量补全中线//temp = centerLine[findLine+1] + (rightLine[findLine] - rightLine[findLine+1]);if(temp <= 0){// 中线超出范围则跳出循环，记录该行为转向行centerLine[findLine] = 0;break;}else if(temp >= IMAGE_W-1){// 中线超出范围则跳出循环，记录该行为转向行centerLine[findLine] = IMAGE_W-1;break;}elsecenterLine[findLine] = temp;}
#endif}if(findLine<0 && centerLine[0]<0)centerLine[0] = 0;else if(findLine<0 && centerLine[0]>IMAGE_W-1)centerLine[0] = IMAGE_W-1;// 最后一个元素用来记录转向行centerLine[IMAGE_H] = (findLine < 0) ? 0 : findLine;/* 检查停车线 */
#ifdef RELEASE  if(centerLine[IMAGE_H] == 0 && leftLine[0] >= 23 && rightLine[0] < IMAGE_W-23&& (centerLine[0]-centerLine[29] <= 3 || centerLine[0]-centerLine[29] >= -3)){for(temp = IMAGE_H-1; temp >= centerLine[IMAGE_H]; temp--){if(carParams.stopCarFlag != STOP_LINE&& image[temp][(leftLine[temp]+centerLine[temp])/2] == IMG_BLACK&& image[temp][(rightLine[temp]+centerLine[temp])/2] == IMG_BLACK&& image[temp][(leftLine[temp]+centerLine[temp])/2-1] == IMG_BLACK&& image[temp][(leftLine[temp]+centerLine[temp])/2+1] == IMG_BLACK&& image[temp][(rightLine[temp]+centerLine[temp])/2-1] == IMG_BLACK&& image[temp][(rightLine[temp]+centerLine[temp])/2+1] == IMG_BLACK//&& image[temp][leftLine[temp]+1] == IMG_WHITE//&& image[temp][rightLine[temp]-1] == IMG_WHITE&& carParams.carRunTime > 3000/ENCODE_TIME)     // 小车起跑超过三秒则为停车线{for(tmp = (leftLine[temp]+centerLine[temp])/2+1; (rightLine[temp]+centerLine[temp])/2-1; tmp++){if(image[temp][tmp] == IMG_WHITE && image[temp][tmp+2] == IMG_WHITE){carParams.stopCarFlag = STOP_LINE;goto STOP_LINE_PASS;}}}}STOP_LINE_PASS:if(carParams.stopCarFlag == STOP_LINE && carParams.passStopLine != PASS_STOP_LINE && temp < 0)carParams.passStopLine = PASS_STOP_LINE;        // 小车已越过停车线}
#endif/*// 在 TFT 上绘制出 3 线for(temp = IMAGE_H-1; temp >= centerLine[IMAGE_H]; temp--){guiDot(3*leftLine[temp], 3*temp, GREEN);guiDot(3*rightLine[temp], 3*temp, RED);guiDot(3*centerLine[temp], 3*temp, BLUE);}*/return (int16 *)centerLine;
}/* 利用最小二乘法生成需要补全的点 */
int16 createPoint(int type, int line)
{int16 *linePointer;int8 tmp = 0;double sumX = 0;double sumY = 0;double averageX = 0;double averageY = 0;double sumUp = 0;double sumDown = 0;double parameterA;double parameterB;if(type == FIND_LEFT)linePointer = &leftLine[line];else if(type == FIND_RIGHT)linePointer = &rightLine[line];elselinePointer = ¢erLine[line];// 取邻近的 POINT_COUNT 个点进行拟合while(++tmp <= POINT_COUNT){sumX += (line+tmp);sumY += linePointer[tmp];}--tmp;averageX = sumX/tmp;averageY = sumY/tmp;do{sumUp += (linePointer[tmp]-averageY) * (line+tmp-averageX);sumDown += (line+tmp-averageX) * (line+tmp-averageX);} while(--tmp > 0);if(sumDown == 0)parameterB = 0;elseparameterB = sumUp/sumDown;parameterA = averageY-parameterB*averageX;return (int16)(parameterA+parameterB*line+0.5);
}

经实践，采用该算法提取出来的路径信息非常稳定，无论在直道、弯道、十字，还是十字入弯和十字出弯等都可以快速而有效的搜索出路径信息，基本不会出现误判和不受噪点的干扰（自己当时忘记录像和拍照了，只能引用其他人的视频，不过效果差不多），具体效果与如下视频接近： http://v.youku.com/v_show/id_XNDEzMzc0NzA0.html

完整的工程：https://github.com/528787067/SmartCar