Milking robot compliance force control method and system based on digital twinning

Abstract

The present application relates to the technical field of automatic milking, in particular to a milking robot compliance force control method and system based on digital twinning. The technical problem that the existing technology is difficult to accurately predict the damage to the equipment or the injury to the cows when the automatic cupping device fails, and it is difficult to distinguish the specific equipment that fails. The milking robot compliance force control method based on digital twinning provided by the present application calculates the theoretical tension value of the traction piece, the actual tension value of the traction piece and the estimated contact force applied on the milking cup through forward and reverse simulation, judges whether a failure has occurred according to the three, and adjusts the working state of the mechanical arm according to the failure type. Whether the electronic device fails or the mechanical structure fails, the accuracy of fault judgment can be improved according to the three, thereby improving the operation safety of the milking robot.

Description

Technical Field

[0001] This invention relates to the field of automated milking technology, and more specifically, to a method and system for controlling the compliance force of a milking robot based on digital twins. Background Technology

[0002] In dairy farming, milking robots can automatically identify and locate the cow's teats, and control the milking cups to automatically attach them to the teats. During this automatic cup-attaching process, the end effector of the milking robot needs to make precise and smooth physical contact with the cow's udder to complete the cup-attaching operation.

[0003] Currently, the primary method for measuring contact force is to integrate a six-dimensional force sensor at the end effector of a robotic arm. However, the humid and corrosive environment of dairy farms makes these force sensors susceptible to corrosion, resulting in short lifespans and frequent malfunctions. Another approach involves using flexible fingers in the end effector to achieve compliant force, but this makes it difficult to detect mechanical faults. Whether it's a sensor malfunction or a mechanical failure, any anomaly leading to incorrect force perception during cup-attaching operations could damage equipment or harm animals, posing a serious hidden safety risk. Summary of the Invention

[0004] The purpose of this invention is to provide a method and system for controlling the compliance force of a milking robot based on digital twins, which solves the technical problems in the prior art where it is difficult to accurately predict equipment damage or cow injury when automatic cupping equipment malfunctions, and it is also difficult to distinguish the specific equipment that malfunctions.

[0005] As a first aspect of the present invention, the present invention provides a compliance force control method for a milking robot based on digital twins. The control method is used to control a milking robot, the milking robot including a robotic arm, an end effector, and a milking cup; wherein, the end effector includes: a drive motor, a traction member, a first discrete block, and a second discrete block, the drive motor driving the traction member to move, so that the free end of the second discrete block rotates to change the angle between two adjacent second discrete blocks; The control method includes: The estimated contact force between the milking cup and the cow's udder is determined based on the angular displacement of the drive motor and the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block. The theoretical tension value of the traction component is determined based on the angular displacement of the drive motor and the estimated contact force. The actual pulling force of the traction component is calculated based on the driving current of the drive motor and the radius of the drive motor's transmission wheel. The working state of the robotic arm is adjusted according to the theoretical pulling force value of the traction component, the actual pulling force value of the traction component, and the estimated contact force.

[0006] In one embodiment of the present invention, adjusting the working state of the robotic arm based on the theoretical tension value of the traction member, the actual tension value of the traction member, and the estimated contact force includes: When the absolute deviation between the theoretical tension value and the actual tension value of the traction component is less than or equal to a preset deviation value, and the estimated contact force is less than the preset contact force, the robotic arm is controlled to perform the cup-attaching operation.

[0007] In one embodiment of the present invention, the step of adjusting the working state of the robotic arm based on the theoretical tension value of the traction member, the actual tension value of the traction member, and the estimated contact force further includes: When the absolute deviation between the theoretical tension value and the actual tension value of the traction component is less than or equal to the preset deviation value, and the estimated contact force is greater than or equal to the preset contact force, the robotic arm is controlled to stop the cup-fitting operation.

[0008] In one embodiment of the present invention, the step of adjusting the working state of the robotic arm based on the theoretical tension value of the traction member, the actual tension value of the traction member, and the estimated contact force further includes: When the absolute deviation between the theoretical pulling force value and the actual pulling force value of the traction component is greater than the preset deviation value, the robotic arm is controlled to return to the preset safe position along a preset motion trajectory. The fault type is determined based on the actual tension value of the traction component and the theoretical tension value of the traction component.

[0009] In one embodiment of the present invention, determining the fault type based on the actual tension value of the traction component and the theoretical tension value of the traction component includes: When the difference between the actual tension value of the traction component and the theoretical tension value of the traction component is continuously greater than a first preset difference and the duration is greater than a first preset duration, sensor fault information is generated.

[0010] In one embodiment of the present invention, determining the fault type based on the actual tension value of the traction component and the theoretical tension value of the traction component includes: When the theoretical tension value of the traction component is continuously greater than the actual tension value of the traction component, and the time duration is greater than the second preset time duration, mechanical transmission path fault information is generated.

[0011] In one embodiment of the present invention, determining the estimated contact force between the milking cup and the cow's udder based on the angular displacement of the drive motor and the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block includes: The total displacement of the traction component is calculated based on the angular displacement of the drive motor. The displacement increment of the traction member at the equivalent hinge is determined based on the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block. The equivalent hinge is the connection point between the second discrete block furthest from the milking cup and the adjacent second discrete block. The total displacement of the traction component of the drive motor and the displacement increment are input into a pre-built inverse model for simulation calculation to obtain the estimated contact force between the milking cup and the cow's udder.

[0012] In one embodiment of the present invention, determining the theoretical tension value of the traction member based on the angular displacement of the drive motor and the estimated contact force includes: The estimated contact force and the total displacement of the traction component are input into a pre-built forward model for simulation calculation to obtain the theoretical tensile force value of the traction component.

[0013] In one embodiment of the present invention, the end effector further includes: a spring plate and an elastic sheet disposed below the spring plate; wherein, strain gauges are disposed on the elastic sheet, and the bottoms of a plurality of second discrete blocks are fixed to the spring plate; The control method further includes: The real-time bending deformation of the elastic sheet detected by the strain gauge is obtained; The angle between the second discrete block furthest from the milking cup and the adjacent second discrete block is calculated based on the real-time bending deformation and the preset mapping relationship between the deformation amount and the angle.

[0014] As a second aspect of the present invention, the present invention also provides a compliance force control system for a milking robot based on digital twins, for controlling a milking robot, the milking robot including a robotic arm, an end effector, and a milking cup; wherein, the end effector includes: a drive motor, a traction member, a first discrete block, a second discrete block, a spring plate, and an elastic sheet disposed below the spring plate; the bottoms of a plurality of second discrete blocks are fixed on the spring plate, and the drive motor drives the traction member to move, so that the free ends of the second discrete blocks rotate to change the angle between two adjacent second discrete blocks; The control system includes: An optical encoder, used to detect the angular displacement of the drive motor; Strain gauges are disposed on the elastic sheet; A current sensor is used to detect the drive current of the drive motor; A controller is used to execute the above-described digital twin-based milking robot compliance force control method.

[0015] This invention provides a compliance force control method for a milking robot based on digital twins. First, the estimated contact force between the milking cup and the cow's udder is determined based on the total displacement of the traction component and the angle between the second discrete block furthest from the milking cup and its adjacent second discrete block. Then, using the total displacement of the traction component and the estimated contact force as boundary conditions, the method inputs these values ​​into a forward digital twin model based on Cosserat theory for simulation calculation to estimate the theoretical pulling force of the traction component. Next, the actual pulling force of the traction component is calculated based on the drive current of the drive motor and the radius of the drive motor's transmission wheel. Finally, the theoretical pulling force, actual pulling force, and estimated contact force are used to jointly determine whether a fault has occurred, and the working state of the robotic arm is adjusted according to the fault type. Whether the fault is in electronic equipment (e.g., sensors) or mechanical structure, the fault can be determined by combining the theoretical pulling force, actual pulling force, and estimated contact force, improving the accuracy of fault diagnosis and thus enhancing the operational safety of the milking robot. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0017] Figure 1 The diagram shown is a structural schematic of an end effector used for milking in a milking robot according to an embodiment of the present invention.

[0018] Figure 2 The image shown is an exploded view of an end effector used for milking in a milking robot according to an embodiment of the present invention.

[0019] Figure 3 The diagram shown is a flowchart illustrating a compliance force control method for a milking robot based on digital twins, according to an embodiment of the present invention.

[0020] Figure 4 The diagram shown is a structural schematic of the milking cup and the discrete block when the milking cup is not subjected to contact force according to an embodiment of the present invention.

[0021] Figure 5The diagram shown is a schematic representation of the structure between the milking cup and the discrete block when the milking cup is subjected to contact force according to an embodiment of the present invention.

[0022] Figure 6 The diagram shown is a schematic diagram of the elastic body's physical equilibrium relationship corresponding to the model theory in a forward simulation provided by an embodiment of the present invention.

[0023] Figure 7 The diagram shown is a flowchart illustrating a compliance force control method for a milking robot based on digital twins, according to another embodiment of the present invention.

[0024] Figure 8 The diagram shown is a flowchart illustrating a compliance force control method for a milking robot based on digital twins, according to another embodiment of the present invention.

[0025] Figure 9 The diagram shown is a flowchart illustrating a compliance force control method for a milking robot based on digital twins, according to another embodiment of the present invention.

[0026] Figure 10 The diagram shown is a working block diagram of a compliance force control system for a milking robot based on digital twins, according to an embodiment of the present invention.

[0027] Figure 11 The diagram shown is a block diagram of an electronic device provided in an embodiment of the present invention. Detailed Implementation

[0028] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0029] In the description of this invention, it should be noted that the terms "upper", "lower", "front", "horizontal", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on this invention.

[0030] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the term "installation" should be interpreted broadly. For example, it can refer to a fixed connection, a detachable connection, or an integral connection; it can refer to a mechanical connection or an electrical connection; it can refer to a direct connection or an indirect connection through an intermediate medium; it can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0031] The present invention will now be described in further detail with reference to specific embodiments and accompanying drawings.

[0032] Exemplary milking robot As a first aspect of the present invention, the present invention provides a milking robot, which includes a robotic arm, an end effector, and a milking cup 40, wherein the power output end of the robotic arm is poweredly connected to the end effector, and the milking cup 40 is connected to the power output end of the end effector (also referred to as the end of the end effector). Figure 1 The diagram shown is a structural schematic of an end effector according to an embodiment of the present invention. Figure 2 The image shown is an exploded view of an end effector provided in an embodiment of the present invention. (In conjunction with...) Figure 1 as well as Figure 2As shown, the end effector includes a drive motor 204, a traction member 205, a first discrete block 201, multiple second discrete blocks 202, a transmission wheel 206, and a support member 207. The first discrete block 201 is fixed to a fixed plate 401, which is fixed to a milking cup 40. One end of the traction member 205 is fixedly connected to the fixed plate 401. The second discrete block 202 furthest from the milking cup 40 is fixed to the support member 207. The other end of the traction member 205 passes through the first discrete block 201 and the multiple second discrete blocks 202 and is then fixed to the transmission wheel 206 of the drive motor 204. The bottoms (or fixed ends) of the multiple second discrete blocks 202 are fixed to a spring plate 203. The elastic bending of the spring plate 203 allows the upper parts (or free ends) of the multiple second discrete blocks 202 to rotate, thereby increasing or decreasing the angle between adjacent second discrete blocks 202. An elastic plate 208 is provided below the spring plate 203, and a strain gauge is provided on the elastic plate 208. When the upper part of the second discrete block 202 rotates, causing the angle between two adjacent second discrete blocks 202 to increase or decrease, both the spring plate 203 and the elastic plate 208 will undergo corresponding bending deformation. After the strain gauge detects the real-time bending deformation, the angle between two adjacent second discrete blocks 202 can be determined according to the correspondence between the real-time bending deformation and the angle. The drive motor 204 drives the transmission wheel 206 to rotate, thereby driving the traction member 205 to reciprocate linearly. The reciprocating linear motion of the traction member 205 can drive the first discrete block 201 and the second discrete block 202 to always move in opposite directions relative to each other. For example, the drive motor 204 drives the transmission wheel 206 to rotate clockwise, thereby driving the traction member 205 to drive the second discrete block 202 to rotate away from the milking cup 40, thereby increasing the angle between the first discrete block 201 and the second discrete block 202, releasing the milking cup 40, and fitting the cup onto the cow's teat for milking. The drive motor 204 drives the transmission wheel 206 to rotate counterclockwise, thereby driving the traction member 205 to rotate the second discrete block 202 towards the milking cup 40, thereby reducing the angle between the first discrete block 201 and the second discrete block 202, and thus retracting the milking cup 40.

[0033] The working principle of the end effector in this invention when retracting and releasing the milking cup 40 is as follows: Combination Figure 4 as well as Figure 5 When the milking cup 40 is retracted, meaning there is no contact between the milking cup and the cow's body, and the milking cup is not subjected to contact force, the angle between any two adjacent second discrete blocks 202 is 0, and the angle between the second discrete block 202 closest to the milking cup and the first discrete block 201 is... θ f .

[0034] When the cow's udder comes into contact with the milking cup 40, the milking cup 40 is subjected to a contact force. Under the synergistic action of multiple second discrete blocks 202 and spring plate 203, the angle between the second discrete block 202 furthest from the milking cup and its adjacent second discrete block 202 changes. The first angle change is Δ. θ f The angle between the first discrete block 201 and the second discrete block 202, which is closest to the milking cup 40, also changes, and the amount of this second angle change is Δ. θ f '.

[0035] Since one end of the traction member 205 is fixed to the milking cup 40 and the other end is fixed to the transmission wheel 206 of the drive motor 204, the length of the traction member 205 is constant. The angle between the second discrete block 202 furthest from the milking cup and its adjacent second discrete block 202 changes, causing a first change in the length of the traction member 205 between them. The angle between the first discrete block 201 and the second discrete block 202 closest to the milking cup 40 changes, causing a second change in the length of the traction member 205 between them. The change in the length of the traction member 205 between the second discrete block 202 furthest from the milking cup and its adjacent second discrete block 202 can be calculated using the angular displacement of the transmission wheel 206 on the drive motor 204.

[0036] The second length change of the traction member 205 between the first discrete block 201 and the second discrete block 202 closest to the milking cup 40 is related to the contact force on the milking cup 40. The first length change is equal to the second length change. Correspondingly, the angle change between the second discrete block 202 furthest from the milking cup and its adjacent second discrete block 202, and the angle change between the first discrete block 201 and the second discrete block 202 closest to the milking cup 40 are also approximately equal, i.e., the second angle change Δ θ f 'Approximately equal to the change in the first angle Δ between the second discrete block 202 furthest from the milking cup and its adjacent second discrete block 202 θ f , that is, Δ θ f '≈ Δ θ f .

[0037] Therefore, the contact force applied to the milking cup 40 can be estimated by driving the displacement of the traction member 205 via the drive motor 204 and by measuring the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block. This allows the working state of the robotic arm to be adjusted based on whether the contact force is abnormal, ensuring that the contact force between the cow's udder and the milking cup 202 is within a preset range, thus guaranteeing the safety and reliability of the milking operation.

[0038] Then, the theoretical pulling force of the traction component 205 is estimated based on the contact force applied to the milking cup 40, and the actual pulling force of the traction component 205 is calculated based on the actual driving current of the drive motor 204. Thus, based on the actual pulling force and the theoretical pulling force, it is determined whether each mechanical device or sensor in the milking robot has malfunctioned, and the working state of the robotic arm is controlled according to the fault type to improve the safety of the equipment.

[0039] Exemplary control method As a second aspect of the present invention, the present invention provides a compliance force control method for a milking robot based on digital twins, the control method being used to control the milking robot described above. Figure 3 The diagram shown is a flowchart illustrating a method for controlling the compliance force of a milking robot based on digital twins, according to an embodiment of the present invention. Figure 3 As shown, the compliance force control method for milking robots based on digital twins includes the following steps: S100: Determine the estimated contact force between the milking cup and the cow's udder based on the angular displacement of the drive motor and the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block. Specifically, the angular displacement of the drive motor can be detected by an coded sensor installed on the drive motor.

[0040] Specifically, such as Figure 4 As shown, there is no contact between the milking cup and the cow's body, resulting in no external force being applied to the milking cup. The angle between the second discrete block 202 furthest from the milking cup and its adjacent second discrete block 202 is 0 degrees. Figure 5 As shown, if the cow's body comes into contact with the milking cup, causing an external force to be applied to the milking cup, then the angle between the second discrete block 202 furthest from the milking cup and the adjacent second discrete block 202 will change, for example, by a change of Δ. θ f Δ θ f This is the angle between the second discrete block 202 furthest from the milking cup and its adjacent second discrete block 202. Specifically, the angle between the second discrete block furthest from the milking cup and its adjacent second discrete block can be obtained as follows: (1) Combination Figure 2 As shown, an elastic sheet 208 is provided under the spring plate 203, and a strain gauge is provided on the elastic sheet 208. When the contact force applied to the milking cup exceeds the preset contact force, the angle between the second discrete block 202 farthest from the milking cup and the adjacent second discrete block 202 increases. Both the spring plate 203 and the elastic sheet 208 will correspondingly bend and deform. After the strain gauge detects the real-time bending deformation, the angle between the second discrete block 202 farthest from the milking cup and the adjacent second discrete block 202 can be determined based on the mapping relationship between the real-time bending deformation, the preset deformation, and the angle. Specifically, the estimated contact force between the milking cup and the cow's udder is determined based on the angular displacement of the drive motor and the angle between the second discrete block farthest from the milking cup and the adjacent second discrete block. The specific principle is as follows: Among them, such as Figure 4 As shown, the angle between the second discrete block and the first discrete block closest to the milking cup when the milking cup is in the cup-closed state (i.e., when the milking cup is not subjected to contact force) is... θ f The angle between the second discrete block 202 furthest from the milking cup and its adjacent second discrete block 202 is 0.

[0041] like Figure 5 As shown, if the cow's body comes into contact with the milking cup, causing an external force to be applied to the milking cup, then the angle between the second discrete block 202 furthest from the milking cup and the adjacent second discrete block 202 will change, for example, by a change of Δ. θ f Δ θ f It is the angle between the second discrete block 202 furthest from the milking cup and the adjacent second discrete block 202.

[0042] So, when there is contact force between the cow's body and the milking cup, combined with... Figure 5 As shown, the angle between the second discrete block closest to the milking cup and the first discrete block. θ f 'Equals the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block + θ f ,Right now

[0043] in, θ f 'The angle between the second discrete block closest to the milking cup and the first discrete block when the milking cup is subjected to a contact force.' θ f Δ is the angle between the second discrete block closest to the milking cup and the first discrete block when the milking cup is not subjected to a contact force. θf It is the angle between the second discrete block 202 furthest from the milking cup and the adjacent second discrete block 202 when the milking cup is subjected to contact force.

[0044] Since the angular displacement of the drive motor is related to the power of the drive motor, and the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block is related to the contact force applied to the milking cup, the estimated contact force between the milking cup and the cow's udder can be estimated based on the angular displacement of the drive motor and the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block. S200: Determine the theoretical tension value of the traction component based on the angular displacement of the drive motor and the estimated contact force; Optionally, S200 (determining the theoretical pulling force of the traction component based on the angular displacement of the drive motor and the estimated contact force) specifically includes the following steps: S210: Calculate the total displacement of the traction component based on the angular displacement of the drive motor; S220: Input the estimated contact force and the total displacement of the traction component into the pre-built forward model for simulation calculation to obtain the theoretical tensile force value of the traction component.

[0045] Specifically, the forward model is a digital twin forward model based on Cosserat theory.

[0046] Combination Figure 6 As shown, the system of ordinary differential equations characterizing the Cosserat model is as follows:

[0047] f e This indicates the tension in the wire rope. F e As an external force, M e It is an external torque. The force balance equation at the front segment is as follows:

[0048] Considering that the direction of the tension in the wire rope is collinear with the wire rope itself, and assuming the rope driving force is DL, the geometric constraint can be expressed as:

[0049] Considering the side conditions are fixed at s=0, R(0)= R0, P(0)= s(0). Solving equations (1), (2), and (3) simultaneously, the estimated tension of the wire rope can be obtained based on the external force and the rope's driving force, using the target-shooting method. f e .

[0050] Therefore, by inputting the estimated contact force and the total displacement of the traction component into the pre-built forward model for simulation calculation, the theoretical tensile force value of the traction component can be obtained.

[0051] S300: The actual pulling force of the traction component is calculated based on the drive current of the drive motor and the radius of the drive motor's transmission wheel; Specifically, the drive current I (unit: A) of the drive motor can be read in real time at a frequency of not less than 1kHz by using the built-in current sensor of the drive motor.

[0052] Based on the torque constant Kt of the drive motor (unit: Nm / A), the reduction ratio N of the reducer, and the radius r of the transmission wheel (unit: m), the formula is used:

[0053] The theoretical tensile force of the traction component can be calculated. .

[0054] In the formula, η is the efficiency coefficient of the transmission system, I is the driving current of the drive motor, Kt is the torque constant of the drive motor, N is the reduction ratio of the reducer, and r is the radius of the transmission wheel in the drive motor.

[0055] S400: Adjust the working state of the robotic arm based on the theoretical tension value of the traction component, the actual tension value of the traction component, and the estimated contact force.

[0056] Specifically, first, based on the theoretical tension value of the traction component, the actual tension value of the traction component, and the estimated contact force, it is determined whether a fault has occurred and the type of fault. Then, the working state of the robotic arm is adjusted according to the fault and the type of fault.

[0057] Specifically, the working states of the robotic arm include, but are not limited to: stopped operation, normal operation, and returning to a preset safe position.

[0058] This invention provides a compliance force control method for a milking robot based on digital twins. First, the estimated contact force between the milking cup and the cow's udder is determined based on the total displacement of the traction component and the angle between the second discrete block furthest from the milking cup and its adjacent second discrete block. Then, using the total displacement of the traction component and the estimated contact force as boundary conditions, the method inputs these values ​​into a forward digital twin model based on Cosserat theory for simulation calculation to estimate the theoretical pulling force of the traction component. Next, the actual pulling force of the traction component is calculated based on the drive current of the drive motor and the radius of the drive motor's transmission wheel. Finally, the theoretical pulling force, actual pulling force, and estimated contact force are used to jointly determine whether a fault has occurred, and the working state of the robotic arm is adjusted according to the fault type. Whether the fault is in electronic equipment (e.g., sensors) or mechanical structure, the fault can be determined by combining the theoretical pulling force, actual pulling force, and estimated contact force, improving the accuracy of fault diagnosis and thus enhancing the operational safety of the milking robot.

[0059] In one embodiment of the present invention, such as Figure 7 As shown, S400 (adjusting the working state of the robotic arm based on the theoretical tension value of the traction component, the actual tension value of the traction component, and the estimated contact force) specifically includes the following steps: S410: Calculate the absolute deviation value based on the theoretical tension value and the actual tension value of the traction component; Specifically, the absolute deviation value is equal to the absolute value of the difference between the theoretical tension value and the actual tension value of the traction component.

[0060] S420: Determine whether the absolute deviation value is greater than the preset deviation value; Specifically, the preset deviation value can be 2N.

[0061] When the judgment result of S420 is negative, that is, the absolute deviation value is less than or equal to the preset deviation value, for example, the absolute deviation value is less than 2N, it means that the deviation between the estimated contact force based on the milking cup, the theoretical pulling force value of the traction component calculated by forward simulation based on the total displacement of the traction component, and the actual theoretical pulling force value calculated by reverse simulation based on the drive current of the drive motor and the radius of the drive motor's transmission wheel is small, indicating that the mechanical structure of the entire milking robot and the sensors in the corresponding control system are all in normal working condition. At this time, it is necessary to further judge whether the contact force between the milking cup and the cow's udder during the milking robot's cup-attaching operation is within the preset range based on the estimated contact force of the milking cup (i.e., the estimated contact force between the milking cup and the cow's udder) to ensure the safety and reliability of the milking operation. That is, when the judgment result of S420 is negative, S430-S450 are executed.

[0062] S430: Determine whether the estimated contact force is less than the preset contact force; Specifically, the preset contact force is 15N.

[0063] When the judgment result of S430 is yes, that is, the estimated contact force between the milking cup and the cow's udder is less than the preset contact force, that is, the estimated contact force meets the preset range, the robotic arm is controlled to continue to perform the cup-fitting operation, that is, S440 is executed.

[0064] When the judgment result of S430 is negative, that is, the estimated contact force between the milking cup and the cow's udder is greater than or equal to the preset contact force, i.e. the estimated contact force exceeds the preset range, if the robotic arm continues to perform the cup-fitting operation, it means that the force applied to the cow's udder is too large, which will make the cow uncomfortable and reduce the safety of the operation and the cow. At this time, the robotic arm is controlled to stop the cup-fitting operation, i.e., S450 is executed to prevent discomfort to the cow and improve the safety of the operation and the cow.

[0065] S440: Controls the robotic arm to perform cup-setting operations.

[0066] S450: Controls the robotic arm to stop the cup-packing operation.

[0067] This invention controls the working state of the robotic arm by setting two judgment conditions: whether the system is stable and whether the estimated contact force between the milking cup and the cow's udder meets a preset range. In one embodiment of the present invention, when the judgment result of S420 is yes, that is, the absolute deviation value is greater than the preset deviation value, for example, the absolute deviation value is greater than 2N, it indicates that there is a large deviation between the estimated contact force based on the milking cup, the theoretical pulling force value of the traction component calculated by forward simulation based on the total displacement of the traction component, and the actual theoretical pulling force value calculated by reverse simulation based on the drive current of the drive motor and the radius of the drive wheel of the drive motor. This indicates that a mechanical structure or sensor in the milking robot has malfunctioned. Therefore, the robotic arm is controlled to return to the preset safe position along the preset motion trajectory, and a warning message is generated. Furthermore, it is necessary to further determine the fault type based on the difference between the theoretical pulling force value and the actual pulling force value of the traction component, and adjust the working state of the robotic arm accordingly based on the fault type. That is, as shown... Figure 8 As shown, when the judgment result of S420 is yes, S400 (adjusting the working state of the robotic arm based on the theoretical tension value of the traction component, the actual tension value of the traction component, and the estimated contact force) specifically includes the following steps S460-S470: S460: Controls the robotic arm to return to a preset safe position along a preset motion trajectory; S470: Determine the fault type based on the actual tension value of the traction component and the theoretical tension value of the traction component.

[0068] Optionally, S470 (determining the fault type based on the actual and theoretical tensile force of the traction component) specifically includes the following steps S471-S472: S471: When the difference between the actual tension value of the traction component and the theoretical tension value of the traction component is continuously greater than the first preset difference and the duration is greater than the first preset duration, sensor fault information is generated.

[0069] Specifically, the first preset difference can be three times the preset deviation value in S420.

[0070] When the difference between the actual tension value and the theoretical tension value of the traction component is consistently greater than a first preset difference, and the duration of this difference is greater than a first preset duration, it indicates that the actual tension value of the traction component is consistently significantly greater than the theoretical tension value, and the duration of this significant difference is greater than the first preset duration. This indicates that the drive motor is outputting a large torque, but the deformation sensed by the sensor is very small, meaning that the sensor in the system (e.g., a complete sensor) has malfunctioned (e.g., drift). At this time, sensor fault information is generated, and the user can determine that the sensor has malfunctioned based on the sensor fault information, thus accurately locating the faulty equipment.

[0071] S472: When the theoretical tension value of the traction component is continuously greater than the actual tension value of the traction component, and the timing duration is greater than the second preset duration, mechanical transmission path fault information is generated.

[0072] When the difference between the theoretical tension value and the actual tension value of the traction component is continuously greater than the second preset difference and the duration of this difference is greater than the second preset duration, it indicates that the actual tension value of the traction component is continuously significantly greater than the theoretical tension value, and the duration of this significant difference is greater than the first preset duration. This indicates that the sensor has detected a large deformation, but the actual output torque of the drive motor is very small, indicating that a mechanical structure in the mechanical transmission path has malfunctioned. At this time, mechanical transmission path fault information is generated. Based on the mechanical transmission path fault information, the user can determine that a mechanical structure in the mechanical transmission path has malfunctioned, and can accurately locate the faulty equipment.

[0073] This invention can detect sensor drift or mechanical failure in a timely manner, fundamentally solving the problem of unreliability of a single sensing path and greatly improving system safety.

[0074] In one embodiment of the present invention, such as Figure 9 As shown, S100 (determining the estimated contact force between the milking cup and the cow's udder based on the angular displacement of the drive motor and the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block) specifically includes the following steps: S110: Calculate the total displacement of the traction component based on the angular displacement of the drive motor; S120: Determine the displacement increment of the traction member at the equivalent hinge point based on the angle between the second discrete block farthest from the milking cup and the adjacent second discrete block. The equivalent hinge point is the connection between the second discrete block farthest from the milking cup and the adjacent second discrete block. Specifically, in combination Figure 4 As shown, when the milking cup 40 is retracted, the angle between any two adjacent second discrete blocks 202 is 0, and the angle between the second discrete block 202 closest to the milking cup and the first discrete block 201 is... θ f .

[0075] Combination Figure 5 As shown, when the cow's udder comes into contact with the milking cup 40, the milking cup 40 is subjected to a contact force. Under the synergistic action of multiple second discrete blocks 202 and spring plates 203, the angle between the second discrete block furthest from the milking cup and its adjacent second discrete block changes. The first angle change is Δ. θ f The angle between the first discrete block 201 and the second discrete block 202, which is closest to the milking cup 40, also changes, and the amount of this second angle change is Δ. θ f '。Then Δ θ f '≈ Δ θ f When the milking cup is subjected to a contact force, the angle between the second discrete block 202 furthest from the milking cup and the adjacent second discrete block 202 changes by Δ. θ f Then, the angle between the second discrete block closest to the milking cup and the first discrete block... θ f ’ =Δ θ f + θ f .

[0076] Where, Δ θ f The angle between the second discrete block 202 furthest from the milking cup and its adjacent second discrete block 202. Combined with... Figure 5 As shown, the displacement increment of the traction member 205 between the second discrete block 202 furthest from the milking cup and the adjacent second discrete block 202 is Δ. DL f Satisfy the following formula:

[0077] In the formula, r is the height of the second discrete block 202 (the radius of rotation of the second discrete block 202 relative to the adjacent second discrete block 202); Δ θ f The angle between the second discrete block 202 furthest from the milking cup and the adjacent second discrete block 202.

[0078] Because the angle between the second discrete block 202 furthest from the milking cup and the adjacent second discrete block 202 has changed Δ θ f , then Δ θ f '≈ Δ θ f Therefore, the displacement increment of the traction component at the equivalent hinge is equal to Δ DL f .

[0079] S130: Input the total displacement and displacement increment of the drive motor's traction component into the pre-built inverse model for simulation calculation to obtain the estimated contact force between the milking cup and the cow's udder.

[0080] Specifically, the inverse model is a multinomial fitting model. This inverse model establishes the model from geometric parameters ( DL f ,Δ DL f The nonlinear mapping relationship from the end contact force (Fe) to the end contact force.

[0081] Therefore, the total displacement and displacement increment of the traction component of the drive motor are input into a pre-built inverse model for simulation calculation to obtain the estimated contact force between the milking cup and the cow's udder.

[0082] Exemplary control system As a third aspect of the invention, the invention also provides a digital twin-based compliance force control system for milking robots, used to control the milking robot described above. Figure 10 The diagram shown is a working block diagram of a compliance force control system for a milking robot based on digital twins, according to an embodiment of the present invention. Figure 10 As shown, a compliance control system 800 for a milking robot based on digital twins includes: The photoelectric encoder 810 is used to detect the angular displacement of the drive motor; the angular displacement can be converted into the total displacement of the traction component.

[0083] A strain gauge 820 is installed on the elastic sheet. The strain gauge 820 is used to detect the angle between the second discrete block 202 farthest from the milking cup and the adjacent second discrete block. When the contact force applied to the milking cup exceeds the preset contact force, the angle between the second discrete block 202 farthest from the milking cup and the adjacent second discrete block 202 increases. The spring plate 203 and the elastic sheet 208 will both bend and deform accordingly. After the strain gauge detects the real-time bending deformation, the angle between the second discrete block 202 farthest from the milking cup and the adjacent second discrete block 202 can be determined according to the mapping relationship between the real-time bending deformation, the preset deformation and the angle.

[0084] Current sensor 830 is used to detect the drive current of the drive motor; Controller 840 is used to execute the described digital twin-based milking robot compliance force control method.

[0085] Optionally, pre-built forward and inverse models may also be included.

[0086] Among them, the forward model is a digital twin forward model based on Cosserat theory. The estimated contact force between the milking cup and the cow's udder and the total displacement of the traction component are input into the pre-built forward model for simulation calculation, and the theoretical tensile force value of the traction component can be obtained.

[0087] Exemplary electronic devices As a third aspect of the present invention, the present invention also provides an electronic device, including a processor and a memory, wherein the memory stores a computer program, and when the processor executes the computer program, it executes the above-described method for controlling the compliance force of a milking robot based on digital twins.

[0088] Specifically, the internal structure of electronic devices can be as follows: Figure 11 As shown, the electronic device includes a processor, a memory, a network interface, and an input device connected via a device bus. The processor provides computing and control capabilities. The memory includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores operating devices and computer programs. The internal memory provides an environment for the operation of the operating devices and computer programs stored in the non-volatile storage medium. The network interface is used to communicate with external terminals via a network connection. When the computer program is executed by the processor, it follows the steps of the digital twin-based compliance force control method for milking robots according to various embodiments of this specification, as described in the above embodiments.

[0089] The processor may include the main processor, as well as baseband chips, modems, etc.

[0090] The memory stores a program that executes the technical solution of this invention, and may also store operating devices and other key business functions. Specifically, the program may include program code, which includes computer operation instructions. More specifically, the memory may include read-only memory (ROM), other types of static storage devices capable of storing static information and instructions, random access memory (RAM), other types of dynamic storage devices capable of storing information and instructions, disk storage, flash memory, etc.

[0091] The processor can be a general-purpose processor, such as a general-purpose central processing unit (CPU), a microprocessor, etc., or an application-specific integrated circuit (ASIC), or one or more integrated circuits used to control the execution of the program of the present invention. It can also be a digital signal processor (DSP), an application-specific integrated circuit (ASIC), an off-the-shelf programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components.

[0092] Input devices may include devices that receive data and information input by the user, such as keyboards, mice, cameras, scanners, light pens, voice input devices, touch screens, pedometers, or gravity sensors.

[0093] Output devices may include devices that allow information to be output to the user, such as displays, printers, speakers, etc.

[0094] The communication interface may include any transceiver-like device for communicating with other devices or communication networks, such as Ethernet, Radio Access Network (RAN), Wireless Local Area Network (WLAN), etc.

[0095] The processor executes the program stored in the memory and calls other devices, which can be used to implement each step of any of the digital twin-based milking robot compliance control methods provided in the above embodiments of this specification.

[0096] The electronic device may also include a display component and a voice component. The display component may be a liquid crystal display screen or an e-ink display screen. The input device of the electronic device may be a touch layer covering the display component, or a button, trackball or touchpad set on the casing of the electronic device, or an external keyboard, touchpad or mouse, etc.

[0097] Those skilled in the art will understand that Figure 11The structures shown are merely block diagrams of a portion of the structure related to the scheme described in this specification, and do not constitute a limitation on the electronic devices to which the scheme described in this specification is applied. Specific electronic devices may include more or fewer components than those shown in the figures, or may combine certain components, or may have different component arrangements.

[0098] Exemplary computer program products and storage media In addition to the methods and devices described above, the ion implantation control method provided in the embodiments of this specification can also be a computer program product, which includes computer program instructions that, when executed by a processor, cause the processor to perform the steps in the digital twin-based compliance force control method for milking robots according to various embodiments of this specification as described in the "Exemplary Methods" section above.

[0099] The aforementioned computer program product can be implemented through hardware, software, or a combination thereof. In one optional embodiment, the computer program product is specifically embodied in a computer storage medium; in another optional embodiment, the computer program product is specifically embodied in a software product, such as a software development kit (SDK), etc.

[0100] The computer program product can be written in any combination of one or more programming languages ​​to perform the operations of the embodiments of this specification. The programming languages ​​include object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as the "C" language or similar programming languages.

[0101] Furthermore, embodiments of this specification also provide a computer-readable storage medium having a computer program stored thereon, the computer program being executed by a processor of the steps in the digital twin-based compliance force control method for milking robots according to various embodiments of this specification as described in the "Exemplary Methods" section above.

[0102] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided in this specification can include non-volatile and / or volatile memory. Non-volatile memory may include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory may include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in a variety of forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), RAMbus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0103] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0104] The embodiments described above are merely illustrative of several implementation methods outlined in this specification. While the descriptions are specific and detailed, they should not be construed as limiting the scope of the solutions provided in this specification. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this specification, and these all fall within the scope of protection of this specification. Therefore, the scope of protection for this patent should be determined by the appended claims.

Claims

1. A method for controlling the compliance force of a milking robot based on digital twins, characterized in that, The control method is used to control a milking robot, which includes a robotic arm, an end effector, and a milking cup; wherein the end effector includes a drive motor, a traction component, a first discrete block, and a second discrete block, and the drive motor drives the traction component to move so that the free end of the second discrete block rotates to change the angle between two adjacent second discrete blocks; The control method includes: The estimated contact force between the milking cup and the cow's udder is determined based on the angular displacement of the drive motor and the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block. The theoretical tension value of the traction component is determined based on the angular displacement of the drive motor and the estimated contact force. The actual pulling force of the traction component is calculated based on the driving current of the drive motor and the radius of the drive motor's transmission wheel. The working state of the robotic arm is adjusted according to the theoretical pulling force value of the traction component, the actual pulling force value of the traction component, and the estimated contact force.

2. The control method according to claim 1, characterized in that, The method of adjusting the working state of the robotic arm based on the theoretical tension value of the traction component, the actual tension value of the traction component, and the estimated contact force includes: When the absolute deviation between the theoretical tension value and the actual tension value of the traction component is less than or equal to a preset deviation value, and the estimated contact force is less than the preset contact force, the robotic arm is controlled to perform the cup-attaching operation.

3. The control method according to claim 2, characterized in that, The method of adjusting the working state of the robotic arm based on the theoretical tension value of the traction component, the actual tension value of the traction component, and the estimated contact force further includes: When the absolute deviation between the theoretical tension value and the actual tension value of the traction component is less than or equal to the preset deviation value, and the estimated contact force is greater than or equal to the preset contact force, the robotic arm is controlled to stop the cup-fitting operation.

4. The control method according to claim 2, characterized in that, The method of adjusting the working state of the robotic arm based on the theoretical tension value of the traction component, the actual tension value of the traction component, and the estimated contact force further includes: When the absolute deviation between the theoretical pulling force value and the actual pulling force value of the traction component is greater than the preset deviation value, the robotic arm is controlled to return to the preset safe position along a preset motion trajectory. The fault type is determined based on the actual tension value of the traction component and the theoretical tension value of the traction component.

5. The control method according to claim 4, characterized in that, Determining the fault type based on the actual tension value and the theoretical tension value of the traction component includes: When the difference between the actual tension value of the traction component and the theoretical tension value of the traction component is continuously greater than a first preset difference and the duration is greater than a first preset duration, sensor fault information is generated.

6. The control method according to claim 4, characterized in that, Determining the fault type based on the actual tension value and the theoretical tension value of the traction component includes: When the theoretical tension value of the traction component is continuously greater than the actual tension value of the traction component, and the time duration is greater than the second preset time duration, mechanical transmission path fault information is generated.

7. The control method according to claim 1, characterized in that, The estimated contact force between the milking cup and the cow's udder is determined based on the angular displacement of the drive motor and the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block, including: The total displacement of the traction component is calculated based on the angular displacement of the drive motor. The displacement increment of the traction member at the equivalent hinge is determined based on the angle between the second discrete block furthest from the milking cup and the adjacent second discrete block. The equivalent hinge is the connection point between the second discrete block furthest from the milking cup and the adjacent second discrete block. The total displacement of the traction component of the drive motor and the displacement increment are input into a pre-built inverse model for simulation calculation to obtain the estimated contact force between the milking cup and the cow's udder.

8. The control method according to claim 7, characterized in that, The step of determining the theoretical tension value of the traction component based on the angular displacement of the drive motor and the estimated contact force includes: The estimated contact force and the total displacement of the traction component are input into a pre-built forward model for simulation calculation to obtain the theoretical tensile force value of the traction component.

9. The control method according to claim 1, characterized in that, The end effector further includes: a spring plate and an elastic sheet disposed below the spring plate; wherein, strain gauges are disposed on the elastic sheet, and the bottoms of the plurality of second discrete blocks are fixed to the spring plate; The control method further includes: The real-time bending deformation of the elastic sheet detected by the strain gauge is obtained; The angle between the second discrete block furthest from the milking cup and the adjacent second discrete block is calculated based on the real-time bending deformation and the preset mapping relationship between the deformation amount and the angle.

10. A compliance force control system for a milking robot based on digital twins, characterized in that, For controlling a milking robot, the milking robot includes a robotic arm, an end effector, and a milking cup; wherein, the end effector includes: a drive motor, a traction component, a first discrete block, a second discrete block, a spring plate, and an elastic sheet disposed below the spring plate; the bottoms of a plurality of second discrete blocks are fixed on the spring plate, and the drive motor drives the traction component to move, so that the free ends of the second discrete blocks rotate to change the angle between two adjacent second discrete blocks; The control system includes: An optical encoder, used to detect the angular displacement of the drive motor; Strain gauges are disposed on the elastic sheet; A current sensor is used to detect the drive current of the drive motor; A controller for performing the compliance force control method for a milking robot based on digital twins as described in any one of claims 1-9.

Images