# car2018

**Repository Path**: messier201/car2018

## Basic Information

- **Project Name**: car2018
- **Description**: No description available
- **Primary Language**: C++
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 1
- **Forks**: 0
- **Created**: 2018-06-25
- **Last Updated**: 2024-05-24

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README


项目描述：在大三下半学期参加的智能车比赛。
比赛任务如下：
![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-054154.png)
![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-054020.png)
![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-054315.png)
![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-054349.png)

## 分析
　　比赛的任务是要制作一辆小车，能在8*8的棋盘格内自由移动，并且搬运棋子，使八颗棋子满足八皇后问题。比赛的任务还是很复杂的，可以分解成车身的定位，棋子的识别，路径的规划，以及车体控制部分。
　　车体控制方面，包括控速，转向，棋子的抓取与放置。后面实际做的过程中，采用了麦克纳姆轮小车，用机械臂加电磁铁的方式来实现棋子的抓取与放置。
　　本文主要论述除车体控制外的部分。
  
## 总体框架
　　算法运行平台：控制算法在官方要求的k60单片机上运行，而定位的算法则放在树莓派上运行。两者之间用串口进行通信，传递位置信息。
　　树莓派上搭载一个单目相机相机，用openCV进行图像的读取与处理，用wiringPi进行串口通信，用pthreads线程库实现图像进程和串口进程的并行。操作系统为ubuntu mate，语言为C++，使用cmake构建工程。平时在我的Mac上编写代码，并用离线的视频与图片进行调试，定期用git同步到树莓派上进行实际的测试。
  
## 算法简述
### 定位算法
总体思路如下：
　首先明确输入输出。输入是一张图像，输出即是车体在**世界坐标系**下的位置与姿态。
　算法的核心是slam的PnP算法。通过输入相机的参数矩阵，平面内4个点的世界参考系与图像坐标，就能得到世界坐标变换到相机坐标的旋转矩阵R，平移向量t。
　场地是铺设在地面上，刚好满足PnP的平面的约束。而棋盘格的每个方格中都有一个数字，指定世界参考系后，通过数值可以计算出方格的4个角点的世界坐标。而如果准确识别出了四个角点，则图像坐标也可以得到，PnP的输入就满足了。
　所以，可以确定算法流程大体如下:找出方框角点，识别数字，计算世界坐标，填入世界坐标与图像坐标，调用pnp算法，用R和t得出车体姿态和位置。

### 路径规划算法
  整个比赛流程可以简化为：前往棋子方格->抓取->前往目标方格->放置。
　八皇后一共有92种放置方法，可以简单的用回溯递归解出并保存。
　当前棋子摆放情况已知的情况下，可以判断当前情况到每一种解法，小车所要行驶的最短距离和搬运次数，得出最优解。
　
## 算法详述　　
### 方框角点提取与数字的识别
　对于这个问题，我想起了以前看到过的openCV识别数独的教程。在开始时，学校的场地也没有铺设好，所以我先用数独的图片进行算法的测试素材。
　![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-064957.jpg)
　看了这篇[文章](https://blog.csdn.net/xingchenbingbuyu/article/details/70169665)，我决定也采用判断轮廓层级的方式来提取方格。
　提出方格后，调用caffe的Mnist模型识别数字。整个流程比较顺利的完成了。
　场地铺好后，拍摄了图片。
　![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-IMG_03.jpg)
可以发现算法有很多地方要补足:
1.层级判断的方式不能用了，因为视角有限，不可能像数独一样看到整个棋盘格。
2.反光，明暗不均，二值化处理的效果如下。
　![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-071350.png)
　以上因素均会影响到数字的识别与方框的提取。
3.由于畸变和透视，方框不再是严格的正方形，也可能由于视角的不同，方框是旋转过一定角度的。
4.一个格子内会有1-2个数字，不能简单的将方框内的图片提出然后用Mnist判断数值。

　对于问题1，2，我在用findContours得到轮廓后。首先，用层序遍历把每个结点的孩子结点和深度保存（在这个过程中滤掉一些小轮廓-噪点）。然后进行判断，一个轮廓是否深度小于等于2并且有1或2个孩子结点，若是的话，进一步使用多边形逼近函数对轮廓进行逼近，看能否得到一个四边形，若能得到的话，则将其判断成一个方框，并存下角点的图像坐标以便后续处理。效果如下：
　　![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-073550.png)
 对于问题3，4，小车实际行驶时的画面是这样的:
 ![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-080150.png)
　　方框旋转一定角度，是不影响方框的提取的。
　　但是，存在两个问题：１.如何裁剪出数字２.多边形逼近得到的角点的保存顺序是未知的，也就是说，无法直接得到４个点对应的是哪个结点。
　裁剪数字方面，我先尝试用了下平面Ｈ变换，把蓝色通道图像中的四边形成一个正方形，然后局部二值化处理，效果如下：
　　![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-080540.png)
　但是，并不是每次都能得到这样的结果，Ｈ变换的结果可能是转过180°或90°的，因为角点顺序未知，如下:
　![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-082037.png)
　对于这个问题，我的思考如下:可以确定的是，4个点是连续存放的。
　以这张图的数字36举例:
　![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-082349.png)
　进行编号(对应数组下标)，绿色点是0，红色1，黑色2，蓝色3。而进行H变换时，同样有4个映射点。比如映射成一个56*56的正方形时:
　

```
static int length = 56; 
Point rightPts[4] = {Point(0, 0), Point(length - 1, 0), Point(length - 1, length - 1), Point(0, length - 1)};

```
　　要得到正确的映射图像，也就是要找到正确的点的对应关系。一共有4种可能性。
　　`static int order[4][4] = {{0, 1, 2, 3}, {1, 2, 3, 0}, {2, 3, 0, 1}, {3, 0 , 1, 2}};`
　　假定映射点的顺序是：左上->右上->右下->左下。那么在进行H变换时，先对输入的原图点的坐标顺序重新排列，由0，1，2，3变为3，0，1，2，就可以得到正确的映射图像。
　　所以，每次我都进行4次H变换，然后对每一张映射图像单独判断正确性。
　　判断正确性是在识别数字的同时进行的。得到正方形图片后，提取轮廓的最小外接矩形(可以注意到，每个数字的外接矩形应该是宽度<高度的)，然后生成一副新的单独只有一个数字的图片，如下:
　　![](https://blog-1255455112.cossh.myqcloud.com/2018-09-29-084426.png)
　此时，就可以调用Mnist判断了。判断后将十位*10与个位相加，进而得出结果。
　
### PnP算法
在之前的过程中已经能得到数字周围四个角点的世界坐标与像素坐标，接下来就是调用solvePnP函数了。
> 
bool cv::solvePnP	(	InputArray 	objectPoints,
InputArray 	imagePoints,
InputArray 	cameraMatrix,
InputArray 	distCoeffs,
OutputArray 	rvec,
OutputArray 	tvec,
bool 	useExtrinsicGuess = false,
int 	flags = SOLVEPNP_ITERATIVE 
)		

假定世界坐标原点为数字1的左上角点，x轴为1-9方向，y轴为1-8方向，z轴竖直向上。
要得到车身的位置和姿态，要对得到的旋转向量和平移向量作处理。

先将旋转向量转换为旋转矩阵。

```
Mat rot;
Rodrigues(rVec, rot);
```
$$
 \left[
 \begin{matrix}
   X \\
   Y \\
   Z
  \end{matrix}
  \right] =R\left[
 \begin{matrix}
   U \\
   V \\
   W
  \end{matrix}
  \right] +t
$$　
　在这里，$
 \left[
 \begin{matrix}
   X \\
   Y \\
   Z
  \end{matrix}
  \right]$是相机坐标，$\left[
 \begin{matrix}
   U \\
   V \\
   W
  \end{matrix}
  \right]$是世界坐标。
  先求位置，求出的t向量是世界坐标原点在相机坐标下的方位，我们要求的是相机在世界坐标中的位置，很简单,如下计算即可：
  $t'=R^Tt$
  再求姿态，我们只需要小车在xOy平面上的角度即可。这里要用到一些线性代数的知识，上面的旋转矩阵R是世界参考系到相机参考系的变换矩阵，根据**基变换**的原理，R的第三列即为变换后的世界坐标的z轴，也即是相机的光轴在世界参考系下的单位方向向量。
那么，想求得角度，只要将该向量投影在xOy平面上，求与x轴夹角即可。
代码如下：


```    Mat rVec, tVec;
    solvePnP(obj, dig.imgPos, cameraMatrix, distCoeffs, rVec, tVec, false, SOLVEPNP_P3P);

    Mat rot;
    Rodrigues(rVec, rot);
    Mat tVec2 = rot.t()*(-1*tVec);
   // column vector !
    double Zx, Zy;

 
    Zx = rot.at<double>(2, 0);
    Zy = rot.at<double>(2, 1);
   degree = atan2(Zy, Zx)/3.14159*180;
  carPos_x = int(tVec2.at<double>(0, 0));
   carPos_y = int(tVec2.at<double>(1, 0));  
    
```
  
### 结果显示
为了方便调试，写了个简单的画图函数将当前位置和姿态显示出来。
 ![](https://blog-1255455112.cossh.myqcloud.com/2018-09-30-073358.png)
 
### 路径规划算法
问题如下：当前已知8个棋子的位置，给定一种解法，求移动到这种解法所需的最短距离。 
可以先找出所有需要移动的棋子，举例说有4个，那么要移动4*2次，到达8个点，4个拿起的点，4个放下的点。那么这可以理解成一个旅行商问题，求遍历所有点的最短路径和距离。因为每次总是先拿起，再放下，再拿起，如此循环。在建图时，4个拿起点之间，4个放下点之间都是不连通的，而任意一个拿起与任意一个放下点之间都是连通的。
如此，就可以成功地建图然后解出最短路径了。


## 总结&感谢
　　感谢我的指导老师陈东晓老师和杨力老师，感谢队友们。从大一刚接触单片机学习编程，到大三学习计算机视觉做了这个比赛，这一路经历了很多，在实验室的这三年也学到了很多东西，收获了珍贵的友情。虽然这次比赛的结果不尽如人意，我也因此备受打击。anyway，心态也还是调整过来了。在大一时，曾看到过一个四旋翼的视频，里面演示的精妙的控制算法令我心驰神往，想着自己有一天也能做出这样厉害的东西就好了。
　　如今，也不再是当时那个只能仰望着的我了，有幸学习接触到了很多新的知识，也认识到了这个世界的广阔。
　　不会止步于此的，因为还想探寻更广阔的世界！