基于双目摄像头定位的自动驾驶小车

最新推荐文章于 2024-02-02 16:05:29 发布

cmzz

最新推荐文章于 2024-02-02 16:05:29 发布

阅读量3.9k

点赞数 7

分类专栏： python 文章标签：自动驾驶

本文链接：https://blog.csdn.net/qq_40965177/article/details/108585803

版权

python 专栏收录该内容

44 篇文章 2 订阅

订阅专栏

基于双目定位的自动驾驶小车

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所示：
在这里插入图片描述