Merge branch 'master' of https://github.com/dstarer/Robotics

Author: dstarer

README.md

@@ -43,7 +43,7 @@
 
 [pure-pursuit]
 
-[stanley]
+[stanley](doc/lane detection and following.md)
 
 **车辆运动学模型介绍**
 
@@ -81,11 +81,13 @@
 
 [LOAM]
 
-**autoware**
+**Autoware**
 
-[基于NDT的激光雷达SLAM]
+[基于NDT的激光雷达SLAM代码分析]
 
 [点云分割与聚类]
 
-[apollo]
+[点云目标跟踪方法讨论]
+
+**Apollo**
 

doc/PointCloudProcess/Normal Estimation.md

@@ -0,0 +1,75 @@
+# 平面法线估计
+
+法线估计是点云处理中比较重要的一个步骤。这里主要介绍如何计算法线以及其推导过程。
+
+三维中平面经常可以表达成如下形式：
+$$
+ax + by + cz + d = 0
+$$
+用向量的方式表达：
+$$
+\mathbb w^T \mathbb x = -d
+$$
+其中，$\mathbb w$是平面的法向量。
+
+### 问题定义
+
+给定平面点集$X={\mathbb x_1, \cdots, \mathbb x_n}$,现在要求解法向量 $\mathbb w $ 。
+
+根据点到平面的距离公式（可以由向量点积计算得到）：
+$$
+f(\mathbb x_i, \mathbb w) = \frac {\mathbb w^T\mathbb x_i + d} {\| \mathbb w \|_2^2}
+$$
+为了方便计算，我们假设$\mathbb w$是单位向量，也即是$\mathbb w^T \mathbb w = I$
+
+所以，我们可以将式(3)改写为：
+$$
+f(\mathbb x_i, \mathbb w, d) = \mathbb w^T\mathbb x_i + d \\
+s.t. \mathbb w^T\mathbb w = I
+$$
+由于点都在一个平面上，所以使得距离最小，即使得$\| f\|$最小，也即是：
+$$
+min\ F(\mathbb w, d) = \frac{1}{n} \Sigma_{i = 1} ^ n \| f(\mathbb x_i, \mathbb w, d) \| \\
+s.t. \mathbb w^T \mathbb w = I
+$$
+由于直接求解$\| f\|$比较困难，这里使用$f^T f$代替$\| f\|$，所以也即是：
+$$
+min\ F(\mathbb w, d) = \frac{1}{n} \Sigma_{i=1}^n f^T(\mathbb x_i, \mathbb w, d) f(\mathbb x_i, \mathbb w, d) \\
+s.t. \mathbb w^T \mathbb w = I
+$$
+使用拉格朗日算子去掉约束条件有：
+$$
+F(\mathbb w, d) = \frac{1}{n} \Sigma_{i=1}^n f^T(\mathbb x_i, \mathbb w, d) f(\mathbb x_i, \mathbb w, d) + \lambda  \mathbb w^T \mathbb w  \\
+= \frac{1}{n} \Sigma_{i=1}^n (\mathbb x_i^T \mathbb w \mathbb w^T \mathbb x_i + 2d\mathbb w^T\mathbb x_i + d^2) + \lambda \mathbb w^T \mathbb w \\
+= \frac{1}{n} \Sigma_{i=1}^n (\mathbb w^T \mathbb x_i \mathbb x_i^T \mathbb w  + 2d\mathbb w^T\mathbb x_i + d^2) + \lambda \mathbb w^T \mathbb w
+$$
+求导有:
+$$
+\frac {\nabla F} {\nabla \mathbb w} = \frac{1}{n} \Sigma_{i=1}^{n} (2 \mathbb x_i \mathbb x_i^T \mathbb w + 2d \mathbb x_i) + 2\lambda \mathbb w \\
+= 2(\frac{1}{n}\Sigma_{i=1}^n \mathbb x_i \mathbb x_i^T) \mathbb w + 2(\frac{1}{n}\Sigma_{i=1}^n \mathbb x_i)d + 2\lambda \mathbb w
+$$
+
+$$
+\frac {\nabla F} {\nabla d} =\frac{1}{n} \Sigma_{i=1}^n 2\mathbb w^T \mathbb x_i + 2d \\
+= 2 \mathbb w^T (\frac{1}{n} \Sigma_{i=1}^n \mathbb x_i) + 2d
+$$
+
+我们令：
+$$
+\mathbb u = \frac{1}{n} \Sigma_{i=1}^n \mathbb x_i \\
+P = \frac{1}{n} \Sigma_{i=1}^n \mathbb x_i \mathbb x_i^T
+$$
+令式(9)为0，有：
+$$
+d = - \mathbb w^T \mathbb u = - \mathbb u^T \mathbb w
+$$
+令式(8)为0，有：
+$$
+P\mathbb w - \mathbb u \mathbb u^T \mathbb w + \lambda \mathbb w = 0 \\
+(P-\mathbb u \mathbb u^T) \mathbb w = \lambda \mathbb w
+$$
+式(12)是求矩阵的特征值和特征方程。在计算前均值化，P则是协方差矩阵。
+
+**注意**式(12)实质上就是PCA分解，所以这里得到的$\mathbb w$只是一组平面的一组基。对这组基做平面叉乘，就得到了平面的法向量。
+
+**Important**: 假设有特征值$\sigma_0, \sigma_1, \sigma_2$，我们不妨假设$\sigma_0 <= \sigma_1 <= \sigma_2$，则，$\sigma_0​$对应的向量就是法向量。

doc/lane detection and following.md

@@ -0,0 +1,150 @@
+
+
+# 车道线检测及循线行驶方案
+
+## 循线行驶控制
+
+### 观测量与控制量
+
+*视觉循轨迹的控制方法中，常用的控制量主要有两个量，横向距离偏差和航向角偏差。*
+
+假设当前车辆的状态$S(t) = \begin{pmatrix} x_t , y_t, \theta_t, v_t \end{pmatrix}$，其中$x_t, y_t$是车辆的位置，$\theta_t$是车辆的航向角，$v_t$是车辆的速度。设$c(cx, cy, \theta_c)$是参考轨迹上距离当前车辆位置最近的一点，则有：​		
+
+![stanley](/home/yunle/2018-10-24 13-45-38.png)
+
+​				图1. 车辆位置与参考轨迹观测量及控制量示意图。
+
+1. 横向距离偏差(eod/ $e_{fa}$/$e_d$)：
+   $$
+   eod = e_{fa} = e_d = \sqrt{(cx- x_t)^2 + (cy-y_t)^2}
+   $$
+
+2. 车辆航向角偏差(eoa/$\theta_e$):
+   $$
+   eoa=\theta_e = \theta_c - \theta_t
+   $$
+
+
+
+### 控制方法 Stanley method
+
+$$
+\delta(t) = \theta_e(t) + tan^{-1}(\frac{ke_{fa}(t)} {v(t)})
+$$
+
+其中，$k$是调节系数。$\delta(t)$由两个部分决定，一个是当前的航向偏差，一个是横向距离。距离越大，调节速度越快。
+
+低速情况下，车辆运动假设按照自行车模型，不考虑侧偏滑动，车辆状态的增量方程$\dot{S(t)}$有：
+$$
+\begin{bmatrix} \dot{x_t} \\ \dot{y_t} \\ \dot{\theta_t} \\ \dot{\delta_t}  \end{bmatrix} = \begin{bmatrix} cos(\theta_t) \\ sin(\theta_t) \\ \frac{tan(\delta_t)} {L}  \\ 0 \end{bmatrix} v_t \quad + \quad \begin{bmatrix} \\0 \\0 \\0 \\1 \end{bmatrix}  \dot \delta
+$$
+其中$S(t+1) = S(t) + \dot{S(t)} \Delta t $。
+
+基于状态方程，并且做近似假设(方向变动非常小)，可以推算出$eod, eoa$的变化量：
+$$
+\begin{bmatrix} 
+    \dot{e_d(t)} \\ \dot{\theta_e(t)} \\ \dot{\delta(t)}  
+\end{bmatrix} = 
+\begin{bmatrix} 
+	0 \quad  v_t \quad 0 \\  
+	0 \quad 0 \quad -\frac{v_t}{L} \\
+	0 \quad 0 \quad 0
+\end{bmatrix} 
+\begin{bmatrix}
+e_d(t) \\ \theta_e(t) \\ \delta_t(t)
+\end{bmatrix} \quad + \quad
+\begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} \dot{\delta(t)}
+$$
+所以可以根据控制量计算出下一时刻的偏差量。
+
+快速推算，如果按照控制周期0.2s算，速度在2m/s，距离只跟上一时刻的航向角偏差有关，所以，单个周期内，变化量不超过，$0.4\theta_e(t)$，考虑到误差控制在5度以内，实际距离变化不超过4cm。航向角的增量只跟当前输入的$\delta(t)$有关，可以估算得到，实际方差变化，不超过$\delta(t)$。
+
+所以，我们可以确定基于Stanley方法，误差变化主要取决于速度和控制角度，控制角度越小，则实际变化量越小，则控制效果非常平滑，并且不存在明显抖动。
+
+### 视觉循车道中的应用
+
+主要问题：
+
+​	**如何寻找参考轨迹，并计算横向距离误差和航向角偏差？**
+
+#### 参考轨迹计算
+
+由于车辆一开始是保持在车道内，可以基于车道线检测，计算车道中心线作为实际的参考轨迹。
+
+*坐标定义：相机坐标系 x是图片的宽，y是图片的高度，通常照片底部y取值为0。 车体坐标系： x指向右，y指向前*
+
+*相机坐标系到车体坐标系的变换： 相机坐标中此处是以俯视图做计算，所以只需要考虑像素与真实距离之间的变换关系即可，即横向一个像素代表的距离和纵向一个像素代表的距离*
+
+1. 对检测得到的每一条车道线，分别做多项式拟合(通常是二阶或者是三阶多项式)，主要是平滑车道线，可以用于计算车道线曲率等；
+
+2. 根据计算得到的两条曲线，采样合成车道中心线
+
+   假设左车道线上一点($y_i$, $x_{i}^l$)， 右车道线上一点($y_i$, $x_i^r$)，中心线上一点为$(y_i, \frac{ (x_i^l + x_i^r)} {2})$，由采样的n个点，拟合得到车道中心线。
+
+3. 将车道中心线变换到机体坐标系上考虑(以图片的底部中点做为原点，然后乘以距离与像素的比值)。
+
+*注：计算得到的车道中心线的计算过程一定是在俯视图下计算得到*
+
+#### 横向距离误差和航向角偏差计算
+
+在俯视图下，摄像头所在位置定义为原点，简易计算，$y=0$时，中心线上的点$p(x_0, 0)$作为最近点，则有:
+$$
+e_d = \| x_0\| \\
+\theta_e = atan(dp) - \frac{\pi} {2}
+$$
+其中$dp$是$p$的导数。
+
+**关键**：计算高精度的航向偏差值，距离偏差值，换言之，需要低方差数据。
+
+### 实验结果
+
+此处主要计算横向距离(eod)和航向角偏差(eoa)的计算误差分布。对于这两个值主要关心其方差分布，不考虑其均值，只要方差分布比较小，其精度才会比较高。
+
+**实验测量条件**
+
+仿真环境下，对同一直线路段测试，该部分路段，不下发任何角度和速度控制指令(保证视觉检测部分没有明显变化)，测试次数12次。
+
+![横向误差对比](/home/yunle/Documents/eoa_groups_50.png)
+
+对比12次实验数据，航向角的均值和误差值，均值单位是度，可以看出由于未做其他控制，度数应该比较稳定，但是检测得到的结果上来看，误差和均值变化都比较大。
+
+![航向角序列对比](/home/yunle/Documents/eoa_seq.png)
+
+对每一组数据，分别对比，由于未控制方向，所以计算得到的方向叫应该比较一致，并且方差应当比较小，这里可以看出得到的方差比较大，也就是检测得到的航向偏差值不稳定；
+
+![距离偏差](/home/yunle/Documents/eod_groups_50.png)
+这里是对比12次实验过程的横向误差值，误差值平均值比较小，但是方差变化浮动比较大，也就是检测值在距离上也表现出不稳定；
+
+**具体几组数据的分析**
+
+**Todo 这里弧度转成度表示**
+
+| 时间(ms)      | eoa（度） | eod(m)         | $\delta$(度) (k=0.1, v=2) |
+| ------------- | --------- | -------------- | ------------------------- |
+| 1540350213277 | 2.0626    | 0.271179335178 | 2.84                      |
+| 1540350214826 | -1.872    | 0.186094065342 | -1.34                     |
+| 1540350214996 | -1.9175   | 0.174853182086 | -1.42                     |
+| 1540350215243 | -2.7617   | 0.10903390798  | -2.45                     |
+
+这是具体某一次开始的检测，对比法线第一次检测和第二次检测，图片之间的事件差为1.6s左右，角度值变化了3度左右，但是这个过程中，并没有控制，也即是检测的偏差值比较大。
+
+| 时间(ms)      | EOA(度) | EOD(M)         | $\delta$（度）(k=0.1, v=2) |
+| ------------- | ------- | -------------- | -------------------------- |
+| 1540350370573 | -1.1224 | 0.134357696775 | -0.74                      |
+| 1540350372006 | -0.1231 | 0.19561673033  | 0.44                       |
+| 1540350372296 | -1.0118 | 0.188130211378 | -0.47                      |
+| 1540350372422 | -1.9985 | 0.117381406227 | -1.66                      |
+| 1540350372855 | 0.0016  | 0.23715206907  | 0.68                       |
+| 1540350373055 | -2.7788 | 0.187747556826 | -2.24                      |
+
+同样在最后几帧，检测得到的弧度制变化非常大，基本转了2.8度。
+
+### 结论
+
+控制方法上不存在问题，主要原因在检测结果的不稳定上。
+
+针对检测结果的不稳定，有两种解决方式，
+
+1. 改进检测算法，目前检测算法上还有大幅度可以改进的点，从检测结果的稳定性上来考虑。
+2. 基于检测结果以及误差的增量模型做卡尔曼滤波融合，平滑检测结果。但是需要解决两个问题，一个是初值的设定(设置为0是否可靠？不同场景衔接过程中如何保证初值的可靠性)，一个是方差的衡量，目前可以通过多次实验设定一个值。**除此之外，应用卡尔曼滤波，还有一个重要的问题，基于模型推算是存在误差的，误差会累计下来，所以必须要提供一种方式能够纠正推算误差，这种纠正误差来源于检测结果，也就是说检测结果的误差必须是高精的(低方差)**
+

src/PathTracking/lqr_steer_control.py

@@ -210,13 +210,15 @@ def control(cx, cy, cyaw, ck, speed_profile, goal, show_animation=True):
 def main():
     show_animation = True
     print("LQR steering control tracking start!!")
-    ax = [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0]
-    ay = [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0]
+    # ax = [0.0, 6.0, 12.5, 10.0, 7.5, 3.0, -1.0]
+    # ay = [0.0, -3.0, -5.0, 6.5, 3.0, 5.0, -2.0]
+    ax = [0.0, 100.0, 100.0, 50.0, 60.0]
+    ay = [0.0, 0.0, -30.0, -20.0, 0.0]
     goal = [ax[-1], ay[-1]]
 
     cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course(
         ax, ay, ds=1.0)
-    target_speed = 20.0 / 3.6  # simulation parameter km/h -> m/s
+    target_speed = 30.0 / 3.6  # simulation parameter km/h -> m/s
 
     sp = calc_speed_profile(cx, cy, cyaw, target_speed)
 

src/PathTracking/stanley_controller.py

@@ -64,6 +64,7 @@ def latitude_control(state, cx, cy, cyaw, last_target_idx, k=1.0):
     theta_d = np.arctan2(k * error_front_axle, state.v)
 
     delta = theta_e + theta_d
+    print "error_front_axle ", error_front_axle, " theta_e ", theta_e, " delta ", delta
     return delta, current_idx
 
 
@@ -95,7 +96,7 @@ def main():
     max_simulation_time = 100.0
 
     # Initial state
-    state = State(x=-0.0, y=5.0, yaw=np.radians(20.0), v=0.0)
+    state = State(x=-0.0, y=5.0, yaw=np.radians(0.0), v=2.0)
 
     last_idx = len(cx) - 1
     time = 0.0