基于雙目定位的自動駕駛小車
github連結
視訊示範
B站位址
小車自動駕駛
小車自動駕駛錄屏
小車路線識别
基本構思：
1.使用樹莓派作為小車的操作中心，樹莓派通過攝像頭進行道路的檢測，進而使用PWM模式對L298N電機驅動子產品進行對小車輪子的前進、後退、左右轉彎。
2.主機使用MATLAB對雙目攝像頭進行标定，使用SGBM/BM算法進行對小車的空間的三維坐标的建立。
3.在主機pygame界面上建構出真實地圖的平面圖，通過雙目攝像頭獲得到小車真實的三維坐标，主機在pygame上的平面圖出小車的位置，并把真實的三維坐标和地圖資訊通過tcp發給樹莓派小車。
樹莓派小車通過道路檢測和地圖資訊并使用Dijkstra進行最短路徑的路線規劃并進行自動駕駛，并實時通過接收真實的三維坐标進行自我矯正和分析。
難點：不管是樹莓派還是主機下的圖像處理能力都很慢，導緻定位不夠即使，小車電池電壓下降，左右輪的PWM調速不能為固定值導緻轉彎困難
把raspberryPi檔案夾移動樹莓派，分别運作mian.py
未來構想圖
總體分為兩部分
硬體:樹莓派3一個，L298N電機一個，樹莓派官方攝像頭一個，HC-SR04超音波子產品一個.使用一個12V的電池兩頭供電,一個12V轉到5V供到樹莓派，一個12V直接供給電機
參考:
Sunny的樹莓派小車DIY教程（附視訊）
樹莓派上使用HC-SR04超音波測距子產品
樹莓派3的GPIO圖
要注意Python驅動GPIO的子產品為RPi.GPIO,下面這個BOARD模式指的是Pin#，不是NAME（如下圖所示）
import RPi.GPIO as GPIO
import time
from config import Road, Car
# 設定 GPIO 模式為 BOARD
GPIO.setmode(GPIO.BOARD)
成品圖:
雙目攝像頭如下
軟體部分
使用雙目攝像頭進行定位小車
先使用matlab對雙目攝像頭進行标定
參考
python、opencv 雙目視覺測距代碼
使用OpenCV/python進行雙目測距
matlab雙目标定（詳細過程）
雙目測距理論及其python實作！
錄影機标定和立體标定
其中：
Matlab導出參數如下：
左右相機畸變系數:[k1, k2, p1, p2, k3]
k1, k2, k3 = CameraParameters1.RadialDistortion[0, 1, 2],
p1, p2 = CameraParameters1.TangentialDistortion[0, 1]
T = 平移矩陣 stereoParams.TranslationOfCamera2
R = 旋轉矩陣 stereoParams.RotationOfCamera2
标定前
标定後
深度圖
記錄:
mode=cv2.STEREO_SGBM_MODE_HH 不同模式之間沒有太特别差異
總的來說，調節 num 、blockSize和UniquenessRatio是比較明顯的
定位
MARK（紅色）如下圖所示：
def get_coordinate(mark_pos_ofcamera: tuple, power_pos_ofcamera: tuple,
camera_pos_ofmap: tuple, threeD, mark_pos_ofmap) -> tuple:
'''
一般位置的擺放如下，機關都是cm
---------------------------------MARK
- -
- CAR -
- / | -
- / | -
- / | -
- / | -
- / | -
---------------CAMERA-----------------
:param mark_pos_ofmap: 标記物在真實地圖上的位置
:param camera_pos_ofmap: 相機在真實地圖上的位置
:param power_pos_ofcamera: 電池在相機上的位置
:param mark_pos_ofcamera: 标記物在相機上的位置
:param threeD: 使用MATLAB矯正和opencv得到的深度矩陣
'''
# 計算深度
car_deepth = threeD[power_pos_ofcamera[1]][power_pos_ofcamera[0]][-1]
mark_deepth = threeD[mark_pos_ofcamera[1]][mark_pos_ofcamera[0]][-1]
if car_center != 'inf' and mark_deepth != 'inf':
if car_deepth > 0 and mark_deepth > 0:
# car_deepth /= 10.0
mark_deepth /= 10.0
# if abs(mark_deepth - math.sqrt(mark_pos_ofmap[0]**2 + mark_pos_ofmap[1] ** 2)) > 50:
# print("标記與相機測量誤差太大")
# else:
# 計算小車與标記的橫向距離,兩點之間的距離在除以5，這個5是通過實際測量和圖像點計算出來的
x_length = math.sqrt((power_pos_ofcamera[0] - mark_pos_ofcamera[0]) ** 2
+ ((power_pos_ofcamera[1] - mark_pos_ofcamera[1]) ** 2)) / 5.0
# 在通過标記的坐标得到小車的橫向坐标
x = mark_pos_ofmap[0] - x_length
# print("小車與标記的距離為{}, 橫向坐标為{}".format(x_length, x))
# 再用勾股定理得到y
y = math.sqrt(car_deepth ** 2 - (x - camera_pos_ofmap[0]) ** 2)
print("坐标為{},{}".format(x, y/10.0))
return x, y / 10
定位如下
地圖模拟
真實地圖如下
小車的定位有有兩種模式，一個是平拍模式，另外一種是俯拍模式，這是為了探讨道路上主要的兩種攝像頭。
3.8.1 平拍定位
由HSV顔色追蹤可實時擷取到小車藍色輪框的質心位置，也就是在像素坐标的位置，通過SGBM算法和轉換可以獲得到深度距離，想要獲得小車相對于相機的地圖坐标，還需要相對的參考物來進行計算和對照，用一個mark标記固定在一個确定的位置上，通過已知的世界級坐标和像素坐标的mark标記與小車之間比例，可以計算出小車的地圖坐标，真實行車地圖如圖3.23真實地圖所示紅色箭頭指向的是camera，藍色箭頭指向的是mark，白色的是地圖，對應的俯視圖數學平面圖如圖3.24所示：
3.8.2 俯拍定位
俯視角與平時角的不同在于，俯視角可以獲得到比平時角度獲得更多的資訊，在相機中俯拍可以直接通過透視變換、線性變換與計算可以定位到地圖中小車，這個過程可以用單目相機來完成且不需要标記物輔助定位。如圖3.26所示，左邊為俯拍視角，右邊為經過透視變換後的視角。
opencv畫車道參考：
[1]
[2]