CN102322857A - 机动设备位姿测量***及测量方法 - Google Patents

Abstract

本发明涉及一种机动设备位姿测量***，其特点是：在机动设备后方巷道顶部安装四个激光测距模块，使其测量机动设备上特定位置的点获得四个不同的测距距离，由这四个距离计算机动设备的相对预定轨迹的位置偏移以及在垂直面上的角度偏移，并把偏移数据通过无线通信模块传送到机动设备上的中央处理***，结合在机动设备上安装三轴加速度传感器测量机动设备相对水平面的倾角，可以得到机动设备的位姿参数，并可获得定位导航数据，中央处理***通过CAN总线将定位导航数据传送到机动设备控制箱。本发明测量速度快，实时性好，测量精度高，无累积误差，适用于矿井下机动设备的位姿测量及定位导航。

Description

机动设备位姿测量***及测量方法

技术领域

本发明属于一种用于煤矿开采，隧道挖掘等环境下的测量***，特别是一种机动设备位姿测量***及测量方法。

背景技术

井下机动设备的定位导航是井下巷道机动设备行进、作业时的一个关键问题。目前国内井下机动设备的定位采用激光指向仪给出预定运动轨迹，井下设备运动时依靠工作人员肉眼观察机动设备与指向仪指示方向的相对位置关系来决策机动设备的动作数据。当机动设备稍微偏离预定轨迹时，人眼不能及时发现，同时由于井下环境恶劣，工作面粉尘量大，光线弱，且工作人员的视线易受其他物体遮挡，种种原因导致机动设备很难沿预定轨迹运动，且存在较大偏差，这将导致机动设备运动轨迹不定，行进效率低下，人工劳动强度大等。

为解决以上问题，实现机动设备远程控制和自动控制，必须获取机动设备的位姿信息。公开号为CN201013380，名称为“全自动掘进机”的实用新型专利提出了一种采用航天导航定位仪测量掘进机的位置，但是该专利没有测量掘进机的机身姿态；公开号为CN101629807A，名称为“掘进机机身位姿参数测量***及其方法”的发明专利提出利用扇形激光束在掘进机身形成线性光斑，依靠带有光敏元件的激光标靶来产生电流信号，经过处理后得出掘进机的偏向角和偏向位移，但是该方法在扇形激光束的获取上很有难度，且扇形激光束的加工工艺和精度也难以保证。

发明内容

本发明的目的是解决机动设备行进时机身位姿测量问题，并为机动设备提供沿预定轨迹自动行进的数据，而提供一种机动设备位姿测量***及测量方法。

为了实现上述目的，本发明所采用的技术方案是：

一种机动设备位姿测量***，包括：激光测距模块安装机架，激光测距模块，中央处理***，ARM微控制器，无线通信模块，平面激光反射面，锥形激光反射面，三轴加速度传感器，其特征在于：

激光测距模块：安装在激光测距模块安装机架上，激光测距模块用于测量机动设备的垂直面上的偏离数据和运动距离；

ARM微控制器：安装在机动设备和激光测距模块安装机架上；

无线通信模块：安装在机动设备和激光测距模块安装机架上，用于将激光测距模块的数据传输到中央处理***；

平面激光反射面及锥形激光反射面：安装在机动设备尾部；

激光测距模块安装机架：安装在机动设备后方巷道顶部；

三轴加速度传感器：安装在机动设备上，利用其垂直轴的测量数据来辅助获取机动设备相对水平面倾角；

中央处理***：用于采集激光测距模块和三轴加速度模块的数据并进行数学处理，获得机动设备行走数据，并通过CAN总线将行走数据传送到机动设备控制箱中。

所述激光测距模块安装机架距离机动设备距离为1m-200m。

所述激光测距模块有四个，每两个激光测距模块为一组，每个激光测距模块的激光射出方向为机动设备预定运动方向，其中第一激光测距模块和第二激光测距模块为一组，第一激光测距模块和第二激光测距模块相互平行，且间距为0.5m，第三激光测距模块和第四激光测距模块为一组，第三激光测距模块和第四激光测距模块相互平行，且间距为0.5m。

所述平面激光反射面位于第一激光测距模块和第二激光测距模块发出的激光能投射到的位置，反射面颜色为白色或者橙黄色。

所述锥形激光反射面位于第三激光测距模块J 3和第四激光测距模块J4发出的激光能投射到的位置，反射面颜色为白色或者橙黄色。

所述机动设备位姿测量***的测量方法是：使用第一激光测距模块和第二激光测距模块测量激光测距模块安装机架到机动设备上平行反射面的距离，根据两个距离之差以及第一激光测距模块和第二激光测距模块的间距换算出机动设备的水平偏向角度；使用第三激光测距模块和第四激光测距模块测量激光测距模块安装机架到机动设备上锥形反射面的距离，根据两个距离之差以及第三激光测距模块和第四激光测距模块间距算出机动设备的水平偏向位移。

本发明机动设备的位姿测量及定位导航***与传统技术相比，其优点和积极效果在于：

(1)本发明使用激光测距模块，加速度传感器，平行和锥形激光反射面和ARM微控制器，完成机动设备位姿测量和运动导航，可获取机动设备相对其预定轨迹的夹角和偏移距离。本测量***的测量精度可达到角度精度在7°以内，偏移距离精度在5cm以内，且不存在误差累积过程。

(2)本发明机动设备的位姿测量***，可根据机动设备的位姿参数获取机动设备的行走数据，实现机动设备自动化控制，进一步可实现多机联动。

本发明尤其适用于井下机动设备的远程控制和自动控制，利于实现控制台后移，多机联动，提高作业质量和作业速度，为最终实现机动设备无人化和自动化奠定基础。

附图说明

图1为本发明的原理框图。

图2为本发明的机动设备俯仰角示意图。

图3为本发明的机动设备偏向角和偏向位移示意图。

图4为本发明的机动设备侧偏角示意图。

图5为本发明的安装示意图。

图6为本发明的偏向角测量示意图。

图7为本发明的偏向位移测量示意图。

图中：1-中央处理***、2-三轴加速度传感器、3-无线模块、4-锥形反射面、5-平面反射面、6-激光测距模块安装机架、7-激光测距模块、8-ARM微控制器、9-机架校正仪、10-激光指向仪。

具体实施方式

本发明在上述基础上，以下通过描述测量***的安装方法和测量方法来具体说明本发明的技术实施方案。本发明只提供机动设备的位姿测量***和导航定位***及其测量方法，不提供有关软件程序。

本发明的安装方法：

a如图1所示在机动设备后方安装激光测距模块支架，调整激光发射方向保证其与机动设备运动预定轨迹相同；

b在激光测距模块支架上安装两组，每组两个相互平行的激光测距模块，同时安装一ARM微控制器和一个无线通信模块。ARM微控制器将四个测距模块的数据通过无线通信模块传送到中央处理***。所述激光测距模块为，所述ARM微控制器型号为STM32F103C8，所述无线通信模块使用的芯片型号为SI4432；

c在机动设备尾端安装激光反射面，激光反射面分为一个平面和一个锥形面，安装时保证两组激光中一组照射到平面上，另一组照射到锥形面上。激光反射面材质为硬质钢铁，合金或塑料等，温湿度剧烈变化不会产生明显形变。激光反射面颜色为白色、橙色等反射率高的颜色；

d在机动设备中心部位安装一个三轴加速度模块。所述加速度模块包含一个三轴加速度传感器和一ARM微控制器。所述三轴加速度传感器型号为ADXL345，所述ARM微控制器为STM32F103C8；

e在机动设备中心部位安装一个中央处理******。所述中央处理***包含一个ARM处理器，一个无线通信模块，一个485总线收发器，一个CAN总线收发器。所述ARM处理器为STM32F103ZE，所述无线通信模块使用的芯片型号为SI4432，所述485总线收发器为SP3485，所述CAN收发器为SJA1000。

下面结合附图来描述本发明的测量方法及定位导航原理：

为了方便描述，作如下定义：如图2，定义机动设备前后方向中轴线与巷道前进方向中轴线在水平面上的夹角为俯仰角α；如图4，定义机动设备横向中轴线与巷道前进方向中轴线在水平面上的夹角为侧偏角β；如图3，定义机动设备前后方向中轴线与巷道前进方向中轴线在垂直面上的夹角为偏向角γ；定义机动设备前后方向中轴线与巷道前进方向中轴线在水平面上的偏移距离为偏向位移d。

如图6所示，第一激光测距模块和第二激光测距模块平行的照射到安装于机动设备尾部的平行反射面上，激光测距模块获得距离数据为D₁、D₂，两者距离差为ΔS，两束激光间的距离为S₁，那么可以计算出偏向角为γ＝arctan(ΔS/S₁)；

如图7所示，第三激光测距模块和第四激光测距模块平行的照射到安装于机动设备尾部的锥形反射面上，两束激光间的距离为S₁，锥形面俯视图为等腰三角形，其底角为θ。激光测距模块获得测距数据为D₃、D₄，两侧差为Δd，通过调整锥形反射面的安装位置，使得机动设备不存在偏向位移和偏向距离时Δd＝0，当Δd≠0时，机动设备存在偏向位移，其计算方法为d＝Δd/(2*arctanθ)。当机动设备存在偏向角时，获取的数据需要修正，修正方法为 $d=\frac{2\mathrm{\Δ}d\cos (\mathrm{\θ}+\mathrm{\γ})\cos (\mathrm{\θ}-\mathrm{\γ})-s_1\sin \left(2\mathrm{\γ}\right)}{\sin \left(2\mathrm{\θ}\right)}.$

三轴加速度传感器能输出传感器在三维空间三个轴向上的加速度。若重力加速度在传感器水平面上纵向轴上的加速度分量为g₁，在传感器水平面上横向轴上的加速度分量为g₂，在垂直轴上的加速度分量为g₃，机动设备静止时，计算本地重力加速度的大小

当传感器无倾角且机动设备静止时，g₁＝g₂＝0，g₃为重力加速度g大小；当存在倾角时，可根据g₁和g₂大小确定俯仰角和侧偏角，俯仰角α＝arctan(g₁/g)，β＝arctan(g₂/g)；当机动设备运动时，需要修正俯仰角和滚动角的计算公式。

中央处理***使用高性能ARM微处理器，通过无线通信模块获取机动设备的偏向角、偏向位移和移动距离，通过CAN总线获取机动设备的俯仰角和侧偏角，即可得出机动设备的位置参数和姿态参数，然后通过CAN总线将机动设备的行进数据传送到机动设备的控制箱。

本说明书中未做详细描述的内容属于本领域中专业技术人员公知的现有技术。

Claims (6)

1.一种机动设备位姿测量***，包括：激光测距模块安装机架，激光测距模块，中央处理***，ARM微控制器，无线通信模块，平面激光反射面，锥形激光反射面，三轴加速度传感器，其特征在于：

激光测距模块：安装在激光测距模块安装机架上，激光测距模块用于测量机动设备的垂直面上的偏离数据和运动距离；

ARM微控制器：安装在机动设备和激光测距模块安装机架上；

无线通信模块：安装在机动设备和激光测距模块安装机架上，用于将激光测距模块的数据传输到中央处理***；

平面激光反射面及锥形激光反射面：安装在机动设备尾部；

激光测距模块安装机架：安装在机动设备后方巷道顶部；

三轴加速度传感器：安装在机动设备上，利用其垂直轴的测量数据来辅助获取机动设备相对水平面倾角；

中央处理***：用于采集激光测距模块和三轴加速度模块的数据并进行数学处理，获得机动设备行走数据，并通过CAN总线将行走数据传送到机动设备控制箱中。

2.如权利要求1所述的机动设备位姿测量***，其特征在于：激光测距模块安装机架距离机动设备距离为1m-200m。

3.如权利要求1所述的机动设备位姿测量***，其特征在于：激光测距模块有四个，每两个激光测距模块为一组，每个激光测距模块的激光射出方向为机动设备预定运动方向，其中第一激光测距模块和第二激光测距模块为一组，第一激光测距模块和第二激光测距模块相互平行，且间距为0.5m，第三激光测距模块和第四激光测距模块为一组，第三激光测距模块和第四激光测距模块相互平行，且间距为0.5m。

4.如权利要求1或3所述的机动设备位姿测量***，其特征在于：平面激光反射面位于第一激光测距模块和第二激光测距模块发出的激光能投射到的位置，反射面颜色为白色或者橙黄色。

5.如权利要求1或3所述的机动设备位姿测量***，其特征在于：锥形激光反射面位于第三激光测距模块和第四激光测距模块发出的激光能投射到的位置，反射面颜色为白色或者橙黄色。

6.如权利要求1所述的机动设备位姿测量***，其测量方法是：使用第一激光测距模块和第二激光测距模块测量激光测距模块安装机架到机动设备上平行反射面的距离，根据两个距离之差以及第一激光测距模块和第二激光测距模块的间距换算出机动设备的水平偏向角度；使用第三激光测距模块和第四激光测距模块测量激光测距模块安装机架到机动设备上锥形反射面的距离，根据两个距离之差以及第三激光测距模块和第四激光测距模块间距算出机动设备的水平偏向位移。

Images