倒车轨迹线的实现方法（基于前人基础改进和验证）

背景：

网上对于倒车轨迹的大多数原创和转发的博客应该都基于同一篇，其他人没有进一步思考就对其进行转载和借鉴，其中原理的说明基本也是原版照抄，我看了一下，对于有些地方还是应该存疑，这些地方其他转发和借鉴的朋友也没有修改。

这是我能找到的最早的，借鉴最多的对于倒车轨迹研究的博客，我接下来的分析也是根据这篇博客，有些地方就稍微带过，主要对其中介绍不足的地方进行介绍，请参照原博客一起看。

原理

1.俯瞰监督倒车轨迹形成原理

先看上面示意图，这是俯瞰图，我们从二维平面来分析一下，不管是车辆前进还是后退，我们对其后轮进行追踪，我们通过生活经验可以发现，如果在某一时刻的方向盘角度不变，一直前进或者后退，车轮将会在地面画出圆形的轨迹，我们现在摘取后轮的轨迹来研究；
先声明要使用的参数：

• φ： 前轮同水平方向的夹角
• L：前后轮轴距
• w：后轮轴长
• D：后轮距离车尾的距离

从几何关系可知，后轮轴心的运动轨迹可以描述为以半径Lcot(φ)的圆周运动。两个后轮的轨迹分别为Lcot(φ)-W/2和Lcot(φ)+W/2的圆。从等式可以看到，当φ接近0度时候行进轨迹近似直线，接近90度时半径呈缩小趋势，符合我们日常经验值。

坐标转换

我们都知道单位圆的几何方程为：
$X^{2}+Y^{2} =R^{2}（R为圆半径）$
那么在这里我们的后轮画出的圆的轨迹方程为：(以外侧轮为例)
$X^{2}+Y^{2}=(Lcot(φ)-\frac{w}{2})^{2}$
注意：上面的方程建立在以后轮轨迹圆心为坐标原点的，所以我们需要将坐标原点建立在摄像头所在位置，如上图1所示，此时我们的方程就变为：

$（X+Lcot(φ)）^{2}+（Y+D）^{2}=(Lcot(φ)+\frac{w}{2})^{2} ①$

同理：我们的内侧后轮轨迹方程为：

$（X+Lcot(φ)）^{2}+（Y+D）^{2}=(Lcot(φ)-\frac{w}{2})^{2}②$
但是，从实际出发。从车尾摄像头看车尾部的景象是有角度的。我们还需要对其进行角度的转化

角度转换

上面我们推算出的轨迹是俯瞰的结果，但是驾车人员看屏幕中从摄像头上传的倒车景象是带有角度（并不是俯瞰）。我们现在通过数学模型简化一下视图来源的流程：这里如果摄像头相对于屏幕为一个点，会造成实际计算结果的一定偏差。关于偏差的细节数学计算不属本文讨论的重点
先声明要使用的参数：

• 2α： camera的张角的二分之一（ 假设camera的张角为2α）
• h： 摄像头距离地面
• β：摄像头中心线同水平面的夹角
• H: 输出屏幕的高度
• W： 输出屏幕的宽度

我们实际观察到的Yr为地面y在显示屏H上的投影，y方向的转换过程如图二：
上图简化camera图像采集和显示到屏幕的过程：现在完全可以理解为显示屏上的景象是实际远处景象在中间的投影。
上图中的虚线为水平线，点线为屏幕的中心点到摄像头的连线，β为中心线和水平线的夹角
将上图进行简化可转为下列数学问题，等腰三角形中同顶点成已知角度所相应的边长，简化后的计算如图三。
通过正弦定理、几何运算可得：(这里原博客把下面③④式子中的*写成x,现改正过来)
$Yr = \frac{\sin\theta _{1}}{\sin(\theta _{1}+\theta _{2})}*\frac{H}{2\sin\alpha } ③$
其中 $\theta _{1} =\alpha+\beta -\theta$
$\theta_{2} =90^o-\alpha$
代入③式可得：

$Yr = \frac{\sin(\alpha+\beta-\arctan (\frac{h}{y}))}{\cos( \beta-\arctan (\frac{h}{y}) )}* \frac{H}{2\sin\alpha } ④$

扫描二维码关注公众号，回复： 8632698 查看本文章

sh上面等式要求满足以下条件：表现为对车尾局部区域不可见，即盲区如下图
$y> h*cot(α+β)$

从上面公式可以看出来，当β越小，当β = 0；y趋向无穷远，上面等式约等于Yr = H/2;即最远距离图像在频幕上的显示高度不会超过屏幕高度一半；现在从实际出发，倒车时对camera可视距离的需求只是车尾3-5米，所以β 的角度一定存在，并且不会太小。

Xr对应屏幕宽度方向上的位置：(这里原博客求Xr时误差太大，已改经进)

在求Xr时，我们可以想像camera是可以转动的：

• 当camera垂直地面时：
  
  根据上图建立的模型，我们可以计算出当camera垂直面向地面时的影像关系为：
  $Xr =\frac{x}{h\tan \alpha} *\frac{W}{2} ⑤（x为地面X方向距离）$

• 当camera向上稍微转动，如上图二所示：即camera面向车尾后下方（具体车型安装角度不一）：（这个图就不画了，根据已有的图发散想象一下）
  此时⑤式就不能满足；但是我们仍然可以从中发现规律，现在参考（camera垂直图）当camera角度转动后，w/2不变，α也不变，x在计算中是变量，只有h在变，并且h->h`是可以找到具体量化的关系的
  $h^{2}+y^{2} =（h^{'}）^{2} （y为地面Y方向距离）$
  所以：
  $Xr =\frac{x}{h^{'}\tan \alpha} *\frac{W}{2}$
  最后得：

$Xr =\frac{x}{\sqrt{h^{2}+y^{2}}\tan \alpha} *\frac{W}{2} ⑥$

计算轨迹的条件

从以上计算我们可以看到，需要计算出轨迹，必须提供以下参数：

1 摄像头的可视角范围2α

2 摄像头距离地面距离h

3摄像头中心线同水平面的夹角β

4输出屏幕的高度H和宽度W

5 汽车前后轮轴距L

6 汽车轴长w，后轮距离车尾的距离D

7前轮同水平方向的夹角φ

以上参数中除转角φ外对于固定的车型和安装方式都已经固定，转角φ的获取可以有两种方式：1 对于有方向盘角度信息的车型可以直接通过输出接口如can总线获取。2外加角度传感器获取角度信息。两种方式都需要对采集的信息作一定的校正。

下面是我自己在Android上验证结果：

注意
因为测试时众多参数都是我自己未经证实就设定的，导致整体看倒车轨迹线变化幅度有点浮夸（还未叠加倒车录像），轨迹线远小近大，符合生活经验，误差未具体计算，不过可以证实这个思路是正确的。

参考博客：https://blog.csdn.net/shuaiff/article/details/5411280
参考资料：［1］ 张永亮 智能可视倒车系统 武汉科技大学硕士学位论文3.1倒车轨迹数学模型