Method for estimating the pose of a boom of a working machine, and working machine

Abstract

The invention relates to a method for estimating the pose of a boom of a working machine (2), which boom comprises a plurality of links (20) connected in pairs by joints (24) such that a kinematic chain is formed, wherein the working machine (2) comprises a carrier element (4) and a superstructure (6), wherein a first of the links (20) is mounted on the superstructure (6) by way of one of the joints (24), wherein the superstructure (6) is mounted on the carrier element (4) so as to be rotatable about a first axis (34) and at least one of the joints (24) is rotatable about a second axis which is not parallel to the first axis (34), wherein the rotation of the superstructure (6) is effected by an electric drive (8), wherein an inertial sensor (26) is attached to each link (20), said inertial sensor measuring linear accelerations and / or rotation rates in order to obtain inertial sensor data; wherein the method comprises: detecting (110) or determining inertial sensor data; determining (120) an angular velocity and / or an angular acceleration of the rotation of the superstructure (6) about the first axis (34) in order to obtain rotation data; and determining (130) a pose of the boom by means of an estimation method based on a state model of the kinematic chain, in which estimation method the inertial sensor data and the rotation data are used, wherein the pose indicates, for at least one of the links (20), a position and / or orientation relative to a reference system of the carrier element (6) or of the structure (4).

Description

[0001] R.415633 DE

[0002] Method for estimating the pose of a boom of a working machine and working machine

[0003]

[0004] The present invention relates to a method for estimating the pose of a boom of a working machine, a working machine, a computing unit and a computer program for carrying out the method.

[0005] Background of the invention

[0006] Working machines, especially mobile working machines, can have a boom consisting of several rotatably connected sections. A first section of the boom is, for example, rotatably mounted on a rotating superstructure or the machine's frame. A final section at a free end of the boom is formed by a working attachment, such as a bucket, shovel, fork, or similar device for handling loads. The rotational movement of the sections relative to each other and to the superstructure is achieved, for example, by hydraulic cylinders. For automation functions, methods can be used to determine or estimate the position and / or orientation of the working attachment.

[0007] Disclosure of the invention

[0008] According to the invention, a method for estimating the pose of a boom of a machine, a machine, a computing unit, and a computer program for carrying out the method, with the features of the independent claims, are proposed. Advantageous embodiments are the subject of the dependent claims and the following description.

[0009] The invention utilizes the measure of acquiring or determining inertial sensor data in a method for estimating the pose of a boom of a working machine, which comprises several links connected in pairs by joints, each of which has an inertial sensor attached that measures linear accelerations and / or rotation rates.

[0010] Page 1 of 24 R.415633 DE

[0011] The aim is to determine the angular velocity and / or angular acceleration of a rotation of a working machine component relative to a support element of the working machine in order to obtain rotational data, and to determine the pose of the boom using an estimation method based on a state model of the kinematic chain, in which the inertial sensor data and the rotational data are used. By considering the rotational data, i.e., the angular velocity and / or acceleration of the rotation of the first link relative to the support element, the accuracy of the pose estimation is increased compared to a pose estimation based solely on the inertial sensor data.This is particularly due to the fact that, on the one hand, additional data is used, and on the other hand, the rotational data can have a higher accuracy than the measurement data of the inertial sensors on the links, which can be very noisy, for example, due to vibrations of the boom during work processes.

[0012] The pose specifies a position and / or orientation for at least one of the elements relative to a reference system (or coordinate system) of the supporting element or the structure. The term "pose" is specifically intended to denote one or more state variables by which the position and / or orientation of one or more (especially all) of the elements relative to the structure and / or relative to the supporting element is described or characterized (e.g., in the coordinate system of the structure or in a coordinate system of the supporting element). The position specified in the pose can also refer to a specific point on the respective element.

[0013] The state model of the kinematic chain is based in particular on the forward kinematics of the kinematic chain. The forward kinematics are assumed to be known, meaning that the structure of the working machine itself is known (e.g., the dimensions of the links, distances between the joints, maximum rotation angles of the joints, and similar parameters are known).

[0014] The joints are specifically pivot joints (i.e., joints that connect two links so that they can rotate around an axis). In general, the method can also be applied if shear joints are at least partially used.

[0015] The structure is, for example, the superstructure of a mobile work machine. The support element can be movable, e.g., the undercarriage of a mobile work machine or a support element moving on rails, or it can be stationary. The second axle is, for example,

[0016] Page 2 of 24 R.415633 DE

[0017] The joints are defined in relation to the structure, e.g., in relation to a coordinate system fixed with respect to the structure. The joints are, in particular, reciprocating joints whose axes of rotation are parallel to each other, i.e., oriented along the second axis. The second axis is, in particular, orthogonal to the first axis. Overall, e.g., in a typical mobile machine, such as an excavator, the first axis runs in the direction of the z-axis, the second axis in the direction of the y-axis, and the boom is oriented in the direction of the x-axis within the coordinate system of the structure (with appropriately chosen x-, y-, and z-axes). The term "axis" is generally intended to denote an abstract or imaginary axis (or imaginary straight line) around which the rotation of elements (on circular or circular segment paths) takes place. It may, but does not necessarily, refer to a corresponding actual or physical axis (e.g.,A wave) is provided through which the respective abstract axis is realized, and which is, for example, mounted in a rotary bearing to enable the rotation of the respective element.

[0018] In a typical operating situation, the first axis of the machine runs perpendicular to the ground surface on which the machine stands, meaning it is parallel to the direction of gravity or at an angle to it, the angle being determined by the inclination of the ground surface. The second axis, in a typical operating situation, does not run parallel to the direction of gravity (and is, for example, parallel to the ground surface), so that, at least when the joints are stationary, the linear acceleration measured by the inertial sensors indicates the direction of gravity. From this, the angles of the inertial sensors, and thus of the links, relative to the direction of gravity and, consequently, relative to each other, can be determined. With known forward kinematics (e.g., encoded in a state model), the pose of the boom can then be determined.This involves, in particular, estimation methods based on a state model of the kinematic chain, such as Caiman filters or extended Caiman filters, or similar iterative filtering methods, or estimation methods in which a functional is optimized or minimized. The state model includes terms or expressions that relate state variables of different links to each other; that is, they encode the fact that the state variables of different links are not independent of each other, since the links in the kinematic chain are connected via joints.

[0019] The estimation method determines corresponding state variables of the state model, which in particular characterize the pose of the boom. These are observed.

[0020] Page 3 of 24 R.415633 DE

[0021] In addition to the inertial sensor data, rotational data will also be considered. The state model includes, in particular, terms or expressions that relate the inertial sensor data to the rotational data; that is, equations that depend on both. In simplified terms, an inertial sensor, which, depending on its position, is located at a distance r from the axis around which the setup rotates, moves at a velocity v. t = ωr in the tangential direction (circumferential direction), where a > is the angular velocity of the setup. This results in accelerations acting on the inertial sensor in the radial direction (v̇). r = ω 2 r, where v r the velocity of the inertial sensor in the radial direction) and in the tangential direction (v̇ t= ω̇r + ωṙ), which influence the measured values ​​for the linear accelerations of the inertial sensor (a dot above a quantity, as usual, denotes its derivative with respect to current). Intuitively expressed, there are therefore consistency conditions (v̇ r − ω 2 r = 0, v̇ t − ω̇r − ωṙ = 0), which directly or indirectly (via the state model) are incorporated into the estimation of the state variables, so that a higher accuracy of the pose estimation can be achieved, even if the rotation data (rotation about the first axis) does not directly relate to a statement about the pose (e.g. rotation about the second axis).

[0022] Angular velocity, angular acceleration, and similar quantities, as well as vectors, are generally vectors (or matrices) in three-dimensional space, i.e., characterized by specifying several values ​​(three values), although in some cases the specification of fewer than three values ​​may be sufficient to characterize the corresponding rotation, e.g., when rotating the setup around the first axis.

[0023] According to one embodiment, an inertial sensor is mounted on the structure, whereby the rotational data are determined from linear accelerations and / or rotational rates measured by the inertial sensor, or are determined taking into account the linear accelerations and / or rotational rates measured by the inertial sensor. This embodiment requires relatively little computational effort to determine the rotational data.

[0024] According to one embodiment, the rotational data are determined from the drive data of the electric drive or determined taking the drive data of the electric drive into account. The drive data consists of measurements from inertial sensors, which, for example,

[0025] Page 4 of 24 R.415633 DE

[0026] They may exhibit systematic errors (offset), independently, so that accuracy can be increased.

[0027] According to one embodiment, the drive data includes a rotor angular velocity and / or a rotor angular acceleration of an electric drive rotor. Specifically, values ​​for the angular velocity and / or angular acceleration of the rotating structure are determined by multiplying the rotor angular velocity and / or rotor angular acceleration, respectively, by a gear ratio between the rotation of the rotor and the rotation of the structure. Using rotor angular velocity and / or rotor angular acceleration to determine the rotation data is advantageous because a high gear ratio (e.g., greater than 50, greater than 100, or greater than 200) exists between an output shaft of the electric drive and the rotation of the structure, so that an inaccuracy in the rotation angle of the output shaft (e.g., expressed in degrees) corresponds to a much smaller inaccuracy in the rotation angle of the structure.As a result, the angular velocity and / or angular acceleration of the rotation of the setup around the first axis, i.e. the rotational data, can be determined very accurately.

[0028] According to one embodiment, the rotor angular velocity and / or rotor angular acceleration are determined from angular position data acquired by an angular position measuring device, in particular a resolver, of the electric drive. This represents a simple and computationally efficient method for determining the rotor angular velocity and / or rotor angular acceleration, as the angular position data is typically already available in the electric drive's controller. The angular position data includes, in particular, the rotational angle of the rotor relative to the stator of the electric drive. Additionally or alternatively, the rotational angle of the structure relative to the support element about the first axis can also be determined from the angular position data (taking the gear ratio into account). This angle can be used, for example, to determine the pose of the boom or...to determine the orientation of at least one member relative to the supporting element.

[0029] According to one embodiment, the rotor angular velocity and / or the rotor angular acceleration are determined by means of a second-order filter, which in particular includes two

[0030] Page 5 of 24 R.415633 DE

[0031] The series of used PT1 filters is determined from the angular position data. This, as well as subsequent modifications, avoids the explicit calculation of time derivatives of the angular position measuring device's signal. Any resulting increase in noise, particularly from the second derivative, can thus be avoided or reduced.

[0032] According to one embodiment, a drive state-space model is used for the electric drive, which takes into account the moment of inertia of the superstructure, the boom, and in particular a load moment, in order to determine the rotor angular velocity and / or the rotor angular acceleration. The drive state-space model considers the dynamics of the system, thus increasing the accuracy of pose determination, especially in dynamic situations.

[0033] According to one embodiment, the drive state-space model is used in an observer, wherein the observed quantities in the drive state-space model are electric current intensities and an angular velocity for the rotor, which is determined from angular position data of an angular position measuring device, in particular a resolver, of the electric drive, and wherein the rotor angular acceleration is an unobserved quantity determined by the observer. In particular, the rotor angular velocity is an unobserved quantity determined by the observer, or the rotor angular velocity is determined to be equal to the angular velocity for the rotor determined from angular position data. For example, a Luenberger observer, an extended Kalman filter, a high-gain observer, or similar device can be used as the observer.

[0034] According to one embodiment of the estimation method, corrected values ​​for the linear accelerations of the inertial sensors are used. Linear acceleration corrections for the inertial sensors are determined from the rotational data, and the measured values ​​for the linear accelerations or the inertial sensor data are modified by these corrections to arrive at the corrected values. To determine the linear acceleration corrections, transformations between the reference frame of the support element or the structure and the respective reference frames of the inertial sensors can be used. For example, an iterative estimation method can be employed here.

[0035] Page 6 of 24 R.415633 DE

[0036] A computing unit according to the invention, e.g. a control unit of a mobile working machine, is, in particular in terms of programming, equipped to carry out a method according to the invention.

[0037] Implementing a method according to the invention in the form of a computer program or computer program product with program code for carrying out all method steps is also advantageous, as this incurs particularly low costs, especially if an executing control unit is already used for other tasks and is therefore already available. Suitable data carriers for providing the computer program are, in particular, magnetic, optical, and electrical storage media, such as hard drives, flash memory, EEPROMs, DVDs, etc. Downloading a program via computer networks (Internet, intranet, etc.) is also possible.

[0038] Further advantages and embodiments of the invention will become apparent from the description and the accompanying drawing.

[0039] It is understood that the features mentioned above and those to be explained below can be used not only in the combinations specified, but also in other combinations or on their own, without leaving the scope of the present invention.

[0040] The invention is schematically illustrated in the drawing using exemplary embodiments and is described in detail below with reference to the drawing.

[0041] Character description

[0042] Figures 1A and 1B show the structural design of an excavator.

[0043] Figure 2 illustrates an element or segment of a boom with an attached inertial sensor and the forces acting upon it.

[0044] Figure 3 illustrates an element or segment of a boom with an attached inertial sensor, as well as vectors to describe the position and orientation of the boom element and the inertial sensor.

[0045] Page 7 of 24 R.415633 DE

[0046] Figure 4 shows a flowchart of a method for estimating the pose of a boom of a working machine according to an embodiment of the invention.

[0047] Figure 5 shows an example of a possible observation structure that can be used in determining rotation data.

[0048] Detailed description of the drawing

[0049] Figures 1A and 1B show the structural design of an excavator, which serves as an example of a construction machine. Figure 1A shows a side view and Figure 1B a top view.

[0050] The excavator 2 has a support structure or support element 4 (chassis or undercarriage) and a superstructure 6 (upper carriage) mounted on it so as to be rotatable about a slewing axis 14. For an excavator, the support element 4 is arranged on wheels (as in Figure 1A) or tracks. In other construction machines, the support element can also be stationary, e.g., in a stationary material handler. The term "slewing axis" is intended to denote an axis about which the superstructure can rotate, i.e., in the sense of an imaginary or abstract axis (similar to a coordinate system). Accordingly, no corresponding physical component (shaft, etc.) needs to be present (although it is not excluded that such a component is present). For example, the superstructure could be mounted on the support element by means of a bearing ring.Figure 1B shows a corresponding rotation angle 16, with the rotated orientation of the superstructure, including the boom, shown as a dashed line in the top view of Figure 1B. The rotation of the superstructure 6 relative to the support element 4 is effected by an electric drive 8 arranged on the superstructure 6, which drives a gear or pinion 10 that meshes with a toothed ring 12 arranged on the support element (i.e., their teeth mesh with each other). The pinion 10 is, for example, arranged directly on an output shaft of the electric drive 8, but can generally also be coupled to it via a gearbox, which in particular has a gear ratio other than 1. Overall, the gear ratio between the revolutions of the electric drive 8 and the revolutions of the superstructure 6 relative to the support element 4 can be, for example, greater than 50 or greater than 100, depending on the number of teeth on the toothed ring 12 and the pinion 10, and, if applicable, the gear ratio.

[0051] Page 8 of 24 R.415633 DE

[0052] or greater than 200. The angular velocity of the assembly 6 is correspondingly smaller than the angular velocity of the electric drive 8. The electric drive, optionally the gearbox, the pinion, the ring gear, the optional slewing shaft, and any other components that enable the rotation of the assembly (e.g., bearings, bearing rings), can be collectively referred to as the slewing mechanism.

[0053] A boom with several rigid boom elements or links 20 is attached to the superstructure 6. The links 20 are connected to each other in pairs by pivot joints 22. A first link 20 is also connected to the superstructure 6 by a pivot joint 24. Rotation about the pivot joints 22 and 24 is effected by hydraulic cylinders. The boom and its links 20 form a kinematic chain, with the last link 20, located at a free end of the kinematic chain, serving to support loads and, in the figure, being designed as a bucket or, more generally, as a work attachment. The superstructure 6 can also be considered part of the kinematic chain.

[0054] The support element 4, the assembly 6, and the links 20 can each be assigned their own coordinate systems (or reference systems). For example, a coordinate system for the assembly 6, or assembly coordinate system, is shown, which has an x-axis 30, a y-axis 32, and a z-axis 34. The coordinate system of the support element 4, or support element coordinate system, can also be considered a world coordinate system. The origin of each coordinate system can, for example, be located at the pivot point, or an axis of the respective pivot point, about which the element (assembly or link) is rotatable. For example, the assembly coordinate system and the link coordinate systems can be chosen according to the Denavit-Hartenberg convention (which is known to those skilled in the art in the field of kinematic chains).The coordinate systems can be transformed into one another by means of respective coordinate transformations (which include a translation and a rotation). The pose of the boom generally refers to information that at least partially characterizes its position and / or orientation in space. In particular, a pose is given by specifying a position and / or orientation of one or more of the links 20 in relation to the structure 6 or to the support element 4. This can be given, for example, by specifying a coordinate transformation that converts the coordinate system of the respective link or a point within it to the structure's coordinate system or the...

[0055] Page 9 of 24 R.415633 DE

[0056] The support element coordinate system connects. The one or more links 20, of which the position and / or orientation in the pose is specified, particularly includes the link arranged at the free end of the kinematic chain (working equipment).

[0057] Each of the links 20 is equipped with an inertial sensor package or inertial sensor 26 (also referred to simply as a sensor). Each inertial sensor comprises a (linear) accelerometer, which measures the linear acceleration of the sensor, and / or a gyroscope, which measures the angular rate (i.e., the rotational velocity) of the sensor. An inertial sensor 28 (also referred to as the assembly inertial sensor) may also be attached to the assembly 6. Linear acceleration and angular velocity are each referenced to a reference frame of the inertial sensor, i.e., to an inertial sensor coordinate system. The origin of each inertial sensor coordinate system typically corresponds to the position of the respective inertial sensor.

[0058] The linear acceleration and / or rotational rate values ​​measured by the inertial sensors 26, or filtered values ​​for linear acceleration and / or rotational rate determined from these measured values ​​by means of filtering (in particular low-pass filtering), constitute inertial sensor data or are referred to as such. Filtering can optionally be performed by the inertial sensors themselves, so that the inertial sensors determine the filtered values ​​for linear acceleration and / or rotational rate. The measured values ​​for linear acceleration and / or rotational rate, or, if the inertial sensors themselves perform filtering, the filtered values ​​for linear acceleration and / or rotational rate, are transmitted by the inertial sensors to a processing unit of the machine (not shown, e.g., a control unit of the machine).When the measured values ​​for linear accelerations and / or rotation rates are transmitted to the computing unit, the filtering to determine the filtered values ​​for linear accelerations and / or rotation rates can be performed by the computing unit (if desired).

[0059] Figure 2 shows an element or section of the boom of an excavator with an inertial sensor attached to it, as well as forces or accelerations acting on it.

[0060] A link 20 and the inertial sensor 26, which is fixedly mounted on this link, are shown. The inertial sensor has its own inertial sensor coordinate system 50. If the

[0061] Page 10 of 24 R.415633 DE

[0062] With the boom, and thus link 20, at rest, the inertial sensor measures the corresponding component of gravity, allowing the resulting gravitational vector 38 and, consequently, the angle 38 to be determined. This angle 38 can then be used as a measure of the orientation of link 20 relative to the Earth's gravitational field. Knowing the angle of a subsequent link, determined using the same method, allows the relative angle between the two links to be calculated. Furthermore, by considering the forward kinematics, the position of the working equipment can be deduced.

[0063] However, if the boom is moved in space, the accelerometers built into the inertial sensor 26 measure the resulting acceleration vector 40, which is a combination of the gravitational vector 38 and the kinematic acceleration state 42. This consequently leads to an incorrect angle 46, from which the orientation of the link would be incorrectly derived.

[0064] Figure 3 shows an element or segment of the boom of an excavator with an attached inertial sensor and vectors to describe the position and orientation of the boom element and the inertial sensor.

[0065] A link 20 (link i) and the inertial sensor 26, which is fixedly mounted on this link, are shown. The sensor has its own inertial sensor coordinate system KSsi (reference numeral 50). The link's own coordinate system KSu (reference numeral 52) is defined at the corresponding joint point (rotary joint 22, 24) of link i. Furthermore, assume a link i-1, not shown here, which also has a link coordinate system KSu-i (reference numeral 54). To describe the desired position of the sensor 26 with respect to a fixed coordinate system (e.g., support element coordinate system or assembly coordinate system), the position of each link coordinate system relative to the preceding one is defined by a vector, starting from a fixed coordinate system KSw (reference numeral 56). w r L ,ii (vector 58) or vector w r Li+1 Li(Vector 66) is described. This has the advantage that the algorithms for determining the pose for each limb can be combined into a software module, thus enabling a modular software structure. Therefore, the vector w r Li Li-1 as well as its derivations by the upstream software module for determining the pose of link i-1 are known and can be used accordingly for determining the pose of link i. For this purpose, a

[0066] Page 11 of 24 R.415633 DE

[0067] corresponding vector addition of all vectors w r LiiLi-1 This is done to obtain the corresponding absolute position vector starting from the fixed world coordinate KSw to the joint point of the considered member i. To determine the absolute position w r WiS To describe t (vector 60) of sensor 26 with respect to the fixed coordinate system KSw, the following equation applies:

[0068] w

[0069]

[0070] ?w, Si w?w, Li T w^Li, Si

[0071] The vector w r Li si (Vector 62) describes the position of the inertial sensor within member i in the fixed coordinate system KSw. The vector w r w Li (Vector 64) describes the position of term i or its term coordinate system in the fixed coordinate system KSw. Using a rotation transformation matrix R w Li The relative vector can be determined Li r Li)Si Describe in member-specific coordinates:

[0072] w

[0073]

[0074] ^w, Si w^~w, Li T ^w, Li ' Li^Li, Si

[0075] The time derivative of this equation leads to a formulation for the velocity state of the inertial sensor:

[0076] w

[0077]

[0078] r w, Si ~ w r w, Li TR\v, Li ' Li r Li, Si ~ w rw, Li T w^w, Li * w r Li, Si

[0079] The derivation of the rotation transformation matrix leads to a formulation through which the rotational component of the guide velocity can be expressed as the cross product of the angular velocity vector. w a) W)Li and from the relative vector w r LiiSi This can be described. To obtain the state of acceleration, this equation is then differentiated with respect to time:

[0080] w

[0081]

[0082] r w, Si ~ w r w, Li T w^w, Li * w r Li, Si T w^w, Li * (w^w. Li * w r Li, Si)

[0083] The acceleration state calculated here w r w Si can be transformed into the coordinates of the sensor. si r WiSlThese values ​​are converted and subtracted from the measured acceleration vector. If the calculated acceleration states are accurate enough, the resulting measured vector will be only the...

[0084] Page 12 of 24 R.415633 DE

[0085] Gravity and the position of element i can also be better estimated in the state of motion.

[0086] Figure 4 shows a flowchart of a method for estimating the pose of a boom of a machine according to an embodiment of the invention. The machine has the structure described below in summary, e.g., as shown in Figures 1A and 1B. The boom comprises several links connected in pairs by joints, forming a kinematic chain. The machine includes a support element and a superstructure. A first link is mounted to the superstructure by a joint, the superstructure being rotatably mounted on the support element about a first axis. At least one of the joints is rotatable about a second axis that is not parallel to the first axis. The rotation of the superstructure is effected by an electric drive. Each link is equipped with a corresponding inertial sensor that measures linear accelerations and / or rotation rates to obtain inertial sensor data.The inertial sensors transmit the linear accelerations and / or rotational rates, or the inertial sensor data, to a processing unit of the machine. The process is carried out primarily by the machine's processing unit and comprises the following steps.

[0087] In step 110, inertial sensor data is acquired or determined. This data consists of measured values ​​of linear accelerations and / or rotational rates, or filtered values ​​of these measured values. If filtering is required, it can be performed by the inertial sensors themselves or by the processing unit of the machine.

[0088] In step 120, the angular velocity and / or angular acceleration of the structure's rotation about the first axis are determined to obtain rotational data. This rotational data includes values ​​for the angular velocity and / or angular acceleration of the structure's rotation about the first axis. Various methods for performing this step, i.e., determining the angular velocity and / or angular acceleration of the structure relative to the support element (i.e., about the first axis), are described below.

[0089] In step 130, a pose of the boom is determined using an estimation method based on a state model of the kinematic chain, in which the

[0090] Page 13 of 24 R.415633 DE

[0091] Inertial sensor data and rotation data are used. The pose specifies a position and / or orientation for at least one of the links relative to a reference frame of the support element or the superstructure (e.g., in the form of a corresponding transformation from the reference frame of the support element or the superstructure to the reference frame of the respective link). The position specified in the pose can also refer to a specific point of the respective link, e.g., the so-called Tool Center Point (TCP), which is a predetermined point of the link at the free end of the boom, which is, for example, a work attachment or to which such is attached. If the pose is specified relative to the support element, e.g., if the pose includes an orientation of the at least one link relative to the support element, an angle of rotation of the superstructure relative to the support element about the first axis can be used to determine the pose, which, for example,can be determined from the angular position data and / or by integration from the angular velocity.

[0092] The following describes various ways to determine the angular velocity and / or angular acceleration of the structure relative to the support element (step 120).

[0093] One way to determine angular velocity and / or angular acceleration is to use the angular position and angular velocity data available in the control system of the electric drive. Electric drives of a rotary mechanism in a machine, such as an excavator or material handler, typically use three-phase motors. To operate these motors with speed control, the angular position and angular velocity (rotational speed) of the rotor relative to a stator coordinate system are required. In mobile applications, resolvers are primarily used to determine the rotor's angular position. The angular velocity (of the rotor or the electric drive) can then be determined by calculating the first time derivative of the resolver signal.This speed signal is then adjusted for translation (translation between pinion and ring gear and, if applicable, gear ratio between motor and pinion) and fed into the corresponding component of the speed vector. w wLi This is also included. Furthermore, the rotational speed or angular velocity signal of the rotor relative to the stator can be derived again in time to determine the rotational acceleration of the rotor relative to the stator.

[0094] Page 14 of 24 R.415633 DE

[0095] The obtained signal can also be used, after translation correction, to describe the angular acceleration vector. w 5 w Li be used.

[0096] Another way to determine the angular velocity and / or angular acceleration of the setup relative to the support element is to evaluate measurement data from an inertial sensor attached to the setup (setup inertial sensor), assuming that the support element remains stationary during the setup's rotation. Measurements from the setup inertial sensor can be used for the rotation rates and / or, provided the setup inertial sensor is positioned at a distance from the axis of rotation, for the linear accelerations. The component of the measured rotation rate that is parallel to the axis of rotation can be used, as it is related to the setup's angular velocity for a given geometry (i.e., a known position of the setup inertial sensor relative to the axis of rotation).In the case of measured linear accelerations, the components lying in a plane orthogonal to the axis of rotation are used. In particular, the tangential component (i.e., the component pointing in the circumferential direction) can be used, since, for a given geometry, the linear acceleration in the tangential direction is related to the angular acceleration. The radial component (the component pointing away from the axis of rotation, or the component lying in the plane and passing through the axis of rotation, which is clearly orthogonal to the tangential component) can also be used, since, for a given geometry, the linear acceleration in the radial direction is related to the angular velocity (due to centrifugal force). Therefore, values ​​for the angular velocity and / or the angular acceleration can be determined from the rotation rates and / or linear accelerations measured by the inertial sensor.

[0097] Another possibility is presented below. This involves additionally utilizing knowledge of the electrical parameters of the rotary drive. For example, the following apply to the current build-up in flux-forming (j d ) as well as in moment-forming (j q ) Direction for PMSM (Permanent Magnet Synchronous Machine) the following equations:

[0098] —— = u d - R - i d + a) - ip q

[0099]

[0100] at

[0101] Page 15 of 24 R.415633 DE

[0102] dip q _.

[0103] u q R ■ i q (*) ■ ipd

[0104]

[0105] The tensions u d and u q are the voltage vectors of the resulting phase voltage transformed into the dq plane, R represents the winding resistance, and a> is the rotational speed or angular velocity of the rotor. The linked fluxes dand q can be described by the following relationships:

[0106] ^Pd L d 'id ”1” ^PPM

[0107] ^P q L q • i q

[0108] The term describes the chained magnetic flux caused by the permanent magnets. L d and L q The inductances are in the flux-generating and moment-generating directions. From these equations, and the fact that the linked fluxes fj d and fj q both from the streams i d and i q being dependent, the following two expressions result:

[0109] dxpd di d dip d di q _

[0110] di d ' dt di q 'dt Ud R ' l d + ^ - L q - i q dip q did dip q di q >

[0111]

[0112] did ' dt+ di q ' dt ~ Uq R ' lq a) ' ^ PM + Ld ' ld)

[0113] Based on the working arm structure of the exemplary mobile excavator, schematically depicted in Figures 1A and 1B, it can be seen that the moment of inertia of the superstructure with respect to its axis of rotation changes depending on the individual positions of the sections of its working arm. If we assume, for the sake of simplicity, that the superstructure (framework) rotates primarily around a fixed principal axis of inertia with respect to the fixed coordinate system, the following equation can be formulated to describe the rotational motion of the rotor. Here, J represents the moment of inertia of the entire superstructure, including the working arm and gear ratios, reduced to the motor shaft.

[0114] "77 ' i q (L q L d ^ • i q • p • T Load

[0115]

[0116] Page 16 of 24 R.415633 DE

[0117] The load torque T reduced to the motor shaft Load This represents the friction in the drive train and also the load or mass on the working equipment of the working arm relative to the Earth's gravitational field of the inclined superstructure. In the case of free movement of the working arm or the vehicle, this primarily consists of the weight force components of the load being carried (bulk material, etc.) as well as the entire superstructure including the working equipment. The number of pole pairs is taken into account by the constant p.

[0118] The equations above describe the dynamic behavior of the electric machine, which is directly connected to the superstructure via appropriate transmissions. The goal is to express these three equations in a form that allows conclusions to be drawn about quantities that are, for example, not measurable, using known observer structures. For this purpose, the equations can be transformed into a state-space representation. The equation above, which describes the rotational motion of the rotor, can then be differentiated with respect to time. Since both the magnetic flux of the permanent magnets and the inductances L d and L q depending on the current components i d and i q If these quantities are to be considered, they must also be partially differentiated with respect to time. The derivative is thus given by the following expression:

[0119] 2 r( dlpPM di q , ÖTpPM di d \ di n p L \ di q dt did dt y dt dL q di q dL q di d _ dL d di q dL d di d \ di q dt di d dt di q dt di d dt J lq ld ,■.. di q diff

[0120]

[0121] Lq ~ Ld ) '~di' ld + ^ ' iq '~dt^ ~ p ' TLOad

[0122] Rearranging this equation leads to the following formulation:

[0123] > di d > di q ...

[0124] 3p ■ ip ers , d ' 3p • ip

[0125]

[0126] erS: q • + ] • ü> ~J ' (iJ 2 • J • d) p • T Load

[0127] with

[0128] .. y_dipp M ., (dL d dL q \..

[0129] Wers,d\J'q' l d) ' l q + Ö^d / Ld ' lq

[0130]

[0131] Page 17 of 24 R.415633 DE

[0132] dippM.. fdL d dL q \, x.

[0133] ' l q ^PM + I a ■ a ■ ) ' l q ' l d \Lq ^d) ' l d

[0134]

[0135] U \ UL n UL n /

[0136] This results in the following system of differential equations for formulating the overall state-space representation:

[0137] dipd di d dtp d di q n .,,.

[0138] di d ' dt di q 'dt Ud R - l d + ^ - L q - i q dipg di d dtp q di q _.

[0139]

[0140] di d ' dt di q ' dt ~ Uq R ' lq ü) ' (lppM Ld '

[0141] cö = a

[0142] > di d > di q ..

[0143] 3p • ip ers , d • 3p • ip erS q • + J • öc —J - co 2 • ] • ap ■ T Load

[0144] Combining the terms on the left side of the differential equations yields the following in matrix-vector notation:

[0145] / dxpd / di d \ di d V diq 1 » dt R ' i d co ' L q • i q dlpq ^ d L o diq Uq R ■ iq CO ■ (ippM + L d ■ di d diq dt a 0 0 1. -] - co - 2 -i - a - p - T Load

[0146]

[0147] \ 3p ' Ipers.d 3p • lp erS q 0

[0148] To rearrange this nonlinear system of differential equations to solve for the time-derived state variables, the individual equations can be expressed as a linear system of equations with the unknown quantities —,

[0149]

[0150] —, cö, ä

[0151] dt dt are understood and this corresponds r The equations can be rearranged mathematically, or one can form the corresponding inverse of the matrix and then solve the system of equations for the desired quantities:

[0152] / dip d dip d o / di d \ »v 1 dt di d di q R ' i d CO ' L q • iq di q dlpq dlpg Uq R ■ i q CO ■ (ippw + L d • 1^) dt di d diq a 0 0 1 -] ■ CO - 2 -] ■ a - p - t Load

[0153]

[0154] \ 3p ■ lp ers ,d 3p ' Ipers.q 1 /

[0155] Page 18 of 24 R.415633 DE

[0156] This equation essentially corresponds to a state-space representation for nonlinear systems. Depending on the degree of nonlinearity, the derivatives of the linked magnetic fluxes and inductances appearing in the equations with respect to the two current components i can be d and i q These can be simplified as constants or as derivatives of corresponding characteristic curves. The nonlinear state-space model looks like this:

[0157] ( X = f(x, Ü)

[0158] *d\ I u d \ / i \

[0159] i \ \ l d \

[0160] 'M u = I u qy = g(x) = j )

[0161] Ü) / \ y, / \ /

[0162] / \' Load / \C0 /

[0163]

[0164] Here, y represents the vector of those quantities that are available as measured quantities. In this case, these are the two current components i. d and i qas well as the angular velocity a of the electric machine. If, for example, observer approaches for nonlinear systems are used (e.g., Luenberger observer, extended Kalman filter, high-gain observer, etc.), a comparison of these system states between the model (observer) and the real path can be made. The deviation can be fed back into the observer path via a feedback function or matrix L, resulting in an improvement of the estimate and allowing the desired quantity a (angular acceleration) to be obtained.

[0165] An example of a possible observation structure is shown in Figure 5. The actual controlled system 70 (drive system of the corresponding mobile application) and the nonlinear observer 72 are shown.

[0166] The controlled system 70 comprises the system under consideration (block 74), according to the equation x = f(x,u) above, and a measurement (block 76), according to the function g(x) above. The observer 72 receives the same input quantities as the electric drive, namely the phase voltages u transformed into the dq plane (resulting from block 78). d and u q Using the relationships formulated in the equation above, the state vector x can be determined by (numerical) integration of the state differential equation (in Block 80). From this, the corresponding measurable states i are derived. d , i q

[0167] Page 19 of 24 R.415633 DE

[0168] and ) are selected (Block 82) and combined as the output vector y, which can be compared with the measured quantities y and the resulting deviation e can be fed back into the integration of the state differential equation via the function or matrix L (Block 84). The observed quantities angular velocity w and angular acceleration ä, which are of interest for determining the pose of the working arm, are selected by a corresponding function block 86 and can, in particular, be fused with quantities determined by other means (e.g., as described above) (Block 90), e.g., using a Kalman filter. This can result in an improvement of the desired signal. The desired pose, e.g., the position w r TCPThe tool center point (TCP) or the link at the free end of the boom, or other parameters of the working arm's kinematics, can then be determined in a corresponding function block 92. To further improve the accuracy of the observed rotational acceleration of the superstructure, the load acting on the working equipment can be determined using a function block 94. This estimation can be performed, for example, by determining the mass of the working tool. The signal, or its first time derivative in the direction of the axis of rotation, can then be fed back into the state differential equation as an input.

[0169] Page 20 of 24

Claims

R.415633 DE 1. Method for estimating the pose of a boom of a working machine (2) comprising several links (20) connected in pairs by joints (24) to form a kinematic chain, wherein the working machine (2) comprises a support element (4) and a superstructure (6), wherein a first of the links (20) is mounted on the superstructure (6) by one of the joints (24), wherein the superstructure (6) is rotatably mounted on the support element (4) about a first axis (34) and at least one of the joints (24) is rotatable about a second axis which is not parallel to the first axis (34), wherein the rotation of the superstructure (6) is effected by an electric drive (8), wherein a respective inertial sensor (26) is attached to each link (20) which measures linear accelerations and / or rotation rates in order to obtain inertial sensor data; the procedure includes: Acquiring (110) or determining the inertial sensor data; Determining (120) an angular velocity and / or an angular acceleration of the rotation of the structure (6) about the first axis (34) in order to obtain rotation data; Determining (130) a pose of the boom using an estimation method based on a state model of the kinematic chain, in which the inertial sensor data and the rotation data are used, wherein the pose for at least one of the links (20) specifies a position and / or orientation relative to a reference system of the support element (6) or the structure (4).

2. Method according to claim 1, wherein a structure-integrated inertial sensor (28) is attached to the structure (6); wherein the rotation data are determined from linear accelerations and / or rotation rates measured by the structure-integrated inertial sensor or are determined taking into account the linear accelerations and / or rotation rates measured by the structure-integrated inertial sensor (28).

3. Method according to one of the preceding claims, wherein the rotational data are determined from drive data of the electric drive (8) or are determined taking into account the drive data of the electric drive (8).

4. Method according to claim 3, wherein the drive data includes a rotor angular velocity and / or a rotor angular acceleration of a rotor or a Page 21 of 24 R.415633 DE Include the output shaft of the electric drive (8); in particular, values ​​for the angular velocity and / or angular acceleration of the rotation of the structure (6) are determined by multiplication with a transmission ratio between the rotation of the rotor or the output shaft and the rotation of the structure from the rotor angular velocity or the rotor angular acceleration.

5. Method according to claim 3 or 4, wherein the rotor angular velocity and / or the rotor angular acceleration are determined from angular position data determined by an angular position measuring device, in particular a resolver, of the electric drive (8).

6. Method according to claim 5, wherein the rotor angular velocity and / or the rotor angular acceleration are determined from the angular position data by means of a second-order filter, which in particular includes two PT1 filters used in series.

7. Method according to one of claims 3 or 4, wherein a drive state space model (80) for the electric drive (8) is used which takes into account the moment of inertia of the structure, the boom and in particular a load moment to determine the rotor angular velocity and / or the rotor angular acceleration.

8. Method according to claim 7, wherein the drive state space model (80) is used in an observer (72); wherein the observed quantities in the drive state space model (80) are electric current intensities and an angular velocity for the rotor or the output shaft, which is determined from angular position data of an angular position measuring device, in particular a resolver, of the electric drive (8); wherein the rotor angular acceleration is an unobserved quantity that is determined by the observer (72).

9. Method according to claim 8, wherein the rotor angular velocity is an unobserved quantity determined by the observer (72); or wherein the rotor angular velocity is determined to be equal to the angular velocity for the rotor or the output shaft determined from angular position data. Page 22 of 24 R.415633 DE 10. Method according to one of the preceding claims, wherein corrected values ​​for the linear accelerations of the inertial sensors are used in the estimation method; wherein, in order to determine the corrected values, linear acceleration corrections for the inertial sensors (26) are determined from the rotational data by which the measured values ​​for the linear accelerations or the inertial sensor data are changed.

11. Method according to one of the preceding claims, wherein the state model of the kinematic chain is based on the forward kinematics of the kinematic chain.

12. Computing unit comprising a processor configured to perform the method according to any of the preceding claims.

13. Working machine (2) comprising a support element (4), a superstructure (6) and a boom; wherein the boom comprises several links (20) connected in pairs by joints (24) so ​​that a kinematic chain is formed; wherein a first of the links is mounted on the structure by one of the joints (24), wherein the structure is rotatably mounted on the support element about a first axis (34) and at least one of the joints (24) is rotatable about a second axis which is not parallel to the first axis, wherein the rotation of the structure is effected by an electric drive, wherein a respective inertial sensor (26) is attached to each link which measures linear accelerations and / or rotation rates in order to obtain inertial sensor data; further comprising a computing unit according to claim 12.

14. Computer program comprising instructions which, when the program is executed by a computer, cause it to execute the method according to claims 1 to 11.

15. Computer-readable data carrier on which the computer program according to claim 14 is stored. Page 23 of 24

Images