CN103292778A - 一种地下方位测量装置及其方法 - Google Patents

Abstract

本发明公开了一种地下方位测量装置及测量方法，该地下方位测量装置包括左激光测距传感器、右激光测距传感器、定向板、俯仰滚转角测量装置、中央控制器。该方位测量方法包括：俯仰滚转角测量装置根据重力加速度的分量值来计算所述被测量设备的滚转角和俯仰角；中央处理器利用左激光测距传感器到定向板的左侧距离和右激光测距传感器到定向板右侧距离值来计算被测量设备的偏航角。本发明可在地下高精度、快速测量被测量设备的方位角，具有结构简单、造价低廉、质量轻、体积小、环境适应能力强、可靠性高等特点。

Description

一种地下方位测量装置及其方法

技术领域

本发明涉及地下定向技术领域，尤其涉及一种地下方位测量装置及其方法。

背景技术

在许多地下作业现场，譬如：煤矿掘进、深孔钻车探水、探瓦斯、探断层，都需要快速准确测量方位。针对此需求，目前主要有四种解决方法：①拉线法，拉线法是通过工人拉线的方式来测量当前方位与参考方位(譬如煤矿巷道中线)的相对角度，这种原始的方法误差大、效率低、受人工因素影响大；②转盘法，转盘法是指将一个标有角度读数的转盘同轴固定安装在转动机械的转动轴上，并在转动机械的转动轴上装有指针，当机械转动时，指针所指读数即为转动的相对角度。然而，要应用这种方法必须要保持该机械的转盘与参考方位平行，并且首先要测量出转盘0度方向与参考方向的夹角，在工业现场，很难保证这两点，因此这种方法的应用也受到了很大限制；③地磁测量法，这种方法的原理类似于指南针，然而在地底下，地磁场较地面相对微弱，并且地下作业机械多为钢铁等铁磁物质，对测量有很大影响，尤为恶劣的是，地下矿产对地磁场也会产生很大的干扰，因此这种方法在实际中很少有应用；④陀螺仪测量法，这种方法的原理与地面导弹、飞机、船用陀螺仪类似，测量精度高，受环境因素影响小，然而其造价昂贵、体积大、质量重、对机械振动较敏感、需要定期校正和维护，对于本身市场售价不高的机械(譬如煤矿深孔钻车)而言，很难采用这种方案。

发明内容

(一)要解决的技术问题

本发明旨在解决现有地下方位测量系统存在的不能兼顾性能与成本的问题，提出了一种地下方位测量方法，并应用该方法设计了一种装置。该装置具有造价低、精度高、质量轻、测量速度快等特点，能够较好地解决已有的问题。

(二)技术方案

为解决上述的技术问题，本发明提供一种地下方位测量装置，包括左激光测距传感器、右激光测距传感器、定向板、俯仰滚转角测量装置和中央控制器，所述定向板安装在被测量设备上，并且定向板的方位与被测量设备的方位相同，定向板为漫反射白板，用于反射激光测距传感器发射的激光，从而测得左激光测距传感器、右激光测距传感器到定向板的距离；所述左激光测距传感器和右激光测距传感器分别用于发射激光到所述定向板上，并分别用于接收由所述左、右反射点反射的激光，由此分别测定左激光测距传感器至定向板的左反射点的距离，以及右激光测距传感器至定向板的右反射点的距离；所述俯仰滚转角测量装置位于所述被测量设备的转动中心，用于测量被测量设备的滚转角和俯仰角；所述中央控制器分别与所述左激光测距传感器、右激光测距传感器和俯仰滚转角测量装置进行有线或无线通信，用于分别获取左激光测距传感器与左参考反射点的距离d_L、右激光测距传感器与右参考反射点之间的距离d_R、所述俯仰滚转角测量装置测量的滚转角

和俯仰角θ，计算出被测设备的偏航角ψ，从而得到被测量设备的方位

根据本发明的一种具体实施方式，所述中央控制器包括接口电路、微控制器、人机交互电路和中央控制器电源，其中，所述接口电路用于接收左激光测距传感器传回来的左侧距离d_L、右激光测距传感器传回来的右侧距离d_R、俯仰滚转角测量装置传回来的滚转角和俯仰角θ；所述微控制器用于接收所述接口电路传回来的信号，计算出被测设备的偏航角ψ，从而得到被测量设备的方位

并从所述人机交互电路接收外界设置信息，向人机交互电路输出显示信息；所述人机交互电路用于接收用户输入和显示输出结果。

根据本发明的一种具体实施方式，所述俯仰滚转角测量装置包括三轴加速度计、微处理器、通信接口和俯仰滚转测量装置电源，其中，所述三轴加速度计用于测重力加速度在其联体坐标系的三个轴上的分量，而三轴加速度计的联体坐标系的原点和三个轴的方向均与被测设备联体坐标系O₁X₁Y₁Z₁一致，所述被测设备联体坐标系O₁X₁Y₁Z₁的原点O₁设置于被测量设备的转动中心，当其滚转角

俯仰角θ＝0、偏航角ψ＝0时，X₁轴的方向与方位参考坐标系OXYZ的X轴一致，Y₁轴的方向与方位参考坐标系OXYZ的Y轴一致，Z₁轴的方向与方位参考坐标系OXYZ的在Z轴一致。方位参考坐标系OXYZ的原点O在点A与点B的中点，Y轴过AB两点水平向右，Z轴指向地心朝下(亦即铅垂方向)，X轴的方向根据右手螺旋法确定。

所述微处理器用于根据所述三轴加速度计传输来的重力加速度在被测设备联体坐标系的三个轴上的分量，计算出被测量设备的滚转角

和俯仰角θ。

根据本发明的一种具体实施方式，所述滚转角测量装置的微处理器根据如下公式计算被测量设备的滚转角

其中，[g_x g_y g_z]^T是重力加速度在被测设备联体坐标系O₁X₁Y₁Z₁三个轴上的分量值。

根据本发明的一种具体实施方式，所述滚转角测量装置的微处理器根据如下公式计算被测量设备的俯仰角 $\mathrm{\θ}:\mathrm{\θ}=-\arctan (g_x/\sqrt{g_y^2+g_z^2}),$ 其中，[g_x g_y g_z]^T是重力加速度在被测设备联体坐标系O₁X₁Y₁Z₁三个轴上的分量值。

根据本发明的一种具体实施方式，所述中央控制器的微控制器根据如下公式计算被测量设备的ψ：ψ＝arctan(Δd/D_AB)，其中D_AB为所述左激光测距传感器和右激光测距传感器的中心距，而Δd＝d_L-d_R。

本发明还提出一种地下方位测量方法，应用于前述的地下方位测量装置中，包括如下步骤：

步骤S1：利用俯仰滚转角测量装置的三轴加速度计测量重力加速度在其联体坐标系的三个坐标轴上的重力加速度的分量值，而三轴加速度计的联体坐标系的原点和三个轴的方向均与被测设备联体坐标系O₁X₁Y₁Z₁一致，所述被测设备联体坐标系O₁X₁Y₁Z₁的原点O₁设置于被测量设备的转动中心，当其滚转角俯仰角θ＝0、偏航角ψ＝0时，X₁轴的方向与方位参考坐标系OXYZ的X轴一致，Y₁轴的方向与方位参考坐标系OXYZ的Y轴一致，Z₁轴的方向与方位参考坐标系OXYZ的在Z轴一致。方位参考坐标系OXYZ的原点O在点A与点B的中点，Y轴过AB两点水平向右，Z轴指向地心朝下(亦即铅垂方向)，X轴的方向根据右手螺旋法确定；步骤S2：利用左激光测距传感器测量从该传感器到定向板的左反射点的距离和右激光测距传感器从该传感器到定向板的右反射点的距离；步骤S3：根据所述重力加速度的分量值来计算所述被测量设备的滚转角和俯仰角；步骤S4：利用左激光测距传感器到左参考反射点的距离和右激光测距传感器到右参考反射点的距离值，计算被测量设备的偏航角。

根据本发明的一种具体实施方式，所述滚转角测量装置根据如下公式计算被测量设备的滚转角其中，[g_x g_y g_z]^T是重力加速度在被测设备联体坐标系O₁X₁Y₁Z₁三个轴上的分量值。

根据本发明的一种具体实施方式，所述滚转角测量装置(4)根据如下公式计算被测量设备的俯仰角 $\mathrm{\θ}:\mathrm{\θ}=-\arctan (g_x/\sqrt{g_y^2+g_z^2}),$ 其中，[g_x g_y g_z]^T是重力加速度在被测设备联体坐标系O₁X₁Y₁Z₁三个轴上的分量值。

根据本发明的一种具体实施方式，所述中央控制器根据如下公式计算被测量设备的偏航角ψ：ψ＝arctan(Δd/D_AB)，其中D_AB为所述左激光测距传感器和右激光测距传感器的中心距，而Δd＝d_L-d_R。

(三)有益效果

本发明所提供的一种地下方位测量方法及其装置具有以下有益效果：一是精度高、环境适应能力强、可靠性高；二是能实时测量被测物体的方位、计算效率高；三是系统工作原理和结构比较简单、质量轻、造价低廉、容易制造；四是运行及维护成本低廉。

附图说明

图1是根据本发明的一种地下方位测量装置的总体结构示意图；

图2是根据本发明的计算偏航角方法的几何原理图；

图3是根据本发明的中央控制器的原理框图；

图4是根据本发明的俯仰滚转角测量装置的原理框图；

图5是根据本发明的一种地下方位测量方法的工作流程图。

具体实施方式

针对现有方法存在问题，本发明提出一种地下方位测量方法，并设计了一种适于实际应用的测量装置。本发明利用简单的几何原理和相对廉价的测量传感器，能够快速准确地测量出地下方位。

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明作进一步的详细说明。

图1是根据本发明的地下方位测量装置的总体结构示意图。如图1所示，该测量装置包括：左激光测距传感器1、右激光测距传感器2、定向板3、俯仰滚转角测量装置4、中央控制器5。

所述定向板3安装在被测量设备上(譬如深孔钻车的定向钻上)，并且定向板3的方位与被测量设备的方位相同。由此，只要测得定向板的方向，即可得知被测量设备的方向。定向板3主要作用是反射激光测距传感器发射的激光，左激光测距传感器1、右激光测距传感器2在定向板3上的反射点在此分别称为左反射点和右反射点，这里说的“左”、“右”仅是指图1中的左右相对位置关系，在实际应用中可以改变“左”、“右”的定义。

所述左激光测距传感器1和右激光测距传感器2分别用于发射激光到所述定向板3上，并分别用于接收由所述左、右反射点反射的激光，由此分别测定左激光测距传感器1至定向板3的左反射点的距离，以及右激光测距传感器2至定向板3的右反射点的距离。左激光测距传感器1和右激光测距传感器2位于同一水平面上，其连线与参考方位平行(参考方位一般为巷道中线)。

为了说明本发明的原理，如图1所示，设左激光测距传感器1和右激光测距传感器2分别位于A点和B点，定向板3的左、右反射点分别为A1点和B1点，从左激光测距传感器1到左反射点的距离为d_L，右激光测距传感器2到右反射点的距离为d_R。

为了便于描述，首先定义一个方位参考坐标系OXYZ和一个被测设备联体坐标系O₁X₁Y₁Z₁。方位参考坐标系是已知的确定了方位的坐标系，被测设备联体坐标系O₁X₁Y₁Z₁是指原点始终位于测量设备的转动中心，且能随着被测量设备转动的坐标系。

在图1所示的示例中，方位参考坐标系OXYZ的原点O在点A与点B的中点，Y轴过AB两点水平向右，Z轴指向地心朝下(亦即铅垂方向)，X轴的方向根据右手螺旋法确定。

被测设备联体坐标系O₁X₁Y₁Z₁的原点O₁设置于被测量设备的转动中心，当其滚转角

俯仰角θ＝0、偏航角ψ＝0时，X₁轴的方向与方位参考坐标系OXYZ的X轴一致，Y₁轴的方向与方位参考坐标系OXYZ的Y轴一致，Z₁轴的方向与方位参考坐标系OXYZ的在Z轴一致。

所述俯仰滚转角测量装置4位于所述被测量设备的转动中心，用于测量被测量设备的滚转角和俯仰角。其包括一个三轴加速度计41。根据一种具体实施方式，俯仰滚转角测量装置4位于定向板3上，且安装时要保证三轴加速度计41的联体坐标系的原点和三个轴的方向与被测设备联体坐标系O₁X₁Y₁Z₁的三个轴的方向一致。

中央控制器5分别与所述左激光测距传感器1、右激光测距传感器2和俯仰滚转角测量装置4进行有线或无线通信，分别获取左激光测距传感器1、右激光测距传感器2测得的距离d_L、d_R以及滚转角和俯仰角θ。

图3是本发明的中央控制器的原理框图。所述中央控制器5包括接口电路51、微控制器52、人机交互电路53、中央控制器电源54四个部分。其中接口电路51接收左激光测距传感器传回来的左侧距离d_L、右激光测距传感器传回来的右侧距离d_R、俯仰滚转角测量装置4传回来的滚转角

和俯仰角θ，计算出被测设备的偏航角ψ，从而得到被测量设备的方位

微控制器52是中央控制器5的核心，其功能为接收接口电路51传回来的信号，计算出被测设备的偏航角ψ，从而得到被测量设备的方位

并从人机交互电路53接收外界设置信息，向人机交互电路53输出显示信息。人机交互电路53包括键盘和显示屏等，用于接收用户输入和显示输出结果等。中央控制器电源54为中央控制器5各个部分提供适当电压级别的电能。

图4是本发明的俯仰滚转角测量装置的原理框图。所述俯仰滚转角测量装置4包括三轴加速度计41、微处理器42、通信接口43和俯仰滚转测量装置电源44。其中三轴加速度计41用于测量被测量设备的线加速度，其主要功能是测量重力加速度在被测设备联体坐标系三个轴上的分量[g_x g_y g_z]^T，g_x表示重力加速度在X₁轴上的分量，g_y表示重力加速度在Y₁轴上的分量，g_z表示重力加速度在Z₁轴上的分量，将其转换为数字值，并传送给处理器电路42(具体传输方式可为SPI总线、I2C总线、USART总线等方式)。处理器42用于采集所述三轴加速度计41传输来的三轴加速度分量值，然后计算出被测量设备的滚转角

和俯仰角θ。通信接口43的主要功能是将计算结果传输给中央控制器5，并可从中央控制器5接收设置参数。俯仰滚转测量装置电源44为俯仰滚转角测量装置4的各个部分提供适当电压级别的电能。

下面参照图5来说明利用上述测量装置的测量地下方位的方法。本发明还提供了一种地下方位测量方法，本发明中被测量设备的方位用欧拉角

来表示，其中为滚转角、θ为俯仰角、ψ为偏航角，该方法包含如下步骤：

步骤S1：利用俯仰滚转角测量装置测量重力加速度在被测设备联体坐标系的三个坐标轴上的的分量值。

如前所述，可得在被测设备联体坐标系O₁X₁Y₁Z₁的三个轴上的重力加速度的分量值[g_x g_y g_z]^T。

步骤S2：利用左激光测距传感器测量从该传感器到定向板的左反射点的距离和右激光测距传感器从该传感器到定向板的右反射点的距离。

如前所述，利用左激光测距传感器1测量从该左激光测距传感器1到定向板3的左反射点A1的距离值d_L和右激光测距传感器2从该右激光测距传感器到定向板3的右反射点B1的距离值d_R。

其中，步骤S1和步骤S2可同时进行，也可不同时进行。

步骤S3：根据所述重力加速度的分量值来计算所述被测量设备的滚转角和俯仰角。

如前所述，根据重力加速度在被测物体联体坐标系O₁X₁Y₁Z₁三个轴上的分量值[g_x g_y g_z]^T，计算被测量设备的滚转角

的计算公式如下：

根据测量重力加速度在被测物体联体坐标系O₁X₁Y₁Z₁三个轴上的分量值[g_x g_y g_z]^T，计算被测量设备的俯仰角θ的计算公式如下：

$\mathrm{\θ}=-\arctan (g_x/\sqrt{g_y^2+g_z^2})---\left(2\right)$

步骤S4：利用左激光测距传感器到左反射点的距离和右激光测距传感器到右反射点的距离值，计算被测量设备相对于参考方位的偏航角。

该步骤利用左激光测距传感器1测量从该传感器到定向板3的左反射点的距离值d_L和右激光测距传感器2从该传感器到定向板3的右反射点的距离值d_R，计算出被测量设备相对于参考方位的偏航角度ψ。

参照图2，通过左激光测距传感器1测量从该传感器到定向板3的距离值d_L和右激光测距传感器2从该传感器到定向板3的距离值d_R，计算出被测量设备相对于参考方位的偏航角ψ，其计算公式如下：

ψ＝arctan(Δd/D_AB)                        (3)

其中D_AB为左激光测距传感器1和右激光测距传感器2的中心距，而Δd＝d_L-d_R。

其中，步骤S1和步骤S2可同时进行，也可不同时进行。

根据本发明的测量设备可以将滚转角

俯仰角θ、偏航角ψ显示出来，供用户使用。

下面举一个优选实施例来进一步说明本发明的系统和方法。在该优先例中，左激光测距传感器1、右激光测距传感器2均采用带RS232串口的激光测距仪器，精度为正负1毫米。定向板3采用普通漫反射白板。俯仰滚转角测量装置4中的三轴加速度计电路41采用ST公司的LSM303DLHC芯片。微控制器52和微处理器42均采用ST公司的STM32F103单片机。接口电路51、通信接口43均采用MAX3232芯片，采用主从式通信协议，中央控制器5作为通信主机，轮询左激光测距传感器1、右激光测距传感器2、俯仰滚转角测量装置4，从而获得所需要的被测量。人机交互电路53采用键盘输入、七段数码管显示。中央控制器电源54和俯仰滚转测量装置电源44均采用广州金升阳科技有限公司的PWA_YMD-6W系列，将24V电压转换为5V后，再采用LM1117-3.3转换为3.3V。实验表明，该优选实施例与传统技术相比，有以下几方面有益效果：一是精度高(当D_AB为1米时，精度在0.2％以内)、环境适应能力强(对温度湿度不敏感)、可靠性高；二是能实时测量被测物体的方位、计算效率高；三是系统工作原理和结构比较简单、质量轻、造价低廉、容易制造；四是运行及维护成本低廉。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种地下方位测量装置，包括左激光测距传感器、右激光测距传感器、定向板、俯仰滚转角测量装置和中央控制器，

所述定向板安装在被测量设备上，并且定向板的方位与被测量设备的方位相同，定向板用于反射激光测距传感器发射的激光，从而测得左激光测距传感器、右激光测距传感器到定向板的距离；

所述左激光测距传感器和右激光测距传感器分别用于发射激光到所述定向板上，并分别用于接收由所述左、右反射点反射的激光，由此分别测定左激光测距传感器至定向板的左反射点的距离，以及右激光测距传感器至定向板的右反射点的距离；

所述俯仰滚转角测量装置位于所述被测量设备的转动中心，用于测量被测量设备的滚转角和俯仰角；

所述中央控制器分别与所述左激光测距传感器、右激光测距传感器和俯仰滚转角测量装置进行有线或无线通信，用于分别获取左激光测距传感器与左反射点的距离d_L、右激光测距传感器与右反射点之间的距离d_R、所述俯仰滚转角测量装置测量的滚转角

和俯仰角θ，计算出被测设备的偏航角ψ，从而得到被测量设备的方位

2.如权利要求1所述的地下方位测量装置，其特征在于：所述中央控制器包括接口电路、微控制器、人机交互电路和中央控制器电源，其中，

所述接口电路用于接收左激光测距传感器传回来的左侧距离d_L、右激光测距传感器传回来的右侧距离d_R、俯仰滚转角测量装置传回来的滚转角和俯仰角θ；

所述微控制器用于接收所述接口电路传回来的信号，计算出被测设备的偏航角ψ，从而得到被测量设备的方位并从所述人机交互电路接收外界设置信息，向人机交互电路输出显示信息；

所述人机交互电路用于接收用户输入和显示输出结果。

3.如权利要求2所述的地下方位测量装置，其特征在于：所述俯仰滚转角测量装置包括三轴加速度计、微处理器、通信接口和俯仰滚转测量装置电源，其中，

所述三轴加速度计用于测量重力加速度在其联体坐标系的三个轴上的分量，而三轴加速度计的联体坐标系的原点和三个轴的方向均与被测设备联体坐标系O₁X₁Y₁Z₁一致，所述被测设备联体坐标系O₁X₁Y₁Z₁的原点O₁设置于被测量设备的转动中心，当其滚转角

俯仰角θ＝0、偏航角ψ＝0时，X₁轴的方向与方位参考坐标系OXYZ的X轴方向一致，Y₁轴的方向与方位参考坐标系OXYZ的Y轴方向一致，Z₁轴的方向与方位参考坐标系OXYZ的在Z轴方向一致；所述方位参考坐标系OXYZ的原点O在点A与点B的中点，Y轴过AB两点水平向右，Z轴指向地心朝下(亦即铅垂方向)，X轴的方向根据右手螺旋法确定；

所述微处理器用于根据所述三轴加速度计传输来的重力加速度在联体坐标系的三个轴上的分量值，计算出被测量设备的滚转角

和俯仰角θ。

4.如权利要求3所述的地下方位测量装置，其特征在于：

所述滚转角测量装置的微处理器根据如下公式计算被测量设备的滚转角

其中，[g_x g_y g_z]^T是重力加速度在被测设备联体坐标系O₁X₁Y₁Z₁三个轴上的分量值。

5.如权利要求3所述的地下方位测量装置，其特征在于：

所述滚转角测量装置的微处理器根据如下公式计算被测量设备的俯仰角 $\mathrm{\θ}:\mathrm{\θ}=-\arctan (g_x/\sqrt{g_y^2+g_z^2}),$ 其中，[g_x g_y g_z]^T是重力加速度在被测设备联体坐标系O₁X₁Y₁Z₁三个轴上的分量值。

6.如权利要求3所述的地下方位测量装置，其特征在于：

所述中央控制器的微控制器根据如下公式计算被测量设备的偏航角ψ：ψ＝arctan(Δd/D_AB)，其中D_AB为所述左激光测距传感器和右激光测距传感器的中心距，而Δd＝d_L-d_R。

7.一种地下方位测量方法，应用于如权利要求1所述的地下方位测量装置中，包括如下步骤：

步骤S1：利用俯仰滚转角测量装置的三轴加速度计测量重力加速度在其联体坐标系的三个坐标轴上的重力加速度的分量值，而三轴加速度计的联体坐标系的原点和三个轴的方向均与被测设备的联体坐标系O₁X₁Y₁Z₁一致；所述被测设备联体坐标系O₁X₁Y₁Z₁的原点O₁设置于被测量设备的转动中心，当其滚转角

俯仰角θ＝0、偏航角ψ＝0时，X₁轴的方向与方位参考坐标系OXYZ的X轴一致，Y₁轴的方向与方位参考坐标系OXYZ的Y轴一致，Z₁轴的方向与方位参考坐标系OXYZ的在Z轴一致；所述方位参考坐标系OXYZ的原点O在点A与点B的中点，Y轴过AB两点水平向右，Z轴指向地心朝下(亦即铅垂方向)，X轴的方向根据右手螺旋法确定。

步骤S2：利用左激光测距传感器测量从该传感器到定向板的左反射点的距离和右激光测距传感器从该传感器到定向板的右反射点的距离；

步骤S3：根据所述重力加速度的分量值来计算所述被测量设备的滚转角和俯仰角θ；

步骤S4：利用左激光测距传感器到左反射点的距离和右激光测距传感器到右反射点的距离值，计算被测量设备的偏航角ψ。

8.如权利要求7所述的地下方位测量方法，其特征在于，所述滚转角测量装置根据如下公式计算被测量设备的滚转角

其中，[g_x g_y g_z]^T是重力加速度在被测设备联体坐标系O₁X₁Y₁Z₁三个轴上的分量值。

9.如权利要求7所述的地下方位测量方法，其特征在于：

所述滚转角测量装置根据如下公式计算被测量设备的俯仰角 $\mathrm{\θ}:$ $\mathrm{\θ}=-\arctan (g_x/\sqrt{g_y^2+g_z^2}),$ 其中，[g_x g_y g_z]^T是重力加速度在被测设备联体坐标系O₁X₁Y₁Z₁三个轴上的分量值。

10.如权利要求7所述的地下方位测量方法，其特征在于：所述中央控制器根据如下公式计算被测量设备的偏航角ψ：ψ＝arctan(Δd/D_AB)，其中D_AB为所述左激光测距传感器和右激光测距传感器的中心距，而Δd＝d_L-d_R。

Images