CN107532897B - 提供对运动学加速度的补偿的方法和补偿测量结果的系统 - Google Patents

提供对运动学加速度的补偿的方法和补偿测量结果的系统 Download PDF

Info

Publication number
CN107532897B
CN107532897B CN201680021501.5A CN201680021501A CN107532897B CN 107532897 B CN107532897 B CN 107532897B CN 201680021501 A CN201680021501 A CN 201680021501A CN 107532897 B CN107532897 B CN 107532897B
Authority
CN
China
Prior art keywords
sensor
coordinate system
kinematic
movable member
acceleration
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201680021501.5A
Other languages
English (en)
Other versions
CN107532897A (zh
Inventor
D·杜沙
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Leica Geosystems AG
Original Assignee
Leica Geosystems AG
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Priority claimed from AU2015901311A external-priority patent/AU2015901311A0/en
Application filed by Leica Geosystems AG filed Critical Leica Geosystems AG
Publication of CN107532897A publication Critical patent/CN107532897A/zh
Application granted granted Critical
Publication of CN107532897B publication Critical patent/CN107532897B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • E—FIXED CONSTRUCTIONS
    • E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
    • E02F—DREDGING; SOIL-SHIFTING
    • E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
    • E02F9/26—Indicating devices
    • E02F9/267—Diagnosing or detecting failure of vehicles
    • E02F9/268—Diagnosing or detecting failure of vehicles with failure correction follow-up actions
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B21/00—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant
    • G01B21/02—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant for measuring length, width, or thickness
    • G01B21/04—Measuring arrangements or details thereof, where the measuring technique is not covered by the other groups of this subclass, unspecified or not relevant for measuring length, width, or thickness by measuring coordinates of points
    • G01B21/045—Correction of measurements
    • E—FIXED CONSTRUCTIONS
    • E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
    • E02F—DREDGING; SOIL-SHIFTING
    • E02F3/00—Dredgers; Soil-shifting machines
    • E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
    • E02F3/28—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets
    • E02F3/30—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets with a dipper-arm pivoted on a cantilever beam, i.e. boom
    • E02F3/32—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets with a dipper-arm pivoted on a cantilever beam, i.e. boom working downwardly and towards the machine, e.g. with backhoes
    • E—FIXED CONSTRUCTIONS
    • E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
    • E02F—DREDGING; SOIL-SHIFTING
    • E02F3/00—Dredgers; Soil-shifting machines
    • E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
    • E02F3/28—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets
    • E02F3/36—Component parts
    • E02F3/42—Drives for dippers, buckets, dipper-arms or bucket-arms
    • E02F3/43—Control of dipper or bucket position; Control of sequence of drive operations
    • E02F3/431—Control of dipper or bucket position; Control of sequence of drive operations for bucket-arms, front-end loaders, dumpers or the like
    • E—FIXED CONSTRUCTIONS
    • E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
    • E02F—DREDGING; SOIL-SHIFTING
    • E02F3/00—Dredgers; Soil-shifting machines
    • E02F3/04—Dredgers; Soil-shifting machines mechanically-driven
    • E02F3/28—Dredgers; Soil-shifting machines mechanically-driven with digging tools mounted on a dipper- or bucket-arm, i.e. there is either one arm or a pair of arms, e.g. dippers, buckets
    • E02F3/36—Component parts
    • E02F3/42—Drives for dippers, buckets, dipper-arms or bucket-arms
    • E02F3/43—Control of dipper or bucket position; Control of sequence of drive operations
    • E02F3/435—Control of dipper or bucket position; Control of sequence of drive operations for dipper-arms, backhoes or the like
    • E—FIXED CONSTRUCTIONS
    • E02—HYDRAULIC ENGINEERING; FOUNDATIONS; SOIL SHIFTING
    • E02F—DREDGING; SOIL-SHIFTING
    • E02F9/00—Component parts of dredgers or soil-shifting machines, not restricted to one of the kinds covered by groups E02F3/00 - E02F7/00
    • E02F9/26—Indicating devices
    • E02F9/264—Sensors and their calibration for indicating the position of the work tool
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C19/00—Gyroscopes; Turn-sensitive devices using vibrating masses; Turn-sensitive devices without moving masses; Measuring angular rate using gyroscopic effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C9/00—Measuring inclination, e.g. by clinometers, by levels
    • G01C9/02—Details
    • G01C9/08—Means for compensating acceleration forces due to movement of instrument

Landscapes

  • Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Civil Engineering (AREA)
  • Mining & Mineral Resources (AREA)
  • General Engineering & Computer Science (AREA)
  • Structural Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Operation Control Of Excavators (AREA)
  • Gyroscopes (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

提供对运动学加速度的补偿的方法和补偿测量结果的系统。补偿影响工作设备(诸如挖掘机)的传感器测量结果的运动学加速度的方法和系统。该方法和系统识别工作设备的相对于彼此可移动的构件(例如，斗杆、动臂、铲斗)，并且限定针对每个可移动构件的坐标系。运动学关系优选地为运动学链。然后根据运动学关系和每个所识别构件的相对位置修改传感器测量结果。

Description

提供对运动学加速度的补偿的方法和补偿测量结果的系统
技术领域
本发明涉及动态运动补偿。特别地，本发明涉及但不限于针对由于动态移动(诸如例如在车辆或车辆的部分移动时)引起的不想要变化来补偿传感器测量结果(诸如测斜仪测量结果)。
背景技术
这里对背景技术的参考不被解释为这种技术构成常见一般知识的许可。
精确机器引导主要由于可以在准确机器控制系统的帮助下获得的生产力改善而在许多行业中变得越来越常见。在土木建筑和采矿中，机械(诸如挖掘机、推土机以及平地机)全部可以受益于精确机器引导和控制。
因为精确引导设备能够计算工作边缘(例如，挖掘机铲斗或推土铲)相对于参考(诸如弦线)的位置，所以装配有精确引导的机器要求与现场测量员的大幅减少交互。在机器还装配有精确定位系统(诸如全球导航卫星系统(GNSS)实时运动学(RTK)系统)或光学仪器(诸如全站仪)的情况下，显著降低或在一些情况下能够完全消除对在现场工作的测量员的需要。
工作边缘的位置的实时反馈还借助对被移动(例如，切割或填充)材料的快速识别且在减少获得期望结果所要求的返工时允许机器的操作员更高效。甚至依赖精确且及时定位的更高级应用包括工作设备的半自动或全自动控制，这进一步提高机器可以工作的速度，并且减少由操作员控制机械所需的训练和经验。
土木建筑和开采场所通常要求厘米精度甚至毫米精度。这种高精度需要直接影响引导设备的性能要求，并且寻求不断改进，以用合理的成本提高准确度。
市场上许多成功的机器引导解决方案(诸如Leica Geosystems iCON ExcavateiXE3系统)由安装到机器的可移动构件(诸如具有动臂、斗杆以及铲斗的挖掘机的臂)的测斜仪或加速度计构成。测斜仪在它附接到的构件的坐标系中测量当地重力向量，由此可以计算该构件相对于纵轴的角度。每个可移动构件的角度的知识连同机架的位置和方位(节距、滚动以及航向)和机器的几何结构允许计算工作边缘(例如，挖掘机铲斗的唇)的位置。
当前系统通常具有良好静态性能(即，确定当不移动时的位置)，但具有非常差动态性能(即，确定当存在移动时的位置)。在典型使用中，操作员必须为了取得准确位置测量结果而定期暂停，以允许设备稳定。根据从事的动作，可能需要多次暂停。这种不工作时段由于它们减少设备可以工作的时间量而明显地降低效率。
虽然例如可以使用旋转或角度编码器等测量构件位置的直接测量结果，但这种传感器难以改造成机器且经受维护和校准。因此，这种直接测量系统通常不实用或对于售后引导系统来说在商业上不可行，因此，大多数系统依赖用于角测量的测斜仪。
基于测斜仪的测量系统的差动态性能的根本原因是由于重力向量的测量结果被由于机器及其移动构件的移动而引起的加速度、震动以及振动破坏。
在航空和航天工业中观测到操纵控制飞机可能将大量误差引入到重力向量的测量结果中的这种效应。航空和航天工业所采用的解决方案是使用陀螺仪，该陀螺仪测量角度随着时间的变化率，累积其测量结果，以追踪角度。因为陀螺仪测量结果的累积还将累积误差，所以所计算的角度被缓慢引导到重力向量的测量结果。传感器的该互补布置被称为姿态航向参考系统(AHRS)。
陀螺仪测量结果可以帮助提高机器引导解决方案的动态性能的认识正在慢慢地寻找其到市场上的产品中的道路。虽然陀螺仪的包括提高测斜仪的性能，但仅使重力向量中的变化平滑提供次最佳性能。即，因为来自陀螺仪的累积噪声、偏置以及其它传感器误差最终使得角度漂移，所以解决方案的准确度仍然受测斜仪的质量和重力向量的关联测量结果限制。即使具有完美的测斜仪测量结果，由于移动导致的所引入的加速度最终也将限制这种系统的性能。
因此，期望理解在测斜仪测量结果上引起的加速度的原因和效应。如果可以识别、量化并观测所引起的加速度，那么可以适当地补偿重力向量的测量结果。
发明的目的
本发明的目的是提供克服或减轻以上所描述的缺点或问题中的一个或更多个或至少提供有用另选方案的动态运动补偿。
本发明的其它优选目的将从以下描述变得明显。
发明内容
在一个形式(虽然该形式不需要为唯一或实际上最广形式)中，提供了一种提供针对影响工作设备的传感器测量结果的运动学加速度的补偿的方法，该方法包括以下步骤：
识别工作设备的相对于彼此可移动的一个或更多个构件；
限定针对每个可移动构件的坐标系；
确定针对每个坐标系的运动学关系；
确定每个所识别的构件的相对位置；
根据运动学关系和每个所识别构件的相对位置修改传感器或传感器测量结果。
优选地，确定每个所识别构件的相对位置的步骤包括确定每个所识别构件的角度。优选地，修改传感器或传感器测量的步骤还包括根据所测量内容的以下组中的至少一个修改传感器或传感器测量结果：线性加速度、角速率或角加速度。
优选地，确定针对每个坐标系的运动学关系的步骤包括确定运动学链。优选地，确定运动学链的步骤包括连续地确定从第一可移动构件到最终可移动构件的运动学关系。优选地，针对随后可移动构件的运动学关系依赖之前可移动构件的运动学关系。
优选地，确定运动学链的步骤包括确定一组机械联接连杆之间的运动学关系，其中，每个连杆连接到多个相邻连杆。
优选地，根据运动学关系和每个所识别构件的相对位置修改传感器或传感器测量结果的步骤包括使用每个所识别构件的相对位置的时间导数。优选地，使用每个所识别构件的相对位置的时间导数包括使用至少一个二阶或更高时间导数。
优选地，传感器测量结果是来自测斜仪传感器的测量结果。测斜仪传感器可以是加速度计传感器。另选地，测斜仪传感器可以是测量相对于当地垂线的一个或更多个角度的传感器。优选地，坐标系是笛卡尔坐标系。优选地，坐标系具有三个维度，优选地为具有三个正交轴的笛卡尔坐标系。
优选地，修改传感器或传感器测量结果的步骤包括补偿线性加速度、相对于旋转坐标系的加速度以及假力中的一个或更多个。优选地，假力包括离心加速度、科里奥利(Coriolis)加速度以及欧拉(Euler)加速度中的一个或更多个。优选地，修改传感器或传感器测量结果的步骤包括使用来自一个或更多个角速率传感器或一个或更多个角加速度传感器的测量结果。修改传感器或传感器测量结果的步骤可以包括使用来自附接到可移动构件上的空间不同位置的多个传感器的测量结果。
定义针对每个可移动构件的坐标系的步骤可以包括定义针对与可移动构件关联的一个或更多个传感器的坐标系的步骤。根据运动学关系和每个所识别构件的相对位置修改传感器或传感器测量结果的步骤还可以包括根据与可移动构件关联的一个或更多个传感器和针对与可移动构件关联的一个或更多个传感器的坐标系修改传感器或传感器测量结果。
优选地，该方法还包括以下步骤：确定可移动构件的坐标系与可移动构件的传感器的坐标系之间的位移。优选地，该方法还可以包括以下步骤：确定可移动构件的坐标系的轴与优选地附接到可移动构件的传感器的坐标系的轴之间的角度偏移。
限定针对每个可移动构件的坐标系的步骤优选地包括限定针对与可移动构件关联的接合点(joint)的坐标系。确定每个所识别构件的相对位置的步骤还可以包括确定可移动构件之间的每个接合点的角度。
优选地，确定针对每个坐标系的运动学关系的步骤包括连续地确定从第一可移动构件到最终可移动构件的运动学关系。
传感器可以位于机架上。该方法优选地还包括以下步骤：定义针对工作设备的机架的坐标系。
优选地，该方法还包括以下步骤：确定工作边缘的位置。优选地，确定工作边缘的位置的步骤包括使用修改后的传感器测量结果来确定工作边缘的位置。优选地，确定工作边缘的位置的步骤包括确定相邻可移动构件之间的角度。优选地，确定相邻可移动构件之间的角度包括考虑(account for)运动学效应。确定相邻可移动构件之间的角度可以包括使用粗调平算法并考虑运动学效应。
优选地，确定相邻可移动构件之间的角度包括使用姿态航向参考系统(AHRS)并考虑运动学效应。优选地，使用AHRS的步骤包括使用递归估计技术。优选地，使用AHRS的步骤包括使用互补滤波器、卡尔曼(Kalman)滤波器、扩展卡尔曼滤波器、Sigma点卡尔曼滤波器、集合卡尔曼滤波器以及粒子滤波器的家族中的一个。考虑运动学效应可以包括补偿针对运动学加速度的传感器测量结果。考虑运动学效应可以包括忽略在确定可感知运动学加速度超过预定等级时的传感器测量结果。
根据另一种形式，提供了一种补偿针对运动学加速度的工作设备的一个或更多个测量结果，该系统包括：
工作设备，该工作设备具有相对于彼此可移动的一个或更多个构件；以及
处理器，该处理器与传感器通信，该处理器被构造为：
取得传感器测量结果；
获得针对每个可移动构件的坐标系；
获得针对每个坐标系的运动学关系；
确定可移动构件的相对位置；以及
根据运动学关系和每个所识别构件的相对位置修改传感器或传感器测量结果，以考虑运动学加速度。
优选地，传感器包括安装到相对于彼此可移动的多个构件中的一个或更多个的一个或更多个传感器。优选地，确定可移动构件的相对位置包括确定每个所识别构件的角度。优选地，处理器还被配置为确定所测量的一组以下参数中的至少一个：线性加速度、角速率或角加速度；并且处理器被配置为使用所确定的线性加速度、角速率和/或角加速度修改传感器或传感器测量结果。
优选地，工作设备包括机架，并且传感器位于机架上。优选地，处理器位于工作设备上。优选地，处理器包括嵌入式系统。优选地，嵌入式系统包括微控制器。
优选地，处理器被配置为获得针对每个可移动构件的已存储坐标系。优选地，处理器被配置为获得针对每个坐标系的已存储运动学关系。
优选地，工作设备是施工车辆、农业车辆或采矿车。甚至更优选地，车辆是挖掘机、推土机、平地机、装载机、雪道整理机、拉铲挖掘机、拖拉机、反铲挖土机、收割机、起重机、钻孔机或铲车中的一个。优选地，工作设备是挖掘机，并且优选地，一个或更多个可移动构件包括动臂、斗杆以及铲斗。
本发明的另外特征和优点将从以下详细描述变得明显。
附图说明
仅用示例的方式，在下文中将参照附图更完全地描述本发明的优选实施方式，附图中：
图1例示了高级流程图，该高级流程图例示了补偿针对运动学加速度的传感器测量结果的方法；
图2例示了示出了机架坐标系的示例挖掘机；
图3例示了示出了机架传感器坐标系的示例挖掘机；
图4例示了示出了动臂接合点坐标系的示例挖掘机；
图5例示了示出了动臂传感器坐标系的示例挖掘机；
图6例示了示出了斗杆接合点坐标系的示例挖掘机；
图7例示了示出了斗杆传感器坐标系的示例挖掘机；
图8例示了示出了狗骨(dogbone)接合点坐标系的示例挖掘机；
图9例示了示出了狗骨传感器坐标系的示例挖掘机；
图10例示了示出了器具(implement)接合点坐标系的示例挖掘机；
图11例示了示出了器具传感器坐标系的示例挖掘机；
图12例示了示出了倾斜铲斗接合点坐标系的示例挖掘机；
图13例示了示出了倾斜铲斗传感器坐标系的示例挖掘机；以及
图14例示了挖掘机的铲斗和狗骨部的近视图，示出到铲斗的狗骨连杆。
具体实施方式
图1例示了补偿针对当地运动学加速度(诸如影响传感器测量结果的工作设备的移动)的传感器测量结果(优选地为测斜仪或加速度计测量结果)的方法的流程图。该方法与具有独立可移动构件的工作设备(诸如图2至图13例示的挖掘机10)特别有关。本发明主要参照挖掘机10来描述，但是在上下文允许的情况下，不旨在限于此，并且将理解，本发明可以应用于受运动学加速度(诸如移动)影响的其它类型的设备和车辆(诸如推土机、平地机、前端装载机、雪道整理机、钻孔机或反铲挖土机)。
概括地说，识别一个或更多个可移动构件(步骤100)。通常，构件是独立可移动构件(诸如图2所例示的挖掘机10的动臂22、斗杆24以及铲斗26)。图2所例示的挖掘机10的动臂22、斗杆24以及铲斗26全部相对于机架(chassis)20且相对于彼此可移动。铲斗26经由器具接合点302连接到斗杆24。
一旦被识别，就针对每个构件限定坐标系(步骤110)，并且确定针对每个坐标系的运动学关系(步骤115)。还确定每个构件的相对位置(步骤120)。然后可以使用运动学关系(来自步骤115)和位置数据(来自步骤120)补偿由这些构件产生的运动学加速度来修改(步骤140)传感器测量结果(步骤130)。
位置数据优选地包括地点数据、角度数据及其时间导数(例如，角速率和加速度)。角速率(即，角的一阶时间导数)可以用于补偿离心加速度，角速率和速度(两个时间导数)可以用于补偿科里奥利加速度，线性加速度(即，位置的二阶时间导数)可以用于运动学补偿，并且角加速度(即，角度的二阶时间导数)可以用于补偿欧拉加速度。这些运动学效应可以根据系统的可用数据、计算能力以及期望准确度来选择性地补偿。另选地，如果确定运动学加速度超过预定等级(例如，在认为它们足够大以不利地影响系统的准确度时)，那么在此期间可以简单地忽略传感器测量结果。
现在将关于图2至图13中例示的挖掘机10进一步详细地描述整个处理。
图2例示了具有可移动构件的挖掘机10，这些可移动构件为相对于机架20和彼此均独立可移动的动臂22、斗杆24以及铲斗26的形式。动臂22、斗杆24以及铲斗26可以由开放链运动学关系来限定。一旦理解可移动构件22、24以及26与机架20之间的运动学关系，则它们可以与可观察量(即，来自惯性传感器的测量结果等)有关。
为了建立各种运动学关系，导出旋转坐标系的一般运动学。该一般关系然后应用于每个可移动构件及其接合点，每个可移动构件及其接合点关于挖掘机10与动臂22、斗杆24和铲斗以及其间的接合点有关。一旦建立运动学关系，则考虑多个传感器测量结果修改策略，以利用所限定的关系，优选策略通常被选择为平衡性能对可用计算资源。
因为加速度计和陀螺仪关于惯性空间来测量，所以对用于旋转坐标系的一般运动学的表达式感兴趣，以便对在传感器上引入的真实加速度建模。考虑一般b坐标系，该坐标系关于另一个一般a坐标系旋转。在a坐标系中解析的b坐标系关于a坐标系的位置被表示为
Figure GDA0002407420630000071
现在考虑空间中的点c，该点关于b坐标系可以被写为
Figure GDA0002407420630000072
相对于a坐标系的点c由
Figure GDA0002407420630000073
给出。
当在b坐标系中解析点c时，表达式为：
Figure GDA0002407420630000074
关于时间求微分产生速度：
Figure GDA0002407420630000075
引入旋转微分方程：
Figure GDA0002407420630000076
代入产生速度：
Figure GDA0002407420630000081
关于时间对速度求微分产生加速度：
Figure GDA0002407420630000082
代入旋转微分方程并重新整理产生：
Figure GDA0002407420630000083
这些项可以被识别为：
Figure GDA0002407420630000084
是坐标系本身的加速度；
Figure GDA0002407420630000085
是关于旋转坐标系的加速度；
Figure GDA0002407420630000086
是“假”离心加速度；
Figure GDA0002407420630000087
是“假”科里奥利加速度；以及
Figure GDA0002407420630000088
是“假”欧拉加速度。
“假”离心加速度、科里奥利加速度以及欧拉加速度由挖掘机10的运动学运动产生。凭借可移动构件的已知位置、速度、加速度、角速度以及角加速度，使用如下的运动学关系可以考虑通常破坏测斜仪测量结果的这些另外运动学加速度。
因为地球是旋转参考坐标系，所以惯性传感器通常不仅测量真实惯性加速度，还测量由导航坐标系的选择引入的效应。因为参考的纯惯性坐标系对于陆地活动不实用，所以必须检查使用基于地的参考坐标系(诸如当地测地坐标系(即，当地垂线)或地心地固(ECEF)参考坐标系)的效应。
为了容易解释，由于对齐z轴与当地重力，姿态可以关于当地导航坐标系来跟踪。然而，因为陀螺仪测量关于惯性空间的角速率(即，不是当地导航坐标系)，所以应考虑关于当地测地坐标系的关系。
以关于当地导航坐标系的主体的旋转微分方程开始：
Figure GDA0002407420630000089
我们必须导出主体关于当地导航坐标系的角速率
Figure GDA00024074206300000810
与关于惯性空间的陀螺仪测量结果
Figure GDA00024074206300000811
之间的某一关系。这可以使用以下表达式来完成：
Figure GDA0002407420630000091
对于当地测地(LG)坐标系中的导航，应考虑三项：陀螺仪测量结果
Figure GDA0002407420630000092
由于地球的旋转而产生的贡献
Figure GDA0002407420630000093
以及LG坐标系关于ECEF坐标系的旋转(“转移率”)
Figure GDA0002407420630000094
地球具有近似15deg/hr的旋转速率。因为最新的工业级MEMS陀螺仪具有在10deg/hr量级上的偏置稳定性，所以忽略该项是传感器的次最佳使用。
可以基于设备的当前位置和速度计算的转移率项为：
Figure GDA0002407420630000095
项RE(L)、RN(L)以及h分别是作为纬度的函数的横向曲率半径、作为纬度的函数的地心半径以及在地球椭球体上方的高度。下标N、E以及D分别是速度的北向分量、东向分量以及向下分量。
从该方程，以30km/hr移动的机器将经历0.27°/hr的转移率。通常具有高精度操作的以5km/hr缓慢移动的机器将经历小于0.05°/hr的转移率。这两个值低于当代工业MEMS的可解析性，因此可以安全地忽略。
以与陀螺仪类似的方式，LG坐标系不是惯性坐标系的事实将使得测斜仪测量由于除了重力加速度之外的原因而产生的加速度。旋转坐标系的一般运动学可以被应用以确定这种效应的程度：
Figure GDA0002407420630000096
因为地球的旋转速率是恒定的，所以至少为了导航目的，欧拉加速度项消失。此外，因为导航坐标系的原点与地心惯性坐标系(ECI)一致，所以线性加速度项也消失。由此，一般运动学方程降至：
Figure GDA0002407420630000097
项
Figure GDA0002407420630000098
描述了关于被旋转到当地导航坐标系中的惯性空间的加速度。除了测量重力加速度(即，作为地球上的位置的函数的基本质量力)之外，加速度计还测量该真实加速度。
Figure GDA0002407420630000099
代入到运动学方程中产生：
Figure GDA0002407420630000101
第一项是由加速度计测量且被旋转到当地导航坐标系中的比力(specificforce)。第二项包含重力加速度和由地球的旋转引起的加速度这两者。它们一起形成作为位置函数的当地重力
Figure GDA0002407420630000102
该重力可以使用例如索米利亚纳(Somigliana)模型来计算。最后项是由于当地水平坐标系的位置的变化而产生的科里奥利项。
如果假定挖掘机在行走期间可以行进的最大速度是30km/h，那么可以经历的最大加速度是0.12mg。当在例如以5km/h工作的同时仅进行小操纵控制时，那么最大加速度是0.02mg。考虑0.12mg与具有与垂线的偏转相同的数量级的近似25弧秒对应，那么对于许多机器控制应用(诸如例如，通常以较低速度操作的挖掘机10)忽略科里奥利加速度被认为是安全的。
凭借所识别的惯性量，接着识别用于运动学关系的坐标系。一旦已经识别坐标系，则运动学关系可以被应用至每个可移动构件(通常应用于可移动构件的接合点和传感器)。
关于挖掘机10，图2至图13例示了优选的坐标系。在所例示的示例中，存在用于与可移动构件(3、5、7、9、11以及13)关联的每个接合点(图2、图4、图6、图8、图10以及图12)和每个传感器的单独坐标系。
不同坐标系是可以的。示例坐标系已经在它们与挖掘机10上的可容易测量的几何点对齐时被选择。另一个坐标系例如还可以为为了直接估计角加速度而每可移动构件包括两个传感器的坐标系，角加速度转可以用于直接补偿欧拉加速度。
图2例示了机架坐标系200，该机架坐标系200具有位于旋转轴和将挖掘机10的底架与机架20的驾驶室分离的平面的交叉202处的原点。机架坐标系200具有：x轴，该x轴与动臂22平行且位于动臂22的方向上；正交z轴，该正交z轴与旋转轴对齐，指向底架；以及y轴，该y轴与x轴和z轴这两者正交。
图3例示了机架传感器坐标系210，该机架传感器坐标系210具有位于机架传感器212的导航中心处的原点。机架传感器212具有与机架坐标系200的原点的固定机架位移214。机架传感器坐标系210使其轴与机架传感器212的感测轴对齐，并且具有关于机架坐标系200的固定角度偏移。
图4例示了动臂接合点坐标系220，该动臂接合点坐标系220具有处于动臂接合点的旋转轴与动臂22的对称平面的交叉处的原点222。动臂接合点坐标系220具有与在动臂接合点与斗杆接合点之间延伸的动臂参考轴221对齐的x轴。因为动臂接合点与斗杆接合点转轴彼此平行，所以动臂参考轴221垂直于动臂接合点与斗杆接合点这两者的转轴。动臂接合点坐标系220还具有y轴，该y轴沿着动臂转轴，与机架坐标系200的y轴平行并且位于与机架坐标系200的y轴相同的方向上，并且具有z轴，该z轴与x轴和y轴这两者正交。
图5例示了动臂传感器坐标系230，该动臂传感器坐标系230具有位于动臂22的动臂传感器232的导航中心处的原点。动臂传感器坐标系230具有离动臂接合点坐标系222的原点的固定动臂位移234和关于动臂接合点坐标系220的固定角度偏移。通常，动臂传感器坐标系230的y轴与动臂接合点坐标系220的y轴平行并且位于与动臂接合点坐标系220的y轴相同的方向上。
图6例示了斗杆接合点坐标系240，该斗杆接合点坐标系240具有处于斗杆接合点与斗杆24的对称平面的交叉处的原点242。斗杆接合点坐标系240具有与在斗杆接合点与铲斗接合点之间延伸的斗杆参考轴241对齐的x轴。因为斗杆接合点与器具接合点转轴彼此平行，所以斗杆参考轴241垂直于斗杆接合点与铲斗接合点这两者的转轴。斗杆接合点坐标系240还具有沿着斗杆转轴的y轴和与x轴和y轴这两者正交的z轴。
图7例示了斗杆传感器坐标系250，该斗杆传感器坐标系250具有位于斗杆24的斗杆传感器252的导航中心处的原点。斗杆传感器坐标系250具有离斗杆接合点坐标系242的原点的固定斗杆位移254和关于斗杆接合点坐标系240的固定角度偏移。通常，斗杆传感器坐标系250的y轴与斗杆接合点坐标系240的y轴平行。
图8例示了狗骨接合点坐标系280，该狗骨接合点坐标系280具有处于狗骨接合点的转轴与狗骨接合点28的对称平面的交叉处的原点282。狗骨接合点坐标系280具有与狗骨接合点参考轴(为了清楚起见未示出)对齐的x轴，该x轴位于狗骨接合点对称平面上，将狗骨接合点28接合到H型接合点。狗骨接合点坐标系280还具有沿着狗骨接合点转轴的y轴，并且具有与x轴和y轴这两者正交的z轴。
图9例示了狗骨接合点传感器坐标系290，该狗骨接合点传感器坐标系290具有位于狗骨接合点传感器292的导航中心处的原点。狗骨接合点传感器坐标系290具有离狗骨接合点坐标系282的原点的固定狗骨位移294和关于狗骨接合点坐标系280的固定角度偏移。
图10例示了器具坐标系300，该器具坐标系300具有位于斗杆24的对称平面与器具接合点的转轴的交叉处的原点302。器具坐标系300具有x轴，该x轴位于与器具接合点坐标系的原点302的对称平面上并且与倾斜铲斗接合点交叉，并且具有y轴，该y轴位于器具接合点的转轴上，并且具有z轴，该z轴与x轴和y轴这两者正交。
图11例示了器具传感器坐标系310，该器具传感器坐标系310具有位于器具传感器312的导航中心处的原点。器具传感器坐标系310具有离器具坐标系302的原点的固定器具位移314和关于器具坐标系300的固定角度偏移。
图12例示了铲斗接合点坐标系260。所例示挖掘机10的铲斗26是倾斜铲斗。铲斗接合点坐标系260具有处于倾斜接合点销262与铲斗26的垂直于倾斜接合点销的对称平面的交叉处的原点。铲斗接合点坐标系260具有与铲斗26的工作边缘(例如，切削刀片)交叉的x轴、与倾斜轴销对齐的z轴、以及与x轴和z轴这两者正交的y轴。
图13例示了铲斗传感器坐标系270，该铲斗传感器坐标系270具有位于铲斗26的铲斗传感器272的导航中心处的原点并且其轴与铲斗传感器272感测轴对齐。铲斗传感器坐标系270具有离铲斗接合点坐标系262的原点的固定铲斗位移274和关于铲斗接合点坐标系260的固定角偏移。
一旦已经限定坐标系，就可以通过将一般运动学方程应用于该坐标系来限定用于每个可移动构件的运动学关系。每个运动学方程连续向下应用于运动学链，使得用于随后可移动构件的运动学关系取决于之前可移动构件的运动学关系。
坐标系指数如下来限定：机架坐标系200：v0，机架传感器坐标系210：b0，动臂接合点传感器坐标系220：v1，动臂传感器坐标系230：b1，斗杆接合点传感器坐标系240：v2，斗杆传感器坐标系250：b2，狗骨接合点坐标系280：v3，狗骨接合点传感器坐标系290：b3，器具接合点坐标系300：v4，器具传感器坐标系310：b4，倾斜铲斗接合点坐标系260：v5，以及倾斜铲斗传感器坐标系270：b5。
动臂接合点位置可以被描述为：
Figure GDA0002407420630000121
其中，
Figure GDA0002407420630000122
是机架20关于导航坐标系的旋转，并且
Figure GDA0002407420630000123
是动臂接合点坐标系220关于机架20的位移，
Figure GDA0002407420630000131
是机架20关于导航坐标系的位置，并且
Figure GDA0002407420630000132
是动臂22关于导航坐标系的位置。
关于时间求微分产生动臂接合点坐标系220的速率：
Figure GDA0002407420630000133
因为动臂接合点相对于车辆是固定的，所以
Figure GDA0002407420630000134
这产生：
Figure GDA0002407420630000135
再次求微分产生动臂接合点坐标系220的加速度：
Figure GDA0002407420630000136
再次注意，动臂接合点关于机架坐标系200是固定的，加速度降为：
Figure GDA0002407420630000137
动臂接合点的运动学是确定动臂传感器232处的运动学(是观测加速度的地点)时的必要步骤。动臂传感器232的位置可以如下从挖掘机10的位置和动臂接合点坐标系220的位置这两者来计算：
Figure GDA0002407420630000138
其中，
Figure GDA0002407420630000139
是动臂传感器232相对于导航坐标系的位置，并且
Figure GDA00024074206300001310
是动臂传感器232相对于动臂接合点坐标系220的位置。因为动臂接合点的位置形成该方程的一部分，所以动臂传感器位置可以根据动臂接合点来表达：
Figure GDA00024074206300001311
关于时间求微分，并且注意动臂传感器232在动臂接合点坐标系220中固定产生动臂传感器232处的速度：
Figure GDA00024074206300001312
再次关于时间求微分并且应用相同约束产生动臂传感器232关于导航坐标系的加速度：
Figure GDA00024074206300001313
既然已经确定动臂接合点处的运动学关系，就可以构造动臂传感器232中的测斜仪的观察模型。传感器通常被认为是“完美的”，但实现应考虑任意缺点。
动臂传感器232加速度计模型应与动臂传感器232加速度测量结果
Figure GDA00024074206300001314
和动臂传感器232陀螺仪测量结果
Figure GDA00024074206300001315
一致。角加速度可以由一个或更多个角加速度计直接测量或通过估计角速率的导数来测量。另选地，角加速度可以以准确度为代价完全被忽略。
作用在动臂传感器上的运动学加速度另选地可以由安装在动臂上的空间不同位置处的多个传感器来考虑。因为相同角速率和角加速度作用在每个所述动臂传感器上，但经由每个传感器的不同杠杆臂
Figure GDA0002407420630000141
引入不同加速度，所以可以确定并适当地补偿效应。
将旋转
Figure GDA0002407420630000142
分成两个分量
Figure GDA0002407420630000143
(其是动臂22关于挖掘机10的相对旋转)和
Figure GDA0002407420630000144
(其是挖掘机10姿态)也是便利的。这两个分量通常单独地估计。
给定动臂传感器坐标系230与动臂接合点坐标系之间的已知(或已估计)旋转
Figure GDA0002407420630000145
则动臂坐标系中的传感器测量结果可以被表达为：
Figure GDA0002407420630000146
Figure GDA0002407420630000147
将重力考虑在内，由动臂传感器232加速度计测量的比力为：
Figure GDA0002407420630000148
可以清楚地识别由于线性加速度、欧拉加速度以及离心加速度产生的运动学贡献。因此，如果测量结果或至少估计值存在角速率、角加速度以及线性加速度，那么可以完全补偿影响动态性能的运动学效应。另选地，所引入的欧拉加速度和离心加速度在有或没有角速率和/或角加速度测量结果的帮助下可以借助于多个传感器(优选地为安装到动臂的传感器)来补偿。
在与针对动臂传感器232描述的过程类似的过程之后，机架传感器212运动学关系和可观察量概括如下：
位置：
Figure GDA0002407420630000149
速度：
Figure GDA00024074206300001410
加速度：
Figure GDA00024074206300001411
可观察量：
Figure GDA0002407420630000151
在类似过程之后，斗杆接合点242运动学关系概括如下：
位置：
Figure GDA0002407420630000152
速度：
Figure GDA0002407420630000153
加速度：
Figure GDA0002407420630000154
在类似过程之后，斗杆传感器252运动学关系和可观察量概括如下：位置：
Figure GDA0002407420630000155
速度：
Figure GDA0002407420630000156
加速度：
Figure GDA0002407420630000157
可观察量：
Figure GDA0002407420630000158
在类似过程之后，狗骨接合点282运动学关系概括如下：
位置：
Figure GDA0002407420630000159
速度：
Figure GDA00024074206300001510
加速度：
Figure GDA00024074206300001511
在类似过程之后，狗骨传感器292运动学关系和可观察量概括如下：位置：
Figure GDA00024074206300001512
速度：
Figure GDA0002407420630000161
加速度：
Figure GDA0002407420630000162
可观察量：
Figure GDA0002407420630000163
在类似过程之后，器具接合点302运动学关系概括如下：
位置：
Figure GDA0002407420630000164
速度：
Figure GDA0002407420630000165
加速度：
Figure GDA0002407420630000166
在类似过程之后，器具传感器312运动学关系和可观察量概括如下：位置：
Figure GDA0002407420630000167
速度：
Figure GDA0002407420630000168
加速度：
Figure GDA0002407420630000169
可观察量：
Figure GDA00024074206300001610
在类似过程之后，倾斜铲斗接合点262运动学关系概括如下：
位置：
Figure GDA00024074206300001611
速度：
Figure GDA00024074206300001612
加速度：
Figure GDA0002407420630000171
在类似过程之后，倾斜铲斗传感器272运动学关系和可观察量概括如下：
位置：
Figure GDA0002407420630000172
速度：
Figure GDA0002407420630000173
加速度：
Figure GDA0002407420630000174
可观察量：
Figure GDA0002407420630000175
传感器不是必须被直接安装到运动学链的每一个构件。而是，可以优选的是将传感器安装在机械联接到感兴趣的连杆的相关连杆。因此，可能需要闭合链运动学的模型。
例如，传感器可以安装在挖掘机28的狗骨28上，而不是安装在器具连杆上，使得它在正常操作期间不太可能被损坏。狗骨28和器具连杆到斗杆24和倾倒连杆321的附接形成闭合运动学链。如图14例示，闭合运动学链在这种情况下是具有接合点p1、p2、v3以及v4的平面四边形连杆320。
给定连杆的几何结构，要求狗骨相对于斗杆24的角度
Figure GDA0002407420630000176
和狗骨28相对于斗杆24的角速率
Figure GDA0002407420630000177
器具连杆322相对于斗杆24的角度
Figure GDA0002407420630000178
器具连杆322相对于斗杆24的角速率
Figure GDA0002407420630000179
以及器具连杆322相对于斗杆24的角加速度。一旦期望值被计算，则它们可以代入之前描述的开链运动学。
对于这种示例，狗骨坐标系280保持如之前所描述那样，但现在必须考虑两个新坐标系(即，倾倒连杆接合点坐标系和H型接合点坐标系)。倾倒连杆接合点坐标系被限定为具有位于倾倒连杆接合点(p2)的转轴与平面连杆的对称平面的交叉处的原点，x轴与接合点v4交叉，y轴平行于转轴，并且z轴被限定为与x轴和y轴正交。
H型接合点坐标系被限定为具有位于H型接合点(p1)的转轴与平面连杆的对称平面的交叉处的原点，x轴与倾倒杆接合点(p2)交叉，y轴平行于转轴，并且z轴与x轴和y轴正交。
考虑位于与斗杆24坐标系240的x-z平面平行的由四边形连杆320形成的平面。接合点中的两个(即，狗骨接合点和器具接合点)相对于斗杆24坐标系240在几何上是固定的，并且可以在校准期间测量。因此：
Figure GDA0002407420630000181
这是时不变量。
因为已知连杆的长度，所以每个连杆可以如下在其自己的坐标系中被限定：
Figure GDA0002407420630000182
Figure GDA0002407420630000183
Figure GDA0002407420630000184
因为所有连杆都位于平面上，所以用于任意给定构件的每个旋转矩阵是围绕y轴的固有旋转，如下：
Figure GDA0002407420630000185
Figure GDA0002407420630000186
Figure GDA0002407420630000187
因为链被闭合，所以连杆的位置必须满足：
Figure GDA0002407420630000188
从斗杆坐标系到它们的原生坐标系的旋转产生：
Figure GDA0002407420630000189
鉴于已知和未知量重新整理：
Figure GDA00024074206300001810
因为该方程的左手侧完全由已知值构成，所以它可以被更简洁地写为：
Figure GDA00024074206300001811
其中，
Figure GDA00024074206300001812
明确地，书写矩阵方程产生以下内容：
Figure GDA00024074206300001813
这可以被排列并重写为如下的两个联立方程：
Figure GDA0002407420630000191
Figure GDA0002407420630000192
使用恒等式：
cos2(θ)+sin2(θ)＝1
因此：
Figure GDA0002407420630000193
这可以被重新整理为：
Figure GDA0002407420630000194
这可以具有以下形式：
P cos(θ)+Q sin(θ)＝M
它的解为：
Figure GDA0002407420630000195
或等效地：
Figure GDA0002407420630000196
因为正弦(sin)和余弦(cos)直接感兴趣，所以必须通过使用以下方程在不计算角度的情况下直接计算它们：
Figure GDA0002407420630000197
和
Figure GDA0002407420630000198
根据几何结构和已知角度的替换M、P以及Q可以产生多达两个解。然而，这些角度中的仅一个将在物理上可能实现。因此，可以消除不可能的解。
一旦已经计算出角度
Figure GDA0002407420630000199
则它可以被直接代入以确定
Figure GDA00024074206300001910
可以存在满足该解的多于一个可能角度，但仅一个角度将在物理上可能实现。因此，可以消除不可能的解。
记得闭合链位置必须满足：
Figure GDA0002407420630000201
从斗杆坐标系240到它们的原生坐标系的旋转产生：
Figure GDA0002407420630000202
取该方程两侧的时间导数，注意它们的原生坐标系中的向量且注意
Figure GDA0002407420630000203
恒定产生：
Figure GDA0002407420630000204
注意用于每个构件的旋转矩阵的时间导数：
Figure GDA0002407420630000205
Figure GDA0002407420630000206
Figure GDA0002407420630000207
应用：
Figure GDA0002407420630000208
乘以
Figure GDA0002407420630000209
并简化产生：
Figure GDA00024074206300002010
因为系统为平面的且完全沿着x-z平面，所以角速率围绕y轴发生。而且，上述方程中的每个向量完全沿着其x轴。因此，关于向量rx的角速率ωy可以被写为：
Figure GDA00024074206300002011
代入且根据固有旋转明确书写：
Figure GDA00024074206300002012
使x中的分量相等：
Figure GDA00024074206300002013
对
Figure GDA0002407420630000211
求解：
Figure GDA0002407420630000212
为了计算
Figure GDA0002407420630000213
上述结果可以直接被代入：
Figure GDA0002407420630000214
因此，可以根据狗骨28的角速率和连杆几何结构320来找到器具连杆的相对角速率。
重复闭合链位置的时间导数：
Figure GDA0002407420630000215
取时间导数并重新整理产生：
Figure GDA0002407420630000216
因为仅旋转自由度围绕y轴，所以角加速度将总是围绕该轴。因此，每项可以被简化为：
Figure GDA0002407420630000217
应用并乘以
Figure GDA0002407420630000218
Figure GDA0002407420630000219
扩展并收集沿着x的分量产生：
Figure GDA00024074206300002110
对于
Figure GDA00024074206300002111
重新整理等于：
Figure GDA0002407420630000221
扩展并收集沿着z的分量产生：
Figure GDA0002407420630000222
对于
Figure GDA0002407420630000223
求解产生：
Figure GDA0002407420630000224
因此，器具接合点的角加速度可以根据连杆的几何结构320、连杆的角速率以及狗骨接合点的角加速度来描述。
动态补偿所要求的最小传感器构造取决于机器类型、被执行的工作以及期望的准确度。每个感测节点处的全部三种测斜仪、陀螺仪以及角加速度计提供系统设计的灵活性，并且可以提供冗余测量和自校准方面的优点。然而，由于实际和商业原因，可能期望更少的传感器。
将挖掘机10用作示例，动臂22、斗杆24、狗骨接合点28以及倾斜铲斗26仅提供一个移动轴。因此，为了提供测量结果的最小集合，每个感测节点仅需要一个陀螺仪。剩余两个角速率可以从在挖掘机10的其它位置处的传感器来重构。
将动臂传感器232用作示例，观测方程包括表示动臂22关于导航坐标系的角速率的项。因为最小构造将仅包括围绕动臂坐标系的y轴的角速率传感器，所以x轴和y轴的角速率必须如下从剩余传感器重构：
Figure GDA0002407420630000225
因为挖掘机10的动臂接合点坐标系220总是平行于机架坐标系200的x-z平面，旋转由围绕y轴的固有旋转构成。如果机架传感器212装配有围绕x轴和z轴的角速率，那么可以使用以下表达式确定感兴趣的角速率：
Figure GDA0002407420630000226
如果为了估计目的，对量
Figure GDA0002407420630000231
感兴趣(例如，传播姿态项
Figure GDA0002407420630000232
)，那么在机架传感器212上包括y轴陀螺仪允许这被计算。针对剩余传感器(例如，斗杆传感器252、狗骨传感器292以及倾斜铲斗传感器272)的必要构造和重构角速率可以使用类似过程来确定。
最后，每个接合点的角度估计的准确度取决于测斜仪的质量。如行业中已知的，一轴、二轴以及三轴构造全部是可以的，具有与每种方法关联的益处和缺陷。
虽然本说明书主要公开了基于测斜仪的加速度传感器，但将理解，测斜仪可以以直接测量关于当地垂线的一个或更多个角度的方式来构造。例如，测斜仪可以使用钟摆式或水平仪式构造，其直接测量角度，但尽管如此受运动学加速度影响。
将动态补偿应用至基于角度的测斜仪的最简单手段是基于基于角度的测斜仪的当前读数计算将在基于加速度的测斜仪上引入的等效测量结果并根据说明书修改等效测量结果。
补偿基于角度的测斜仪的第二种手段是从补偿来自测斜仪的角度测量结果所要求的运动学加速度计算角度偏差。这样做的合适手段是经由泰勒级数，其中，应用的角度偏差可以借助于可应用测量方程的雅可比行列式(Jacobian)来生成。
在非常高精度应用中，测斜仪可以不仅包括感测元件，还可以包括对齐传感器与参考方位以使感测元件的范围和灵敏度最大化的机械驱动器(诸如步进马达)。当补偿这种机电传感器中的运动学加速度时，最大灵敏度通过命令机械驱动器不与垂线参考对齐而是与是重力和任意运动学加速度的总和的视在当地垂线对齐来获得。因此，当使用机电测斜仪时，补偿运动学效应不必须要求传感器测量结果的修改，而是可以通过例如通过命令传感器元件的等效参考方位修改传感器本身来执行。
角加速度计较稀少且昂贵，但在经常观测到角速率的大变化的特定场景中可能是有用的。在没有它们时，补偿欧拉加速度所必要求的角加速度可以被数字地估计为角速率的导数，或者从同一刚性座架上的空间不同加速度计来估计。因此，针对欧拉加速度的补偿被认为是成本与所要求性能的平衡。
在一些接合点上，可以使用角度编码器或活塞直接测量角度。这在转轴几乎平行于重力向量的接合点(例如，平地机刀片)或可以完全旋转的接合点(例如回转倾斜铲斗)上经常看到。因为这种接合点之间的角度可直接观测，所以不需要从组合陀螺仪和加速度计测量结果推断角度。
虽然接合点角度可直接观测，但接合点的角速率仍然引入使运动学链进一步下降的加速度。因此，角速率必须从角度测量结果的序列来估计。分别给定在时间k+1和k时的相对角度测量结果
Figure GDA0002407420630000241
和
Figure GDA0002407420630000242
增量旋转可以被计算为：
Figure GDA0002407420630000243
在间隔期间的恒定角速率的假定下，可以通过将解反转为旋转微分方程来计算角速率向量，即：
Figure GDA0002407420630000244
可以围绕数值奇异性采取应有关注来计算一般旋转矩阵R的矩阵对数ω：
Figure GDA0002407420630000245
Figure GDA0002407420630000246
计算工作边缘(例如，挖掘机铲斗26的刀片)的位置需要每个接合点与挖掘机10的姿态之间的角度的估计。各种策略可以用于估计角度，最合适的策略通常通过针对可用计算资源平衡所需性能来选择。
用于角度估计的最简单策略涉及简单地补偿每个接合点上的已知运动学效应并如正常将对测斜仪唯一解决方案执行的修正测量结果执行角度估计。例如，粗调平可以是应用的合适算法。因为对标准测斜仪解决方案的另外所需计算能力微小，所以使用该方法对于计算资源来说不可能繁重。
凭借角速率测量结果的可用性，可以通过累积角速率测量结果并使解趋向当地垂线来实现显著更大的性能。这类算法(俗称为姿态航向参考系统(AHRS))具有能够在维持良好动态性能的同时过滤垂线参考的不同优点。
能够补偿工作设备(诸如挖掘机10)的运动学关系从当地垂线去除显著误差源。因此，更多信任可以置于垂线参考上，这在不牺牲动态响应的情况下允许更准确的解。
另选地，因为AHRS算法中的角度确定的主要手段是角速率测量结果的积分，所以该算法可以在没有垂线参考的情况下在短时间段内维持准确度。如果发现垂线参考被充分破坏(例如，由运动学加速度)，那么在此时间期间忽略而不是补偿垂线参考。
许多AHRS算法在它们的公式化方面是确定的(诸如互补滤波器)。估计角度的另一种方法是经由递归估计技术(诸如卡尔曼滤波器、扩展卡尔曼滤波器、Sigma点卡尔曼滤波器、集合卡尔曼滤波器以及粒子滤波器的家族)。递归估计器通常采取处理模型和测量模型的形式，诸如：
x(k+1)＝f(x(k)，u(k)，v(k))
y(k)＝h(x(k)，w(k))
其中，x(k)是由感兴趣量(例如，角度)和多余参数(诸如陀螺仪偏置)构成的状态向量。处理模型f通常依据传播陀螺仪测量结果和多余参数的统计来实现。测量模型h由每个传感器的观测模型形成，将如详细说明的运动学效应考虑在内。
与状态向量关联的是描述了状态向量估计值的不确定性(或离差)的状态协方差。显著地，它追踪状态向量参数之间的相关性。特别是，在可观察量是运动学链的一部分的该情况下，感兴趣的变量之间将存在显著相关性。例如，清楚的是，斗杆传感器252处的测量结果不仅取决于斗杆24角度，还取决于动臂22角度和机架20的姿态。
该相关性可以被用来提供性能增益，特别是在存在可用冗余测量结果的上下文中。它还为自校准提供可能性，特别是在仔细选择多余参数(诸如对感测节点关于可移动构件的坐标系的角度偏差建模)的情况下。
为了演示，考虑存在在每个感测节点处可用的三轴测斜仪的情况。因为过多的测量结果在每个传感器地点处可用，所以观测方程组由多种因素决定。在特定几何构造中或在几何构造随着时间的过去的变化期间，可以减小特定状态之间的相关性。因此，系统的性能提高。另选地，可以公式化借助几何构造随着时间的类似改变来随着时间的过去降低多余参数(诸如偏置与角度偏差)与感兴趣状态之间的相关性。因为相关性被降低，所以也随着时间的过去而降低多余参数中的不确定性，这允许随着时间的过去“学习”这些参数。因此，这种系统可以用于提高性能或用于自校准目的。
递归估计的成本在与AHRS或提高的测斜仪解决方案相比时是大大提高的计算能力要求。尽管如此，这种滤波器仍被认为在现代嵌入式系统的能力内。
有利地，本发明允许补偿传感器测量结果中的动态移动，产生更准确测量结果。特别地，通过仔细量化并补偿关于由工作设备运动学关系引起的测量结果的所引起加速度来大幅提高机器引导系统的动态准确度。
另外，使允许传感器确定静止时的位置的不工作时段被最小化，甚至完全避免该时段，这显著提高操作效率。实现可以被调节以平衡性能与可用计算资源，以允许实现应用于具有变化等级的可用计算资源的各种不同类型的设备。
在本说明书中，修饰词(诸如第一和第二、左和右、顶部和底部等)仅可以用于区分一个元件或动作与另一个元件或动作，而不是必须要求或暗示任何实际的这种关系或顺序。在上下文允许的情况下，对整数或分量或步骤(等)的参考不被解释为仅限于该整数、分量或步骤中的一个，相反可以为该整数、分量或步骤等中的一个或更多个。
本发明的各种实施方式的以上描述为了描述的目的而被提供给相关领域的普通技术人员。其不旨是详尽的，或者将本发明限于单个所公开的实施方式。如上面提及的，对本发明的大量替代和变更在上述教导下对本领域技术人员是显而易见的。因此，虽然已经具体讨论了一些另选实施方式，但其它实施方式将是显而易见的或由本领域普通技术人员较容易地开发。本发明旨在包含这里讨论的本发明的所有替代、修改以及变更、以及落在以上所描述的本发明的精神和范围内的其它实施方式。
在本说明书中，术语“包括”或类似术语旨在意指非排他性包含，使得包括一系列元素的方法、系统或设备不仅仅包括这些元素，还可以适当地包括未列出的其它元素。

Claims (33)

1.一种提供对影响来自工作设备的传感器的传感器测量结果的运动学加速度的补偿的方法，所述方法包括以下步骤：
识别所述工作设备的相对于彼此可移动的多个构件；
限定针对与每个可移动构件相关的一个或更多个传感器的坐标系；
确定针对每个坐标系的运动学关系，包括确定运动学链；
确定每个所识别的可移动构件的相对位置，所述相对位置包括该可移动构件的角度；
确定所述运动学链中的每个可移动构件相对于相邻可移动构件的相对移动；以及
根据每个所识别的可移动构件的所述运动学关系、相对位置、以及相对移动修改所述传感器或传感器测量结果，使得补偿由所述可移动构件的所有相对移动产生的加速度。
2.根据权利要求1所述的方法，其中，修改所述传感器或传感器测量结果的步骤还包括：根据所测量内容的以下组中的至少一个修改所述传感器或传感器测量结果：线性加速度、角速率或角加速度。
3.根据权利要求1所述的方法，其中，确定运动学链的步骤包括：连续地确定从第一可移动构件到最终可移动构件的运动学关系，使得针对随后可移动构件的运动学关系依赖于之前可移动构件的运动学关系。
4.根据权利要求1所述的方法，其中，确定运动学链的步骤包括：确定一组机械联接的连杆之间的运动学关系，其中，每个连杆连接到多个相邻连杆。
5.根据权利要求1所述的方法，其中，根据所述运动学关系和每个所识别构件的相对位置修改所述传感器或传感器测量结果的步骤包括：使用每个所识别构件的相对位置的时间导数。
6.根据权利要求5所述的方法，其中，使用每个所识别构件的相对位置的时间导数包括：使用至少一个二阶或更高阶时间导数。
7.根据权利要求1所述的方法，其中，所述传感器测量结果是来自测斜仪传感器的测量结果。
8.根据权利要求7所述的方法，其中，所述测斜仪传感器是加速度计传感器。
9.根据权利要求7所述的方法，其中，所述测斜仪传感器是测量关于当地垂线的一个或更多个角度的传感器。
10.根据权利要求1所述的方法，其中，所述坐标系是具有三个正交轴的笛卡尔坐标系。
11.根据权利要求1所述的方法，其中，修改所述传感器或传感器测量结果的步骤包括：补偿线性加速度、关于旋转坐标系的加速度以及假想力中的一个或更多个。
12.根据权利要求11所述的方法，其中，所述假想力包括离心加速度、科里奥利加速度以及欧拉加速度中的一个或更多个。
13.根据权利要求1所述的方法，其中，修改所述传感器或传感器测量结果的步骤包括：使用来自一个或更多个角速率传感器或一个或更多个角加速度传感器的测量结果。
14.根据权利要求1所述的方法，其中，修改所述传感器或传感器测量结果的步骤包括：使用来自附接到可移动构件上的空间不同位置的多个传感器的测量结果。
15.根据权利要求1所述的方法，其中，限定针对每个可移动构件的坐标系的步骤包括：限定针对与所述可移动构件关联的一个或更多个传感器的坐标系的步骤。
16.根据权利要求1所述的方法，其中，根据所述运动学关系和每个所识别构件的相对位置修改所述传感器或传感器测量结果的步骤还包括：根据与所述可移动构件关联的所述一个或更多个传感器修改所述传感器或传感器测量结果。
17.根据权利要求1所述的方法，其中，所述方法还包括以下步骤：确定可移动构件的坐标系与所述可移动构件的传感器的坐标系之间的位移。
18.根据权利要求1所述的方法，其中，所述方法还包括以下步骤：确定可移动构件的坐标系的轴与所述可移动构件的传感器的坐标系的轴之间的角度偏移。
19.根据权利要求1所述的方法，其中，所述传感器位于所述工作设备的机架上，并且所述方法还包括以下步骤：限定针对所述工作设备的所述机架的坐标系。
20.根据权利要求1所述的方法，所述方法还包括：使用修改后的传感器测量结果确定所述工作设备的工作边缘的位置，以确定所述工作边缘的位置；以及
确定所述工作边缘的位置的步骤包括：考虑运动学效应来确定相邻可移动构件之间的角度。
21.根据权利要求20所述的方法，其中，确定相邻可移动构件之间的角度包括：使用粗调平算法。
22.根据权利要求20所述的方法，其中，确定相邻可移动构件之间的角度包括：使用姿态航向参考系统AHRS。
23.根据权利要求22所述的方法，其中，使用AHRS包括：使用互补滤波器、卡尔曼滤波器、扩展卡尔曼滤波器、Sigma点卡尔曼滤波器、集合卡尔曼滤波器以及粒子滤波器的家族中的一个。
24.根据权利要求20所述的方法，其中，考虑运动学效应包括：补偿针对运动学加速度的传感器测量结果，并且忽略当确定运动学加速度超过预定等级时的传感器测量结果。
25.一种针对运动学加速度补偿来自工作设备的传感器的一个或更多个传感器测量结果的系统，所述系统包括：
工作设备，所述工作设备具有相对于彼此可移动的多个构件；以及
处理器，所述处理器与所述传感器通信，所述处理器被构造为：
取得传感器测量结果；
获得针对与每个可移动构件相关的一个或更多个传感器的坐标系；
获得针对每个坐标系的运动学关系，包括确定运动学链；
确定所述可移动构件的相对位置，所述相对位置包括该可移动构件的角度；
确定所述运动学链中的每个可移动构件相对于相邻可移动构件的相对移动；以及
根据每个所识别的可移动构件的所述运动学关系、相对位置、以及相对移动修改所述传感器或传感器测量结果，以考虑并且补偿由所有相对移动产生的所述运动学加速度。
26.根据权利要求25所述的系统，其中，所述工作设备包括机架，并且所述传感器位于所述机架上。
27.根据权利要求25所述的系统，其中，所述处理器位于所述工作设备上。
28.根据权利要求25所述的系统，其中，所述处理器被配置为获取针对每个可移动构件的已存储坐标系。
29.根据权利要求25所述的系统，其中，所述处理器被配置为获得针对每个坐标系的已存储运动学关系。
30.根据权利要求25所述的系统，其中，所述处理器还被配置为确定所测量内容的以下组中的至少一个：线性加速度、角速率或角加速度；并且所述处理器还被配置为使用所确定的线性加速度、角速率和/或角加速度修改所述传感器或传感器测量结果。
31.根据权利要求25所述的系统，其中，所述工作设备是施工车辆、农业车辆或采矿车。
32.根据权利要求31所述的系统，其中，所述车辆是挖掘机、推土机、平地机、装载机、雪道整理机、拉铲挖掘机、拖拉机、反铲挖土机、收割机、起重机、钻孔机或铲车中的一个。
33.根据权利要求25所述的系统，其中，所述工作设备是挖掘机，并且所述一个或更多个可移动构件至少包括动臂、斗杆以及铲斗。
CN201680021501.5A 2015-04-13 2016-04-13 提供对运动学加速度的补偿的方法和补偿测量结果的系统 Active CN107532897B (zh)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
AU2015901311 2015-04-13
AU2015901311A AU2015901311A0 (en) 2015-04-13 Dynamic motion compensation
PCT/AU2016/050270 WO2016164975A1 (en) 2015-04-13 2016-04-13 Dynamic motion compensation

Publications (2)

Publication Number Publication Date
CN107532897A CN107532897A (zh) 2018-01-02
CN107532897B true CN107532897B (zh) 2020-11-17

Family

ID=57125477

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201680021501.5A Active CN107532897B (zh) 2015-04-13 2016-04-13 提供对运动学加速度的补偿的方法和补偿测量结果的系统

Country Status (6)

Country Link
US (1) US10704233B2 (zh)
EP (1) EP3283841B1 (zh)
CN (1) CN107532897B (zh)
AR (1) AR104232A1 (zh)
AU (2) AU2016247575B2 (zh)
WO (1) WO2016164975A1 (zh)

Families Citing this family (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102016014759A1 (de) * 2016-12-10 2018-06-14 Hydac Electronic Gmbh Verfahren und System zur Schätzung eines Winkels sowie Arbeitsmaschine
DE102018200060B4 (de) 2018-01-04 2024-05-08 Robert Bosch Gmbh Verfahren zum Betreiben einer mobilen Arbeitsmaschine und mobile Arbeitsmaschine
US10724842B2 (en) * 2018-02-02 2020-07-28 Caterpillar Trimble Control Technologies Llc Relative angle estimation using inertial measurement units
GB2573304A (en) 2018-05-01 2019-11-06 Caterpillar Inc A method of operating a machine comprising am implement
CN108956003B (zh) * 2018-07-17 2020-10-20 崧智智能科技(苏州)有限公司 一种实时标定六维传感器姿态的方法、装置及终端设备
DE102018118147A1 (de) 2018-07-26 2020-01-30 Liebherr-Mining Equipment Colmar Sas Verfahren zum Bestimmen eines Winkels eines Arbeitsgeräts einer Maschine
CN109214050B (zh) * 2018-08-02 2023-10-17 中国人民解放军61540部队 一种极区垂线偏差无奇异性详细计算模型及其建模方法
DE102019201099A1 (de) * 2019-01-29 2020-07-30 Robert Bosch Gmbh Verfahren zur Zustandsschätzung von Lage und Orientierung von mehreren beweglichen Modulen eines gemeinsamen Systems
DE102019201091A1 (de) * 2019-01-29 2020-07-30 Robert Bosch Gmbh Verfahren zur Zustandsschätzung von Lage und Orientierung von mehreren beweglichen Modulen eines gemeinsamen Systems
CN109932736B (zh) * 2019-04-08 2022-05-10 上海致灵信息科技有限公司 一种室外全场景全天候的厘米级定位系统和方法
US11702818B2 (en) 2019-05-15 2023-07-18 Deere & Company Motor grader cutting edge wear calibration and warning system
US11686067B2 (en) 2019-05-15 2023-06-27 Deere & Company Motor grader cutting edge wear calibration and warning system
DE102019207159A1 (de) * 2019-05-16 2020-11-19 Robert Bosch Gmbh Verfahren zur Arretierung eines Werkzeugs einer Baumaschine in einer vorgegebenen Neigung
DE102019207164A1 (de) * 2019-05-16 2020-11-19 Robert Bosch Gmbh Verfahren zum Ablegen eines Werkzeugs einer Baumaschine
JP7255364B2 (ja) * 2019-05-31 2023-04-11 セイコーエプソン株式会社 移動体、センサーモジュール及びセンサーモジュールの較正方法
US11549240B2 (en) * 2019-11-19 2023-01-10 Caterpillar Trimble Control Technologies Llc Tracking rotation with a swing sensor
CN111179303B (zh) * 2020-01-07 2021-06-11 东南大学 基于粒子滤波的谷物收获机器人视觉导航方法及其应用
US11624169B2 (en) 2020-06-18 2023-04-11 Deere & Company Excavator with improved movement sensing
US11873621B2 (en) 2020-11-30 2024-01-16 Deere & Company System and method for tracking motion of linkages for self-propelled work vehicles in independent coordinate frames
US12006663B2 (en) 2021-08-05 2024-06-11 Deere & Company Calibrating mounting misalignments of sensors on an implement of a work machine using swing motion
DE102022203962A1 (de) 2022-04-25 2023-10-26 Robert Bosch Gesellschaft mit beschränkter Haftung Verfahren zur Lageschätzung einer Arbeitskinematik einer Arbeitsmaschine und Arbeitsmaschine

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005061024A (ja) * 2003-08-11 2005-03-10 Hitachi Constr Mach Co Ltd 作業機械の位置計測表示システム
CN201247045Y (zh) * 2008-04-30 2009-05-27 山西焦煤集团有限责任公司 悬臂掘进机头位姿的测量系统
CN102066875A (zh) * 2008-06-17 2011-05-18 卡特彼勒天宝控制技术有限责任公司 测斜仪测量系统和提供用于运动诱导加速度误差的校正的方法
DE202011003220U1 (de) * 2011-02-25 2011-08-11 Jan Rotard Steuerung für Arbeitsmaschinen mit Ausleger
CN102459766A (zh) * 2009-04-20 2012-05-16 罗伯特·博世有限公司 具有工作臂的位置控制装置的移动工作机械和用于控制移动工作机械的工作臂的位置的方法
CN103899338A (zh) * 2014-03-11 2014-07-02 中国矿业大学 一种基于空间坐标变换的液压支架工作姿态确定方法

Family Cites Families (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6253160B1 (en) * 1999-01-15 2001-06-26 Trimble Navigation Ltd. Method and apparatus for calibrating a tool positioning mechanism on a mobile machine
AU2002331786A1 (en) * 2001-08-31 2003-03-18 The Board Of Regents Of The University And Community College System, On Behalf Of The University Of Coordinated joint motion control system
US8065037B2 (en) * 2007-08-07 2011-11-22 Board Of Regents Of The Nevada System Of Higher Education, On Behalf Of The University Of Nevada, Reno Control method and system for hydraulic machines employing a dynamic joint motion model
DE102007045846A1 (de) * 2007-09-26 2009-04-02 Deere & Company, Moline Landwirtschaftliche Maschine und Verfahren zur Positionsbestimmung
US8033751B2 (en) 2009-10-08 2011-10-11 Caterpillar Trimble Control Technologies Llc Gyro compensated inclinometer for cross slope control of concrete screed
EP2586918A4 (en) * 2010-06-23 2014-10-29 Doosan Infracore Co Ltd APPARATUS AND METHOD FOR CONTROLLING A WORKING TRACK OF A CONSTRUCTION MACHINE
US8620610B2 (en) * 2011-03-16 2013-12-31 Honeywell International Inc. Crane jib attitude and heading reference system and method
EP2511677B1 (en) * 2011-04-12 2020-08-19 Tamtron Oy Monitoring system for a material transfer vehicle
US9020776B2 (en) 2011-09-28 2015-04-28 Caterpillar Inc. Inclination angle compensation systems and methods
US9618338B2 (en) * 2014-03-18 2017-04-11 Caterpillar Inc. Compensating for acceleration induced inclination errors
US9279235B1 (en) * 2014-09-03 2016-03-08 Caterpillar Inc. Implement position control system having automatic calibration
DE112015000031B4 (de) * 2015-03-27 2017-03-02 Komatsu Ltd. Kalibrierungsvorrichtung für eine Baumaschine und Verfahren zum Kalibrieren eines Arbeitsausrüstungs-Parameters für eine Baumaschine
US9617708B2 (en) * 2015-08-06 2017-04-11 Honeywell International, Inc. Methods and apparatus for correcting a position of an excavation vehicle using tilt compensation
US10002469B2 (en) * 2016-09-09 2018-06-19 The University Of Hong Kong Inclinometer methods and systems for generating and calibrating an in-vehicle inclination angle
US10017914B1 (en) * 2016-12-20 2018-07-10 Caterpillar Trimble Control Technologies Llc Excavator four-bar linkage length and angle offset determination using a laser distance meter

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005061024A (ja) * 2003-08-11 2005-03-10 Hitachi Constr Mach Co Ltd 作業機械の位置計測表示システム
CN201247045Y (zh) * 2008-04-30 2009-05-27 山西焦煤集团有限责任公司 悬臂掘进机头位姿的测量系统
CN102066875A (zh) * 2008-06-17 2011-05-18 卡特彼勒天宝控制技术有限责任公司 测斜仪测量系统和提供用于运动诱导加速度误差的校正的方法
CN102459766A (zh) * 2009-04-20 2012-05-16 罗伯特·博世有限公司 具有工作臂的位置控制装置的移动工作机械和用于控制移动工作机械的工作臂的位置的方法
DE202011003220U1 (de) * 2011-02-25 2011-08-11 Jan Rotard Steuerung für Arbeitsmaschinen mit Ausleger
CN103899338A (zh) * 2014-03-11 2014-07-02 中国矿业大学 一种基于空间坐标变换的液压支架工作姿态确定方法

Also Published As

Publication number Publication date
AU2016247575A1 (en) 2017-10-26
CN107532897A (zh) 2018-01-02
US20180298593A1 (en) 2018-10-18
WO2016164975A1 (en) 2016-10-20
EP3283841A4 (en) 2018-12-26
EP3283841B1 (en) 2024-07-03
AR104232A1 (es) 2017-07-05
EP3283841A1 (en) 2018-02-21
AU2021201358A1 (en) 2021-03-18
AU2016247575B2 (en) 2020-12-10
US10704233B2 (en) 2020-07-07
AU2021201358B2 (en) 2022-05-12

Similar Documents

Publication Publication Date Title
CN107532897B (zh) 提供对运动学加速度的补偿的方法和补偿测量结果的系统
US11530920B2 (en) Controlling movement of a machine using sensor fusion
US7139651B2 (en) Multi-source positioning system for work machines
US9020776B2 (en) Inclination angle compensation systems and methods
KR101833603B1 (ko) 작업 기계의 제어 시스템 및 작업 기계
JP7307736B2 (ja) 慣性計測装置を使用した相対角度推定
JP6976412B2 (ja) 単一の位置センサを用いる回転プラットフォームのヨーおよび回転中心の決定方法
US20140122016A1 (en) Machine Positioning System Having Angular Rate Correction
AU2015305864B9 (en) Earthmoving machine comprising weighted state estimator
US9580104B2 (en) Terrain-based machine comprising implement state estimator
AU2015200793A1 (en) Compensating for acceleration induced inclination errors
US9145144B2 (en) Inclination detection systems and methods
EP3767036B1 (en) Estimation with gyros of the relative attitude between a vehicle body and an implement operably coupled to the vehicle body
US10371522B2 (en) Iterative estimation of centripetal accelerations of inertial measurement units in kinematic chains
CN117616178A (zh) 用于摆动动臂式挖掘机的竖直轴接头部角度估计的基于imu的系统
US9052391B2 (en) Backup velocity estimation utilizing traction device speed
RU2566153C1 (ru) Устройство для определения положения рабочего органа машины
RU2478757C2 (ru) Способ определения положения режущей кромки отвала автогрейдера
US20240044109A1 (en) Yaw estimation systems and methods for rigid bodies of earthmoving machines connected by a revolute joint
CN116203564A (zh) 一种基于惯导的掘锚机多源信息融合定位定向系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant