Tank car tank opening self-service opening and closing system and opening and closing method thereof

Abstract

The invention relates to the technical field of automation, in particular to a tank car tank opening self-service switching system and a switching method thereof.The tank car tank opening self-service switching system comprises an information collection module, an upper computer and a butt joint AI module, the information collection module collects position information of a tank opening and a mechanical arm in real time, and the butt joint AI module comprises a track planning module, an interpolation calculation module and a real-time control module; the information acquisition module transmits shot real-time pictures to the upper computer, the butt joint AI module combines instructions to form instruction cache according to a generation sequence, and the trajectory planning module performs path planning on the instructions according to the instruction generation sequence. The real-time control module receives the path subjected to interpolation calculation processing and controls the mechanical arm to complete the operation of opening or closing the tank opening in real time, a homogeneous transformation matrix of a connecting rod is constructed through a D-H parameter method to calculate the absolute position of an end effector, and absolute accurate positioning is achieved; a touch sensor and a torque sensor of the end effector are used for controlling corresponding force, so that accurate control over the force is achieved.

Description

Technical Field

[0001] The present invention relates to the field of automation technology, in particular to a self-service opening and closing system of a tanker tank opening and a method thereof. Background Art

[0002] The traditional manhole device cover of a tank truck mainly has two opening and closing methods: flipping and translation. It is generally fastened by 3-point or 6-point claw positioning. The opening and closing process requires manual operation, and since the cover has a large dead weight, manual operation is time-consuming and laborious. In addition, the manhole device is generally set on the top of the tank body, at a large height from the ground. Especially in rainy and snowy weather, tank top operations pose a great safety hazard to related operations.

[0003] Among them, the automatic cover-opening robot arm is used in the manufacturing workshop to complete various tasks such as handling and loading and unloading. For example, in the wooden box automatic cover-opening robot arm solution designed by Truman, the truss robot arm cooperates with the production line to quickly move the box cover of the wooden box product to the designated position according to the system instructions, realizing the automatic cover-opening process of the wooden box; at the same time, the automatic cover-opening robot arm is also widely used in automobile manufacturing, electronic appliances, food and beverage, metal processing, shipbuilding, aerospace and other fields. In automobile manufacturing, they are used for the assembly of car doors, car hoods, tires and other parts; in the electronic appliance industry, they are used for the precise grasping and placement of parts; in the food and beverage industry, they are used for product packaging and sealing operations.

[0004] Although there are many types of robotic arms on the market, most of them still have many problems: such as complex structure, poor precision, and lack of control over the force of rotating the lid.

[0005] In order to address the above problems, there is an urgent need for a tank truck tank mouth self-service opening and closing system and a opening and closing method thereof. Summary of the invention

[0006] The object of the present invention is to provide a tank truck tank opening self-service opening and closing system and a closing and closing method thereof, so as to solve the problems raised in the above-mentioned background technology.

[0007] To achieve the above-mentioned purpose, a self-service opening and closing system for the tank mouth of a tank truck is provided, comprising an information acquisition module, a host computer and a docking AI module. The information acquisition module comprises a laser radar and a camera and collects the position information of the tank mouth and the robotic arm in real time. The docking AI module comprises a trajectory planning module, an interpolation calculation module and a real-time control module. The information acquisition module transmits the captured real-time picture to the host computer. The host computer sends several instruction combinations to the docking AI module through the built-in instruction module. The docking AI module forms an instruction cache for the instruction combination in the order of generation. The trajectory planning module plans the path of the instructions in the order of instruction generation. The interpolation calculation module processes the path. The real-time control module receives the path processed by the interpolation calculation and controls the robotic arm in real time to complete the operation of opening or closing the tank mouth.

[0008] Furthermore, the information acquisition module also includes a visual sensor, a depth sensor, a distance sensor, a torque sensor, a tactile sensor and a position sensor. The visual sensor is installed at the end of the robotic arm, and uses a camera to obtain an image of the can mouth and locate the position of the can mouth and the filling cap through 3D vision; The depth sensor cooperates with the visual sensor to provide depth information of the can mouth and surrounding objects; The distance sensors are installed on different joints of the robot arm to detect the distance between the can mouth and other objects; The position sensor is installed on the joints and the end effector of the robot arm, and is used to measure the angles of each joint of the robot arm and the position information of the end effector; The torque sensor is installed on the end effector of the robot arm and is used to detect the force and torque of the robot arm when grasping and rotating the capping; The tactile sensor is mounted on the gripper of the robot arm.

[0009] A tanker tank opening self-service opening and closing method, applied to the tanker tank opening self-service opening and closing system, comprises the following steps: Step 1: The information collection module collects the position information of the tank mouth and the robotic arm in real time; Step 2: Set the basic parameters of the robot arm, locate the joints and base of the robot arm in the local coordinate system, and use the DH parameter method to construct the homogeneous transformation matrix of each link from the base to the end effector ;

[0010] in, is the distance between the Z axes of two adjacent connecting rods, is the angle between the Z axes of two adjacent connecting rods, is the distance from the current link's Z axis to the next link's X axis, is the angle of rotation around the Z axis of the current link, that is, the joint variable; Step 3: Calculate the overall transformation matrix from the base coordinate system to the end effector coordinate system Tn +1, Tn +1= T 1⋅ T 2⋅...⋅ Tn ; Step 4: Calculate the The linear velocity and angular velocity of the joints, where the linear velocity , angular velocity ,in It is The Z-axis direction vector of the connecting rod, is the position vector of the end effector, It is The position vector of the end of the connecting rod; The linear and angular velocities of all joints are combined to construct the complete Jacobian matrix J, which is expressed as ; Step 5: Calculate the difference between the current end effector position and the target position, and use the pseudo-inverse or transpose of the Jacobian matrix to calculate the updated joint velocity; Step 6: Repeat steps 3, 4, and 5 until the error is less than a predetermined threshold or the maximum number of iterations is reached, and finally the end effector of the robot arm reaches the target position accurately; Step 7: The real-time control module executes instructions to open or close the tank opening.

[0011] Furthermore, in step 5, the calculation formula of the joint speed is as follows: θ · =J + ·Δ p , where Δp is the position error vector, θ · is the joint velocity, J + is the pseudo-inverse matrix of the Jacobian matrix J.

[0012] Furthermore, in step 3 to step 5, the most suitable position of the end effector to the tank opening is calculated in real time according to the real-time distance between the end effector and the tank opening collected by the information collection module.

[0013] Furthermore, in step 4, the joints can be divided into rotational joints or translational joints according to their movement mode. For rotational joints, their angular velocity , for the translation joint, its angular velocity is zero.

[0014] Compared with the prior art, the present invention has the following beneficial effects: Image processing technology is used to determine the type and size of the can mouth. After determining that the robotic arm can reach the location of the can mouth, the rotation angle of the rotation axis of each joint on the robotic arm is obtained through the position sensor. The joint angle is converted into the displacement distance of the end of the robotic arm through the inverse kinematics model of the robotic arm. The DH parameter method is used to construct the homogeneous transformation matrix of the connecting rod to calculate the absolute position of the end effector to achieve absolutely precise positioning. At the same time, the tactile sensor and torque sensor set on the end effector are used to control the corresponding force to ensure that the can mouth cover does not fall off and the gripping force is not too large. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 It is a structural block diagram of the present invention; Figure 2 is a flow chart of the steps of the present invention; Figure 3 It is a schematic diagram of the operation of the mechanical arm in the present invention to open the can cover in a rotating manner; Figure 4 It is a schematic diagram of the operation of the robot arm opening the can cover in a side-opening manner in the present invention. DETAILED DESCRIPTION

[0016] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0017] Those skilled in the art will appreciate that, unless otherwise stated, the singular forms "a", "an" and "the" used herein may also include plural forms. It should be further understood that the term "comprising" used in the specification of the present invention refers to the presence of the features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof.

[0018] See also Figure 1-Figure 4As shown, a self-service opening and closing system for the tank mouth of a tank truck is provided, including an information acquisition module, a host computer and a docking AI module. The information acquisition module includes a laser radar and a camera and collects the position information of the tank mouth and the mechanical arm in real time. The docking AI module includes a trajectory planning module, an interpolation calculation module and a real-time control module. The information acquisition module transmits the captured real-time picture to the host computer. The host computer sends several instruction combinations to the docking AI module through the built-in instruction module. The docking AI module forms an instruction cache for the instruction combination according to the generation order. The trajectory planning module plans the path of the instructions according to the order in which the instructions are generated. The interpolation calculation module processes the path. The real-time control module receives the path processed by the interpolation calculation and controls the mechanical arm in real time to complete the operation of opening or closing the tank mouth.

[0019] The information acquisition module further includes a visual sensor, a depth sensor, a distance sensor, a torque sensor, a tactile sensor and a position sensor. The visual sensor is installed at the end of the robotic arm, and uses a camera to obtain an image of the can mouth and locate the position of the can mouth and the filling cap through 3D vision. The depth sensor cooperates with the visual sensor to provide depth information of the can mouth and surrounding objects; The distance sensors are installed on different joints of the robot arm to detect the distance between the can mouth and other objects; The position sensor is installed on the joints and the end effector of the robot arm, and is used to measure the angles of each joint of the robot arm and the position information of the end effector; The torque sensor is installed on the end effector of the robot arm and is used to detect the force and torque of the robot arm when grasping and rotating the capping; The tactile sensor is mounted on the gripper of the robotic arm; It is worth noting that the robotic arm adopts a connecting rod structure design, with two joints at the shoulder, one joint at the elbow, and two joints at the wrist. The entire robotic arm can rotate by itself, its shoulder and elbow joints can swing, and the wrist can perform pitch adjustment and rotation. The robotic arm is driven by a servo motor, and the overall control is carried out in a closed-loop manner.

[0020] The precise positioning process of the robotic arm is as follows: First, a spatial coordinate system is established with the position of the robot arm's shoulder as the coordinate origin. At the same time, the position of the tanker's mouth is collected through the laser radar and camera in the information collection module. A camera coordinate system is established based on the position of the tanker's mouth. The origin of the camera coordinate system is the position of the camera. It is determined whether the robot arm can reach the position of the tanker's mouth. If it cannot reach the position, a reminder is issued. After determining that the robot arm can reach the position of the can mouth, the rotation angle of the rotation axis of each joint on the robot arm is obtained through the position sensor, and the joint angle is converted into the displacement distance of the end of the robot arm through the inverse kinematics model of the robot arm; By accumulating the displacement distance, the absolute position of the end effector of the robot arm is calculated; Use a camera to shoot the target object, use image processing technology to calculate the distance between the end effector of the robot arm and the can mouth, determine the moving direction and distance, and continuously calibrate; The next moving direction and distance are determined according to the distance between the end effector of the robot arm and the can opening and the angles of each joint axis of the robot arm, until the end effector reaches a suitable position near the can opening; Image processing technology is used to determine the type and size of the can mouth, and a suitable operation method is selected to open or close the can mouth. The types of can mouths include but are not limited to threaded can mouths, snap-on can mouths, or plug-in can mouths. The operation method is pre-set and trained. When the end effector contacts the can mouth, the tactile sensor and the torque sensor control the corresponding force to ensure that the can mouth cover does not fall off and the gripping force is not too large.

[0021] A tanker tank opening self-service opening and closing method, applied to the tanker tank opening self-service opening and closing system, comprises the following steps: Step 1: The information collection module collects the position information of the tank mouth and the robotic arm in real time; Step 2: Set the basic parameters of the robot arm, including the joint angle, position, maximum force limit and movement speed of the robot arm. At the same time, locate the joints and base of the robot arm in the local coordinate system, and use the DH parameter method to construct the homogeneous transformation matrix of each link from the base to the end effector. ;

[0022] in, is the distance between the Z axes of two adjacent connecting rods, is the angle between the Z axes of two adjacent connecting rods, is the distance from the current link's Z axis to the next link's X axis, is the angle of rotation around the Z axis of the current link, that is, the joint variable; in this step, the joints and base of the robot are positioned in the local coordinate system, and then the four parameters of each link are determined, namely, , , and .

[0023] Step 3: Calculate the overall transformation matrix from the base coordinate system to the end effector coordinate system Tn+1, Tn +1= T 1⋅ T 2⋅...⋅ Tn ; Step 4: Calculate the The linear velocity and angular velocity of the joints, where the linear velocity , angular velocity ,in It is The Z-axis direction vector of the connecting rod, is the position vector of the end effector, It is The position vector of the end of the connecting rod; The linear and angular velocities of all joints are combined to construct the complete Jacobian matrix J, which is expressed as ; Step 5: Calculate the difference between the current end effector position and the target position, and use the pseudo-inverse or transpose of the Jacobian matrix to calculate and update the velocity of each joint; Step 6: Repeat steps 3, 4, and 5 until the error is less than a predetermined threshold or the maximum number of iterations is reached, and finally the end effector of the robot arm reaches the target position accurately; Step 7: The real-time control module executes instructions to open or close the tank opening.

[0024] In actual application, in step 5, the calculation formula of the joint speed is as follows: θ · =J + ·Δ p , where Δp is the position error vector, θ · is the joint velocity, J + is the pseudo-inverse matrix of the Jacobian matrix J; In step 3 to step 5, the most suitable position of the end effector to the tank opening is calculated in real time according to the real-time distance between the end effector and the tank opening collected by the information collection module; In step 4, the joints can be divided into rotational joints or translational joints according to their movement mode. For rotational joints, their angular velocity , for the translation joint, its angular velocity is zero.

[0025] In the process of controlling the force of the robot arm, the force detection algorithm is used to control the force. The end effector of the robot arm is equipped with a torque sensor, which uses the USB bus and the sensor interface to read the raw data. The digital filter is used to remove high-frequency noise and subtract the bias (zero offset) from the signal to eliminate the inherent error of the sensor and perform normalization processing. The three force channels f are obtained while retaining the useful signal frequency.x 、F y 、F z and three torque channels T x , T y , T z Channel data, according to the extracted features, design the force control strategy, use the PID controller to adjust the force of the robot arm, set the maximum output force allowed, the code is as follows: class PIDController: def __init__(self, kp, ki, kd): self.kp = kp self.ki = ki self.kd = kd self.last_error = 0 self.integral = 0 def update(self, error, dt): self.integral += error * dt derivative = (error - self.last_error) / dt output = self.kp * error + self.ki * self.integral + self.kd* derivative self.last_error = error return output The laser radar and camera use the OpenCV library to perform human body detection and human body tracking, and the main algorithms are as follows: # Initialize the camera cap = cv2.VideoCapture(0) while True: ret, frame = cap.read() if not ret: print("Unable to read") break gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # scaleFactor: controls the ratio of image size reduction each time # minNeighbors: The number of neighbors required for each human detection window # minSize: minimum size of the detection window bodies=body_cascade.detectMultiScale(gray,scaleFactor=1.1,minNeighbors=5, minSize=(30, 30)) for (x, y, w, h) in bodies: cv2.rectangle(frame, (x, y), (x+w, y+h), (255, 0, 0), 2) cv2.imshow('Protect-People', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break cap.release() When the environment is detected to be unqualified, the robot arm will automatically stop running.

[0026] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited by the above embodiments. The above embodiments and descriptions are only preferred examples of the present invention and are not intended to limit the present invention. Without departing from the spirit and scope of the present invention, the present invention may have various changes and improvements, which fall within the scope of the present invention. The scope of protection of the present invention is defined by the attached claims and their equivalents.

Claims

1. A self-service opening and closing system for a tanker tank opening, characterized in that: It includes an information acquisition module, a host computer and a docking AI module. The information acquisition module includes a laser radar and a camera and collects the position information of the tank mouth and the robotic arm in real time. The docking AI module includes a trajectory planning module, an interpolation calculation module and a real-time control module. The information acquisition module transmits the captured real-time image to the host computer. The host computer sends several instruction combinations to the docking AI module through the built-in instruction module. The docking AI module forms an instruction cache for the instruction combination according to the generation order. The trajectory planning module plans the path of the instructions according to the order in which the instructions are generated. The interpolation calculation module processes the path. The real-time control module receives the path processed by the interpolation calculation and controls the robotic arm in real time to complete the operation of opening or closing the tank mouth.

2. The self-service opening and closing system of a tanker tank opening according to claim 1, characterized in that: The information acquisition module also includes a visual sensor, a depth sensor, a distance sensor, a torque sensor, a tactile sensor and a position sensor. The visual sensor is installed at the end of the robotic arm, and uses a camera to obtain the image of the can mouth and locate the position of the can mouth and the filling cap through 3D vision; The depth sensor cooperates with the visual sensor to provide depth information of the can mouth and surrounding objects; The distance sensors are installed on different joints of the robot arm to detect the distance between the can mouth and other objects; The position sensor is installed on the joints and the end effector of the robot arm, and is used to measure the angles of each joint of the robot arm and the position information of the end effector; The torque sensor is installed on the end effector of the robot arm and is used to detect the force and torque of the robot arm when grasping and rotating the capping; The tactile sensor is mounted on the gripper of the robot arm.

3. A tanker tank opening self-service opening and closing method, applied to the tanker tank opening self-service opening and closing system according to any one of claims 1-2, characterized in that: The following steps are involved: Step 1: The information collection module collects the position information of the tank mouth and the robotic arm in real time; Step 2: Set the basic parameters of the robot arm, locate the joints and base of the robot arm in the local coordinate system, and use the DH parameter method to construct the homogeneous transformation matrix of each link from the base to the end effector ; 4. Among them, is the distance between the Z axes of two adjacent connecting rods, is the angle between the Z axes of two adjacent connecting rods, is the distance from the current link's Z axis to the next link's X axis, is the angle of rotation around the Z axis of the current link, that is, the joint variable; Step 3: Calculate the overall transformation matrix from the base coordinate system to the end effector coordinate system Tn +1, Tn +1= T 1⋅ T 2⋅...⋅ Tn ; Step 4: Calculate the The linear velocity and angular velocity of the joints, where the linear velocity , angular velocity ,in It is The Z-axis direction vector of the connecting rod, is the position vector of the end effector, It is The position vector of the end of the connecting rod; The linear and angular velocities of all joints are combined to construct the complete Jacobian matrix J, which is expressed as ; Step 5: Calculate the difference between the current end effector position and the target position, and use the pseudo-inverse or transpose of the Jacobian matrix to calculate the updated joint velocity; Step 6: Repeat steps 3, 4, and 5 until the error is less than a predetermined threshold or the maximum number of iterations is reached, and finally the end effector of the robot arm reaches the target position accurately; Step 7: The real-time control module executes instructions to open or close the tank opening.

5. The tanker mouth self-service opening and closing system recovery system according to claim 3, characterized in that: In step 5, the calculation formula of the joint speed is as follows: θ · =J + ·Δ p , where Δp is the position error vector, θ · is the joint velocity, J + is the pseudo-inverse matrix of the Jacobian matrix J.

6. The tanker mouth self-service opening and closing system recovery system according to claim 3, characterized in that: In step 3 to step 5, the most suitable position of the end effector to the tank opening is calculated in real time according to the real-time distance between the end effector and the tank opening collected by the information collection module.

7. The tanker mouth self-service opening and closing system recovery system according to claim 3, characterized in that: In step 4, the joints can be divided into rotational joints or translational joints according to their movement mode. For rotational joints, their angular velocity , for the translation joint, its angular velocity is zero.

Images