CN101970763A - 确定电采掘机铲的空间位姿的实时方法 - Google Patents

Abstract

已知采掘挖掘机的全球定位为采掘操作的管理和自动化提供了很多好处。本发明描述了一种全球定位电采掘机铲的位姿的方法。***从安装在机器外壳上的任意数目个RTK-GPS天线以及适合机器摇摆轴的解算器获得测量。使用卡尔曼滤波器来产生全球方位位姿的估计。

Description

确定电采掘机铲的空间位姿的实时方法

技术领域

本发明涉及对设备进行定位的领域，具体地，公开了一种用于确定在采掘操作中使用的转臂式装载设备(如，电采掘机铲)的空间位姿的***。

背景技术

参考文献

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 6191733

Gelb，A.，1996，Applied optimal estimation，The M.I.T.Press，Cambridge

Kalafut，J.J.，Alig，J.S.，2002，Method for determining a position and heading of a work machine，US Patent 6418364

Pike，J.，2006，‘World Geodetic System 1984’，[0nline]Available at：http://www.globalsecurity.org/military/library/policy/army/fm/6-2/fige-1. gif

Sahm，W.C.et al.，1995，Method and apparatus for determining the location of a work implement，US Patent 5404661

Tu，C.H.et al.，1997，GPS compass：A novel navigation equipment，IEEE Transactions on Aerospace and Electronic Systems，33，1063-1068.

Vaniceck，P.，Krakiwsky，E.，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，y，z)位置参数。如果假定机铲的底架在挖掘周期期间是固定的，则可以随时间测量点集合，以产生传感器所在的平面。该估计可以与来自传感器的位置的当前测量一起用于估计机铲勺斗的当前位置。

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坐标系的方位

●n个RTK-GPS接收机的g坐标系中的位置；

●如h坐标系中固定的双轴倾斜计测量的，z_h轴相对于z_g轴的明显的方向；

●h坐标系绕z_e轴的旋转；

●摇摆发动机的角速度；

●摇摆发动机的电枢电流和电枢电压；

●来自操纵杆的操作者命令参考。

这些量限定了测量矢量z和输入矢量u。

第二级的目的是在找到c坐标系相对于g坐标系的方位的情况下使用以下测量来计算h坐标系相对于c坐标系的方位

●n个RTK-GPS接收机的g坐标系中的位置；

●在h坐标系中固定的点在三个正交方向上的角速率和线性加速度的测量，但是这在瞬时沿着正交传感器轴的惯性系中测量的；

●h坐标系绕z_c轴的旋转；

●摇摆发动机的角速度；

●摇摆发动机的电枢电流和电枢电压；

●来自操纵杆的操作者命令。

这些量限定了第二测量矢量z和第二输入矢量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需要以下形式的***模型

w～N(0，Q)                               (1)

z_k＝h(x_k，u_k，k)+v_k，v_k～N(0，R)

其中，f(x，u，t)是描述***动态的矢量值函数，所述***用于基于操作者命令参考的测量及时传播状态和状态协方差的当前估计，因此f(x，u，t)可以与新得到的测量数据相结合。矢量值函数h(x_k，u_k，k)表示关于状态矢量x和输入u的测量。

EKF需要关于估计状态轨迹对f(x，u，t)和h(x_k，u_k，k)的线性化，以及线性化连续动态到离散时间形式的转换。希望使用以下表示法：

$F=\exp \left(\frac{\∂f(x,t)}{\∂x}\mathrm{\Δt}\right)---\left(2\right)$

其中，Δt是测量更新速率，以及

${\▿h}_k=\frac{\∂h(x_k,k)}{\∂x_k}{|}_{x_k={\hat{x}}_k}---\left(3\right)$

等式1中的矢量w和v表示过程和测量噪声，并且被认为是通过分别具有协方差Q和R的零均值高斯过程而产生的。

EKF的计算方案包括以下五个步骤(Gelb，1974)：

1.状态估计传播

$x_{k+1}^{-}=F{\hat{x}}_k^{+}---\left(7\right)$

2.状态协方差传播

$P_{k+1}^{-}={\mathrm{FP}}_k^{+}+P_k^{+}{F}^T+Q---\left(8\right)$

3.卡尔曼增益的计算

$K_{k+1}=P_{k+1}^{-}\▿{h_{k+1}}^T{(\▿h_{k+1}{P}_{k+1}^{-}\▿{h_{k+1}}^T+R)}^{-1}---\left(9\right)$

4.状态估计更新

${\hat{x}}_{k+1}^{+}={\hat{x}}_{k+1}^{-}+K_{k+1}(z_{k+1}-h({\hat{x}}_{k+1}^{-},k))---\left(10\right)$

5.以时间步长k计算误差协方差

$P_{k+1}^{+}=(I-K_{k+1}\▿h_{k+1})P_{k+1}^{-}---\left(11\right)$

等式7至11限定了EKF算法，所述EKF算法提供了以最小均方误差测量的非线性***的最佳线性状态估计器。等式7至11中的上标“-”和“+”指示在执行测量之前和之后对量的评估。

动态模型的形成

用于在级1和级2传播机铲位姿的动态模型包括(作为共有元件)：将操作者操纵杆参考与矢量值函数f(x，u，t)中的摇摆运动相关联的因果模型。以下给出了用于Centurion使能P&H机铲的该模型的优选实施例。

P&H Centurion使能电采掘机铲使用ABB DCS/DCF600多驱动控制器来调节三个DC发动机中每个DC发动机中的发动机速度、电枢电流和场电流。控制器由四个集成组件构成：PID或PI发动机速度控制回路、束点电流饱和限制器、PI电流控制回路以及EMF场电流调节器。

摇摆驱动使用转矩控制和继电器式速度控制的组合，从而摇摆操纵杆位置产生逐段速度参考和电枢电流饱和限制。图4示出了摇摆驱动模型的示意图。参考摇摆发动机速度与实际摇摆发动机速度之差馈送至并入了微分滤波的比例积分微分(PID)速度控制器41。将速度控制器的输出缩放42成根据摇摆操纵杆参考的幅度来成比例限制的参考电枢电流。受限电流参考与实际电枢电流之间的误差馈送至PI电流控制器43，PI电流控制器43向摇摆发动机输出电枢电压。如推压驱动一样，摇摆驱动具有恒定的场电流，其中DCF600将场电压维持在稳定水平。

对机铲的驱动进行有效建模需要用于将发动机电枢电流中存在的非线性饱和效应并入的装置。为了将这些效应包含在预测模型中，使用正弦输入描述函数。描述函数(将简称为DF)主要被开发用于研究非线性动态***中的极限周期，参见Gelb和Vander Velde(1968)以及Graham和McRuer(1961)。描述函数方法的基本构思是将动态***中的非线性元件替换成拟线性描述符或描述函数等效增益，所述描述函数等效增益的幅度是输入幅度的函数。

使用正弦饱和的DF等效增益对电枢电流饱和进行建模。图5示出了饱和放大器的正弦波输出的效应。对于至放大器的、幅度小于饱和极限的输入(a/AK＞1)，输出51与输入成比例。对于幅度大于饱和极限的输入(a/AK＜1)，输出53变成“被削波的”并且可以由傅里叶级数55来表示，其中，项b₃ sin 3ωt、b₅ sin 5ωt等表示由非线性饱和元件产生的新频率。对这种饱和进行建模的DF方法假定饱和输出中的高阶项是可忽略的。因此，正弦饱和的DF等效增益采用如下形式

$G=\frac{b_1}{A}=\frac{2K}{\mathrm{\π}}[{\sin }^{-1}\left(\frac{a}{\mathrm{AK}}\right)+\frac{a}{\mathrm{AK}}\sqrt{1-{\left(\frac{a}{\mathrm{AK}}\right)}^2}]$

其中，b₁是输出的傅里叶级数的首项或基项，A是输入的幅度。

将DF实现成预测模型需要在每个时间步长评估等效增益。如果至电流饱和块的输入大于饱和极限，则通过将输入幅度乘以等效增益来计算饱和输出。

对于饱和输出的傅里叶变换的高阶项不起主要作用的假设由以下概念来支持：机铲的驱动动态起到低通滤波器的作用，输出的基频在经过***时的衰减比输出的高阶谐波要小得多。

驱动预测模型被表示为如下形式的连续线性状态空间***

$\stackrel{\·}{x}=\mathrm{Ax}+\mathrm{Bu}$

输入矢量u包含：根据操纵杆信号而产生的参考发动机速度

发动机上由于引力效应而引起的静转矩负载T_s，以及库伦摩擦干扰输入f_s。对于每个模型，状态矢量x包含摇摆电枢电流I_s、摇摆发动机速度ω_s、摇摆发动机位置θ_s、以及速度和电流控制器的误差的积分

和

摇摆驱动模型还包含在饱和极限之前的摇摆参考电枢电流该状态是由于摇摆发动机速度控制器中的微分组件而产生的。摇摆驱动的全状态空间模型由以下给出：

$+\left[\begin{array}{ccc}0& 0& 0\\ 0& -\frac{1}{J_s}& -\frac{1}{J_s}\\ 0& 0& 0\\ \left(\frac{K_{{\mathrm{\ω}}_s}{K}_{T1}}{T_{{\mathrm{\ω}}_s}^f}\right)(1+\frac{T_{{\mathrm{\ω}}_s}^f}{T_{{\mathrm{\ω}}_s}^i})& 0& 0\\ 1& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_s^d\\ T_s\\ f_s\end{array}\right]$

描述函数增益G_s就像是驱动***和输入矩阵中的元素，该元素是在每个时间步长重新计算的。至电流控制器中的输入是摇摆参数电枢电流状态

可以根据摇摆操纵杆位置来确定摇摆电枢电流饱和极限。

注意，为了有效，级1和级2模型应当包含所谓的“整形级”，所述整形级调整传感器中的偏置并处理诸如传动后冲之类的物理假象。

注意，还可以利用所谓的Ornstein-Uhlenbeck随机过程来实现级2模型，所述Ornstein-Uhlenbeck随机过程的参数可以根据后续自相关分析来确定。

测量模型

在给定h坐标系中第n个GPS天线的位置(x_a，y_a，z_a)的情况下，等式12表示关于机铲车体的位置(x_c，y_c，z_c)和方向余弦矩阵R_c2g和R_h2c在g坐标系中进行的GPS测量，所述方向余弦矩阵R_c2g和R_h2c分别描述在c坐标系与g坐标系之间以及在h坐标系与c坐标系之间的3D旋转。可以以多种方式来计算这些矩阵，例如，欧拉角或四元数。描述这些矩阵的参数是估计器的状态。

z_gps＝(x_c，y_c，z_c)^T+R_c2gR_h2c(x_a，y_a，z_a)^T  (12)

GPS测量基于双轴椭球形式的地球表面的近似。这些椭球的尺寸由若干标准或数据之一来限定。图2示出了地球的WGS84椭球近似20，其中表示了GPS天线的维度、经度和海拔。用于将以e坐标系维度、经度和海拔测量的来自GPS接收机的传感器读数变换成g坐标系中的坐标的方法如下：

●第一级是将测量转换成笛卡尔坐标，其中原点在地球的中心，x轴被限定在0°的经度值处(如以上在图2中可以看出的)。

●Vanicek(1986)定义了对于任何点，椭球近似上的p₀：

p₀＝N₀ cosφ₀

其中N₀是与椭球中心的距离，φ₀是与平面x_e-y_e的角度(点p₀处的海拔)。与椭球中心的距离被定义为：

$N_0=\frac{a^2}{{(a^2{\cos }^2{\mathrm{\φ}}_0+b^2{\sin }^2{\mathrm{\φ}}_0)}^{\frac{1}{2}}}---\left(13\right)$

●可以重新布置这些等式以给出全球笛卡尔坐标中点p₀的位置矢量：

${r_0}^G=\left[\begin{array}{c}{x}_0\\ y_0\\ z_0\end{array}\right]=N_0\left[\begin{array}{c}\cos {\mathrm{\φ}}_0\cos {\mathrm{\λ}}_0\\ \cos {\mathrm{\φ}}_0\sin {\mathrm{\λ}}_0\\ (b_2/a^2)\sin {\mathrm{\φ}}_0\end{array}\right]---\left(14\right)$

其中，λ₀是点p₀处的经度。

●为了将该全球位置变换到本地坐标系中，必须定义合适的本地坐标系。定义了以点p₀为中心的轴集合，使得y轴与椭球表面相切，该方向上的点将表现为在站在点p₀处的观察者北方与该观察者相对。如果将该方向定义为“视北”，则“视东”是与椭球表面相切并且与矢量r₀ ^G和矢量视北正交的矢量。这从而可以定义合适的本地坐标系，其中，y轴沿着视北，x轴沿着视东，z轴表示椭球表面上方的高度。该公式化对于任何点p₀都成立，使得λ₀≠±90°。如果λ₀≠±90°，则x轴和y轴的方向是正交的，只要x轴和y轴正交并形成与椭球表面相切的平面。

●可以认为本地x轴(视东)具有零海拔(相对于p₀)。如果将该轴表示为矢量r_a的单位矢量，则可以将y_a任意地设置为1，并使用等式14给出：

$r_a=\left[\begin{array}{c}{x}_a\\ y_a\\ z_a\end{array}\right]=\left[\begin{array}{c}-y_0/x_0\\ 1\\ 0\end{array}\right]---\left(15\right)$

●由于需要y轴(视北)与x轴和z轴正交，可以找到y轴(视北)：

r_b＝r_G×r_a                           (16)

●最后，从全球到本地坐标的变换矩阵可以被限定为：

$T=\left[\begin{array}{ccc}{\hat{x}}_a& {\hat{y}}_a& {\hat{z}}_a\\ {\hat{x}}_b& {\hat{y}}_b& {\hat{z}}_b\\ {\hat{x}}_G& {\hat{y}}_G& {\hat{z}}_G\end{array}\right]---\left(17\right)$

其中，

$\hat{r}=\left[\begin{array}{c}\hat{x}\\ \hat{y}\\ \hat{z}\end{array}\right]$

●这给出了对于点p_n从全球到本地坐标的最终变换：

$p_n^{\mathrm{local}}=T.(p_n^{\mathrm{global}}-{r_0}^G)---\left(18\right)$

如倾斜计所测量的机器外壳的纵摇和横摇倾斜被确定为平面(x_g，y_g)内x_h轴和y_h轴上单位矢量的夹角。通过使用标准Z-X-Z旋转惯例，将c坐标系相对于g坐标系的旋转表示为欧拉角

纵摇(α)和横摇(β)角是

可以通过对传感器的倾斜提取公因子来改善粗倾斜计测量。

IMU的加速度被测量为该IMU在机器外壳中的方位(x_i，y_i，z_i)的全球加速度，被旋转为沿着正交的传感器轴。在给定机铲车***置(x_c，y_c，z_c )以及分别描述c坐标系与g坐标系之间和h坐标系与c坐标系之间的3D旋转的方向余弦矩阵R_c2g和R_h2c的情况下，加速度测量是：

z_acc＝R_g2cR_c2ha_i  (21)

其中根据以下等式得到g坐标系中IMU的加速度

$a_i=\frac{d^2}{{\mathrm{dt}}^2}({(x_c,y_c,z_c)}^T+R_{h2c}{R}_{c2g}{(x_i,y_i,z_i)}^T)---\left(22\right)$

假定g坐标系是非加速且非旋转的。

IMU的角速度将被测量为机器外壳的全球角速度，被旋转为沿着正交的传感器轴。通过使用标准Z-X-Z旋转惯例将h坐标系相对于c坐标系的旋转表示为欧拉角

关于RPY轴的测量角速度是：

P&H类机器的级3计算

级3计算依赖于所讨论的机器。在优选实施例中，针对P&H类采掘机铲来执行计算。

图6示出了用于描述P&H类电采掘机铲的几何结构的参数(长度和角度)以及用于描述这些机器的主要移动装置的相对位置的坐标系。通过设计固定了标记为l的长度以及标记为φ的角度；标记为d的长度和标记为θ的角度根据机器运动而变化。需要该几何结构来确定勺斗相对于h坐标系的方位。

c坐标系表示为O_cx_cy_cz_c；h坐标系表示为O_hx_hy_hz_h并且嵌入机器外壳中；O_mx_my_mz_mm坐标系在鞍座中；O_bx_by_bz_b b坐标系在铲斗中。所有体固定坐标系的x轴和z轴都在机器外壳的矢状平面中，即，与包含摇摆轴的、图6所示的投影平面平行的平面。所有坐标系的y轴都与该平面垂直。

可以使用四乘四齐次变换矩阵来描述坐标系之间的关系。将描述从O_ix_iy_iz_i到O_jx_jy_jz_j的变换的矩阵表示为D_i→j，注意该矩阵的动作将j坐标系中的(齐次)点映射到i坐标系。例如，如果p是方位在O_bx_by_bz_b中已知的、固定在勺斗中的点，则可以据此得到该点在坐标系O_cx_cy_cz_c中的坐标。

p′＝D_0→3p.

D_i→j的结构是

$D_{i\→j}=\left(\begin{array}{cc}{R}_{i\→j}& t_{i\→j}\\ 0& 1\end{array}\right)$

其中，R_i→j是3×3旋转矩阵，是3维转换矢量。四乘四齐次变换矩阵根据以下等式交换：

D_i→k＝D_i→jD_j→k。

c坐标系的原点位于履带上表面与机器外壳下表面之间的接合处。z_c轴与摇摆轴共线。x_c轴指向履带的前向行进方向，y_c轴完成右侧三面形坐标系。

坐标系O_hx_hy_hz_h的原点O_h与O_c重合，z_h与z_c共线。当θ₁＝0时，坐标系O_cx_cy_cz_c和O_hx_hy_hz_h重合。当从上看时，正角度θ₁与机器外壳相对于履带的逆时针旋转相对应。齐次变换矩阵D_c→h由以下给出：

$D_{0\→1}=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_1& -{\sin \mathrm{\θ}}_1& 0& 0\\ \sin {\mathrm{\θ}}_1& \cos {\mathrm{\θ}}_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right).$

O_mx_my_mz_m被固定到鞍座，其中O₂在鞍座的旋转中心。当θ₂等于0时，O_mx_my_mz_m的坐标方向与O_hx_hy_hz_h的坐标方向平行。描述从坐标系h到坐标系n的刚***移的位移矩阵由以下给出：

$D_{h\→m}=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_2& 0& -\sin {\mathrm{\θ}}_2& l_1\cos {\mathrm{\φ}}_1\\ 0& 1& 0& 0\\ \sin {\mathrm{\θ}}_2& 0& \cos {\mathrm{\θ}}_2& l_1\sin {\mathrm{\φ}}_1\\ 0& 0& 0& 1\end{array}\right),$

其中设计参数l₁和φ₁如图6所示。

O_bx_by_bz_b的原点O_b定位如下。鞍座角(θ₂)被设置为等于90度，使得柄水平。然后移位柄，使得提升绳垂直降落(θ₅＝90度，并且θ₆＝0度)。原点O_b位于柄支架和提升绳的节线的交叉处；z_b被设置为与提升绳的轴共线；x_b被设置为与柄支架的节线平行。注意，轴x_m与x_b正交。描述刚***移D_m→b的位移矩阵由以下给出

$D_{2\→3}=\left(\begin{array}{cccc}0& 0& 1& l_2\\ 0& 1& 0& 0\\ -1& 0& 0& -d_3\\ 0& 0& 0& 1\end{array}\right).$

上述等式的相乘给出：

$D_{c\→b}=D_{c\→h}{D}_{h\→m}{D}_{m\→b}$

$=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2& -\sin {\mathrm{\θ}}_1& \cos {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_2& \cos {\mathrm{\θ}}_1(l_1\cos {\mathrm{\φ}}_1+l_2\cos {\mathrm{\θ}}_2+d_3\sin {\mathrm{\θ}}_2)\\ \sin {\mathrm{\θ}}_1\sin {\mathrm{\θ}}_2& \cos {\mathrm{\θ}}_2& \sin {\mathrm{\θ}}_1\cos {\mathrm{\θ}}_2& \sin {\mathrm{\θ}}_1(l_1\cos {\mathrm{\φ}}_1+l_2\cos {\mathrm{\θ}}_2+d_3\sin {\mathrm{\θ}}_2)\\ -\cos {\mathrm{\θ}}_2& 0& \sin {\mathrm{\θ}}_2& l_1\sin {\mathrm{\φ}}_1+l_2\sin {\mathrm{\θ}}_2-d_3\cos {\mathrm{\θ}}_2\\ 0& 0& 0& 1\end{array}\right).$

旋角θ₁、摆角θ₂、和推压伸长d₃使体固定坐标系相对于世界坐标系的位移和旋转参数化。这些配置变量可以如下分组：

θ＝(θ₁，θ₂，d₃)^T。

摇摆发动机、推压发动机θ_c和提升发动机θ_h可以类似地被分组为：

ψ＝(θ_s，θ_c，θ_h)^T。

θ的值确定ψ，反之亦然。这些映射不是双射的。然而，在这些变量的物理工作范围内，这些变量是一一对应的。注意，θ或ψ的说明确定了提升绳的倾斜，图6中标记为θ₅的第七变量。

为了建立约束方程，首先注意相关坐标θ_s和θ_c通过传动比与θ₁和d₃有关，得到：

$0={\mathrm{\γ}}_1(\mathrm{\θ},\mathrm{\ψ})=\left[\begin{array}{c}{G}_s{\mathrm{\θ}}_1-{\mathrm{\θ}}_s\left(42\right)\\ G_c{d}_3-{\mathrm{\θ}}_c\end{array}\right],---\left(23a\right)$

其中，G_s是摇摆驱动的传动比，G_c是推压驱动的传动比。

可以使用图6所示的矢量循环来使θ₂和θ₅与θ_c和θ_h有关的约束方程。为了简化表示，引入映射到物理(x₁，z₁)平面的复平面，其中实轴与x₁共线，虚轴与z₁共线。

对图6的矢量循环中的矢量进行求和，其中将z₁表示为复变量，得到：

0＝ζ(θ，ψ，θ₅)＝z₁+z₂+z₃+z₄+z₅-z₆-z₇，

其可以被扩展以得到：

$0=\mathrm{\ζ}(\mathrm{\θ},\mathrm{\ψ},{\mathrm{\θ}}_5)=l_1{e}^{i{\mathrm{\φ}}_1}+l_2{e}^{i{\mathrm{\θ}}_2}+d_2{e}^{i({\mathrm{\θ}}_2-\mathrm{\π}2)}+l_4{e}^{i{\mathrm{\θ}}_4}+d_5{e}^{i{\mathrm{\θ}}_5}-l_6{e}^{i{\mathrm{\θ}}_6}-l_7{e}^{i{\mathrm{\φ}}_7}---\left(24\right)$

其中，变量l_i、d_i、θ_i和φ_i如图7中所定义的。然后使用通过检查而确定的以下关系：

${\mathrm{\θ}}_6={\mathrm{\θ}}_5-\frac{\mathrm{\π}}{2},---\left(25\right)$

θ₄＝θ₂，

等式24可以被写成：

$\mathrm{\ζ}(\mathrm{\θ},\mathrm{\ψ},{\mathrm{\θ}}_5)=l_1{e}^{i{\mathrm{\φ}}_1}+l_2{e}^{i{\mathrm{\θ}}_2}+d_3{e}^{i({\mathrm{\θ}}_2-\mathrm{\π}2)}+l_4{e}^{i{\mathrm{\θ}}_2}+d_5{e}^{i{\mathrm{\θ}}_5}-l_6{e}^{i({\mathrm{\θ}}_5-\frac{\mathrm{\π}}{2})}-l_7{e}^{i{\mathrm{\φ}}_7}---\left(26\right)$

为了从该等式中去除d₅，方便的是首先引入变量d_h。如图6所示，d_h表示当提升绳垂直悬挂(即，θ₅＝90度)时从吊桶销到提升滑轮的外部扇形齿轮之间的距离。d_h通过以下等式与提升发动机的角位移相关：

$d_h=\frac{{\mathrm{\θ}}_h}{G_h},$

其中，G_h是提升传动比。上述表达式和等式25可以用于使d₅与d_h相关，

$d_5=d_h+l_6{\mathrm{\θ}}_6=\frac{{\mathrm{\θ}}_h}{G_h}+l_6({\mathrm{\θ}}_5-\frac{\mathrm{\π}}{2}).---\left(27\right)$

等式7右侧最后一项表示围绕滑轮的提升绳的包角。将等式(27)代入等式(26)，得到

$\mathrm{\ζ}(\mathrm{\θ},\mathrm{\ψ},{\mathrm{\θ}}_5)=l_1{e}^{i{\mathrm{\φ}}_1}+l_2{e}^{i{\mathrm{\θ}}_2}+d_3{e}^{i({\mathrm{\θ}}_2-\mathrm{\π}2)}+l_4{e}^{i{\mathrm{\θ}}_2}---\left(28\right)$

$+[\frac{{\mathrm{\θ}}_h}{G_h}+l_6({\mathrm{\θ}}_5-\frac{\mathrm{\π}}{2})]e^{i{\mathrm{\θ}}_5}-l_6{e}^{i({\mathrm{\θ}}_5-\frac{\mathrm{\π}}{2})}-l_7{e}^{i{\mathrm{\φ}}_7}=0.$

考虑等式28的实分量和虚分量，使θ_h和θ₅与广义坐标相关：

${\mathrm{\γ}}_2(\mathrm{\θ},\mathrm{\ψ},{\mathrm{\θ}}_5)=$

$\left[\begin{array}{c}{l}_1\cos {\mathrm{\φ}}_1+l_2\cos {\mathrm{\θ}}_2+d_3\sin {\mathrm{\θ}}_2+l_4\cos {\mathrm{\θ}}_2+[\frac{{\mathrm{\θ}}_h}{G_h}+l_6({\mathrm{\θ}}_5-\frac{\mathrm{\π}}{2})]\cos {\mathrm{\θ}}_5-l_6\sin {\mathrm{\θ}}_5-l_7\cos {\mathrm{\φ}}_7\\ l_1\sin {\mathrm{\φ}}_1+l_2\sin {\mathrm{\θ}}_2-d_3\cos {\mathrm{\θ}}_2+l_4\sin {\mathrm{\θ}}_2+[\frac{{\mathrm{\θ}}_h}{G_h}+l_6({\mathrm{\θ}}_5-\frac{\mathrm{\π}}{2})]\sin {\mathrm{\θ}}_5+l_6\cos {\mathrm{\θ}}_5-l_7\sin {\mathrm{\φ}}_7\end{array}\right]---\left(29\right)$

注意，在推导等式(29)的过程中使用了以下三角关系

$\cos ({\mathrm{\θ}}_i-\frac{\mathrm{\π}}{2})={\sin \mathrm{\θ}}_i,$ $\sin ({\mathrm{\θ}}_i-\frac{\mathrm{\π}}{2})=-\cos {\mathrm{\θ}}_i$

连接等式23a和29，得到：

$0=\mathrm{\Γ}(\mathrm{\θ},\mathrm{\ψ},{\mathrm{\θ}}_5)=$

$\left[\begin{array}{c}{G}_s{\mathrm{\θ}}_1-{\mathrm{\θ}}_s\\ G_c{d}_3-{\mathrm{\θ}}_c\\ l_1\cos {\mathrm{\φ}}_1+l_2\cos {\mathrm{\θ}}_2+d_3\sin {\mathrm{\θ}}_2+l_4\cos {\mathrm{\θ}}_2+[\frac{{\mathrm{\θ}}_h}{G_h}+l_6({\mathrm{\θ}}_5-\frac{\mathrm{\π}}{2})]\cos {\mathrm{\θ}}_5-l_6\sin {\mathrm{\θ}}_5-l_7\cos {\mathrm{\φ}}_7\\ l_1\sin {\mathrm{\φ}}_1+l_2\sin {\mathrm{\θ}}_2-d_3\cos {\mathrm{\θ}}_2+l_4\sin {\mathrm{\θ}}_2+[\frac{{\mathrm{\θ}}_h}{G_h}+l_6({\mathrm{\θ}}_5-\frac{\mathrm{\π}}{2})]\sin {\mathrm{\θ}}_5+l_6\cos {\mathrm{\θ}}_5-l_7\sin {\mathrm{\φ}}_7\end{array}\right]---\left(30\right)$

利用牛顿-拉夫申法的运动跟踪

运动跟踪问题是确定在给定ψ的情况下的θ和θ₅的值以及在给定ψ情况下确定ψ和θ₅。将第一种问题称作是前向运动跟踪，将第二种问题称作是反向运动跟踪。为了便于区分这两种问题，当工作于正向运动跟踪问题的领域并且

时，写作

θ_F＝(θ₁，θ₂，θ₃，θ₅)

ψ_F＝(θ_s，θ_c，θ_h)

这里区别在于θ₅的分组。这两种问题数学上相当于解非线性约束方程。本发明选择使用多变量牛顿方法来迭代地执行这一操作。在表示牛顿方法解决方案中开发的雅可比矩阵用于将发动机惯性引入配置变量并解决静力问题。

正向运动跟踪

对等式30应用泰勒级数扩展

$\mathrm{\Γ}({\mathrm{\θ}}_F+\mathrm{\Δ}{\mathrm{\θ}}_F,{\mathrm{\ψ}}_F+\mathrm{\Δ}{\mathrm{\ψ}}_F)$

$=\mathrm{\Γ}({\mathrm{\θ}}_F,{\mathrm{\ψ}}_F)+\frac{\mathrm{\δ\Γ}({\mathrm{\θ}}_F,{\mathrm{\ψ}}_F)}{{\mathrm{\δ\θ}}_F}{\mathrm{\Δ\θ}}_F+\frac{\mathrm{\δ\Γ}({\mathrm{\θ}}_F,{\mathrm{\ψ}}_F)}{{\mathrm{\δ\ψ}}_F}{\mathrm{\Δ\ψ}}_F+\mathrm{HOT}=0$

目的是找到有效配置，即，Γ(θ_F+Δθ_F，ψ_F+Δψ_F)＝0。然后

$\mathrm{\Γ}({\mathrm{\θ}}_F,{\mathrm{\ψ}}_F)\≈-[\frac{\mathrm{\δ\Γ}({\mathrm{\θ}}_F,{\mathrm{\ψ}}_F)}{{\mathrm{\δ\θ}}_F}{\mathrm{\Δ\θ}}_F+\frac{\mathrm{\δ\Γ}({\mathrm{\θ}}_F,{\mathrm{\ψ}}_F)}{{\mathrm{\δ\ψ}}_F}{\mathrm{\Δ\ψ}}_F]$

这得到迭代方程：

${\mathrm{\Δ\θ}}_F^k=-{\left(\frac{\∂\mathrm{\Γ}({\mathrm{\θ}}_F^{k-1},{\mathrm{\ψ}}_F^k)}{{\∂\mathrm{\θ}}_F}\right)}^{-1}(\mathrm{\Γ}({\mathrm{\θ}}_F^{k-1},{\mathrm{\ψ}}_F^k)+\frac{\∂\mathrm{\Γ}({\mathrm{\θ}}_F^{k-1},{\mathrm{\ψ}}_F^k)}{{\∂\mathrm{\ψ}}_F}\mathrm{\Δ}{\mathrm{\ψ}}_F^k)---\left(31\right)$

其中

${\mathrm{\θ}}_F^k={\mathrm{\θ}}_F^{k-1}+{\mathrm{\Δ\θ}}_F^k---\left(32\right)$

以下算法给出了针对P&H类铲斗的运动跟踪算法。该算法采用当前发动机位置并使用等式31和32来寻找符合约束方程的

的新值。为了可靠收敛，算法需要良好的初始值

实际上，这可以通过从例如图6中提供的良好配置进行初始化来实现，其中，可以使用三角法来明确地解决正向运动。

算法3：使用牛顿方法的正向运动跟踪

输入：当前发动机位置

输出：符合约束方程的配置变量

的值。

优先：先前的发动机和配置变量：

初始化：

${\mathrm{\Δ\ψ}}_F^k={\mathrm{\ψ}}_F^k-{\mathrm{\ψ}}_F^{k-1}$

${\mathrm{\θ}}_F^k={\mathrm{\θ}}_F^{k-1}$

${\mathrm{\Γ}}_F=\mathrm{\Γ}({\mathrm{\θ}}_F^k,{\mathrm{\ψ}}_F^k)$

迭代直到收敛：

$\left|\right|{\mathrm{\&Gamma;}}_F\left|\right|<\mathrm{tol},{\mathrm{\&Delta;\&theta;}}_F^k=-{\left(\frac{\&PartialD;\mathrm{\&Gamma;}({\mathrm{\&theta;}}_F^k,{\mathrm{\&psi;}}_F^k)}{{\&PartialD;\mathrm{\&theta;}}_F}\right)}^{-1}(\mathrm{\&Gamma;}({\mathrm{\&theta;}}_F^k,{\mathrm{\&psi;}}_F^k)+\frac{\&PartialD;\mathrm{\&Gamma;}({\mathrm{\&theta;}}_F^k,{\mathrm{\&psi;}}_F^k)}{\&PartialD;{\mathrm{\&psi;}}_F}\mathrm{\&Delta;}{\mathrm{\&psi;}}_F^k)$

${\mathrm{\&theta;}}_F^k={\mathrm{\&theta;}}_F^k+{\mathrm{\&Delta;\&theta;}}_F^k$

${\mathrm{\&Gamma;}}_F=\mathrm{\&Gamma;}({\mathrm{\&theta;}}_F^k,{\mathrm{\&psi;}}_F^k)$

可以看出，优选实施例提供了一种始终维持铲斗位置的相近近似的精确方法。

尽管参考特定实施例描述了本发明，然而本领域技术人员将意识到，可以以多种其他形式来实现本发明。

Claims (11)

1.一种确定采掘机铲的全球位姿的方法，所述方法包括应用多级计算的步骤，所述多级计算包括：

(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.一种用于确定采掘机铲的全球空间位姿的设备，实质上按照参考附图和/或示例中所示的本发明任一实施例而描述的来确定采掘机铲的全球空间位姿。

Images