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

1、10申请公布号CN101970763A43申请公布日20110209CN101970763ACN101970763A21申请号200980105400622申请日20090107200890008120080108AUE02F3/43200601E02F9/2020060171申请人易斯麦私人有限公司地址澳大利亚昆士兰州72发明人彼得罗斯麦卡里安东尼沃尔顿里德74专利代理机构中科专利商标代理有限责任公司11021代理人戎志敏54发明名称确定电采掘机铲的空间位姿的实时方法57摘要已知采掘挖掘机的全球定位为采掘操作的管理和自动化提供了很多好处。本发明描述了一种全球定位电采掘机铲的位姿的方法。系统从

2、安装在机器外壳上的任意数目个RTKGPS天线以及适合机器摇摆轴的解算器获得测量。使用卡尔曼滤波器来产生全球方位位姿的估计。30优先权数据85PCT申请进入国家阶段日2010081686PCT申请的申请数据PCT/AU2009/0000192009010787PCT申请的公布数据WO2009/086601EN2009071651INTCL19中华人民共和国国家知识产权局12发明专利申请权利要求书1页说明书14页附图8页CN101970763A1/1页21一种确定采掘机铲的全球位姿的方法，所述方法包括应用多级计算的步骤，所述多级计算包括A第一级，使用全球定位系统、倾斜计以及摇摆轴解算器来计算采掘机

3、铲车体C坐标系相对于本地大地坐标系G坐标系的方位；B第二级，使用全球定位系统、轴惯性传感器和摇摆轴解算器来计算相对于C坐标系的外壳位姿H坐标系；C第三级，使用推压和提升轴解算器来计算相对于H坐标系的勺斗位姿B坐标系。2根据权利要求1所述的方法，其中，使用扩展卡尔曼滤波器来执行所述步骤A和B。3根据前述权利要求中的任一项权利要求所述的方法，其中，使用迭代例程来执行步骤A直到收敛。4根据前述权利要求中的任一项权利要求所述的方法，其中，倾斜计是双轴倾斜计。5根据前述权利要求中的任一项权利要求所述的方法，其中，倾斜计是六轴惯性传感器。6根据前述权利要求中的任一项权利要求所述的方法，其中，机铲的第一部分

4、包括机器外壳。7一种全球定位采掘机铲的位姿的方法，实质上按照参考附图和/或示例中所示的本发明任一实施例而描述的来全球定位采掘机铲的位姿。8一种按照三级计算过程来确定电采掘机铲的全球位姿的方法，其中，A在第一级，使用全球定位系统、双轴倾斜计以及摇摆轴解算器来计算车体C坐标系相对于本地大地坐标系G坐标系的方位，直到收敛；B在第二级，使用全球定位系统、六轴惯性传感器三个速率陀螺仪和三个线性加速度和摇摆轴解算器来计算相对于C坐标系的外壳位姿H坐标系；C在第三级，使用推压和提升轴解算器来计算相对于H坐标系的勺斗位姿B坐标系。9一种确定采掘机铲的全球空间位姿的方法，所述方法包括以下步骤A指定参考的第一地心

5、地固ECEF坐标系或E坐标系；B指定在采掘机铲附近表示为G坐标系的本地大地坐标系，G坐标系被限定为E坐标系中的笛卡尔坐标轴集合；C指定靠近采掘机铲的本体或底架、表示为C坐标系的笛卡尔坐标轴集合；D确定G坐标系内C坐标系的方位；E指定在采掘机铲的机器外壳附近表示为H坐标系的笛卡尔坐标轴集合；F确定C坐标系内H坐标系的方位；G指定被固定到机铲柄和铲斗装置附近、表示为B坐标系的笛卡尔坐标轴集合；以及H确定H坐标系内B坐标系的方位。10一种确定采掘机铲的全球空间位姿的方法，实质上按照参考附图和/或示例中所示的本发明任一实施例而描述的来确定采掘机铲的全球空间位姿。11一种用于确定采掘机铲的全球空间位姿的

6、设备，实质上按照参考附图和/或示例中所示的本发明任一实施例而描述的来确定采掘机铲的全球空间位姿。权利要求书CN101970763A1/14页3确定电采掘机铲的空间位姿的实时方法技术领域0001本发明涉及对设备进行定位的领域，具体地，公开了一种用于确定在采掘操作中使用的转臂式装载设备如，电采掘机铲的空间位姿的系统。背景技术0002参考文献0003DEPARTMENTOFTHEARMY，1993，FM62TACTICS，TECHNIQUES，ANDPROCEDURESFORFIELDARTILLERYSURVEY，DEPARTMENTOFTHEARMY，WASHINGTONDC0004DIZCHA

7、VEZ，RF，2001，TWOANTENNAPOSITIONINGSYSTEMFORSURFACEMINEEQUIPMENT，USPATENT61917330005GELB，A，1996，APPLIEDOPTIMALESTIMATION，THEMITPRESS，CAMBRIDGE0006KALAFUT，JJ，ALIG，JS，2002，METHODFORDETERMININGAPOSITIONANDHEADINGOFAWORKMACHINE，USPATENT64183640007PIKE，J，2006，WORLDGEODETICSYSTEM1984，0NLINEAVAILABLEATHTTP/W

8、WWGLOBALSECURITYORG/MILITARY/LIBRARY/POLICY/ARMY/FM/62/FIGE1GIF0008SAHM，WCETAL，1995，METHODANDAPPARATUSFORDETERMININGTHELOCATIONOFAWORKIMPLEMENT，USPATENT54046610009TU，CHETAL，1997，GPSCOMPASSANOVELNAVIGATIONEQUIPMENT，IEEETRANSACTIONSONAEROSPACEANDELECTRONICSYSTEMS，33，106310680010VANICECK，P，KRAKIWSKY，E，

9、1986，GEODESYTHECONCEPTS，ELSEVIERSCIENCEPUBLISHERSBV，AMSTERDAM0011GELB，AANDVANDERVELDE，WE，MULTIPLEINPUTDESCRIBINGFUNCTIONSANDNONLINEARSYSTEMDESIGN，MCGRAWHILLBOOKCOMPANY，NEWYORK19680012GRAHAM，DANDMCRUER，D，ANALYSISOFNONLINEARCONTROLSYSTEMS，JOHNWILEYSONSINC，NEWYORK19610013DUDDEKETAL1992；METHODOFDETERMIN

10、INGMININGPROGRESSINOPENCASTMININGBYMEANSOFSATELLITEGEODESY，USPATENT51443170014之前已经提出了确定移动设备单元的位置和方向的问题的多种解决方案。解决方案不变地利用多种定位传感器，包括上述传感器中的一些。0015DUDDEK等1992公开了一种使用勺轮附近的GPS传感器和接收机来确定挖掘机勺斗末端的位置和方向的方法。0016KALAFUT等2002提出了一种用于通过使用单个定位传感器来确定机器的位置和方向的系统。随时间从定位传感器获取读数，产生运动曲线图以估计机器的方向。该方法具体可应用于一般在运动中并且具有良好动态特性

11、的机器。在采掘应用中，只要这些托运卡车在运动中，则托运卡车是这种方法的良好备选。0017单传感器定位系统的另一示例是由SAHM等1995提出的单传感器定位系统，该说明书CN101970763A2/14页4单传感器定位系统使用单个传感器，所述单个传感器能够采集与采掘机铲的悬臂有关的X，Y，Z位置参数。如果假定机铲的底架在挖掘周期期间是固定的，则可以随时间测量点集合，以产生传感器所在的平面。该估计可以与来自传感器的位置的当前测量一起用于估计机铲勺斗的当前位置。0018DIZCHAVEZ2001还提出了根据所估计的平面来定位的方法。两个GPS天线安装在机器外壳上等高度的已知位置。在机器的操作期间，可

12、以测量外壳的旋转，使用基于标准差分析的计算来形成对两个天线所在平面的估计。根据该平面以及该平面内传感器的当前位置和方向，在给定运动模型和适当接合位置信息的情况下可以定位机器的另一部分。0019希望提供一种确定采掘设备等的空间位姿的方法和设备。0020说明书全文中对现有技术的任何讨论不应被看作是承认现有技术是公知的或构成本领域公知常识的一部分。0021除非上下文明确要求，否则，在说明书全文和权利要求中，词语“包括”等具有与排他或穷尽含义相对的包含含义；也就是说，具有“包括但不限于”的含义。发明内容0022根据本发明的第一方面，提供了一种确定采掘机铲的全球位姿的方法，所述方法包括应用多级计算的步骤

13、，所述多级计算包括0023A第一级，使用全球定位系统、倾斜计以及摇摆轴解算器来计算采掘机铲车体C坐标系相对于本地大地坐标系G坐标系的方位；0024B第二级，使用全球定位系统、轴惯性传感器和摇摆轴解算器来计算相对于C坐标系的外壳位姿H坐标系；0025C第三级，使用推压和提升轴解算器来计算相对于H坐标系的勺斗位姿B坐标系。0026优选地，使用扩展卡尔曼滤波器来执行所述步骤A和B。可以使用迭代例程来执行步骤A直到收敛。倾斜计可以是双轴倾斜计。轴惯性传感器可以是六轴惯性传感器。机铲的第一部分可以包括机器外壳。0027根据本发明的另一方面，提供了一种按照三级计算过程来确定电采掘机铲的全球位姿的方法，其中

14、A在第一级，使用全球定位系统、双轴倾斜计以及摇摆轴解算器来计算车体C坐标系相对于本地大地坐标系G坐标系的方位，直到收敛；B在第二级，使用全球定位系统、六轴惯性传感器三个速率陀螺仪和三个线性加速度和摇摆轴解算器来计算相对于C坐标系的外壳位姿H坐标系；C在第三级，使用推压和提升轴解算器来计算相对于H坐标系的勺斗位姿B坐标系。0028根据本发明的另一方面，提供了一种确定采掘机铲的全球空间位姿的方法，所述方法包括以下步骤A指定参考的第一地心地固ECEF坐标系或E坐标系；B指定在采掘机铲附近表示为G坐标系的本地大地坐标系，G坐标系被限定为E坐标系中的笛卡尔坐标轴集合；C指定靠近采掘机铲的本体或底架、表示

15、为C坐标系的笛卡尔坐标轴集合；D确定G坐标系内C坐标系的方位；E指定在采掘机铲的机器外壳附近表示为H坐标系的笛卡尔坐标轴集合；F确定C坐标系内H坐标系的方位；G指定被固定到机铲柄和铲斗装置附近、表示为B坐标系的笛卡尔坐标轴集合；以及H确定H坐标系内B坐标系的方位。说明书CN101970763A3/14页5附图说明0029现在将参考附图来描述本发明的优选形式0030图1示出了装载托运卡车的电采掘机铲；0031图2示出了E坐标系和G坐标系的定义；0032图3示出了C坐标系、H坐标系和B坐标系的定义；0033图4示出了PHCENTURION控制机铲的摇摆轴的控制系统；0034图5示出了饱和型非线性的

16、特性，包括作为输入函数的描述函数增益；0035图6示出了PH类电采掘机铲的坐标系；0036图7示出了出于定义B坐标系的目的而处于直角配置中的PH类电采掘机铲；以及0037图8示出了优选实施例的方法步骤的流程图。具体实施方式0038如图8所示，优选实施例提供了一种确定电采掘机铲的全球空间位置的改进方法80。全球空间位姿包括0039地心地固ECEF坐标系或E坐标系81的指示；0040本地大地坐标系G坐标系的标识，所述本地大地坐标系被限定为在E坐标系中例如沿着北、东和下惯例的笛卡尔坐标轴集合。该坐标系的原点在采掘机铲附近，典型地在机器所位于的矿产内82；0041固定到采矿机铲83的车体或底架的笛卡尔

17、坐标轴集合的指示。这些轴所限定的笛卡尔坐标系已知为C坐标系；0042G坐标系内C坐标系的方位位置和方向的指示84；0043固定到采掘机铲的机器外壳的笛卡尔坐标轴的指示。这些轴所限定的笛卡尔坐标系已知为H坐标系85；0044C坐标系内H坐标系的方位位置和方向的确定86；0045固定到机铲柄和铲斗勺斗装置的笛卡尔坐标轴集合的指示。这些轴所限定的笛卡尔坐标系已知为B坐标系；0046H坐标系内B坐标系的方位位置和方向的确定。0047这些坐标的定义使得可以在全球坐标系中建立勺斗的方位。0048如图1所示，采掘机铲1和其他类似挖掘机的操作的基本特性是，采掘机铲1和其他类似挖掘机每次维持C坐标系的方位若干分

18、钟。即，对使用履带2的机器的重新定位是不频繁地进行的，当挖掘机顺序地挖掘材料并将材料装载到托运卡车4中时在移动之间的主要活动是机器外壳3的前后摇摆运动。0049优选实施例利用采掘机铲1的这种操作特性来解决确定机铲位姿的问题。0050优选实施例还利用若干可用附加传感器测量的组合，包括0051固定到H帧的一个或多个标识点位置在E坐标系中的实时运动全球定位系统；0052由H坐标系相对于G坐标系的三个正交加速度和三个角速率构成的惯性测量；0053H坐标系相对于G坐标系的纵摇和横摇的倾斜计测量；说明书CN101970763A4/14页60054三个主运动执行器即，摇摆发动机、推压发动机和提升发动机的速度

19、和位置测量；0055来自三个主运动执行器即，摇摆发动机、推压发动机和提升发动机的电压和电流测量；0056机铲操作者通常通过操纵杆设置的参考值，这些参考值是向三个主运动执行器即，摇摆发动机、推压发动机和提升发动机的控制系统的输入。0057优选实施例提出了基于扩展卡尔曼滤波器的递归算法的公式化，所述卡尔曼滤波器使用这些测量的组合来确定全球机铲位姿。0058已知实时机铲位姿有多种用途，这些用途包括00591已存在商业系统的应用，使用对挖掘期间铲斗相对于资源分布图的位置的知识作为允许操作者区分矿石与废料的手段；00602显露重要性的应用，用于使采掘设备自动化，其中需要解决方案的重要问题控制与诸如托运卡

20、车之类的其他设备的交互。如果这样的设备单元具有类似的位姿估计能力，则可以确定设备之间的相对位姿；00613来自扫描距离传感器例如，激光传感器以及可能用于自动化系统中的测距以及用于开发本地数字地形图的毫米波雷达的数据的正确空间登记也需要机铲位姿的知识。0062现有解决方案忽略了可以适于估计问题的估计理论。具体地，可以将问题简化化为状态估计训练，其中，可以将G坐标系、C坐标系、H坐标系和B坐标系的相对位置和方向表示为动态系统的状态，使用对测量的操作者命令参考与机器的结果运动之间的因果关系“过程模型”的知识及时传播机铲位姿的当前知识以概率分布的形式，以便于与来自先前标识的传感器的测量的组合相融合。0

21、063问题简化0064图2和图3示出了与包括多种坐标系的问题有关的几何结构。首先转向图2，示出了相对于地球21来定位地球坐标系E坐标系和大地坐标系G坐标系的几何坐标系。图3示出了车体坐标系C坐标系、外壳坐标系H坐标系和勺斗坐标系B坐标系。0065以两级来计算机铲勺斗的位姿。0066第一级的目的是根据以下测量来计算C坐标系相对于H坐标系的方位0067N个RTKGPS接收机的G坐标系中的位置；0068如H坐标系中固定的双轴倾斜计测量的，ZH轴相对于ZG轴的明显的方向；0069H坐标系绕ZE轴的旋转；0070摇摆发动机的角速度；0071摇摆发动机的电枢电流和电枢电压；0072来自操纵杆的操作者命令参

22、考。0073这些量限定了测量矢量Z和输入矢量U。0074第二级的目的是在找到C坐标系相对于G坐标系的方位的情况下使用以下测量来计算H坐标系相对于C坐标系的方位0075N个RTKGPS接收机的G坐标系中的位置；0076在H坐标系中固定的点在三个正交方向上的角速率和线性加速度的测量，但是说明书CN101970763A5/14页7这在瞬时沿着正交传感器轴的惯性系中测量的；0077H坐标系绕ZC轴的旋转；0078摇摆发动机的角速度；0079摇摆发动机的电枢电流和电枢电压；0080来自操纵杆的操作者命令。0081这些量限定了第二测量矢量Z和第二输入矢量U。0082第三级的目的是使用以下测量以及挖掘装置的

23、运动模型来计算B坐标系相对于H坐标系的方位0083提升发动机的位置0084推压发动机的位置0085级3计算是运动的并且计算B坐标系相对于H坐标系的方位。0086计算进行至确定如下机铲位姿0087在机器已完成任何推进运动并进入正常挖掘活动特征在于反复地往复摇摆之后，第一级计算立即运行足够的时间以得到C坐标系相对于G坐标系的方位的收敛估计。假定G坐标系相对于E坐标系的方位是先验已知的；0088在级1得到收敛之后，发起第二级计算和随后的第三级计算，以规则时间步长来进行第二和第三级计算以确定H坐标系相对于C坐标系的位置以及B坐标系相对于H坐标系的位置；0089当操作者下一次驱动机器时，在驱动运动完成之

24、前计算停止，其中，当驱动运动完成时，再次执行第一级计算以找到C坐标系相对于G坐标系的方位的新的收敛估计。然后计算进行至级2，以此类推。0090支持该分级计算过程的是以下构思在级1使用的测量可以提供与机器的低频运动有关的丰富信息，这些信息足以精确地确定C坐标系相对于G坐标系的位置。在与正常生产相关联的正常往复运动期间，除了大规模摇摆运动之外，具体在挖掘期间机器的外壳也可能发生较小幅度的摆动运动。级2使用的的传感器的测量目的在于精确地确定这些运动。在这种情况下，级2滤波器目的在于更高的估计带宽。0091级1和2的计算方法是扩展卡尔曼滤波器EKF、CELB1974。EKF需要以下形式的系统模型009

25、2WN0，Q10093ZKHXK，UK，KVK，VKN0，R0094其中，FX，U，T是描述系统动态的矢量值函数，所述系统用于基于操作者命令参考的测量及时传播状态和状态协方差的当前估计，因此FX，U，T可以与新得到的测量数据相结合。矢量值函数HXK，UK，K表示关于状态矢量X和输入U的测量。0095EKF需要关于估计状态轨迹对FX，U，T和HXK，UK，K的线性化，以及线性化连续动态到离散时间形式的转换。希望使用以下表示法00960097其中，T是测量更新速率，以及说明书CN101970763A6/14页800980099等式1中的矢量W和V表示过程和测量噪声，并且被认为是通过分别具有协方差Q

26、和R的零均值高斯过程而产生的。0100EKF的计算方案包括以下五个步骤GELB，197401011状态估计传播010201032状态协方差传播010401053卡尔曼增益的计算010601074状态估计更新010801095以时间步长K计算误差协方差01100111等式7至11限定了EKF算法，所述EKF算法提供了以最小均方误差测量的非线性系统的最佳线性状态估计器。等式7至11中的上标“”和“”指示在执行测量之前和之后对量的评估。0112动态模型的形成0113用于在级1和级2传播机铲位姿的动态模型包括作为共有元件将操作者操纵杆参考与矢量值函数FX，U，T中的摇摆运动相关联的因果模型。以下给出了

27、用于CENTURION使能PH机铲的该模型的优选实施例。0114PHCENTURION使能电采掘机铲使用ABBDCS/DCF600多驱动控制器来调节三个DC发动机中每个DC发动机中的发动机速度、电枢电流和场电流。控制器由四个集成组件构成PID或PI发动机速度控制回路、束点电流饱和限制器、PI电流控制回路以及EMF场电流调节器。0115摇摆驱动使用转矩控制和继电器式速度控制的组合，从而摇摆操纵杆位置产生逐段速度参考和电枢电流饱和限制。图4示出了摇摆驱动模型的示意图。参考摇摆发动机速度与实际摇摆发动机速度之差馈送至并入了微分滤波的比例积分微分PID速度控制器41。将速度控制器的输出缩放42成根据摇

28、摆操纵杆参考的幅度来成比例限制的参考电枢电流。受限电流参考与实际电枢电流之间的误差馈送至PI电流控制器43，PI电流控制器43向摇摆发动机输出电枢电压。如推压驱动一样，摇摆驱动具有恒定的场电流，其中DCF600将场电压维持在稳定水平。0116对机铲的驱动进行有效建模需要用于将发动机电枢电流中存在的非线性饱和效应并入的装置。为了将这些效应包含在预测模型中，使用正弦输入描述函数。描述函数将简称为DF主要被开发用于研究非线性动态系统中的极限周期，参见GELB和VANDERVELDE1968以及GRAHAM和MCRUER1961。描述函数方法的基本构思是将动态系统中的说明书CN101970763A7/

29、14页9非线性元件替换成拟线性描述符或描述函数等效增益，所述描述函数等效增益的幅度是输入幅度的函数。0117使用正弦饱和的DF等效增益对电枢电流饱和进行建模。图5示出了饱和放大器的正弦波输出的效应。对于至放大器的、幅度小于饱和极限的输入A/AK1，输出51与输入成比例。对于幅度大于饱和极限的输入A/AK1，输出53变成“被削波的”并且可以由傅里叶级数55来表示，其中，项B3SIN3T、B5SIN5T等表示由非线性饱和元件产生的新频率。对这种饱和进行建模的DF方法假定饱和输出中的高阶项是可忽略的。因此，正弦饱和的DF等效增益采用如下形式01180119其中，B1是输出的傅里叶级数的首项或基项，A

30、是输入的幅度。0120将DF实现成预测模型需要在每个时间步长评估等效增益。如果至电流饱和块的输入大于饱和极限，则通过将输入幅度乘以等效增益来计算饱和输出。0121对于饱和输出的傅里叶变换的高阶项不起主要作用的假设由以下概念来支持机铲的驱动动态起到低通滤波器的作用，输出的基频在经过系统时的衰减比输出的高阶谐波要小得多。0122驱动预测模型被表示为如下形式的连续线性状态空间系统01230124输入矢量U包含根据操纵杆信号而产生的参考发动机速度发动机上由于引力效应而引起的静转矩负载TS，以及库伦摩擦干扰输入FS。对于每个模型，状态矢量X包含摇摆电枢电流IS、摇摆发动机速度S、摇摆发动机位置S、以及速

31、度和电流控制器的误差的积分和0125摇摆驱动模型还包含在饱和极限之前的摇摆参考电枢电流该状态是由于摇摆发动机速度控制器中的微分组件而产生的。摇摆驱动的全状态空间模型由以下给出01260127说明书CN101970763A8/14页100128描述函数增益GS就像是驱动系统和输入矩阵中的元素，该元素是在每个时间步长重新计算的。至电流控制器中的输入是摇摆参数电枢电流状态0129可以根据摇摆操纵杆位置来确定摇摆电枢电流饱和极限。0130注意，为了有效，级1和级2模型应当包含所谓的“整形级”，所述整形级调整传感器中的偏置并处理诸如传动后冲之类的物理假象。0131注意，还可以利用所谓的ORNSTEINU

32、HLENBECK随机过程来实现级2模型，所述ORNSTEINUHLENBECK随机过程的参数可以根据后续自相关分析来确定。0132测量模型0133在给定H坐标系中第N个GPS天线的位置XA，YA，ZA的情况下，等式12表示关于机铲车体的位置XC，YC，ZC和方向余弦矩阵RC2G和RH2C在G坐标系中进行的GPS测量，所述方向余弦矩阵RC2G和RH2C分别描述在C坐标系与G坐标系之间以及在H坐标系与C坐标系之间的3D旋转。可以以多种方式来计算这些矩阵，例如，欧拉角或四元数。描述这些矩阵的参数是估计器的状态。0134ZGPSXC，YC，ZCTRC2GRH2CXA，YA，ZAT120135GPS测量

33、基于双轴椭球形式的地球表面的近似。这些椭球的尺寸由若干标准或数据之一来限定。图2示出了地球的WGS84椭球近似20，其中表示了GPS天线的维度、经度和海拔。用于将以E坐标系维度、经度和海拔测量的来自GPS接收机的传感器读数变换成G坐标系中的坐标的方法如下0136第一级是将测量转换成笛卡尔坐标，其中原点在地球的中心，X轴被限定在0的经度值处如以上在图2中可以看出的。0137VANICEK1986定义了对于任何点，椭球近似上的P00138P0N0COS00139其中N0是与椭球中心的距离，0是与平面XEYE的角度点P0处的海拔。与椭球中心的距离被定义为01400141可以重新布置这些等式以给出全球

34、笛卡尔坐标中点P0的位置矢量01420143其中，0是点P0处的经度。0144为了将该全球位置变换到本地坐标系中，必须定义合适的本地坐标系。定义了以点P0为中心的轴集合，使得Y轴与椭球表面相切，该方向上的点将表现为在站在点P0处的观察者北方与该观察者相对。如果将该方向定义为“视北”，则“视东”是与椭球表面相切并且与矢量R0G和矢量视北正交的矢量。这从而可以定义合适的本地坐标系，其中，Y轴沿着视北，X轴沿着视东，Z轴表示椭球表面上方的高度。该公式化对于任何点P0都成立，使得090。如果090，则X轴和Y轴的方向是正交的，只要X轴和Y轴正交并形成与椭球表面相切的平面。说明书CN101970763A

35、9/14页110145可以认为本地X轴视东具有零海拔相对于P0。如果将该轴表示为矢量RA的单位矢量，则可以将YA任意地设置为1，并使用等式14给出01460147由于需要Y轴视北与X轴和Z轴正交，可以找到Y轴视北0148RBRGRA160149最后，从全球到本地坐标的变换矩阵可以被限定为01500151其中，01520153这给出了对于点PN从全球到本地坐标的最终变换01540155如倾斜计所测量的机器外壳的纵摇和横摇倾斜被确定为平面XG，YG内XH轴和YH轴上单位矢量的夹角。通过使用标准ZXZ旋转惯例，将C坐标系相对于G坐标系的旋转表示为欧拉角纵摇和横摇角是015601570158可以通过对

36、传感器的倾斜提取公因子来改善粗倾斜计测量。0159IMU的加速度被测量为该IMU在机器外壳中的方位XI，YI，ZI的全球加速度，被旋转为沿着正交的传感器轴。在给定机铲车体位置XC，YC，ZC以及分别描述C坐标系与G坐标系之间和H坐标系与C坐标系之间的3D旋转的方向余弦矩阵RC2G和RH2C的情况下，加速度测量是0160ZACCRG2CRC2HAI210161其中根据以下等式得到G坐标系中IMU的加速度01620163假定G坐标系是非加速且非旋转的。0164IMU的角速度将被测量为机器外壳的全球角速度，被旋转为沿着正交的传感器轴。通过使用标准ZXZ旋转惯例将H坐标系相对于C坐标系的旋转表示为欧拉

37、角关于RPY轴的测量角速度是0165说明书CN101970763A10/14页120166PH类机器的级3计算0167级3计算依赖于所讨论的机器。在优选实施例中，针对PH类采掘机铲来执行计算。0168图6示出了用于描述PH类电采掘机铲的几何结构的参数长度和角度以及用于描述这些机器的主要移动装置的相对位置的坐标系。通过设计固定了标记为L的长度以及标记为的角度；标记为D的长度和标记为的角度根据机器运动而变化。需要该几何结构来确定勺斗相对于H坐标系的方位。0169C坐标系表示为OCXCYCZC；H坐标系表示为OHXHYHZH并且嵌入机器外壳中；OMXMYMZMM坐标系在鞍座中；OBXBYBZBB坐标

38、系在铲斗中。所有体固定坐标系的X轴和Z轴都在机器外壳的矢状平面中，即，与包含摇摆轴的、图6所示的投影平面平行的平面。所有坐标系的Y轴都与该平面垂直。0170可以使用四乘四齐次变换矩阵来描述坐标系之间的关系。将描述从OIXIYIZI到OJXJYJZJ的变换的矩阵表示为DIJ，注意该矩阵的动作将J坐标系中的齐次点映射到I坐标系。例如，如果P是方位在OBXBYBZB中已知的、固定在勺斗中的点，则可以据此得到该点在坐标系OCXCYCZC中的坐标。0171PD03P0172DIJ的结构是01730174其中，RIJ是33旋转矩阵，是3维转换矢量。四乘四齐次变换矩阵根据以下等式交换0175DIKDIJDJ

39、K。0176C坐标系的原点位于履带上表面与机器外壳下表面之间的接合处。ZC轴与摇摆轴共线。XC轴指向履带的前向行进方向，YC轴完成右侧三面形坐标系。0177坐标系OHXHYHZH的原点OH与OC重合，ZH与ZC共线。当10时，坐标系OCXCYCZC和OHXHYHZH重合。当从上看时，正角度1与机器外壳相对于履带的逆时针旋转相对应。齐次变换矩阵DCH由以下给出01780179OMXMYMZM被固定到鞍座，其中O2在鞍座的旋转中心。当2等于0时，OMXMYMZM的坐标方向与OHXHYHZH的坐标方向平行。描述从坐标系H到坐标系N的刚体位移的位移矩阵由以下给出说明书CN101970763A11/14

40、页1301800181其中设计参数L1和1如图6所示。0182OBXBYBZB的原点OB定位如下。鞍座角2被设置为等于90度，使得柄水平。然后移位柄，使得提升绳垂直降落590度，并且60度。原点OB位于柄支架和提升绳的节线的交叉处；ZB被设置为与提升绳的轴共线；XB被设置为与柄支架的节线平行。注意，轴XM与XB正交。描述刚体位移DMB的位移矩阵由以下给出01830184上述等式的相乘给出018501860187旋角1、摆角2、和推压伸长D3使体固定坐标系相对于世界坐标系的位移和旋转参数化。这些配置变量可以如下分组01881，2，D3T。0189摇摆发动机、推压发动机C和提升发动机H可以类似地被

41、分组为0190S，C，HT。0191的值确定，反之亦然。这些映射不是双射的。然而，在这些变量的物理工作范围内，这些变量是一一对应的。注意，或的说明确定了提升绳的倾斜，图6中标记为5的第七变量。0192为了建立约束方程，首先注意相关坐标S和C通过传动比与1和D3有关，得到01930194其中，GS是摇摆驱动的传动比，GC是推压驱动的传动比。0195可以使用图6所示的矢量循环来使2和5与C和H有关的约束方程。为了简化表示，引入映射到物理X1，Z1平面的复平面，其中实轴与X1共线，虚轴与Z1共线。0196对图6的矢量循环中的矢量进行求和，其中将Z1表示为复变量，得到01970，5Z1Z2Z3Z4Z5

42、Z6Z7，0198其可以被扩展以得到说明书CN101970763A12/14页1401990200其中，变量LI、DI、I和I如图7中所定义的。然后使用通过检查而确定的以下关系0201020242，0203等式24可以被写成02040205为了从该等式中去除D5，方便的是首先引入变量DH。如图6所示，DH表示当提升绳垂直悬挂即，590度时从吊桶销到提升滑轮的外部扇形齿轮之间的距离。DH通过以下等式与提升发动机的角位移相关02060207其中，GH是提升传动比。上述表达式和等式25可以用于使D5与DH相关，02080209等式7右侧最后一项表示围绕滑轮的提升绳的包角。将等式27代入等式26，得到

43、021002110212考虑等式28的实分量和虚分量，使H和5与广义坐标相关021302140215注意，在推导等式29的过程中使用了以下三角关系02160217连接等式23A和29，得到02180219说明书CN101970763A13/14页150220利用牛顿拉夫申法的运动跟踪0221运动跟踪问题是确定在给定的情况下的和5的值以及在给定情况下确定和5。将第一种问题称作是前向运动跟踪，将第二种问题称作是反向运动跟踪。为了便于区分这两种问题，当工作于正向运动跟踪问题的领域并且时，写作0222F1，2，3，50223FS，C，H0224这里区别在于5的分组。这两种问题数学上相当于解非线性约束方

44、程。本发明选择使用多变量牛顿方法来迭代地执行这一操作。在表示牛顿方法解决方案中开发的雅可比矩阵用于将发动机惯性引入配置变量并解决静力问题。0225正向运动跟踪0226对等式30应用泰勒级数扩展022702280229目的是找到有效配置，即，FF，FF0。然后02300231这得到迭代方程02320233其中02340235以下算法给出了针对PH类铲斗的运动跟踪算法。该算法采用当前发动机位置并使用等式31和32来寻找符合约束方程的的新值。为了可靠收敛，算法需要良好的初始值实际上，这可以通过从例如图6中提供的良好配置进行初始化来实现，其中，可以使用三角法来明确地解决正向运动。0236算法3使用牛顿

45、方法的正向运动跟踪0237输入当前发动机位置0238输出符合约束方程的配置变量的值。说明书CN101970763A14/14页160239优先先前的发动机和配置变量0240初始化0241024202430244迭代直到收敛0245024602470248可以看出，优选实施例提供了一种始终维持铲斗位置的相近近似的精确方法。0249尽管参考特定实施例描述了本发明，然而本领域技术人员将意识到，可以以多种其他形式来实现本发明。说明书CN101970763A1/8页17图1说明书附图CN101970763A2/8页18图2说明书附图CN101970763A3/8页19图3说明书附图CN101970763A4/8页20图4说明书附图CN101970763A5/8页21图5说明书附图CN101970763A6/8页22图6说明书附图CN101970763A7/8页23图7说明书附图CN101970763A8/8页24图8说明书附图