CN108643277A - 基于gnss-sins与位移传感器的挖掘机位置信息服务系统及方法 - Google Patents

Abstract

本发明公开了基于GNSS‑SINS与位移传感器的挖掘机位置信息服务系统及方法，挖掘机的下部为行走部分，所述行走部分的上部为上部平台，上部平台的前端设有工作装置，所述上部平台的上部设有GNSS‑SINS组合导航系统，服务方法包括：步骤一，设置当地地平坐标系、固连于挖掘机上部平台的车身坐标系及工作装置坐标系；步骤二，计算车身坐标系下动臂旋转轴至铲齿的坐标增量；步骤三，计算地平坐标系下的铲齿坐标。能够实时得到挖掘机各关键组件尤其是铲齿在地平坐标系下的三维坐标，提高施工效率和施工精度。

Description

基于GNSS-SINS与位移传感器的挖掘机位置信息服务系统及 方法

技术领域

本发明涉及挖掘机铲齿定位技术领域，尤其涉及基于GNSS-SINS与位移传感器的挖掘机位置信息服务系统及方法。

背景技术

在传统的挖掘机施工场景中，通常需要经过控制测量、施工放样、施工和检核精度等多个步骤。其中，控制测量和施工放样步骤对测量人员的技术经验水平和现场发挥有较高的要求；而施工步骤对挖掘机操作者的技术经验水平和现场发挥有较高要求。施工过程的多个步骤均受到参与者的个人因素影响，且从整体过程来看具有周期长、影响因素多、流程复杂的特点，这使得传统施工方法费时费事，人工成本高。现代传感器技术的发展使得空间信息技术可以方便地应用于诸多领域，并提供准确、可靠和实时的空间信息服务。

发明内容

本发明的目的就是为了解决上述问题，提供一种基于GNSS-SINS与位移传感器的挖掘机位置信息服务系统及方法，能够实时得到挖掘机各关键组件尤其是铲齿在地平坐标系下的三维坐标，提高施工效率和施工精度

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

基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法，挖掘机的下部为行走部分，所述行走部分的上部为上部平台，上部平台的前端设有工作装置，所述上部平台的上部设有GNSS-SINS组合导航系统，包括：

步骤一，设置当地地平坐标系、固连于挖掘机上部平台的车身坐标系及工作装置坐标系；

步骤二，计算车身坐标系下动臂旋转轴至铲齿的坐标增量；

步骤三，计算地平坐标系下的铲齿坐标。

所述当地地平坐标系以当地东、北、上方向为X、Y、Z轴，所述车身坐标系C_veh的X_veh,Y_veh,Z_veh分别平行于上部平台的右、前、上方向，所述工作装置坐标系C_wor的X_2D与Y_2D轴分别平行于C_veh的Y_veh轴和Z_veh轴。

所述步骤二的方法为：根据固定于动臂油缸、斗杆油缸及铲斗油缸的位移传感器计算得到在工作装置坐标系下动臂与上部平台之间旋转轴到铲齿的坐标增量。

4.如权利要求3所述基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法，其特征是，所述步骤二的具体方法为：利用函数C＝T(A,B)求出铲齿的具体坐标，函数C＝T(A,B)表示的含义是三角形ABC内由A和B点坐标求C点坐标，具体推算顺序为：

其中，P₁为动臂与上部平台连接的动臂旋转轴的坐标，P₂为动臂与斗杆间旋转轴的坐标，P₃为斗杆与铲斗间旋转轴的坐标，P₄为摇杆、连杆、铲斗油缸间旋转轴的坐标，P₅为摇杆与斗杆间旋转轴的坐标，P₆为铲斗与连杆间旋转轴的坐标，P₇为铲齿的坐标，P₈为动臂油缸与上部平台间旋转轴的坐标，P₉为动臂与动臂油缸间旋转轴的坐标，P₁₀为动臂与斗杆油缸间旋转轴的坐标，P₁₁为斗杆与斗杆油缸间旋转轴的坐标，P₁₂为斗杆与铲斗油缸间旋转轴的坐标。

所述步骤三包括：

步骤3.1，将工作装置坐标系下的二维坐标增量转换为车身坐标系下的从GNSS天线中心至铲齿的坐标增量

步骤3.2，利用GNSS-SINS组合导航系统输出的GNSS天线坐标、GNSS-SINS组合导航系统输出的车身坐标系相对于地平坐标系的旋转矩阵及所述步骤3.1求得的坐标增量求得地平坐标系下的铲齿坐标。

所述步骤3.1利用的公式为其中，为车身坐标系下由GNSS天线至P₁的坐标增量，P₁为下动臂与上部平台连接的动臂旋转轴的坐标，P₇为铲齿的坐标。

所述步骤3.2利用的公式为其中，pos_GNSS ^-SINS为组合导航系统输出的GNSS天线坐标，R_attitude为组合导航系统输出的车身坐标系相对于地平坐标系的旋转矩阵。

采用所述基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法的服务系统，所述GNSS-SINS组合导航系统包括GNSS定位设备、惯性测量单元和计算芯片，所述GNSS定位设备和惯性测量单元都与计算芯片连接；

所述GNSS定位设备包括相互连接的GNSS天线与GNSS接收机，用于测量天线在WGS-84坐标系下的坐标和速度；

所述动臂油缸上、斗杆油缸上及铲斗油缸上分别安装有位移传感器；

所述计算芯片和位移传感器都与中控器通信，所述中控器提供最终的位置信息服务。

所述惯性测量单元包括三部互相垂直安装的陀螺仪和与之分别同向安装的三部加速度计，用于在载体长时间静止时给出载体的姿态信息；在载体运动期间测量载体运动的加速度和角速度，并按照捷联惯性导航算法求得实时的载体姿态信息。

所述计算芯片为嵌入式MCU，用于按照捷联惯性导航算法计算惯性测量单元测得的载体位置、速度与姿态，并将此位置、速度与GNSS定位设备测得的结果进行组合以降低误差。

本发明的有益效果：

本发明运用多传感器融合技术实时解算出挖掘机各关键组件尤其是铲齿在地平坐标系下的三维坐标，在与合适的辅助施工系统结合后能够辅助操作者进行施工，降低施工流程复杂度和出错概率，提高施工效率和施工精度。

附图说明

图1为挖掘机整体、车身坐标系及上部平台各传感器示意图；

图2为挖掘机工作装置及位移传感器示意图；

图3为挖掘机工作装置简化及点号注记示意图。

其中，1.GNSS-SINS组合导航系统，2.工作装置，3.上部平台，4.行走部分，5.斗杆油缸，6.铲斗油缸，7.斗杆，8.摇杆，9.连杆，10.铲齿，11.铲斗，12.位移传感器，13.动臂油缸，14.动臂旋转轴，15.动臂。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

采用基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法的服务系统，如图1所示，挖掘机的下部为行走部分4，所述行走部分4的上部为上部平台3，上部平台3的前端设有工作装置2，所述上部平台3的上部设有GNSS-SINS组合导航系统1。

GNSS-SINS组合导航系统由GNSS定位设备、惯性测量单元和计算芯片等组成。

其中GNSS定位设备由GNSS天线与GNSS接收机组成，用于测量天线在WGS-84坐标系下的坐标和速度；

惯性测量单元主要由三部互相垂直安装的陀螺仪和与之分别同向安装的三部加速度计组成，用于(1)在载体长时间静止时给出载体的姿态信息(俯仰角、横滚角、航向角)，(2)在载体运动期间测量载体运动的加速度和角速度，并按照捷联惯性导航算法求得实时的载体姿态信息；

计算芯片主要由一部嵌入式MCU组成，用于按照捷联惯性导航算法计算惯性测量单元测得的载体位置、速度与姿态，并将此位置、速度与GNSS定位设备测得的结果进行组合以降低误差；

固定于动臂油缸的位移传感器12。本设备用于获得动臂油缸的长度信息；

固定于斗杆油缸的位移传感器12。本设备用于获得斗杆油缸的长度信息；

固定于铲斗油缸的位移传感器12。本设备用于获得铲斗油缸的长度信息；

所述计算芯片和位移传感器都与中控器通信，所述中控器用于执行位置信息服务方法，提供最终的位置信息服务。

如图2所示，工作装置2包括动臂15，动臂15与上部平台通过动臂旋转轴14连接，动臂15设有动臂油缸13，动臂15的上端与斗杆7的一端连接，斗杆7的另一端连接铲斗11连接，铲斗11上有铲齿10，铲斗11与连杆9的一端连接，摇杆8与斗杆7连接，同时摇杆8、斗杆7及铲斗油缸6连接在一起，动臂上还有斗杆油缸5。

基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法，包括：

步骤一，定义坐标系：以当地东、北、上方向为X、Y、Z轴建立当地地平坐标系；X_veh,Y_veh,Z_veh分别平行于上部平台的右、前、上方向，组成固连于挖掘机上部平台的车身坐标系C_veh；X_2D与Y_2D组成二维的工作装置坐标系C_wor，且两轴分别平行于C_veh的Y轴和Z轴。

步骤二，计算车身坐标系下动臂旋转轴至铲齿的坐标增量：根据三部位移传感器的输出量，可计算得到在C_wor下动臂与上部平台之间旋转轴到铲齿的坐标增量。令表示P_i与P_j两点在设计上的距离，令表示P_i与P_j两点连线的方向，令P_i＝[x_i x_j]^T表示点的坐标，令表示由三点组成的夹角，令L₁,L₂,L₃表示位移传感器测得的边长，即有：

在一个三角形ABC中，如果其中AB两点的坐标已知且abc三边的长度已知，则可按照下式求得C点坐标，其中atan2为C语言标准库中的求方位角函数：

将上述在三边已知三角形内由AB点坐标推C点坐标的过程简记为C＝T(A,B)。

接下来按照下列顺序进行坐标推算：

至此求得铲齿点P₇的坐标；如图3所示，其中，P₁为动臂与上部平台连接的动臂旋转轴的坐标，P₂为动臂与斗杆间旋转轴的坐标，P₃为斗杆与铲斗间旋转轴的坐标，P₄为摇杆、连杆、铲斗油缸间旋转轴的坐标，P₅为摇杆与斗杆间旋转轴的坐标，P₆为铲斗与连杆间旋转轴的坐标，P₇为铲齿的坐标，P₈为动臂油缸与上部平台间旋转轴的坐标，P₉为动臂与动臂油缸间旋转轴的坐标，P₁₀为动臂与斗杆油缸间旋转轴的坐标，P₁₁为斗杆与斗杆油缸间旋转轴的坐标，P₁₂为斗杆与铲斗油缸间旋转轴的坐标。

步骤三，计算地平坐标系下的铲齿坐标：首先将C_wor下的二维坐标增量转换为C_veh下的从GNSS天线中心至铲齿的坐标增量公式如下：

其中为C_veh下由GNSS天线至P₁的坐标增量。

令pos_GNSS-SINS为组合导航系统输出的GNSS天线坐标，令R_attitude为组合导航系统输出的车身坐标系相对于地平坐标系的旋转矩阵。按下式计算铲齿的地平坐标系坐标pos₇：

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (10)

1.基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法，挖掘机的下部为行走部分，所述行走部分的上部为上部平台，上部平台的前端设有工作装置，其特征是，所述上部平台的上部设有GNSS-SINS组合导航系统，包括：

步骤一，设置当地地平坐标系、固连于挖掘机上部平台的车身坐标系及工作装置坐标系；

步骤二，计算车身坐标系下动臂旋转轴至铲齿的坐标增量；

步骤三，计算地平坐标系下的铲齿坐标。

2.如权利要求1所述基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法，其特征是，所述当地地平坐标系以当地东、北、上方向为X、Y、Z轴，所述车身坐标系C_veh的X_veh,Y_veh,Z_veh分别平行于上部平台的右、前、上方向，所述工作装置坐标系C_wor的X_2D与Y_2D轴分别平行于C_veh的Y_veh轴和Z_veh轴。

3.如权利要求1所述基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法，其特征是，所述步骤二的方法为：根据固定于动臂油缸、斗杆油缸及铲斗油缸的位移传感器计算得到在工作装置坐标系下动臂与上部平台之间旋转轴到铲齿的坐标增量。

4.如权利要求3所述基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法，其特征是，所述步骤二的具体方法为：利用函数C＝T(A,B)求出铲齿的具体坐标，函数C＝T(A,B)表示的含义是三角形ABC内由A和B点坐标求C点坐标，具体推算顺序为：

其中，P₁为动臂与上部平台连接的动臂旋转轴的坐标，P₂为动臂与斗杆间旋转轴的坐标，P₃为斗杆与铲斗间旋转轴的坐标，P₄为摇杆、连杆、铲斗油缸间旋转轴的坐标，P₅为摇杆与斗杆间旋转轴的坐标，P₆为铲斗与连杆间旋转轴的坐标，P₇为铲齿的坐标，P₈为动臂油缸与上部平台间旋转轴的坐标，P₉为动臂与动臂油缸间旋转轴的坐标，P₁₀为动臂与斗杆油缸间旋转轴的坐标，P₁₁为斗杆与斗杆油缸间旋转轴的坐标，P₁₂为斗杆与铲斗油缸间旋转轴的坐标。

5.如权利要求1所述基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法，其特征是，所述步骤三包括：

步骤3.1，将工作装置坐标系下的二维坐标增量转换为车身坐标系下的从GNSS天线中心至铲齿的坐标增量

步骤3.2，利用GNSS-SINS组合导航系统输出的GNSS天线坐标、GNSS-SINS组合导航系统输出的车身坐标系相对于地平坐标系的旋转矩阵及所述步骤3.1求得的坐标增量求得地平坐标系下的铲齿坐标。

6.如权利要求5所述基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法，其特征是，所述步骤3.1利用的公式为其中，为车身坐标系下由GNSS天线至P₁的坐标增量，P₁为下动臂与上部平台连接的动臂旋转轴的坐标，P₇为铲齿的坐标。

7.如权利要求5所述基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法，其特征是，所述步骤3.2利用的公式为其中，pos_GNSS ^-SINS为组合导航系统输出的GNSS天线坐标，R_attitude为组合导航系统输出的车身坐标系相对于地平坐标系的旋转矩阵。

8.采用权利要求7所述基于GNSS-SINS与位移传感器的挖掘机位置信息服务方法的服务系统，其特征是，所述GNSS-SINS组合导航系统包括GNSS定位设备、惯性测量单元和计算芯片，所述GNSS定位设备和惯性测量单元都与计算芯片连接；

所述GNSS定位设备包括相互连接的GNSS天线与GNSS接收机，用于测量天线在WGS-84坐标系下的坐标和速度；

所述动臂油缸上、斗杆油缸上及铲斗油缸上分别安装有位移传感器；

所述计算芯片和位移传感器都与中控器通信，所述中控器提供最终的位置信息服务。

9.如权利要求8所述的服务系统，其特征是，所述惯性测量单元包括三部互相垂直安装的陀螺仪和与之分别同向安装的三部加速度计，用于在载体长时间静止时给出载体的姿态信息；在载体运动期间测量载体运动的加速度和角速度，并按照捷联惯性导航算法求得实时的载体姿态信息。

10.如权利要求8所述的服务系统，其特征是，所述计算芯片为嵌入式MCU，用于按照捷联惯性导航算法计算惯性测量单元测得的载体位置、速度与姿态，并将此位置、速度与GNSS定位设备测得的结果进行组合以降低误差。