专利名称：确定电采掘机铲的空间位姿的实时方法
技术领域：
本发明涉及对设备进行定位的领域，具体地，公幵了一种用于确定在釆掘操作中 使用的转臂式装载设备(如，电釆掘机铲)的空间位姿的系统。
背景技术：
参考文献Department of the Army，1993，FM 6-2. Tactics, Techniques，and Procedures for Field Artillery Survey, Department of the Army, Washington DC.Dizchavez, R. F.，2001，Two-antenna positioning system for surface-mine equipment，US Patent 6191733Gelb，A.，1996，Applied optimal estimation，The M. I. T. Press，CambridgeKalafut，J. J.，Alig，J. S.，2002，Method for determining a position and heading of a work machine，US Patent 6418364Pike, J. ,2006, iWorld Geodetic System 1984’， [Online]Available at http://www, globalsecurity. org/military/library/policy/army/fm/6-2/fige-l. gifSahm,W. C. et al. ,1995,Method and apparatus for determining the location of a work implement，US Patent 5404661Tu, C. H. et al.，1997，GPS compass :A novel navigation equipment，IEEE Transactions on Aerospace and Electronic Systems，33，1063—1068·Vaniceck，P.，Krakiwsky, Ε.，1986，Geodesy :The concepts，Elsevier Science Publishers B. V.，Amsterdam.Gelb, A. and Vander Velde，W. E.，Multiple-Input Describing Functions and Nonlinear System Design, McGraw-Hill Book Company, New York(1968).Graham，D. and McRuer, D.，Analysis of Nonlinear Control Systems, John Wiley & Sons Inc，New York(1961).Duddek et al. 1992 ；Method of determining mining progress in open cast mining by means of satellite geodesy, US Patent 5144317.之前已经提出了确定移动设备单元的位置和方向的问题的多种解决方案。解决方 案不变地利用多种定位传感器，包括上述传感器中的一些。Duddek等(1992)公开了一种使用勺轮附近的GPS传感器和接收机来确定挖掘机 勺斗末端的位置和方向的方法。Kalafut等(2002)提出了一种用于通过使用单个定位传感器来确定机器的位置 和方向的系统。随时间从定位传感器获取读数，产生运动曲线图以估计机器的方向。该方 法具体可应用于一般在运动中并且具有良好动态特性的机器。在采掘应用中，只要这些托 运卡车在运动中，则托运卡车是这种方法的良好备选。单传感器定位系统的另一示例是由Sahm等(1995)提出的单传感器定位系统，该单传感器定位系统使用单个传感器，所述单个传感器能够采集与采掘机铲的悬臂有关的 (X，1，ζ)位置参数。如果假定机铲的底架在挖掘周期期间是固定的，则可以随时间测量点 集合，以产生传感器所在的平面。该估计可以与来自传感器的位置的当前测量一起用于估 计机铲勺斗的当前位置。Dizchavez (2001)还提出了根据所估计的平面来定位的方法。两个GPS天线安装 在机器外壳上等高度的已知位置。在机器的操作期间，可以测量外壳的旋转，使用基于标准 差分析的计算来形成对两个天线所在平面的估计。根据该平面以及该平面内传感器的当前 位置和方向，在给定运动模型和适当接合位置信息的情况下可以定位机器的另一部分。希望提供一种确定采掘设备等的空间位姿的方法和设备。说明书全文中对现有技术的任何讨论不应被看作是承认现有技术是公知的或构 成本领域公知常识的一部分。除非上下文明确要求，否则，在说明书全文和权利要求中，词语“包括”等具有与排 他或穷尽含义相对的包含含义；也就是说，具有“包括但不限于”的含义。
发明内容
根据本发明的第一方面，提供了一种确定采掘机铲的全球位姿的方法，所述方法 包括应用多级计算的步骤，所述多级计算包括(a)第一级，使用全球定位系统、倾斜计以及摇摆轴解算器来计算采掘机铲车体 (c坐标系)相对于本地大地坐标系(g坐标系)的方位；(b)第二级，使用全球定位系统、轴惯性传感器和摇摆轴解算器来计算相对于c坐 标系的外壳位姿(h坐标系)；(c)第三级，使用推压和提升轴解算器来计算相对于h坐标系的勺斗位姿(b坐标 系)。优选地，使用扩展卡尔曼滤波器来执行所述步骤(a)和(b)。可以使用迭代例程 来执行步骤(a)直到收敛。倾斜计可以是双轴倾斜计。轴惯性传感器可以是六轴惯性传感 器。机铲的第一部分可以包括机器外壳。根据本发明的另一方面，提供了一种按照三级计算过程来确定电采掘机铲的全球 位姿的方法，其中(a)在第一级，使用全球定位系统、双轴倾斜计以及摇摆轴解算器来计 算车体(C坐标系)相对于本地大地坐标系(g坐标系)的方位，直到收敛；(b)在第二级， 使用全球定位系统、六轴惯性传感器(三个速率陀螺仪和三个线性加速度)和摇摆轴解算 器来计算相对于c坐标系的外壳位姿(h坐标系)；(C)在第三级，使用推压和提升轴解算器 来计算相对于h坐标系的勺斗位姿(b坐标系)。根据本发明的另一方面，提供了一种确定采掘机铲的全球空间位姿的方法，所述 方法包括以下步骤(a)指定参考的第一地心地固ECEF坐标系或e坐标系；(b)指定在采 掘机铲附近表示为g坐标系的本地大地坐标系，g坐标系被限定为e坐标系中的笛卡尔坐 标轴集合；(c)指定靠近采掘机铲的本体或底架、表示为c坐标系的笛卡尔坐标轴集合；(d) 确定g坐标系内c坐标系的方位；(e)指定在采掘机铲的机器外壳附近表示为h坐标系的笛 卡尔坐标轴集合；(f)确定c坐标系内h坐标系的方位；(g)指定被固定到机铲柄和铲斗装 置附近、表示为b坐标系的笛卡尔坐标轴集合；以及(h)确定h坐标系内b坐标系的方位。
现在将参考附图来描述本发明的优选形式图1示出了装载托运卡车的电采掘机铲；图2示出了 e坐标系和g坐标系的定义；图3示出了 c坐标系、h坐标系和b坐标系的定义；图4示出了 P&H Centurion控制机铲的摇摆轴的控制系统；图5示出了饱和型非线性的特性，包括作为输入函数的描述函数增益；图6示出了 P&H类电采掘机铲的坐标系；图7示出了出于定义b坐标系的目的而处于直角配置中的P&H类电采掘机铲；以 及图8示出了优选实施例的方法步骤的流程图。
具体实施例方式如图8所示，优选实施例提供了一种确定电采掘机铲的全球空间位置的改进方法 80。全球空间位姿包括 地心地固(ECEF)坐标系或e坐标系81的指示； 本地大地坐标系(g坐标系)的标识，所述本地大地坐标系被限定为在e坐标系 中例如沿着北、东和下惯例的笛卡尔坐标轴集合。该坐标系的原点在采掘机铲附近，典型地 在机器所位于的矿产内82 ； 固定到采矿机铲83的车体或底架的笛卡尔坐标轴集合的指示。这些轴所限定 的笛卡尔坐标系已知为c坐标系；· g坐标系内c坐标系的方位(位置和方向)的指示84 ； 固定到采掘机铲的机器外壳的笛卡尔坐标轴的指示。这些轴所限定的笛卡尔坐 标系已知为h坐标系85 ；· c坐标系内h坐标系的方位(位置和方向)的确定86 ； 固定到机铲柄和铲斗(勺斗)装置的笛卡尔坐标轴集合的指示。这些轴所限定 的笛卡尔坐标系已知为b坐标系；· h坐标系内b坐标系的方位(位置和方向)的确定。这些坐标的定义使得可以在全球坐标系中建立勺斗的方位。如图1所示，采掘机铲1和其他类似挖掘机的操作的基本特性是，采掘机铲1和其 他类似挖掘机每次维持c坐标系的方位若干分钟。即，对使用履带2的机器的重新定位是 不频繁地进行的，当挖掘机顺序地挖掘材料并将材料装载到托运卡车4中时在移动之间的 主要活动是机器外壳3的前后摇摆运动。优选实施例利用采掘机铲1的这种操作特性来解决确定机铲位姿的问题。优选实施例还利用若干可用附加传感器测量的组合，包括 固定到h帧的一个或多个标识点位置在e坐标系中的实时运动全球定位系统； 由h坐标系相对于g坐标系的三个正交加速度和三个角速率构成的惯性测量；· h坐标系相对于g坐标系的纵摇和横摇的倾斜计测量；
三个主运动执行器(即，摇摆发动机、推压发动机和提升发动机)的速度和位置 测量； 来自三个主运动执行器(即，摇摆发动机、推压发动机和提升发动机)的电压和 电流测量； 机铲操作者通常通过操纵杆设置的参考值，这些参考值是向三个主运动执行器 (即，摇摆发动机、推压发动机和提升发动机)的控制系统的输入。优选实施例提出了基于扩展卡尔曼滤波器的递归算法的公式化，所述卡尔曼滤波 器使用这些测量的组合来确定全球机铲位姿。已知实时机铲位姿有多种用途，这些用途包括1.已存在商业系统的应用，使用对挖掘期间铲斗相对于资源分布图的位置的知识 作为允许操作者区分矿石与废料的手段；2.显露重要性的应用，用于使采掘设备自动化，其中需要解决方案的重要问题控 制与诸如托运卡车之类的其他设备的交互。如果这样的设备单元具有类似的位姿估计能 力，则可以确定设备之间的相对位姿；3.来自扫描距离传感器(例如，激光传感器以及可能用于自动化系统中的测距以 及用于开发本地数字地形图的毫米波雷达)的数据的正确空间登记也需要机铲位姿的知 识。现有解决方案忽略了可以适于估计问题的估计理论。具体地，可以将问题简化化 为状态估计训练，其中，可以将g坐标系、c坐标系、h坐标系和b坐标系的相对位置和方向 表示为动态系统的状态，使用对测量的操作者命令参考与机器的结果运动之间的因果关系 (“过程模型”)的知识_及时传播机铲位姿的当前知识(以概率分布的形式)，以便于与来 自先前标识的传感器的测量的组合相融合。问题简化图2和图3示出了与包括多种坐标系的问题有关的几何结构。首先转向图2，示出 了相对于地球21来定位地球坐标系(e坐标系)和大地坐标系(g坐标系)的几何坐标系。 图3示出了车体坐标系(c坐标系)、外壳坐标系(h坐标系)和勺斗坐标系(b坐标系)。以两级来计算机铲勺斗的位姿。第一级的目的是根据以下测量来计算c坐标系相对于h坐标系的方位· η个RTK-GPS接收机的g坐标系中的位置； 如h坐标系中固定的双轴倾斜计测量的，Zh轴相对于Zg轴的明显的方向；· h坐标系绕Ze轴的旋转； 摇摆发动机的角速度； 摇摆发动机的电枢电流和电枢电压； 来自操纵杆的操作者命令参考。这些量限定了测量矢量ζ和输入矢量U。第二级的目的是在找到c坐标系相对于g坐标系的方位的情况下使用以下测量来 计算h坐标系相对于c坐标系的方位· η个RTK-GPS接收机的g坐标系中的位置； 在h坐标系中固定的点在三个正交方向上的角速率和线性加速度的测量，但是这在瞬时沿着正交传感器轴的惯性系中测量的；· h坐标系绕ζ。轴的旋转； 摇摆发动机的角速度； 摇摆发动机的电枢电流和电枢电压； 来自操纵杆的操作者命令。这些量限定了第二测量矢量ζ和第二输入矢量U。第三级的目的是使用以下测量以及挖掘装置的运动模型来计算b坐标系相对于h 坐标系的方位 提升发动机的位置 推压发动机的位置级3计算是运动的并且计算b坐标系相对于h坐标系的方位。计算进行至确定如下机铲位姿 在机器已完成任何推进运动并进入正常挖掘活动(特征在于反复地往复摇摆) 之后，第一级计算立即运行足够的时间以得到c坐标系相对于g坐标系的方位的收敛估计。 假定g坐标系相对于e坐标系的方位是先验已知的； 在级1得到收敛之后，发起第二级计算和随后的第三级计算，以规则时间步长 来进行第二和第三级计算以确定h坐标系相对于c坐标系的位置以及b坐标系相对于h坐 标系的位置； 当操作者下一次驱动机器时，在驱动运动完成之前计算停止，其中，当驱动运动 完成时，再次执行第一级计算以找到c坐标系相对于g坐标系的方位的新的收敛估计。然 后计算进行至级2，以此类推。支持该分级计算过程的是以下构思在级1使用的测量可以提供与机器的低频运 动有关的丰富信息，这些信息足以精确地确定c坐标系相对于g坐标系的位置。在与正常 生产相关联的正常往复运动期间，除了大规模摇摆运动之外，具体在挖掘期间机器的外壳 也可能发生较小幅度的摆动运动。级2使用的的传感器的测量目的在于精确地确定这些运 动。在这种情况下，级2滤波器目的在于更高的估计带宽。级1和2的计算方法是扩展卡尔曼滤波器(EKF)、Celb (1974)。EKF需要以下形式 的系统模型i=f(>,u，O + w,w N(0，Q)(1)zk = h(xk, uk, k) +vk, vk N(0，R)其中，f(x, u, t)是描述系统动态的矢量值函数，所述系统用于基于操作者命令参 考的测量及时传播状态和状态协方差的当前估计，因此f(x，U, t)可以与新得到的测量数 据相结合。矢量值函数h(xk，Uk, k)表示关于状态矢量X和输入U的测量。EKF需要关于估计状态轨迹i对f (X，u, t)和h(xk，uk, k)的线性化，以及线性化连 续动态到离散时间形式的转换。希望使用以下表示法
「 (Sf (χ,/) )、F = exp ~-——-At
\ ^x J其中，At是测量更新速率，以及
权利要求
一种确定采掘机铲的全球位姿的方法，所述方法包括应用多级计算的步骤，所述多级计算包括(a)第一级，使用全球定位系统、倾斜计以及摇摆轴解算器来计算采掘机铲车体(c坐标系)相对于本地大地坐标系(g坐标系)的方位；(b)第二级，使用全球定位系统、轴惯性传感器和摇摆轴解算器来计算相对于c坐标系的外壳位姿(h坐标系)；(c)第三级，使用推压和提升轴解算器来计算相对于h坐标系的勺斗位姿(b坐标系)。
2.根据权利要求1所述的方法，其中，使用扩展卡尔曼滤波器来执行所述步骤(a)和(b)。
3.根据前述权利要求中的任一项权利要求所述的方法，其中，使用迭代例程来执行步 骤(a)直到收敛。
4.根据前述权利要求中的任一项权利要求所述的方法，其中，倾斜计是双轴倾斜计。
5.根据前述权利要求中的任一项权利要求所述的方法，其中，倾斜计是六轴惯性传感ο
6.根据前述权利要求中的任一项权利要求所述的方法，其中，机铲的第一部分包括机 器外壳。
7.—种全球定位采掘机铲的位姿的方法，实质上按照参考附图和/或示例中所示的本 发明任一实施例而描述的来全球定位采掘机铲的位姿。
8.一种按照三级计算过程来确定电采掘机铲的全球位姿的方法，其中，(a)在第一级，使用全球定位系统、双轴倾斜计以及摇摆轴解算器来计算车体(c坐标 系)相对于本地大地坐标系(g坐标系)的方位，直到收敛；(b)在第二级，使用全球定位系统、六轴惯性传感器(三个速率陀螺仪和三个线性加速 度)和摇摆轴解算器来计算相对于c坐标系的外壳位姿(h坐标系)；(c)在第三级，使用推压和提升轴解算器来计算相对于h坐标系的勺斗位姿(b坐标系)。
9.一种确定采掘机铲的全球空间位姿的方法，所述方法包括以下步骤(a)指定参考的第一地心地固ECEF坐标系或e坐标系；(b)指定在采掘机铲附近表示为g坐标系的本地大地坐标系，g坐标系被限定为e坐标 系中的笛卡尔坐标轴集合；(c)指定靠近采掘机铲的本体或底架、表示为c坐标系的笛卡尔坐标轴集合；(d)确定g坐标系内c坐标系的方位；(e)指定在采掘机铲的机器外壳附近表示为h坐标系的笛卡尔坐标轴集合；(f)确定c坐标系内h坐标系的方位；(g)指定被固定到机铲柄和铲斗装置附近、表示为b坐标系的笛卡尔坐标轴集合；以及(h)确定h坐标系内b坐标系的方位。
10.一种确定采掘机铲的全球空间位姿的方法，实质上按照参考附图和/或示例中所 示的本发明任一实施例而描述的来确定采掘机铲的全球空间位姿。
11.一种用于确定采掘机铲的全球空间位姿的设备，实质上按照参考附图和/或示例 中所示的本发明任一实施例而描述的来确定采掘机铲的全球空间位姿。
全文摘要
已知采掘挖掘机的全球定位为采掘操作的管理和自动化提供了很多好处。本发明描述了一种全球定位电采掘机铲的位姿的方法。系统从安装在机器外壳上的任意数目个RTK-GPS天线以及适合机器摇摆轴的解算器获得测量。使用卡尔曼滤波器来产生全球方位位姿的估计。
文档编号E02F3/43GK101970763SQ200980105400
公开日2011年2月9日 申请日期2009年1月7日 优先权日2008年1月8日
发明者安东尼·沃尔顿·里德, 彼得·罗斯·麦卡里 申请人:易斯麦私人有限公司