基于载波相位差分技术的水平定姿挖掘装置

张 帆， 刘 瑾， 杨海马， 肖 俊， 何施晶， 袁宝龙

(1.上海工程技术大学 电子电气工程学院，上海 201620； 2.上海理工大学 光电信息与计算机工程学院，上海 200093； 3.上海淮科智能科技有限公司，上海 201900)

随着科学技术的不断进步，定位辅助系统已经得到了越来越广泛的应用。从近几年的发展趋势来看，挖掘机自动化作业成为挖掘机研究领域的主要方向，国内外一些工程机械龙头企业和高校对此做了大量的研究工作。在卫星导航中，辅助定位通常作为一种辅助定位方式，在连续性、精度等方面对卫星导航进行增强。但是在挖掘设备中，定位辅助技术受到了一定的限制[1-12]。挖掘机自身定位和工作装置姿态获取是挖掘机自动化研究中不可或缺的环节[13-15]。装置姿态的测量工作多采用光电编码器、位移传感器或倾角传感器[16-17]。然而挖掘机在进行挖掘作业过程中，由于地形的复杂性，工作装置不可避免地会与土壤、岩石等发生碰撞，容易对传感器造成一定程度上的损坏[18]。基于此，笔者提出一种基于载波相位差分技术的挖掘设备水平定姿定位辅助方法，弥补了传统姿态测量系统的不足，可安全、可靠、准确地获取工作装置姿态。

1 姿态测量方法

挖掘头的定姿施工控制模块包括挖掘头的实时坐标定位和定姿施工保持。其中，挖掘头的实时坐标定位是以基准站为原点O建立三维坐标系，将挖掘头的关节坐标变换与空间GPS定位相结合，用户主机根据挖掘机头所处的坐标点相对于任意两个不同方位点的距离变化，计算挖掘头的实时坐标信息，从而定位挖掘头的实时坐标；定姿施工保持是在关节坐标运算后，用户主机控制挖掘设备的主臂、次臂做出相应动作以维持挖掘头始终保持固定的工作姿态，垂直施工。

对于关节坐标运算，采取的坐标变换将关节坐标变换与GPS相结合，推导出一个完整的定姿公式，能够准确定位挖掘头的姿态信息，从而方便驾驶室的施工人员通过用户主机控制主次臂关节精准、灵活地定位挖掘。姿态测量示意图如图1所示。

图1 姿态测量示意图

GPS中,a为椭球元素，设基准站于点O，任一点A为方位点。设A=(XA，YA，ZA)，此时A的大地坐标为(BA，LA，hA)，即

(1)

(2)

(3)

其中，式(1)～式(3)可以解出任一点A的坐标(XA，YA，ZA)。

设基准站于点O，任一点B为方位点，设B=(XB，YB，ZB)，此时B的大地坐标为(BB，LB，hB)，同理可得：

(4)

(5)

(6)

其中，由式(4)～式(6)可以解出任一点B的坐标(XB，YB，ZB)。

如图1所示，设任一方位点A与M点之间的距离为DAM，任一方位点B与M点之间的距离为DBM，挖掘机头所处的坐标为M=(X0，Y0，Z0)。

由两点间的距离公式：

(7)

(8)

(X0-XA)(YB-YA)=(Y0-YA)(XB-XA)

(9)

即可得：

(10)

(11)

(12)

其中，结合式(10)～式(12)可以得到挖掘机头所处的坐标点(X0，Y0，Z0)。向量AB确定的角度记为角度θ。由图1姿态图可知，θ为AB在xoy平面上的投影与y轴的夹角，其计算公式为

(13)

结合关节坐标变换与GPS，可推导出上述定位姿态公式，从而确定挖掘头的位置姿态信息。

2 系统设计

2.1 系统组成

挖掘机系统由主臂、次臂、GPS装置、通信线缆、驾驶室、用户主机、旋转车体、挖掘机底盘和铲斗组成。本辅助系统的核心为GPS加上载波相位差分系统以及姿态传感装置。为保证系统可靠性，同时具有友好的人机接口，驾驶室中利用工控一体机显示屏辅助显示当前挖掘头的空间姿态，指导挖掘工作。挖掘机完成一次挖掘任务的空间路径与挖掘机的工作参数有关。挖掘深坑时，铲斗提升、回转以及运行至下一起始点时，属于空间PTP(Point to Point)运动，轨迹规划比较简单[19-20]。

设备工作流程图如图2所示。

图2 设备工作流程图

设备工作时，首先打开上电开关，查看通信状态灯，闪烁则表示连接正常。GPS工程挖掘头已接收正常信号，实时通信状态指示。空间 GPS进行动作测试，触发动作测试键，主臂、次臂做对应动作，姿态传感器指示正常，则表示线路正常。开始对施工区域进行网格设置工作：① 施工人员可以用画图笔在液晶屏的水平网格面上设定施工平面，形成对应实际施工区域的工作网格；② 工作人员可以手持移动GPS终端(即流动站)设定位置保护点；③ 支持不规则施工区的设定。设置工作网格结束后，单击“开始”按钮进行辅助指示工作；实时指示当前挖掘头所处的网格位置。运用点阵液晶显示模块进行平板坐标辅助显示，用户主机有效地将定位空间识别定位在水平面网格中，再根据流动站的观测量在水平面网格内将实际的施工区域对应设置为工作网格，可以设置位置保护点，便于施工人员完成网格辅助设计、布局；然后用户主机根据流动站的观测量将挖掘头的实时坐标在水平面网格中显示其所处的当前网格位置，通过用户界面可完成网格辅助设计、布局，以及施工指示、越界提醒。此外，用户主机还根据姿态传感器的监控信息控制挖掘设备的主臂、次臂做出相应动作以使铲斗始终保持固定的工作姿态，垂直施工，从而能够在短时间内高效地对施工区域进行挖掘，有效地完成施工设备挖掘头空间姿态的定位指示功能。

2.2 差分定位系统组成

GPS-5 Hz发送GPS位置，PC接收数据为Hex数据，波特率为115200，8数据位，无奇偶校验，1停止位，总长138 B。

如图3所示，采用GPS加上载波相位差分系统进行定位，基准站的接收机执行两个操作：① 接收GPS卫星信号；② 把采集的载波相位观测量发送给流动站；流动站的接收机执行3个操作：① 接收GPS卫星信号；② 接收基准站所发送的基准站观测量；③ 对卫星信号与基准站观测量进行对比处理，从而对观测量进行修正并传送给用户主机。

图3 差分定位系统

2.3 软硬件设计

本系统上位机软件通过LabVIEW 平台开发，有4个子界面，分别为设置、GPS数据、GPS图表和手册指南，如图4所示。

图4 软件挖掘检测界面

硬件采用的是三菱FX3U-485ADP-MB的PLC，并采用Modbus协议。后台采用Modbus DTU协议，8位数据，偶校验，1个停止位，波特率为19200。表1和表2分别表示PC发送的数据地址以及PLC后台的返回数据。

表1 PC发送数据

表2 PLC后台返回数据

3 实验与结果分析

选择一块空旷施工区域进行试验，挖掘地段需要考虑崎岖不平、深浅未知等因素。基于挖掘的特殊性和生产工艺的独特性，研究基于载波相位差分技术的水平定姿挖掘装置，系统包括施工区域和挖掘头的空间GPS定位模块、挖掘头的定姿施工控制模块和挖掘空间位置辅助指示模块。

为了保证施工挖掘高效顺利进行，挖掘机操作室服务器端已经提供挖掘头当前位置下方的路段地质数据、采场边界和相关测量数据等。现场应用测试时，只需验证挖掘机坐标和铲斗的坐标准确性。该系统利用载波相位差分系统进行定位，完成了整个挖掘机的精确定位和工作装置的姿态监测。

进行现场验证时采用标准差分系统，在相同坐标系统、相同差分信息、相同坐标转换参数下进行测量对比验证，验证结果如表3和表4所示。

表3 挖掘设备终端监测挖掘机平面坐标(WGS-84)

表4 载波相位差分技术测量挖掘机平面坐标(WGS-84)

挖掘机的平面坐标可以用它与本初子午线与0°纬线的相对位置(°)来表示。根据表3和表4挖掘机设备平面坐标(WGS-84)对比可知，挖掘机平面坐标终端检测与利用载波相位差分技术测量的坐标数值非常接近，检测经纬度在秒级稍有差别，经度(E)最大相差0.07″，纬度(N)最大相差0.08″，检测精度非常高。

对比表5和表6所示的挖掘机终端监测铲斗坐标与利用载波相位差分技术测量的坐标可以看出，铲斗三维坐标数值平面位置相差最大值为 0.085 m，垂直相差最大值为0.091 m，满足生产应用的需求，表明该装置应用数据可靠。

表5 挖掘设备终端监测铲斗坐标(北京 54)

表6 利用载波相位差分技术测量铲斗坐标(北京 54)

对挖掘设备终端监测与提出的载波相位差分方法的结果进行比较，三维拟合区别不明显。通过Matlab软件对X(m)、Y(m)、H(m)坐标分别进行拟合处理,由于数据数值比较大(超过6位)，拟合不方便，因此只对十分位进行数据拟合对比，效果明显。

通过Matlab软件，对X坐标进行线性拟合，挖掘设备终端监测铲斗X坐标线性拟合方程为

f(X)=0.06191X+0.3476

利用载波相位差分技术,铲斗X坐标线性拟合方程为

f′(X)=0.0231X+0.3179

同理，对Y坐标通过两种方法得到的线性拟合方程分别为

f(Y)=0.03776Y+0.4244f′(Y)=0.04439Y+0.4129

对H坐标的两种方法得到的线性拟合方程分别为

f(H)=-0.1216H+0.359f′(H)=-0.09705H+0.3415

相关拟合结果如图5～图7所示。图中，X1、Y1、H1分别表示挖掘设备终端监测铲斗的X坐标、Y坐标和H坐标；X2、Y2、H2分别表示利用载波相位差分技术测量铲斗的X坐标、Y坐标和H坐标。

图5 水平位置(X坐标)结果比较

图6 水平位置(Y坐标)结果比较

图7 高度位置结果比较

由图5～图7可以看出，其中无论是铲斗三维坐标数值平面位置还是高度定位误差，均优于0.1 m，达到目前国内领先水平。试验结果表明基于载波相位差分技术的水平定姿挖掘装置是可行的，且实时性和高效性满足挖掘机自动化控制的要求。

4 结束语

基于载波相位差分技术定姿的基本原理，实现对挖掘机的精确定位以及工作装置的姿态检测。整套装置进行施工作业时可保证挖掘机平面坐标以及铲斗坐标准确，定位精度高，包括对目标位置、铲斗位置、施工边界、挖掘机位置和姿态及施工范围图形化显示。本装置满足了现在工业施工中挖掘机定位高精度、高实时性和高可靠性的需求，显著提高了挖掘效率，为科学开展工农业挖掘工作提供了一种可供参考的技术方案。