【感知·单目测距】单目摄像头测距原理与前向碰撞预警（FCWS）实现

【感知·单目测距】单目摄像头测距原理与前向碰撞预警（FCWS）实现

你好！这篇文章我想和你聊聊单目摄像头测距这个挺有意思的技术，以及如何用它来做前向碰撞预警（FCWS）。这是我在做ADAS项目时踩了不少坑才搞明白的，希望能帮到你。

一、单目测距是什么原理？

首先，单目测距，顾名思义，就是只用一个摄像头来估计目标距离。听起来很神奇，其实原理很简单------相似三角形！

1.1 相似三角形原理

就是这么简单！不过真正用起来还有不少细节要注意。实际上，车辆抖动和相机内参都需要保证绝对稳定，才有单目测距的可用性。

二、代码实现

2.1 距离计算核心代码

这是我项目里的测距实现，你可以看看：

class SingleCamDistanceMeasure(object):
    # 定义各种目标的真实尺寸（单位：英寸）
    scale = 3.5
    INCH = 0.39 * scale
    RefSizeDict = {
        "person": (160 * INCH, 50 * INCH),      # 人
        "bicycle": (98 * INCH, 65 * INCH),        # 自行车
        "motorbike": (100 * INCH, 100 * INCH),    # 摩托车
        "car": (150 * INCH, 180 * INCH),           # 小轿车
        "bus": (319 * INCH, 250 * INCH),          # 公交车
        "truck": (346 * INCH, 250 * INCH),        # 卡车
    }

    def __init__(self, object_list=None):
        if object_list is None:
            object_list = ["person", "bicycle", "car", "motorbike", "bus", "truck"]
        self.object_list = object_list
        self.f = 1000  # 焦距，这个需要自己调！
        self.distance_points = []

    def updateDistance(self, boxes):
        """根据检测到的目标框计算距离"""
        self.distance_points = []
        for box in boxes:
            xmin, ymin, xmax, ymax = box.tolist()
            label = box.label
            
            if label in self.object_list:
                point_x = (xmax + xmin) // 2
                point_y = ymax
                
                try:
                    # 核心公式！
                    distance = (self.RefSizeDict[label][0] * self.f) / (ymax - ymin)
                    distance = distance / 12 * 0.3048  # 英尺转米
                    self.distance_points.append([point_x, point_y, distance])
                except:
                    pass

2.2 关键点解释

1. 真实尺寸字典：我给每种目标都定义了真实高度，这个很重要，需要先验测量物体的尺寸！
2. 焦距 f：这个是需要自己调试的参数，我是用1000，你可能要根据自己的摄像头调
3. 单位转换：最后把英寸转成米，方便理解

---

三、判断目标是否在本车道

光算出距离还不够，还得判断这个目标是不是在我们车道里，不然旁边车道的车我们也预警就太吵了。

3.1 代码实现

def calcCollisionPoint(self, poly):
    """
    判断目标是否在车道区域内
    poly: 车道区域的多边形点
    """
    if len(self.distance_points) == 0 or len(poly) == 0:
        return None
    
    # 按距离排序，只关心最近的
    sorted_distance_points = sorted(self.distance_points, key=lambda arr: arr[2])
    
    for x, y, d in sorted_distance_points:
        # 用OpenCV的pointPolygonTest判断点是否在多边形内
        status = cv2.pointPolygonTest(poly, (x, y), False) >= 0
        
        if status:
            return [x, y, d]  # 返回最近的在车道内的目标
    
    return None

这里用了 cv2.pointPolygonTest，这个函数太好用了，一行代码就解决问题！

---

四、前向碰撞预警（FCWS）

现在有了距离，接下来就是判断什么时候该预警了。

4.1 预警等级

我定义了三个等级：

等级	说明	距离范围
NORMAL	正常	> 10米
PROMPT	提醒注意	5-10米
WARNING	危险预警	< 5米

4.2 状态平滑处理

直接用单帧的距离容易抖动厉害，我加了个滑动窗口，用中位数滤波，这样更稳：

class LimitedList(list):
    """有限长度的列表，自动滑窗"""
    def __init__(self, maxlen):
        super().__init__()
        self._maxlen = maxlen
        self._is_full = False

    def append(self, element):
        if len(self) >= self._maxlen:
            self.pop(0)
        super().append(element)
        self._is_full = len(self) >= self._maxlen

    def full(self):
        return self._is_full

4.3 预警判断代码

class TaskConditions(object):
    def __init__(self):
        self.collision_msg = CollisionType.UNKNOWN
        self.vehicle_collision_record = LimitedList(5)  # 存最近5帧

    def UpdateCollisionStatus(self, vehicle_distance, lane_area, distance_thres=5):
        """
        更新碰撞预警状态
        """
        if vehicle_distance is not None:
            x, y, d = vehicle_distance
            self.vehicle_collision_record.append(d)
            
            if self.vehicle_collision_record.full():
                # 用中位数，比平均值稳
                avg_vehicle_collision = np.median(self.vehicle_collision_record)
                
                if avg_vehicle_collision <= distance_thres:
                    self.collision_msg = CollisionType.WARNING
                elif distance_thres < avg_vehicle_collision <= 2 * distance_thres:
                    self.collision_msg = CollisionType.PROMPT
                else:
                    self.collision_msg = CollisionType.NORMAL
        else:
            if lane_area:
                self.collision_msg = CollisionType.NORMAL
            else:
                self.collision_msg = CollisionType.UNKNOWN
            self.vehicle_collision_record.clear()

---

五、实际使用中的经验

做这个的时候我踩了不少坑，给你分享几个我摸索出来的经验：

5.1 焦距怎么调？

这个焦距 f 是最关键的参数，我的方法是：

1. 找一辆车，停在离你知道距离的地方（比如10米）
2. 看代码算出的距离是多少
3. 按比例调整 f，直到算出的距离和真实距离差不多
4. 多试几个距离，取个平均值

5.2 为什么用中位数不用平均值？

因为：

• 中位数对异常值不敏感
• 比如偶尔一帧检测错了，不会影响太大
• 更稳定，不会乱跳

5.3 滑窗大小设多少合适？

我试了5帧，感觉刚好：

• 太少：太灵敏，容易误报
• 太多：反应迟钝，预警不及时
• 5帧：30fps的话就是0.17秒延迟，完全可以接受

5.4 预警阈值怎么设？

这个要看车速：

• 市区开得慢：阈值可以设小一点（30米）
• 高速开得快：阈值要设大一点（100米）
• 可以做成动态的，根据车速调整

---

六、整合到主程序

这是完整的主流程：

# 初始化
objectDetector = EfficientdetDetector(...)      # 车辆检测
distanceDetector = SingleCamDistanceMeasure()     # 测距
laneDetector = UltrafastLaneDetectorV2(...)   # 车道检测
analyzeMsg = TaskConditions()                  # 状态分析

while cap.isOpened():
    ret, frame = cap.read()
    if ret:
        # 1. 检测车辆
        objectDetector.DetectFrame(frame)
        
        # 2. 检测车道线
        laneDetector.DetectFrame(frame)
        
        # 3. 计算距离
        distanceDetector.updateDistance(objectDetector.object_info)
        
        # 4. 找出本车道内最近的车
        vehicle_distance = distanceDetector.calcCollisionPoint(laneDetector.lane_info.area_points)
        
        # 5. 判断预警
        analyzeMsg.UpdateCollisionStatus(vehicle_distance, laneDetector.lane_info.area_status, distance_thres=3)
        
        # 6. 显示结果
        displayPanel.DisplayCollisionPanel(frame, analyzeMsg.collision_msg, ...)

---

七、效果展示

• 绿色图标：正常
• 橙色图标：提醒
• 红色图标：预警

---

八、可以改进的地方

如果你想改进这个项目，可以试试：

1. 动态阈值：根据车速自动调整预警距离
2. 多目标跟踪：用ByteTrack跟踪，测距更稳定
3. 夜间优化：夜间检测效果差点，可以加个夜间判断
4. 更多目标类型：比如摩托车、电动车
5. TensorRT加速：让检测更快

---

九、总结

单目测距虽然原理简单，但要做好还是挺多细节的。希望这篇文章能帮你少走点弯路。如果你有什么好想法或者问题，欢迎一起交流！

*这个项目还有车道偏离预警（LDWS）和车道保持辅助（LKAS），有兴趣可以点个关注，催更有效哦～

祝你做项目顺利！