机器人自主定位系统的制作方法

专利名称：机器人自主定位系统的制作方法
技术领域：
本发明属于机器人智能控制装置，具体涉及一种基于惯性传感器和 三维数字罗盘的组合自主定位系统，用于煤矿、隧道等地下环境智能机 器人的自主定位。
背景技术：
机器人定位技术是用于确定机器人在环境中的实时位姿、引导机器 人到达目标点的关键技术。在实际工作中，当机器人要自主实现某一预 期功能时，首要的是要对自身位姿进行实时监测，然后才能根据当前和 预期位姿之间的差异来规划机器人下一步的运动轨迹，并实时更新自己 的位姿。
机器人的定位技术经历了由单定位方式向组合定位方式转化的过程。当前应用较多的定位方式有GPS、图像匹配、惯导等，这些方式各 有所长。自主性和定位精度一直是评价定位系统的两个重要指标，目前 还没有一种定位方式能同时具有这两个优点。整合多种定位的优势，设 计新型组合定位系统是提高定位自主性和精度的主要途径。
智能机器人应用己经从传统的工业应用向矿井地图绘制、地下隧道 测量与施工、人员救援探测和定位等领域扩展，对定位系统提出了更高 要求，如自主性，实时性等。现有的定位理论和技术在人工环境下能提 供可靠而准确的机器人位姿信息，但对于存在大量未知信息的地下环境， 现有定位方法存在很多局限性，甚至无法工作。
地下环境作业智能机器人对其定位系统的自主性提出了明确要求。 在地下非结构化环境中，无法接收到GPS信号，只能利用机器人自身内
部传感器来获取运动状态，再通过数据处理获取自身位姿。惯性传感器 不需要环境信息即可感知、测量机器人运动参数，完全自主、可全天候 工作、更新速度快、瞬时精度高。但是，惯性传感器存在误差随时间积 累快、使用前需初始对准的缺点，这限制了惯性传感器在需要长时间高 精度定位场合的应用。三维数字罗盘是一种根据地球磁场测量机器人三 维姿态的传感器，但它不能提供机器人运动加速度信息、信号输出频率 低、容易受环境干扰磁场影响，这些致命的缺点限制了三维数字罗盘的 广泛应用。
为解决机器人的高精度自主定位问题，传统上主要采用惯导与GPS 结合的定位方法。这种方法具有自主性强、定位精度高的特点。在实际
使用中，当GPS信号有效时，其定位效果良好。然而，在无GPS信号的 环境中，利用惯导虽然能实现自主定位，但其定位误差会随时间急剧累 积。图像匹配方法也是智能机器人定位中常用的方法，但这种方式需要 预先存储匹配图像或地图，然后才能通过图像匹配来确定机器人当前位 置。这些定位方式不适用于在地下非结构化环境内工作的机器人定位。 因此，研制针对地下环境的机器人自主定位系统是当前研究的一个难点 问题，也是智能机器人技术的一个重要发展方向。
自主定位技术是智能机器人能否顺利完成预期任务需具备关键核心 技术之一，现有的定位技术存在自主性和精度之间的矛盾，不能满足地 下环境作业要求。因此，针对地下环境的特点，结合智能控制和机器人 技术发展的最新进展，研制具有自主性的定位系统是机器人定位导航技 术发展的一个重要方向，也是提高机器人智能水平的一个重要手段。

发明内容
本发明提供一种机器人自主定位系统，解决机器人在地下环境作业 的高精度自主定位问题。本发明的一种机器人自主定位系统，包括安装平台、设置于安装平 台上的传感器子系统、数据处理子系统以及电压转换模块，安装平台同 时为传感器子系统标定提供所需的采样点；传感器子系统实时感测机器 人运动状态信息，经数据处理子系统进行去噪、滤波、融合和计算，得 出机器人的位姿和速度信息；电压转换模块将车载直流电源转换为所需 的工作电压；其特征在于
所述安装平台上部为承载传感器子系统的转台，转台运动范围为 ±360°，由电机驱动模块驱动，运动误差控制在土0.1。以内；安装平台下部 的固定基座为密封腔式结构，内部安装数据处理子系统和电机驱动模块；
所述安装平台具有锁紧装置，锁紧装置的一端与转台底部固定连接， 锁紧装置另一端通过可分离螺杆与安装平台下部的固定基座锁紧；在安 装平台上转台需要运动时，螺杆与安装平台下部固定基座分离，转台旋 转，传感器子系统标定结束后，螺杆与安装平台下部固定基座锁紧；
所述传感器子系统包括惯性传感器模块和三维数字罗盘，惯性传感 器模块由三维加速度计及固联在其三个敏感轴上相互正交的光纤陀螺仪 组成，测量惯性传感器模块参考坐标系相对于地球惯性坐标系运动角速 度和加速度；三维数字罗盘测量三维数字罗盘参考坐标系相对地理坐标 系的三维姿态；
所述数据处理子系统包括中央控制器及装载于其内部的数据预处理 模块、数据融合模块和航迹推算模块；中央控制器具有多路RS-232串口 和以太网网络接口 ，利用这些接口分别与传感器子系统和外部设备进行 通信；数据预处理模块采用小波变换方法分别对光纤陀螺仪、三维加速 度计和三维数字罗盘的数据预处理，降低数据噪声、隔离振动与冲击； 数据融合模块利用卡尔曼算法融合光纤陀螺仪和三维数字罗盘经过预处 理的数据，获取安装平台的三维姿态信息；航迹推算模块根据安装平台
所在地的经纬度和引力场向量、初始速度向量或各个时刻的速度和位姿 向量，采用相对定位方式计算并更新安装平台实时位姿和速度信息。
所述的机器人自主定位系统，其特征在于，所述数据预处理模块的 处理步骤为(1)对光纤陀螺仪、三维加速度计和三维数字罗盘的数据 进行多通道多分辨小波分解；(2)对前述分解数据中的高频分量，采取 强制消噪处理；(3)提取前述分解数据中的低频分量进行多通道多分辨 小波重构；(4)输出经预处理后的数据。
所述的机器人自主定位系统，其特征在于，所述数据融合模块的处 理步骤为(1)对光纤陀螺仪和三维数字罗盘的数据计算统计学参数； (2)实时更新卡尔曼滤波器参数；(3)自适应卡尔曼滤波；(4)计算自 主定位系统姿态参数，将其返回步骤(2)。
所述的机器人自主定位系统，其特征在于，所述航迹推算模块的处 理步骤为(1)初始化系统参数，包括初始点地理坐标系，地理坐标系 下机器人初始角速度向量、加速度向量、线速度向量、位置向量、当地 经纬度以及引力场；
(2) 建立机器人自主定位系统坐标系；
(3) 根据步骤(1)、 (2)，对机器人自主定位系统进行初始对准；
(4) 计算方向变换矩阵；
(5) 根据前一时刻机器人位姿和传感器数据预处理与融合更新，计 算当前时刻地理坐标系下机器人运动状态参数，包括地理坐标系下线加 速度向量、相对于地理坐标系的姿态向量、速度向量和位移向量；
(6) 机器人自主定位数据存储和更新，定位数据通过通信接口传输 到外部设备，更新后的速度向量、位移向量、姿态向量进入步骤(4)进行循环。
所述的机器人自主定位系统，其特征在于所述电机驱动模块由固 定在支座上的附编码盘的直流电机构成，直流电机通过传动轴与转台下 部固定联接；
所述的机器人自主定位系统，其特征在于所述安装平台主体材质 为铝合金，联接螺栓为铜质螺栓，除三维数字罗盘外安装平台的整体采 用磁性屏蔽材料包覆，以隔离电磁场对三维数字罗盘的干扰。
本发明利用惯性传感器和三维数字罗盘组合方式实现自主定位，主 要原理和过程如下 一方面，利用光纤陀螺仪测量机器人相对于地球惯 性坐标系的角速度向量，经方向矩阵变换和剔除地球自转角速度向量， 计算机器人相对于地理坐标系的角速度向量，然后对时间积分一次即可 获取机器人姿态向量；三维数字罗盘直接测量机器人在地理坐标系下的 姿态向量；然后采用数据融合方法计算机器人姿态向量。另一方面，三
维加速度计测量机器人相对于地球惯性坐标系的线加速度向量，经方向 矩阵变换和剔除有害加速度向量后得到机器人在地理坐标系下的线加速
度向量；根据前一时刻机器人的位姿、速度，对时间积分两次计算机器 人当前时刻的位置；不断更新传感器数据和重复计算过程循环，自主定 位系统即可实时获取机器人位姿。
传感器数据处理过程中，由于传感器测量的不确定性、对机器人运 动过程缺乏精确的描述、环境信息描述的缺失等原因，传感器数据被大 量的噪声污染；传感器数据中含有的误差和噪声在进行时间积分时会被 放大，机器人运动时产生的振动和冲击为传感器输出数据的无效分量， 这些因素降低了测量数据的可信度。三维数字罗盘在工作过程中，环境
中铁磁性材料和干扰电磁场都对三维数字罗盘测量的准确性产生严重影 响，尤其是电磁场的干扰对三维数字罗盘的干扰很难彻底消除。因此， 必须对传感器数据进行各种处理。数据处理子系统采用小波变换和卡尔 曼滤波组合的方法对传感器数据预处理和传感器数据融合后，经航迹推 算计算出机器人位姿，并根据不断更新的传感器数据循环上述计算过程，
即可实时获取机器人位姿信息；电压转换模块提供符合各子系统和模块
的工作电压。
本发明的自主定位系统主要针对应用于地下环境作业机器人自主定 位，可用于煤矿、隧道、溶洞等地下环境工作智能机器人(如机器人，盾 构机，地铁机车等)自主定位，为智能机器人在地下环境完成勘测、探险 和搜索等任务提供有效的位姿信息，解决了智能机器人在地下环境作业 的自主定位问题，为智能机器人完成预期任务提供了决策依据。


图1本发明总体框图2本发明安装平台结构示意图3数据处理子系统流程示意图4 (a)数据处理子系统数据预处理模块处理流程示意图； 图4 (b)数据处理子系统数据融合模块处理流程示意图； 图5航迹推算模块处理流程示意图； 图6电压转换模块原理图。
具体实施例方式
图1为本发明的总体框图，本发明包括安装平台、设置于安装平台 上的传感器子系统、数据处理子系统以及电压转换模块，数据处理子系 统的中央控制器l具有RS-232接口和以太网接口，利用这些接口，自主 定位系统将机器人定位信息实时传输到指定设备；中央控制器1内部装 载有数据预处理模块8、数据融合模块9和航迹推算模块10。传感器子 系统2包括惯性传感器模块3、三维数字罗盘4，惯性传感器模块3和三 维数字罗盘4内部分别集成了内部处理器，内部处理器将传感器测量数 据转换为计算机能直接传输和处理的数字信号，并通过RS-232C接口输 出到中央控制器1。惯性传感器模块参考坐标系由光纤陀螺仪和三维加速 度计的敏感轴构成，三维数字罗盘4的敏感轴构成的参考坐标系与惯性 传感器模块参考坐标系三个敏感轴始终平行，将自主定位系统参考坐标 系设置为与惯性传感器模块参考坐标系重合。因此，光纤陀螺仪、三维 加速度计和三维数字罗盘测量信息是机器人在自主定位系统参考坐标系 内的角速度向量、线加速度向量和姿态向量，然后通过数据处理子系统 实现机器人的自主定位功能。安装平台的电机驱动模块5接收中央控制 器1的控制信号，并转换为直流电机7的驱动电压，从而实现传感器标 定。电压转换模块6提供定位系统所需的工作电压。
本发明安装平台如图2所示，包括具有锁紧装置的转台、固定基座 和电机驱动模块。转台由盖板11和半封闭式铝筒12构成封闭空间，三 维数字罗盘4固定安装在盖板11上部。半封闭式铝筒12内部安装惯性 传感器模块3，惯性传感器模块3和三维数字罗盘4两种传感器参考坐标 系对应轴之间的平行度误差由制造和安装控制在±0.5°。转台在电机驱动 下能绕自身轴线作±360°范围内转动，转动误差控制在±0.1°。固定基座为 基座盖板13和底座14形成密封腔式结构，内部安装有电机驱动模块5 和中央控制器l，底座14可与机器人本体通过螺栓固联。电机驱动模块 5包括附编码盘的直流电机7、电机固定支座15、传动轴16，然后通过 传动轴16与转台的半封闭式铝筒12下部固定联接；锁紧装置17与半封 闭式铝筒12固联，然后通过螺钉与固定基座实现锁紧与解锁。安装平台、 除直流电机7外的电机驱动系统组件以及所有固联螺栓材质为非铁磁性
材料。除三维数字罗盘外，安装平台的转台、固定基座以及直流电机等 组件外部均包覆磁性屏蔽材料，以隔离惯性传感器、直流电机对三维数 字罗盘和中央控制器的电磁干扰。
如图3所示，数据处理子系统的中央控制器是实现传感器数据处理
的硬件基础，其内部装载有数据预处理模块8、数据融合模块9和航迹推 算模块10，通过对传感器原始数据进行预处理、融合以及航迹推算，实 时获取机器人在运动过程中的位姿信息。
数据预处理模块8的具体流程如图4 (a)所示；数据预处理模块的 目的是针对自主定位系统特点、使用环境和传感器精度水平、误差特点， 利用算法对传感器数据预处理，去除或削弱干扰噪声对传感器精度的影 响，从而为提高机器人定位精度做好准备。传感器数据预处理的主要方 法是多通道多分辨小波分解与重构，即采用多分辨小波分解，剔除传感 器数据中的高频分量，仅提取最终的低频分量重构传感器数据。经预处 理后，传感器数据具有真实反应环境和传感器随时间变化的特点，更能 反应机器人的运动状态。加速度计数据经数据预处理模块后，其高频噪 声已经大大降低或剔除，环境造成的振动和冲击干扰能有效被隔离。光 纤陀螺仪和三维数字罗盘预处理数据经数据融合模块融合，获得机器人 自主定位系统的姿态向量。
数据融合模块9的具体流程如图4 (b)所示；数据融合模块的核心 算法是自适应卡尔曼融合滤波算法，即根据传感器预处理数据及其统计 特性，利用最优估计的方法估算机器人姿态信息。经传感器数据预处理 和融合后，传感器数据精度得到很好的改善，由此计算出的自主定位系 统的精度大大提高。
如图5所示，机器人自主定位系统的航迹推算模块10是一个实时更 新和循环计算模块，其主要功能是根据预先设定的机器人初始位姿、地 理环境(包括经度、纬度、引力场等)、上一时刻机器人速度和位姿信息以
及传感器更新数据等，实时计算机器人相对于初始点的位姿信息，并不 断更新机器人位姿。航迹推算模块采用相对定位方式，输出的是机器人 相对于初始点的位姿。航迹推算模块的推算过程如下
(1) 初始化系统参数，包括初始点地理坐标系，地理坐标系下机器 人初始角速度向量、加速度向量、线速度向量、位置向量、当地经纬度 以及引力场等；
(2) 建立机器人自主定位系统坐标系；
(3) 根据(1)、 (2)，对机器人自主定位系统进行初始对准；
(4) 计算方向变换矩阵；
(5) 根据前一时刻机器人位姿和传感器数据预处理与融合更新，计 算当前时刻地理坐标系下机器人运动状态参数，包括地理坐标系下线加 速度向量、相对于地理坐标系的姿态向量、速度向量和位移向量；
(6) 机器人自主定位数据存储和更新，定位数据通过通信接口传输 到外部设备，更新后的速度向量、位移向量、姿态向量进入步骤(4)进 行循环。
如图6所示，电压转换模块为地下环境机器人自主定位系统提供满 足要求的电压。该自主定位系统可使用电压范围为+24V +35V的车载直 流电源，自主定位系统内部各传感器、中央控制器以及电机驱动模块等 所需的电源电压为+5V、 +12V两种。电压转换模块通过标准联接头 CON2、开关S,与车载直流电源相连，经7812芯片U,和7805芯片U2 提将车载直流电源分别转换为+12V和+5V电压，其中图中GND为共地
上山顿。
本实例的具体实施例
机器人自主定位系统外形最大外形尺寸为250mmx250mmx320mm (长x宽x高)。
惯性传感器模块采用北京星网迅达科技发展有限公司研发的XW
VG7200。
三维数字罗盘美国Honeywell公司的HMR3000三维数字罗盘。 中央控制器采用研华科技股份有限公司的PCM-3341嵌入式系统，
该系统具备数据处理的软硬件条件，且具有4个RS-232接口和1个以太
网接口。数字I/O模块PC3730提供A/D转换接口。
电压转换模块:采用7805芯片和7812芯片为自主定位系统提供+5V、
+12V电压。
电机驱动模块采用附带旋转编码器的直流电机，直流电机的工作
电压为+12V。电机的控制信号来自处理器的A/D接口，经驱动电路驱动 电机。
数据处理子系统利用多分辨小波变换重构传感器数据低频量的方
法，有效剔除噪声和干扰。光纤陀螺仪和三维数字罗盘数据经预处理后 由卡尔曼融合滤波算法融合，获得机器人自主定位系统的姿态向量，并 作为下一时刻方向变换矩阵的姿态参数。加速度计数据经预处理和方向 变换矩阵后进入航迹推算模块。而且，随着传感器数据的更新，数据处 理子系统不断循环，实时计算机器人位姿。
权利要求
1.一种机器人自主定位系统，包括安装平台、设置于安装平台上的传感器子系统、数据处理子系统以及电压转换模块，安装平台同时为传感器子系统标定提供所需的采样点；传感器子系统实时感测机器人运动状态信息，经数据处理子系统进行去噪、滤波、融合和计算，得出机器人的位姿和速度信息；电压转换模块将车载直流电源转换为所需的工作电压；其特征在于所述安装平台上部为承载传感器子系统的转台，转台运动范围为±360°，由电机驱动模块驱动，运动误差控制在±0.1°以内；安装平台下部的固定基座为密封腔式结构，内部安装数据处理子系统和电机驱动模块；所述安装平台具有锁紧装置，锁紧装置的一端与转台底部固定连接，锁紧装置另一端通过可分离螺杆与安装平台下部的固定基座锁紧；在安装平台上转台需要运动时，螺杆与安装平台下部固定基座分离，转台旋转，传感器子系统标定结束后，螺杆与安装平台下部固定基座锁紧；所述传感器子系统包括惯性传感器模块和三维数字罗盘，惯性传感器模块由三维加速度计及固联在其三个敏感轴上相互正交的光纤陀螺仪组成，测量惯性传感器模块参考坐标系相对于地球惯性坐标系运动角速度和加速度；三维数字罗盘测量三维数字罗盘参考坐标系相对地理坐标系的三维姿态；所述数据处理子系统包括中央控制器及装载于其内部的数据预处理模块、数据融合模块和航迹推算模块；中央控制器具有多路RS-232串口和以太网网络接口，利用这些接口分别与传感器子系统和外部设备进行通信；数据预处理模块采用小波变换方法分别对光纤陀螺仪、三维加速度计和三维数字罗盘的数据预处理，降低数据噪声、隔离振动与冲击；数据融合模块利用卡尔曼算法融合光纤陀螺仪和三维数字罗盘经过预处理的数据，获取安装平台的三维姿态信息；航迹推算模块根据安装平台所在地的经纬度和引力场向量、初始速度向量或各个时刻的速度和位姿向量，采用相对定位方式计算并更新安装平台实时位姿和速度信息。
2. 如权利要求l所述的机器人自主定位系统，其特征在于，所述数 据预处理模块的处理步骤为(1)对光纤陀螺仪、三维加速度计和三维 数字罗盘的数据进行多通道多分辨小波分解；(2)对前述分解数据中的 高频分量，采取强制消噪处理；(3)提取前述分解数据中的低频分量进 行多通道多分辨小波重构；(4)输出经预处理后的数据。
3. 如权利要求l所述的机器人自主定位系统，其特征在于，所述数 据融合模块的处理步骤为(1)对光纤陀螺仪和三维数字罗盘的数据计 算统计学参数；(2)实时更新卡尔曼滤波器参数；(3)自适应卡尔曼滤 波；(4)计算自主定位系统姿态参数，将其返回步骤(2)。
4. 如权利要求l所述的机器人自主定位系统，其特征在于，所述航 迹推算模块的处理步骤为(1)初始化系统参数，包括初始点地理坐标 系，地理坐标系下机器人初始角速度向量、加速度向量、线速度向量、 位置向量、当地经纬度以及引力场；(2) 建立机器人自主定位系统坐标系；(3) 根据步骤(1)、 (2)，对机器人自主定位系统进行初始对准；(4) 计算方向变换矩阵；(5) 根据前一时刻机器人位姿和传感器数据预处理与融合更新，计 算当前时刻地理坐标系下机器人运动状态参数，包括地理坐标系下线加 速度向量、相对于地理坐标系的姿态向量、速度向量和位移向量；(6) 机器人自主定位数据存储和更新，定位数据通过通信接口传输到外部设备，更新后的速度向量、位移向量、姿态向量进入步骤(4)进行循环。
5. 如权利要求l、 2、 3或4所述的机器人自主定位系统，其特征在 于所述电机驱动模块由固定在支座上的附编码盘的直流电机构成，直 流电机通过传动轴与转台下部固定联接；
6. 如权利要求5所述的机器人自主定位系统，其特征在于所述安 装平台主体材质为铝合金，联接螺栓为铜质螺栓，除三维数字罗盘外安 装平台的整体采用磁性屏蔽材料包覆，以隔离电磁场对三维数字罗盘的 干扰。
全文摘要
机器人自主定位系统，属于机器人智能控制装置，解决机器人在地下环境作业的高精度自主定位问题。本发明包括安装平台、设置于安装平台上的传感器子系统、数据处理子系统以及电压转换模块；安装平台上部为承载传感器子系统的转台，安装平台下部的固定基座为密封腔式结构，内部安装数据处理子系统和电机驱动模块；传感器子系统包括惯性传感器模块和三维数字罗盘，数据处理子系统包括中央控制器及装载于其内部的数据预处理模块、数据融合模块和航迹推算模块；本发明可用于在煤矿、隧道、溶洞等地下环境工作的智能机器人，如机器人，盾构机，地铁机车等的自主定位，为智能机器人在地下环境完成勘测、探险和搜索等任务提供有效的位姿信息。
文档编号G05D1/02GK101201626SQ200710168718
公开日2008年6月18日 申请日期2007年12月10日 优先权日2007年12月10日
发明者尹周平, 熊有伦, 熊蔡华, 韩冬桂 申请人:华中科技大学