确定电采掘机铲的空间位姿的实时方法 技术领域 本发明涉及对设备进行定位的领域， 具体地， 公开了一种用于确定在采掘操作中 使用的转臂式装载设备 ( 如， 电采掘机铲 ) 的空间位姿的系统。
背景技术 参考文献
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 坐标系中固定的双轴倾斜计测量的， zh 轴相对于 zg 轴的明显的方向 ；
● h 坐标系绕 ze 轴的旋转 ；
●摇摆发动机的角速度 ；
●摇摆发动机的电枢电流和电枢电压 ；
●来自操纵杆的操作者命令参考。
这些量限定了测量矢量 z 和输入矢量 u。
第二级的目的是在找到 c 坐标系相对于 g 坐标系的方位的情况下使用以下测量来 计算 h 坐标系相对于 c 坐标系的方位
● n 个 RTK-GPS 接收机的 g 坐标系中的位置 ；
●在 h 坐标系中固定的点在三个正交方向上的角速率和线性加速度的测量， 但是
这在瞬时沿着正交传感器轴的惯性系中测量的 ；
● h 坐标系绕 zc 轴的旋转 ；
●摇摆发动机的角速度 ；
●摇摆发动机的电枢电流和电枢电压 ；
●来自操纵杆的操作者命令。
这些量限定了第二测量矢量 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)
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 需要关于估计状态轨迹 对 f(x， u， t) 和 h(xk， uk， k) 的线性化， 以及线性化连 续动态到离散时间形式的转换。希望使用以下表示法 ：
其中， Δt 是测量更新速率， 以及等式 1 中的矢量 w 和 v 表示过程和测量噪声， 并且被认为是通过分别具有协方差 Q 和 R 的零均值高斯过程而产生的。
EKF 的计算方案包括以下五个步骤 (Gelb， 1974) ：
1. 状态估计传播
2. 状态协方差传播 3. 卡尔曼增益的计算4. 状态估计更新5. 以时间步长 k 计算误差协方差等式 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 来表示， 其中， 项 b3 sin 3ωt、 b5 sin 5ωt 等表示由非线性饱和元件产 生的新频率。对这种饱和进行建模的 DF 方法假定饱和输出中的高阶项是可忽略的。因此， 正弦饱和的 DF 等效增益采用如下形式
其中， b1 是输出的傅里叶级数的首项或基项， A 是输入的幅度。
将 DF 实现成预测模型需要在每个时间步长评估等效增益。如果至电流饱和块的 输入大于饱和极限， 则通过将输入幅度乘以等效增益来计算饱和输出。
对于饱和输出的傅里叶变换的高阶项不起主要作用的假设由以下概念来支持 ： 机 铲的驱动动态起到低通滤波器的作用， 输出的基频在经过系统时的衰减比输出的高阶谐波 要小得多。
驱动预测模型被表示为如下形式的连续线性状态空间系统
输入矢量 u 包含 ： 根据操纵杆信号而产生的参考发动机速度发动机上由于引以及库伦摩擦干扰输入 fs。对于每个模型， 状态矢量 x 包含 力效应而引起的静转矩负载 Ts， 摇摆电枢电流 Is、 摇摆发动机速度 ωs、 摇摆发动机位置 θs、 以及速度和电流控制器的误差 的积分
和 摇摆驱动模型还包含在饱和极限之前的摇摆参考电枢电流 该状态是由于摇摆发动机速度控制器中的微分组件而产生的。摇摆驱动的全状态空间模型由以下给出 ：
描述函数增益 Gs 就像是驱动系统和输入矩阵中的元素， 该元素是在每个时间步长重新计算的。至电流控制器中的输入是摇摆参数电枢电流状态 可以根据摇摆操纵杆位置来确定摇摆电枢电流饱和极限。
注意， 为了有效， 级 1 和级 2 模型应当包含所谓的 “整形级” ， 所述整形级调整传感 器中的偏置并处理诸如传动后冲之类的物理假象。
注意， 还可以利用所谓的 Ornstein-Uhlenbeck 随机过程来实现级 2 模型， 所述 Ornstein-Uhlenbeck 随机过程的参数可以根据后续自相关分析来确定。
测量模型
在给定 h 坐标系中第 n 个 GPS 天线的位置 (xa， ya， za) 的情况下， 等式 12 表示关于 机铲车体的位置 (xc， yc， zc) 和方向余弦矩阵 Rc2g 和 Rh2c 在 g 坐标系中进行的 GPS 测量， 所 述方向余弦矩阵 Rc2g 和 Rh2c 分别描述在 c 坐标系与 g 坐标系之间以及在 h 坐标系与 c 坐标 系之间的 3D 旋转。可以以多种方式来计算这些矩阵， 例如， 欧拉角或四元数。描述这些矩 阵的参数是估计器的状态。
zgps ＝ (xc， yc， zc)T+Rc2gRh2c(xa， ya， za)T (12)
GPS 测量基于双轴椭球形式的地球表面的近似。这些椭球的尺寸由若干标准或数 据之一来限定。图 2 示出了地球的 WGS84 椭球近似 20， 其中表示了 GPS 天线的维度、 经度 和海拔。用于将以 e 坐标系维度、 经度和海拔测量的来自 GPS 接收机的传感器读数变换成 g 坐标系中的坐标的方法如下 ：
●第一级是将测量转换成笛卡尔坐标， 其中原点在地球的中心， x 轴被限定在 0° 的经度值处 ( 如以上在图 2 中可以看出的 )。
● Vanicek(1986) 定义了对于任何点， 椭球近似上的 p0 ：
p0 ＝ N0 cosφ0
其中 N0 是与椭球中心的距离， φ0 是与平面 xe-ye 的角度 ( 点 p0 处的海拔 )。与椭 球中心的距离被定义为 ：
●可以重新布置这些等式以给出全球笛卡尔坐标中点 p0 的位置矢量 ：
其中， λ0 是点 p0 处的经度。
●为了将该全球位置变换到本地坐标系中， 必须定义合适的本地坐标系。定义了 以点 p0 为中心的轴集合， 使得 y 轴与椭球表面相切， 该方向上的点将表现为在站在点 p0 处 的观察者北方与该观察者相对。如果将该方向定义为 “视北” ， 则 “视东” 是与椭球表面相切 G 并且与矢量 r0 和矢量视北正交的矢量。这从而可以定义合适的本地坐标系， 其中， y 轴沿 着视北， x 轴沿着视东， z 轴表示椭球表面上方的高度。该公式化对于任何点 p0 都成立， 使 得 λ0 ≠ ±90°。如果 λ0 ≠ ±90°， 则 x 轴和 y 轴的方向是正交的， 只要 x 轴和 y 轴正交 并形成与椭球表面相切的平面。
●可以认为本地 x 轴 ( 视东 ) 具有零海拔 ( 相对于 p0)。如果将该轴表示为矢量 ra 的单位矢量， 则可以将 ya 任意地设置为 1， 并使用等式 14 给出 ：
●由于需要 y 轴 ( 视北 ) 与 x 轴和 z 轴正交， 可以找到 y 轴 ( 视北 ) ： rb ＝ rG×ra (16) ●最后， 从全球到本地坐标的变换矩阵可以被限定为 ：
其中，
●这给出了对于点 pn 从全球到本地坐标的最终变换 ：如倾斜计所测量的机器外壳的纵摇和横摇倾斜被确定为平面 (xg， yg) 内 xh 轴和 yh 轴上单位矢量的夹角。通过使用标准 Z-X-Z 旋转惯例， 将 c 坐标系相对于 g 坐标系的旋转
表示为欧拉角
纵摇 (α) 和横摇 (β) 角是
可以通过对传感器的倾斜提取公因子来改善粗倾斜计测量。
IMU 的加速度被测量为该 IMU 在机器外壳中的方位 (xi， yi， zi) 的全球加速度， 被 旋转为沿着正交的传感器轴。在给定机铲车体位置 (xc， yc， zc ) 以及分别描述 c 坐标系与 g 坐标系之间和 h 坐标系与 c 坐标系之间的 3D 旋转的方向余弦矩阵 Rc2g 和 Rh2c 的情况下， 加速度测量是 ：
zacc ＝ Rg2cRc2hai (21)
其中根据以下等式得到 g 坐标系中 IMU 的加速度
假定 g 坐标系是非加速且非旋转的。 IMU 的角速度将被测量为机器外壳的全球角速度， 被旋转为沿着正交的传感器轴。通过使用标准 Z-X-Z 旋转惯例将 h 坐标系相对于 c 坐标系的旋转表示为欧拉角 关于 RPY 轴的测量角速度是 ：
P&H 类机器的级 3 计算 级 3 计算依赖于所讨论的机器。在优选实施例中， 针对 P&H 类采掘机铲来执行计算。 图 6 示出了用于描述 P&H 类电采掘机铲的几何结构的参数 ( 长度和角度 ) 以及用 于描述这些机器的主要移动装置的相对位置的坐标系。通过设计固定了标记为 l 的长度以 及标记为 φ 的角度 ； 标记为 d 的长度和标记为 θ 的角度根据机器运动而变化。需要该几 何结构来确定勺斗相对于 h 坐标系的方位。
c 坐标系表示为 Ocxcyczc ； h 坐标系表示为 Ohxhyhzh 并且嵌入机器外壳中 ； Omxmymzmm 坐标系在鞍座中 ； Obxbybzb b 坐标系在铲斗中。所有体固定坐标系的 x 轴和 z 轴都在机器外 壳的矢状平面中， 即， 与包含摇摆轴的、 图 6 所示的投影平面平行的平面。所有坐标系的 y 轴都与该平面垂直。
可以使用四乘四齐次变换矩阵来描述坐标系之间的关系。将描述从 Oixiyizi 到 Ojxjyjzj 的变换的矩阵表示为 Di → j， 注意该矩阵的动作将 j 坐标系中的 ( 齐次 ) 点映射到 i 坐标系。 例如， 如果 p 是方位在 Obxbybzb 中已知的、 固定在勺斗中的点， 则可以据此得到该点 在坐标系 Ocxcyczc 中的坐标。
p′＝ D0 → 3p.
Di → j 的结构是
其中， Ri → j 是 3×3 旋转矩阵， 是 3 维转换矢量。四乘四齐次变换矩阵根据以下等 式交换 ：
Di → k ＝ Di → jDj → k。
c 坐标系的原点位于履带上表面与机器外壳下表面之间的接合处。zc 轴与摇摆轴 共线。xc 轴指向履带的前向行进方向， yc 轴完成右侧三面形坐标系。
坐标系 Ohxhyhzh 的原点 Oh 与 Oc 重合， zh 与 zc 共线。当 θ1 ＝ 0 时， 坐标系 Ocxcyczc 和 Ohxhyhzh 重合。当从上看时， 正角度 θ1 与机器外壳相对于履带的逆时针旋转相对应。齐 次变换矩阵 Dc → h 由以下给出 ：
Omxmymzm 被固定到鞍座， 其中 O2 在鞍座的旋转中心。当 θ2 等于 0 时， Omxmymzm 的坐 标方向与 Ohxhyhzh 的坐标方向平行。描述从坐标系 h 到坐标系 n 的刚体位移的位移矩阵由 以下给出 ：
其中设计参数 l1 和 φ1 如图 6 所示。
Obxbybzb 的原点 Ob 定位如下。鞍座角 (θ2) 被设置为等于 90 度， 使得柄水平。然 后移位柄， 使得提升绳垂直降落 (θ5 ＝ 90 度， 并且 θ6 ＝ 0 度 )。原点 Ob 位于柄支架和提 升绳的节线的交叉处 ； zb 被设置为与提升绳的轴共线 ； xb 被设置为与柄支架的节线平行。 注 意， 轴 xm 与 xb 正交。描述刚体位移 Dm → b 的位移矩阵由以下给出
上述等式的相乘给出 ：
旋角 θ1、 摆角 θ2、 和推压伸长 d3 使体固定坐标系相对于世界坐标系的位移和旋 转参数化。这些配置变量可以如下分组 ：
θ ＝ (θ1， θ2， d3)T。
摇摆发动机、 推压发动机 θc 和提升发动机 θh 可以类似地被分组为 ：
ψ ＝ (θs， θc， θh)T。
θ 的值确定 ψ， 反之亦然。这些映射不是双射的。然而， 在这些变量的物理工作 范围内， 这些变量是一一对应的。注意， θ 或 ψ 的说明确定了提升绳的倾斜， 图 6 中标记 为 θ5 的第七变量。
为了建立约束方程， 首先注意相关坐标 θs 和 θc 通过传动比与 θ1 和 d3 有关， 得 到：
其中， Gs 是摇摆驱动的传动比， Gc 是推压驱动的传动比。
可以使用图 6 所示的矢量循环来使 θ2 和 θ5 与 θc 和 θh 有关的约束方程。为了 简化表示， 引入映射到物理 (x1， z1) 平面的复平面， 其中实轴与 x1 共线， 虚轴与 z1 共线。
对图 6 的矢量循环中的矢量进行求和， 其中将 z1 表示为复变量， 得到 ：
0 ＝ ζ(θ， ψ， θ5) ＝ z1+z2+z3+z4+z5-z6-z7，
其可以被扩展以得到 ：
其中， 变量 li、 d i、 θi 和 φi 如图 7 中所定义的。然后使用通过检查而确定的以下关系 ：
θ4 ＝ θ2， 等式 24 可以被写成 ：为了从该等式中去除 d5， 方便的是首先引入变量 dh。如图 6 所示， dh 表示当提升绳 垂直悬挂 ( 即， θ5 ＝ 90 度 ) 时从吊桶销到提升滑轮的外部扇形齿轮之间的距离。dh 通过 以下等式与提升发动机的角位移相关 ：
其中， Gh 是提升传动比。上述表达式和等式 25 可以用于使 d5 与 dh 相关，等式 7 右侧最后一项表示围绕滑轮的提升绳的包角。 将等式 (27) 代入等式 (26)，得到
考虑等式 28 的实分量和虚分量， 使 θh 和 θ5 与广义坐标相关 ：
注意， 在推导等式 (29) 的过程中使用了以下三角关系连接等式 23a 和 29， 得到 ：利用牛顿 - 拉夫申法的运动跟踪
运动跟踪问题是确定在给定 ψ 的情况下的 θ 和 θ5 的值以及在给定 ψ 情况下确 定 ψ 和 θ5。将第一种问题称作是前向运动跟踪， 将第二种问题称作是反向运动跟踪。为
了便于区分这两种问题， 当工作于正向运动跟踪问题的领域并且
时， 写作θF ＝ (θ1， θ2， θ3， θ5)
ψF ＝ (θs， θc， θh)
这里区别在于 θ5 的分组。这两种问题数学上相当于解非线性约束方程。本发明 选择使用多变量牛顿方法来迭代地执行这一操作。 在表示牛顿方法解决方案中开发的雅可 比矩阵用于将发动机惯性引入配置变量并解决静力问题。
正向运动跟踪 对等式 30 应用泰勒级数扩展目的是找到有效配置， 即， Γ(θF+ΔθF， ψF+ΔψF) ＝ 0。然后这得到迭代方程 ：其中 以下算法给出了针对 P&H 类铲斗的运动跟踪算法。 该算法采用当前发动机位置 实际上， 这可以通过从例如图 6 中提供的良好配置进行初始化来实现， 其中， 可以并使用等式 31 和 32 来寻找符合约束方程的 的新值。为了可靠收敛， 算法需要良好的初 始值 使用三角法来明确地解决正向运动。
算法 3 ： 使用牛顿方法的正向运动跟踪
输入 ： 当前发动机位置 输出 ： 符合约束方程的配置变量 的值。优先 ： 先前的发动机和配置变量 ： 初始化 ：迭代直到收敛 ：可以看出， 优选实施例提供了一种始终维持铲斗位置的相近近似的精确方法。
尽管参考特定实施例描述了本发明， 然而本领域技术人员将意识到， 可以以多种 其他形式来实现本发明。