爱折腾-创客智造实验室 - 无人驾驶汽车系统入门
通过无人驾驶汽车系统入门学习，了解无人驾驶相关细节，算法，实现路径(来自CSDN的AdamShan)
无人驾驶汽车系统入门（一）——卡尔曼滤波与目标追踪
无人驾驶汽车系统入门（一）——卡尔曼滤波与目标追踪 说明： 介绍无人驾驶汽车系统感知模块的重要技术——卡尔曼滤波 卡尔曼滤波按如下三个章节说明： 卡尔曼滤波与行人状态估计 扩展卡尔曼滤波（EKF）与传感器融合 处理模型，无损卡尔曼滤波（UKF）与车辆状态轨迹 本节为卡尔曼滤波，主要讲解卡尔曼滤波的具体推导，卡尔曼滤波在行人状态估计中的一个小例子。 为什么要学卡尔曼滤波？ 卡尔曼滤波以及其扩展算法能够应用于目标状态估计，如果这个目标是行人，那么就是行人状态估计（或者说行人追踪） 如果这个目标是自身，那么就是车辆自身的追踪（结合一些地图的先验，GPS等数据的话就是自身的定位）。 在很多的无人驾驶汽车项目中，都能找到卡尔曼滤波的扩展算法的身影（比如说EKF，UKF等等）。 本节我们从最简单的卡尔曼滤波出发，完整的理解一遍卡尔曼滤波的推导过程，并实现一个简单的状态估计Python程序。 卡尔曼滤波是什么？ 我们通常要对一些事物的状态去做估计，为什么要做估计呢？因为我们通常无法精确的知道物体当前的状态。 为了估计一个事物的状态，我们往往会去测量它，但是我们不能完全相信我们的测量，因为我们的测量是不精准的，它往往会存在一定的噪声，这个时候我们就要去估计我们的状态。 卡尔曼滤波就是一种结合预测（先验分布）和测量更新（似然）的状态估计算法。 概率论的知识基础 下面是一些概率论的基础知识，如果之前有这方面的知识储备那当然是最好的，很有利于我们理解整个博客内容 如果没有这方面的基础而且也看不懂下面的内容也没关系，我会以一个相对直观的方式来展现整个理论部分。 先验概率 P(X)P(X)：仅仅依赖主观上的经验，事先根据已有的只是的推断 后验概率 P(X|Z)P(X|Z)：是在相关证据或者背景给定并纳入考虑以后的条件概率 似然 P(Z|X)P(Z|X)：已知结果区推测固有性质的可能性 贝叶斯公式： 后验分布正比于先验分布乘以似然。 卡尔曼滤波完整推导 1.简单的例子 若干年后，我们的可回收火箭要降落到地球，我们比较关心的状态就是我们的飞行器的高度了，飞行器的高度就是我们想要估计的状态，我们会通过一些传感器去测量我们当前的高度信息，比如说使用气压计。 假如我们每一次测量，当前高度都变成上一次测量的 95%，那么我们就可以得到如下关系： 我们可以使用递归来表示这样的公式——为了计算我们当前的高度，我们必须知道我们上一个测量的高度，以此递推，我们就会推到我们的飞行器的初始高度。 但是，由于我们的测量往往来自于一些传感器（比如说GPS，气压计），所以测量的结果总是带有噪声的，这个噪声是有传感器本身引起的，那么我们的表达式就变成了： 这个噪声我们称之为测量噪声 (Measurement Noise)。 通常来说，这种噪声都满足高斯分布。我们用符号描述以上两部分： 第一个式子是我们的 处理模型，是我们的经验（比如说一些运动模型，牛顿力学等等），我们用这种处理模型去 预测 我们考察的事物的状态（在没有任何信息的情况下，或者说在没有任何测量数据的情况下）； 第二个式子是测量的表达式，它大致描述了我们测量的组成成分，我们用这个测量去 更新 我们对状态的估计。 其中 xk 是我们的飞行器当前的状态，xk−1 是我们上一个状态（注意状态和测量的区别） zk 是我们当前对飞行器的测量，vk 是我们当前的测量噪声， a 是一个常数，在我们这个例子里面就是 0.95 很显然，现实中的运动不会像我们这个简单的处理模型一样高度按比例缩小。 在这里为了简化，我们先假设我们的模型挺好的，能够大致描述出这颗“神奇火箭”的运动规律，只是偶尔会存在一定的偏差（比如所空气湍流的影响） 那么我们在计算xkxk 时在加一个噪声来描述我们的处理模型与实际运动的差异，这个噪声我们称之为处理噪声 (ProcessProcess NoiseNoise)，我们用 wt 来表示这种噪声，那么 xk 的计算公式就变成： 为了简化，后面的分析我们会先忽略处理噪声，但是我们在传感器融合的部分会将处理噪声重新考虑进来。 2.状态估计 因为我们是要估计飞行器的状态（高度），所以我们把测量的公式变换成： 计算卡尔曼增益 预测和更新 使用线性代数的方法来表示预测和更新 至此，卡尔曼滤波的完整推导就结束了。 下面，我们来看看卡尔曼滤波在无人汽车的感知模块的应用 卡尔曼滤波算法为什么会叫滤波算法？ 以一维卡尔曼滤波为例，如果我们单纯的相信测量的信号，那么这个信号是包含噪声的，是很毛糙的 但是当我们运行卡尔曼滤波算法去做估计，我们估计的信号会很光滑，看起来似乎滤掉了噪声的影响，所以称之为滤波算法。 实际上，卡尔曼滤波不仅仅过滤掉了测量信号的噪声，它同时也结合了以往的估计，卡尔曼滤波在线性问题中被证明是最优估计。 卡尔曼滤波在无人驾驶汽车感知模块的应用 无人车感知模块的传感器 无人驾驶汽车要安全的在道路上行驶，需要“耳听六路，眼观八方”。 那么无人车的耳朵和眼睛是什么呢？那就是安装在无人车上的各种各样的传感器了。 无人车上的传感器能够多达几十个，而且是不同种类的，比如： 立体摄像机 交通标志摄像机 雷达（RADAR） 激光雷达（LIDAR） 立体摄像机往往用于获取图像和距离信息；交通标志摄像机用于交通标志的识别； 雷达一般安装在车辆的前保险杆里面，用于测量相对于车辆坐标系下的物体，可以用来定位，测距，测速等等，容易受强反射物体的干扰，通常不用于静止物体的检测 激光雷达往往安装在车顶，使用红外激光束来获得物体的距离和位置，但是空间分辨率高，但是笨重，容易被天气影响。 由此可知，各种传感器都有其优点和缺点，在实际的无人驾驶汽车里，我们往往结合多种传感器的数据去感知我们的车辆周边的环境。 这种结合各种传感器的测量数据的过程我们称之为传感器融合（Sensor Fusion）。 本系列博客后面的章节我将详细给大家介绍基于扩展卡尔曼滤波和无损卡尔曼滤波在传感器融合中的应用。 在本节中，我们主要考虑卡尔曼滤波基于单一的传感器数据来估算行人位置的方法 2.基于卡尔曼滤波的行人位置估算 卡尔曼滤波虽然简单，确实无人驾驶汽车的技术树中非常重要的一部分，当然，在真实的无人驾驶汽车项目中使用到的技术是相对更加复杂的，但是其基本原理仍然是本博客介绍的这些内容。 在无人驾驶中，卡尔曼滤波主要可以用于一些状态的估计，主要应用于行人，自行车以及其他汽车的状态估计。下面，我们以行人的状态估计为例展开。 之所以称之为恒定速度模型，是因为我们将上面这个行列式展开可以得到： 3.卡尔曼滤波行人状态估计Python例子 首先我们看一下卡尔曼滤波的整个流程，其实在实际的论文和资料中，预测矩阵通常使用FF来表示（我们前文中一直是使用AA）,测量矩阵通常使用 HH 表示（我们前文中使用的是 CC）,卡尔曼增益通常使用 KK 来表示（我们前文中使用的是 GG） 下面是文献材料中通常意义的卡尔曼滤波过程： 注意：公式还包含一项：Bμ ,这一项是指我们在追踪一个物体的状态的时候把它内部的控制也考虑进去了，这在行人，自行车，其他汽车的状态估计问题中是无法测量的，所以在这个问题中我们设置 Bμ 为 0. 我们的代码将在IPython中执行，大家可以中jupyter notebook交互式地运行。 首先我们载入必要的库： import numpy as np %matplotlib inline import matplotlib.pyplot as plt from scipy.stats import norm 接着我们初始化行人状态x, 行人的不确定性（协方差矩阵）P，测量的时间间隔dt，处理矩阵F以及测量矩阵H： x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T print(x, x.shape) P = np.diag([1000.0, 1000.0, 1000.0, 1000.0]) print(P, P.shape) dt = 0.1 # Time Step between Filter Steps F = np.matrix([[1.0, 0.0, dt, 0.0], [0.0, 1.0, 0.0, dt], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) print(F, F.shape) H = np.matrix([[0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]) print(H, H.shape) ra = 10.0**2 R = np.matrix([[ra, 0.0], [0.0, ra]]) print(R, R.shape) 计算测量过程的噪声的协方差矩阵R和处理噪声的协方差矩阵Q： ra = 0.09 R = np.matrix([[ra, 0.0], [0.0, ra]]) print(R, R.shape) sv = 0.5 G = np.matrix([[0.5*dt**2], [0.5*dt**2], [dt], [dt]]) Q = G*G.T*sv**2 from sympy import Symbol, Matrix from sympy.interactive import printing printing.init_printing() dts = Symbol('dt') Qs = Matrix([[0.5*dts**2],[0.5*dts**2],[dts],[dts]]) Qs*Qs.T 定义一个单位矩阵： I = np.eye(4) print(I, I.shape) 我们随机产生一些测量数据： m = 200 # Measurements vx= 20 # in X vy= 10 # in Y mx = np.array(vx+np.random.randn(m)) my = np.array(vy+np.random.randn(m)) measurements = np.vstack((mx,my)) print(measurements.shape) print('Standard Deviation of Acceleration Measurements=%.2f' % np.std(mx)) print('You assumed %.2f in R.' % R[0,0]) fig = plt.figure(figsize=(16,5)) plt.step(range(m),mx, label='$\dot x$') plt.step(range(m),my, label='$\dot y$') plt.ylabel(r'Velocity $m/s$') plt.title('Measurements') plt.legend(loc='best',prop={'size':18}) 一些过程值，用于结果的显示： xt = [] yt = [] dxt= [] dyt= [] Zx = [] Zy = [] Px = [] Py = [] Pdx= [] Pdy= [] Rdx= [] Rdy= [] Kx = [] Ky = [] Kdx= [] Kdy= [] def savestates(x, Z, P, R, K): xt.append(float(x[0])) yt.append(float(x[1])) dxt.append(float(x[2])) dyt.append(float(x[3])) Zx.append(float(Z[0])) Zy.append(float(Z[1])) Px.append(float(P[0,0])) Py.append(float(P[1,1])) Pdx.append(float(P[2,2])) Pdy.append(float(P[3,3])) Rdx.append(float(R[0,0])) Rdy.append(float(R[1,1])) Kx.append(float(K[0,0])) Ky.append(float(K[1,0])) Kdx.append(float(K[2,0])) Kdy.append(float(K[3,0])) 卡尔曼滤波： for n in range(len(measurements[0])): # Time Update (Prediction) # ======================== # Project the state ahead x = F*x # Project the error covariance ahead P = F*P*F.T + Q # Measurement Update (Correction) # =============================== # Compute the Kalman Gain S = H*P*H.T + R K = (P*H.T) * np.linalg.pinv(S) # Update the estimate via z Z = measurements[:,n].reshape(2,1) y = Z - (H*x) # Innovation or Residual x = x + (K*y) # Update the error covariance P = (I - (K*H))*P # Save states (for Plotting) savestates(x, Z, P, R, K) 显示一下关于速度的估计结果： def plot_x(): fig = plt.figure(figsize=(16,9)) plt.step(range(len(measurements[0])),dxt, label='$estimateVx$') plt.step(range(len(measurements[0])),dyt, label='$estimateVy$') plt.step(range(len(measurements[0])),measurements[0], label='$measurementVx$') plt.step(range(len(measurements[0])),measurements[1], label='$measurementVy$') plt.axhline(vx, color='#999999', label='$trueVx$') plt.axhline(vy, color='#999999', label='$trueVy$') plt.xlabel('Filter Step') plt.title('Estimate (Elements from State Vector $x$)') plt.legend(loc='best',prop={'size':11}) plt.ylim([0, 30]) plt.ylabel('Velocity') plot_x() 预测的结果如图所示： 位置的估计结果： def plot_xy(): fig = plt.figure(figsize=(16,16)) plt.scatter(xt,yt, s=20, label='State', c='k') plt.scatter(xt[0],yt[0], s=100, label='Start', c='g') plt.scatter(xt[-1],yt[-1], s=100, label='Goal', c='r') plt.xlabel('X') plt.ylabel('Y') plt.title('Position') plt.legend(loc='best') plt.axis('equal') plot_xy() 至此，最最简单的卡尔曼滤波以及其在无人驾驶汽车的感知模块中的应用就完成了。 这篇博客仅仅只讲了卡尔曼滤波的基本形式，并不是实际无人车项目中的所使用的技术，但是其基本原理与我们目前在Google无人车感知模块的技术是相通的。 虽然本例中以无人驾驶的行人检测作为落脚点讲解卡尔曼滤波，但是实际上我们的无人车并不会仅仅使用原始的卡尔曼滤波作为行人感知的状态估计，因为卡尔曼滤波存在着一个非常大的局限性——它仅能对线性的处理模型和测量模型进行精确的估计，在非线性的场景中并不能达到最优的估计效果，为了能够设定线性的环境，我们假定了我们的处理模型为恒定速度模型，但是显然在显示的应用中不是这样的，不论是处理模型还是测量模型，都是非线性的 下一篇我将介绍一种能够应用于非线性情况的卡尔曼滤波算法——扩展卡尔曼滤波。 参考： https://blog.csdn.net/AdamShan/article/details/78248421
无人驾驶汽车系统入门（二）——高级运动模型和扩展卡尔曼滤波
无人驾驶汽车系统入门（二）——高级运动模型和扩展卡尔曼滤波 说明： 介绍高级运动模型和扩展卡尔曼滤波 前言： 上一篇文章的最后我们提到卡尔曼滤波存在着一个非常大的局限性——它仅能对线性的处理模型和测量模型进行精确的估计，在非线性的场景中并不能达到最优的估计效果。 所以之前为了保证我们的处理模型是线性的，我们上一节中使用了恒定速度模型，然后将估计目标的加减速用处理噪声来表示，这一模型用来估算行人的状态其实已经足够了 但是在现实的驾驶环境中，我们不仅要估计行人，我们除了估计行人状态以外，我们还需要估计其他车辆，自行车等等状态，他们的状态估计显然不能使用简单的线性系统来描述 这里，我们介绍非线性系统情况下的一种广泛使用的滤波算法——扩展卡尔曼滤波(Extended Kalman Filter, EKF) 本节讲解非线性系统中广泛使用的扩展卡尔曼滤波算法，我们通常将该算法应用于实际的车辆状态估计（或者说车辆追踪）中。 另外，实际的车辆追踪运动模型显然不能使用简单的恒定速度模型来建模 在本节中会介绍几种应用于车辆追踪的高级运动模型。 并且已经其中的CTRV模型来构造我们的扩展卡尔曼滤波。 最后，在代码实例中，我会介绍如何使用EKF做多传感器融合。 应用于车辆追踪的高级运动模型 首先要明确的一点是，不管是什么运动模型，本质上都是为了帮助我们简化问题，所以我们可以根据运动模型的复杂程度（次数）来给我们常用的运动模型分一下类。 一次运动模型（也别称为线性运动模型）： 恒定速度模型（Constant Velocity, CV） 恒定加速度模型（Constant Acceleration, CA） 这些线性运动模型假定目标是直线运动的，并不考虑物体的转弯。 二次运动模型： 恒定转率和速度模型（Constant Turn Rate and Velocity，CTRV） 恒定转率和加速度模型（Constant Turn Rate and Acceleration，CTRA） CTRV CTRV目前多用于机载追踪系统（飞机），这些二次运动模型大多假定速度 vv 和 偏航角速度（yaw rate） ωω 没有关系，因此，在这类运动模型中，由于偏航角速度测量的扰动（不稳定），即使车辆没有移动，我们的运动模型下的角速度也会发生细微的变化。 为了解决这个问题，速度 vv 和 偏航角速度 ωω 的关联可以通过设定转向角 ΦΦ 恒定来建立，这样就引出了 恒定转向角和速度模型（Constant Steering Angle and Velocity，CSAV）, 另外，速度可以别假定为线性变化的，进而引出了常曲率和加速度模型（Constant Curvature and Acceleration，CCA）。 这些运动模型的关系如图： 运动模型的状态转移公式 由于除CCA以外，以上的运动模型都非常著名，故本文不提供详细的推导过程。本文提供CV和CTRV模型的状态转移公式。 状态转移公式：就是我们的处理模型由上一状态的估计计算下一个状态的先验分布的计算公式，可以理解为我们基于一定的先验 知识总结出来的运动公式。 扩展卡尔曼滤波 雅可比矩阵 处理噪声 测量 将预测映射到激光雷达测量空间： 虽然这个雅可比矩阵看似非常复杂，但是我们待会编程的时候并不需要完整的推导出这个雅可比矩阵的表达式，在本篇中，我们采用numdifftools这个公式来直接求解雅可比矩阵。 综上，EKF的整个过程为： 4.Python实现 和之前一样，为了实现交互式的代码，实例的代码为了便于理解，我们仍然使用大家熟悉的Python来实现 当然，实际无人车项目肯定是需要用C++来实现的，要将下面的示例代码使用C++来改写是非常简单快速的。 首先引入相关的库： from __future__ import print_function import numpy as np import matplotlib.dates as mdates import matplotlib.pyplot as plt from scipy.stats import norm from sympy import Symbol, symbols, Matrix, sin, cos, sqrt, atan2 from sympy import init_printing init_printing(use_latex=True) import numdifftools as nd import math 首先我们读取我们的数据集，该数据集包含了追踪目标的LIDAR和RADAR的测量值，以及测量的时间点 同时为了验证我们追踪目标的精度，该数据还包含了追踪目标的真实坐标。（数据下载链接见文章末尾） 其中第一列表示测量数据来自LIDAR还是RADAR，LIDAR的2，3列表示测量的目标 (x,y)(x,y),第4列表示测量的时间点，第5，6，7，8表示真实的(x,y,vx,vy)(x,y,vx,vy), RADAR测量的(前三列)是(ρ,ψ,ρ˙)(ρ,ψ,ρ˙)，其余列的数据的意义和LIDAR一样。 首先我们读取整个数据： dataset = [] # read the measurement data, use 0.0 to stand LIDAR data # and 1.0 stand RADAR data with open('data_synthetic.txt', 'rb') as f: lines = f.readlines() for line in lines: line = line.strip('\n') line = line.strip() numbers = line.split() result = [] for i, item in enumerate(numbers): item.strip() if i == 0: if item == 'L': result.append(0.0) else: result.append(1.0) else: result.append(float(item)) dataset.append(result) f.close() P = np.diag([1.0, 1.0, 1.0, 1.0, 1.0]) print(P, P.shape) H_lidar = np.array([[ 1., 0., 0., 0., 0.], [ 0., 1., 0., 0., 0.]]) print(H_lidar, H_lidar.shape) R_lidar = np.array([[0.0225, 0.],[0., 0.0225]]) R_radar = np.array([[0.09, 0., 0.],[0., 0.0009, 0.], [0., 0., 0.09]]) print(R_lidar, R_lidar.shape) print(R_radar, R_radar.shape) # process noise standard deviation for a std_noise_a = 2.0 # process noise standard deviation for yaw acceleration std_noise_yaw_dd = 0.3 在整个预测和测量更新过程中，所有角度量的数值都应该控制在 [−π,π]，我们知道角度加减 2π 不变， 所以用如下函数表示函数来调整角度： def control_psi(psi): while (psi > np.pi or psi < -np.pi): if psi > np.pi: psi = psi - 2 * np.pi if psi < -np.pi: psi = psi + 2 * np.pi return psi 具体状态初始化代码为： state = np.zeros(5) init_measurement = dataset[0] current_time = 0.0 if init_measurement[0] == 0.0: print('Initialize with LIDAR measurement!') current_time = init_measurement[3] state[0] = init_measurement[1] state[1] = init_measurement[2] else: print('Initialize with RADAR measurement!') current_time = init_measurement[4] init_rho = init_measurement[1] init_psi = init_measurement[2] init_psi = control_psi(init_psi) state[0] = init_rho * np.cos(init_psi) state[1] = init_rho * np.sin(init_psi) print(state, state.shape) 写一个辅助函数用于保存数值： # Preallocation for Saving px = [] py = [] vx = [] vy = [] gpx = [] gpy = [] gvx = [] gvy = [] mx = [] my = [] def savestates(ss, gx, gy, gv1, gv2, m1, m2): px.append(ss[0]) py.append(ss[1]) vx.append(np.cos(ss[3]) * ss[2]) vy.append(np.sin(ss[3]) * ss[2]) gpx.append(gx) gpy.append(gy) gvx.append(gv1) gvy.append(gv2) mx.append(m1) my.append(m2) 定义状态转移函数和测量函数，使用numdifftools库来计算其对应的雅可比矩阵， 这里我们先设 Δt=0.05，只是为了占一个位置，当实际运行EKF时会计算出前后两次测量的时间差，一次来替换这里的 Δt。 measurement_step = len(dataset) state = state.reshape([5, 1]) dt = 0.05 I = np.eye(5) transition_function = lambda y: np.vstack(( y[0] + (y[2] / y[4]) * (np.sin(y[3] + y[4] * dt) - np.sin(y[3])), y[1] + (y[2] / y[4]) * (-np.cos(y[3] + y[4] * dt) + np.cos(y[3])), y[2], y[3] + y[4] * dt, y[4])) # when omega is 0 transition_function_1 = lambda m: np.vstack((m[0] + m[2] * np.cos(m[3]) * dt, m[1] + m[2] * np.sin(m[3]) * dt, m[2], m[3] + m[4] * dt, m[4])) J_A = nd.Jacobian(transition_function) J_A_1 = nd.Jacobian(transition_function_1) # print(J_A([1., 2., 3., 4., 5.])) measurement_function = lambda k: np.vstack((np.sqrt(k[0] * k[0] + k[1] * k[1]), math.atan2(k[1], k[0]), (k[0] * k[2] * np.cos(k[3]) + k[1] * k[2] * np.sin(k[3])) / np.sqrt(k[0] * k[0] + k[1] * k[1]))) J_H = nd.Jacobian(measurement_function) # J_H([1., 2., 3., 4., 5.]) EKF的过程代码： for step in range(1, measurement_step): # Prediction # ======================== t_measurement = dataset[step] if t_measurement[0] == 0.0: m_x = t_measurement[1] m_y = t_measurement[2] z = np.array([[m_x], [m_y]]) dt = (t_measurement[3] - current_time) / 1000000.0 current_time = t_measurement[3] # true position g_x = t_measurement[4] g_y = t_measurement[5] g_v_x = t_measurement[6] g_v_y = t_measurement[7] else: m_rho = t_measurement[1] m_psi = t_measurement[2] m_dot_rho = t_measurement[3] z = np.array([[m_rho], [m_psi], [m_dot_rho]]) dt = (t_measurement[4] - current_time) / 1000000.0 current_time = t_measurement[4] # true position g_x = t_measurement[5] g_y = t_measurement[6] g_v_x = t_measurement[7] g_v_y = t_measurement[8] if np.abs(state[4, 0]) < 0.0001: # omega is 0, Driving straight state = transition_function_1(state.ravel().tolist()) state[3, 0] = control_psi(state[3, 0]) JA = J_A_1(state.ravel().tolist()) else: # otherwise state = transition_function(state.ravel().tolist()) state[3, 0] = control_psi(state[3, 0]) JA = J_A(state.ravel().tolist()) G = np.zeros([5, 2]) G[0, 0] = 0.5 * dt * dt * np.cos(state[3, 0]) G[1, 0] = 0.5 * dt * dt * np.sin(state[3, 0]) G[2, 0] = dt G[3, 1] = 0.5 * dt * dt G[4, 1] = dt Q_v = np.diag([std_noise_a*std_noise_a, std_noise_yaw_dd*std_noise_yaw_dd]) Q = np.dot(np.dot(G, Q_v), G.T) # Project the error covariance ahead P = np.dot(np.dot(JA, P), JA.T) + Q # Measurement Update (Correction) # =============================== if t_measurement[0] == 0.0: # Lidar S = np.dot(np.dot(H_lidar, P), H_lidar.T) + R_lidar K = np.dot(np.dot(P, H_lidar.T), np.linalg.inv(S)) y = z - np.dot(H_lidar, state) y[1, 0] = control_psi(y[1, 0]) state = state + np.dot(K, y) state[3, 0] = control_psi(state[3, 0]) # Update the error covariance P = np.dot((I - np.dot(K, H_lidar)), P) # Save states for Plotting savestates(state.ravel().tolist(), g_x, g_y, g_v_x, g_v_y, m_x, m_y) else: # Radar JH = J_H(state.ravel().tolist()) S = np.dot(np.dot(JH, P), JH.T) + R_radar K = np.dot(np.dot(P, JH.T), np.linalg.inv(S)) map_pred = measurement_function(state.ravel().tolist()) if np.abs(map_pred[0, 0]) < 0.0001: # if rho is 0 map_pred[2, 0] = 0 y = z - map_pred y[1, 0] = control_psi(y[1, 0]) state = state + np.dot(K, y) state[3, 0] = control_psi(state[3, 0]) # Update the error covariance P = np.dot((I - np.dot(K, JH)), P) savestates(state.ravel().tolist(), g_x, g_y, g_v_x, g_v_y, m_rho * np.cos(m_psi), m_rho * np.sin(m_psi)) 这里有几点需要注意，首先，要考虑清楚有几个地方被除数有可能为 0，比如说ω=0，以及 ρ=0 的情况。 处理完以后我们输出估计的均方误差（RMSE），并且把各类数据保存以便我们可视化我们的EKF的效果： def rmse(estimates, actual): result = np.sqrt(np.mean((estimates-actual)**2)) return result print(rmse(np.array(px), np.array(gpx)), rmse(np.array(py), np.array(gpy)), rmse(np.array(vx), np.array(gvx)), rmse(np.array(vy), np.array(gvy))) # write to the output file stack = [px, py, vx, vy, mx, my, gpx, gpy, gvx, gvy] stack = np.array(stack) stack = stack.T np.savetxt('output.csv', stack, '%.6f') 最后我们来看一下我们的EKF在追踪目标的时候的均方误差： 0.0736336090893 0.0804598933194 0.229165985264 0.309993887661 我们把我们的EKF的估计结果可视化： 我们放大一个局部看一下： 其中，蓝色的点为我们的EKF对目标位置的估计，橙色的点为来自LIDAR和RADAR的测量值，绿色的点是目标的真实位置 由此可知，我们的测量是不准确的，因此我们使用EKF在结合运动模型的先验知识以及融合两个传感器的测量的基础上做出了非常接近目标正是状态的估计。 EKF的魅力其实还不止在此！我们使用EKF，不仅能够对目标的状态（我们关心的）进行准确的估计，从这个例子我们会发现，EKF还能估计我们的传感器无法测量的量（比如本例中的v,ψ,ψ˙） 那么，扩展卡尔曼滤波就到此结束了，大家可能会问，怎么没看到可视化结果那一部分的代码？哈哈，为了吸引一波人气，请大家关注我的博客，我会在下一期更新中（无损卡尔曼滤波）提供这一部分代码。 最后，细心的同学可能会发现，我们这个EKF执行的效率太低了，实际上，EKF的一个最大的问题就是求解雅可比矩阵计算量比较大，而且相关的偏导数推导也是非常无聊的工作，因此引出了我们下一篇要讲的内容，无损卡尔曼滤波（Unscented Kalman Filter，UKF） 附数据下载链接：http://download.csdn.net/download/adamshan/10033533 参考： https://blog.csdn.net/AdamShan/article/details/78265754
无人驾驶汽车系统入门（三）——无损卡尔曼滤波，目标追踪，C++
无人驾驶汽车系统入门（三）——无损卡尔曼滤波，目标追踪，C++ 说明： 介绍前面两篇文章我们了解了卡尔曼滤波以及扩展卡尔曼滤波在目标追踪的应用，我们在上一篇文章中还具体用Python实现了EKF，但是细心的同学会发现，EKF的效率确实很低，计算雅可比矩阵确实是一个很费时的操作，当问题（非线性的）一旦变得复杂，其计算量就变得十分不可控制。在此再向大家接受一种滤波——无损卡尔曼滤波（Unscented Kalman Filter， UKF） UKF 通过上一篇文章，我们已经知道KF不适用于非线性系统，为处理非线性系统，我们通过一阶泰勒展式来近似（用线性函数近似） 这个方案直接的结果就是，我们对于具体的问题都要求解对应的一阶偏导（雅可比矩阵），求解雅可比矩阵本身就是费时的计算，而且我们上一篇还使用了Python而非C++，而且我们图省事还用了一个叫做numdifftools的库来实现雅可比矩阵的求解而不是自行推导，这一切都造成了我们上一次博客的代码执行效率奇低！ 显然现实应用里面我们的无人车是不能接受这种延迟的，我相信很多同学也和我一样讨厌求解雅可比矩阵，那么，就让我们来学习一种相对简单的状态估计算法——UKF。 UKF使用的是统计线性化技术，我们把这种线性化的方法叫做无损变换（unscented transformation）这一技术主要通过n个在先验分布中采集的点（我们把它们叫sigma points）的线性回归来线性化随机变量的非线性函数 由于我们考虑的是随机变量的扩展，所以这种线性化要比泰勒级数线性化（EKF所使用的策略）更准确。 和EKF一样，UKF也主要分为预测和更新。 运动模型 非线性处理/测量模型 我们知道我们在应用KF是面临的主要问题就是非线性处理模型（比如说CTRV）和非线性测量模型（RADAR测量）的处理。 我们从概率分布的角度来描述这个问题： UKF的基本思路就是： 近似非线性函数的概率分布要比近似非线性函数本身要容易！ 那么如何去找一个与真实分布近似的高斯分布呢？——找一个与真实分布有着相同的均值和协方差的高斯分布。 那么如何去找这样的均值和方差呢？——无损变换。 无损变换 计算一堆点（术语叫做sigma point），通过一定的手段产生的这些sigma点能够代表当前的分布， 然后将这些点通过非线性函数（处理模型）变换成一些新的点， 然后基于这些新的sigma点计算出一个高斯分布（带有权重地计算出来）如图： 预测 由一个高斯分布产生sigma point 通常，假定状态的个数为 n ，我们会产生 2n+1 个sigma点， 其中第一个就是我们当前状态的均值 μ ，sigma点集的均值的计算公式为： 预测sigma point 预测均值和方差 测量更新 1.预测测量（将先验映射到测量空间然后算出均值和方差） 这篇博客继续使用上一篇（EKF）中的测量实验数据，那么我们知道，测量更新分为两个部分，LIDAR测量和RADAR测量， 其中LIDAR测量模型本身就是线性的，所以我们重点还是放在RADAR测量模型的处理上面，RADAR的测量映射函数为： 2.更新状态 http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.136.6539&rep=rep1&type=pdf https://www.cse.sc.edu/~terejanu/files/tutorialUKF.pdf http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/pdf/slam06-ukf-4.pdf UKF实例 我们继续基于上一篇文章的数据来做状态估计的实例，不过，为了更加接近实际实际的应用，我们本节采用C++实现。 由于本节使用的C++代码量相对较大，而且为多文件的项目，代码就不再一一贴出，所有代码我都已经上传至如下地址： http://download.csdn.net/download/adamshan/10041096 运行-效果 首先解压，编译: mkdir build cd build cmake .. make 运行： ./ukf ../data/data_synthetic.txt ../data/output.txt 得到计算的结果： Accuracy - RMSE: 0.0723408 0.0821208 0.342265 0.23017 我们发现，这个UKF要比我们上一篇博客写的EKF的效率要高得多得多。 我们执行可视化的python脚本看一下效果： cd ../data python plot.py 得到如下结果： 放大一点： UKF在我们这个问题中的执行效率和估计精度都高于我们上一篇文章中实现的EKF,下面就让我们看一下核心代码。 核心代码 首先是预测，预测主要包含三部分，分别是： 产生sigma点集 基于CTRV模型预测sigma点集 计算新的均值核方差 产生sigma点集 void UKF::AugmentedSigmaPoints(MatrixXd *Xsig_out) { //create augmented mean vector VectorXd x_aug = VectorXd(7); //create augmented state covariance MatrixXd P_aug = MatrixXd(7, 7); //create sigma point matrix MatrixXd Xsig_aug = MatrixXd(n_aug_, 2 * n_aug_ + 1); //create augmented mean state //create augmented covariance matrix //create square root matrix //create augmented sigma points x_aug.head(5) = x_; x_aug(5) = 0; x_aug(6) = 0; P_aug.fill(0.0); P_aug.topLeftCorner(5, 5) = P_; P_aug(5,5) = std_a_*std_a_; P_aug(6,6) = std_yawdd_*std_yawdd_; MatrixXd A = P_aug.llt().matrixL(); //create augmented sigma points Xsig_aug.col(0) = x_aug; for (int i = 0; i< n_aug_; i++) { Xsig_aug.col(i+1) = x_aug + sqrt(lambda_+n_aug_) * A.col(i); Xsig_aug.col(i+1+n_aug_) = x_aug - sqrt(lambda_+n_aug_) * A.col(i); } //write result *Xsig_out = Xsig_aug; } 2.基于CTRV模型预测sigma点集 void UKF::SigmaPointPrediction(MatrixXd &Xsig_aug, double delta_t) { for(int i =0; i < (2 * n_aug_ + 1); i++){ VectorXd input_x = Xsig_aug.col(i); float px = input_x[0]; float py = input_x[1]; float v = input_x[2]; float psi = input_x[3]; float psi_dot = input_x[4]; float mu_a = input_x[5]; float mu_psi_dot_dot = input_x[6]; VectorXd term2 = VectorXd(5); VectorXd term3 = VectorXd(5); VectorXd result = VectorXd(5); if(psi_dot < 0.001){ term2 << v * cos(psi) * delta_t, v * sin(psi) * delta_t, 0, psi_dot * delta_t, 0; term3 << 0.5 * delta_t*delta_t * cos(psi) * mu_a, 0.5 * delta_t*delta_t * sin(psi) * mu_a, delta_t * mu_a, 0.5 * delta_t*delta_t * mu_psi_dot_dot, delta_t * mu_psi_dot_dot; result = Xsig_aug.col(i).head(5) + term2 + term3; } else{ term2 << (v/psi_dot) * (sin(psi + psi_dot * delta_t) - sin(psi)), (v/psi_dot) * (-cos(psi + psi_dot * delta_t) + cos(psi)), 0, psi_dot * delta_t, 0; term3 << 0.5 * delta_t*delta_t * cos(psi) * mu_a, 0.5 * delta_t*delta_t * sin(psi) * mu_a, delta_t * mu_a, 0.5 * delta_t*delta_t * mu_psi_dot_dot, delta_t * mu_psi_dot_dot; result = Xsig_aug.col(i).head(5) + term2 + term3; } Xsig_pred_.col(i) = result; } } 3.计算新的均值核方差 void UKF::PredictMeanAndCovariance() { x_.fill(0.0); for(int i=0; i<2*n_aug_+1; i++){ x_ = x_+ weights_[i] * Xsig_pred_.col(i); } P_.fill(0.0); for(int i=0; i<2*n_aug_+1; i++){ VectorXd x_diff = Xsig_pred_.col(i) - x_; while (x_diff[3]> M_PI) x_diff[3] -= 2.*M_PI; while (x_diff[3] <-M_PI) x_diff[3]+=2.*M_PI; P_ = P_ + weights_[i] * x_diff * x_diff.transpose(); } } 测量更新主要分为 预测LIDAR测量 预测RADAR测量 更新状态 预测LIDAR测量 void UKF::PredictLaserMeasurement(VectorXd &z_pred, MatrixXd &S, MatrixXd &Zsig, long n_z) { for(int i=0; i < 2*n_aug_+1; i++){ float px = Xsig_pred_.col(i)[0]; float py = Xsig_pred_.col(i)[1]; VectorXd temp = VectorXd(n_z); temp << px, py; Zsig.col(i) = temp; } z_pred.fill(0.0); for(int i=0; i < 2*n_aug_+1; i++){ z_pred = z_pred + weights_[i] * Zsig.col(i); } S.fill(0.0); for(int i=0; i < 2*n_aug_+1; i++){ //residual VectorXd z_diff = Zsig.col(i) - z_pred; S = S + weights_[i] * z_diff * z_diff.transpose(); } S = S + R_laser_; } 预测RADAR测量 void UKF::PredictRadarMeasurement(VectorXd &z_pred, MatrixXd &S, MatrixXd &Zsig, long n_z) { for(int i=0; i < 2*n_aug_+1; i++){ float px = Xsig_pred_.col(i)[0]; float py = Xsig_pred_.col(i)[1]; float v = Xsig_pred_.col(i)[2]; float psi = Xsig_pred_.col(i)[3]; float psi_dot = Xsig_pred_.col(i)[4]; float temp = px * px + py * py; float rho = sqrt(temp); float phi = atan2(py, px); float rho_dot; if(fabs(rho) < 0.0001){ rho_dot = 0; } else{ rho_dot =(px * cos(psi) * v + py * sin(psi) * v)/rho; } VectorXd temp1 = VectorXd(3); temp1 << rho, phi, rho_dot; Zsig.col(i) = temp1; } z_pred.fill(0.0); for(int i=0; i < 2*n_aug_+1; i++){ z_pred = z_pred + weights_[i] * Zsig.col(i); } S.fill(0.0); for(int i=0; i < 2*n_aug_+1; i++){ //residual VectorXd z_diff = Zsig.col(i) - z_pred; //angle normalization while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI; while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI; S = S + weights_[i] * (Zsig.col(i) - z_pred) * (Zsig.col(i) - z_pred).transpose(); } S = S + R_radar_; } 3.更新状态 void UKF::UpdateState(VectorXd &z, VectorXd &z_pred, MatrixXd &S, MatrixXd &Zsig, long n_z) { //create matrix for cross correlation Tc MatrixXd Tc = MatrixXd(n_x_, n_z); //calculate cross correlation matrix //calculate Kalman gain K; //update state mean and covariance matrix Tc.fill(0.0); for(int i=0; i < 2*n_aug_+1; i++){ VectorXd x_diff = Xsig_pred_.col(i) - x_; while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI; while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI; //residual VectorXd z_diff = Zsig.col(i) - z_pred; if(n_z == 3){ //angle normalization while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI; while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI; } Tc = Tc + weights_[i] * x_diff * z_diff.transpose(); } MatrixXd K = MatrixXd(5, 3); K = Tc * S.inverse(); VectorXd y = z - z_pred; //angle normalization if(n_z == 3){ while (y(1)> M_PI) y(1)-=2.*M_PI; while (y(1)<-M_PI) y(1)+=2.*M_PI; } x_ = x_ + K * y; P_ = P_ - K * S * K.transpose(); } 以上就是我们的UKF的核心算法实现了，可能光看这些核心代码还是没办法理解，所以感兴趣的童鞋就去下载来再详细研究把～ 上一篇文章我买了个关子，没有贴出把结果可视化的代码，本次代码已经一本包含在项目里（具体见/data/plot.py） 前面三篇文章都介绍了卡尔曼滤波相关的算法，大家应该已经看出卡尔曼滤波这类贝叶斯滤波器在无人驾驶汽车系统哦的重要性了，卡尔曼滤波作为重要的状态估计算法，是无人驾驶的必备技能点，所以相关的扩展阅读大家可以多多开展， 本系列暂时就不再讨论了，下面我想向大家介绍应用于无人驾驶中的控制算法，控制作为自动化中重要的技术，有着很多的分支，后面的文章我会继续由理论到实践逐一向大家介绍相关技术～ 参考： https://blog.csdn.net/AdamShan/article/details/78359048
无人驾驶汽车系统入门（四）——反馈控制入门，PID控制
无人驾驶汽车系统入门（四）——反馈控制入门，PID控制 说明： 前面几篇博客介绍了卡尔曼滤波的一些基本算法，其实目标追踪，定位，传感器融合还有很多问题要处理，这些我们在以后的系列博客中在进一步细讲，现在我想给大家介绍一下无人驾驶汽车系统开发中需要的控制相关的理论和技术，还是和第一篇说的那样，我想到哪就写到哪，追踪和定位等更高级的算法我在后面会继续写。所以感兴趣的同学可以关注我的博客，无人驾驶汽车系统入门系列博客会一直更新下去。 为什么需要控制理论 试想有如下场景，当你驾驶一辆汽车通过这个弯道的时候，假设你已经知道你要开的路线，那么你会怎么去操作控制你的车呢？ 显然，如果你不是专业的选手的话，你无法做到一步到位的控制，你需要一边观察车辆相对于你想要开的路线的相对偏差，一边调整你的方向盘的角度和油门踏板的力度，这种基于环境反馈的控制我们称为 反馈控制 。 反馈控制是现代控制理论的基础，这是反馈控制的一般思路： 我们希望我们控制的对象（无人车）能够按照我们希望（规划好）的路径行驶， 我们会将环境当前给我们的反馈（我们当前的位置）和参考线进行比较，得到我们当前偏离参考线的距离（误差）， 基于这个误差，我们设计一定的算法来产生输出信号，使得这个误差不断的变小，这样的过程就是反馈控制的一般过程。 那么我们如何基于这个误差来产生控制指令呢？ 我们最直观的感觉就是要让误差在我们的控制下逐渐变小直到为0： 0误差就意味着车一直在你想让它开的路径上开。如何减少误差就是我们这几篇博客要向大家介绍的内容。 为了了解反馈控制，我先向大家介绍 PID控制，PID控制是目前利用最为广泛的控制理论，我们以它为出发点讨论控制理论。 比例，积分和导数 PID就是指 比例（proportion）、积分（integral）、导数（derivative）， 这三项表示我们如何使用我们的误差来产生控制指令，整个流程如下： 1.P控制 考虑一个简单的情况，假设我们希望无人车按照图中绿线行驶，但是我们的车在如图所示的位置： 那么我们要转多少度角呢？如果都按照固定的角度转（如下图），那么车的轨迹将如图中所示： 那么显然坐这样的车是不舒服的。一个直观的解决方法就是使用比例控制。 如图所示，当偏差大的时候，我们偏转更多的角度，当偏差小的时候，则偏转小一点。 那么这就是P control（比例控制）这里我们使用 CTE（Cross Track Error） 作为偏差度量 ， CTE就是我们到参考线的距离。那么这个时候转角就变成了： 此时车辆虽然在参考线上，但是并不是我们希望的状态（它在下一刻就会偏离），但是对于P控制而言，这是理想状态，此时控制转角为0,因此，P控制会一次又一次的超过参考线（overshot），为了矫正这种overshot，我们需要考虑一个额外的误差项——CTE变化率。 2.PD控制 CTE的变化率描述了我们的无人车向着参考线方向移动的有多快，如果我们的无人车一直都完美的在参考线上运动的话，那么我们的CTE变化率就为0。那么这一项（描述误差的变化率）就可以用导数来表示，那么，现在我们的控制输出就变成了比例项和导数项求和的形式： PD控制似乎已经能够胜任良好的反馈控制了，但其实还不够，PD控制器可以保证正常的控制的需求，但是当环境存在扰动的时候，比如说下面这种情况： 车在受力发生轻微偏移以后，由于PD控制中下 P 项倾向于向参考线方向运动，而 D 项则尝试抵消这种倾向，造成无人车始终都无法沿着参考线运动，这个问题叫做 steady state error 为了解决这个问题，我们再引入一项—— 积分项。 3.PID控制 我们将积分项也就如到我们的控制输出函数中，这个时候，无人车的转角就可以表示为： 我们发现其实PID实现确实不难，但是三个系数的选择却很难，那么如何选择PID系数呢？ 我们可以在我们的控制循环中通过一定的算法不断尝试，下面我提供给大家一种寻找参数的算法： 具体的算法见我的C++代码实例。 PID C++代码 1.pid.cpp #include <limits> #include <iostream> #include "PID.h" //using namespace std; PID::PID() {} PID::~PID() {} void PID::Init(double Kp, double Ki, double Kd) { parameter.push_back(Kp); parameter.push_back(Ki); parameter.push_back(Kd); this->p_error = 99999999.; this->d_error = 0.0; this->i_error = 0.0; //twiddle parameters need_twiddle = false; step = 1; // let the car run at first 100 steps, then in the next 3000 steps add the cte^2 to the total_error val_step = 100; test_step = 2000; for (int i = 0; i < 3; ++i) { // init the change rate with the value of 0.1*parameter changes.push_back(0.1 * parameter[i]); } index_param = 0; best_error = std::numeric_limits<double>::max(); total_error = 0; // fail to make the total_error better times fail_counter = 0; } void PID::UpdateError(double cte) { if(step == 1){ p_error = cte; } d_error = cte - p_error; p_error = cte; i_error += cte; if(need_twiddle){ if(step % (val_step + test_step) > val_step){ total_error += (cte * cte); } if(step % (val_step + test_step) == 0){ std::cout<<"============== step "<<step<<" =============="<<std::endl; std::cout << "P: "<< parameter[0]<<" I: "<<parameter[1]<<" D: "<<parameter[2]<<std::endl; if (step == (val_step + test_step)){ if(total_error < best_error){ best_error = total_error; } parameter[index_param] += changes[index_param]; } else{ if(total_error < best_error){ best_error = total_error; changes[index_param] *= 1.1; IndexMove(); parameter[index_param] += changes[index_param]; fail_counter = 0; } else if(fail_counter == 0){ parameter[index_param] -= (2*changes[index_param]); fail_counter++; } else{ parameter[index_param] += changes[index_param]; changes[index_param] *= 0.9; IndexMove(); parameter[index_param] += changes[index_param]; fail_counter = 0; } } std::cout << "best_error: "<< best_error<<" total_error: "<<total_error<<std::endl; std::cout << "change_index: "<<index_param<<" new_parameter: "<<parameter[index_param]<<std::endl; std::cout << std::endl; total_error = 0; } } step++; } double PID::TotalError() { return -parameter[0] * p_error - parameter[1] * i_error - parameter[2] * d_error; } void PID::IndexMove() { index_param++; if(index_param >=3){ index_param = 0; } } pid.h #ifndef PID_H #define PID_H #include <cmath> #include <vector> class PID { private: int step; std::vector<double> changes; double best_error; double total_error; int index_param; int val_step; int test_step; int fail_counter; void IndexMove(); bool need_twiddle; public: /* * Errors */ double p_error; double i_error; double d_error; /* * Coefficients, the order is P, I, D */ std::vector<double> parameter; /* * Constructor */ PID(); /* * Destructor. */ virtual ~PID(); /* * Initialize PID. */ void Init(double Kp, double Ki, double Kd); /* * Update the PID error variables given cross track error. */ void UpdateError(double cte); /* * Calculate the total PID error. */ double TotalError(); }; #endif /* PID_H */ 3.用法 在你的实际控制循环中，调用： PID pid; pid.Init(0.3345, 0.0011011, 2.662); //your init parameters for (in your control loop) { pid.UpdateError(cte); steer_value = pid.TotalError(); } 参考： https://blog.csdn.net/adamshan/article/details/78458325
无人驾驶汽车系统入门（五）——运动学自行车模型和动力学自行车模型
无人驾驶汽车系统入门（五）——运动学自行车模型和动力学自行车模型 说明： 在简要了解了PID控制以后，我们就要接触一些现代的控制算法。 在了解高级的车辆控制算法之前，掌握车辆运动模型是非常有必要的。 车辆运动模型就是一类能够描述我们的车辆的运动规律的模型。 显然，越复杂的模型就越接近现实中的车辆运动规律，本节我们一起了解一下两个广泛使用的车辆模型 运动学自行车模型（Kinematic Bicycle Model） 和 动力学自行车模型（Dynamic Bicycle Model） 无人驾驶系统 无人驾驶系统往往分成感知，决策和控制三个模块， 其中无人车的路径规划和底层控制是工作在不同的层的，路径规划层往往会基于更加高层的（感知层，定位层）的信息和底层的（控制层）的实时信息指定行驶的路径， 那么从路径规划层传来的就是车辆的参考路径，控制系统需要做的就是严格按照这个参考路径（以及速度等控制输入量）去驾驶我们的车辆， 一般来说，我们会用多项式的行驶来描述这个路径曲线，如下所示的三次多项式就可以描述绝大多数的路径了： 自行车模型（Bicycle Model） 首先我们要简化汽车运动，其中自行车模型就是简单且有效的简化方式。自行车模型基于如下几个假设： 车辆在垂直方向的运动被忽略掉了，也就是说我们描述的车辆是一个二维平面上的运动物体（可以等价与我们是站在天空中的俯视视角） 我们假设车辆的结构就像自行车一样，也就是说车辆的前面两个轮胎拥有一直的角度和转速等，同样后面的两个轮胎也是如此，那么前后的轮胎就可以各用一个轮胎来描述 我们假设车辆运动也和自行车一样，这意味着是前面的轮胎控制这车辆的转角 首先我们简单的在一个二维平面上描述一个车辆： 其中 θ是其在 Yaw 方向的偏转角度，它是相对于 x 轴的逆时针方向的角度，v 是 θ 方向的速度，L 是车辆的轴距（前后轮胎的距离）, (x,y) 是车辆的坐标。 下图是该车辆的自行车模型： 运动学自行车模型 然后我们定义我们模型中的状态量，运动学自行车模型使用四个状态量来描述车辆的当前状态： x : 即车辆当前的 x 坐标 y : 即车辆当前的 y 坐标 ψ : 即车辆当前的偏航角（Yaw 方向的偏角，往往用弧度来描述, 逆时针方向为正） v : 即车辆的速度 一个简单的运动学自行车模型如图所示： 由于绝大多数的汽车后轮都不能够偏转，所以我们的自行车模型就假定后轮的转角控制输入 δr=0， 也就是说，方向盘上的控制输入，都反映到了前轮的转角上了。 那么基于这个简单的运动学自行车模型，在给定了一个时刻的控制输入以后，我们可以计算求得 dt 时间以后我们车辆的状态信息（坐标，偏航角以及速度），那么这个模型就可以作为我们模型预测控制下的基础车辆模型了。 动力学自行车模型 前面的车辆运动学自行车模型其实还隐含着一个重要的假设，那就是： 车前轮的方向即是车辆当前的速度方向，在实际车辆运动过程中，当车辆在以相对高的速度行驶时，车轮的方向并不一定车辆当前的速度方向，这个时候，我们引入车辆的动力学自行车模型。 车辆动力学模型通过对轮胎和路面之间的复杂相互作用来描述车辆的运动。在一个动力模型中，我们需要考虑各种各样的力的作用，他们可以大致分为两类： 纵向力(Longitudinal force) 和 侧向力（Lateral force）, 纵向力就是使车辆前后移动的力量，而侧向力则促使车辆在横向移动，在力的相互作用过程中，轮胎起着决定性的作用（根据一定的物理常识，轮胎是车辆运动的一个重要的力的来源）。 运动学自行车模型的Python实现 我们使用Python代码简单实现一个运动学自行车模型类，作为后期我们使用模型预测控制的车辆模型。 from __future__ import print_function import math class KinematicModel(object): def __init__(self, x, y, psi, v, f_len, r_len): self.x = x self.y = y self.psi = psi self.v = v self.f_len = f_len self.r_len = r_len def get_state(self): return self.x, self.y, self.psi, self.v def update_state(self, a, delta, dt): beta = math.atan((self.r_len / (self.r_len + self.f_len)) * math.tan(delta)) self.x = self.x + self.v * math.cos(self.psi * beta) * dt self.y = self.y + self.v * math.sin(self.psi * beta) * dt self.psi = self.psi + (self.v / self.f_len) * math.sin(beta) * dt self.v = self.v + a * dt return self.x, self.y, self.psi, self.v 这个简易的车辆模型中，我们的控制量包含了一个前胎的转角和一个加速度a，由于受车辆机械的限制，车辆本身有很多动作是实现不了的，比如说加速度a的值不可能过大，轮胎的转角也会有极限，我们称这种性质叫做模型非完整性（model nonholonomic）
无人驾驶汽车系统入门（六）——基于传统计算机视觉的车道线检测(1)
无人驾驶汽车系统入门（六）——基于传统计算机视觉的车道线检测(1) 说明： 介绍如何进行车道线检测 感知，作为无人驾驶汽车系统中的“眼睛”，是目前无人驾驶汽车量产和商用化的最大障碍之一（技术角度）， 目前，高等级的无人驾驶汽车系统仍然非常依赖于激光雷达的测量，通过激光雷达构造周围环境的3D地图，从而为无人驾驶系统的决策和规划提供准确的环境信息和自身相对的位置信息。 然而，激光雷达雷达在成本，解析度的方面都不理想，所以基于视觉的无人驾驶相关技术（主要在感知方面）近几年发展迅猛， 本节我们从传统的 计算机视觉（Computer Version, CV）出发，来了解并且实践基于传统计算机视觉算法的车道线检测技术。 视觉感知VS激光雷达 激光雷达能够为无人驾驶汽车提供障碍物检测，高精度地图和定位等多方面的支持，在目前各个研究团队广泛使用。 下图是激光雷达的简易构造： 激光雷达以一定的角速度旋转，发射出波长在600nm到1000nm之间的激光射线，同时收集来自反射点的信息，激光雷达每旋转一周，收集到的反射点的坐标构成的一个集合，我们称之为 点云（Point Cloud）。 下图就是一张点云图： 激光雷达具有可靠性高，障碍物细节分辨以及精准测距等优点，同时不受光线条件的影响。但是，激光雷达的价格居高不下，例如目前在各个研究团队中广泛使用的Velodyne HDL-64E 激光雷达的售价就在10万美元左右，这使得成本成为搭载激光雷达的无人驾驶汽车的商用化的障碍。 作为比较，基于计算机视觉（Computer Version, CV）的感知成本相对低，结合视觉，IMU+GPS以及毫米波雷达的方案是目前已经商用驾驶辅助系统中采用的主流方案（例如特斯拉的Autopilot），计算机视觉是指通过图像来感知和理解我们的世界的科学。 对于无人驾驶而言，计算机视觉可以帮助确定车道线的位置，识别车辆，行人以及其他事物，以确保无人车的形式安全，基于深度学习的技术，我们甚至可以实现端到端的自动驾驶（输入图像，输出操作）。 此外，人类驾驶员在做感知的时候只需要一双眼睛（偶尔可能需要用耳朵去听声音），而不需要精确知道距离障碍物有多少厘米，障碍物长宽高分别是多少厘米等，所以我们可以相信——无人驾驶的最终形态或许就像人类驾驶一样仅仅需要视觉感知。 本节我们将使用传统的计算机视觉技术进行车道线的检测，同时基于检测出来的车道线计算车道线的曲率和车辆偏离车道线中心线的距离。在下一届中，我们将使用TensorFlow实现基于深度学习的车道线检测。 相机标定 相机标定（Camera Calibration） 通常是做计算机视觉的第一步，首先，为什么要做相机标定呢？ 因为我们通过相机镜头记录下的图像往往存在一定程度的失真，这种失真往往表现为图像畸变（Image Distortion） 。 畸变分为两类： 径向畸变（radial distortion）: 由于透镜的特性，光线容易在相机镜头的边缘出现较小或者较大幅度的弯曲，称之为径向畸变。 这种畸变在普通廉价的镜头中表现更加明显，径向畸变主要包括桶形畸变和枕形畸变两种。 以下分别是枕形和桶形畸变示意图： 2.切向畸变（tangential distortion）： 是由于透镜本身与相机传感器平面（成像平面）或图像平面不平行而产生的， 这种情况多是由于透镜被粘贴到镜头模组上的安装偏差导致。 畸变说明： 畸变（distortion） 是对直线投影（rectilinear projection）的一种偏移。 简单来说直线投影是场景内的一条直线投影到图片上也保持为一条直线。 那畸变简单来说就是一条直线投影到图片上不能保持为一条直线了，这是一种光学畸变（optical aberration）。 可能由于摄像机镜头的原因，这里不讨论，有兴趣的可以查阅光学畸变的相关的资料。 直接看公式难以理解，我们看如下的图像： 这就是为什么我们标定相机通常使用棋盘图像了，下面我们使用OpenCV来实现一下相机标定。 相机，棋盘图 首先从各个角度对这个棋盘拍照，得到一系列如下的照片： 一般来说，使用相机在各个角度拍摄20张左右的棋盘图即可完成后面的标定工作。 使用OpenCV找出棋盘的对角点 首先读取图像并转为灰度图： from __future__ import print_function import numpy as np import cv2 import matplotlib.pyplot as plt import pickle %matplotlib inline cal = plt.imread('calibration1.jpg') fig1 = plt.figure(1, figsize=(16, 9)) cal_gray = cv2.cvtColor(cal, cv2.COLOR_RGB2GRAY) plt.subplot(2,2,1) plt.imshow(cal) plt.subplot(2,2,2) plt.imshow(cal_gray, cmap='gray') 使用OpenCV的cv2.findChessboardCorners()函数找出棋盘图中的对角（即图片中黑白相对的点的坐标）, 同时使用cv2.drawChessboardCorners()将之画出来: ret, corners = cv2.findChessboardCorners(cal_gray, (9, 6),None) if ret == True: cal = cv2.drawChessboardCorners(cal, (9, 6), corners, ret) plt.imshow(cal) 在上图中我们查找出了这个棋盘图内 9×6的黑白对角在图片中的像素位置， 接着我们构造这些对角点在在现实世界中的相对位置，我们将这些位置简化成整数值， 比如说第二行的第1个点就表示为 [0,1,0] , 即第0列第1行。 objp = np.zeros((6*9, 3), np.float32) objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2) img_points = [] obj_points = [] img_points.append(corners) obj_points.append(objp) 对所有的的标定用的棋盘图像都进行上述操作求得对角的像素位置和实际相对位置（对于一个棋盘来说，实际相对位置其实恒定的），并将这些都添加到 img_points 和 obj_points 两个列表中。 最后我们使用OpenCV中的 cv2.calibrateCamera() 即可求得这个相机的畸变系数，在后面的所有图像的矫正都可以使用这一组系数来完成。 image_size = (cal.shape[1], cal.shape[0]) ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, image_size, None, None) 矫正一张图像我们使用 cv2.undistort() 方法来实现： # Read in a test image img = cv2.imread('calibration3.jpg') undist = cv2.undistort(img, mtx, dist, None, mtx) plt.subplot(2,2,1) plt.imshow(img) plt.subplot(2,2,2) plt.imshow(undist) 我们发现经过标定以后，相机拍出来的图像更接近于真实情况，因失真造成的”扭曲的直线”也被纠正过来。 确定ROI ROI(Region of interest) 即我们处理一个视觉任务时“感兴趣的区域”，当然不同的任务ROI是不一样的， 对于车道线检测而言（如下图），ROI就是车辆的前方的车道线区域： 我们可以通过透视变换来获得一个相对更加直观的视角（比如说在天空俯视的视角），然后在新的视角来圈出ROI。 透视变换(Perspective Transformation)是将图片投影到一个新的视平面(Viewing Plane)也称作投影映射(Projective Mapping) 。 在OpenCV中，通过使用函数cv2.getPerspectiveTransform()和 cv2.warpPerspective()即可完成对一张图片的透视变换。 透视变换基于给定的映射关系，通过对原图像进行扭曲，从而得到一个近似另一个视角的图像 首先我们读取并且矫正图像，矫正后的结果： 对直道进行透视变换： def perspective_transform(img, M): img_size = (img.shape[1], img.shape[0]) warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR) return warped # left_top to left_bottom, corners = [(603, 445), (677, 445), (1105, 720), (205, 720)] wrap_offset = 150 src_corners = [(603, 445), (677, 445), (1105, 720), (205, 720)] dst_corners = [(205 + wrap_offset, 0), (1105 - wrap_offset, 0), (1105 - wrap_offset, 720), (205 + wrap_offset, 720)] M = cv2.getPerspectiveTransform(np.float32(src_corners), np.float32(dst_corners)) wrap_img= perspective_transform(straight_lines1, M) subplot(1, 2, [straight_lines1, wrap_img]) 结果如图： 我们解读一下代码：cv2.getPerspectiveTransform() 需要两个参数 src 和 dst，他们分别为原图像中能够表示一个矩形的四个点的坐标以及扭曲以后图像的边缘四角在当前图像中的坐标，这两个矩形的坐标不同的相机的数值也不同，不如说实例中相机的分辨率为 1280×7201280×720 , 那么 [(603, 445), (677, 445), (1105, 720), (205, 720)] 在图像中构成一个梯形，这个梯形在俯视图（或者说鸟瞰图）中是一个长方形，然后我们以这个梯形的高作为目标图像的高，前后各减去一个偏移（在实例中这个偏移是150个像素），就是我们的目标图像这个目标图像，也即是我们的ROI。 下面我们就可以在这个鸟瞰图上来做道路线检测了，由于篇幅的原因，道路线检测算法以及完整代码都将在下一篇博客中给出， 参考： https://blog.csdn.net/AdamShan/article/details/78712120
无人驾驶汽车系统入门（七）——基于传统计算机视觉的车道线检测(2)
无人驾驶汽车系统入门（七）——基于传统计算机视觉的车道线检测(2) 说明： 介绍基于视觉的车道线识别 在获得ROI（透视变换）以后，我们对得到的“鸟瞰图”应用色彩阈值化和梯度阈值化，以得到鸟瞰图中“可能为”车道线的像素然后再使用滑动窗口来确定车道线的多项式系数，下面我们来具体讨论这两个步骤。 边缘检测 车道线的一个主要的特征就是它可以看作图像中的边缘（Edge）， 因此我们可以使用一定的 边缘检测(Edge detection) 算法将之从原图像中提取出来。 边缘检测的结果就是标识出了数字图像中亮度变化明显的点。 有许多用于边缘检测的方法，他们大致可分为两类：基于搜索和基于零交叉。 基于搜索的边缘检测方法首先计算边缘强度，通常用一阶导数表示，例如梯度模; 然后，用计算估计边缘的局部方向，通常采用梯度的方向，并利用此方向找到局部梯度模的最大值。 基于零交叉的方法找到由图像得到的二阶导数的零交叉点来定位边缘.通常用拉普拉斯算子或非线性微分方程的零交叉点。 import cv2 import numpy as np from matplotlib import pyplot as plt img = cv2.imread('test_images/shudu.jpeg', 0) laplacian = cv2.Laplacian(img, cv2.CV_64F) sobelx = cv2.Sobel(img, cv2.CV_64F, 1, 0, ksize=3) sobely = cv2.Sobel(img, cv2.CV_64F, 0, 1, ksize=3) plt.subplot(2, 2, 1), plt.imshow(img, cmap='gray') plt.title('Original'), plt.xticks([]), plt.yticks([]) plt.subplot(2, 2, 2), plt.imshow(laplacian, cmap='gray') plt.title('Laplacian'), plt.xticks([]), plt.yticks([]) plt.subplot(2, 2, 3), plt.imshow(sobelx, cmap='gray') plt.title('Sobel X'), plt.xticks([]), plt.yticks([]) plt.subplot(2, 2, 4), plt.imshow(sobely, cmap='gray') plt.title('Sobel Y'), plt.xticks([]), plt.yticks([]) plt.show() 检测的结果： 我们分别使用OpenCV实现以上的内容： def abs_sobel_thresh(img, sobel_kernel=3, orient='x', thresh=(0, 255)): gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY) if orient == 'x': sobel = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel) else: sobel = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel) abs_sobel = np.absolute(sobel) scaled_sobel = np.uint8(255 * abs_sobel / np.max(abs_sobel)) sxbinary = np.zeros_like(scaled_sobel) sxbinary[(scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1 return sxbinary def mag_thresh(img, sobel_kernel=3, mag_thresh=(0, 255)): # Convert to grayscale gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY) # Take both Sobel x and y gradients sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel) sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel) # Calculate the gradient magnitude gradmag = np.sqrt(sobelx**2 + sobely**2) # Rescale to 8 bit scale_factor = np.max(gradmag)/255 gradmag = (gradmag/scale_factor).astype(np.uint8) # Create a binary image of ones where threshold is met, zeros otherwise binary_output = np.zeros_like(gradmag) binary_output[(gradmag >= mag_thresh[0]) & (gradmag <= mag_thresh[1])] = 1 # Return the binary image return binary_output def dir_threshold(img, sobel_kernel=3, thresh=(0, np.pi/2)): # Grayscale gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY) # Calculate the x and y gradients sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel) sobely = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel) # Take the absolute value of the gradient direction, # apply a threshold, and create a binary image result absgraddir = np.arctan2(np.absolute(sobely), np.absolute(sobelx)) binary_output = np.zeros_like(absgraddir) binary_output[(absgraddir >= thresh[0]) & (absgraddir <= thresh[1])] = 1 # Return the binary image return binary_output 分别对上一篇文章中的鸟瞰图进行边缘检测： ksize = 9 # Choose a larger odd number to smooth gradient measurements # Apply each of the thresholding functions gradx = abs_sobel_thresh(wrap_img, orient='x', sobel_kernel=3, thresh=(20, 255)) mag_binary = mag_thresh(wrap_img, sobel_kernel=3, mag_thresh=(30, 100)) dir_binary = dir_threshold(wrap_img, sobel_kernel=15, thresh=(0.7, 1.3)) # Plot the result f, axs = plt.subplots(2, 2, figsize=(16, 9)) f.tight_layout() axs[0, 0].imshow(wrap_img) axs[0, 0].set_title('Original Image', fontsize=18) axs[0, 1].imshow(gradx, cmap='gray') axs[0, 1].set_title('Sobel_x_filter', fontsize=18) axs[1, 0].imshow(dir_binary, cmap='gray') axs[1, 0].set_title('Dir_threshold', fontsize=18) axs[1, 1].imshow(mag_binary, cmap='gray') axs[1, 1].set_title('Mag_threshold', fontsize=18) plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.) plt.show() 不难看出，x方向的索贝尔算子取得了最好的效果，同时，梯度方向也为车道线像素选取提供了一定的参考。 色彩阈值化 检测车道线的另一个依据就是车道线的颜色特征，一般来说，车道线只有两种颜色：白色和黄色，所以我们可以在 RGB 色彩空间(Color Space) 对这两种颜色进行过滤从而提取出车道线的像素。 色彩空间：使用一组值（通常使用三个、四个值或者颜色成分）表示颜色方法的抽象数学模型。有利用原色相混的比例表示的色彩空间，如 RGB (Red, Green, Blue) 颜色空间; 也有利用不同的概念表示的色彩空间,如 HSV (色相 hue, 饱和度 saturation, 明度 value) 以及 HSL (色相 hue，饱和度 saturation，亮度 lightness/luminance) 。 在OpenCV中，RGB三通道的图像的读取 cv2.imread() 的结果是以 BGR 顺序排列的，而在使用matplotlib的 plt.imread() 时， 读取的通道排列顺序则为 RGB 。因此此处应当注意区别。 RGB阈值化处理 我们在RGB色彩空间对黄色和白色进行过滤，得到参考的像素： def r_select(img, thresh=(200, 255)): R = img[:,:,0] binary = np.zeros_like(R) binary[(R > thresh[0]) & (R <= thresh[1])] = 1 return binary def color_mask(hsv,low,high): # Return mask from HSV mask = cv2.inRange(hsv, low, high) return mask def apply_color_mask(hsv,img,low,high): # Apply color mask to image mask = cv2.inRange(hsv, low, high) res = cv2.bitwise_and(img,img, mask= mask) return res def apply_yellow_white_mask(img): image_HSV = cv2.cvtColor(img,cv2.COLOR_RGB2HSV) yellow_hsv_low = np.array([ 0, 100, 100]) yellow_hsv_high = np.array([ 80, 255, 255]) white_hsv_low = np.array([ 0, 0, 160]) white_hsv_high = np.array([ 255, 80, 255]) mask_yellow = color_mask(image_HSV,yellow_hsv_low,yellow_hsv_high) mask_white = color_mask(image_HSV,white_hsv_low,white_hsv_high) mask_YW_image = cv2.bitwise_or(mask_yellow,mask_white) return mask_YW_image 测试： r_binary = r_select(wrap_img, thresh=(220, 255)) yw_binary = apply_yellow_white_mask(wrap_img) Plot the result f, axs = plt.subplots(1, 2, figsize=(16, 9)) f.tight_layout() axs[ 效果： 上面的代码中，R filter是在Red通道上设置合适的阈值，就能够对黄色和白色的导线进行过滤，而Yellow white filter则直接对三个通道设置一定的阈值，通过组合（and操作）来获得最优的过滤效果，RGB颜色阈值化虽然能够在正常光线条件下很好的过滤出道线像素，但是在复杂环境光的情况下性能并不稳定。下面我们进一步探索颜色阈值化，看看在 HLS 色彩空间的阈值化效果。 HLS 阈值化处理 下图是 HLS 色彩空间和 HSV 色彩空间的可视化比较。HLS和HSV都是一种将RGB色彩模型中的点在圆柱坐标系中的表示法。这两种表示法试图做到比RGB基于笛卡尔坐标系的几何结构更加直观。HLS即色相、亮度、饱和度。在本文中，我们重点探讨应用HLS的色彩阈值化于车道线检测。在OpenCV中，我可以直接使用 cv2.cvtColor() 方法将图像从RGB色彩空间转换到HLS色彩空间。 我们对HLS色彩空间的三个通道分别对车道线进行阈值化处理： def hls_select(img, channel='S', thresh=(90, 255)): # 1) Convert to HLS color space # 2) Apply a threshold to the S channel # 3) Return a binary image of threshold result hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS) if channel == 'S': X = hls[:, :, 2] elif channel == 'H': X = hls[:, :, 0] elif channel == 'L': X = hls[:, :, 1] else: print('illegal channel !!!') return binary_output = np.zeros_like(X) binary_output[(X > thresh[0]) & (X <= thresh[1])] = 1 return binary_output 对鸟瞰图测试阈值： l_binary = hls_select(wrap_img, channel='L', thresh=(100, 200)) s_binary = hls_select(wrap_img, channel='S', thresh=(100, 255)) h_binary = hls_select(wrap_img, channel='H', thresh=(100, 255)) f, axs = plt.subplots(2, 2, figsize=(16, 9)) f.tight_layout() axs[0, 0].imshow(wrap_img) axs[0, 0].set_title('Original Image', fontsize=18) axs[0, 1].imshow(h_binary, cmap='gray') axs[0, 1].set_title('H channal filter', fontsize=18) axs[1, 0].imshow(s_binary, cmap='gray') axs[1, 0].set_title('S channal filter', fontsize=18) axs[1, 1].imshow(l_binary, cmap='gray') axs[1, 1].set_title('L channal filter', fontsize=18) plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.) plt.show() 结果如下： 粗略看来，通过组合H和S的阈值化可以获得良好的检测效果。 组合梯度和色彩过滤车道线像素 最理想的自然是通过组合以上的方法来获得对车道线最理想的提取，这种组合方法有很多，没有一个固定的模式，通过组合梯度和色彩过滤器，可以获得一个相对稳定的车道线提取方法，最理想的状态就是我们的组合过滤器能够尽可能少受到环境光，道路背景色以及其他车辆的影响，再次我们给出一个参考的组合方法： def combine_filters(img): gradx = abs_sobel_thresh(img, orient='x', sobel_kernel=3, thresh=(20, 255)) l_binary = hls_select(img, channel='L', thresh=(100, 200)) s_binary = hls_select(img, channel='S', thresh=(100, 255)) yw_binary = apply_yellow_white_mask(wrap_img) yw_binary[(yw_binary !=0)] = 1 combined_lsx = np.zeros_like(gradx) combined_lsx[((l_binary == 1) & (s_binary == 1) | (gradx == 1) | (yw_binary == 1))] = 1 return combined_lsx binary = combine_filters(wrap_img) f, axs = plt.subplots(1, 2, figsize=(16, 9)) f.tight_layout() axs[0].imshow(wrap_img) axs[0].set_title('Original', fontsize=18) axs[1].imshow(binary, cmap='gray') axs[1].set_title('combine filters', fontsize=18) plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.) plt.show() 在提取完车道线像素以后，我们用两条曲线来拟合这些像素，在做拟合这一步操作之前，我们首先得确定哪些像素是车道线的，哪些不是，这里我们使用滑动窗口的方法。 滑动窗口与多项式拟合 滑动窗口的代码如下： def find_line_fit(img, nwindows=9, margin=100, minpix=50): histogram = np.sum(img[img.shape[0]//2:,:], axis=0) # Create an output image to draw on and visualize the result out_img = np.dstack((img, img, img)) * 255 # Find the peak of the left and right halves of the histogram # These will be the starting point for the left and right lines midpoint = np.int(histogram.shape[0]/2) leftx_base = np.argmax(histogram[:midpoint]) rightx_base = np.argmax(histogram[midpoint:]) + midpoint # Set height of windows window_height = np.int(img.shape[0]/nwindows) # Identify the x and y positions of all nonzero pixels in the image nonzero = img.nonzero() nonzeroy = np.array(nonzero[0]) nonzerox = np.array(nonzero[1]) # Current positions to be updated for each window leftx_current = leftx_base rightx_current = rightx_base # Create empty lists to receive left and right lane pixel indices left_lane_inds = [] right_lane_inds = [] # Step through the windows one by one for window in range(nwindows): # Identify window boundaries in x and y (and right and left) win_y_low = img.shape[0] - (window+1)*window_height win_y_high = img.shape[0] - window*window_height win_xleft_low = leftx_current - margin win_xleft_high = leftx_current + margin win_xright_low = rightx_current - margin win_xright_high = rightx_current + margin # Draw the windows on the visualization image cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high), (0,255,0), 2) cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high), (0,255,0), 2) # Identify the nonzero pixels in x and y within the window good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xleft_low) & (nonzerox < win_xleft_high)).nonzero()[0] good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_xright_low) & (nonzerox < win_xright_high)).nonzero()[0] # Append these indices to the lists left_lane_inds.append(good_left_inds) right_lane_inds.append(good_right_inds) # If you found > minpix pixels, recenter next window on their mean position if len(good_left_inds) > minpix: leftx_current = np.int(np.mean(nonzerox[good_left_inds])) if len(good_right_inds) > minpix: rightx_current = np.int(np.mean(nonzerox[good_right_inds])) # Concatenate the arrays of indices left_lane_inds = np.concatenate(left_lane_inds) right_lane_inds = np.concatenate(right_lane_inds) # Extract left and right line pixel positions leftx = nonzerox[left_lane_inds] lefty = nonzeroy[left_lane_inds] rightx = nonzerox[right_lane_inds] righty = nonzeroy[right_lane_inds] # to plot out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0] out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255] # Fit a second order polynomial to each left_fit = np.polyfit(lefty, leftx, 2) right_fit = np.polyfit(righty, rightx, 2) return left_fit, right_fit, out_img 生成图： # Generate x and y values for plotting def get_fit_xy(img, left_fit, right_fit): ploty = np.linspace(0, img.shape[0]-1, img.shape[0]) left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2] right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2] return left_fitx, right_fitx, ploty left_fit, right_fit, out_img = find_line_fit(binary) left_fitx, right_fitx, ploty = get_fit_xy(binary, left_fit, right_fit) fig = plt.figure(figsize=(16, 9)) plt.imshow(out_img) plt.plot(left_fitx, ploty, color='white', linewidth=3.0) plt.plot(right_fitx, ploty, color='white', linewidth=3.0) plt.xlim(0, 1280) plt.ylim(720, 0) plt.show() 结果： 具体来说，为了确定哪些像素属于车道线，首先确定左右两条车道线的大致位置，这一步非常简单，只需要将图片中的像素沿y轴累加，找出图片中间点左右的峰值，即为车道线可能的区域，然后自底向上使用滑动窗口，计算窗口内的不为0的像素点，如果像素点的数量大于某个阈值，那么就以这些点的均值作为下一个滑动窗口的中心。 得到候选的像素以后，使用numpy中的 np.polyfit()方法来拟合这些点，我们使用一个二次多项式来拟合。 拟合的曲线是x关于y的多项式表述，即自变量是y,因变量是x。 还原至原视角 接着在将拟合的曲线使用透视变换还原到原视角： def project_back(wrap_img, origin_img, left_fitx, right_fitx, ploty, M): warp_zero = np.zeros_like(wrap_img).astype(np.uint8) color_warp = np.dstack((warp_zero, warp_zero, warp_zero)) # Recast the x and y points into usable format for cv2.fillPoly() pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))]) pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))]) pts = np.hstack((pts_left, pts_right)) # Draw the lane onto the warped blank image cv2.fillPoly(color_warp, np.int_([pts]), (0, 0, 255)) # Warp the blank back to original image space using inverse perspective matrix (Minv) newwarp = perspective_transform(color_warp, M) # Combine the result with the original image result = cv2.addWeighted(origin_img, 1, newwarp, 0.3, 0) return result M = cv2.getPerspectiveTransform(np.float32(dst_corners), np.float32(src_corners)) result = project_back(binary, test_img, left_fitx, right_fitx, ploty, M) fig = plt.figure(figsize=(16, 9)) plt.imshow(result) 效果： 后记：在得到鸟瞰图的拟合曲线以后，我们是可以根据该曲线来计算车道的曲率的，同时如果知道车辆和相机的相对位置，还可以计算车辆距离左右车道线的距离，这就是目前基于视觉的车道偏离预警的实现方法之一，读者可以根据需要自行实现。 完整代码见：http://download.csdn.net/download/adamshan/10148446