Motion control method and system for double triangular drill arms, electronic equipment and storage medium

Abstract

The invention discloses a motion control method and system for a double-triangle drill boom, electronic equipment and a storage medium. According to the motion control method for the double-triangle drill boom, decoupling calculation is carried out based on an angle target value of a coupling joint, and virtual displacement target values of two supporting cylinders of the coupling joint can be determined; mutual influence between two joints with a coupling relation in the movement process is eliminated, the movement control precision of each joint is improved, accordingly, the positioning precision, construction precision and quality of the tail end of the drill boom are improved, a synchronous control mechanism is adopted for controlling the joints to move synchronously, the movement time of the tail end of the drill boom is greatly shortened, and the construction efficiency is improved. And the construction efficiency is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of tunnel and mine construction, and in particular to a motion control method and system of a double-triangle drill arm, electronic equipment, and a computer-readable storage medium. Background Art

[0002] At present, in the construction of small tunnels and mine tunnels in China, mechanical equipment with a double-triangle drill arm structure is often used for construction because of its advantages of simple structure, reliability and lightness. The motion control of the existing double-triangle drill arm usually adopts manual control. However, due to the mutual influence of its coupled joints, the operation is complicated and the labor intensity is high. The construction efficiency and quality are heavily dependent on the operator's experience and subjective judgment. With the development of the kinematic equations and motion control of the triangle arm, the automatic control technology of the double-triangle drill arm has gradually been widely used. It mainly controls the motion by establishing a three-dimensional model of the double-triangle drill arm and performing kinematic simulation through a virtual prototype. Although the automatic control technology of the double-triangle drill arm improves the shortcomings of manual control, it still has the following problems:

[0003] 1) Due to the mutual influence between the coupled joints, it is difficult to achieve high-precision synchronous motion by controlling each joint individually. This mutual influence leads to an increase in the positioning error at the end of the drill arm, thus affecting the accuracy and quality of the construction.

[0004] 2) Each joint moves in sequence, and the movement time of the end of the drill arm is relatively long, which affects the construction efficiency.

[0005] 3) During joint movement, speed state switching will cause jitter and instability in movement, affecting the overall construction safety. Summary of the invention

[0006] The present invention provides a motion control method and system for a double-triangle drill arm, an electronic device, and a computer-readable storage medium, which can not only improve the positioning accuracy, construction accuracy and quality of the drill arm end, but also control the synchronous movement of each joint, greatly shortening the movement time of the drill arm end and improving construction efficiency.

[0007] According to one aspect of the present invention, a motion control method for a double-delta drill arm is provided, comprising the following contents:

[0008] Obtain the angle target value and displacement target value of each joint when the end of the drill arm moves to the target position;

[0009] Decoupling calculation is performed based on the angle target value of the coupling joint to obtain the virtual displacement target values ​​of the two cylinders of the coupling joint;

[0010] According to the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint, a synchronous control mechanism is used to control the synchronous motion of each joint.

[0011] Furthermore, the process of performing decoupling calculation based on the angle target value of the coupling joint to obtain the virtual displacement target values ​​of the two cylinders of the coupling joint includes the following contents:

[0012] Establish the forward kinematics model of the coupled joints;

[0013] Get the initial angle value of the coupling joint;

[0014] Based on the forward kinematics model of the coupling joint, the initial angle value of the coupling joint and the Jacobian matrix, decoupling calculation is performed to obtain the virtual displacement target values ​​of the two cylinders of the coupling joint.

[0015] Furthermore, the process of using a synchronous control mechanism to perform synchronous motion control on each joint according to the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint includes the following:

[0016] Set a unified time window;

[0017] For uncoupled joints, the S-type function is used to interpolate the angle target value and displacement target value of the uncoupled joint, and the angle acceleration and displacement acceleration of the uncoupled joint in the time window are calculated;

[0018] For the coupling joint, the time window is divided into three stages: uniform acceleration, uniform speed and uniform deceleration, and the virtual displacement acceleration of the coupling joint in each stage is calculated according to the virtual displacement target values ​​of the two cylinders of the coupling joint.

[0019] Based on the angular acceleration and displacement acceleration of the uncoupled joint and the virtual displacement acceleration of the coupled joint, the angular thrust target value and displacement thrust target value of the uncoupled joint and the virtual displacement thrust target value of the coupled joint are calculated respectively;

[0020] Based on each thrust target value, each joint is controlled to move synchronously within a time window.

[0021] Furthermore, the angular acceleration and displacement acceleration of the uncoupled joint in the time window are calculated based on the following formula:

[0022] a θ (t) = (θ d -θ 0 )×S″(t,T)

[0023] a X (t) = (X d -X 0)×S″(t,T)

[0024] Among them, a θ (t) represents the angular acceleration of the uncoupled joint in the time window, a X (t) represents the displacement acceleration of the uncoupled joint in the time window, θ d and X d They represent the angle target value and displacement target value of the uncoupled joint, θ 0 and X 0 They represent the initial angle value and displacement value of the uncoupled joint respectively, S″(t,T) represents the second-order derivative of the S-type function S(t,T), t represents time, and T represents the length of the time window.

[0025] Furthermore, the virtual displacement acceleration of the coupling joint at each stage is calculated based on the following formula:

[0026]

[0027] Among them, a x (t) represents the virtual displacement acceleration of the coupling joint, x d and x 0 They represent the virtual displacement target value and the virtual displacement initial value of the two cylinders of the coupling joint, respectively. 0 ,t 1 ,t 2 and t 3 They represent the starting time point of the time window T, the starting time point of the uniform motion, the ending time point of the uniform motion and the ending time point of the time window T, respectively. 1 Denotes the duration of the uniform acceleration phase, Δt 1 =t 1 -t 0 , Δt 2 Indicates the duration of the uniform deceleration phase, Δt 2 =t 3 -t 2 .

[0028] Furthermore, the angle thrust target value and displacement thrust target value of the uncoupled joint and the virtual displacement thrust target value of the coupled joint are calculated based on the following formula:

[0029]

[0030] Among them, F θ (t) represents the target value of the angle thrust of the uncoupled joint, F X (t) represents the displacement thrust target value of the uncoupled joint, F x (t) represents the target value of the virtual displacement thrust of the coupling joint, F f1 、Ff2 and F f3 Indicates the resistance during joint movement, m 1 、m 2 and m 3 represents the theoretical mass of the joint drive part, a θ (t) represents the angular acceleration of the uncoupled joint, a X (t) represents the displacement acceleration of the uncoupled joint, a x (t) represents the virtual displacement acceleration of the coupling joint, k 1 , and They represent the angle deviation adjustment coefficient, angular velocity deviation adjustment coefficient and angular acceleration deviation adjustment coefficient respectively, k 2 , and They represent displacement deviation adjustment coefficient, displacement velocity deviation adjustment coefficient and displacement acceleration deviation adjustment coefficient respectively, k 3 , They respectively represent the virtual displacement deviation adjustment coefficient, the virtual displacement velocity deviation adjustment coefficient and the virtual displacement acceleration deviation adjustment coefficient.

[0031] Furthermore, the following contents are also included:

[0032] The real-time angle values ​​and displacement values ​​of the uncoupled joints and the real-time angle values ​​of the coupled joints are collected, and decoupling calculation is performed based on the real-time angle values ​​of the coupled joints to obtain the real-time virtual displacement values ​​of the two cylinders of the coupled joints. According to the real-time angle values ​​and displacement values ​​of the uncoupled joints and the real-time virtual displacement values ​​of the two cylinders of the coupled joints, the synchronous motion control process of each joint is adaptively adjusted.

[0033] In addition, the present invention also provides a motion control system for a double-triangle drill arm, comprising:

[0034] A target value acquisition module is used to obtain the angle target value and displacement target value of each joint when the end of the drill arm moves to the target position;

[0035] A decoupling calculation module is used to perform decoupling calculation based on the angle target value of the coupling joint to obtain the virtual displacement target value of the two cylinders of the coupling joint;

[0036] The synchronous motion control module is used to perform synchronous motion control on each joint using a synchronous control mechanism according to the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint.

[0037] In addition, the present invention also provides an electronic device, including a processor and a memory, wherein the memory stores a computer program, and the processor executes the steps of the above method by calling the computer program stored in the memory.

[0038] In addition, the present invention also provides a computer-readable storage medium for storing a computer program for motion control of a double-delta drill arm, wherein the computer program executes the steps of the method described above when running on a computer.

[0039] The present invention has the following beneficial effects:

[0040] The motion control method of the double-triangle drill arm of the present invention can determine the virtual displacement target values ​​of the two cylinders of the coupling joint by performing decoupling calculation based on the angle target value of the coupling joint, eliminates the mutual influence between the two joints in a coupling relationship during the motion process, improves the motion control accuracy of each joint, thereby improving the positioning accuracy, construction accuracy and quality of the drill arm end, and adopts a synchronous control mechanism to control the synchronous movement of each joint, which greatly shortens the movement time of the drill arm end and improves the construction efficiency.

[0041] In addition, the motion control system of the double-triangle drill arm of the present invention also has the above advantages.

[0042] In addition to the above-described purposes, features and advantages, the present invention has other purposes, features and advantages. The present invention will be further described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The drawings constituting a part of this application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0044] Figure 1 It is a schematic diagram of arranging sensors on a double-triangle drill arm according to the present invention;

[0045] Figure 2 It is a flow chart of a motion control method of a double-triangle drill arm according to a preferred embodiment of the present invention;

[0046] Figure 3 yes Figure 2 Schematic diagram of the sub-process of step S2;

[0047] Figure 4 yes Figure 2 Schematic diagram of the sub-process of step S3;

[0048] Figure 5 is another flow chart of the motion control method of the double-triangle drill arm according to the preferred embodiment of the present invention;

[0049] Figure 6 It is a schematic diagram of the module structure of the motion control system of the double-triangle drill arm according to another embodiment of the present invention.

[0050] Description of Reference Numerals

[0051] 1. Trolley; 2. Double triangle drilling arm; 3. Cab; 4. Angle sensor; 5. Displacement sensor; 6. Pull rope sensor; 7. Boom controller; 8. Computer. DETAILED DESCRIPTION

[0052] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0053] It is understood that in the present invention, Figure 1 As shown, the double triangular drilling arm 2 on the trolley 1 generally includes a boom sway joint, a boom pitch joint, a propulsion beam sway joint, a propulsion beam pitch joint, a propulsion beam rotation joint, a propulsion beam angle adjustment joint, a boom telescopic joint and a propulsion beam telescopic joint, wherein the boom sway joint, the boom pitch joint, the propulsion beam sway joint, the propulsion beam pitch joint, the propulsion beam rotation joint and the propulsion beam angle adjustment joint are provided with an angle sensor 4, which can detect the angle value of each joint in real time, the boom telescopic joint is provided with a displacement sensor 5, which can detect the displacement value of the boom telescopic joint in real time, and the propulsion beam telescopic joint is provided with a pull rope sensor 6, which can detect the displacement value of the propulsion beam telescopic joint in real time. Among them, the boom sway joint and the boom pitch joint are coupled joints, and the propulsion beam sway joint and the propulsion beam pitch joint are also coupled joints, that is, the two joints affect each other during the movement. The angle data and displacement data collected by the angle sensor 4, the displacement sensor 5 and the rope sensor 6 are transmitted to the computer 8 in the cab 3 for kinematic calculation and the calculated values ​​are sent to the boom controller 7, which can control the automatic, synchronous, smooth and accurate movement of each joint of the double triangle drilling arm.

[0054] Reference Figure 2 The preferred embodiment of the present application provides a motion control method for a double-triangle drill arm, comprising the following contents:

[0055] Step S1: Obtaining the angle target value and displacement target value of each joint when the end of the drill arm moves to the target position;

[0056] Step S2: performing decoupling calculation based on the angle target value of the coupling joint to obtain the virtual displacement target values ​​of the two cylinders of the coupling joint;

[0057] Step S3: Based on the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint, a synchronous control mechanism is used to perform synchronous motion control on each joint.

[0058] It can be understood that the motion control method of the double triangle drill arm of this embodiment first obtains the angle target value and displacement target value of each joint when the end of the drill arm moves to the target position, then performs decoupling calculation based on the angle target value of the coupled joint to obtain the virtual displacement target value of the two cylinders of the coupled joint, and finally, according to the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint, a synchronous control mechanism is used to perform synchronous motion control on each joint. By performing decoupling calculation based on the angle target value of the coupled joint, the virtual displacement target value of the two cylinders of the coupled joint can be determined, eliminating the mutual influence between the two joints in a coupled relationship during the motion process, improving the motion control accuracy of each joint, thereby improving the positioning accuracy, construction accuracy and quality of the end of the drill arm, and adopting a synchronous control mechanism to control the synchronous motion of each joint, which greatly shortens the movement time of the end of the drill arm and improves construction efficiency.

[0059] It can be understood that in step S1, according to the kinematic equation of the double-triangle drill arm, the angle target value and displacement target value of each joint when the end of the drill arm moves to the target position can be calculated, wherein the kinematic equation of the double-triangle drill arm belongs to the existing formula, and the process of calculating the angle target value and displacement target value of each joint is also the existing technology, which will not be repeated here.

[0060] It can be understood that the angle target value and displacement target value of each joint are calculated in step S1. For uncoupled joints, motion control can be performed directly based on their angle target value and displacement target value. For coupled joints, such as the boom yaw joint and the boom pitch joint, the propeller beam yaw joint and the propeller beam pitch joint, since there is a coupling relationship between the two joints, the movement of one joint will affect the posture of the other joint. Therefore, it is necessary to perform decoupling calculation based on the angle target value of the coupled joint to obtain the virtual displacement target value of the two cylinders of the coupled joint. Figure 3 As shown, the process of performing decoupling calculation based on the angle target value of the coupling joint to obtain the virtual displacement target value of the two cylinders of the coupling joint specifically includes the following contents:

[0061] Step S21: establishing a forward kinematics model of the coupling joint;

[0062] Step S22: Obtaining the initial angle value of the coupling joint;

[0063] Step S23: performing decoupling calculation based on the forward kinematics model of the coupling joint, the initial angle value of the coupling joint and the Jacobian matrix to obtain the virtual displacement target values ​​of the two cylinders of the coupling joint.

[0064] Specifically, the position and posture of the coupling joints in the double triangle drill arm can be expressed by the homogeneous change matrix Q as follows: Q = f(θ 1 ,θ 2), where f(θ 1 ,θ 2 ) represents the forward kinematic model of the coupling joint, that is, according to the yaw angle θ of the yaw joint 1 and the pitch angle θ of the pitch joint 2 Calculate the pose of the coupling joint, where the coupling joint includes the yaw joint and the pitch joint. Then, obtain the initial angle value of the coupling joint, assuming that θ 10 =θ 20 =0°, where θ 10 represents the initial value of the yaw angle of the yaw joint, θ 20 Represents the initial value of the pitch angle of the pitch joint. Next, decoupling control is performed based on the Jacobian matrix, and the Jacobian matrix J can be expressed as: The target angle difference Δθ of the coupled joint d =Δθ d =[θ 1d -θ 10 ,θ 2d -θ 20 ] T =[θ 1d ,θ 2d ] T , where θ 1d and θ 2d They represent the target yaw angle and pitch angle of the coupling joint respectively. Then the target virtual displacement of the two cylinders of the coupling joint is: d =J -1 ×Δθ d , where x d =[x 1d ,x 2d ] T , x 1d and x 2 d represents the virtual displacement target value of the yaw joint cylinder and the virtual displacement target value of the pitch joint cylinder respectively.

[0065] It can be understood that the present invention uses the Jacobian matrix for decoupling calculation, which can solve the angle target value of the coupling joint into the virtual displacement target value of the two cylinders. Subsequently, the actions of the two cylinders can be controlled separately based on the virtual displacement target values ​​of the two cylinders, thereby eliminating the mutual influence between the two joints in a coupling relationship during the movement process, and can improve the motion control accuracy of each joint in the coupling joint, thereby improving the positioning accuracy, construction accuracy and quality of the drill arm end.

[0066] In addition, in step S2, the real-time value of the angle of the coupling joint, that is, the real-time value of the yaw angle of the yaw joint θ 1 (t) and the real-time value of the pitch angle of the pitch joint θ 2(t), the decoupling calculation can also be performed based on the Jacobian matrix. The real-time angle difference Δθ(t) of the coupled joint is: Δθ(t) = [θ 1 (t)-θ 10 ,θ 2 (t)-θ 20 ] T =[θ 1 (t),θ 2 (t)] T , then the real-time virtual displacement value of the two cylinders of the coupling joint is: x(t) = J -1 ×Δθ(t), where x(t)=[x 1 (t),x 2 (t)] T , x 1 (t) and x 2 (t) represent the real-time value of the virtual displacement of the yaw joint cylinder and the real-time value of the virtual displacement of the pitch joint cylinder respectively.

[0067] It can be understood that in step S3, in order to achieve coordinated movement among various joints, the present invention introduces a synchronous control mechanism to ensure that all joints move toward the target value in the same time period to avoid instability caused by asynchronous movement. Figure 4 As shown, the process of using a synchronous control mechanism to perform synchronous motion control on each joint according to the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint includes the following:

[0068] Step S31: setting a unified time window;

[0069] Step S32: for the uncoupled joint, interpolation is performed using an S-type function based on the angle target value and the displacement target value of the uncoupled joint, and the angle acceleration and displacement acceleration of the uncoupled joint moving within the time window are calculated;

[0070] Step S33: for the coupling joint, the time window is divided into three stages: uniform acceleration, uniform speed and uniform deceleration, and the virtual displacement acceleration of the coupling joint in each stage is calculated according to the virtual displacement target values ​​of the two cylinders of the coupling joint;

[0071] Step S34: based on the angular acceleration and displacement acceleration of the uncoupled joint and the virtual displacement acceleration of the coupled joint, respectively calculating the angular thrust target value and displacement thrust target value of the uncoupled joint and the virtual displacement thrust target value of the coupled joint;

[0072] Step S35: Controlling each joint to move synchronously within the time window based on each thrust target value.

[0073] Specifically, a unified time window T is set first. The length of the time window can be set according to the need for control accuracy and is not specifically limited here. For non-coupled joints, an S-type function is used for interpolation. The definition of the S-type function is as follows: S(t, T) = a(t / T) 5 -b(t / T) 4 +c(t / T) 3 , a, b, c are constants, and the function needs to satisfy S(0) = 0, S(T) = 1, S′(0) = S′(T) = 0, S″(0) = S″(T) = 0, so the angular acceleration and displacement acceleration of the uncoupled joint in the time window can be calculated based on the following formula:

[0074] a θ (t) = (θ d -θ 0 )×S″(t,T)

[0075] a X (t) = (X d -X 0 )×S″(t,T)

[0076] Among them, a θ (t) represents the angular acceleration of the uncoupled joint in the time window, a X (t) represents the displacement acceleration of the uncoupled joint in the time window, θ d and X d They represent the angle target value and displacement target value of the uncoupled joint, θ 0 and X 0 They represent the initial angle and displacement of the uncoupled joint, θ 0 =θ(t 0 ), X 0 =X(t 0 ), S″(t,T) represents the second-order derivative of the sigmoid function S(t,T),

[0077]

[0078] t represents time, and T represents the length of the time window.

[0079] For the coupling joint, the time window T is divided into three stages: uniform acceleration, uniform speed, and uniform deceleration. The virtual displacement acceleration of the coupling joint in each stage is calculated based on the virtual displacement target values ​​of the two cylinders of the coupling joint. Specifically, the virtual displacement acceleration of the coupling joint in each stage is calculated based on the following formula:

[0080]

[0081] Among them, a x(t) represents the virtual displacement acceleration of the coupling joint, x d and x 0 They represent the virtual displacement target value and the virtual displacement initial value of the two cylinders of the coupling joint, respectively. 0 =x(t 0 ), t 0 ,t 1 ,t 2 and t 3 They represent the starting time point of the time window T, the starting time point of the uniform motion, the ending time point of the uniform motion and the ending time point of the time window T, respectively. 1 Denotes the duration of the uniform acceleration phase, Δt 1 =t 1 -t 0 , Δt 2 Indicates the duration of the uniform deceleration phase, Δt 2 =t 3 -t 2 .

[0082] Then, the angle thrust target value and displacement thrust target value of the uncoupled joint and the virtual displacement thrust target value of the coupled joint are calculated based on the following formula:

[0083]

[0084] Among them, F θ (t) represents the target value of the angle thrust of the uncoupled joint, F X (t) represents the displacement thrust target value of the uncoupled joint, F x (t) represents the target value of the virtual displacement thrust of the coupling joint, F f1 、F f2 and F f3 Indicates the resistance during joint movement, m 1 、m 2 and m 3 represents the theoretical mass of the joint drive part, a θ (t) represents the angular acceleration of the uncoupled joint, a X (t) represents the displacement acceleration of the uncoupled joint, a x (t) represents the virtual displacement acceleration of the coupling joint, k 1 , and They represent the angle deviation adjustment coefficient, angular velocity deviation adjustment coefficient and angular acceleration deviation adjustment coefficient respectively, k 2 , and They represent displacement deviation adjustment coefficient, displacement velocity deviation adjustment coefficient and displacement acceleration deviation adjustment coefficient respectively, k 3 , They respectively represent the virtual displacement deviation adjustment coefficient, the virtual displacement velocity deviation adjustment coefficient and the virtual displacement acceleration deviation adjustment coefficient.

[0085] Finally, after the computer sends each thrust target value to the arm controller, the arm controller converts each thrust target value into corresponding valve control parameters, thereby performing synchronous motion control on each joint.

[0086] It can be understood that the present invention makes the motion trajectory of the uncoupled joint smoother by interpolating the S-type function, and makes the motion trajectory of the coupled joint smoother by dividing the time window into three segments, so that the movement of the double triangle drill arm is more stable, ensuring the construction safety, and the coupled joint also adopts synchronous control, further improving the positioning accuracy of the end of the drill arm. In addition, the synchronous movement of each joint is controlled within a unified time window, which greatly reduces the arm moving time and improves the construction efficiency.

[0087] Optional, such as Figure 5 As shown, the motion control method of the double triangle drill arm also includes the following contents:

[0088] Step S4: Collect the real-time angle values ​​and displacement values ​​of the uncoupled joints and the real-time angle values ​​of the coupled joints, perform decoupling calculations based on the real-time angle values ​​of the coupled joints, obtain the real-time virtual displacement values ​​of the two cylinders of the coupled joints, and adaptively adjust the synchronous motion control process of each joint according to the real-time angle values ​​and displacement values ​​of the uncoupled joints and the real-time virtual displacement values ​​of the two cylinders of the coupled joints.

[0089] Specifically, by collecting the real-time angle values ​​and displacement values ​​of uncoupled joints and the real-time angle values ​​of coupled joints, the instantaneous velocity of each joint can be calculated by digital differentiation using the collected real-time angle / displacement data, and then the velocity data can be digitally differentiated to calculate the instantaneous acceleration of each joint. The angle, displacement, velocity, and acceleration data monitored in real time are then comprehensively analyzed to identify factors that may cause system instability or reduced responsiveness, such as excessive angle / displacement deviations, velocity / acceleration outside the normal range, etc. The synchronous motion control process of each joint is adaptively adjusted to improve stability and responsiveness in complex environments. Among them, the process of decoupling calculations based on the real-time angle values ​​of the coupled joints to obtain the real-time virtual displacement values ​​of the two cylinders of the coupled joints has been described in detail in the above step S2 and will not be repeated here.

[0090] The process of adaptively adjusting the synchronous motion control process of each joint according to the angle deviation and the displacement deviation includes the following:

[0091] First, collect the current angle real-time value θ(t) and the current displacement real-time value X(t) of the uncoupled joint, and obtain the current virtual displacement real-time value x(t) of the two cylinders of the coupled joint obtained by decoupling calculation;

[0092] For uncoupled joints, the current theoretical angle and current theoretical displacement are calculated based on the following formula:

[0093] θ c (t) = θ 0 +(θ d -θ 0 )*S(t,T)

[0094] X c (t) = X 0 +(X d -X 0 )*S(t,T)

[0095] S(t, T) = a(t / T) 5 -b(t / T) 4 +c(t / T) 3

[0096] Among them, θ c (t) represents the current theoretical angle of the uncoupled joint, X c (t) represents the current theoretical displacement of the uncoupled joint;

[0097] For the coupled joint, the current theoretical virtual displacement x is calculated based on the following formula: c (t):

[0098]

[0099] Then, the current angle deviation value and current displacement deviation value of the uncoupled joint during the motion of each joint, as well as the current virtual displacement deviation value of the coupled joint are calculated. The calculation formula is as follows:

[0100] Δθ(t)=θ c (t)-θ(t)

[0101] ΔX(t)=X c (txt)

[0102] Δx(t)=x c (txt)

[0103] Wherein, Δθ(t) represents the current angle deviation value of the uncoupled joint, ΔX(t) represents the current displacement deviation value of the uncoupled joint, and Δx(t) represents the current virtual displacement deviation value of the coupled joint;

[0104] Finally, adjust the angle deviation adjustment coefficient k of each joint according to the deviation value1 , displacement deviation adjustment coefficient k 2 , Virtual displacement deviation adjustment coefficient k 3 If the deviation value is positive, the deviation adjustment coefficient automatically increases based on the reference value. If the deviation value is negative, the deviation adjustment coefficient automatically decreases based on the reference value. For example, if Δθ(t)>0, then k 1 increases, if Δθ(t)<0, then k 1 decreases; if ΔX(t)>0, then k 2 Increases, if ΔX(t)<0, then k 2 decreases; if Δx(t)>0, then k 3 Increases, if Δx(t)<0, then k 3 Reduce.

[0105] Similarly, the process of adaptively adjusting the synchronous motion control process of each joint according to the angular velocity deviation and the displacement velocity deviation includes the following:

[0106] First, differential calculation is performed based on the collected real-time angle value and real-time displacement value to obtain the current real-time angular velocity value ω(t) and the current real-time displacement velocity value v of the uncoupled joint. X (t), and the current virtual displacement velocity real-time value v of the two cylinders of the coupling joint x (t);

[0107] For uncoupled joints, the current theoretical angular velocity ω is calculated based on the following formula c (t) and the current theoretical displacement velocity v Xc (t):

[0108] ω c (t) = (θ d -θ 0 )*S'(t,T)

[0109] v Xc (t) = (X d -X 0 )*S'(t,T)

[0110]

[0111] For the coupled joint, the current theoretical virtual displacement velocity v is calculated based on the following formula: xc (t):

[0112]

[0113] Then, the current angular velocity deviation Δω(t) and the current displacement velocity deviation Δv of the uncoupled joint during the motion of each joint are calculated. X (t), and the current virtual displacement velocity deviation value Δv of the coupled jointx (t), the calculation formula is as follows:

[0114] Δω(t)=ω c (t)-ω c (t)

[0115] Δv X (t) = v Xc (t)-v X (t)

[0116] Δv x (t) = v xc (t)-v x (t);

[0117] Finally, adjust the angular velocity deviation adjustment coefficient of each joint according to the deviation value Displacement speed deviation adjustment coefficient Virtual displacement speed deviation adjustment coefficient If the deviation value is positive, the deviation adjustment coefficient will automatically increase based on the reference value. If the deviation value is negative, the deviation adjustment coefficient will automatically decrease based on the reference value. For example, if Δω(t)>0, then Increases, if Δω(t)<0, then Decrease; if Δv X (t)>0, then Increase, if Δv X (t)<0, then Decrease; if Δv x (t)>0, then Increase, if Δv x (t)<0, then Reduce.

[0118] In addition, the process of adaptively adjusting the synchronous motion control process of each joint according to the angular acceleration deviation and the displacement acceleration deviation includes the following:

[0119] First, the real-time value of angular velocity and real-time value of displacement velocity are differentiated to obtain the current real-time value of angular acceleration a(t) and the current real-time value of displacement acceleration a of the uncoupled joint. X (t), and the current virtual displacement acceleration real-time value a of the two cylinders of the coupling joint x (t);

[0120] For uncoupled joints, the current theoretical angular acceleration a is calculated based on the following formula: c (t) and the current theoretical displacement acceleration a Xc (t):

[0121] a c (t) = (θ d -θ0 )×S″(t,T)

[0122] a Xc (t) = (X d -X 0 )×S″(t,T);

[0123] For the coupled joint, the current theoretical virtual displacement acceleration a is calculated based on the following formula: xc (t):

[0124]

[0125] Then, the current angular acceleration deviation value Δa(t) and the current displacement acceleration deviation value Δa of the uncoupled joint during the motion of each joint are calculated. X (t), and the current virtual displacement acceleration deviation value Δa of the coupled joint x (t), the calculation formula is as follows:

[0126]

[0127] Finally, adjust the angular acceleration deviation adjustment coefficient of each joint according to the deviation value Displacement acceleration deviation adjustment coefficient Virtual displacement acceleration deviation adjustment coefficient If the deviation value is positive, the deviation adjustment coefficient will automatically increase based on the reference value. If the deviation value is negative, the deviation adjustment coefficient will automatically decrease based on the reference value. For example, if Δa(t)>0, then Increases, if Δa(t)<0, then Decrease; if Δa X (t)>0, then Increase, if Δa X (t)<0, then Decrease; if Δa x (t)>0, then Increase, if Δa x (t)<0, then Reduce.

[0128] In addition, if Figure 6 As shown, another embodiment of the present invention further provides a motion control system for a double-triangle drill arm, preferably using the motion control method for the double-triangle drill arm as described above, comprising:

[0129] A target value acquisition module is used to obtain the angle target value and displacement target value of each joint when the end of the drill arm moves to the target position;

[0130] A decoupling calculation module is used to perform decoupling calculation based on the angle target value of the coupling joint to obtain the virtual displacement target value of the two cylinders of the coupling joint;

[0131] The synchronous motion control module is used to perform synchronous motion control on each joint using a synchronous control mechanism according to the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint.

[0132] It can be understood that the motion control system of the double triangle drill arm of the present invention first obtains the angle target value and displacement target value of each joint when the end of the drill arm moves to the target position, and then performs decoupling calculation based on the angle target value of the coupled joint to obtain the virtual displacement target value of the two cylinders of the coupled joint. Finally, according to the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint, a synchronous control mechanism is used to perform synchronous motion control on each joint. By performing decoupling calculation based on the angle target value of the coupled joint, the virtual displacement target value of the two cylinders of the coupled joint can be determined, eliminating the mutual influence between the two joints in a coupled relationship during the motion process, improving the motion control accuracy of each joint, thereby improving the positioning accuracy, construction accuracy and quality of the end of the drill arm, and adopting a synchronous control mechanism to control the synchronous motion of each joint, which greatly shortens the movement time of the end of the drill arm and improves construction efficiency.

[0133] In addition, the motion control system of the double delta drill arm also includes:

[0134] The adaptive adjustment module is used to collect the real-time angle values ​​and displacement values ​​of the uncoupled joints and the real-time angle values ​​of the coupled joints, perform decoupling calculations based on the real-time angle values ​​of the coupled joints, obtain the real-time virtual displacement values ​​of the two cylinders of the coupled joints, and adaptively adjust the synchronous motion control process of each joint according to the real-time angle values ​​and displacement values ​​of the uncoupled joints and the real-time virtual displacement values ​​of the two cylinders of the coupled joints.

[0135] It can be understood that the various modules of the system embodiment correspond to the various steps of the above method embodiment, so the specific working principles of each module are not repeated here, and the corresponding references can be made to the various steps of the above method embodiment.

[0136] In addition, another embodiment of the present invention further provides an electronic device, including a processor and a memory, wherein the memory stores a computer program, and the processor executes the steps of the above method by calling the computer program stored in the memory.

[0137] In addition, another embodiment of the present invention further provides a computer-readable storage medium for storing a computer program for motion control of a double-delta drill arm, wherein the computer program executes the steps of the method described above when running on a computer.

[0138] The general form of computer readable storage media includes: floppy disk, flexible disk, hard disk, magnetic tape, any other magnetic medium, CD-ROM, any other optical medium, punch cards, paper tape, any other physical medium with a pattern of holes, random access memory (RAM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), flash erasable programmable read-only memory (FLASH-EPROM), any other memory chip or cartridge, or any other medium that can be read by a computer. The instructions can further be transmitted or received by a transmission medium. The term transmission medium can include any tangible or intangible medium that can be used to store, encode or carry instructions for execution by a machine, and includes digital or analog communication signals or other intangible media that facilitate the communication of the above instructions. Transmission media include coaxial cables, copper wires and optical fibers, which include the wires of a bus used to transmit a computer data signal.

[0139] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of complete hardware embodiments, complete software embodiments, or embodiments in combination with software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code. The scheme in the embodiments of the present application can be implemented in various computer languages, for example, object-oriented programming language Java and literal scripting language JavaScript, etc.

[0140] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0141] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0142] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0143] Although the preferred embodiments of the present application have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications falling within the scope of the present application.

[0144] Obviously, those skilled in the art can make various changes and modifications to the present application without departing from the spirit and scope of the present application. Thus, if these modifications and variations of the present application fall within the scope of the claims of the present application and their equivalents, the present application is also intended to include these modifications and variations.

[0145] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A motion control method for a double-triangle drill arm, characterized in that: Includes the following: Obtain the angle target value and displacement target value of each joint when the end of the drill arm moves to the target position; Decoupling calculation is performed based on the angle target value of the coupling joint to obtain the virtual displacement target values ​​of the two cylinders of the coupling joint; According to the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint, a synchronous control mechanism is used to control the synchronous motion of each joint.

2. The motion control method of the double-triangle drill arm according to claim 1, characterized in that: The process of performing decoupling calculation based on the angle target value of the coupling joint to obtain the virtual displacement target values ​​of the two cylinders of the coupling joint includes the following contents: Establish the forward kinematics model of the coupled joints; Get the initial angle value of the coupling joint; Based on the forward kinematics model of the coupling joint, the initial angle value of the coupling joint and the Jacobian matrix, decoupling calculation is performed to obtain the virtual displacement target values ​​of the two cylinders of the coupling joint.

3. The motion control method of the double-triangle drill arm according to claim 1, characterized in that: The process of using a synchronous control mechanism to perform synchronous motion control on each joint according to the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint includes the following: Set a unified time window; For uncoupled joints, the S-type function is used to interpolate the angle target value and displacement target value of the uncoupled joint, and the angle acceleration and displacement acceleration of the uncoupled joint in the time window are calculated; For the coupling joint, the time window is divided into three stages: uniform acceleration, uniform speed and uniform deceleration, and the virtual displacement acceleration of the coupling joint in each stage is calculated according to the virtual displacement target values ​​of the two cylinders of the coupling joint. Based on the angular acceleration and displacement acceleration of the uncoupled joint and the virtual displacement acceleration of the coupled joint, the angular thrust target value and displacement thrust target value of the uncoupled joint and the virtual displacement thrust target value of the coupled joint are calculated respectively; Based on each thrust target value, each joint is controlled to move synchronously within a time window.

4. The motion control method of the double delta drill arm according to claim 3 is characterized in that: Based on the following formula The angular acceleration and displacement acceleration of the uncoupled joint in the time window are calculated: a θ (t)=(θ d -θ0)×S″(t,T) a X (t)=(X d -X0)×S″(t,T) Among them, a θ (t) represents the angular acceleration of the uncoupled joint in the time window, a X (t) represents the displacement acceleration of the uncoupled joint in the time window, θ d and X d denote the angle target value and displacement target value of the uncoupled joint, θ0 and X0 denote the angle initial value and displacement initial value of the uncoupled joint, S″(t,T) denotes the second-order derivative of the S-type function S(t,T), t denotes time, and T denotes the length of the time window.

5. The motion control method of the double-triangle drill arm according to claim 3, characterized in that: The virtual displacement acceleration of the coupling joint at each stage is calculated based on the following formula: Among them, a x (t) represents the virtual displacement acceleration of the coupling joint, x d and x0 represent the virtual displacement target value and the virtual displacement initial value of the two cylinders of the coupling joint respectively. t0, t1, t2 and t3 represent the starting time point of the time window T, the starting time point of the uniform motion, the ending time point of the uniform motion and the ending time point of the time window T respectively. Δt1 represents the time length of the uniform acceleration stage, Δt1=t1-t0. Δt2 represents the time length of the uniform deceleration stage, Δt2=t3-t2.

6. The motion control method of the double-triangle drill arm according to claim 3, characterized in that: The angle thrust target value and displacement thrust target value of the uncoupled joint and the virtual displacement thrust target value of the coupled joint are calculated based on the following formula: Among them, F θ (t) represents the target value of the angle thrust of the uncoupled joint, F X (t) represents the displacement thrust target value of the uncoupled joint, F x (t) represents the target value of the virtual displacement thrust of the coupling joint, F f1 、F f2 and F f3 represents the resistance during joint movement, m1, m2 and m3 represent the theoretical mass of the joint drive part, and a θ (t) represents the angular acceleration of the uncoupled joint, a X (t) represents the displacement acceleration of the uncoupled joint, a x (t) represents the virtual displacement acceleration of the coupling joint, k1, and They represent the angle deviation adjustment coefficient, angular velocity deviation adjustment coefficient and angular acceleration deviation adjustment coefficient respectively, k2, and They represent displacement deviation adjustment coefficient, displacement velocity deviation adjustment coefficient and displacement acceleration deviation adjustment coefficient respectively, k3, They respectively represent the virtual displacement deviation adjustment coefficient, the virtual displacement velocity deviation adjustment coefficient and the virtual displacement acceleration deviation adjustment coefficient.

7. The motion control method of the double-triangle drill arm according to claim 1, characterized in that: Also included are the following: The real-time angle values ​​and displacement values ​​of the uncoupled joints and the real-time angle values ​​of the coupled joints are collected, and decoupling calculation is performed based on the real-time angle values ​​of the coupled joints to obtain the real-time virtual displacement values ​​of the two cylinders of the coupled joints. According to the real-time angle values ​​and displacement values ​​of the uncoupled joints and the real-time virtual displacement values ​​of the two cylinders of the coupled joints, the synchronous motion control process of each joint is adaptively adjusted.

8. A motion control system for a double-triangle drill arm, characterized in that: include: A target value acquisition module is used to obtain the angle target value and displacement target value of each joint when the end of the drill arm moves to the target position; A decoupling calculation module is used to perform decoupling calculation based on the angle target value of the coupling joint to obtain the virtual displacement target value of the two cylinders of the coupling joint; The synchronous motion control module is used to perform synchronous motion control on each joint using a synchronous control mechanism according to the angle target value and displacement target value of the uncoupled joint and the virtual displacement target value of the two cylinders of the coupled joint.

9. An electronic device, characterized in that: The method comprises a processor and a memory, wherein the memory stores a computer program, and the processor executes the steps of the method according to any one of claims 1 to 7 by calling the computer program stored in the memory.

10. A computer-readable storage medium for storing a computer program for motion control of a double delta drill arm, characterized in that: When the computer program is run on a computer, the steps of the method according to any one of claims 1 to 7 are executed.

Images