Shovel trajectory planning control method and device, working machine, equipment and medium

Abstract

The application provides a bucket trajectory planning control method, device, working machine, equipment and medium, and belongs to the field of engineering machinery. The method comprises the following steps: acquiring a motion signal; planning a bucket trajectory according to the motion signal, wherein the bucket trajectory comprises a rotation angle trajectory of at least one rotary joint; and controlling the driving of the corresponding rotary joint according to the bucket trajectory and the conversion relationship between the driving stroke and the rotation angle of the rotary joint, so as to control the movement of the bucket according to the bucket trajectory. The rotation angle trajectory of the rotary joint is planned according to the motion signal, the rotation angle trajectory of the at least one rotary joint constitutes the bucket trajectory, and then the driving of the corresponding rotary joint is controlled based on the conversion relationship between the driving stroke and the rotation angle of the rotary joint, so as to realize the movement of the bucket according to the planned bucket trajectory, realize smooth motion, reduce the impact generated by each structure of the excavator during the motion process, and improve the service life of each structure of the excavator.

Description

Technical Field

[0001] The present invention relates to the field of engineering machinery, and in particular to a bucket trajectory planning and control method, a bucket trajectory planning and control device, an operating machine, an electronic device, and a computer-readable storage medium. Background Art

[0002] Excavators are essential tools for modern engineering construction and production, widely used in earthwork, infrastructure, water conservancy, transportation, mining, and other fields. Manual excavator operation often lacks precise planning of movement volume and trajectory. For example, when the arm only needs to rotate a small angle, the operator swings the joystick widely to achieve a quicker movement. This results in large accelerations and rapid movements of the excavator's various components, causing severe impacts. This impacts the lifespan and accuracy of the excavator's components and wastes energy. Summary of the Invention

[0003] The purpose of the embodiments of the present invention is to provide a bucket trajectory planning and control method, device, working machinery, electronic equipment and medium. The method plans the rotation angle trajectory of the rotary joint according to the motion signal. The rotation angle trajectory of at least one rotary joint constitutes the bucket trajectory. Then, based on the conversion relationship between the rotary joint driving stroke and the rotation angle, the corresponding rotary joint drive is controlled to control the bucket to move according to the planned bucket trajectory, achieve smooth movement, reduce the impact of various structures of the excavator during the movement, and improve the life of various structures of the excavator.

[0004] In order to achieve the above-mentioned object, the first aspect of the present invention provides a bucket trajectory planning and control method, the bucket trajectory planning and control method comprising:

[0005] Get action signal;

[0006] planning a bucket trajectory according to the motion signal, wherein the bucket trajectory includes a rotation angle trajectory of at least one rotation joint;

[0007] According to the bucket trajectory and the conversion relationship between the corresponding rotary joint drive stroke and the rotation angle, the corresponding rotary joint drive is controlled to control the bucket to move according to the bucket trajectory.

[0008] According to the above technical means, the rotation angle trajectory of the rotary joint is planned according to the action signal, and the rotation angle trajectory of at least one rotary joint constitutes the bucket trajectory. Then, based on the conversion relationship between the rotary joint drive stroke and the rotation angle, the corresponding rotary joint drive is controlled to control the bucket to move according to the planned bucket trajectory, achieve smooth movement, reduce the impact on the various structures of the excavator during the movement, and improve the life of the various structures of the excavator.

[0009] In the embodiment of the present application, the rotation angle trajectory of the rotation joint is planned according to the action signal, comprising:

[0010] The rotation angle is determined according to the action signal;

[0011] The seven-degree polynomial curve of the rotation joint is constructed;

[0012] The rotation angle trajectory of the rotation joint is planned according to the rotation angle and the seven-degree polynomial curve.

[0013] According to the above technical means, the polynomial curves corresponding to the rotation angle, the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk are constructed, which provides reference data in three dimensions of the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk for subsequent planning of the rotation angle trajectory, and is more conducive to planning a smoother bucket movement trajectory.

[0014] In the embodiment of the present application, the rotation angle trajectory of the rotation joint is planned according to the rotation angle and the seven-degree polynomial curve, comprising:

[0015] The planning time length is configured;

[0016] The rotation angle trajectory is planned according to the planning time length and the rotation angle;

[0017] The rotation angular velocity trajectory, the rotation angular acceleration trajectory and the rotation angular jerk trajectory are generated according to the rotation angle trajectory and the seven-degree polynomial curve;

[0018] The motion ranges corresponding to the rotation angle, the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk are determined according to the kinetic parameters of the joint;

[0019] The rotation angle trajectory, the rotation angular velocity trajectory, the rotation angular acceleration trajectory and the rotation angular jerk trajectory are judged according to the motion ranges corresponding to the rotation angle, the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk respectively;

[0020] If there is a value exceeding the corresponding motion range in any trajectory, the above steps are repeated until the rotation angle trajectory is planned, in which the rotation angle, the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk all conform to the motion ranges corresponding to them respectively.

[0021] According to the above technical means, the planned curves are constrained by using the motion ranges corresponding to the rotation angle, the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk respectively, so as to avoid the impact of the planned bucket trajectory due to the existence of the joint-limited kinetic parameters, thereby making the excavation action more stable and improving the service life and precision of the mechanical mechanism.

[0022] In the embodiment of the present application, the conversion relationship between the joint driving stroke and the rotation angle includes: a conversion relationship between the swing joint driving stroke and the rotation angle.

[0023] The conversion relationship between the swing joint driving stroke and the rotation angle is established by the following method:

[0024] The gear teeth number and the gear ring teeth number of the motor output shaft of the swing joint are acquired.

[0025] The transmission ratio of the swing joint gear and the gear ring is calculated according to the gear teeth number and the gear ring teeth number.

[0026] The conversion relationship between the swing driving motor rotation angle and the swing rotation angle is calculated according to the proportional relationship between the swing driving motor rotation angle and the swing rotation angle and the transmission ratio.

[0027] According to the above technical means, the conversion relationship between the driving motor rotation angle and the swing rotation angle can be determined according to the gear teeth number and the gear ring teeth number of the swing motor output shaft, which provides a theoretical basis for the swing motor driving control.

[0028] In the embodiment of the present application, the conversion relationship between the joint driving stroke and the rotation angle includes: a conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle.

[0029] The conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle is established by the following method:

[0030] The boom joint structural parameters are acquired.

[0031] The boom joint geometric structure diagram is constructed according to the boom joint structural parameters.

[0032] The first calculation formula based on the cosine theorem for the boom driving cylinder stroke is determined according to the boom joint structural parameters and the boom joint geometric structure diagram.

[0033] The second calculation formula representing the corresponding angle geometric relationship between the boom rotation angle and the boom driving cylinder stroke is determined according to the boom joint structural parameters and the boom joint geometric structure diagram.

[0034] The conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle is constructed according to the first calculation formula and the second calculation formula.

[0035] According to the above technical means, the conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle is determined based on the structural parameters and the geometric relationship of the boom joint, which provides conversion relationship support for the boom joint driving control.

[0036] In the embodiment of the present application, the conversion relationship between the joint driving stroke and the rotation angle includes: a conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle.

[0037] The conversion relationship between the arm driving cylinder stroke and the arm joint rotation angle is established by the following method:

[0038] Obtain the arm joint structure parameters;

[0039] Construct an arm joint geometric structure diagram according to the arm joint structure parameters;

[0040] Determine a third calculation formula for the arm driving cylinder stroke based on the cosine theorem according to the arm joint structure parameters and the arm joint geometric structure diagram;

[0041] Determine a fourth calculation formula representing the corresponding angular geometric relationship between the arm rotation angle and the arm driving cylinder stroke according to the arm joint structure parameters and the arm joint geometric structure diagram;

[0042] Construct the conversion relationship between the arm driving cylinder stroke and the arm joint rotation angle according to the third calculation formula and the fourth calculation formula.

[0043] According to the above technical means, the conversion relationship between the arm driving cylinder stroke and the arm joint rotation angle is determined based on the structure parameters and geometric relationship of the arm joint, which provides conversion relationship support for the arm joint driving control.

[0044] In the embodiment of the application, the conversion relationship between the joint driving stroke and the rotation angle includes: a conversion relationship between the bucket driving cylinder stroke and the bucket joint rotation angle;

[0045] The conversion relationship between the bucket driving cylinder stroke and the bucket joint rotation angle is established by the following method:

[0046] Obtain the bucket joint structure parameters;

[0047] Construct a bucket joint geometric structure diagram according to the bucket joint structure parameters;

[0048] Determine a fifth calculation formula for the bucket driving cylinder stroke based on the cosine theorem according to the bucket joint structure parameters and the bucket joint geometric structure diagram;

[0049] Determine a sixth calculation formula representing the corresponding angular geometric relationship between the bucket rotation angle and the bucket driving cylinder stroke according to the bucket joint structure parameters and the bucket joint geometric structure diagram;

[0050] Construct the conversion relationship between the bucket driving cylinder stroke and the bucket joint rotation angle according to the fifth calculation formula and the sixth calculation formula.

[0051] According to the technical means, the conversion relationship between the bucket driving cylinder stroke and the bucket joint rotation angle is determined based on the structural parameters and geometric relationship of the bucket joint, thereby providing conversion relationship support for the bucket joint driving control.

[0052] The second aspect of the present application provides a bucket trajectory planning control device, which comprises:

[0053] a signal acquisition unit configured to acquire an action signal;

[0054] a bucket trajectory planning unit configured to plan a bucket trajectory according to the action signal, wherein the bucket trajectory comprises a rotation angle trajectory of at least one rotation joint;

[0055] a control unit configured to control the corresponding rotation joint driving according to the bucket trajectory and the conversion relationship between the corresponding rotation joint driving stroke and the rotation angle, so as to control the bucket to move according to the bucket trajectory.

[0056] According to the technical means, the rotation angle trajectory of the rotation joint is planned according to the action signal, the rotation angle trajectory of the at least one rotation joint constitutes the bucket trajectory, and the corresponding rotation joint driving is controlled based on the conversion relationship between the rotation joint driving stroke and the rotation angle, so as to realize the control of the bucket to move according to the planned bucket trajectory, realize smooth action, reduce the impact generated by each structure of the excavator during the action, and improve the service life of each structure of the excavator.

[0057] The third aspect of the present application provides a working machine comprising a control device configured to perform the steps of the bucket trajectory planning control method.

[0058] The fourth aspect of the present application provides an electronic device comprising at least a processor and a memory, wherein:

[0059] The memory is configured to store a computer program.

[0060] The processor is configured to execute the computer program stored in the memory, so as to realize the steps of the bucket trajectory planning control method.

[0061] The fifth aspect of the present application provides a computer readable storage medium storing a computer program, wherein when the computer program is executed by a processor, the steps of the bucket trajectory planning control method are realized.

[0062] According to the technical scheme, the rotation angle trajectory of the rotary joint is planned according to the action signal, the rotation angle trajectory of at least one rotary joint constitutes the bucket trajectory, and the corresponding rotary joint driving is controlled based on the conversion relationship between the rotary joint driving stroke and the rotation angle, so as to realize the movement of the bucket according to the planned bucket trajectory, realize smooth action, and have higher action accuracy; the impact of each structure of the excavator in the action process is reduced, and the service life of each structure of the excavator is improved; the smooth action process provides less energy, the impact and collision are reduced, and the energy loss is also reduced.

[0063] Other features and advantages of the embodiments of the present application will be described in detail in the following detailed description. BRIEF DESCRIPTION OF DRAWINGS

[0064] The accompanying drawings are included to provide a further understanding of the embodiments of the present application, and constitute a part of the specification, and are used together with the following detailed description to explain the embodiments of the present application, but do not constitute a limitation on the embodiments of the present application. In the drawings:

[0065] Figure 1 is a model diagram of an excavator provided by an embodiment of the present application;

[0066] Figure 2 is a flowchart of a bucket trajectory planning control method provided by an embodiment of the present application;

[0067] Figure 3 is a rotation angle trajectory diagram provided by an embodiment of the present application;

[0068] Figure 4 is a rotation angle velocity trajectory diagram provided by an embodiment of the present application;

[0069] Figure 5 is a rotation angle acceleration trajectory diagram provided by an embodiment of the present application;

[0070] Figure 6 is a rotation angle jerk trajectory diagram provided by an embodiment of the present application;

[0071] Figure 7 is a boom joint geometric structure diagram provided by an embodiment of the present application;

[0072] Figure 8 is a stick joint geometric structure diagram provided by an embodiment of the present application;

[0073] Figure 9 is a bucket joint geometric structure diagram provided by an embodiment of the present application;

[0074] Figure 10 is a diagram of the conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle provided by an embodiment of the present application;

[0075] Figure 11 is a bucket rod driving oil cylinder stroke and bucket joint rotation angle conversion relationship diagram provided by an embodiment of the present application;

[0076] Figure 12 is a bucket driving oil cylinder stroke and bucket joint rotation angle conversion relationship diagram provided by an embodiment of the present application;

[0077] Figure 13 is a bucket trajectory planning control device block diagram provided by an embodiment of the present application.

[0078] Explanation of reference signs

[0079] 1-rotary platform, 2-boom hydraulic oil cylinder, 3-stick hydraulic oil cylinder, 4-bucket hydraulic oil cylinder, 5-connecting rod, 6-bucket, 7-rocker, 8-stick, 9-boom. DETAILED DESCRIPTION

[0080] The specific embodiments of the present application will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are merely intended to illustrate and explain the present application, and are not intended to limit the present application.

[0081] The main structures of excavators on the market are similar, as shown in Figure 1 The main structure of the excavator is a shovel arm mechanism, a rotary mechanism, and a walking mechanism. The rotary mechanism is installed on the shape mechanism, and the shovel arm mechanism is installed on the rotary mechanism. The shovel arm mechanism mainly consists of a boom 9, a stick 8, a bucket 6, a boom driving oil cylinder 2, a stick driving oil cylinder 3, and a bucket driving oil cylinder 4. The components of the shovel arm mechanism are connected by pin shafts. The rotary mechanism mainly consists of a rotary motor and a rotary platform 1. The walking mechanism mainly consists of a walking motor and a track. Under the action of the driving elements, each component performs its corresponding action. The boom 9 is responsible for the lifting of the shovel arm, the stick 8 is responsible for the forward and backward swinging of the bucket, the bucket 6 is responsible for the scooping in and out, and the rotary device is responsible for the rotary motion of the shovel arm. During the excavation operation of the excavator, the bucket motion trajectory is mainly affected by the rotary structure and the shovel arm mechanism.

[0082] The embodiment of the application provides a bucket trajectory planning control method, which adopts a seven-degree polynomial to plan a rotation angle, a rotation angular velocity, a rotation angular acceleration and a rotation angular jerk of each rotation joint of a excavator, judges whether the rotation angle, the maximum rotation angular velocity, the maximum rotation angular acceleration and the maximum rotation angular jerk exceed a set range, and re-plans the trajectory if the range is exceeded. In combination with modeling of the rotation joint of the excavator, the angle planned for each joint is converted into a rotation angle of a driving motor or a stroke of a driving oil cylinder to control the action of each joint. The rotation joint of the excavator mainly includes a slewing joint, an arm joint, a stick joint and a bucket joint. When the bucket trajectory is planned, the method of the application can be used to plan and control the trajectory of any number of joints, and other joints are planned and controlled by using an existing method. The best effect is that the four joints are all planned and controlled by using the method of the application, so that the impact on the excavator during operation is minimized and the action is most smooth.

[0083] Figure 2 The figure is a flow chart of the bucket trajectory planning control method provided by the embodiment of the application. As shown in the figure, Figure 2 The bucket trajectory planning control method comprises the following steps.

[0084] S1: acquiring an action signal; in the embodiment of the application, the action signal can be a signal acquired by a main control system of the excavator after an operator performs an operation, or can be an action signal given by the main control system after automatic identification.

[0085] S2: planning a bucket trajectory according to the action signal, wherein the bucket trajectory comprises a rotation angle trajectory of at least one rotation joint. In the application, the claimed bucket trajectory is a bucket trajectory controlled by the slewing joint, the arm joint, the stick joint and the bucket joint of the excavator, wherein the rotation angle trajectory of any number of rotation joints can be planned by using the method of the application.

[0086] S3: controlling the corresponding rotation joint driving according to the bucket trajectory and a conversion relationship between the corresponding rotation joint driving stroke and the rotation angle, so as to control the bucket to move according to the bucket trajectory.

[0087] In the embodiment of the application, the bucket trajectory is planned according to the action signal, which comprises the following steps.

[0088] determining the rotation angle of the rotation joint according to the action signal;

[0089] constructing a seven-degree polynomial curve of the rotation joint, so that the seven-degree polynomial curve is convenient for programming of an embedded control system and does not need to be solved online.

[0090] According to the rotation angle and the seventh-degree polynomial curve, a rotation angle trajectory of the rotation joint is planned, and the rotation angle trajectory of the at least one rotation joint constitutes the bucket trajectory. In the embodiment of the present application, the rotation angle trajectory of the rotation joint is planned in the manner of seventh-degree polynomial curve interpolation.

[0091] According to the above technical means, the polynomial curves corresponding to the rotation angle, the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk are constructed, reference data in three dimensions of the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk are provided for subsequent planning of the rotation angle trajectory, and it is more beneficial to subsequently plan a smoother bucket movement trajectory.

[0092] In the embodiment of the present application, planning the rotation angle trajectory of the rotation joint according to the rotation angle and the seventh-degree polynomial curve comprises:

[0093] The planning time period length is configured. In the embodiment of the present application, the planning time period length is configured arbitrarily according to requirements.

[0094] The rotation angle trajectory is planned according to the planning time period length and the rotation angle.

[0095] The rotation angular velocity trajectory, the rotation angular acceleration trajectory and the rotation angular jerk trajectory are generated according to the rotation angle trajectory and the seventh-degree polynomial curve.

[0096] Supposing that t0 is an initial time, the joint rotation angle, the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk are all known, the following equation can be obtained:

[0097]

[0098] Wherein, a7, a6, a5, a4, a3, a2, a1, a0 are polynomial coefficients, t is time, θ(t) is rotation angle, θ'(t) is rotation angular velocity, θ''(t) is rotation angular acceleration, θ'''(t) is rotation angular jerk, θ s is initial rotation angle, θ s ' is initial rotation angular velocity, θ s '' is initial rotation angular acceleration, θ s ''' is initial rotation angular jerk.

[0099] Supposing that t1 is a terminal time, the joint rotation angle, the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk are all known, the following equation can be obtained:

[0100]

[0101] Wherein, θ e is terminal rotation angle, θ eis the terminal rotation angle velocity, θ e is the terminal rotation angle acceleration, θ e is the terminal rotation angle jerk.

[0102] The following matrix form Ax = b can be listed:

[0103] Where:

[0104]

[0105] The solution of the equation is: x = A -1 b;

[0106] Let the initial time t0= 0, substitute the solution of the equation, the seven polynomial curve coefficients can be obtained:

[0107]

[0108] a1= θ s ′;

[0109] a0= θ s .

[0110] According to the planning time period length and the rotation angle, the seven polynomial curve coefficients can be solved, so as to obtain the seven polynomial curve of the rotation angle trajectory, the polynomial curve of the rotation angle velocity trajectory, the polynomial curve of the rotation angle acceleration trajectory and the polynomial curve of the rotation angle jerk trajectory, and then generate the rotation angle trajectory, the rotation angle velocity trajectory, the rotation angle acceleration trajectory and the rotation angle jerk trajectory according to the curves.

[0111] According to the kinematic parameters of the joint, the corresponding motion ranges of the rotation angle, the rotation angle velocity, the rotation angle acceleration and the rotation angle jerk are determined. Because the geometric structure, material and driving system of the joint will determine the kinematic parameters of the joint, i.e. the motion angle of the joint has a range, the rotation angle velocity, the rotation angle acceleration and the rotation angle jerk are all limited by the maximum value, the planned rotation angle cannot exceed the motion range of the rotation angle, and the rotation angle velocity, the rotation angle acceleration and the rotation angle jerk cannot exceed the corresponding maximum value, otherwise the trajectory planning needs to be re-performed. That is, the following relationships are satisfied:

[0112] max(θ(t))≤max_angle;

[0113] min(θ(t))≥min_angle;

[0114] max(θ′(t))≤max_ang_vel;

[0115] max(θ″(t))≤max_ang_acc;

[0116] max(θ″′(t))≤max_ang_jerk;

[0117] Wherein: min_angle is the minimum rotation angle of the joint movement, max_angle is the maximum rotation angle of the joint movement; max_ang_vel is the maximum rotation angular velocity of the joint movement, max_ang_acc is the maximum rotation angular acceleration of the joint movement; max_ang_jerk is the maximum rotation angular jerk of the joint movement.

[0118] According to the corresponding movement range of the rotation angle, the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk, the rotation angle trajectory, the rotation angular velocity trajectory, the rotation angular acceleration trajectory and the rotation angular jerk trajectory are judged.

[0119] If there is a value exceeding the corresponding movement range in any trajectory, the above steps are repeated until the rotation angle trajectory, the rotation angular velocity trajectory, the rotation angular acceleration trajectory and the rotation angular jerk trajectory are planned, which meet the corresponding movement range of each.

[0120] According to the above technical means, the planned trajectory is constrained by the corresponding movement range of the rotation angle, the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk, so as to avoid the impact of the planned bucket trajectory due to the existence of the joint limited kinetic parameters, so as to make the digging action more stable, and improve the life and precision of the mechanical mechanism.

[0121] In one embodiment, the joint angle is set to move from 0 degree to 60 degree at 10s, and the rotation angular velocity, the rotation angular acceleration and the rotation angular jerk of the starting point and the ending point are all 0. The rotation angle trajectory, the rotation angular velocity trajectory, the rotation angular acceleration trajectory and the rotation angular jerk trajectory can be drawn. As shown in the following figure. Figures 3-6

[0122] In the embodiment of the application, the conversion relationship between the joint driving stroke and the rotation angle includes: the conversion relationship between the rotation joint driving stroke and the rotation angle; the rotation joint is driven by the rotation motor to rotate the gear on the output shaft and the gear ring engaged with the gear, and the gear ring is fixed with the rotation platform 1, thereby driving the rotation platform 1 to rotate.

[0123] The conversion relationship between the rotation joint driving stroke and the rotation angle is established by the following way:

[0124] The number of teeth Z0 of the motor output shaft gear of the rotation joint and the number of teeth Z1 of the gear ring are obtained;

[0125] The transmission ratio of the rotation joint gear and the gear ring is calculated according to the number of teeth of the gear and the number of teeth of the gear ring:

[0126] ​

[0127] According to the calculation relationship that the rotation angle of the rotary drive motor is proportional to the rotation angle of the rotary motor, the conversion relationship between the rotation angle of the rotary drive motor and the rotation angle of the rotary motor is calculated according to the transmission ratio:

[0128] λ1=Iθ1;

[0129] Wherein, λ1 is the rotation angle of the rotary drive motor, I is the transmission ratio, and θ1 is the rotation angle of the rotary motor.

[0130] According to the above technical means, the conversion relationship between the drive motor rotation angle and the rotary rotation angle can be determined according to the number of teeth of the rotary motor output shaft gear and the number of teeth of the gear ring, which provides a theoretical basis for the drive control of the rotary motor.

[0131] In the embodiment of the application, the conversion relationship between the joint driving stroke and the rotation angle includes: the conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle. As shown in Figure 1 The boom joint mainly includes a boom 9 and a boom driving cylinder 2, the boom 9 and the boom driving cylinder 2 are respectively hinged to the rotary platform 1 at different positions, the other end of the boom driving cylinder 2 is hinged to the boom 9, and the boom realizes up and down swing when the boom driving cylinder 2 extends and retracts. The conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle is established by the following way:

[0132] Obtain the boom joint structure parameters;

[0133] According to the boom joint structure parameters, a boom joint geometric structure diagram is constructed, as shown in Figure 7 ;

[0134] The first hinge point connected with the rotary platform and the boom driving cylinder is defined as point A, the second hinge point connected with the rotary platform and the boom is defined as point B, the third hinge point connected with the boom driving cylinder and the boom is defined as point C, the fifth hinge point connected with the boom and the stick is defined as point E, and the position of point E when the boom joint rotation angle is zero is defined as point E', then the structure parameters of the excavator related to the boom joint mainly include: the distance AB between the first hinge point and the second hinge point, the distance BC between the second hinge point and the third hinge point, and the angle values of ∠CBE and ∠ABE', the length values of AB and BC, and the angle values of ∠CBE and ∠ABE' are measured according to the actual application of the excavator.

[0135] According to the boom joint structure parameters and the boom joint geometric structure diagram, the boom driving cylinder stroke is determined based on the first calculation formula of the cosine theorem. In the boom joint, according to the geometric relationship in Figure 7 , the first calculation formula of the boom driving cylinder based on the cosine theorem can be obtained from ΔABC. In ΔABC, according to the cosine theorem, the first calculation formula is:

[0136]

[0137] Wherein, λ2 is the boom driving cylinder length.

[0138] According to the boom joint structure parameters and the boom joint geometric structure diagram, a second calculation formula representing the corresponding angle geometric relationship between the boom rotation angle and the boom driving cylinder stroke is determined. In the boom joint, the calculation formula of the geometric relationship between ∠ABC and the boom rotation angle θ2 is needed. According to the geometric relationship in the formula, the second calculation formula is obtained as follows: Figure 7 ∠ABC = ∠ABE'-θ2+∠CBE.

[0139] Wherein, θ2 is the boom rotation angle, θ2<0, negative for upward rotation, positive for downward rotation;

[0140] Then, the second calculation formula can be transformed as follows:

[0141] ∠ABC = ∠ABE'-θ2+∠CBE.

[0142] According to the first calculation formula and the second calculation formula, the conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle is constructed, that is, the transformed second calculation formula is substituted into the first calculation formula, and the conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle is obtained as follows:

[0143]

[0144] Wherein, the length values of AB and BC and the angle values of ∠CBE and ∠ABE' are measured according to the actual application of the excavator.

[0145] According to the above technical means, the conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle is determined based on the structure parameters and the geometric relationship of the boom joint, and conversion relationship support is provided for the boom joint driving control.

[0146] In the embodiments of the present application, the conversion relationship between the joint driving stroke and the rotation angle includes the conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle. As shown in FIG. 1, the boom joint mainly includes a boom 8 and a boom driving cylinder 3, the boom 8 and the boom driving cylinder 3 are respectively hinged at different positions of the boom 9, the other end of the boom driving cylinder 3 is hinged with the boom 8, and when the boom driving cylinder 3 is extended or retracted, the boom realizes left and right swing. Figure 1 The conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle is established by the following method:

[0147] Obtaining the boom joint structure parameters;

[0148]

[0149] ​According to the boom joint structure parameters, a boom joint geometry diagram is constructed, as shown in Figure 8 The second hinge point of the dynamic arm and the slewing platform is defined as point B in the boom joint geometry diagram, the fourth hinge point of the boom driving oil cylinder and the dynamic arm is defined as point D, the fifth hinge point of the dynamic arm and the boom is defined as point E, the eleventh hinge point of the boom driving oil cylinder and the boom is defined as point K, the tenth hinge point of the bucket driving oil cylinder and the boom is defined as point J, and the seventh hinge point of the boom and the bucket is defined as point G. The main structure parameters of the excavator related to the boom joint include the distance EK between the fifth hinge point and the eleventh hinge point, the distance DE between the fourth hinge point and the fifth hinge point, and the angle values of ∠DEB, ∠GEJ and ∠JEK. The length values of EK and DE and the angle values of ∠DEB, ∠GEJ and ∠JEK are measured according to the actual application of the excavator.

[0150] According to the boom joint structure parameters and the boom joint geometry diagram, a third calculation formula for the boom driving oil cylinder stroke based on the cosine theorem is determined. In the boom joint, according to the geometric relationship in Figure 8 the third calculation formula for the boom driving oil cylinder based on the cosine theorem can be obtained. In ΔDEK, the third calculation formula is obtained according to the cosine theorem as follows:

[0151]

[0152] wherein λ3 is the length of the boom driving oil cylinder.

[0153] According to the boom joint structure parameters and the boom joint geometry diagram, a fourth calculation formula representing the corresponding angle geometric relationship between the boom rotation angle and the boom driving oil cylinder stroke is determined. In the boom joint, the calculation formula of the geometric relationship between ∠DEK and the boom rotation angle θ3 is needed. According to the geometric relationship in Figure 8 the fourth calculation formula can be obtained as follows:

[0154] ∠DEK = 360 - ∠DEB - ∠BEG - ∠GEJ - ∠JEK;

[0155] wherein ∠BEG = 180 - θ3, θ3 is the boom joint rotation angle, and θ2 > 0, with upward rotation being negative and downward rotation being positive.

[0156] Therefore, the fourth calculation formula can be transformed as follows:

[0157] ∠DEK = 180 - ∠DEB + θ3 - ∠GEJ - ∠JEK

[0158] The conversion relationship between the boom driving oil cylinder stroke and the boom joint rotation angle is constructed according to the third calculation formula and the fourth calculation formula, that is, the fourth calculation formula after deformation is substituted into the third calculation formula, and the conversion relationship between the boom driving oil cylinder stroke and the boom joint rotation angle is obtained as follows:

[0159]

[0160] The length values of EK and DE and the angle values of ∠DEB, ∠GEJ and ∠JEK are measured according to the actual application of the excavator.

[0161] According to the above technical means, the conversion relationship between the boom driving oil cylinder stroke and the boom joint rotation angle is determined based on the structural parameters and geometric relationships of the boom joint, and conversion relationship support is provided for the boom joint driving control.

[0162] In the embodiments of the present application, the conversion relationship between the joint driving stroke and the rotation angle includes the conversion relationship between the bucket driving oil cylinder stroke and the bucket joint rotation angle. As shown in Figure 1 The bucket joint mainly includes a bucket 6, a bucket driving oil cylinder 4, a rocker 7 and a connecting rod 5. The bucket 6 is hinged to the boom 8. One end of the rocker 7 is hinged to the boom 8, and the other end of the rocker 7 is hinged to one end of the connecting rod 5 and the bucket driving oil cylinder 4. The other end of the bucket driving oil cylinder 4 is hinged to the boom 8, and the other end of the connecting rod 5 is hinged to the bucket.

[0163] The conversion relationship between the bucket driving oil cylinder stroke and the bucket joint rotation angle is established by the following method:

[0164] Obtain the structural parameters of the bucket joint;

[0165] Construct a geometric structure diagram of the bucket joint according to the structural parameters of the bucket joint, as shown in Figure 9 The fifth hinge point E connecting the boom and the boom, the sixth intersection point F connecting the boom and the remote sensing, the seventh hinge point G connecting the boom and the bucket, the eighth hinge point H connecting the bucket and the connecting rod, the ninth hinge point I connecting the remote sensing, the connecting rod and the bucket driving oil cylinder, the tenth hinge point J connecting the bucket driving oil cylinder and the boom, and the eleventh hinge point K connecting the boom driving oil cylinder and the boom are defined. The center point L of the end of the bucket is defined. The structural parameters of the excavator related to the bucket joint mainly include the distance FJ between the sixth hinge point and the tenth hinge point, the distance FI between the sixth hinge point and the ninth hinge point, the distance HI between the eighth hinge point and the ninth hinge point, the distance FG between the sixth hinge point and the seventh hinge point, the distance GH between the seventh hinge point and the eighth hinge point, and the angle values of ∠FGE, ∠LGH, ∠JFE and ∠EFG. The length values of FJ, FI, HI, FG and GH and the angle values of ∠FGE, ∠LGH, ∠JFE and ∠EFG are measured according to the actual application of the excavator.

[0166] The fifth calculation formula of the bucket driving oil cylinder stroke based on the cosine theorem is determined according to the bucket joint structure parameters and the bucket joint geometric structure diagram. In the bucket joint, the fifth calculation formula of the bucket driving oil cylinder based on the cosine theorem can be obtained according to the geometric relationship in Figure 9 The fifth calculation formula based on the cosine theorem is determined according to the geometric relationship in

[0167]

[0168] Wherein, λ4 is the length of the bucket driving oil cylinder.

[0169] The sixth calculation formula representing the corresponding angle geometric relationship between the bucket rotation angle and the bucket driving oil cylinder stroke is determined according to the bucket joint structure parameters and the bucket joint geometric structure diagram. In the bucket joint, the calculation formula of the geometric relationship between ∠JFI and the bucket rotation angle θ4 is needed. According to the geometric relationship in Figure 9 The sixth calculation formula is obtained according to the geometric relationship in

[0170] ∠JFI = 360- ∠JFE- ∠EFG- ∠GFI;

[0171] Wherein, in the round corner F, ∠GFI = ∠HFI + ∠HFG; in ΔHFI, according to the cosine theorem, the following formula can be obtained: In ΔFGH, the sine theorem can be obtained: In ΔFGH, according to the cosine theorem, the following formula can be obtained: In the round corner G:

[0172] ∠FGH = 360- ∠FGE- ∠EGL- ∠LGH; wherein, ∠EGL = 180- θ4, θ4 > 0, negative for upward rotation, positive for downward rotation;

[0173] The conversion relationship between the bucket driving oil cylinder stroke and the bucket joint rotation angle is constructed according to the fifth calculation formula and the sixth calculation formula, that is, the sixth calculation formula is substituted into the fifth calculation formula, wherein the length values of FJ, FI, HI, FG and GH and the angle values of ∠FGE, ∠LGH, ∠JFE and ∠EFG are measured according to the actual application of the excavator.

[0174] According to the above technical means, the conversion relationship between the bucket driving oil cylinder stroke and the bucket joint rotation angle is determined based on the structure parameters and geometric relationship of the bucket joint, which provides conversion relationship support for the driving control of the bucket joint.

[0175] For the slewing joint, the driving stroke trajectory of the rotation joint is represented as:

[0176]

[0177] For the boom joint, the driving stroke trajectory of the rotary joint is represented as:

[0178] Substitute the boom rotation angle, and it is done. In an embodiment, the converted boom driving cylinder stroke and the boom joint rotation angle conversion relationship is as shown in Figure 10 .

[0179] For the arm joint, the driving stroke trajectory of the rotary joint is represented as:

[0180] Substitute the arm rotation angle, and it is done. In an embodiment, the converted arm driving cylinder stroke and the arm joint rotation angle conversion relationship is as shown in Figure 11 .

[0181] For the bucket joint, ∠EGL = 180-θ4(t), according to the aforementioned conversion relationship between the bucket joint driving stroke and the rotation angle, substitute the calculation of ∠FGH, FH, ∠HFG, ∠HFI, ∠JFI into the following formula: The driving stroke trajectory of the rotary joint can be obtained. In an embodiment, the converted bucket driving cylinder stroke and the bucket joint rotation angle conversion relationship is as shown in Figure 12 .

[0182] The second aspect of the present application provides a bucket trajectory planning control device, as shown in Figure 13 , the bucket trajectory planning control device comprises:

[0183] a signal acquisition unit for acquiring an action signal;

[0184] a bucket trajectory planning unit for planning a bucket trajectory according to the action signal, the bucket trajectory comprising a rotation angle trajectory of at least one rotary joint;

[0185] a control unit for controlling the corresponding rotary joint driving according to the bucket trajectory and the conversion relationship between the driving stroke and the rotation angle of the corresponding rotary joint, so as to control the bucket to move according to the bucket trajectory.

[0186] According to the above technical means, the rotation angle trajectory of the rotary joint is planned according to the action signal, the rotation angle trajectory of at least one rotary joint constitutes the bucket trajectory, and then the corresponding rotary joint driving is controlled based on the conversion relationship between the driving stroke and the rotation angle of the rotary joint, so as to realize the movement of the bucket according to the planned bucket trajectory, realize smooth action, reduce the impact generated by each structure of the excavator during the action process, and improve the service life of each structure of the excavator.

[0187] The third aspect of the present application provides a working machine comprising a control device for performing the steps of the bucket trajectory planning control method. In one embodiment, the working machine is an excavator.

[0188] The fourth aspect of the present application provides an electronic device comprising at least a processor and a memory, wherein:

[0189] The memory is configured to store a computer program.

[0190] The processor is configured to execute the computer program stored in the memory to implement the steps of the bucket trajectory planning control method.

[0191] The fifth aspect of the present application provides a computer readable storage medium storing a computer program, when the computer program is executed by a processor, the steps of the bucket trajectory planning control method are implemented.

[0192] Those skilled in the art can understand that all or part of the steps of the method for implementing the above-mentioned embodiments can be completed by programs instructing related hardware, the programs are stored in a storage medium, and the programs include a plurality of instructions for causing a single-chip microcomputer, a chip or a processor to execute all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes a U disk, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and various media capable of storing program codes.

[0193] The above describes optional embodiments of the present application in detail in combination with the drawings, but the embodiments of the present application are not limited to the specific details in the above-mentioned embodiments. Within the technical concept range of the embodiments of the present application, the technical solutions of the embodiments of the present application can be subjected to various simple modifications, and these simple modifications all belong to the protection range of the embodiments of the present application. In addition, it should be noted that each specific technical feature described in the above-mentioned specific embodiments can be combined in any appropriate manner without contradiction. In order to avoid unnecessary repetition, the embodiments of the present application do not further describe various possible combination manners.

[0194] In addition, various different embodiments of the present application can also be combined in any manner, as long as it does not deviate from the idea of the embodiments of the present application, it should also be considered as the disclosed content of the embodiments of the present application.

Claims

1. A bucket trajectory planning and control method, characterized in that: The bucket trajectory planning and control method includes: Get action signal; planning a bucket trajectory according to the motion signal, wherein the bucket trajectory includes a rotation angle trajectory of at least one rotation joint; According to the bucket trajectory and the conversion relationship between the corresponding rotary joint drive stroke and the rotation angle, the corresponding rotary joint drive is controlled to control the bucket to move according to the bucket trajectory; The bucket trajectory planning according to the motion signal includes: Determine the rotation angle of the rotary joint according to the motion signal; Construct the seventh-order polynomial curve of the revolute joint; The rotation angle trajectory of the rotation joint is planned according to the rotation angle and the seventh-order polynomial curve, and the rotation angle trajectory of at least one rotation joint constitutes the bucket trajectory.

2. The bucket trajectory planning and control method according to claim 1, characterized in that: The rotation angle trajectory of the revolute joint is planned based on the rotation angle and the seventh-order polynomial curve, including: Configure the length of the planning time period; Planning a rotation angle trajectory according to the length of the planned time period and the rotation angle; generating a rotation angular velocity trajectory, a rotation angular acceleration trajectory, and a rotation angular acceleration trajectory according to the rotation angle trajectory and the seventh-order polynomial curve; Determine the motion range corresponding to the rotation angle, rotation angular velocity, rotation angular acceleration and rotation angular jerk according to the dynamic parameters of the joint; Determining the rotation angle trajectory, the rotation angular velocity trajectory, the rotation angular acceleration trajectory, and the rotation angular acceleration trajectory according to the motion ranges corresponding to the rotation angle, the rotation angular velocity, the rotation angular acceleration, and the rotation angular jerk, respectively; If any trajectory contains a value exceeding the corresponding motion range, the above steps are repeated until a rotation angle trajectory is planned in which the rotation angle, rotation angular velocity, rotation angular acceleration, and rotation angular jerk all conform to their respective corresponding motion ranges.

3. The bucket trajectory planning and control method according to claim 1, characterized in that: The conversion relationship between the joint driving stroke and the rotation angle includes: the conversion relationship between the rotary joint driving stroke and the rotation angle; The conversion relationship between the driving stroke of the rotary joint and the rotation angle is established as follows: Get the number of teeth on the motor output shaft gear and the number of teeth on the ring gear of the rotary joint; Calculate the transmission ratio of the rotary joint gear and the ring gear based on the number of gear teeth and the number of ring gear teeth; According to the calculation relationship that the rotation angle of the rotary drive motor is proportional to the rotary rotation angle, the conversion relationship between the rotation angle of the rotary drive motor and the rotary rotation angle is calculated in combination with the transmission ratio.

4. The bucket trajectory planning and control method according to claim 1, characterized in that: The conversion relationship between the joint driving stroke and the rotation angle includes: the conversion relationship between the boom driving cylinder stroke and the boom joint rotation angle; The conversion relationship between the boom drive cylinder stroke and the boom joint rotation angle is established as follows: Get the structural parameters of the boom joint; Constructing a boom joint geometric structure diagram according to boom joint structural parameters; Determine the first calculation formula of the boom drive cylinder stroke based on the cosine theorem according to the boom joint structural parameters and the boom joint geometric structure diagram; Determining a second calculation formula representing a corresponding angular geometric relationship between a boom rotation angle and a boom drive cylinder stroke based on boom joint structural parameters and a boom joint geometric diagram; A conversion relationship between the boom drive cylinder stroke and the boom joint rotation angle is constructed according to the first calculation formula and the second calculation formula.

5. The bucket trajectory planning and control method according to claim 1, characterized in that: The conversion relationship between the joint driving stroke and the rotation angle includes: the conversion relationship between the stroke of the bucket rod driving cylinder and the rotation angle of the bucket rod joint; The conversion relationship between the stroke of the boom drive cylinder and the rotation angle of the boom joint is established as follows: Get the stick joint structural parameters; Constructing a bucket arm joint geometric structure diagram according to the bucket arm joint structural parameters; The third calculation formula of the bucket arm driving cylinder stroke based on the cosine theorem is determined according to the bucket arm joint structural parameters and the bucket arm joint geometric structure diagram; Determining a fourth calculation formula representing the corresponding angular geometric relationship between the bucket arm rotation angle and the bucket arm drive cylinder stroke based on the bucket arm joint structural parameters and the bucket arm joint geometric structure diagram; The conversion relationship between the arm drive cylinder stroke and the arm joint rotation angle is constructed according to the third calculation formula and the fourth calculation formula.

6. The bucket trajectory planning and control method according to claim 1, characterized in that: The conversion relationship between the joint driving stroke and the rotation angle includes: the conversion relationship between the bucket driving cylinder stroke and the bucket joint rotation angle; The conversion relationship between the bucket drive cylinder stroke and the bucket joint rotation angle is established as follows: Get bucket joint structure parameters; Constructing bucket joint geometric structure diagram according to bucket joint structural parameters; Determine the fifth calculation formula of bucket drive cylinder stroke based on the cosine theorem according to bucket joint structural parameters and bucket joint geometric structure diagram; Determining a sixth calculation formula representing the corresponding angular geometric relationship between the bucket rotation angle and the bucket drive cylinder stroke based on the bucket joint structural parameters and the bucket joint geometric structure diagram; A conversion relationship between the bucket drive cylinder stroke and the bucket joint rotation angle is constructed according to the fifth calculation formula and the sixth calculation formula.

7. A bucket trajectory planning device, characterized in that: The bucket trajectory planning device comprises: A signal acquisition unit, configured to acquire an action signal; a bucket trajectory planning unit, configured to plan a bucket trajectory according to the motion signal, wherein the bucket trajectory includes a rotation angle trajectory of at least one rotation joint; A control unit, configured to control the drive of the corresponding rotary joint according to the bucket trajectory and the conversion relationship between the drive stroke and the rotation angle of the corresponding rotary joint, so as to control the bucket to move according to the bucket trajectory; The bucket trajectory planning according to the motion signal includes: Determine the rotation angle of the rotary joint according to the motion signal; Construct the seventh-order polynomial curve of the revolute joint; The rotation angle trajectory of the rotation joint is planned according to the rotation angle and the seventh-order polynomial curve, and the rotation angle trajectory of at least one rotation joint constitutes the bucket trajectory.

8. A working machine, characterized in that: The invention comprises a control device, wherein the control device is used to execute the steps of the bucket trajectory planning control method according to any one of claims 1 to 6.

9. An electronic device, characterized in that: The electronic device includes at least a processor and a memory, wherein: The memory is used to store computer programs; The processor is configured to execute a computer program stored in the memory to implement the steps of the bucket trajectory planning and control method according to any one of claims 1 to 6.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the bucket trajectory planning and control method according to any one of claims 1 to 6 are implemented.

Images