CN104569992A - 汽车浅水行驶避障方法、***及装置 - Google Patents

Abstract

本申请提供一种汽车浅水行驶避障的方法、***以及装置，其中方法包括：检测水下的障碍物相对所述汽车的初始位置；实时检测所述汽车的方位信息，结合所述障碍物相对所述汽车的初始位置，计算所述障碍物相对所述汽车的实时位置；根据所述障碍物相对所述汽车的实时位置，在所述汽车按照当前行驶方向会与所述障碍物相撞时，精确显示出障碍物位置。本发明提供的汽车浅水行驶避障的方法、***以及装置，能够在汽车行驶的过程中对于隐藏于水下的障碍物实现有效地检测和实时定位跟踪并区别的显示障碍物所处位置，汽车行驶时能够有针对性的避让该障碍物。进而避免汽车因为发生碰撞带来的损失。

Description

汽车浅水行驶避障方法、***及装置

技术领域

本发明属于水下测量领域，涉及一种汽车浅水行驶避障的方法、***以及装置。

背景技术

在暴雨天，路面会发生大面积积水，遇到行驶路径上的大面积积水，有时无法避免的汽车需要涉水行驶。如图1所示，当汽车100涉水行驶的时候，行驶路线上会存在障碍物101隐藏于水下，现有技术中，常用的汽车行驶装置不能够对隐藏于水下的障碍物进行有有效的检测，此时车辆如果无法自主避让，会导致车辆损伤，财产损失。

发明内容

本发明提供一种汽车浅水行驶避障的方法、***以及装置，能够使得车辆在浅水行驶时对障碍物实行有效避让。

为实现上述技术效果，本发明的方案为：

一种汽车浅水行驶避障的方法，所述方法包括：

检测水下的障碍物相对所述汽车的初始位置；

实时检测所述汽车的方位信息，结合所述障碍物相对所述汽车的初始位置，计算所述障碍物相对所述汽车的实时位置；

根据所述障碍物相对所述汽车的实时位置，在所述汽车按照当前行驶方向会与所述障碍物相撞时，精确显示出障碍物位置。

优选地，所述检测水下的障碍物相对所述汽车的初始位置包括：

测量所述障碍物与第一标志位的距离，测量所述障碍物与第二标志位的距离，所述第一标志位与第二标志位在汽车的固定位置；

当所述障碍物与第一标志位的距离和所述障碍物与第二标志位的距离均大于预订阈值时，根据所述障碍物与第一标志位的距离、所述障碍物与第二 标志位的距离以及所述第一标志位与所述第二标志位间的距离，按照下述计算所述障碍物相对所述汽车的初始位置：

以所述第一标志位与第二标志位中点为原点，所述第一标志位与第二标志位连线方向为纵轴，所述第一标志位与第二标志位连线方向垂直方向为横轴，建立初始坐标系，所述障碍物对应所述初始坐标系的坐标(P_X,P_Y)为：

$(\frac{1}{2}\sqrt{(2{\mathrm{MP}}^2-{\mathrm{MN}}^2+2{\mathrm{NP}}^2)-\frac{{({\mathrm{NP}}^2-{\mathrm{MP}}^2)}^2}{{\mathrm{MN}}^2}},\frac{{\mathrm{NP}}^2-{\mathrm{MP}}^2}{2\mathrm{MN}})$

其中P_X为障碍物对应所述初始坐标系的横坐标，P_Y为障碍物对应所述初始坐标系的纵坐标，MP为所述障碍物与第一标志位的距离，NP为障碍物与第二标志位的距离，MN所述第一标志位与所述第二标志位的距离。

优选地，所述测量所述障碍物与第一标志位的距离包括：

记录从所述第一标志位发射超声波信号到在所述第一标识位接收到超声波信号在遇到所述障碍物后的回波信号的第一时间间隔，所述障碍物与第一标志位的距离按照下述公式计算：

MP＝C×t₁/2；

所述测量所述障碍物与第二标志位的距离包括：

记录从所述第二标志位发射超声波信号到在所述第二标识位接收到超声波信号在遇到所述障碍物后的回波信号的第二时间间隔，所述障碍物与第二标志位的距离按照下述公式计算：

NP＝C×t₂/2；

其中，t₁为第一时间间隔，t₂为第二时间间隔，C为超声波在水中的传播速度，按照下述公式计算：

C＝1400+4.6*T(m/s)，其中T为当前环境温度。

优选地，所述实时检测所述汽车的方位信息包括：

实时检测所述汽车对应所述初始坐标系的汽车位置坐标和所述汽车的实时行驶方向。

所述结合所述障碍物相对所述汽车的初始位置，对所述障碍物进行实时定位跟踪包括：

结合所述汽车位置坐标和所述实时行驶方向，按照下述方法计算障碍物相对实时坐标系的位置坐标：

以汽车当前位置上所述第一标志位与第二标志位中点为原点，所述第一标志位与第二标志位连线方向为纵轴，所述第一标志位与第二标志位连线方向垂直方向为横轴，建立所述实时坐标系，得到所述障碍物对应所述实时坐标系的实时位置坐标(P"_X,P"_Y)为：

(O"P×cosδ,O"P×sin(π+δ))

其中P"_X为障碍物对应实时坐标系的横坐标，P"_Y为障碍物对应实时坐标系的纵坐标，O"P为障碍物相对所述实时坐标系原点的距离，根据所述汽车位置坐标计算得到，δ为障碍物相对所述实时坐标系纵轴方向的夹角，根据所述汽车位置坐标和所述实时行驶方向计算得到。

优选地，所述根据所述对所述障碍物的实时定位跟踪，在所述汽车按照当前行驶方向会与所述障碍物相撞时，精确显示出障碍物位置包括：

当所述实时坐标处于可碰撞区域内，将障碍物的精确位置显示于显示设备的碰撞区域内。

优选地，所述当所述实时坐标处于可碰撞区域内，将障碍物的精确位置显示于显示设备的碰撞区域内还包括：

当所述实时位置坐标处于可探测区间内，显示设备高亮显示障碍物所在的可探测区域。

一种汽车浅水行驶避障***，所述***包括：障碍物检测模块，障碍物跟踪模块，障碍物显示模块

所述障碍物检测模块：检测水下的障碍物相对所述汽车的位置；

所述障碍物跟踪模块：用于检测所述汽车的方位信息，结合所述障碍物检测模块检测的障碍物相对所述汽车的位置，计算所述障碍物相对所述汽车的实时位置。

所述障碍物显示模块：根据所述障碍物跟踪模块计算的所述障碍物相对所述汽车的实时位置，在所述汽车按照当前行驶方向会与所述障碍物相撞时，精确显示障碍物位置。

优选地，所述障碍物检测模块包括：

第一距离测量单元，第二距离测量单元，初始位置计算单元；

第一距离测量单元：用于测量所述障碍物与第一标志位的距离，

第二距离测量单元：用于测量所述障碍物与第二标志位的距离，所述第一标志位与第二标志位在所述汽车的固定位置；

初始位置计算单元：用于当所述障碍物与第一标志位的距离和所述障碍物与第二标志位的距离均大于预订阈值时，根据所述障碍物与第一标志位的距离、所述障碍物与第二标志位的距离以及所述第一标志位与所述第二标志位间的距离，按照下述计算所述障碍物相对所述汽车的位置：

以所述第一标志位与第二标志位中点为原点，所述第一标志位与第二标志位连线方向为纵轴，所述第一标志位与第二标志位连线方向垂直方向为横轴，建立初始坐标系，所述障碍物对应所述初始坐标系的坐标(P_X,P_Y)为：

$(\frac{1}{2}\sqrt{(2{\mathrm{MP}}^2-{\mathrm{MN}}^2+2{\mathrm{NP}}^2)-\frac{{({\mathrm{NP}}^2-{\mathrm{MP}}^2)}^2}{{\mathrm{MN}}^2}},\frac{{\mathrm{NP}}^2-{\mathrm{MP}}^2}{2\mathrm{MN}})$

其中P_X为障碍物对应所述初始坐标系的横坐标，P_Y为障碍物对应所述初始坐标系的纵坐标，MP为所述障碍物与第一标志位的距离，NP为障碍物与第二标志位的距离，MN所述第一标志位与所述第二标志位的距离。

优选地，所述第一距离测量单元用于测量所述障碍物与第一标志位的距离包括：

第一距离测量单元记录从所述第一标志位发射超声波信号到在第一标识位接收到超声波信号在遇到所述障碍物后的回波信号的第一时间间隔，按照下述公式计算所述障碍物与第一标志位的距离：

MP＝C×t₁/2；

所述第二距离测量单元用于测量所述障碍物与第二标志位的距离包括：

第二距离测量单元用于记录从所述第二标志位发射超声波信号到在第二标识位接收到超声波信号在遇到所述障碍物后的回波信号的第二时间间隔，按照下述公式计算所述障碍物与第二标志位的距离：

NP＝C×t₂/2；

其中，t₁为第一时间间隔，t₂为第二时间间隔，C为超声波在水中的传播速度，按照下述公式计算：

C＝1400+4.6*T(m/s)，其中T为当前环境温度。

优选地，所述障碍物跟踪模块包括：车辆方位检测单元，实时位置计算单元；

所述车辆方位检测单元：用于实时检测汽车对应所述初始坐标系的汽车位置坐标和所述汽车的实时行驶方向。

所述实时位置计算单元：用于结合所述汽车位置坐标和所述实时行驶方向，按照下述方法计算障碍物相对实时坐标系的位置坐标：

以汽车当前位置上所述第一标志位与第二标志位中点为原点，所述第一标志位与第二标志位连线方向为纵轴，所述第一标志位与第二标志位连线方向垂直方向为横轴，建立实时坐标系，得到所述障碍物对应所述实时坐标系的实时位置坐标(P"_X,P"_Y)为：

(O"P×cosδ,O"P×sin(π+δ))

其中P"_X为障碍物对应实时坐标系的横坐标，P"_Y为障碍物对应实时坐标系的纵坐标，O"P为障碍物相对所述实时坐标系原点的距离，根据汽车对应所述初始坐标系的位置坐标计算得到，δ为障碍物相对所述实时坐标系根据所述汽车对应所述初始坐标系的位置坐标和汽车行驶方向计算得到。

优选地，所述障碍物显示模块用于据所述障碍物跟踪模块计算的所述障碍物相对所述汽车的实时位置，在所述汽车按照当前行驶方向会与所述障碍物相撞时，精确显示障碍物位置包括：

所述障碍物显示模块用于当所述实时坐标处于可碰撞区域内，将障碍物的精确位置显示于显示设备的碰撞区域内。

优选地，所述障碍物显示模块用于当所述实时坐标处于可碰撞区域内，将障碍物的精确位置显示于显示设备的碰撞区域内还包括：

所述障碍物显示模块用于当所述实时位置坐标处于可探测区间内，显示设备高亮显示障碍物所在的可探测区域。

一种汽车浅水行驶避障的装置，所述装置包括：左侧水声换能器，右侧水声换能器，定时器，陀螺仪，中央控制单元，显示器；

所述左侧水声换能器安装于所述汽车前部左半边较低的高度位置，用于发射超声波信号，并接受超声波信号

所述右侧水声换能器安装于所述汽车前部右半边较低的高度位置，与所述左侧水声换能器对称；用于发射超声波信号，并接受超声波信号，

所述定时器用于记录所述左侧水声换能器与所述右侧发射超声波信号到接受超声波信号之间的时间间隔；

所述陀螺仪用于检测所述汽车的方位信息；

所述中央控制单元处理用于按照定时器记录的时间间隔计算出所述障碍物到左侧水声换能器的距离和右侧水声换能器的距离，结合所述左侧水声换能器与所述右侧水声换能器的距离计算出障碍物相对位置，结合陀螺检测得到的方位信息对所述障碍物实时定位跟踪，根据所述实时定位跟踪，实时确定所述障碍物显示位置；

所述显示器用于根据中央控制单元确定的障碍物显示位置显示所述障碍物。

本发明提供的汽车浅水行驶避障的方法、***以及装置通能够在汽车行驶的过程中对于隐藏于水下的障碍物实现有效地检测和实时定位跟踪，能够判断车辆按照当前方向行驶是否有与障碍物碰撞的危险，并区别的显示障碍物所处位置。通过这样区别显示的障碍物位置，汽车行驶时能够有针对性的避让该障碍物。进而避免汽车因为发生碰撞带来的损失。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1为本发明中汽车浅水行驶示意图；

图2为本发明提供一种汽车浅水行驶避障方法流程图；

图3为本发明中测量汽车上标志位到障碍物的方法；

图4为本发明中检测障碍物相对于汽车初始位置的方法；

图5为本发明中计算障碍物相对于汽车实时位置的方法；

图6为本发明中显示设备显示障碍物的显示区域；

图7为本发明中在可碰撞区域内精确显示障碍物的方法；

图8为本发明中在可探测区域内高亮显示障碍物存在区域的方法；

图9为本发明提供一种汽车浅水行驶避障***实施例示意图；

图10为本发明提供另一种汽车浅水行驶避障***实施例示意图

图11为本发明中水声换能器安装于汽车上位置的示意图

图12为本发明中提供的实际装置的实施例示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图2所示，为本发明提供的一种汽车浅水行驶避障方法实施例：

S1：检测水下的障碍物相对汽车的初始位置；

S2：实时检测所述汽车的方位信息，结合所述障碍物相对汽车的初始位置，计算所述障碍物相对汽车的实时位置；

S3：根据所述障碍物相对汽车的实时位置，在汽车按照当前行驶方向会与障碍物相撞时，精确显示出障碍物位置。具体针对初始位置的检测，由于汽车是在浅水行驶，由于水下情况复杂，检测过程相对较为复杂，这里提供一种参考的方法：

如图3所示，在汽车上确定两个标志位分别为第一标志位201和第二标志位202，两个标志位之间的距离相对固定，在浅水行驶时，从第一标志位201和第二标志位202分别发出超声波信号，当超声波信号遇到障碍物101时会产生返回的回波信号，在第一标志位201和第二标志位202分别接收各自的回波信号，根据发出信号到接收回波信号所用的时间和超声波信号的在水下的传播速度就能够计算出障碍物101相对于第一标志位201和第二标志位202间的距离。通过障碍物101相对第一标志位的距离和障碍物101相对于第二标志位202的距离以及第一标志位201和第二标志位202之间的距离可以利用三角余弦定理确定出障碍物相对汽车的位置，，具体的过程如下：

记录从所述第一标志位201发射超声波信号到在第一标识位201接收到超声波信号在遇到所述障碍物后的回波信号的第一时间间隔，所述障碍物与第一标志位201的距离按照下述公式计算：

MP＝C×t₁/2

其中P为障碍物101所处位置，M为第一标志位201所处位置。 

障碍物101相对第二标志位的距离计算方法与上述的方法相同，结果如下：

NP＝C×t₂/2

N为第二标志位202所处位置。 

t₂为第二时间间隔，C为超声波在水中的传播速度，由于声波在水中的传播速度收到温度的影响，在这里需要按照下述关系式计算：

C＝1400+4.6×T(m/s)

T是当前的环境温度，可以通过测量第一标志位和第二标志位的温度取平均值得到。

如图4所示建立坐标系OXY，该坐标系以第一标志位与第二标志位的中点为原点O，第一标志位与第二标志位连线方向为纵轴，第一标志位与第二标志位连线方向垂直方向为横轴，由于三角余弦定理：

$\cos \∠\mathrm{OMP}=\frac{{\mathrm{MN}}^2+{\mathrm{MP}}^2-{\mathrm{NP}}^2}{2\mathrm{MN}\·\mathrm{MP}}---1$

进一步

$\mathrm{OP}=\sqrt{{\mathrm{OM}}^2+{\mathrm{MP}}^2-2\mathrm{OM}*\mathrm{MP}\cos \∠\mathrm{OMP}}---2$

带入式1得

$\mathrm{OP}=\sqrt{{\mathrm{OM}}^2+{\mathrm{MP}}^2-\mathrm{OM}\·\frac{{\mathrm{MN}}^2+{\mathrm{MP}}^2-{\mathrm{NP}}^2}{\mathrm{MN}}}---3$

由于

$\mathrm{OM}=\frac{1}{2}\mathrm{MN}---4$

得到

$\mathrm{OP}=\frac{\sqrt{{2\mathrm{MP}}^2-{\mathrm{MN}}^2+2{\mathrm{NP}}^2}}{2}---5$

同时利用余弦定理可以得出：

$\cos \mathrm{\β}=\frac{{\mathrm{OM}}^2+{\mathrm{OP}}^2-{\mathrm{MP}}^2}{2\mathrm{OM}*\mathrm{OP}}---6$

$\cos \mathrm{\β}=\frac{{\mathrm{NP}}^2-{\mathrm{MP}}^2}{\mathrm{MN}\·\sqrt{2{\mathrm{MP}}^2-{\mathrm{MN}}^2+2{\mathrm{NP}}^2}}---7$

由三角函数关系可以得到

$\sin \mathrm{\β}=\sqrt{1-\frac{{({\mathrm{NP}}^2-{\mathrm{MP}}^2)}^2}{{\mathrm{MN}}^2\·(2{\mathrm{MP}}^2-{\mathrm{MN}}^2+2{\mathrm{NP}}^2)}}---8$

根据图中P点的横纵坐标可以确定为：

P_X＝OP×sin(π-β)＝OP×sinβ    9

$P_Y=\mathrm{OP}\×\sin (\frac{\mathrm{\π}}{2}-\mathrm{\β})=\mathrm{OP}\×\cos \mathrm{\β}---10$

将式5和8带入式9得到

$P_X=\frac{1}{2}\sqrt{(2{\mathrm{MP}}^2-{\mathrm{MN}}^2+2{\mathrm{NP}}^2)-\frac{{({\mathrm{NP}}^2-{\mathrm{MP}}^2)}^2}{{\mathrm{MN}}^2}}---11$

将式5和7带入到式10

$P_Y=\frac{{\mathrm{NP}}^2-{\mathrm{MP}}^2}{2\mathrm{MN}}---12$

综上可以得到障碍物的坐标(P_X,P_Y)为

$(\frac{1}{2}\sqrt{(2{\mathrm{MP}}^2-{\mathrm{MN}}^2+2{\mathrm{NP}}^2)-\frac{{({\mathrm{NP}}^2-{\mathrm{MP}}^2)}^2}{{\mathrm{MN}}^2}},\frac{{\mathrm{NP}}^2-{\mathrm{MP}}^2}{2\mathrm{MN}})---13$

对于障碍物初始位置的检测在浅水行驶过程中一直在进行的，一般而言当30米内存在障碍物时，上述方法能够在检测到障碍物的存在并能够确定障碍物的相对车辆的初始位置，在检测到初始位置之后，将实时检测汽车的方位信息，方位信息包括了车辆位置的位移变化和车辆行驶的方向变化。结合障碍物相对汽车的初始位置，对计算障碍物相对汽车的实时位置。

对于障碍物的实时位置计算，可以基于上述的障碍物坐标计算方式，对障碍相对汽车的实时坐标进行计算：

识别出障碍物P的坐标后，即将此时的OXY坐标系作为初始坐标系，并通过车载的行驶装置实时记录车辆的方位信息。参考图5所示，图中所示1001为汽车行驶过程中的某一位置,参考初始坐标系OXY，汽车调整方向行驶至1001位置的坐标为(O'_X,O'_Y)，方位相对于初始坐标系OXY的旋转角度为∠X'OX,同理汽车行驶至1002位置的坐标为(O"_X,O"_Y),并且调整方向，现在方位相对于OXY坐标系的旋转角度为∠X"QX。以汽车行驶至1002位置为例，计算障碍物P相对于1002位置的坐标定位计算方法如下：

通过与建立坐标OXY相同方法建立当汽车行驶至1002位置对应的实时坐标系O"X"Y",障碍物P相对于当前车辆的坐标，即为障碍物P位于实时坐标 系O"X"Y"中的坐标，用(P"_X,P"_Y)表示障碍物P相对于当前车辆的实时坐标系的坐标，则计算方法如下：

由于汽车行驶至1002位置的坐标为(O"_X,O"_Y),方位相对于OXY坐标系的旋转角度为∠X"QX，该位置坐标和旋转角度由传感器实时测量所得，均为已知项，所以：

${\mathrm{OO}}^{\′\′}=\sqrt{{\mathrm{OX}}^{\′\′2}+{\mathrm{OY}}^{\′\′2}}---14$

$\∠X{\mathrm{OO}}^{\′\′}=\arctan \frac{{O^{\′\′}}_X}{{O^{\′\′}}_Y}---15$

$\∠\mathrm{X\; OP}=\mathrm{\β}-\frac{\mathrm{\π}}{2}---16$

其中β可由式7或8计算可得。参考图5可知，

θ＝∠X OO"-∠X OP    17

由式15、16和17计算可得：

$\mathrm{\θ}=\frac{\mathrm{\π}}{2}+\arctan \frac{{O^{\′\′}}_X}{{O^{\′\′}}_Y}-\mathrm{\β}---19$

由此可知，在三角形OO"P中，边长OP和OO"分别可以由式(10)和式(14)计算所得，且边长O"P和OO"的夹角θ可由式(19)计算可得，均为已知或可求项，所以通过三角形余弦定理可得：

$O^{\′\′}P=\sqrt{{\mathrm{OO}}^{\′\′2}+{\mathrm{OP}}^2-{2\mathrm{OO}}^{\′\′}\·\mathrm{OP}\×\cos \mathrm{\β}}---20$

$\∠{\mathrm{OO}}^{\′\′}P=\sqrt{\frac{{\mathrm{OO}}^{\′\′2}+O^{\′\′}{P}^2-{\mathrm{OP}}^2}{2{\mathrm{OO}}^{\′\′}\·O^{\′\′}P}}---21$

将式21代入式22可计算出∠OO"P为一已知项。 

参考图5所示，可得以下关系式：

∠OO"X"＝π-∠X OO"-∠X"QX    22

δ＝∠OO"P-∠OO"X"   23

可以通过式17、22、23和24可计算出δ。

所以，障碍物P位于当前坐标系O"X"Y"中的坐标(P"_X,P"_Y)可计算，且结果如下：

P"_X＝O"P×cosδ   24

P"_Y＝O"P×sin(π+δ)   25

综上可以根据汽车的方位与方向，得到在探测到障碍物的实时坐标位置(P"_X,P"_Y)为

(O"P×cosδ,O"P×sin(π+δ))

其中O"P和δ的三角函数值可以通过上面的步骤计算得到。

在得到实时位置坐标后，将按照实时的坐标位置显示障碍物，进一步为了使得达到引导避障的效果，如图6所示，可以对汽车行驶的路线上区域划分，根据汽车与障碍的相对位置将显示设备显示障碍的区域，划分为可碰撞区域A0、A1、B0、B1、可探测区域C0、C1。下面将按照实时位置坐标所处不同区域将障碍物以不同的方式区别显示：

如图7所示，障碍物的实时位置坐标处于可碰撞区域内A0、A1、B0、B1，将障碍物的精确位置显示于显示设备的碰撞区域内。

如图8所示，当实时位置坐标处于可探测区域C0、C1内，沿当前方向不会与障碍物相撞，为了避免在行驶中需要改变方向的时候，不知道障碍物的位置，而发生碰撞，这时显示设备会高亮显示障碍物所在的可探测区域作为提醒。

如果测得的实时位置坐标不在上述的区间内，则已经没有碰撞的风险，可以停止对该障碍物的实时位置坐标的计算。同时上述的障碍物位置的计算和显示的过程会一直进行，直到该障碍的实时坐标位置不在上述的区间内，则车辆可以认为已经基本避开了障碍物，不会有碰撞的可能。

为了上述的方法对于障碍物的检测均以单个障碍物作为示例说明，实际的应用中障碍物未必只存在一个，需要说明的是，该方法完成的是完成每个障碍物检测，跟踪和显示；在多个障碍物存在时，会对这些多个障碍物执行上述方法步骤，以实现对这些障碍物的避让。

通过上述的方法将能够按照障碍物所处位置，有效的将障碍物实时显示在显示设备上，用于汽车浅水行驶过程中进行避障。

如图9所示，本发明还提供一种汽车浅水行驶避障***，包括：障碍物检测模块M1，障碍物跟踪模块M2，障碍物显示模块M3；

障碍物检测模块M1：检测水下的障碍物相对所述汽车的位置；

障碍物跟踪模块M2：检测所述汽车的位移与角位移，结合所述障碍物检测模块检测的障碍物相对所述汽车的位置，对所述障碍物进行实时定位跟踪。

障碍物显示模块M3：根据所述中央控制模块对障碍物的实时定位跟踪，在障碍物所述汽车按照当前行驶方向会与所述障碍物相撞时，精确显示障碍物位置。

进一步，如图10所示，障碍物检测模块M1包括：

第一距离测量单元M11：用于测量所述障碍物与第一标志位的距离，

第二距离测量单元M12：用于测量所述障碍物与第二标志位的距离，所述第一标志位与第二标志位在汽车的固定位置；

位置计算单元M13：用于计算所述障碍物相对汽车的初始位置，建立坐标系以及坐标的计算过程可以与前面方法部分相同，这里也不再赘述。

进一步，两个测量单元测量障碍物与各自标志位的距离的计算方式也可以与方法对应部分相同。

进一步，如图10所示，障碍物跟踪模块M2包括：车辆方位检测单元M21，实时位置计算单元M22；

车辆方位检测单元M21：用于实时检测汽车对应所述初始坐标系的汽车位置坐标和所述汽车的实时行驶方向。

所述实时位置计算单元M22：用于结合所述汽车位置坐标和所述实时行驶方向，计算障碍物相对实时坐标系的位置坐标，建立坐标系以及坐标的计算过程可以与前面方法部分相同，这里也不再赘述。

进一步，障碍物显示模块也将区分障碍物的实时位置坐标处置可碰撞区域或可取探测区域内，将障碍物的紧将障碍物的精确位置显示于显示设备的碰撞区域内或者高亮显示障碍物所在的可探测区。进一步，当障碍物的当所述实时坐标处于障碍物可探测的区间内，显示设备高亮显示障碍物所在的可探测区域；

在实际的装置中，对于测量所需数据的各个功能均有对应的传感器所以本发明中的探测器如下：左侧水声换能器，右侧水声换能器、定时器、陀螺仪。

如图12所示，左侧水声换能器301安装于所述汽车100前部左半边，右侧水声换能器302安装于所述汽车100前部右半边，与所述左侧水声换能器对称；两个水声换能器用于发射超声波信号，并接受超声波信号。

定时器用于记录所述左侧水声换能器与所述右侧发射超声波信号到接受超声波信号之间的时间间隔。

陀螺仪用于检测所述汽车的方位信息。

另外在实际的装置中会把各种信息处理计算过程的单元或者模块集成于中央控制器中。

该中央控制器能够按照定时器记录的时间间隔计算出障碍物到左侧水声换能器的距离和右侧水声换能器的距离；能够结合左侧水声换能器与所述右侧水声换能器的距离计算出障碍物相对初始位置；能够结合陀螺检测得到的方位信息对所述障碍物实时定位跟踪，根据所述实时定位跟踪，实时确定所述障碍物显示位置。

最终障碍物位置的输出由显示器完成：

根据由中央控制器实时确定的障碍物位置，显示器实时显示障碍物的位置。

针对上面所述的各种实际器件，实际汽车浅水行驶避障装置，其实施例如图12所示，左侧水声换能器301、右侧水声换能器302、定时器303、陀螺仪304均连接在中央控制器305，以便于对获得的数据的处理，显示器306同样连接在中央控制器305上，显示中央控制器要求显示的内容。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (13)

1.一种汽车浅水行驶避障的方法，其特征在于，所述方法包括

检测水下的障碍物相对所述汽车的初始位置；

实时检测所述汽车的方位信息，结合所述障碍物相对所述汽车的初始位置，计算所述障碍物相对所述汽车的实时位置；

根据所述障碍物相对所述汽车的实时位置，在所述汽车按照当前行驶方向会与所述障碍物相撞时，精确显示出障碍物位置。

2.根据权利要求1所述方法，其特征在于，所述检测水下的障碍物相对所述汽车的初始位置包括：

测量所述障碍物与第一标志位的距离，测量所述障碍物与第二标志位的距离，所述第一标志位与第二标志位在汽车的固定位置；

当所述障碍物与第一标志位的距离和所述障碍物与第二标志位的距离均大于预订阈值时，根据所述障碍物与第一标志位的距离、所述障碍物与第二标志位的距离以及所述第一标志位与所述第二标志位间的距离，按照下述计算所述障碍物相对所述汽车的初始位置：

以所述第一标志位与第二标志位中点为原点，所述第一标志位与第二标志位连线方向为纵轴，所述第一标志位与第二标志位连线方向垂直方向为横轴，建立初始坐标系，所述障碍物对应所述初始坐标系的坐标(P_X,P_Y)为：

$(\frac{1}{2}\sqrt{({2\mathrm{MP}}^2-{\mathrm{MN}}^2+{2\mathrm{NP}}^2)-\frac{{({\mathrm{NP}}^2-{\mathrm{MP}}^2)}^2}{{\mathrm{MN}}^2}},\frac{{\mathrm{NP}}^2-{\mathrm{MP}}^2}{2\mathrm{MN}})$

其中P_X为障碍物对应所述初始坐标系的横坐标，P_Y为障碍物对应所述初始坐标系的纵坐标，MP为所述障碍物与第一标志位的距离，NP为障碍物与第二标志位的距离，MN所述第一标志位与所述第二标志位的距离。

3.根据权利要求2所述方法，其特征在于，所述测量所述障碍物与第一标志位的距离包括：

记录从所述第一标志位发射超声波信号到在所述第一标识位接收到超声波信号在遇到所述障碍物后的回波信号的第一时间间隔，所述障碍物与第一标志位的距离按照下述公式计算：

MP＝C×t₁/2；

所述测量所述障碍物与第二标志位的距离包括：

记录从所述第二标志位发射超声波信号到在所述第二标识位接收到超声波信号在遇到所述障碍物后的回波信号的第二时间间隔，所述障碍物与第二标志位的距离按照下述公式计算：

NP＝C×t₂/2；

其中，t₁为第一时间间隔，t₂为第二时间间隔，C为超声波在水中的传播速度，按照下述公式计算：

C＝1400+4.6*T(m/s)，其中T为当前环境温度。

4.根据权利要求2所述方法，其特征在于，所述实时检测所述汽车的方位信息包括：

实时检测所述汽车对应所述初始坐标系的汽车位置坐标和所述汽车的实时行驶方向。

所述结合所述障碍物相对所述汽车的初始位置，对所述障碍物进行实时定位跟踪包括：

结合所述汽车位置坐标和所述实时行驶方向，按照下述方法计算障碍物相对实时坐标系的位置坐标：

以汽车当前位置上所述第一标志位与第二标志位中点为原点，所述第一标志位与第二标志位连线方向为纵轴，所述第一标志位与第二标志位连线方向垂直方向为横轴，建立所述实时坐标系，得到所述障碍物对应所述实时坐标系的实时位置坐标(P"_X,P"_Y)为：

(O"P×cosδ,O"P×sin(π+δ))

其中P"_X为障碍物对应实时坐标系的横坐标，P"_Y为障碍物对应实时坐标系的纵坐标，O"P为障碍物相对所述实时坐标系原点的距离，根据所述汽车位置坐标计算得到，δ为障碍物相对所述实时坐标系纵轴方向的夹角，根据所述汽车位置坐标和所述实时行驶方向计算得到。

5.根据权利要求4所述方法，其特征在于，所述根据所述对所述障碍物的实时定位跟踪，在所述汽车按照当前行驶方向会与所述障碍物相撞时，精确显示出障碍物位置包括：

当所述实时坐标处于可碰撞区域内，将障碍物的精确位置显示于显示设备的碰撞区域内。

6.根据权利要求5所述方法，其特征在于，所述当所述实时坐标处于可碰撞区域内，将障碍物的精确位置显示于显示设备的碰撞区域内还包括：

当所述实时位置坐标处于可探测区间内，显示设备高亮显示障碍物所在的可探测区域。

7.一种汽车浅水行驶避障***，其特征在于，所述***包括：障碍物检测模块，障碍物跟踪模块，障碍物显示模块

所述障碍物检测模块：检测水下的障碍物相对所述汽车的位置；

所述障碍物跟踪模块：用于检测所述汽车的方位信息，结合所述障碍物检测模块检测的障碍物相对所述汽车的位置，计算所述障碍物相对所述汽车的实时位置。

所述障碍物显示模块：根据所述障碍物跟踪模块计算的所述障碍物相对所述汽车的实时位置，在所述汽车按照当前行驶方向会与所述障碍物相撞时，精确显示障碍物位置。

8.根据权利要求7所述***，其特征在于，所述障碍物检测模块包括：

第一距离测量单元，第二距离测量单元，初始位置计算单元；

第一距离测量单元：用于测量所述障碍物与第一标志位的距离，

第二距离测量单元：用于测量所述障碍物与第二标志位的距离，所述第一标志位与第二标志位在所述汽车的固定位置；

初始位置计算单元：用于当所述障碍物与第一标志位的距离和所述障碍物与第二标志位的距离均大于预订阈值时，根据所述障碍物与第一标志位的距离、所述障碍物与第二标志位的距离以及所述第一标志位与所述第二标志位间的距离，按照下述计算所述障碍物相对所述汽车的位置：

以所述第一标志位与第二标志位中点为原点，所述第一标志位与第二标志位连线方向为纵轴，所述第一标志位与第二标志位连线方向垂直方向为横轴，建立初始坐标系，所述障碍物对应所述初始坐标系的坐标(P_XP_Y)为：

$(\frac{1}{2}\sqrt{({2\mathrm{MP}}^2-{\mathrm{MN}}^2+{2\mathrm{NP}}^2)-\frac{{({\mathrm{NP}}^2-{\mathrm{MP}}^2)}^2}{{\mathrm{MN}}^2}},\frac{{\mathrm{NP}}^2-{\mathrm{MP}}^2}{2\mathrm{MN}})$

其中P_X为障碍物对应所述初始坐标系的横坐标，P_Y为障碍物对应所述初始坐标系的纵坐标，MP为所述障碍物与第一标志位的距离，NP为障碍物与第二标志位的距离，MN所述第一标志位与所述第二标志位的距离。

9.根据权利要求8所述***，其特征在于，所述第一距离测量单元用于测量所述障碍物与第一标志位的距离包括：

第一距离测量单元记录从所述第一标志位发射超声波信号到在第一标识位接收到超声波信号在遇到所述障碍物后的回波信号的第一时间间隔，按照下述公式计算所述障碍物与第一标志位的距离：

MP＝C×t₁/2；

所述第二距离测量单元用于测量所述障碍物与第二标志位的距离包括：

第二距离测量单元用于记录从所述第二标志位发射超声波信号到在第二标识位接收到超声波信号在遇到所述障碍物后的回波信号的第二时间间隔，按照下述公式计算所述障碍物与第二标志位的距离：

NP＝C×t₂/2；

其中，t₁为第一时间间隔，t₂为第二时间间隔，C为超声波在水中的传播速度，按照下述公式计算：

C＝1400+4.6*T(m/s)，其中T为当前环境温度。

10.根据权利要求8所述***，其特征在于，所述障碍物跟踪模块包括：车辆方位检测单元，实时位置计算单元；

所述车辆方位检测单元：用于实时检测汽车对应所述初始坐标系的汽车位置坐标和所述汽车的实时行驶方向。

所述实时位置计算单元：用于结合所述汽车位置坐标和所述实时行驶方向，按照下述方法计算障碍物相对实时坐标系的位置坐标：

以汽车当前位置上所述第一标志位与第二标志位中点为原点，所述第一标志位与第二标志位连线方向为纵轴，所述第一标志位与第二标志位连线方向垂直方向为横轴，建立实时坐标系，得到所述障碍物对应所述实时坐标系的实时位置坐标(P"_X,P"_Y)为：

(O"P×cosδ,O"P×sin(π+δ))

其中P"_X为障碍物对应实时坐标系的横坐标，P"_Y为障碍物对应实时坐标系的纵坐标，O"P为障碍物相对所述实时坐标系原点的距离，根据汽车对应所述初始坐标系的位置坐标计算得到，δ为障碍物相对所述实时坐标系根据所述汽车对应所述初始坐标系的位置坐标和汽车行驶方向计算得到。

11.根据权利要求10所述装置，其特征在于，所述障碍物显示模块用于据所述障碍物跟踪模块计算的所述障碍物相对所述汽车的实时位置，在所述汽车按照当前行驶方向会与所述障碍物相撞时，精确显示障碍物位置包括：

所述障碍物显示模块用于当所述实时坐标处于可碰撞区域内，将障碍物的精确位置显示于显示设备的碰撞区域内。

12.根据权利要求11所述***，其特征在于，所述障碍物显示模块用于当所述实时坐标处于可碰撞区域内，将障碍物的精确位置显示于显示设备的碰撞区域内还包括：

所述障碍物显示模块用于当所述实时位置坐标处于可探测区间内，显示设备高亮显示障碍物所在的可探测区域。

13.一种汽车浅水行驶避障的装置，其特征在于，所述装置包括：左侧水声换能器，右侧水声换能器，定时器，陀螺仪，中央控制单元，显示器；

所述左侧水声换能器器安装于所述汽车前部左半边较低的高度位置，用于发射超声波信号，并接受超声波信号

所述右侧水声换能器安装于所述汽车前部右半边较低的高度位置，与所述左侧水声换能器对称；用于发射超声波信号，并接受超声波信号，

所述定时器用于记录所述左侧水声换能器与所述右侧发射超声波信号到接受超声波信号之间的时间间隔；

所述陀螺仪用于检测所述汽车的方位信息；

所述中央控制单元处理用于按照定时器记录的时间间隔计算出所述障碍物到左侧水声换能器的距离和右侧水声换能器的距离，结合所述左侧水声换能器与所述右侧水声换能器的距离计算出障碍物相对位置，结合陀螺检测得到的方位信息对所述障碍物实时定位跟踪，根据所述实时定位跟踪，实时确定所述障碍物显示位置；

所述显示器用于根据中央控制单元确定的障碍物显示位置显示所述障碍物。