Ground jar fermented grain tailing cleaning method based on improved self-adaptive impedance control

Abstract

The invention belongs to the technical field of brewing equipment control, and relates to cleaning of fermented grain tailings in a ground jar of a winery, in particular to a ground jar fermented grain tailings cleaning method based on improved self-adaptive impedance control, which mainly comprises two parts, namely ground jar fermented grain tailings gathering operation control and ground jar fermented grain tailings digging operation control. Wherein the ground cylinder fermented grain tailing gathering operation is controlled to be a circulating operation mode, and the ground cylinder fermented grain tailing digging operation is to dig fermented grains gathered together and move the fermented grains to a specified discharging area; in the control process, a self-adaptive impedance control algorithm is improved, so that the problems that the residual material amount is large and a ground cylinder is prone to being collided and damaged when tailings are cleaned through a traditional position control method are solved, and meanwhile the problems that the ground cylinder space is narrow, and the pose of a material taking device is limited and the contact force is complex when a robot works are solved; and quick and efficient cleaning of the ground cylinder fermented grain tailings is realized.

Description

Technical Field

[0001] The invention belongs to the technical field of winemaking equipment control, and relates to the cleaning of wine mash tails in a ground tank of a winery, and specifically is a method for cleaning wine mash tails in a ground tank based on improved adaptive impedance control. Background Art

[0002] At present, the light-fragrance liquor on the market mainly uses ground cylinders as fermentation containers, and the fermentation product is fermented mash, which needs to be removed from the cylinder. At present, most companies use traditional manual methods to remove mash from the cylinder, which has problems such as large workload and high labor intensity. Therefore, in order to realize the automatic removal of mash from the cylinder, a mash-taking composite robot is designed to realize the automatic removal of mash from the cylinder. The traditional robot control method is position control, that is, controlling the robot to move between specific spatial points. However, in the tailing cleaning task, the traditional position control method is only applicable to an ideal working environment. In an ideal working environment, the ground cylinders are placed horizontally, and each ground cylinder has the same structure. The tailing cleaning position control method in an ideal environment includes starting from the initial posture that fits the bottom of the ground cylinder, and moving to the end posture through precise position control, thereby completing a single tailing cleaning operation.

[0003] In the actual working environment, the geometric shape, placement posture and height of the ground tank are different. In the tailings cleaning operation, the ground tank is not completely parallel to the horizontal plane, but there is a certain angle deviation. If the robot does not reserve a safe working distance during the operation, it will collide and rub with the ground tank when moving along the path planned in the ideal environment, causing damage to the tank body. In order to avoid such collisions and frictions, measures to reserve a certain safe working distance are required, but this will cause a large amount of mash to remain, and the cleaning effect will be poor. Therefore, if the position control method is used for tailings cleaning, it is necessary to obtain an accurate spatial three-dimensional model of each ground tank. However, it is quite difficult to accurately obtain the three-dimensional model of each ground tank in the mash fermentation workshop for three reasons: (1) Due to the large number of ground tanks in the mash fermentation workshop, obtaining the three-dimensional model of each ground tank is not only cumbersome, but also time-consuming, which may cause the raw materials to miss the best time for fermentation. (2) After the ground tank is landfilled, the surrounding soil will gradually compact over time, causing the ground tank to be continuously stressed and slightly deformed, which makes the previously established three-dimensional model no longer accurate in subsequent applications. (3) In order to reduce the impact and infection of soil bacteria on the mash and ensure the cleanliness of the ground tank container and the quality of the mash, the ground tank needs to be replaced every year during the ground tank fermentation process of light-fragrance liquor. This practice means that the three-dimensional model of the ground tank needs to be obtained again every year, which undoubtedly increases the workload and makes the process cumbersome. In summary, it is necessary to overcome the above problems by controlling the contact force between the robot and the environment during the tailing cleaning operation to ensure that the robot maintains smooth contact with the ground tank during the tailing cleaning operation, prevent collisions and excessive friction, and clean the mash in the ground tank.

[0004] At present, the tasks based on force control are mainly precision assembly, medical robots, workpiece surface grinding and other daily tasks. In these tasks, the operation process can usually be divided into three stages: operation path design, control the robot to move to the initial operation point, and make contact through a compliant control algorithm. Among them, the design of the operation path is often relatively direct, usually a straight line or curved path. In the compliant control stage, the robot implements the Z-axis force control solution, which requires the robot to always maintain a posture perpendicular to the contact surface during the operation.

[0005] However, the structure of the ground vat is relatively complex, and its shape is similar to a truncated cone, with the mash tailings located at the bottom of the truncated cone structure. The distribution of mash tailings in the ground vat is low in the middle and high at the edge. During the cleaning process of the mash tailings, it is necessary to gather the mash tailings together and at the same time dig the gathered mash out of the ground vat. During the cleaning process, the robot's posture is always restricted by the truncated cone structure, and the above operations are all performed in free space. At the same time, the operation tasks are relatively simple. Therefore, the operation method in the above-mentioned force control-based operation tasks cannot be applied to the cleaning operation of mash tailings in ground vats. Summary of the invention

[0006] The purpose of the present invention is to provide a method for cleaning the mash tailings in a ground vat based on improved adaptive impedance control. In view of the complexity of the internal working environment of the ground vat, the method enables a robot for cleaning the mash tailings in a ground vat to achieve smooth contact with the ground vat during the cleaning process, thereby avoiding damage to the robot or the ground vat during the operation process.

[0007] The technical solution adopted by the present invention to achieve the above-mentioned purpose is:

[0008] A method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control comprises the following steps:

[0009] S1. Gathering operation of fermented grains in ground tanks:

[0010] S11, presetting the material gathering point, material gathering range and the initial point of the gathering operation;

[0011] S12, the robot operation mechanism moves to the initial point of the aggregation operation under the control of the path planning algorithm;

[0012] S13, the robot operating mechanism moves from the initial point of the gathering operation to the side wall of the ground cylinder under the control of the improved adaptive impedance control algorithm until a brief impact force occurs when the robot operating mechanism contacts the side wall of the ground cylinder;

[0013] S14, the robot operating mechanism keeps the Z-axis force controlled X-axis translational movement along the side wall of the ground cylinder toward the bottom of the ground cylinder under the control of the improved adaptive impedance control algorithm, until the contact force between the robot operating mechanism and the ground cylinder suddenly changes;

[0014] S15, the robot operating mechanism moves along the bottom of the ground cylinder toward the material gathering point under the control of the improved adaptive impedance control algorithm, and the robot operating mechanism maintains the Z-axis force control X-axis translation;

[0015] S16, adjusting the working path and working posture of the robot working mechanism, and looping the above-mentioned aggregation working process;

[0016] S2. Digging out the fermented grains from the ground tank:

[0017] S21, presetting the initial point of the excavation operation;

[0018] S22, the robot operating mechanism moves to the initial point of the digging operation under the control of the path planning algorithm;

[0019] S23, the robot operating mechanism completes the tailings digging operation under the control of the improved adaptive impedance control algorithm;

[0020] S231, the robot operating mechanism moves toward the bottom of the ground cylinder under the control of the improved adaptive impedance control algorithm until a brief impact force occurs when the robot operating mechanism contacts the bottom of the ground cylinder;

[0021] S232, the robot operating mechanism moves along the bottom of the ground cylinder toward the material gathering point under the control of the improved adaptive impedance control algorithm until a brief impact force occurs when the robot operating mechanism contacts the side wall of the ground cylinder;

[0022] S233, the robot operating mechanism moves along the side wall of the ground cylinder from the bottom of the ground cylinder to the mouth of the ground cylinder under the control of the improved adaptive impedance control algorithm;

[0023] S24, the robot operating mechanism moves to the material discharging point through the path planning algorithm to complete the material discharging operation;

[0024] The improved adaptive impedance control algorithm is:

[0025]

[0026] In the formula, f e (t) is the actual contact force between the robot operating mechanism and the environment, f d (t) is the expected contact force between the robot operating mechanism and the ground cylinder, where m is the mass coefficient of the impedance control system of the robot operating mechanism, b is the damping coefficient of the impedance control system of the robot operating mechanism, Preset movement paths for the robot operating mechanism at each operating stage; and The first-order and second-order derivatives of the position change of the robot operating mechanism in the force control direction with respect to time; To improve the output of the adaptive impedance control algorithm, i.e. the actual acceleration of the robot's operating mechanism during movement; It is the actual speed of the robot operating mechanism during the movement process; Doing the double integral gives:

[0027]

[0028] x c It is the actual position of the robot operating mechanism during the movement process;

[0029] Among them, Δb(t) is the change of the damping coefficient, which is updated online according to the force error and its change rate. The update formula is:

[0030]

[0031] Where Δf(t) = f e (t)-f d (t), λ is the control period of the robot, η is the update rate, bp is the force error coefficient, b v is the force error change rate coefficient. To ensure the stability of the system, the value range of η is

[0032]

[0033] In the formula, k e is the stiffness coefficient of the ground cylinder.

[0034] Furthermore, the S11 specifically includes:

[0035] (1) The specific setting rules for the gathering point and the gathering range are as follows:

[0036] D gather_range =kL machine

[0037] Among them, D gather_range is the diameter of the aggregate range, L machine is the width of the robot operating mechanism, and k is the contraction coefficient.

[0038] (2) The specific setting requirements for the initial point of the aggregation operation are:

[0039] (r gather_init <R pot_top )∩(h contact >max(H grains ))

[0040] Among them, r gather_init R is the distance between the initial point of the gathering operation and the axis of the ground cylinder, pot_top h is the radius of the ground cylinder mouth, contact H is the height from the contact point between the robot operating mechanism and the side wall of the ground cylinder to the bottom of the ground cylinder. grains It is the height of the mash in the ground jar.

[0041] Furthermore, the S13 specifically includes:

[0042] The motion path of the robot operating mechanism is a straight line along the Z axis of the operating mechanism. During the motion, the posture of the robot operating mechanism is that the Z axis is vertically downward and the X axis points to the axis of the cylinder. During the motion, the Z axis is kept moving downward by force control, and the X axis does not move. When the contact force f between the robot operating mechanism and the environment e (t)Satisfy:

[0043] f e (t)>1.2f d_z (t)

[0044] where f d_z (t) is the expected contact force between the robot operating mechanism and the floor cylinder in the Z-axis direction, indicating that the robot operating mechanism contacts the side wall of the floor cylinder at this time.

[0045] Furthermore, the S14 specifically includes:

[0046] The motion path of the robot operating mechanism is an arc that fits the side wall of the ground cylinder. During the motion, the operating mechanism's posture is that the Z axis is vertically downward and the X axis points to the axis of the ground cylinder. During the motion, the Z axis is kept moving by force control, and the X axis is kept moving by position control.

[0047] Expected contact force f in the Z direction d_z (t) is set to

[0048] f d_z (t) = k grains x side +f c

[0049] In the formula, k grains is the stiffness coefficient of the mash, x side is the horizontal projection of the distance relative to the contact point during the movement of the robot operating mechanism, f c is the expected contact force constant;

[0050] When the contact force between the robot operating mechanism and the environment is e (t)Satisfy:

[0051] f e (t)>1.2f d_z (t)

[0052] Indicates the angle between the bottom and side wall of the floor cylinder where the operating mechanism contacts the floor cylinder.

[0053] Furthermore, the S15 specifically includes:

[0054] The motion path of the robot operating mechanism is a straight line that fits the bottom of the floor cylinder. The initial point is the angle between the bottom of the floor cylinder and the side wall, and the end point is the material gathering point. During the movement, the posture of the robot operating mechanism is that the Z axis is vertically downward and the X axis points to the material gathering point. During the movement, the Z axis force control is maintained to ensure contact with the bottom of the floor cylinder, and the X axis keeps moving through position control.

[0055] Furthermore, the specific step of S16 includes:

[0056] S161. Divide the floor cylinder into several areas according to the width of the robot operating mechanism. The division rules are as follows:

[0057] n≈(360°-θ gather_range ) / θ end_vaild ,n=1,2,3...

[0058] Among them, n is the number of divided areas, θ gather_range is the angle of the aggregate range at the bottom of the ground cylinder, θend_vaild The angle corresponding to the effective operating area of ​​the robot operating mechanism at the bottom of the ground cylinder;

[0059] S162, when the robot operating mechanism moves along the side wall of the ground cylinder, the movement path of the robot operating mechanism is an arc that fits the side wall of the ground cylinder, that is, the midline of each area, the initial point is the contact point between the robot operating mechanism and the side wall, and the end point is the angle between the side wall and the bottom of the ground cylinder; during the movement, the posture of the robot operating mechanism is that the Z axis is vertically downward, the X axis points to the axis of the ground cylinder, and the Y axis is perpendicular to the inner wall of the ground cylinder;

[0060] S163. During the stage where the robot operating mechanism moves along the bottom of the ground cylinder, the movement path of the robot operating mechanism is a straight line that fits the bottom of the ground cylinder. The initial point is the end point of the stage where the side wall of the ground cylinder in the current area moves, and the end point is the material gathering point. During the movement, the posture of the robot operating mechanism changes, and the initial posture is that the Z axis is vertically downward, the X axis points to the axis of the ground cylinder, and the Y axis is perpendicular to the inner wall of the ground cylinder. The end posture is that the Z axis is vertically downward, and the X axis points to the material gathering point.

[0061] Furthermore, the setting requirements for taking the initial point of the operation in S21 are:

[0062] r dig_init <(R pot_bottom -D gather_range )

[0063] Among them, r dig _ init R is the distance between the initial point of the excavation operation and the axis of the ground cylinder, pot_bottom D is the radius of the bottom of the ground cylinder. gather_range is the aggregate range diameter.

[0064] Furthermore, the S231 specifically includes:

[0065] The motion path of the robot operating mechanism is a straight line along the Z axis of the operating mechanism; during the movement, the posture of the robot operating mechanism is that the Z axis is vertically downward and the X axis points to the material gathering point; during the movement, the Z axis is kept moving by force control, and the X axis direction does not move.

[0066] Set the expected contact force f in the Z-axis direction of the robot operating mechanism d_z (t) range, when the contact force f between the robot operating mechanism and the environment e (t)Satisfy:

[0067] f e (t)>1.2f d_z (t)

[0068] This indicates that the robot operating mechanism has touched the bottom of the floor tank.

[0069] Furthermore, the S232 specifically includes:

[0070] When the robot operating mechanism moves along the bottom of the ground cylinder, the movement path of the operating mechanism is a straight line that fits the bottom of the ground cylinder. The initial point is the contact point between the operating mechanism and the bottom of the ground cylinder, and the end point is the angle between the side wall and the bottom of the ground cylinder. During the movement, the initial posture of the operating mechanism is that the Z axis is vertically downward, and the X axis points to the gathering point of the ground cylinder. The Y axis direction of the end posture remains unchanged, and there is an angle α between the Z axis and the horizontal direction. bottom , specifically:

[0071]

[0072] In the formula, R machine R is the radius of the connection end of the operating mechanism. o_back The distance between the axis of the working structure and its back, L is the distance between the midpoint of the front edge of the working mechanism and the edge point of the connection end; α safe H is the safety angle, which is used to avoid collision between the working connection end and the ground cylinder. It should be set according to the actual working environment. pot is the height of the ground tank, D pot_top D is the diameter of the ground cylinder mouth pot_bottom The bottom diameter of the ground cylinder.

[0073] The expected contact force f applied by the operating mechanism in the Z-axis direction to the bottom of the cylinder d_z (t) is:

[0074]

[0075] In the formula, μ k is the dynamic friction coefficient between the operating mechanism and the surface of the ground cylinder, ρ grains is the density of the fermented grains in the ground tank, L machine is the width of the robot operating mechanism, x bottom H is the distance that the material taking mechanism moves relative to the axis of the ground cylinder, grains is the height of the mash in the ground tank, g is the acceleration due to gravity, F zz (t-λ) is the force F exerted by the robot operating mechanism in the Z-axis direction on the bottom of the cylinder in the previous control cycle z The vertical component at the contact point; k grains is the stiffness coefficient of the mash; θ is the angle between the Z-axis direction of the operating structure and the horizontal direction.

[0076] When the contact force between the robot operating mechanism and the environment is e (t)Satisfy:

[0077] f e (t)>1.2f d_z (t)

[0078] This indicates that the robot operating mechanism has contacted the side wall of the floor cylinder.

[0079] Furthermore, the S233 specifically includes:

[0080] When the robot operating mechanism moves along the side wall of the ground cylinder, the movement path of the robot operating mechanism is an arc that fits the side wall of the ground cylinder, and the end position is the farthest distance between the side wall of the ground cylinder and the axis; during the movement, the initial posture of the operating mechanism is the end posture of the previous stage, the Y-axis direction of the end posture remains unchanged, and there is an angle α between the Z-axis and the horizontal direction side , specifically:

[0081]

[0082] In the formula, R machine R is the radius of the connection end of the operating mechanism. o_back The distance between the axis of the working structure and its back, L is the distance between the midpoint of the front edge of the working mechanism and the edge point of the connection end, α safe It is a safety angle, which is used to avoid collision between the working connection end and the ground cylinder. It is set according to the actual working environment. pot_rmax H is the ratio of the height of the farthest point from the axis to the floor cylinder opening to the floor cylinder height. pot is the height of the ground tank, D pot_top is the diameter of the ground cylinder mouth, R pot_max The distance between the farthest point of the side wall of the ground cylinder and the axis of the ground cylinder;

[0083] The expected contact force f exerted by the robot operating mechanism on the side wall of the ground cylinder in the X-axis direction d_x (t) is:

[0084]

[0085] In the formula, μ k is the dynamic friction coefficient between the robot operating mechanism and the surface of the ground cylinder, G grains is the estimated value of the gravity of the mash above the robot operating mechanism, β is the angle between the gravity direction and the vertical direction of the contact point between the operating mechanism and the ground tank, and F zz 、F zx are the forces F exerted by the robot operating mechanism on the side wall of the ground cylinder in the Z-axis direction z The vertical and tangential forces at the contact point, t is the time, λ is the control cycle of the robot operating mechanism, F xz (t-λ) is the force F exerted by the robot operating mechanism in the X-axis direction on the side wall of the ground cylinder in the previous control cycle x The vertical component of force at the contact point; α is the angle between the X-axis direction of the robot operating mechanism and the tangent direction of the contact point between the operating mechanism and the ground cylinder.

[0086] G grains Specifically:

[0087] G grains =ρ grains πR pot_bottom 2 H grains g

[0088] In the formula, ρ grains is the density of the fermented grains in the ground tank, R pot_bottom H is the radius of the bottom of the ground cylinder, grains is the height of the mash in the jar, and g is the acceleration due to gravity.

[0089] The beneficial effect of the present invention is that by controlling the path, posture and contact force of the robot operating mechanism with the environment during the aggregation and digging of the mash tailings in the ground tank, it is ensured that the robot operating mechanism maintains smooth contact with the ground tank during the operation process, preventing the robot operating mechanism from colliding with the ground tank and generating excessive friction to damage the ground tank, while accurately and efficiently cleaning the mash tailings in the ground tank. BRIEF DESCRIPTION OF THE DRAWINGS

[0090] Figure 1 It is a schematic diagram of the setting of the gathering point and the gathering range;

[0091] Figure 2 It is a schematic diagram of the initial point setting of the aggregation operation;

[0092] Figure 3 It is a schematic diagram of the movement of the operating mechanism toward the side wall of the ground cylinder during the gathering operation;

[0093] Figure 4 It is a schematic diagram of the operation mechanism moving along the side wall of the ground cylinder toward the bottom of the ground cylinder during the gathering operation;

[0094] Figure 5 It is a schematic diagram of the operation mechanism moving along the bottom of the ground cylinder toward the gathering point during the gathering operation;

[0095] Figure 6 It is a schematic diagram of the division of the ground cylinder area in the cycle operation of the aggregation operation;

[0096] Figure 7 It is a schematic diagram of the movement of the working mechanism along the side wall of the ground cylinder in the cyclic operation of the aggregation operation;

[0097] Figure 8 It is a schematic diagram of the movement of the working mechanism along the bottom of the ground cylinder in the cyclic operation of the aggregation operation;

[0098] Fig. 9 It is a schematic diagram of the movement of the working mechanism toward the bottom of the earthen jar during the excavation operation;

[0099] Fig.10 It is a schematic diagram of the movement of the operating mechanism along the bottom of the ground cylinder toward the gathering point during the excavation operation;

[0100] Fig.11 It is a schematic diagram of force analysis of the process of the working mechanism moving along the bottom of the ground cylinder during the excavation operation;

[0101] Fig.12 It is a schematic diagram of the operation mechanism moving along the side wall of the ground cylinder toward the ground cylinder mouth during the excavation operation;

[0102] Fig.13 It is a schematic diagram of force analysis of the process of the working mechanism moving along the side wall of the ground cylinder during the excavation operation;

[0103] In the figure, 1, material gathering point; 2, material gathering range; 3, operation initial point; 4, ground cylinder axis; 5, operating mechanism axis. DETAILED DESCRIPTION

[0104] In order to make the above-mentioned purposes, features and beneficial effects of the present invention more obvious and understandable, the technical scheme of the present invention will be clearly and completely described below in combination with the embodiments of the present invention and the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work belong to the scope of protection of the present invention.

[0105] Example

[0106] Aiming at the problem that there is a large amount of residual material and it is easy to collide and break when cleaning the tailings of the ground tank fermentation mash, a robot is used to collect and clean the tailings of the ground tank fermentation mash of the light-flavor liquor fermentation container of the winery based on an improved adaptive impedance control algorithm, and a method for cleaning the tailings of the ground tank fermentation mash based on improved adaptive impedance control is provided, including:

[0107] S1. Gathering operation of fermented grains in ground tanks:

[0108] S11, presetting the material gathering point coordinates, material gathering range and the initial point of the gathering operation;

[0109] (1) Setting of material gathering point coordinates and material gathering range

[0110] The mash gathering operation gathers the mash to the gathering point, and the mash digging operation digs the mash in the gathering range out of the ground tank. Therefore, it is necessary to set the gathering point and the gathering range in advance.

[0111] The material gathering range is a circle with the material gathering point as the center. In order to avoid collision between the robot operating mechanism and the side wall of the ground cylinder and ensure better material gathering effect, the circle where the material gathering range is located is inscribed in the circle where the bottom of the ground cylinder is located.

[0112] The specific setting rules for the gathering point and the gathering range are as follows:

[0113] Dgather_range =kL machine

[0114] Among them, D gather_range is the diameter of the aggregate range, L machine is the width of the robot operating mechanism. During the tailings excavation process, the operating mechanism can cover the area of ​​its structural width, and its arc-shaped front edge can completely fit the side wall of the ground cylinder; to avoid material spillage during the excavation process, the diameter of the material gathering range should be smaller than the width of the operating mechanism, so the contraction coefficient k is set;

[0115] The width of the robot operating mechanism is L. machine =70mm as an example, set the shrinkage coefficient The diameter of the aggregate range is D gather_range ≈46mm; the gathering point and the gathering range are as follows Figure 1 shown.

[0116] (2) Aggregation job initial point setting

[0117] The mouth of the ground cylinder is an inward-retracted structure. To avoid collision, the initial point of the operation needs to be inside the mouth of the ground cylinder, that is, the distance between the initial point of the operation and the axis of the ground cylinder is less than the radius of the ground cylinder mouth.

[0118] The distribution of the mash tails in the ground tank is characterized by low center area and high accumulation in the edge area. In order to avoid the mash in the edge area not being effectively collected, resulting in mash residue, the position of the contact point between the operating mechanism and the side wall of the ground tank should be higher than the highest point of the mash at the edge of the ground tank, that is, the highest point of the mash.

[0119] The specific setting requirements for the initial point of the aggregation job are:

[0120] (r gather_init <R pot_top )∩(h contact >max(H grains ))

[0121] Among them, r gather_init R is the distance between the initial point of the gathering operation and the axis of the ground cylinder, pot_top h is the radius of the ground cylinder mouth, contact H is the height between the contact point between the operating mechanism and the side wall of the ground cylinder and the bottom of the ground cylinder. grains is the height of the mash in the ground tank. The initial point of the aggregation operation is set as follows Figure 2 shown.

[0122] S12. The robot operating mechanism moves to the initial point of the aggregation operation under the control of the path planning algorithm.

[0123] S13, the robot operating mechanism moves from the initial operation point to the side wall of the ground cylinder under the action of the improved adaptive impedance control algorithm until a brief impact force occurs when the robot operating mechanism contacts the side wall of the ground cylinder;

[0124] The motion path of the robot operating mechanism is a straight line along the Z axis of the operating mechanism. During the motion, the posture of the robot operating mechanism is that the Z axis is vertically downward and the X axis points to the axis of the cylinder. The motion path and posture are as follows: Figure 3 As shown; during the movement, the Z axis keeps moving downward through force control, and the X axis does not move.

[0125] The robot operating mechanism carries initial kinetic energy when moving toward the ground cylinder. Therefore, a short impact force will be generated when the robot operating mechanism contacts the ground cylinder. This impact force is used to indicate that the operating mechanism has contacted the side wall of the ground cylinder. That is, when the contact force f between the robot operating mechanism and the environment is e (t) satisfies f e (t)>1.2f d_z (t), it indicates that the operating mechanism contacts the side wall of the floor cylinder.

[0126] At this stage, the expected contact force is only used to explore the position of the side wall of the ground cylinder, so the expected contact force can be set to a constant. The expected contact force setting requirements only need to ensure that it is not too large to damage the ground cylinder, and not too small to cause the sensor to be unable to recognize it. Since the maximum sensor noise is 1N,

[0127] 1.2f d_z (t)-f d_z (t)>1N

[0128] The expected contact force f in the Z-axis direction of this embodiment d_z (t) The specific range is set as:

[0129] f d_z (t) = f c >6N

[0130] In the formula, f c is the desired contact force constant.

[0131] S14, the robot operating mechanism keeps the Z-axis force controlled X-axis translational movement along the side wall of the floor cylinder toward the bottom of the floor cylinder under the action of the improved adaptive impedance control algorithm, until a sudden change occurs in the contact force between the robot operating mechanism and the floor cylinder;

[0132] In this operation phase, the robot operating mechanism needs to move in line with the side wall of the ground tank to ensure that the mash on the side wall of the ground tank is pushed down to the bottom of the ground tank. The movement path of the robot operating mechanism is an arc that fits the side wall of the ground tank. During the movement, the posture of the robot operating mechanism is that the Z axis is vertically downward and the X axis points to the axis of the ground tank. The movement path and posture are as follows: Figure 4 As shown; during the movement, the Z axis is kept moving by force control, and the X axis is kept moving by position control.

[0133] As the amount of mash accumulated near the bottom of the ground tank increases, in order to ensure that the mash on the side wall of the ground tank can be effectively gathered together, the expected contact force f in the Z-axis direction is d_z (t) should also increase accordingly, f d_z (t) Specifically:

[0134] f d_z (t) = k grains x side +f c

[0135] In the formula, k grains is the stiffness coefficient of the mash, k grains =8000~12000N / m,x side is the horizontal projection of the distance relative to the contact point during the movement of the working structure, f c is the desired contact force constant.

[0136] When the slope of the motion path of the robot operating mechanism changes suddenly, the robot operating mechanism will collide with the contact surface at the change point, and the contact force f between the operating mechanism and the environment will be e (t) produces a sudden change; due to the difference in slope between the side wall and the bottom of the ground cylinder, the slope sudden change point is the angle between the two. Therefore, the robot can use the contact force f between the operating mechanism and the environment e The sudden change of (t) can accurately locate the angle between the side wall and the bottom of the ground cylinder. e (t) satisfies f e (t)>1.2f d_z (t), it is considered that the operating mechanism touches the angle between the bottom and the side wall of the ground tank; because the robot operating mechanism can closely fit the angle between the bottom and the side wall of the ground tank under the control of the force control algorithm, when the material taking mechanism moves outward along the angle, it can effectively clean the mash at the angle between the bottom and the side wall of the ground tank.

[0137] S15. The robot operating mechanism moves toward the material gathering point along the bottom of the ground cylinder under the action of the improved adaptive impedance control algorithm, and the robot operating mechanism maintains the Z-axis force to control the X-axis translation.

[0138] Since the placement of the ground vats in the fermentation workshop is not uniform and cannot be guaranteed to be all horizontal, during this operation stage, the robot operating mechanism needs to move in line with the bottom of the ground vat under the action of the improved adaptive impedance control algorithm, so as to ensure that the mash at the bottom of the ground vat is gathered to the gathering point.

[0139] The motion path of the robot operating mechanism is a straight line that fits the bottom of the ground cylinder. The initial point is the angle between the bottom of the ground cylinder and the side wall, and the end point is the gathering point. During the motion, the posture of the computer operating mechanism is that the Z axis is vertically downward and the X axis points to the gathering point. The motion path and posture are as follows: Figure 5As shown; during the movement, the Z-axis force control is maintained to ensure contact with the bottom of the ground cylinder, and the X-axis is kept moving through position control. In this operation stage, the mash is pushed depending on the X-axis position control method, and the purpose of the Z-axis force control is to ensure that the robot operating mechanism always fits the bottom of the ground cylinder. Therefore, at this stage, the expected contact force of the Z-axis of the robot operating mechanism can be set to a constant, and there is no specific requirement. This embodiment sets the expected contact force f in the Z-axis direction d_z (t) = f c .

[0140] S16, adjusting the working path and working posture of the robot working mechanism, and looping the above-mentioned aggregation working process;

[0141] S161. Since the above-mentioned single aggregation operation can only aggregate the mash of one operation path, it is necessary to complete the aggregation operation of all the mash tails at the bottom of the ground tank through a cyclic aggregation operation. In the cyclic operation, the ground tank needs to be divided into several areas according to the width of the robot operation mechanism. The division rule is as follows:

[0142] n≈(360°-θ gather_range ) / θ end_vaild ,n=1,2,3...

[0143] Where n is the number of partitions, θ gather_range is the angle of the aggregate range at the bottom of the ground cylinder, θ end_vaild It is the angle corresponding to the effective operating area of ​​the operating mechanism at the bottom of the ground tank; since there is already accumulated mash within the aggregation range, the mash within the aggregation range will not be operated again during the circulation operation.

[0144] With θ end_vaild =25°,θ gather_range Taking ≈70° as an example, the ground tank can be divided into 11 parts, such as Figure 6 shown.

[0145] S162, when the robot operating mechanism moves along the side wall of the ground cylinder, the movement path and posture are as follows Figure 7 shown.

[0146] The movement path of the operating mechanism is an arc that fits the side wall of the ground cylinder, specifically the center line of each area. The initial point is the contact point between the operating mechanism and the side wall, and the end point is the angle between the side wall and the bottom of the ground cylinder. During the movement, the posture of the operating mechanism is that the Z axis is vertically downward, the X axis points to the axis of the ground cylinder, and the Y axis is perpendicular to the inner wall of the ground cylinder.

[0147] S163, when the robot operating mechanism moves along the bottom of the ground cylinder, the motion path and posture are as follows Figure 8 shown.

[0148] The motion path of the operating mechanism is a straight line that fits the bottom of the ground cylinder. The initial point is the end point of the ground cylinder side wall movement stage in the current area, and the end point is the material gathering point. The posture of the operating mechanism needs to change during the movement. The initial posture is that the Z axis is vertically downward, the X axis points to the axis of the ground cylinder, and the Y axis is perpendicular to the inner wall of the ground cylinder; the end posture is that the Z axis is vertically downward, and the X axis points to the material gathering point.

[0149] S2, digging out the fermented grains from the ground tank;

[0150] S21, presetting the initial point of the excavation operation;

[0151] Since the tailings are gathered in the gathering area during the tailings gathering operation, in order to cover all the gathered mash in the subsequent tailings digging operation, the initial point of the digging operation needs to be outside the gathering area. Therefore, the initial point setting requirements for the digging operation are:

[0152] r dig_init <(R pot_bottom -D gather_range )

[0153] Among them, r dig_init R is the distance between the initial point of the excavation operation and the axis of the ground cylinder, pot_bottom D is the radius of the bottom of the ground cylinder. gather_range is the diameter of the aggregate range; in this example, the initial operation point is set just above the center of the bottom of the ground cylinder.

[0154] S22, the robot operating mechanism moves to the initial point of the digging operation under the control of the path planning algorithm;

[0155] S23, the robot operating mechanism completes the tailings digging operation under the action of the improved adaptive impedance control algorithm;

[0156] S231, the robot operating mechanism moves toward the bottom of the ground cylinder under the action of the improved adaptive impedance control algorithm until a brief impact force occurs when the robot operating mechanism contacts the bottom of the ground cylinder;

[0157] The motion path of the operating mechanism is a straight line along the Z axis of the operating mechanism. During the motion, the posture of the operating mechanism is that the Z axis is vertically downward and the X axis points to the gathering point. The motion path and posture are as follows: Fig. 9 As shown, during the movement, the Z axis is kept moving by force control, and the X axis does not move.

[0158] At this stage, the expected contact force is only used to explore the bottom position of the ground cylinder, so the expected contact force in the Z-axis direction is f d_z (t) is set to:

[0159] f d_z (t) = f c

[0160] In the formula, fc is the desired contact force constant.

[0161] S232, the robot operating mechanism moves along the bottom of the ground cylinder toward the material gathering point under the action of the improved adaptive impedance control algorithm until a brief impact force occurs when the robot operating mechanism contacts the side wall of the ground cylinder;

[0162] During the movement, in order to move most of the mash at the gathering point to the robot operating mechanism, the posture of the robot operating mechanism should be as close to the bottom of the ground cylinder as possible. Therefore, during the movement of the robot operating mechanism along the bottom of the ground cylinder, the posture of the robot operating mechanism needs to be continuously adjusted until it touches the side wall of the ground cylinder. In this process, it is also necessary to prevent the robot operating mechanism from colliding with the ground cylinder due to improper posture changes. During the movement of the robot operating mechanism along the bottom of the ground cylinder, the movement path and posture are as follows: Fig.10 Specifically:

[0163] The motion path of the robot operating mechanism is a straight line that fits the bottom of the ground cylinder. The initial point is the contact point between the robot operating mechanism and the bottom of the ground cylinder, and the end point is the angle between the side wall and the bottom of the ground cylinder. During the motion process, the initial posture of the operating mechanism is that the Z axis is vertically downward, and the X axis points to the gathering point of the ground cylinder; the Y axis direction of the end posture remains unchanged, and there is an angle α between the Z axis and the horizontal direction bottom ,like Fig.10 As shown, specifically:

[0164] α bottom =α machine +α safe +α pot_bottom

[0165] In the formula, α machine is the angle between the back of the operating mechanism and the line connecting the midpoint of the front edge of the operating mechanism and the edge point of the connection end, α safe It is a safety angle, which is used to avoid collision between the working connection end and the ground cylinder. It is set according to the actual working environment. pot_bottom It is the angle between a point at the bottom of the ground cylinder and the cylinder bottom and a point at the cylinder mouth in the diameter direction.

[0166] α machine Specifically:

[0167]

[0168] In the formula, R machine is the radius of the connection end of the robot operating mechanism, R o_back is the distance between the axis of the robot's operating structure and its back, and L is the distance between the midpoint of the front edge of the robot's operating mechanism and the edge point of the connection end.

[0169]

[0170] In the formula, H pot is the height of the ground tank, D pot_top D is the diameter of the ground cylinder mouth pot_bottom The bottom diameter of the ground cylinder.

[0171] The force analysis of the contact point between the robot operating mechanism and the bottom of the ground cylinder is as follows: Fig.11 As shown, F z F is the force applied by the robot operating mechanism to the bottom of the cylinder in the Z-axis direction, zx and F zz are its components in the horizontal and vertical directions respectively. grains is the gravity of the mash pushed above the operating mechanism during its movement, F N is the support force of the bottom of the ground cylinder on the operating mechanism, f is the friction force of the bottom of the ground cylinder on the operating mechanism, F f It is the resistance of the tailings from the ground cylinder to the operating mechanism.

[0172] As the robot operating mechanism moves, more and more mash will be pushed above the robot operating mechanism, and the pressure on the robot operating mechanism will also increase. grains Specifically:

[0173] G grains =ρ grains L machine x bottom H grains g

[0174] In the formula, ρ grains is the density of the fermented grains in the ground tank, L machine is the width of the robot operating mechanism, x bottom H is the distance that the material taking mechanism moves relative to the axis of the ground cylinder, grains is the height of the mash in the jar, and g is the acceleration due to gravity.

[0175] The friction force f is positively correlated with the support force of the bottom of the ground cylinder on the working structure. f is specifically:

[0176] f=μ k F N =μ k (G grains +F zz (t-λ)

[0177] In the formula, μ k is the dynamic friction coefficient between the operating mechanism and the surface of the ground cylinder. Since the force F applied to the bottom of the ground cylinder in the Z-axis direction of the operating structure in the current robot control cycle cannot be z Calculate the component F at the contact point z , so through the previous control cycle F zThe components of the current control cycle are used to calculate F z , t is time, λ is the control period of the robot operation mechanism, F zz (t-λ) is the force F exerted by the operating structure on the bottom of the cylinder in the Z-axis direction in the previous control cycle z The vertical component at the contact point. The dynamic model of the wine mash tailings at the bottom of the ground tank can be approximated as a "spring" model, then the resistance F of the ground tank tailings on the robot operating mechanism is f for

[0178] F f =k grains x bottom

[0179] In the formula, k grains is the stiffness coefficient of the mash, x bottom It is the distance that the material taking mechanism moves relative to the axis of the cylinder.

[0180] As the robot operating mechanism moves toward the mash gathering point, the amount of mash accumulated gradually increases. In order to ensure that the operating mechanism can fit tightly to the bottom of the vat and effectively gather the mash onto the operating mechanism, the lateral thrust applied by the operating mechanism should increase accordingly.

[0181] F zx =f+F f

[0182] The expected contact force f exerted by the robot operating mechanism on the bottom of the cylinder in the Z-axis direction d_z (t) is:

[0183] f d_z (t) = F z =F zx / cosθ

[0184] In the formula, θ is the angle between the Z-axis direction of the working structure and the horizontal direction;

[0185] Right now In the formula, μ k is the dynamic friction coefficient between the operating mechanism and the surface of the ground cylinder, ρ grains is the density of the fermented grains in the ground tank, L machine is the width of the robot operating mechanism, x bottom H is the distance that the material taking mechanism moves relative to the axis of the ground cylinder, grains is the height of the mash in the ground tank, g is the acceleration of gravity, t is the time, λ is the control period of the robot operation mechanism, F zz (t-λ) is the force F exerted by the operating structure on the bottom of the cylinder in the Z-axis direction in the previous control cycle z The vertical component at the contact point; k grainsis the stiffness coefficient of the mash; θ is the angle between the Z-axis direction of the operating structure and the horizontal direction.

[0186] When the robot operating mechanism contacts the side wall of the ground cylinder, an impact force will be generated between the robot operating mechanism and the ground cylinder. This impact force is used to indicate that the operating structure contacts the side wall of the ground cylinder. e (t) satisfies f e (t)>1.2f d_z (t) indicates contact with the side wall of the floor cylinder.

[0187] S233, the robot operating mechanism moves along the side wall of the ground cylinder from the bottom of the ground cylinder to the mouth of the ground cylinder under the action of the improved adaptive impedance control algorithm; in this stage, the robot operating mechanism needs to fit the side wall of the ground cylinder, and control the operating mechanism to gradually move to a horizontal state during the movement toward the mouth of the ground cylinder; during the stage when the robot operating mechanism moves along the bottom of the ground cylinder, the movement path and posture are as follows Fig.12 Specifically:

[0188] The movement path of the operating mechanism is an arc that fits the side wall of the ground cylinder. The starting position is the end position of the previous stage. Since the mouth of the ground cylinder is an inward-retracted structure, inside the ground cylinder, the turning point of this inward-retracted structure is the farthest distance between the side wall of the ground cylinder and the axis. The mash in the operating mechanism can be pushed to the top of the operating mechanism through this inward-retracted structure. Therefore, the end position is the farthest distance between the side wall of the ground cylinder and the axis.

[0189] During the movement, the initial posture of the robot operating mechanism is the final posture of the previous stage. The Y-axis direction of the final posture remains unchanged, and there is an angle α between the Z-axis and the horizontal direction. side ,like Fig.12 As shown, specifically:

[0190] α side =α machine +α safe +α pot_side

[0191] In the formula, α machine is the angle between the back of the operating mechanism and the line connecting the midpoint of the front edge of the operating mechanism and the edge point of the connection end, α safe It is a safety angle, which is used to avoid collision between the working connection end and the ground cylinder. It is set according to the actual working environment. pot_side It is the angle between the point where the side wall of the ground cylinder is farthest from the axis and the line connecting this point and the point at the cylinder mouth in the diameter direction.

[0192] α machine This has been explained above and will not be repeated here. pot_side Specifically:

[0193]

[0194] In the formula, γ pot_rmax H is the ratio of the height of the farthest point from the axis to the floor cylinder opening to the floor cylinder height. pot is the height of the ground tank, D pot_top is the diameter of the ground cylinder mouth, R pot_max The distance between the farthest point of the side wall of the ground cylinder and the axis of the ground cylinder. The force analysis of the contact point between the working structure and the side wall of the ground cylinder is as follows: Fig.13 As shown, F z is the force applied by the working structure to the side wall of the ground cylinder in the Z-axis direction, and its tangential and vertical components at the contact point between the working structure and the side wall are F zx and F zz ; F x is the force applied by the working structure in the X-axis direction to the side wall of the ground cylinder, and its tangential and vertical components at the contact point are F xx and F xz ; G grains is the estimated value of the gravity of the mash above the operating mechanism during its movement; F N is the supporting force of the side wall of the floor cylinder on the operating mechanism, and f is the friction force of the bottom side wall of the floor cylinder on the operating structure.

[0195] The mash above the operating mechanism is the total amount of mash tailings at the bottom of the ground tank. The estimated value of the total weight of the tailings is G grains Specifically:

[0196] G grains =ρ grains πR pot_bottom 2 H grains g

[0197] In the formula, R pot_bottom is the radius of the bottom of the jar.

[0198] The friction force f is positively correlated with the support force of the side wall of the ground cylinder on the working structure. f is specifically:

[0199] f=μ k F N =μ k (G grains cosβ+F zz +F xz (t-λ)

[0200] In the formula, G grains cosβ is G grains The vertical force component at the contact point cannot be passed through F in the current control cycle. x Calculate the components of F x , t is time, λ is the control period of the robot operation mechanism, Fxz (t-λ) is the force F exerted by the operating structure in the X-axis direction on the side wall of the ground cylinder in the previous control cycle x The vertical component of force at the point of contact.

[0201] When the operating mechanism moves upward along the side wall of the ground cylinder, the amount of mash accumulated at the angle between the bucket and the side wall of the ground cylinder decreases, and the amount accumulated on the bucket increases. To ensure that the operating mechanism can move smoothly upward along the side wall of the ground cylinder, the operating mechanism needs to provide greater thrust in the upward direction along the tangent of the side wall.

[0202] F xx =f+F zx +G grains sinβ

[0203] In the formula, G grains sinβ is G grains The tangential component of force at the contact point.

[0204] The expected contact force f exerted by the robot operating mechanism on the side wall of the ground cylinder in the X-axis direction d_x (t) is:

[0205] f d_x (t) = F x =F xx / cosα

[0206] Wherein, α is the angle between the X-axis direction of the working structure and the tangent direction of the contact point between the working structure and the ground cylinder;

[0207] Right now In the formula, μ k G is the dynamic friction coefficient between the operating mechanism and the surface of the ground cylinder, grains is the estimated value of the gravity of the mash above the operating mechanism, β is the angle between the gravity direction and the vertical direction of the contact point between the operating structure and the ground tank, F zz 、F zx They are the forces F exerted by the working structure on the side wall of the ground cylinder in the Z-axis direction z The vertical and tangential forces at the contact point, t is the time, λ is the control cycle of the robot operating mechanism, F xz (t-λ) is the force F exerted by the operating structure in the X-axis direction on the side wall of the ground cylinder in the previous control cycle x The vertical component of force at the contact point; α is the angle between the X-axis direction of the working structure and the tangent direction of the contact point between the working structure and the ground cylinder.

[0208] G grains Specifically:

[0209] G grains =ρ grains πR pot_bottom2 H grains g

[0210] In the formula, ρ grains is the density of the fermented grains in the ground tank, R pot_bottom H is the radius of the bottom of the ground cylinder, grains is the height of the mash in the jar, and g is the acceleration due to gravity.

[0211] In this operation stage, the mash is pushed by the X-axis position control method, and the purpose of the Z-axis force control is to ensure that the operating mechanism always fits the bottom of the ground tank. Therefore, the Z-axis expected contact force can be set to a constant at this stage, and there is no specific requirement. d (t) = f c , f c is the desired contact force constant.

[0212] S24, the robot operating mechanism moves to the material discharge point through the path planning algorithm to complete the material discharge operation.

[0213] The idea of ​​impedance control is to add a "mass-spring-damper" virtual system at the end of the robot, so that the robot's movement can show the desired response characteristics when affected by the contact force, ensuring smooth contact between the robot and the environment. Among them, the improved adaptive impedance control algorithm is:

[0214]

[0215] In the formula, f e (t) is the actual contact force between the robot operating mechanism and the environment, which is obtained by real-time measurement of the six-dimensional force sensor at the end of the robot; f d (t) is the expected contact force between the robot operating mechanism and the ground cylinder. The rules for setting the expected contact force are given in each operation stage. , where m is the mass coefficient of the impedance control system, which affects the robot's acceleration response to external forces. Larger inertia will cause the robot to respond slower, but it can also improve stability and anti-interference capabilities; b is the damping coefficient of the impedance control system, which affects the robot's ability to absorb external forces and the shock absorption effect. Appropriate damping can reduce shock and vibration and improve control stability. m and b are irrelevant to the actual robot structure. Adjusting m and b can change the impedance effect of the impedance control system, that is, change the compliance of the contact between the robot operating mechanism and the ground cylinder.

[0216] It is the preset movement path for the robot operating mechanism in each operation stage; since the accurate position of the inner wall and bottom of the floor cylinder cannot be obtained in advance, the movement path cannot fully represent the specific position of the inner wall and bottom of the floor cylinder, but is only an estimate of the position of the inner wall and bottom of the floor cylinder. In order to maintain a certain contact force with the floor cylinder during the movement of the operating mechanism, it will adjust its own position according to the actual position of the inner wall and bottom of the floor cylinder, so the movement path is only a reference position for the operating mechanism.

[0217] The position of the path during the movement of the robot operating mechanism is related to time. and It is the first and second derivatives of the position change in the force control direction with respect to time.

[0218] To improve the output of the adaptive impedance control algorithm, that is, the actual acceleration of the robot's operating mechanism during movement. It is the actual speed of the robot's operating mechanism during movement. Doing double integral gives:

[0219]

[0220] x c It is the actual position of the operating mechanism during the moving process.

[0221] Among them, Δb(t) is the change of the damping coefficient, which is updated online according to the force error and its change rate. The update formula is:

[0222]

[0223] Where Δf(t) = f e (t)-f d (t), λ is the control period of the robot, η is the update rate, b p is the force error coefficient, b v is the force error change rate coefficient. To ensure the stability of the system, the value range of η is derived according to Lyapunov’s stability theorem.

[0224]

[0225] In the formula, k e It is the stiffness coefficient of the ground cylinder. However, due to the extremely large stiffness coefficient of the ground cylinder, in order to ensure the stability of the system, a soft material will be installed at the end of the operating mechanism. The stiffness coefficient of the soft material will be used as the stiffness coefficient of the ground cylinder.

[0226] The application process of the improved adaptive impedance control algorithm is as follows:

[0227] 1. Initialize f d (t), m, b, b p , bv ;

[0228] 2. Get the actual speed of the robot's operating mechanism and initialize it

[0229] 3. According to the preset path calculate

[0230] 4. Execute loop:

[0231] 1) The actual contact force f between the robot operating mechanism and the ground cylinder is measured by a six-dimensional force sensor e (t). The six-dimensional forces detected by the force sensor are three force components F x 、F y 、F z and three moment components M x 、M y 、M z , the six-dimensional forces are decoupled from each other and do not affect each other. Therefore, the impedance control algorithm can be used separately in each direction of the six-dimensional force. e (t) represents the contact force in a certain direction when the impedance control algorithm is used in that direction.

[0232] 2) Calculate the actual acceleration of the robot's operating mechanism

[0233] f d (t) Characterize the expected contact force in a certain direction when the impedance control algorithm is used in that direction, and provide the direction and expected contact force setting rules of the impedance control algorithm in each operation stage;

[0234] 3) Integrate and calculate the actual speed of the robot's operating mechanism

[0235] 4) Integrate and calculate the actual position of the robot's operating mechanism

[0236] 5) Control the robot's operating mechanism to move to position x c ;

[0237] 6)The loop ends.

[0238] This description is about the tailing cleaning control method of the ground tank taking robot in the fermentation plant based on the improved adaptive impedance control algorithm, but it is not limited to this scenario and can also be applied to environments with the same characteristics. The contents not described in detail in the specification belong to the prior art known to professional and technical personnel in the relevant fields. The present invention does not strictly limit the specific implementation method and scope, and the relevant modifications and descriptions within the scope of the relevant claims of the present invention are all protected by the patent of the present invention.

Claims

1. A method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control, characterized in that: The steps include: S1, gathering operation of fermented grains in ground tanks; S11, presetting the material gathering point coordinates, material gathering range and the initial point of the gathering operation; S12, the robot operation mechanism moves to the initial point of the aggregation operation under the control of the path planning algorithm; S13, the robot operating mechanism moves from the initial operation point to the side wall of the ground cylinder under the action of the improved adaptive impedance control algorithm until a brief impact force occurs when the robot operating mechanism contacts the side wall of the ground cylinder; S14, the robot operating mechanism keeps the Z-axis force controlled X-axis translation moving along the side wall of the ground cylinder toward the bottom of the ground cylinder under the action of the improved adaptive impedance control algorithm, until a sudden change occurs in the contact force between the robot operating mechanism and the side wall of the ground cylinder; S15, the robot operating mechanism moves along the bottom of the ground cylinder toward the material gathering point under the action of the improved adaptive impedance control algorithm, and the robot operating mechanism maintains the Z-axis force to control the X-axis translation; S16, adjusting the working path and working posture of the robot working mechanism, and looping the above-mentioned aggregation working process; S2, digging out the fermented grains from the ground tank; S21, presetting the initial point of the excavation operation; S22, the robot operating mechanism moves to the initial point of the digging operation under the control of the path planning algorithm; S23, the robot operating mechanism completes the tailings digging operation under the action of the improved adaptive impedance control algorithm; S231, the robot operating mechanism moves toward the bottom of the ground cylinder under the action of the improved adaptive impedance control algorithm until a brief impact force occurs when the robot operating mechanism contacts the bottom of the ground cylinder; S232, the robot operating mechanism moves along the bottom of the ground cylinder toward the material gathering point until a brief impact force occurs when the robot operating mechanism contacts the side wall of the ground cylinder; S233, the robot operating mechanism moves along the side wall of the ground cylinder from the bottom of the ground cylinder to the mouth of the ground cylinder under the action of the improved adaptive impedance control algorithm; S24, the robot operating mechanism moves to the material discharging point through the path planning algorithm to complete the material discharging operation; The improved adaptive impedance control algorithm is: In the formula, f e (t) is the actual contact force between the robot operating mechanism and the environment, f d (t) is the expected contact force between the robot operating mechanism and the environment, m is the mass coefficient of the robot operating mechanism impedance control system, and b is the damping coefficient of the robot operating mechanism impedance control system. A preset movement path for the robot operating mechanism at each operation stage; and The first and second derivatives of the position change in the force control direction with respect to time; To improve the output of the adaptive impedance control algorithm, i.e. the actual acceleration of the robot's operating mechanism during movement; It is the actual speed of the robot operating mechanism during movement; right Doing double integral gives: x c It is the actual position of the robot operating mechanism during the movement process; Among them, Δb(t) is the change of the damping coefficient, which is updated online according to the force error and its rate of change. The update formula is: Where Δf(t) = f e (t)-f d (t), λ is the control period of the robot operating mechanism, η is the update rate, b p is the force error coefficient, b v is the force error change rate coefficient. To ensure the stability of the system, the value range of η is In the formula, k e is the stiffness coefficient of the ground cylinder.

2. The method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control according to claim 1, characterized in that: The S11 specifically includes: (1) The specific setting rules for the gathering point and the gathering range are as follows: D gather_range =kL machine Among them, D gather_range is the diameter of the aggregate range, L machine is the width of the robot operating mechanism, and k is the contraction coefficient. (2) The specific setting requirements for the initial point of the aggregation operation are: (r gather_init <R pot_top )∩(h contact >max(H grains )) Among them, r gather_init R is the distance between the initial point of the gathering operation and the axis of the ground cylinder, pot_top h is the radius of the ground cylinder mouth, contact H is the height between the contact point between the operating mechanism and the side wall of the ground cylinder and the bottom of the ground cylinder. grains It is the height of the mash in the ground jar.

3. The method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control according to claim 1, characterized in that: The S13 specifically includes: The motion path of the robot operating mechanism is a straight line along the Z axis of the operating mechanism. During the motion, the operating mechanism posture is that the Z axis is vertically downward and the X axis points to the axis of the cylinder. During the motion, the Z axis is kept moving downward by force control, and the X axis does not move; Set the expected contact force f in the Z-axis direction d_z (t) range, when the contact force between the robot and the environment is f e (t)Satisfy: f e (t)>1.2f d_z (t) Indicates that the operating mechanism contacts the side wall of the floor cylinder.

4. The method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control according to claim 1, characterized in that: The S14 specifically includes: The motion path of the robot operating mechanism is an arc that fits the side wall of the floor cylinder. During the movement, the posture of the operating mechanism is that the Z axis is vertically downward and the X axis points to the axis of the floor cylinder. During the movement, the Z axis is kept moving by force control, and the X axis is kept moving by position control. Expected contact force f in the Z direction d_z (t) is set to f d_z (t)=k grains x side +f c In the formula, k grains is the stiffness coefficient of the mash, x side is the horizontal projection of the distance relative to the contact point during the movement of the robot operating mechanism, f c is the desired contact force constant. When the contact force between the robot and the environment is e (t)Satisfy: f e (t)>1.2f d_z (t) Indicates the angle between the bottom and side wall of the floor cylinder where the robot operating mechanism contacts.

5. The method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control according to claim 1, characterized in that: The S15 specifically includes: The motion path of the robot operating mechanism is a straight line that fits the bottom of the floor cylinder. The initial point is the angle between the bottom of the floor cylinder and the side wall, and the end point is the material gathering point. During the movement, the posture of the operating mechanism is that the Z axis is vertically downward and the X axis points to the material gathering point. During the movement, the Z axis force control is maintained to ensure contact with the bottom of the floor cylinder, and the X axis keeps moving through position control.

6. The method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control according to claim 1, characterized in that: The specific steps of S16 include: S161. First, the floor cylinder is divided into several areas according to the width of the robot operating mechanism. The division rule is: n≈(360°-θ gather_range ) / θ end_vaild ,n=1,2,3... Where n is the number of partitions, θ gather_range is the angle of the aggregate range at the bottom of the ground cylinder, θ end_vaild The angle corresponding to the effective operating area of ​​the robot operating mechanism at the bottom of the ground cylinder; S162, when the robot operating mechanism moves along the side wall of the ground cylinder, the movement path of the operating mechanism is an arc that fits the side wall of the ground cylinder, specifically the center line of each area, the initial point is the contact point between the operating mechanism and the side wall, and the end point is the angle between the side wall and the bottom of the ground cylinder; during the movement, the posture of the operating mechanism is that the Z axis is vertically downward, the X axis points to the axis of the ground cylinder, and the Y axis is perpendicular to the inner wall of the ground cylinder; S163. During the stage where the robot operating mechanism moves along the bottom of the ground cylinder, the movement path of the operating mechanism is a straight line that fits the bottom of the ground cylinder. The initial point is the end point of the stage where the side wall of the ground cylinder in the current area moves, and the end point is the material gathering point. During the movement, the posture of the operating mechanism needs to change. The initial posture is that the Z axis is vertically downward, the X axis points to the axis of the ground cylinder, and the Y axis is perpendicular to the inner wall of the ground cylinder. The end posture is that the Z axis is vertically downward, and the X axis points to the material gathering point.

7. The method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control according to claim 1, characterized in that: The requirements for setting the initial point of the material taking operation in S21 are: r dig_init <(R pot_bottom -D gather_range ) Among them, r dig_init R is the distance between the initial point of the excavation operation and the axis of the ground cylinder, pot_bottom D is the radius of the bottom of the ground cylinder. gather_range is the aggregate range diameter.

8. The method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control according to claim 1, characterized in that: The S231 specifically includes: The motion path of the robot operating mechanism is a straight line along the Z axis of the operating mechanism; during the movement, the posture of the robot operating mechanism is that the Z axis is vertically downward and the X axis points to the material gathering point; during the movement, the Z axis is kept moving by force control, and the X axis direction does not move. Set the expected contact force f in the Z-axis direction d_z (t) range, when the contact force f between the robot operating mechanism and the environment e (t)Satisfy: f e (t)>1.2f d_z (t) This indicates that the robot operating mechanism has touched the bottom of the floor tank.

9. The method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control according to claim 1, characterized in that: The S232 specifically includes: When the robot operating mechanism moves along the bottom of the ground cylinder, the movement path of the operating mechanism is a straight line that fits the bottom of the ground cylinder. The initial point is the contact point between the operating mechanism and the bottom of the ground cylinder, and the end point is the angle between the side wall and the bottom of the ground cylinder. During the movement, the initial posture of the operating mechanism is that the Z axis is vertically downward, and the X axis points to the gathering point of the ground cylinder. The Y axis direction of the end posture remains unchanged, and there is an angle α between the Z axis and the horizontal direction. bottom , specifically: In the formula, R machine R is the radius of the connection end of the operating mechanism. o_back The distance between the axis of the working structure and its back, L is the distance between the midpoint of the front edge of the working mechanism and the edge point of the connection end; α safe H is the safety angle, which is used to avoid collision between the working connection end and the ground cylinder. It should be set according to the actual working environment. pot is the height of the ground tank, D pot_top D is the diameter of the ground cylinder mouth pot_bottom It is the bottom diameter of the ground cylinder. The expected contact force f applied by the operating mechanism in the Z-axis direction to the bottom of the cylinder d_z (t) is: In the formula, μ k is the dynamic friction coefficient between the operating mechanism and the surface of the ground cylinder, ρ grains is the density of the fermented grains in the ground tank, L machine is the width of the robot operating mechanism, x bottom H is the distance that the material taking mechanism moves relative to the axis of the ground cylinder, grains is the height of the mash in the ground tank, g is the acceleration of gravity, t is the time, λ is the control period of the robot operation mechanism, F zz (t-λ) is the force F exerted by the robot operating mechanism in the Z-axis direction on the bottom of the cylinder in the previous control cycle z The vertical component at the contact point; k grains is the stiffness coefficient of the mash; θ is the angle between the Z-axis direction of the operating structure and the horizontal direction. When the contact force between the robot operating mechanism and the environment is e (t)Satisfy: f e (t)>1.2f d_z (t) This indicates that the robot operating mechanism has contacted the side wall of the floor cylinder.

10. The method for cleaning the tailings of fermented grains in a ground tank based on improved adaptive impedance control according to claim 1, characterized in that: The S233 specifically includes: When the robot operating mechanism moves along the side wall of the ground cylinder, the movement path of the robot operating mechanism is an arc that fits the side wall of the ground cylinder, and the end position is the farthest distance between the side wall of the ground cylinder and the axis; during the movement, the initial posture of the operating mechanism is the end posture of the previous stage, the Y-axis direction of the end posture remains unchanged, and there is an angle α between the Z-axis and the horizontal direction side , specifically: In the formula, R machine R is the radius of the connection end of the operating mechanism. o_back The distance between the axis of the working structure and its back, L is the distance between the front midpoint of the working mechanism and the edge of the connection end, α safe It is a safety angle, which is used to avoid collision between the working connection end and the ground cylinder. It is set according to the actual working environment. pot_rmax H is the ratio of the height of the farthest point from the side wall of the ground cylinder to the height of the ground cylinder. pot is the height of the ground tank, D pot_top is the diameter of the ground cylinder mouth, R pot_max The distance between the farthest point of the side wall of the ground cylinder and the axis of the ground cylinder; The expected contact force f exerted by the robot operating mechanism on the side wall of the ground cylinder in the X-axis direction d_x (t) is: In the formula, μ k is the dynamic friction coefficient between the robot operating mechanism and the surface of the ground cylinder, G grains is the estimated value of the gravity of the mash above the robot operating mechanism, β is the angle between the gravity direction and the vertical direction of the contact point between the operating mechanism and the ground tank, and F zz 、F zx are the forces F exerted by the robot operating mechanism on the side wall of the ground cylinder in the Z-axis direction z The vertical and tangential forces at the contact point, t is the time, λ is the control cycle of the robot operating mechanism, F xz (t-λ) is the force F exerted by the robot operating mechanism in the X-axis direction on the side wall of the ground cylinder in the previous control cycle x The vertical force component at the contact point; α is the angle between the X-axis direction of the robot operating mechanism and the tangent direction of the contact point between the operating mechanism and the ground cylinder. grains Specifically: G grains =ρ grains pR pot_bottom 2 H grains g In the formula, ρ grains is the density of the fermented grains in the ground tank, R pot_bottom H is the radius of the bottom of the ground cylinder, grains is the height of the mash in the jar, and g is the acceleration due to gravity.

Images