|
(11) | EP 4 818 413 A1 |
| (12) | EUROPEAN PATENT APPLICATION |
|
|
|
|
||||||||||||||||||||||||
| (54) | CALIBRATING HEAVY EQUIPMENT MACHINES BASED ON SEQUENTIALLY ACQUIRED POINT CLOUDS |
| (57) The invention is notably directed to a computer-implemented method of calibrating
a heavy equipment machine (1). The machine includes a lower structure (12), an upper
structure (14) linked to the lower structure, and limbs (101 - 103) that are successively
linked through joints (111 - 113), from the upper structure (14). The limbs include
a boom (103), a stick (102), and an end effector (101). The method comprises sequentially
acquiring multiple point clouds of the limbs, using a perception sensor system (121,
122). The multiple point clouds correspond to multiple respective joint configurations
of each of the joints (111 - 113). The multiple respective joint configurations are
obtained for each joint, one joint at a time, by incrementally rotating each joint
while keeping all other joints static. Each of the multiple point clouds is acquired
for a respective joint configuration while keeping all of the joints (111 - 113) and
the upper structure (14) static. The method then determines rotation characteristics
of the joints based on the multiple point clouds acquired for each joint. The invention
is further directed to related computerized systems, other systems, and computer program
products.
|
TECHNICAL FIELD
[0001] The invention relates in general to the field of techniques (computer-implemented methods, computerized systems, other systems, and computer program products) of calibrating heavy equipment machines. In particular, it is directed to methods that determine rotation characteristics of joints of a heavy equipment machine (e.g., an excavator) based on multiple point clouds acquired for each of the joints, one joint at a time, by incrementally rotating each joint while keeping all other joints static. The point clouds are preferably obtained thanks to perception sensors on board the heavy equipment machine.
BACKGROUND
[0002] In the construction industry, one of the largest industry sectors worldwide, automation is becoming increasingly important to alleviate the prevalent shortage of trained workforce and thus improve productivity. Additionally, automation on construction sites can help reduce the high number of fatal work injuries. In the longer run, also space applications might become increasingly relevant. Several methods have been proposed to automate operation of heavy equipment machines such as excavators.
[0003] Accurate kinematic measurements and calibration procedures are needed to enable autonomous operation of heavy equipment machines such as excavators. Additionally, autonomous operation of such machines typically requires detailed machine meshes. Traditional methods rely on costly equipment like total stations to determine kinematic properties. Moreover, meshes derived from CAD models may sometimes be inadequate due to manufacturing variances, may not be readily available, and/or may come to deviate from actual part dimensions over time due to material wear.
SUMMARY
[0004] According to a first aspect, the invention is embodied as a computer-implemented method of calibrating a heavy equipment machine. The heavy equipment machine includes a lower structure, an upper structure linked to the lower structure, and limbs that are successively linked through joints, from the upper structure. The limbs include a boom, a stick, and an end effector. The method comprises sequentially acquiring multiple point clouds of the limbs, thanks to a perception sensor system. The multiple point clouds correspond to multiple respective joint configurations of each joint of the joints. The multiple joint configurations are obtained for each j oint, one joint at a time, by incrementally rotating each joint while keeping all other joints static. Each of the multiple point clouds is acquired for a respective one of the joint configurations while keeping all of the joints and the upper structure static. Eventually, the method determines rotation characteristics of the joints based on the multiple point clouds acquired for each of the joints.
[0005] This method provides an efficient and affordable solution to automate the kinematic calibration of joints of heavy equipment machines. This solution leverages point clouds, as opposed to prior calibration methods that use expensive equipment such as total stations. While prior calibration methods are time-intensive and require much human expertise, on-site, the present approach can be entirely automated. This approach conceptually differs from previous approaches, if only because it leverages the machine limbs as calibration features, instead of relying on external calibration targets like prisms, checkerboards, or other tags. While kinematic calibration methods exist for industrial robot arms equipped with electric motors and precise encoders, such methods are not applicable to large and imprecise heavy equipment limbs, hence the benefits of the proposed method. Moreover, one can take advantage of the present approach to calibrate inertial measurement units (IMUs) with respect to their respective limbs, inasmuch as IMU data can be recorded while acquiring the point clouds.
[0006] In preferred embodiments, the perception sensor system includes one or more perception sensors that are on board the heavy equipment machine. Accordingly, the parameters sought (joint rotations, poses, etc.) can be more easily determined with respect to the known reference frame of the perception sensor(s). On the contrary, the reference frames of external sensors must be precisely aligned with the reference frames of the joints, the precise geometry of which is a priori unknown. Moreover, the same on-board sensor can later be exploited to autonomously operate the heavy equipment machine, thereby ensuring better consistency between calibration and autonomous operations. That is, any on-board perception sensor is already calibrated with respect to the upper structure, which solves the sensor-to-machine calibration problem.
[0007] The one or more perception sensors optionally include at least one Lidar. At least one of the one or more perception sensors may for instance be arranged on top of the upper structure, to ensure a more comprehensive perception. In embodiments, the perception sensor system includes at least two perception sensors, including one perception sensor arranged on top of the upper structure and one perception sensor arranged on one of the limbs, for example at an end of the boom.
[0008] In embodiments, the joints include: a first joint linking the end effector to the stick; a second joint linking the stick to the boom; and a third joint linking the boom to the upper structure. The multiple point clouds are sequentially acquired for each joint, one joint at a time, starting with the third joint, then moving on to the second joint, and then to the first joint. Proceeding in this order ensures kinematic consistency; all child limbs of the current joint (i.e., the joint for which multiple configurations are currently being scanned) can move together without relative motion.
[0009] The multiple joint configurations are preferably obtained by incrementally rotating said each joint to cover a majority, preferably an entirety, of a workspace of said each joint, one joint after the other, so as to cover a majority, preferably an entirety, of a workspace of the limbs. Calibration is therefore carried out on the basis of a suitably large working space, ideally as large as the perception sensor system can perceive it.
[0010] In embodiments, determining the rotation characteristics further comprises segmenting limbs from the point clouds acquired, preferably by identifying stationary points in the multiple point clouds acquired, to obtain segmented limbs. The rotation characteristics are determined by executing an alignment procedure based on an Iterative Closest Point (ICP) algorithm. Executing the alignment procedure causes to perform a point cloud alignment to align the segmented limbs by adjusting geometric transformations (e.g., translations and rotations). The rotation characteristics are determined from the adjusted geometric transformations.
[0011] For example, the determined rotation characteristics comprise a rotation axis and a rotation center of said each joint. The adjusted geometric transformations comprise, for any two of the segmented limbs that the ICP algorithm attempts to align, a rotation R and a translation t. In that case, executing the alignment procedure may cause to apply the rotation R to rotate a given segmented limb of said any two of the segmented limbs around a given origin and then translating it by the translation vector t. This is done so as to determine: (i) a rotation axis from the rotation R, this rotation axis indicating a direction around which the given segmented limb rotates, and (ii) a rotation center c for this rotation axis, from a relationship between c and a translated position c' thereof, under the assumption that both the rotation center c and the translated position c' lie on a circle having a radius defined by the adjusted geometric transformations.
[0012] The ICP algorithm makes it possible to obtain accurate translation vectors and rotation matrices. Still, this algorithm allows some task specific improvements. In particular, it can be customized to favor precise identification of joint parameters over mere point cloud alignments. In addition, it can be complemented by a refinement procedure as described below, if necessary.
[0013] In this regard, in preferred embodiments, executing the alignment procedure comprises iteratively running the ICP algorithm and a refinement procedure. That is, estimates of the rotation characteristics of the joints are obtained from the ICP algorithm and then refined through the refinement procedure. As per the iterative procedure, the refined estimates are fed as input to the ICP algorithm to obtain further estimates of the rotation characteristics (two or more iterations are accordingly performed). Such an approach ensures convergence to a remarkably accurate and consistent set of joint rotation characteristics.
[0014] The estimates preferably are refined so as to ensure consistency of the rotation characteristics of the joints across all of the multiple point clouds acquired for said each joint, e.g., by minimizing a point-to-plane error between transformed source points and target points on each of the limbs. In particular, the refinement procedure can be devised to iteratively ensure that the ICP outputs adhere to physically possible geometric transformations, i.e., to the rotation R and the translation t that are mechanically consistent with the joint properties.
[0015] In embodiments, the method further comprises determining, for each given joint of one or more of the joints, a position of a rotation center along a corresponding rotation axis, as determined for each given joint. This is achieved by projecting all of the multiple point clouds as acquired for each given joint onto the corresponding rotation axis to obtain projected points and then averaging the projected points. Thus, even though it is a priori unknown, the position of the rotation center can be determined thanks to the accumulated point clouds.
[0016] Similarly, other features of the end effector may be accurately determined. For instance, in embodiments, the method further comprises projecting an aligned point cloud corresponding to the end effector onto a plane perpendicular to a rotation axis of the joint of the end effector, to identify a farthest point from a rotation center thereof and assigning the identified point to a tip of the end effector, which will be useful for the subsequent autonomous operation of the machine.
[0017] In embodiments, the method further comprises generating a mesh for one or more of the limbs, based on the multiple point clouds acquired. The generated meshes can then advantageously be used for the autonomous operation of the machine. In particular, the mesh may be generated by first classifying points of the point clouds in accordance with predefined parts of the one or more limbs and accordingly determined parameters from the point clouds, and then fitting the point clouds to a parametric mesh using a genetic algorithm and the determined parameters.
[0018] In embodiments, the method further comprises calibrating inertial measurement units arranged on the limbs by: (i) recording data corresponding to signals obtained from the inertial measurement units, while incrementally rotating said each joint; and (ii) determining orientations of the inertial measurement units relative to respective ones of the joints corresponding to said limbs, based on the recorded data. This takes advantage of the point cloud acquisition, inasmuch as IMU data are recorded while acquiring the point clouds.
[0019] According to another aspect, the invention is embodied as a computerized system for calibrating a heavy equipment machine including a lower structure, an upper structure linked to the lower structure, and limbs that are successively linked through joints, from the upper structure, the limbs including a boom, a stick, and an end effector. The computerized system comprises processing means and an interface, where the interface is adapted to interface the processing means with each of a control unit (which is configured to rotate the joints, in operation), and a perception sensor system comprising one or more perception sensors, which are preferably on board the heavy equipment machine. The processing means are configured to perform a method according to any of the embodiments described above. To that aim, the processing means are configured to communicate with each of the control unit and the perception sensor system through the interface.
[0020] According to a further aspect, the invention is embodied as a system comprising the computerized system and the perception sensor system as described above, wherein the one or more perception sensors of the perception sensor system are adapted to be mounted on top of the upper structure of the heavy equipment machine.
[0021] In embodiments, the one or more perception sensors comprises: (i) a sensor rack, which includes a Lidar, a camera (e.g., a fish-eye camera), and an IMU, where the sensor rack is adapted to be mounted on top and at the front of the upper structure; and (ii) two sensor heads, wherein each of the two sensor heads includes a GNSS antenna and an IMU. In addition, each of the two sensor heads includes one or more, preferably each, of: a Lidar, a WIFI antenna, and at least one camera. For example, each sensor head may integrate two fish-eye cameras, in addition, to a WIFI antenna, a Lidar, an IMU, and a GNSS antenna. The sensor heads can be used to achieve a more comprehensive environmental perception during the autonomous operation of the machine. Each sensor head is preferably designed as single milled compact unit, which integrates multiple sensors as listed above. As a result of its compact design, the sensor components of each sensor head are located closely together in the same or nearby location, making calibration easier.
[0022] According to a final aspect, the invention is embodied as a computer program product for calibrating a heavy equipment machine including a lower structure, an upper structure linked to the lower structure, and limbs that are successively linked through joints, from the upper structure, the limbs including a boom, a stick, and an end effector. The computer program product comprises a computer readable storage medium having program instructions embodied therewith. The program instructions are executable by processing means to cause the latter to perform a method according to any of the embodiments described above.
BRIEF DESCRIPTION OF THE DRAWINGS
[0023] These and other objects, features and advantages of the present invention will become apparent from the following detailed description of illustrative embodiments thereof, which is to be read in connection with the accompanying drawings. The illustrations are for clarity in facilitating one skilled in the art in understanding the invention in conjunction with the detailed description. In the drawings:
FIG. 1 schematically depicts a side view of an excavator, which include on-board perception sensors, as involved in embodiments;
FIG. 2 is a realistic 3D view of an excavator, having a sensor rack and two sensor heads on top of its cabin, as in embodiments;
FIG. 3 is a block diagram of a system for calibrating a heavy equipment machine, also illustrating the processing flow from perception sensors to autonomous operation of the machine, according to embodiments;
FIG. 4 schematically illustrates the processing of multiple point clouds acquired sequentially for a given joints, by incrementally rotating this joint, to obtain a registered point cloud, based on which a mesh of a shovel of an excavator and rotation characteristics of its joint are determined, with a view to eventually obtaining a robot description for the excavator, as in embodiments;
FIG. 5 is a flowchart illustrating high-level steps of a calibration method according to embodiments;
FIG. 6 is a block diagram schematically illustrating an iterative procedure that intertwines an Iterative Closest Point (ICP) algorithm and a refinement algorithm to obtain and refine estimates of the rotation characteristics of joints of a machine as shown in FIGS. 1 and 2, as involved in embodiments;
FIG. 7 illustrates an implementation of the ICP algorithm, in 2D, which is useful to understand embodiments of the invention;
FIG. 8A illustrates a method for determining the position of a rotation center of a shovel joint along a corresponding rotation axis, by projecting multiple point clouds as acquired for this joint onto its rotation axis, while FIG. 8B illustrates the determination of the length of the shovel, as in embodiments. Each of FIGS. 8A and 8B shows multiple point clouds, superimposed, and rendered using a halftone process;
FIG. 9 schematically illustrates a classification of points of a point cloud of an excavator bucket into body and teeth, as involved in embodiments; and
FIG. 10 shows the outcome of a fitting procedure, whereby a point cloud is fitted to a parametric mesh of the excavator bucket, to obtain an accurate representation thereof, as in embodiments.
[0024] The accompanying drawings show simplified representations of devices or parts thereof, as involved in embodiments. Technical features depicted in the drawings are not necessarily to scale. Similar or functionally similar elements in the figures have been allocated the same numeral references, unless otherwise indicated.
[0025] Computerized systems, systems, computer-implemented methods, and corresponding computer program products, all embodying the present invention, will now be described, by way of non-limiting examples.
DETAILED DESCRIPTION OF EMBODIMENTS OF THE INVENTION
[0026] The following description is structured as follows. General embodiments and high-level variants are described in section 1. Section 2 addresses particularly preferred embodiments. Section 3 concerns technical implementation details. Note, the present method and its variants are sometimes collectively referred to as the "present methods". All references Sn refer to methods steps of the flowcharts of FIG. 5, while numeral references pertain to devices, components, and concepts, as involved in embodiments of the present invention.
1. General embodiments and high-level variants
[0027] Referring to FIGS. 1 - 5, a first aspect of the invention concerns a computer-implemented method for calibrating a heavy equipment machine 1. The autonomous machine 1 can for instance be a crawler or mobile machine, such as a mobile crane. It may be an electric machine (with electrically actuated limbs) or a hydraulic machine. The machine may for instance be an excavator or another construction machine, such as a crane, a bagger, or a crawler. The machine is equipped with an end effector 101, which is articulated about the arm stick 102.
[0028] This machine 1 includes a lower structure 12 and an upper structure 14 (e.g., with a cabin), which is linked to the lower structure. The machine 1 also includes limbs 101 - 103 originating from the upper structure 14, where the limbs are successively linked through joints 111 - 113. The limbs include a boom 103, a stick 102, and an end effector 101. The end effector 101 is typically a bucket, a shovel, a hammer, a gripper or another excavator attachment. In the accompanying drawings, the end effector 101 is assumed to be a bucket. The end effector is articulated about the arm stick 102, itself articulated about the boom 103. The latter pivots about the upper structure 14, which supports a cabin 104. The upper structure 14 can itself be regarded as a limb of the lower structure 12. That is, the upper structure 14 rotates with respect to the lower structure 12. Its rotation axis relative to the lower structure 12 is defined by the axis of the slew joint 114, also called cabin turn joint.
[0029] As seen in FIG. 1, the limbs may generally be consecutive limbs, i.e., without any in-betweens. That is, the boom 103 is a limb of the upper structure 14, the stick 102 is a limb of the boom 103, and the end effector 101 is a limb of the stick 102. However, the machine 1 may possibly include a two-piece boom i.e., a variable angle boom, and/or one or more additional degrees of freedom in the end-effector. So, as defined above, the limbs and joints can be regarded as a minimal set of limbs, inasmuch as additional limbs, limb parts, and/or additional joints, may possibly be involved. The joints link the limbs, i.e., they pivotably connect the limbs to one another, starting from the upper structure 14.
[0030] According to the present invention, the method comprises sequentially acquiring (step S10 in FIG. 5) multiple point clouds 152 of the limbs using a perception sensor system 120. The perception sensor system 120 may for instance involve one or more Lidars 121 - 123, and/or cameras 125 (such as depth or stereo cameras), or any other type of sensor capable of providing signals that can be interpreted as point clouds. Preferred embodiments involve one or more Lidars 121, 123, as assumed in FIGS. 1 - 3. Additional types of sensors are later described in reference to FIG. 2.
[0031] The acquired point clouds correspond to multiple, respective joint configurations of each of the joints 111 - 113. The joint configurations are obtained for each joint, one at a time, by incrementally rotating (step S14) each joint while keeping all other joints static. In addition, each point cloud 152 is acquired S15 for a respective joint configuration while keeping all joints 111 - 113 and the upper structure 14 static, i.e., motionless with respect to each other and the lower structure. The incremental rotations can be controlled through an autonomous or automatic control system. In embodiments, such rotations are controlled through the same control system as later used to autonomously operate the machine 1 (see step S90 in FIG. 5).
[0032] The method then determines S20 - S40 the rotation characteristics of the joints 111 - 113 based on the multiple point clouds 152 acquired for each joint. The determined rotation characteristics may notably comprise a rotation axis and a rotation center of each of the joints. Such characteristics will advantageously be defined, geometrically, with respect to a reference frame of the perception sensor system 120, or at least one sensor 121 thereof, e.g., with respect to the main lidar 121 in FIG. 1, or the sensor rack 123 in FIG. 2. In other words, the joints 111 - 113 are calibrated with respect to a known configuration of the sensor(s) of the perception sensor system 120.
[0033] A similar calibration procedure may be used to calibrate the upper structure 14, if necessary, subject to a slight modification. That is, the upper structure 14 can be rotated while scanning its environment, in order to determine its rotation axis with respect to the lower structure 12, based on the perceived environment. In other words, only the limbs 111 - 113 are calibrated with respect to the perception sensor system 120. The lower structure 12 is less important, insofar as the end effector actuation (e.g., digging) accuracy depends on the accuracy of the perception sensor system and the kinematic chain from the perception sensors to the end effector through the joints (i.e., boom, stick, and end effector). Besides, one may rely on a geometry as specified by a CAD model to calibrate the lower structure 12, if necessary.
[0034] Explanations are in order. According to the present approach, the perception sensor system 120 is used to acquire S15 multiple point clouds for respective joint configurations. These configurations are obtained for each joint, one joint at a time, by incrementally rotating S14 a given joint, while keeping all of the other joints static. Then, a point cloud 152 is acquired S15 for the resulting joint configuration, now keeping all of the joints 111 - 113 and the upper structure static (i.e., static with respect to each other and the lower structure). This process repeats for all joints, one after the other, in order to obtain point clouds for all desired joint configurations. Eventually, the method determines the rotation characteristics of the joints based on the multiple point clouds 152 as acquired for all the joints of interest.
[0035] The proposed method provides an efficient solution to automate the kinematic calibration of joints 111 - 113 of heavy equipment machines. This solution leverages point clouds, as opposed to prior calibration methods that use expensive equipment such as total stations. Such prior methods are time-intensive and require substantial human expertise, on-site, while the present approach can be entirely automated, it being reminded that a similar procedure can be applied to the upper structure 14 to determine its rotation axis z.
[0036] Moreover, the proposed approach conceptually differs from previous approaches, because it leverages the machine limbs as calibration features, instead of relying on external calibration targets like prisms, checkerboards, or other tags. While kinematic calibration methods exist for industrial robot arms equipped with electric motors and precise encoders, such methods are not applicable to large and imprecise heavy equipment limbs.
[0037] Typically, one inertial measurement unit (IMU) 130 is arranged on each joint 111 - 113. An additional IMU can be arranged on the upper structure 14. Such IMUs may also need calibration. Now, the extrinsic calibration of IMUs with respect to their respective limbs can take advantage of the present approach, inasmuch as IMU data can be recorded while acquiring the point clouds, using the reference frame associated with the respective limb since each IMU is calibrated to each joint link. Such IMUs are typically used to measure the joint angles during machine operation, once the calibration procedure is completed.
[0038] Preferred embodiments of the present invention involve on-board perception sensors, such as Lidars 121 - 123 and cameras 125. In that case, the heavy equipment machine can be automatically calibrated by exploiting its on-board sensors. IMUs 130 can be calibrated too, taking advantage of the joint calibration, as noted above. In addition, further embodiments generate a 3D mesh model by fitting a point cloud obtained from the on-board sensor to a predefined parametric mesh. Eventually, the heavy equipment machine may be autonomously or semi-autonomously operated S90, in accordance with calibration steps and other computations as performed herein, starting with the determined geometry of the joints 111 - 113.
[0039] All this is now described in detail, in reference to particular embodiments of the invention. To start with, referring to FIG. 1, the perception sensor system 120 preferably includes one or more perception sensors 121 - 124, each arranged on board the heavy equipment machine 1. Again, such perception sensors preferably include at least one Lidar 121 - 123. As seen in FIGS. 1 and 2, one perception sensor 121, 123 may be arranged on top of the upper structure 14, e.g., on top and at the front of the cabin, at the center. In the example of FIG. 1, the perception sensor system includes a single Lidar 121. In FIG. 2, the perception sensor system 120 includes a sensor rack 123, which includes a Lidar, a camera, and an IMU. It further includes two sensor heads 124, which themselves include various types of sensors, for reasons that will become apparent later. In embodiments, an additional perception sensor 122 is arranged on one of the limbs, for example at an outer end of the boom 103, close to the stick 102, as in FIG. 1.
[0040] So, at least one perception sensor is needed, and this sensor is advantageously arranged on board the heavy equipment machine 1. Preferred is to rely on at least one Lidar 121, 123. Such Lidars will advantageously have the following specifications. Their horizontal field of view (horizontal FoV) is at least equal to 180°, preferably of 360°, while their vertical field of view (vertical FoV) will at least be equal to 40°, preferably at least 60° (e.g., -50° to 10°, relative to the horizon). Such Lidars will advantageously have a maximum detection range of at least 30 m, preferably 40 m (at 10 % reflectivity) or 70 m (at 80% reflectivity). Their range precision is preferably less than 4 cm (e.g., 2 cm) at 10 m, with a frame rate of 5 - 20 Hz. In general, a large horizontal FoV will be useful to determine the relative orientation of the slew joint axis z. The larger the better, especially where the limbs involve special joints, for example at the basis of the arm, or in the boom link (e.g., a swivel joint). Similarly, a sufficiently large vertical FoV is needed to see all the limbs from the cabin top.
[0041] The main exteroceptive sensors should ideally be placed as high as possible on the cabin, as assumed in FIGS. 1 and 2. They may possibly be arranged at a certain angle (this depending on the available vertical FoV). A trade-off may be sought between the maximal detection range (how far one wants to be able to scan and detect limbs), the maximal sensing height (how high the libs go), and the proximity detection (how close to the machine does one want to be able to detect limbs, e.g., the end effector).
[0042] The advantage of using a perception sensor on board the heavy equipment machine is that all parameters (joint rotations, poses, etc.) can be more easily determined with respect to the known reference frame of this perception sensor. This makes it easier than with external sensors, whose reference frames must be precisely aligned with the reference frames of the joints 111 - 113, the precise geometry of which is a priori unknown. Moreover, any on-board sensor can later be exploited to autonomously operate the heavy equipment machine, thereby ensuring better consistency between calibration and autonomous operations. That is, on-board perception sensors are already calibrated with respect to the upper structure 14, which solves the sensor-to-machine calibration problem.
[0043] An alternative approach is to use an external sensor, such as a mobile scanning device or an infrastructure-mounted device. This variant has some flexibility as it does not require the heavy equipment machine to have a built-in scanning device, and the point clouds can be collected from different locations. However, this also complicates the process of determining the relative positions of the joints with respect to the external sensor, as noted above. One solution, though, is to first bring the heavy equipment machine to a predetermined position and in a given configuration with respect to an infrastructure containing the perception sensor(s), e.g., using a conveyor or tunnel system, a rollover, or an in-bay system, as in car wash systems. Algorithms are then needed to find a transformation from the external sensor to a distinct point on the machine. In addition, a 360° rotation can be performed to determine the rotation axis and also compute the z value by observing a prism. Then, it is possible to compute all transformations to that pose on the machine. One may also use special infrastructure like a calibration room, which include multiple targets all around, as used for autonomous vehicles.
[0044] As said, the multiple point clouds 152 are sequentially acquired S10 for each joint, one joint at a time. Now, the order used may prove more or less advantageous. As seen in FIG. 1, the joints 111 - 113 include a first joint 111 (linking the end effector 101 to the stick 102), a second joint 112 (linking the stick 102 to the boom 103), and a third joint 113 (linking the boom 103 to the upper structure 14). The multiple point clouds 152 can be acquired S10 for the third joint 113, then moving on to the second joint 112, and then to the first joint 11. For example, for an excavator 1 with a bucket 101, data acquisition can be performed sequentially for each joint, beginning with the boom joint 113, followed by the stick joint 112, and concluding with the bucket joint 111. This ensures kinematic coherence: all child limbs of the current j oint i.e., the joint for which multiple configurations are currently being scanned must move together without relative motion. In other words, one first rotates the boom, stick, and end effector all together, without relative motion, to calibrate the boom, then the stick and the end effector to calibrate the stick, and finally the sole end effector to calibrate the end effector. In variants, one may also proceed in the reverse order (first 111, then 112, and then 113); the kinematic coherence is still maintained.
[0045] Beyond joints 111 - 113, additional joints are involved, such as the turn joint 114 connecting the upper structure 14 (i.e., with the cabin 104) to the lower structure 12. However, the upper structure will likely undergo a different calibration procedure, as noted earlier-the upper structure 14 (with the cabin 104) rotates relative to the lower structure 12, while scanning its environment, based on which the turn joint rotation axis z is determined. This operation can be performed last, i.e., after having acquired the point clouds for the joints 111 - 113 (e.g., the third joint 113, the second joint 112, and then the first joint 111). Alternatively, it is performed first, i.e., before sequentially acquiring the multiple point clouds 152 for each joint, one at a time.
[0046] Ideally, the multiple respective joint configurations are obtained by incrementally rotating S14 the joints 111 - 114 (one at a time), so as to cover the entire workspace of each joint and, in turn, the entire workspace of the limbs 101 - 103. In practice, however, it may be sufficient to scan a subspace of the full workspace, also because the limbs may go outside of the sensor's field of view. As this subspace should nevertheless be as large as possible, one will preferably seek to cover a majority of the workspaces of the joints and limbs. In practice, multiple point clouds (and possibly corresponding images) are captured for each joint by accumulating point clouds for a few seconds while all other joints 111 - 114 (i.e., including joint 114 linking the upper and lower structures 12, 14) remain static. This is followed by an incremental rotation by a specified degree (e.g., 10°). In general, incremental rotations may typically be of between 5 and 20°). This stepwise approach is repeated until the entire (or, at least, a major part of the) operational workspace is covered, resulting in a comprehensive set of point clouds and images for each joint.
[0047] The calibration is then performed by processing the point clouds acquired, as seen in FIG. 3. A preferred pipeline, see FIG. 5, is one that involves:
- A data acquisition procedure S10 as described above;
- Data preprocessing S20;
- A limb segmentation process S30;
- A joint parameter identification S40; and
- A mesh generation S70, notably for the end effector 101, e.g., a bucket.
[0048] The same calibration procedure can be applied to all limbs 101 - 103. Again, a slightly different procedure is used for the turn joint calibration.
[0049] As apparent from the preferred pipeline above, embodiments of the calibration procedure S20 - S40 involve segmenting S30 limbs from the point clouds acquired, so as to obtain segmented limbs. The segmentation step S30 preferably relies on a stationary point filtering, as further discussed below. The limb segmentation causes to obtain a distinct point cloud for each joint and, thus, for each limb. The process defined by steps S10 - S40 typically aims at determining the limb lengths and their location with respect to a reference frame of the machine (particularly the on-board sensor, if any). I.e., step S40 typically returns the lengths of the limbs and their locations.
[0050] The segmentation step S30 is preferably carried out by identifying stationary points and then filtering out the stationary points. The stationary points are identified in the multiple point clouds 152 acquired. The stationary points are points that maintain consistent relative spatial positions across the multiple joint configurations as scanned for all of the joints. The stationary points identified are then filtered out to obtain a filtered dataset. The goal is to obtain distinct point clouds of each limb, so as to segment the limbs. This is performed for each limb, one after the other. For example, when moving only the bucket, points on the boom and stick are stationary, according to the point cloud acquisition scheme discussed above. So, by only retaining the moving points of the bucket, one obtains the segmented bucket points. This procedure makes it possible to determine segments corresponding to the limbs 111 - 113, i.e., segments corresponding to the end effector 101, the stick 102, and the boom 103.
[0051] The upper structure is not involved in the above approach. The perception sensors are preferably mounted rigidly to the cabin and thus cannot see the cabin (at least not entirely). Still, the cabin can be perceived by another sensor, e.g., a Lidar 122, placed on the boom (as assumed in FIG. 1) or, alternatively, on the dipper limb.
[0052] Next, the rotation characteristics are determined by executing S40 an alignment procedure. This procedure is advantageously based on an Iterative Closest Point (ICP) algorithm. Executing S40 this alignment procedure amounts to performing S42 a point cloud alignment procedure, which aligns the segmented limbs by adjusting geometric transformations. Still, this algorithm allows some flexibility. E.g., it can be adapted to favor precise identification of joint parameters over mere point cloud alignments. The rotation characteristics are eventually determined from the adjusted geometric transformations. The determined rotation characteristics may notably comprise a rotation axis and a rotation center of each of the joints, as noted earlier.
[0053] Suitable geometric transformations typically combine a rotation and a translation. In the present context, the adjusted geometric transformations may for instance comprise, for any two of the segmented limbs that the ICP algorithm attempts to align, a rotation R and a translation t. In that case, executing S40 the alignment procedure causes to: (i) apply the rotation R to rotate a given segmented limb of the two segmented limbs around a given origin; and then (ii) translate it by the translation vector t. This determines a rotation axis and a rotation center c. The rotation axis is determined from the rotation R; the rotation axis indicates a direction around which the given segmented limb rotates. The rotation center c is determined for this rotation axis, from a relationship between c and a translated position c' thereof, under the assumption that both the rotation center c and the translated position c' lie on a circle having a radius defined by the adjusted geometric transformations.
[0054] This assumes that the rotation and translation movement of the joint lies on a circle, something that advocates customizing the ICP procedure, as discussed below in detail. General ICP algorithm can nevertheless be used, too. However, the additional information obtained from the above steps can advantageously be exploited in a method as described below. The procedure is designed to match pairs of points. It can be customized to prioritize the identification of j oint parameters over the point cloud alignment, as discussed later.
[0055] In more detail, when point clouds from two different poses of a limb are aligned using an ICP-like algorithm, the transformation T consists of a rotation matrix R and a translation vector t. So, the alignment procedure aligns each of the limbs across any two configurations by first rotating it around the origin and then translating it by the vector t. The rotation matrix R describes how the limb has rotated from one pose to another. From R, we can derive the rotation axis e, which indicates the direction around which the limb rotates. The translation vector t involved in the ICP algorithm represents the movement required to align the point clouds after rotation. However, it does not directly provide the position of the rotation center c, which is needed for kinematic modelling. To estimate c, the point cloud is first rotated around the origin by an angle θ and then translated by a vector t resulting in a transformed position c', as illustrated in FIG. 7. Both c and c' are assumed to lie on a circle with a radius defined by the transformation properties.
[0056] The relationships between c(x, y), t(tx, ty), and c'(x - tx, y - ty), forms a system of equations, which can be solved to obtain c. Namely
[0057] Similarly, in 3D, a set of equations can be devised as follows. First, one defines a vector c ∈
satisfying the following three conditions:
and From the first two conditions, one obtains
and
[0063] So, one can decompose c into a component parallel to t, denoted by α t, and a component perpendicular to t, called x. That is,
[0068] Depending on t · e, one can solve for the part of x that ensures that c is orthogonal to e. Putting everything together, one arrives at:
where n is a unit vector orthogonal to t and is arranged so that c · e = 0. Using the initial ICP transformation, one can determine whether the radical expression should be added or subtracted.
[0069] As seen in the flow of FIG. 5, preferred embodiments intertwine the ICP algorithm S42 with a refinement procedure S44. That is, executing S40 the alignment procedure comprises iteratively running the ICP algorithm S42 and the refinement procedure S44: estimates of the rotation characteristics of the joints 111 - 113 are obtained from the ICP algorithm and then refined S44 through the refinement procedure. So, the refined estimates are fed as input to the ICP algorithm to obtain further estimates of the rotation characteristics. Several iterations can accordingly be performed.
[0070] The process is illustrated in FIG. 6. This pipeline customizes the ICP algorithm and operates iteratively, alternating between ICP step S42 and a joint refinement step S44 (i.e., an optimization). Each stage S42, S44 iteratively uses the outputs of the other as initial guesses, continually refining the joint parameter estimates. This iterative process may continue for a predetermined (fixed) number of iterations. Alternatively, the iterative process continues until the changes in the parameter estimates fall below a predefined threshold, ensuring a convergence to an accurate and consistent set of joint parameters.
[0071] Preferably, the estimates are refined S44 so as to ensure consistency of the rotation characteristics of the joints 111 - 113 across all of the multiple point clouds 152 acquired for each joint. This is preferably performed by minimizing a point-to-plane error between the transformed source points and target points on each of the limbs. A plane-to-plane error minimization procedure may also be used, as an alternative. Other methods are known, which can be used as part of the ICP algorithm.
[0072] In embodiments, the method further comprises determining S50, for a given joint, a position of a rotation center along a corresponding rotation axis, as determined for this given joint. This is achieved by projecting all of the multiple point clouds as acquired for the given joint onto the corresponding rotation axis, so as to obtain projected points (see FIG. 8A) and then averaging the projected points. The position of the rotation center is obtained by projecting all point clouds as accumulated for each joint, as illustrated in FIG. 8A for a bucket joint. The same procedure can actually be performed for any other one of the joints 111 - 113, if necessary. The determined positions of the rotation centers form part of the rotation characteristics. Because there can be more points on the side where the perception sensor(s) is (are) located, one may want to asymmetrically sample the projected points (to restore some balance) before averaging the resulting points to find the average position on the rotation axis In detail, the determination of the joint parameters may be performed so as to obtain an accurate estimation of the rotation axis and the location of this axis-one ideally wants an estimation that is both accurate and precise. That is, the rotation characteristics as initially determined by the alignment procedure may comprise an accurate estimation of the rotation axis of each joint and the relative position of this axis with respect to the reference frame of the main scanning device 121. However, this approach inherently leaves one degree of freedom undetermined: the exact position of the rotation center along the rotation axis. To address this issue, one may employ geometric reasoning based on the physical structure of the excavator's limbs. Typically, the rotation center lies at a significant structural point, such as the midpoint of the limb along the axis of rotation. For the bucket, this midpoint can be estimated by projecting the point cloud onto the identified rotation axis (FIG. 8A), if necessary, after sampling the projected points. By averaging the coordinates of the edge points along the rotation axis, one obtains an approximation of the rotation center's position. The bucket's rotation center can then be used for the other joint rotation centers on their rotation axes, as typically these rotation centers are all in the same plane, which is perpendicular to the rotation axes of all the joints 111 - 113.
[0073] Referring back to the flow of FIG. 5, embodiments of the method further comprise projecting S60 an aligned point cloud corresponding to the end effector 101 onto a plane perpendicular to the rotation axis of the joint 111 of the end effector 101. On this plane, one identifies the farthest point from the rotation center, which corresponds to the end effector's position. The same can be performed for each rotational increment. This makes it possible to identify the farthest point from the rotation center of the end effector 101, as illustrated in FIG. 8B for each rotation increment (the perpendicular planes are identified by dashed lines in FIG. 8B). In turn, the identified point can be assigned to a tip of the end effector 101. This makes it possible to determine the position of the end effector 101, such as the tip of the bucket in FIG. 8B, and measure the length of the shovel limb. All such parameters can then advantageously be used to operate the machine 1 autonomously.
[0074] In embodiments, the method further comprises generating S70 a mesh for one or more of the limbs, based on the multiple point clouds 152 acquired. As indicated in FIG. 4, the mesh generation may for instance involve classifying S72 points of the point clouds. This classification is performed based on predefined parts of the limbs, as well as parameters for such parts, as determined from the point clouds. The point clouds can then be fitted S74 to a parametric mesh, e.g., using a genetic algorithm and the determined parameters.
[0075] For example, the mesh of the bucket 101 may be generated as follows. The multiple point clouds are first processed using intensity filtering and statistical outlier removal. The normal vector of each point is computed to classify bucket points into inner and outer layers. Then, using a projected version of the point cloud, the data is segmented into the bucket body and teeth (see FIG. 9), allowing the algorithm to determine the number of teeth, as well as their width ("Teeth width"), length ("Teeth length"), and the end-effector points of the bucket body ("EE Body") and the teeth ("EE Teeth"), see FIG. 9. Finally, these extracted parameters are used to match a predefined 3D mesh model through an optimization process, such as a genetic algorithm. The mesh model is characterized by 25 parameters in the example of FIG. 9.
[0076] Referring back to the flow of FIG. 5, embodiments of the method further comprise calibrating S80 inertial measurement units (IMUs) arranged on the limbs 101 - 103. One or more additional IMUs will typically be arranged on the upper structure 104. As noted earlier, data corresponding to signals obtained from the IMUs can advantageously be recorded S15 while incrementally rotating S14 each joint. On can then determine S80 orientations of the inertial measurement units relative to the respective joints (the joints corresponding to the limbs, on which the IMUs are arranged), based on the recorded data. In addition to the recorded data, use can be made of the determined rotation characteristics or the joint link frames of the joints, as determined earlier. The relative orientations of the IMUs can for instance be determined based on rotations as estimated from all the linear acceleration measurements. A simpler approach, though, is to use the gravity force vectors computed from the linear acceleration measurements obtained from the IMUs. However, the latter can be inaccurate if there is an accuracy bias. One may also rely on fused outputs from the IMU measurements that take into account angular velocities as well.
[0077] An example of suitable procedure is the following. Two non-colinear vectors are used to determine the rotation between two frames. In order to overcome noise-related inaccuracies, multiple vector pairs are utilized for improved accuracy. For each of the joint 111 - 113 and the upper structure rotation joint 114 (see FIG. 1), the algorithm computes the gravity vector and rotation axis from IMU data, and these vectors are then employed to determine the rotation between the IMUs of adj acent joints. Additionally, the IMU mounted on the upper structure 14 is calibrated with respect to the perception sensor 121, where the joints' coordinate frames are defined, allowing computation of the relative orientation of each joint's IMU to its corresponding coordinate frame.
[0078] Referring now to FIG. 3, another aspect of the invention concerns a computerized system 200 for calibrating a heavy equipment machine 1 as described earlier. The computerized system 200 comprises processing means 210, as well as an interface 205, 215. The interface 205, 215 may involve software, computer hardware, and peripheral devices, or any combination thereof. In all cases, this interface is adapted to interface the processing means 210 with each of the perception sensor system 120 and a control unit 220 for rotating the joints, in operation. In practice, the control unit 220 may notably include, or essentially amount to, a motion controller and form part of an actuation system of the machine 1, as assumed in FIG. 3. In particular, this interface 205, 215 may possibly include several physical interface units 205, 215, as assumed in FIG. 3, where distinct interface units interface the processing means with the perception sensor system 120 and the actuation system's motion controller 220.
[0079] So, the interface 205, 215 is adapted to receive signals from the perception sensor system 120, possibly from any perception sensor thereof, individually, or from a processing system (not shown) of the perception sensor system. Conversely, the interface may relay signals from the processing means 210 to trigger and control perception (e.g., to start and end a scanning sequence). As noted earlier, such perception sensors are preferably on board the machine 1, in operation. In addition, the interface 205, 215 can send signals to, and receive signals from, the control unit of the actuation system 220 of the machine 1. As said, the control unit may include a motion controller. Besides, the actuation system 220 may notably include actuators and/or motors, to actuate the joints and limbs, whether electrically or hydraulically (as known per se).
[0080] The processing means 210 are configured to communicate with each of the control unit and the perception sensor system 120 through the interface 205, 215. The processing means can process data received through the interface. In particular, the processing means are able to process signals obtained from the perception sensors. If necessary, the signals transmitted by the perception sensors may be pre-processed by respective sensor processing systems (not shown) before being passed to the processing means 210. The processing means are otherwise configured to implement steps as described above, with a view to performing methods as described above, starting with the sequential acquisition of the multiple point clouds and the determination of the rotation characteristics of the joints.
[0081] As usual, the processing means 210 may include one or more processors (e.g., a CPU, as well as one or more GPUs) and memory circuits. In operation, the processors read from and write to the main memory. Some redundancy may be ensured, if necessary. In addition, the computerized system typically includes a persistent storage, which may store computerized methods as described earlier in the form of software instructions. In operation, such computerized methods can be loaded in the main memory, for the processing means to implement method steps as described above, through the interface 205, 215.
[0082] The processing means 210 may come to instruct to transmit signals to the control unit 220 of the actuation system of the machine 1, if only to incrementally rotate S14 the joints or to autonomously operate S90 the machine 1. Such steps result in generating commands (e.g.? low-level commands), which are transmitted to the actuation system and, in particular, the motion controller 220. The transmitted commands may notably be dispatched to individual actuators and/or motors, e.g., motors that generate currents to follow the defined commands, as known per se. The transmitted commands may be further processed by the actuation system, if necessary, before being dispatched to the actuators and/or motors. This makes it possible to control the various joints of the machine 1 according to the generated commands. Note, the same processing means 210 may be configured to subsequently operate S90 the machine 1, autonomously. To that aim, feedback signals may be sent to the processing means 210, through the interface unit 215, as shown in FIG. 3. Feedback signals may be sent during the calibration procedure, too.
[0083] A related aspect of the invention concerns a computer program product, which comprises a computer readable storage medium having program instructions embodied therewith. The program instructions are executable by processing means 210 of a computerized system 200 as described above to cause the latter to perform a method according to any of the embodiments as described herein. Section 3 provides additional implementation details related to computer program products and computerized systems as contemplated herein.
[0084] Referring to FIGS. 3 and 4, a further aspect of the invention concerns a general system, which comprises a computerized system 200 as described above, as well as the perception sensor system 120. As noted earlier, the perception sensors 121 - 125 of the perception sensor system 120 may all be structured and configured so as to be arranged on board the heavy equipment machine 1 and, in particular, to be mounted on top of the upper structure 14 (e.g., on top of the cabin 114) of the heavy equipment machine 1.
[0085] Preferably, the perception sensors 121 - 125 include a sensor rack 123, as shown in FIG. 2. The sensor rack 123 may notably include a Lidar and a camera. The camera can be of any type, e.g. a fish-eye camera. As seen in FIG. 2, the sensor rack 123 is adapted to be mounted on top, and at the front of, the upper structure 14 (in fact on top of the cabin 114 in the example of FIG. 2). The Lidar of the sensor rack normally serves as the main Lidar, similar to Lidar 121 in FIG. 1. So, the calibration procedure described earlier can be primarily performed based on the Lidar included in the sensor rack 123, although additional Lidars and other sensors may be involved as now described in detail.
[0086] The perception sensors may advantageously include two sensor heads 124, in addition to the sensor rack 123. The sensor heads 124 are preferably adapted to be mounted on top, and at the back of, the upper structure 14 (at the back of the cabin 114 in the example of FIG. 2). Each sensor head 124 may notably include a GNSS (Global Navigation Satellite System) antenna, an IMU, as well as, e.g., two fish-eye cameras, a Lidar, and a WIFI antenna. Each sensor head is preferably designed as a compact unit, which integrates the multiple sensors. Together with the IMUs of the sensor heads, the GNSS antennas can be used to compute the pose of the machine 1. Since the pose can, in principle, already be derived from Lidar measurement, the pose derived from GNSS in the sensor heads is redundant (albeit obtained from other types of sensors), something that can be exploited to lower the uncertainty in the pose estimate, through fusing multiple measurements, and for verification purposes, in the interest of safety. Additionally, each sensor head is preferably designed as single compact unit (e.g., milled, as assumed in FIG. 2), which integrates multiple sensors as listed above. As a result of its compact design, the sensor components of each sensor head are located closely together in the same or nearby location, making calibration easier.
[0087] The point cloud obtained from the Lidar in the sensor rack 123 can be fused with point clouds obtained from the additional Lidars in the sensor heads 124. So, in embodiments, the multiple point clouds 152 are sequentially acquired S10 as follows. Each joint configuration is obtained for each joint (one joint at a time), by incrementally rotating S14 a given joint while keeping all other joints static. Next, a point cloud 152 is acquired S15 by all of the Lidars involved (i.e., three Lidars, one in the rack 123, two in each sensor head 124, in the example of FIG. 2) for this joint configuration and, this, while keeping all joints and the upper structure 14 static, before moving on to the next joint configuration. The point clouds obtained from the various Lidars are then fused, whereby the calibration process described earlier operates on fused point clouds. Similarly, point clouds obtained from additional sensors like depth cameras can be fused with Lidar point clouds, if necessary.
[0088] Conventional machine control systems typically use two GNSS antennas mounted at the rear of the machine. In the present case, however, the sensor heads 124 replace such antennas for enhanced autonomy and precision. Each sensor head is preferably designed as a single milled, compact unit. As a whole, the two sensor heads 124 and the sensor rack 123 can be designed to provide 360° Lidar coverage and 360° camera coverage, ensuring a comprehensive environmental perception.
[0089] When using a sensor configuration as shown in FIG. 2, the calibration process can be performed as follows, to ensure a precise sensor alignment with the machine 1:
- Data acquisition for the lidar-to-lidar calibration. Use is made of data obtained through the same data recording procedure as for the turn joint calibration method discussed in section 1, except that point clouds are now acquired through the three Lidars. So, this data acquisition step is preferably performed while calibrating the turn j oint, as described earlier.
- Lidar-to-lidar calibration. For each Lidar (sensor rack 123 and sensor heads 124), individual scans are aligned using an ICP algorithm to build a comprehensive map. Use can be made of the same ICP algorithm as described earlier, although no refinement procedure is needed here. The Lidar-generated maps are then compared to determine the precise transformation between the maps and hence between the Lidars.
- Calibration of the sensor heads' sensors.
∘ The calibration of the locations of the GNSS antenna to the machine 1 utilizes a computer-aided design (CAD) model. Because the GNSS system only provides position data and the sensor head is manufactured as a single compact unit, the translation accuracy is maintained in the millimeter range.
∘ The camera calibration is achieved using a calibration method that aligns each camera with the Lidar data.
∘ Similarly, the IMU calibration is achieved by precisely aligning the IMUs with the main Lidar, i.e., the Lidar in the sensor rack 123.
[0090] To summarize, the calibration of the GNSS antennas to the machine 1 is performed in three steps: (1) The GNSS antennas are calibrated to the Lidars of the sensor heads, based on CAD data of the sensor heads; (2) the Lidars of the sensor heads are calibrated to the Lidar of the sensor rack, thanks to an ICP algorithm; and (3) The Lidar of the sensor rack is calibrated to the machine 1, thanks to methods as discussed earlier. Such a cascaded calibration process ensures that all sensors are calibrated to each other and to the machine 1. All such sensors can then be used for the autonomous operation of the machine 1.
[0091] The above embodiments have been succinctly described in reference to the accompanying drawings and may accommodate a number of variants. Several combinations of the above features may be contemplated. Examples are given in the next section.
2. Particularly preferred embodiments
2.1. Preferred implementation of the calibration methods
[0092] The following concerns an automatic Robot Description Format generation process that facilitates the rapid and precise setup of excavators on-site using onboard sensors such as cameras and Lidars. Accurate kinematic measurements and a detailed machine mesh can advantageously be exploited to enable efficient autonomous excavation. Traditional methods rely on costly equipment like total stations to determine kinematic properties, and meshes from CAD models may fall short due to manufacturing variances, unavailability, and changes during operation due to material wear. The approach below eliminates these constraints by leveraging onboard sensors to perform kinematic calibration and generate a tailored mesh for each excavator. Strictly speaking, the methods discussed below only needs to be executed when setting up a new machine 1, although the mesh generation part can be repeated if, for example, the teeth of the bucket are worn and the mesh needs adjustment.
[0093] The preferred process consists of three key components.
- 1. Kinematic calibration algorithm. This algorithm results in kinematic limb length, joint axis, and location in the sensor's reference frame, as illustrated in FIG. 5.
- 2. Bucket parameter extraction. This step precisely identifies bucket-specific parameters such as length, width, thickness, number, and thickness of teeth directly from the point cloud, see FIG. 9.
- 3. Mesh generation method. This step constructs a detailed and accurate mesh of the limbs from the accumulated point cloud, including the extracted bucket parameters, as illustrated in FIG. 10.
[0094] A simplified diagram is shown in FIG. 4, which illustrates the processing of point clouds acquired sequentially. The figure illustrates the sequential acquisition of point clouds for different joint configurations (top). These point clouds are then aligned to determine joint axes and locations. This makes it possible to generate accurate mesh models (notably for the bucket). Eventually, a robot description is achieved (symbolized by the depiction at the bottom of the diagram), which represents the excavator model. Examples of robot description formats include Unified Robot Description Format (URDF), Simulation Description Format (SDF), Collada, and MuJoCo XML.
[0095] A complementary process concerns an IMU calibration method, which re-uses the same dataset as already recorded for the kinematic calibration process. IMUs 130 are strategically mounted on each limb to compute the arm's state, necessitating precise determination of each IMU's pose relative to its respective limb's coordinate frame.
[0097] Data acquisition. The data acquisition is sequentially performed for each joint, beginning with the boom joint 113, then the dipper joint 112, and the bucket joint 111, concluding with the turn (slew) joint 114. To ensure kinematic coherence, all child limbs must move together without relative motion. For each joint, multiple point clouds and corresponding images are captured, using several sensors. This process involves accumulating point clouds for a few seconds while the joint remains stationary, followed by incremental rotation by a specified degree. The point clouds are fused, as explained in the previous section. This stepwise approach is repeated until the entire operational workspace of the joint is covered or the joint is outside of the sensor's field of view, resulting in a comprehensive set of point clouds and images for each joint.
[0098] Data preprocessing (step S20). Point clouds are processed to exclude non-machine points through filtering, followed by denoising using statistical outlier removal, radius-based outlier removal, and intensity-based filtering techniques.
[0099] Limb segmentation (step S30). The point clouds are processed to identify distinct segments representing the boom, dipper, and bucket, through stationary point filtering. This involves analyzing static points across multiple joint poses. Points that maintain consistent spatial positions across these poses are identified as stationary and subsequently excluded from the dataset.
[0100] Joint parameters identification (steps S40 - S60). The identification of joint parameters represents the core of the pipeline shown in FIG. 6, which customizes a conventional ICP algorithm as follows.
- 1. ICP with center of rotation extraction (step S42). This step deviates from the standard ICP algorithm by prioritizing precise identification of joint parameters over simple point cloud alignment. This results in determining the rotation axis and the center of rotation for each limb.
- 2. Filtering and correspondence set refinement (step S42). Estimates are aggregated and processed to exclude outliers, ensuring the selection of only high-quality correspondences. Points are retained only if they are sufficiently close, and their normal vectors are aligned in the same direction.
- 3. Joint optimization (S44): Utilizing the refined correspondences from the previous step (S42), a least-squares optimization algorithm minimizes the point-to-plane error between the source and target point clouds across multiple point cloud pairs. The pipeline operates iteratively, stopping only when the changes in parameter estimates between successive iterations fall below a predefined threshold (S46: Yes).
[0101] The above process outputs the rotation axis and its spatial location relative to the sensor frame.
[0102] However, one degree of freedom remains undetermined: the position of the rotation center along the axis of rotation. To resolve this, the rotation center is assumed to coincide with the midpoint of the limb along the identified rotation axis. This midpoint is estimated by projecting the bucket's point cloud onto the rotation axis and averaging the projected points, as illustrated in FIG. 8A.
[0103] For the calibration of the turn joint (or slew j oint), the pipeline undergoes a slight modification. Instead of processing the machine's point cloud, the algorithm operates on the point cloud of the surrounding environment while the upper structure 14 (with the cabin 104) rotates. This is equivalent to the environment rotating around the upper structure.
[0104] Mesh Generation for the Bucket (FIGS. 9 and 10). The same point cloud of the bucket after the data preprocessing step is used. Additional processing is involved, such as classifying the points into inner, outer layers, and into body and teeth (FIG. 9). Then, a number of parameters such as end-effector contact body ("EE Body"), end-effector teeth ("EE Teeth"), number of teeth, and tooth thickness and width are directly computed on the point cloud, as seen in FIG. 9. A general parametric mesh model of an excavator bucket consisting of 25 different parameters is built. Using a genetic algorithm and the extracted parameters, the point cloud is fitted to the parametric mesh. The result is shown in FIG. 10.
[0105] In comparison, the current solutions on the market do not involve system that perform kinematic calibration using onboard sensors. Kinematic calibration is either entirely omitted or relies on expensive equipment such as total stations, which is time-intensive and requires human expertise in the field. Additionally, the current solutions do not use the excavator's own limbs as calibration features (they instead rely on external calibration targets like prisms or checkerboards), nor do they generate a mesh using the onboard sensor's point cloud fitted to a predefined parametric mesh.
2.2 Autonomous operation
[0106] Once calibrated as described above, the machine 1 can be autonomously operated S90. The control model used to autonomously operate S90 the machine 1 may be trained using model-based, model-free, or offline reinforcement learning. Practically speaking, this control model may for instance be trained with an actor-critic reinforcement algorithm, i.e., using an actor network that interacts with a critic network for training purposes. However, only the actor network is used at runtime, for inference purposes. The actor network is used to infer a desired action. It may for instance be configured as a multilayer perceptron (MLP) network. The critic network is used for estimating a value function. It may also include an MLP network. The control model is trained by jointly training the actor network and the critic network, for the actor network to learn to generate suitable control data at each algorithmic cycle at runtime. An actor-critic architecture has advantages in terms of sample efficiency, convergence, stability, and flexibility. Other reinforcement learning architectures can be contemplated.
[0107] The control model is preferably trained with a proximal policy optimization (PPO) algorithm. PPO algorithms are known per se. If necessary, conditions can be used for truncation of episodes, e.g., when a timer has run out or undesired states are reached.
2.3. Description of systems according to embodiments
[0108] Figure 3 is a block diagram illustrating the perception sensor system 120, its components, and processing means 205 - 215 of the computerized system 200. The diagram shows how data flows from the perception sensors 121 - 125 to the processing means 210, where they are analyzed to calibrate the machine's joints and limbs and obtain corresponding data 230. Such data can then be re-used by the processing means, for other operations as described herein, including the autonomous operation of the machine 1.
[0109] The perception sensor system 120 is responsible for acquiring data, starting with point clouds 152. The system 120 includes several Lidars 121 - 124, used for capturing point clouds 152. The Lidars are positioned at selected locations on the roof of the cabin, see FIG. 2, for comprehensive coverage. The system also includes cameras 125, e.g., as part of the sensors heads 124 (see FIG. 2), for capturing images 154, complementing the Lidar data. All such data are timely transmitted to the processing means 210.
[0110] Inertial measurement units (IMUs) 130 provide data (captured by corresponding signals 156) on the orientation and movement of the various components 101 - 104 of the machine 1, typically an excavator as shown in FIG. 1. The IMU signals 156 are transmitted from the IMUs 130 to the processing means 210.
[0111] The processing means 210 include processors and memory circuit, preferably in a redundant (or partly redundant) configuration. The redundancy may leverage a heterogeneous perception, whereby perception from, e.g., depth cameras, is used to (in)validate perception from the Lidars.
2.4. Detailed flow
[0112] FIG. 5 is a flowchart detailing steps of a preferred calibration method, providing a comprehensive overview of a calibration process, from data acquisition to autonomous operation of the excavator. The flow includes the following steps.
[0113] General step S10 (data acquisition) concerns the sequential acquisition of point clouds using the perception sensor system. Step S12 selects the next j oint for incremental rotation. Step S14 performs an incremental joint rotation (while all other joints remain static). At step S15, point clouds are obtained and IMU data are recorded, while all joints remain static, for the current joint configuration. Once all operations for the current joint are completed, the process goes back to step S12 to select the next joint. Once all joints have been processed (Step S16: Yes) and calibrated, the process moves on to step S20.
[0114] Data preprocessing is performed at step S20, to preparing the acquired data for further analysis. The Limb segmentation process is performed at step S30, whereby limbs are segmented from the point clouds.
[0115] General step S40 denotes the alignment procedure, which is based on the ICP algorithm. At step S42, the ICP algorithm is used to determine rotation axes and rotation centers. At step S44, the refinement procedure (with filtering and joint optimization) is started to refine estimates obtained from the ICP algorithm. Steps S42 and S44 are iteratively performed (step S46: No), until a convergence is achieved (step S46: Yes).
[0116] In that case, the process moves on to step S50, which projects and averages points from accumulated point clouds to determine the rotation centers of the limbs. Step S60 determines the bucket dimensions and the tip position.
[0117] Meshes are calculated at step S70. The latter decomposes into step S72, which classifies points of the aligned point clouds, based on predefined parts of the limbs, and step S74, which fits point clouds to the parametric meshes (i.e., mesh models), using a genetic algorithm.
[0118] IMUs are calibrated at step S80, thanks to data recorded at steps S14 and S15. Once the IMUs are calibrated, a control model is tuned in accordance with parameters and data obtained from the calibration procedure, after which the underlying computerized system can operate S90 the excavator autonomously.
3. Technical implementation details
[0119] Computerized systems can be suitably designed for implementing embodiments of the present invention as described herein. In that respect, it can be appreciated that the methods described herein are non-interactive, i.e., automated, although human inputs may be required in certain cases, e.g., should an anomaly or emergency be detected or during a deployment phase of the excavator. Automated parts of such methods can be implemented in software, hardware, or a combination thereof. In exemplary embodiments, automated parts of the methods described herein are implemented in software, as a service or an executable program (e.g., an application), the latter executed by suitable digital processing devices.
[0120] In the present context, each processing system 210 is preferably mapped onto one or more respective sets of processors or, even, one or more respective computers. In particular, the computerized system may typically involve several processors or computers. A suitable computer will typically include at least one processor and a memory (possibly several memory units) coupled to one or memory controllers. Each processor is a hardware device for executing software. The processor, which may in fact comprise one or more processing units (e.g., processor cores), can be any custom made or commercially available processor, possibly subject to some certification.
[0121] The memory typically includes a combination of volatile memory elements (e.g., random access memory) and non-volatile memory elements, e.g., a solid-state device. The software in memory may include one or more separate programs, each of which comprises an ordered listing of executable instructions for implementing logical functions. The software in the memory captures methods described herein in accordance with exemplary embodiments, as well as a suitable operating system (OS). The OS essentially controls the execution of other computer (application) programs and provides scheduling, input-output control, file and data management, memory management, and communication control and related services. It may further control the distribution of tasks to be performed by the processors. The methods described herein shall typically be in the form of executable programs, scripts, or, more generally, any form of executable instructions.
[0122] In exemplary embodiments, each computer further includes an interface, e.g., including a network interface or a transceiver for coupling to a network (not shown). In addition, each computer will typically include one or more input and/or output devices (or peripherals) that are communicatively coupled via a local input/output controller. A system bus interfaces all components. Further, the local interface may include address, control, and/or data connections to enable appropriate communications among the aforementioned components. The I/O controller may have additional elements, which are omitted for simplicity, such as controllers, buffers (caches), drivers, repeaters, and receivers, to allow data communication.
[0123] When a computer is in operation, one or more processing units executes software stored within the memory of the computer to communicate data to and from the memory and/or the storage unit (e.g., a solid-state memory), and to generally control operations pursuant to software instruction. The methods described herein and the OS, in whole or in part are read by the processing elements, typically buffered therein, and then executed. When the methods described herein are implemented in software, the methods can be stored on any computer-readable medium for use by or in connection with any computer-related system or method.
[0124] Computer-readable program instructions described herein can be downloaded to processing elements from a computer-readable storage medium, via a network, for example, the Internet and/or a wireless network. A network adapter card or network interface may receive computer-readable program instructions from the network and forward such instructions for storage in a computer-readable storage medium interfaced with the processing means. All computers and processors involved can be synchronized using any suitable protocol or thanks to timeout messages.
[0125] Aspects of the present invention are described herein notably with reference to a flowchart and a block diagram. It will be understood that each block, or combinations of blocks, of the flowchart and the block diagram can be implemented by computer-readable program instructions.
[0126] These computer-readable program instructions may be provided to one or more processing elements as described above, to produce a machine, such that the instructions, which execute via the one or more processing elements create means for implementing the functions or acts specified in the block or blocks of the flowchart and the block diagram. These computer-readable program instructions may also be stored in a computer-readable storage medium.
[0127] The flowchart and the block diagram in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of the computerized systems, methods of operating it, and computer program products according to various embodiments of the present invention. Note that each computer-implemented block in the flowchart or the block diagram may represent a module, or a portion of instructions, which comprises executable instructions for implementing the functions or acts specified therein. In variants, the functions or acts mentioned in the blocks may occur out of the order specified in the figures. For example, two blocks shown in succession may actually be executed in parallel, concurrently, or still in reverse order, depending on the functions involved and the algorithm optimization retained. It is also reminded that each block and combinations thereof can be adequately distributed among special purpose hardware components.
[0128] While the present invention has been described with reference to a limited number of embodiments, variants, and the accompanying drawings, it will be understood by those skilled in the art that various changes may be made, and equivalents may be substituted without departing from the scope of the present invention. In particular, a feature (device-like or method-like) recited in a given embodiment, variant, or shown in a drawing, may be combined with or replace another feature in another embodiment, variant, or drawing, without departing from the scope of the present invention. Various combinations of the features described in respect of any of the above embodiments or variants may accordingly be contemplated, that remain within the scope of the appended claims. In addition, many minor modifications may be made to adapt a particular situation or material to the teachings of the present invention without departing from its scope. Therefore, it is intended that the present invention is not limited to the particular embodiments disclosed, but that the present invention will include all embodiments falling within the scope of the appended claims. In addition, many other variants than explicitly touched on above can be contemplated.
[0129] For example, several architecture variants may be contemplated for the computerized system, which involves one or more distinct computers. In addition, various other machine learning techniques and computational models may possibly be relied on. And besides MLP architectures as evoked above, the control model may involve other types of networks, such as recurrent networks or attention-based networks (e.g., transformers).
REFERENCE LIST
[0130]
- 1
- Heavy equipment machine (excavator)
- 12
- Lower structure
- 14
- Upper structure
- 101
- End effector (bucket)
- 102
- Stick
- 103
- Boom
- 104
- Cabin
- 111
- First joint (linking end effector to stick)
- 112
- Second joint (linking stick to boom)
- 113
- Third joint (linking boom to upper structure)
- 114
- Turn j oint (or slew j oint, connecting upper structure to lower structure)
- 120
- Perception sensor system
- 121 - 125
- Perception sensors
- 121, 122
- Lidars
- 124
- Sensor head
- 125
- Camera
- 130
- IMUs
- 152
- Point clouds
- 154
- Images
- 156
- IMU signals
- 200
- Computerized system
- 205, 215
- Interface units
- 210
- Processing means
- 220
- Control unit (motion controller of actuation system)
- 230
- Limb geometry and other calibrated parameters
1. A computer-implemented method of calibrating a heavy equipment machine (1) including
a lower structure (12), an upper structure (14) linked to the lower structure, and
limbs (101 - 103) that are successively linked through joints (111 - 113), from the
upper structure (14), the limbs including a boom (103), a stick (102), and an end
effector (101), wherein the method comprises:
sequentially acquiring (S10), using a perception sensor system (120), multiple point clouds (152) of the limbs, the multiple point clouds (152) corresponding to multiple respective joint configurations of each joint of said joints (111 - 113), wherein
sequentially acquiring (S10), using a perception sensor system (120), multiple point clouds (152) of the limbs, the multiple point clouds (152) corresponding to multiple respective joint configurations of each joint of said joints (111 - 113), wherein
the multiple respective joint configurations are obtained for said each joint, one joint at a time, by incrementally rotating (S14) said each joint while keeping all other joints of said joints static, and
each of the multiple point clouds (152) is acquired (S15) for a respective one of the joint configurations while keeping all of said joints (111 - 113) and the upper structure static; and
determining (S20 - S40) rotation characteristics of the joints (111 - 113) based on the multiple point clouds (152) acquired for each of the joints.2. The method according to claim 1, wherein
the perception sensor system (120) includes one or more perception sensors that are on board the heavy equipment machine, the one or more perception sensors preferably including at least one Lidar (120),
preferably, at least one of the one or more perception sensors is arranged on top of the upper structure (14), and,
more preferably, the perception sensor system (120) includes at least two perception sensors, including one perception sensor (121) arranged on top of the upper structure (14) and one perception sensor (123) arranged on one of the limbs, for example at an end of the boom (103).
3. The method according to claim 1 or 2, wherein
said joints (111 - 113) include:
a first joint (111) linking the end effector (101) to the stick (102);
a second joint (112) linking the stick (102) to the boom (103); and
a third joint (113) linking the boom (103) to the upper structure (14), and
the multiple point clouds (152) are sequentially acquired (S10) for said each joint, one joint at a time, starting with the third joint (113), then moving on to the second joint (112), and then to the first joint (11).
4. The method according to any one of claims 1 to 3, wherein
the multiple respective joint configurations are obtained by incrementally rotating (S14) said each joint to cover a majority, preferably an entirety, of a workspace of said each joint, and one joint after the other, so as to cover a majority, preferably an entirety, of a workspace of the limbs (101 - 103).
the multiple respective joint configurations are obtained by incrementally rotating (S14) said each joint to cover a majority, preferably an entirety, of a workspace of said each joint, and one joint after the other, so as to cover a majority, preferably an entirety, of a workspace of the limbs (101 - 103).
5. The method according to any one of claims 1 to 4, wherein
determining (S20 - S40) the rotation characteristics further comprises segmenting (S30) limbs from the point clouds acquired, preferably by identifying stationary points in the multiple point clouds (152) acquired, to obtain segmented limbs, and
said rotation characteristics are determined (S20 - S40) by executing (S40) an alignment procedure based on an Iterative Closest Point (ICP) algorithm, whereby
executing (S40) the alignment procedure causes to perform (S42) a point cloud alignment to align the segmented limbs by adjusting geometric transformations, and
said rotation characteristics are determined from the adjusted geometric transformations.
6. The method according to claim 5, wherein
executing (S40) said alignment procedure comprises iteratively running the ICP algorithm (S42) and a refinement procedure (S44) to obtain estimates of the rotation characteristics of the joints (111 - 113) from the ICP algorithm and then refine (S44) the estimates obtained through the refinement procedure, whereby the refined estimates are fed as input to the ICP algorithm to obtain further estimates of the rotation characteristics,
the refinement procedure is preferably devised so as to ensure consistency of the rotation characteristics of the joints across all of the multiple point clouds acquired for said each joint, the refinement procedure being more preferably devised to ensure that the iteratively refined estimates of the rotation characteristics adhere to geometric transformations that are mechanically consistent with mechanical properties of the joint.
7. The method according to claim 5 or 6, wherein the method further comprises
determining (S50), for each given joint of one or more of the joints (111 - 113), a position of a rotation center along a corresponding rotation axis, as determined for said each given joint, by projecting all of the multiple point clouds as acquired for said each given joint onto the corresponding rotation axis to obtain projected points and then averaging the projected points.
determining (S50), for each given joint of one or more of the joints (111 - 113), a position of a rotation center along a corresponding rotation axis, as determined for said each given joint, by projecting all of the multiple point clouds as acquired for said each given joint onto the corresponding rotation axis to obtain projected points and then averaging the projected points.
8. The method according to claim 7, wherein the method further comprises
projecting (S70) an aligned point cloud corresponding to the end effector (101) onto a plane perpendicular to a rotation axis of the joint of the end effector (101), to identify a farthest point from a rotation center thereof and assigning the identified point to a tip of the end effector (101).
projecting (S70) an aligned point cloud corresponding to the end effector (101) onto a plane perpendicular to a rotation axis of the joint of the end effector (101), to identify a farthest point from a rotation center thereof and assigning the identified point to a tip of the end effector (101).
9. The method according to any one of claims 1 to 8, wherein the method further comprises
generating (S70) a mesh for one or more of the limbs, based on the multiple point clouds (152) acquired.
generating (S70) a mesh for one or more of the limbs, based on the multiple point clouds (152) acquired.
10. The method according to claim 9, wherein generating the mesh comprises
classifying (S72) points of the point clouds in accordance with predefined parts of the one or more limbs, and accordingly determined parameters from the point clouds, and
fitting (S74) the point clouds to a parametric mesh using a genetic algorithm and the determined parameters.
11. The method according to any one of claims 1 to 10, wherein the method further comprises calibrating (S80) inertial measurement units
arranged on the limbs by:
recording data corresponding to signals obtained from the inertial measurement units, while incrementally rotating (S14) said each joint; and
determining orientations of the inertial measurement units relative to respective ones of the joints corresponding to said limbs, based on the recorded data.
12. A computerized system (200) for calibrating a heavy equipment machine (1) including
a lower structure (12), an upper structure (14) linked to the lower structure, and
limbs (101 - 103) that are successively linked through joints (111 - 113), from the
upper structure (14), the limbs including a boom (103), a stick (102), and an end
effector (101), wherein
the computerized system comprises:
processing means (210); and
an interface (205, 215), adapted to interface the processing means (210) with each of
a control unit (220) configured to rotate the joints, and
a perception sensor system (120) comprising one or more perception sensors, and
the processing means (210) are configured to communicate with each of the control unit and the perception sensor system through the interface to perform a method according to any one of claims 1 to 11.
13. A system (120, 200) comprising:
a computerized system (200) according to claim 12, and
the perception sensor system (120), wherein
the one or more perception sensors (121 - 125) are adapted to be mounted on top of
the upper structure (14) of the heavy equipment machine (1).
14. The system (120, 200) according to claim 13, wherein the one or more perception sensors (121 - 125) comprises:
a sensor rack (121), which includes a Lidar, a camera, and an inertial measurement unit (IMU), the sensor rack (121) adapted to be mounted on top and at the front of the upper structure (14); and
two sensor heads (124), wherein each of the two sensor heads (124) includes a GNSS antenna and an IMU, as well as one or more, preferably each, of
at least one camera,
a Lidar, and
a WIFI antenna.
15. Computer program product for calibrating a heavy equipment machine (1) including a
lower structure (12), an upper structure (14) linked to the lower structure, and limbs
(101 - 103) that are successively linked through joints (111 - 113), from the upper
structure (14), the limbs including a boom (103), a stick (102), and an end effector
(101), the computer program product comprising a computer readable storage medium
having program instructions embodied therewith, the program instructions executable
by processing means to cause the latter to perform a method according to any one of
claims 1 to 11.