计算机控制系统-车道保持-坐标转换

---

一、坐标系定义对齐（结合PDF与小车实际）

1.1 PDF讲义中的坐标系定义

• 相机坐标系 （Camera Frame）：原点在相机光心，Yc轴垂直于地面向下，Zc轴指向车辆前方，Xc轴指向车辆右侧（右手系）。

• 像素坐标系（Pixel Frame）：u为列，v为行。

• 车辆坐标系 （Vehicle Frame）：原点在前轮轴心地面投影，X轴横向 （右为正），Z轴纵向（前为正）。

1.2 教学小车实际安装参数（我实验时小车安装情况）

• 相机安装点：前轮轴心正上方，高度 H = 300 mm = 0.3 m。

• 纵向偏置：相机光心向前偏移 D = 70 mm = 0.07 m（在车辆坐标系中，Z方向 +0.07 m）。

• 相机姿态：垂直向下 ，即相机光轴与地面垂直。此时相机坐标系的 Yc 轴垂直向下 ，与车辆坐标系的 Y 轴（垂直向上） 相反。PPT中正是如此定义的。

1.3 坐标系示意图

侧视图（沿车辆X轴方向看）：

               相机光心 ●
                  |
                  |  Yc (垂直向下)
                  ↓
  地面===========================  (Z=0 平面)
        ↑                      
     前轮轴心投影点 O_v (车辆坐标系原点)
        |← D = 0.07m →|
        
俯视图（从上往下看）：

                Z_v (车辆前进方向)
                 ↑
                 |
                 |   [相机视野范围]
                 |   +--------+
                 |   |        |
                 |   |    ●   | ← 车道线上一点 P
                 |   |        |
                 |   +--------+
                 |
                 *------------→ X_v (车辆右侧)
             原点 O_v

---

二、垂直俯视情况下的坐标转换推导

2.1 已知量

• 内参矩阵（从Matlab标定获得）：

• 

注意：PDF中Matlab输出的 IntrinsicMatrix 是转置形式，使用前需转置。

• 安装参数：

  • 相机高度（地面到光心）：H = 0.3 m

  • 前向偏置（光心在车辆坐标系Z向坐标）：D = 0.07 m

2.2 相机坐标系与车辆坐标系的关系（垂直向下）

因为相机垂直向下，旋转矩阵 R=IR=I（单位阵），但坐标轴方向需注意：

• 相机坐标系：XcXc​ 向右，YcYc​ 向下，ZcZc​ 向前（与车辆坐标系 ZvZv​ 同向）。

• 车辆坐标系：XvXv​ 向右，YvYv​ 向上，ZvZv​ 向前。

因此，地面一点在相机坐标系 下的坐标 (Xc,Yc,Zc)(Xc​,Yc​,Zc​) 与其在车辆坐标系下的坐标 (Xv,Yv=0,Zv) 的关系为：

​Xc​=Xv​

Yc​=H(地面在相机下方 H 处)

Zc​=Zv​−D​

注意：这里 Yc=HYc​=H 是因为地面点恰好在光心正下方距离H处。 所有地面点的 Yc​ 均恒等于相机高度 HH。

2.3 投影方程

透视投影公式：

将 Yc=HYc​=H 代入得：

2.4 反解车辆坐标（横向偏差与纵向距离）

由第二式解出纵向距离 Zv ：

再由第一式解出横向坐标 XvXv​：

因此：

也可以直接用已知的 代入，得到：

若 fx≈fyfx​≈fy​（通常如此），则简化为：

教学提示：这个简化公式极具物理意义------横向偏差与像素横向偏移成正比，与像素纵向坐标成反比（越往下看，距离越近，相同横向像素对应的实际横向距离越小）。但对于垂直安装，其实因为 YcYc​ 恒为H，纵向距离 ZvZv​ 完全由 vv 决定，公式简单直接。

2.5 最终实用公式（垂直俯视专用）

在 fx=fy=ffx​=fy​=f 的近似下（多数廉价USB相机成立），可进一步简化为：

---

三、Matlab实现（融合PDF标定结果与安装参数）

3.1 加载相机参数并提取内参

matlab

% 加载标定结果（假设已通过Camera Calibrator导出cameraParams.mat）
load('cameraParams.mat');

% 提取内参矩阵（注意Matlab存储为转置形式，需转置）
K = cameraParams.IntrinsicMatrix';   % 3x3矩阵
fx = K(1,1);
fy = K(2,2);
cx = K(1,3);
cy = K(2,3);

% 安装参数（单位：米）
H = 0.3;    % 相机高度 300mm
D = 0.07;   % 前向偏置 70mm

3.2 定义坐标转换函数（包含畸变校正）

，应先使用 undistortPoints 校正像素坐标。

matlab

function [Xv, Zv] = pixel2vehicle_vertical(u, v, cameraParams, H, D)
    % 垂直俯视情况下的像素坐标转车辆坐标
    % 输入：u, v 可以是标量或向量
    % 输出：Xv, Zv 单位：米
    
    % 第一步：畸变校正（使用Matlab内置函数）
    points = [u(:), v(:)];
    undistortedPoints = undistortPoints(points, cameraParams);
    u_undist = undistortedPoints(:,1);
    v_undist = undistortedPoints(:,2);
    
    % 第二步：提取内参
    K = cameraParams.IntrinsicMatrix';
    fx = K(1,1);
    fy = K(2,2);
    cx = K(1,3);
    cy = K(2,3);
    
    % 第三步：计算纵向距离 Zv
    Zv = (fy * H) ./ (v_undist - cy) + D;
    
    % 第四步：计算横向坐标 Xv
    Xv = (u_undist - cx) / fx .* (Zv - D);
    
    % 恢复原始形状
    Xv = reshape(Xv, size(u));
    Zv = reshape(Zv, size(u));
end

3.3 应用示例：计算车道中心横向偏差

matlab

% 假设已检测到左右车道线在图像底部的交点像素坐标
u_left = 180;   % 左侧车道线底部u坐标
u_right = 500;  % 右侧车道线底部u坐标
v_bottom = 440; % 图像底部v坐标（靠近车头）

% 转换为车辆坐标
[X_left, Z_left] = pixel2vehicle_vertical(u_left, v_bottom, cameraParams, H, D);
[X_right, Z_right] = pixel2vehicle_vertical(u_right, v_bottom, cameraParams, H, D);

% 车道中心横向位置
lane_center_X = (X_left + X_right) / 2;

% 横向偏差 e（车辆中心位于 X=0）
e = lane_center_X;

fprintf('左车道线位置：X=%.3f m, Z=%.3f m\n', X_left, Z_left);
fprintf('右车道线位置：X=%.3f m, Z=%.3f m\n', X_right, Z_right);
fprintf('车道中心横向偏差 e = %.3f m\n', e);

---

四、与教学中PDF中通用方法的对比说明

项目	PDF通用方法（θ ≠ 0）	教学小车专用方法（θ = 0）
外参获取	需标定或测量俯仰/偏航/滚转角	无需外参，旋转矩阵为单位阵
Yc 已知条件	Yc = H（地面在相机下方）	同左，且所有地面点 Yc 恒为 H
求解复杂度	需联立两个方程解 Zc 和 Xc	直接由 v 坐标线性解出 Zv
畸变校正	使用 undistortPoints	同左
控制器输入	由4个点计算 D 和 θ（见PPT第12页）	直接得到横向偏差 e，配合简单Stanley控制

向学生强调：

PDF中给出的控制器公式 steer = -k1*θ - k2*D 是针对一般斜俯视情况的。在我们的垂直俯视实验中，航向角 θ 可以通过左右车道线的斜率差计算，但由于视野小、速度低，也可以只用横向偏差 e 进行比例控制 ，即 steer = -k2 * e，配合 Stanley 的 atan(k*e/v) 项即可。

---

五、完整的实验流程总结

1. 相机标定（PDF Camera Calibration部分）：

   • 打印棋盘格，拍摄10-20张不同角度的照片。

   • 使用Matlab Camera Calibrator工具箱获得 cameraParams。

2. 图像采集：

   • 垂直安装相机于小车前轮轴心上方。

   • 实时采集视频帧。

3. 车道线检测（PDF Feature Extraction部分）：

   • 灰度化 → Canny边缘检测 → ROI掩膜 → Hough直线检测 → 左右线分离与拟合。

4. 坐标转换（本文档方法）：

   • 选取检测到的车道线在图像底部的两个点（近点）。

   • 调用 pixel2vehicle_vertical 函数转换为车辆坐标系的 X, Z。

5. 控制量计算：

   • 横向偏差 e = (X_left + X_right)/2。

   • 可选航向误差 θ = atan2(Z_right - Z_left, X_right - X_left) - pi/2

   • 代入Stanley控制器输出转向角。

6. 执行控制：

   • 通过串口发送PWM信号给Arduino舵机（实验小车是arduino的板子）。

---

六、常见问题与调试技巧

Q1：为什么计算出的Zv与实际距离对不上？

A1：检查三个数值：

• 焦距 f 是否正确（需标定，不能随意填写）。

• 相机高度 H 是否准确测量（用尺子量光心到地面的垂直距离）。

• 图像主点 cy 是否准确（标定给出）。可用已知距离（如1米处放物体）反推验证。

Q2：图像边缘处的偏差计算误差较大，为什么？

A2：尽管垂直安装避免了透视变形，但镜头畸变 在边缘处依然显著。务必使用 undistortPoints 进行校正（如代码中所示）。若未校正，桶形畸变会使边缘的物体看起来比实际更靠近中心。

Q3：能否不经过坐标转换，直接在图像上控制？

A3：可以，但不鲁棒。因为小车若发生俯仰振动，图像中车道线位置会大幅变化。经过坐标转换后，横向偏差以米为单位，对振动不敏感（高度H变化的影响是线性的）。