无人驾驶：制作鸟瞰图，路径规划，路径跟踪，调试摄像头，测速度，测角度

本文是【马悦宁老师】指导完成的山东大学引航计划公益人工智能科研实训项目。
注：本文所用代码皆已上传至Github上。

无人驾驶

路径规划

路径规划之前

1、制作并生成鸟瞰图（Python）：

• 内参标定（1_intrinsic_calibrate.py），获得相机参数：
  
  注：找到14张有效的相机标定图片；DIM：大小；rms：均方根；K：内参；D：去畸变。
  73，74 行注释取消后，其中一张图片结果：
  

• 图像去畸变（2_distort_correct.py）：
  

• 透视变换（3_perspective.py），38~68行取四角点获取透视变换参数：
  

2、相机的成像原理与标定：

• 小孔成像数学模型，可参考如下网址：
  https://blog.csdn.net/weixin_41480034/article/details/121684847
  x_correct与x的差离中央位置越远，畸变程度越大。
• Matlab实验演示，可参考如下网址：
  https://ww2.mathworks.cn/help/vision/ug/camera-calibration.html ，
  https://ww2.mathworks.cn/help/vision/ug/using-the-single-camera-calibrator-app.html

路径规划

1、A*算法（广度优先算法），可参考如下网址：
https://www.redblobgames.com/pathfinding/a-star/introduction.html
2、运行Astar_demo.py：

3、运行Astar.py：

注：

open_list：丢掉旧的点，加入新的点。

路径跟踪

1、Pure Pursuit算法（阿克曼转向几何：推半径与转角的关系），可参考如下网址：
https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/BicycleModel.html ，
https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html
2、运行pure_pursuit_without_imu.py）：

调试摄像头

1、将IP_Camera.ino烧录进摄像头，两根线连上开发板；
2、将server_without_html.ino烧录进开发板，连上电源后，网页连开发板热点；
3、运行test_http_request.py进行调试，角度和速度都可更改，驱动小车；

• 按开发板RESET键重启；
• 等灯亮两次后即为矫正完成，开始运行调试代码；
• 将小车转90度，等灯亮两次后再次运行代码；
• 观察输出，相差近90即为调试成功。

4、运行test_ip_camera.py，读取照片，将照片转码成熟悉的格式：

5、运行test_ip_camera_thread.py，多线程：主程序跑循环，线程管着摄像头函数找画面。

测速度

1、接测速传感器（宽体对射式传感器）和主板之间的引脚：
红：OUT（信号线）；橙： GND；黄：VCC。
2、运行test_http_request.py，读取速度：

测角度

1、安装MPU6050惯性测量模块：测角度。
注：绿：VCC，3.3V；蓝：GND；紫：SCL，17；灰：SDA，16；白：INT，G05。
2、注释切换test_http_request.py测角度：

3、重新烧录要先把信号线（红）拔下来。

多环节联调（6_multi_steps）

1、运行主程序1_pure_pursuit_with_imu.py进行调试：

• sin函数
• 直线
• L路

2、运行3_mask_to_path.py（局部追踪）：
（1）定义A*算法：

• 初始设定：
  • 设定地图的相关信息（颜色、宽高）；
  • 设定好开始点和目标点。
• 开关列表初始化，添加start_node到open_list；
• 寻找children子节点，若新的方位在8个方位中，即周围点，进行节点路径计算，将周围点加入open_list中；
• 从周围点中选择最小的代价点作为下一步current_node，从open_list中移除，并加入close_list中；
• 接着执行step3，直至开启列表中出现目标方块，路径寻找结束。

（2）根据图像分割结果，规划行驶路线：

• 图像去畸变：

  • cv2.fisheye.intUndistorRectifyMap；
  • cv2.remap。

  

• 转化鸟瞰图：

  • np.loadtxt 读取透视变换矩阵；
  • cv2.warpPerspective 执行透视变换。

  

• 可视化行驶区域图：

  • cv2.getStructuringElement将图片返回成指定形状（椭圆）和尺寸结构；
  • morphology.remove_small_objects去除grass和background的面积小于60/4000的连通区域；
  • cv2.erode和cv2.dilate函数对图片进行腐蚀和膨胀。

  

• 补全车前盲区：
  相机视野焦点所在位置为车头中点，小车行驶原点为后车轮中点位置，因此需要加上车身长度进行补全。

• 选取车辆行驶终点：

  • cv2.circle选定车辆局部区域候选半径（如80cm）；
  • 可行驶区域与候选半径范围进行交集，利用random函数随机生成最后目标点。

  

• 车辆路径规划：
  利用astar算法进行小车与目标点之间的路径规划。
  

（3）路径平滑：
全局路径规划算法计算完路径后，其结果是一串用来表示经过的路径点的坐标，但该路径通常有“锯齿”形状，不符合实际需求，因此进行平滑处理。

3、运行4_photo_to_path.py（拍照追踪）：
（1）将图像分割训练及预测的代码封装为库seg进行调用；
（2）运行主程序：

• 主程序中构建Unet网络；
• 用unet_predict函数预测最终的路径图；
• 剩余步骤同3_mask_to_path.py。

4、运行5_photo_to_path_final.py：
（1）将图像分割的训练及预测模型封装至seg库进行调用；
（2）将拍照追踪（4_photo_to_path.py）代码封装至path-plan.utils中进行调用；
（3）运行主程序（同3）：

• 道路分割；
• 相机标定；
• 路径规划find_path；
• 路径平滑smooth。

5、运行6_photo_to_pursuit.py：
（1）将图像分割的训练及预测模型封装至seg库进行调用；
（2）将拍照追踪（4_photo_to_path.py）的代码封装至path-plan.utils中进行调用；
（3）将路径追踪的pursuit算法代码封装至pure-pursuit.utils库中进行调用；
（4）运行主程序：

• 同5_photo_to_path_final.py；
• 用pure-pursuit进行路径跟踪。

6、调试摄像头：
（1）开发板烧入server_without_html代码后，连上电源，电脑连接开发板热点；将IP_Camera代码烧入摄像头中，连接开发板热点。
注：开发板重启后摄像头也要重启重连WiFi。
（2）运行主程序：

• 7_get_camera.py展现即时的摄像画面；
• 8_test_ip_camera_thread.py多线程：
  • 读取每一帧存到img全局变量里；
  • 主程序进行其他环节。
• 9_capture_to_pursuit.py将最新即时的照片进行路径追踪；
• 10_multi_capture_to_pursuit.py多次路径追踪。

综合实验

运行7_final中的final.py，常见错误见下图：

实现仿真地图上的基于局部路径规划的无人驾驶。