Irregular rotating object recognition and interaction algorithm

Abstract

The invention discloses an irregular rotating object recognition and interaction algorithm, which is composed of a blade speed calculation algorithm, a blade position scanning calculation algorithm and a robot trajectory offset calculation algorithm, and is characterized in that the blade speed calculation algorithm is used for calculating the blade speed and providing a theoretical basis for subsequent fixed-point stopping of blades; the blade position scanning calculation algorithm is used for calculating the spatial position of the blade, and the robot trajectory offset calculation algorithm is used for performing trajectory output offset. The problems that in the automatic control field of production equipment mixing machines for PBX pouring explosive preparation in existing warhead charging and composite solid propellant preparation in engine charging, due to the fact that the control logic of the mixing machines is complex, the mixing machines cannot be precisely stopped to any needed posture and position at a fixed point, and the production efficiency is low are solved. Therefore, the problem that the matched stirring paddle blade automatic cleaning and scraping equipment is difficult to operate conveniently is solved, and the interactive operation precision and efficiency of the robot on irregular rotating objects are improved to a great extent.

Description

Technical Field

[0001] The present invention relates to the field of automatic control of mixers, which are production equipment for preparing PBX cast explosives in warhead charges of weapons and ammunition and for preparing composite solid propellants in engine charges. Specifically, it relates to an algorithm for identifying and interacting with randomly rotating objects. Background Art

[0002] In the field of automatic control of mixers, which are production equipment for preparing PBX cast explosives in warhead charges and for preparing composite solid propellants in engine charges, due to the complex control logic of the mixer, it is impossible to accurately stop at a fixed point to any desired attitude and position, resulting in difficulty in realizing convenient operation for the supporting automated cleaning and scraping equipment system of the stirring paddle blades.

[0003] Publication No. CN 221815994 U discloses an automatic control fixed-point parking device for the blades of a vertical mixer, which performs visual recognition by pasting color marks to realize the spatial position recognition and positioning control of the stirring paddle, and realizes the support for the rough positioning structure of the target position of the blade.

[0004] Publication No. CN 117839523 A discloses an automatic cleaning device for the blades of a vertical mixer. By comparing and analyzing the deviation value between the real-time three-dimensional point cloud data recognized by visual feedback and a preset template, based on the deviation amount, it is used to control the blades of the vertical mixer with no operating rules to stop at a specified position and control the robot to perform trajectory operations.

[0005] Publication No. CN 118698410 A discloses an automatic cleaning control system for the blades of a vertical mixer. Based on the encoder reading the angle of the input shaft, the angle of the output blade is deduced, and a control logic scheme that approximately accepts the error range with weak periodicity is used to realize a control scheme for blade spatial positioning based on position calculation within a certain error range.

[0006] Due to the complex control logic of the mixer, the prior art cannot accurately position the stirring paddle blades and can only adopt a positioning scheme within an error range. At the same time, this positioning scheme needs to accept the continuous rotation of the positioning object in order to approximately adopt it under the condition of non-logical rotation. Generally, there are problems of low positioning accuracy and low positioning efficiency. At the same time, due to the approximate error acceptance scheme, the data is based on the historical accumulation of encoder feedback, there is a problem of cumulative error. As time goes by, the cumulative error will ultimately affect the control result, and it is an open-loop logic in terms of error control. Summary of the Invention

[0007] The present invention provides an algorithm for identifying and interacting with irregularly rotating objects, aiming to solve the problem in the field of automatic control of mixers in the production equipment for preparing PBX cast explosives in existing warhead charges and composite solid propellants in engine charges. Due to the complex control logic of the mixer, it is impossible to accurately stop at a fixed point to any desired attitude and position, resulting in difficulties in the convenient operation of the automated cleaning and scraping equipment for the mixing paddle blades.

[0008] To solve the above technical problems, the technical solution adopted by the present invention is: an algorithm for identifying and interacting with irregularly rotating objects, which consists of a paddle blade speed calculation algorithm, a paddle blade position scanning calculation algorithm, and a robot trajectory offset calculation algorithm. Among them,

[0009] The paddle blade speed calculation algorithm is used to calculate the paddle blade speed, providing a theoretical basis for the subsequent fixed-point stop of the paddle blade. The logic of the paddle blade speed recognition algorithm includes:

[0010] 1) Image acquisition: Continuously acquire image frames at a set frame rate through a scanning camera, extract relevant features of the rotating target from each frame of the image, and ensure the clear visibility of the feature points through image enhancement and filtering means after identifying the feature points;

[0011] 2) Template matching tracking: Use the algorithm to track the change of the position of the feature points over time, and record the angle or position of the feature points in each frame of the image using the template matching method;

[0012] 3) Angular velocity calculation: Based on the feature points, perform angle conversion based on polar coordinate conversion, perform time conversion based on the time interval between frames, and finally obtain the angular velocity based on the angle and time parameters;

[0013] 4) Smoothing filtering: Use relevant filtering algorithms such as Kalman filtering and mean filtering to reduce noise interference, output a smooth angular velocity curve, and obtain a stable speed output;

[0014] The paddle blade position scanning calculation algorithm is used to calculate the spatial position of the paddle blade. The algorithm logic mainly includes:

[0015] 1) Point cloud sampling: Obtain the complete point cloud data P from a 3D camera, and use voxel grid filtering to control the sampling density through the voxel size v;

[0016] 2) Statistical denoising: Calculate the average distance of the neighboring points around each point based on the Euclidean distance or k-nearest neighbor algorithm. After setting a threshold, if the neighborhood distance of the point is not within the threshold range, it is removed;

[0017] 3) Distance segmentation: Based on the distance information between the target and the background, use the depth threshold d min ≤d≤d maxFilter the point cloud, use the DBSCAN method to divide the point cloud into different clusters, and select the target by the size, centroid position, and geometric features of the clusters;

[0018] 4) Fast matching: After calculating the main direction of the point cloud based on principal component analysis, rotate the point cloud to align its main axis with the coordinate axes, and use ICP to determine the initial point cloud P source and the target point cloud P target Output the pose of the target point cloud;

[0019] 5) Calculate the grasping position: By extracting the surface normal vector of the point cloud, find flat areas or regular geometric features for target surface analysis;

[0020] 6) Grasping position coordinate transformation: Transform the coordinates of the grasping position to the base coordinate system of the robot arm, P 基座 = T·P 抓取 ;

[0021] The robot trajectory offset calculation algorithm is used to perform trajectory output offset, and the algorithm logic includes:

[0022] 1) Determine the key coordinate systems of the camera coordinate system C, the simulation reference coordinate system S, and the robot base coordinate system B;

[0023] 2) Obtain the calibration relationship T from the camera to the robot base C B and the transformation relationship of the simulation reference to the robot base T S B ;

[0024] 3) Through the position T of the actual target in the base coordinate system B 目标实际 = T C B * T C 目标 and the position T of the simulation target in the base coordinate system B 目标仿真 = T S B * T S 目标 , calculate the difference ΔT between the actual target and the simulation target = (T B 目标实际 )·(T B 目标仿真 ) -1 ;

[0025] 4) Each point P in the simulation trajectory S 轨迹 represents the trajectory point in the simulation reference coordinate system, and the trajectory point is transformed to the corresponding position of the actual target: P B 轨迹调整 = TS B *P S 轨迹 , adjust the trajectory points according to the deviation matrix: P B 轨迹调整 = T S B *P S 轨迹 , and finally output the actual trajectory points P in the base coordinate system B 轨迹实际 , for the robot to execute.

[0026] An algorithm for identifying and interacting with an irregularly rotating object designed by the present invention using the above technical solutions makes full use of the associated transformation relationship between the actual coordinates of the object (the blade of the mixer agitator) and the robot trajectory reference. On the premise that the actual coordinates of the object do not repeat regularly, as long as the target object is within a large tolerance range (the tolerance range exceeds 60 degrees, which is dozens of times higher than the tolerance range of a few tenths of a degree proposed in the related patents for approximate operations, and at the same time ensures the re-closed loop of each operation to prevent cumulative errors), the offset of the robot output trajectory can be adjusted to fit the actual state of the target, solving the problems in the prior art such as low positioning accuracy, long positioning cycle, and poor interaction accuracy in the positioning and implementation of interactions with targets without repeated regularities. Based on the existing sensors and equipment, through the implementation of control methods and algorithms, the interaction operation accuracy and efficiency of the robot for irregularly rotating objects can be greatly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It shows a schematic diagram of an algorithm for identifying and interacting with an irregularly rotating object of the present invention;

[0028] Figure 2 It shows a schematic diagram of an algorithm for calculating the speed of the blade of the mixer agitator of the present invention;

[0029] Figure 3 It shows a schematic diagram of an algorithm for calculating the position scanning of the blade of the mixer agitator of the present invention;

[0030] Figure 4 It shows a schematic diagram of the principle of the cleaning trajectory offset of the blade of the mixer agitator of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0031] The following specifically describes an algorithm for identifying and interacting with an irregularly rotating object of the present invention with reference to the accompanying drawings.

[0032] An algorithm for identifying and interacting with an irregularly rotating object of the present invention, see Figure 1 , and mainly consists of a blade speed calculation algorithm, a blade position scanning calculation algorithm, and a robot trajectory offset calculation algorithm.

[0033] The blade speed calculation algorithm of the present invention is used to calculate the blade speed, providing a theoretical basis for subsequent fixed-point stopping of the blade. Refer to Figure 2 , and the logic of the blade speed recognition algorithm mainly includes:

[0034] 1) Image acquisition: Continuously acquire image frames at a set frame rate through a scanning camera, extract relevant features of the rotating target from each frame of the image, and ensure that the feature points are clearly visible through means such as image enhancement and filtering after identifying the feature points (such as edges, color markers, or textures);

[0035] 2) Template matching tracking: Use an algorithm to track the change of the position of the feature points over time, and record the angle or position of the feature points in each frame of the image using the template matching method;

[0036] 3) Angular velocity calculation: Based on the feature points, perform angle conversion based on polar coordinate conversion, perform time conversion based on the time interval between frames, and finally obtain the angular velocity based on the angle and time parameters;

[0037] 4) Smoothing filtering: Use filtering algorithms such as Kalman filtering and mean filtering to reduce noise interference, output a smooth angular velocity curve, and obtain a stable speed output.

[0038] The blade position scanning calculation algorithm of the present invention is used to calculate the spatial position of the blade. Refer to Figure 3 , and the algorithm logic mainly includes:

[0039] 1) Point cloud sampling: Obtain the complete point cloud data P from a 3D camera, use voxel grid filtering (divide the space into cubes of a fixed size (voxels), and replace the point cloud within each voxel with its center point or average value), and control the sampling density through the voxel size v;

[0040] 2) Statistical denoising: Calculate the average distance of the surrounding neighborhood points for each point based on the Euclidean distance or k-nearest neighbor algorithm. After setting a threshold, if the neighborhood distance of the point is not within the threshold range, it is removed;

[0041] 3) Distance segmentation: Based on the distance information between the target and the background, use the depth threshold d min ≤d≤d max to screen the point cloud, and use the DBSCAN (density-based spatial clustering) method to divide the point cloud into different clusters, and select the target through the size, centroid position, and geometric features of the clusters;

[0042] 4) Fast matching: After calculating the main direction of the point cloud based on the principal component analysis (PCA), rotate the point cloud to align its main axis with the coordinate axes, and use the ICP (iterative closest point algorithm) to determine the initial point cloud P source and the target point cloud P targetOutput the pose (translation vector T and rotation matrix R) of the target point cloud;

[0043] 5) Calculate the grasping position: By extracting the surface normal vector of the point cloud, find flat areas or regular geometric features for target surface analysis; 6) Coordinate transformation of the grasping position: Transform the coordinates of the grasping position to the base coordinate system of the robot, P 基座 = T·P 抓取 .

[0044] After calculating the grasping position in the present invention, there must be a deviation from the preset grasping position, and the trajectory output offset needs to be calculated through the robot trajectory offset calculation algorithm. Refer to Figure 3 , and the algorithm logic mainly includes:

[0045] 1) Determine key coordinate systems such as the camera coordinate system (C), the simulation reference coordinate system (S), and the robot base coordinate system (B);

[0046] 2) Obtain the calibration relationship (T C B ) from the camera to the robot base and the transformation relationship (T S B ) from the simulation reference to the robot base;

[0047] 3) Calculate the difference ΔT between the actual target and the simulation target through the position T B 目标实际 = T C B *T C 目标 of the actual target in the base coordinate system and the position T B 目标仿真 = T S B *T S 目标 of the simulation target in the base coordinate system, where ΔT = (T B 目标实际 )·(T B 目标仿真 ); -1 ;

[0048] 4) Each point P S 轨迹 in the simulation trajectory represents a trajectory point in the simulation reference coordinate system. The trajectory point is transformed to the corresponding position of the actual target: P B 轨迹调整 = T S B *P S 轨迹 , and the trajectory point is adjusted according to the deviation matrix: P B 轨迹调整 = T SB *P S 轨迹 , the final actual trajectory point P in the base coordinate system B 轨迹实际 , for the robot to execute.

Claims

1. An irregularly rotating object recognition and interaction algorithm, characterized by It consists of blade speed calculation algorithm, blade position scanning calculation algorithm and robot trajectory offset calculation algorithm, among which: The blade speed calculation algorithm is used to calculate the blade speed and provide a theoretical basis for the subsequent fixed-point stop of the blade. The blade speed identification algorithm logic includes: 1) Image acquisition: The scanning camera continuously acquires image frames at a set frame rate, and extracts the relevant features of the rotating target from each frame. After identifying the feature points, image enhancement and filtering are used to ensure that the feature points are clear and visible; 2) Template matching tracking: Use an algorithm to track the position of feature points over time, and use template matching to record the angle or position of feature points in each frame of the image; 3) Angular velocity calculation: angle conversion is performed based on feature points and polar coordinate conversion, time conversion is performed based on the time interval between frames, and angular velocity is finally converted based on angle and time parameters; 4) Smoothing filtering: Use Kalman filtering, mean filtering and related filtering algorithms to reduce noise interference, output a smooth angular velocity curve, and obtain a stable velocity output; The blade position scanning calculation algorithm is used to calculate the spatial position of the blade. The algorithm logic mainly includes: 1) Point cloud sampling: Get the complete point cloud data P from the 3D camera, filter it using a voxel grid, and control the sampling density by the voxel size v; 2) Statistical denoising: Based on the Euclidean distance or k-nearest neighbor algorithm, the average distance of each point to its surrounding neighborhood points is calculated. After setting the threshold, if the neighborhood distance of the point is not within the threshold range, it will be removed; 3) Distance segmentation: Based on the distance information between the target and the background, the depth threshold d is used min ≤d≤d max Filter the point cloud and use the DBSCAN method to divide the point cloud into different clusters, and select the target by cluster size, centroid position, and geometric features; 4) Fast matching: After calculating the main direction of the point cloud based on principal component analysis, the point cloud is rotated so that its main axis is aligned with the coordinate axis, and the initial point cloud P is determined using ICP source and the target point cloud P target Output the pose of the target point cloud; 5) Calculate the grasping position: By extracting the surface normal vector of the point cloud, find the flat area or regular geometric features to analyze the target surface; 6) Grasping position coordinate conversion: Convert the coordinates of the grasping position to the base coordinate system of the robot arm, P 基座 =T·P 抓取 ; The robot trajectory offset calculation algorithm is used to perform trajectory output offset, and the algorithm logic includes: 1) Determine the key coordinate systems of the camera coordinate system C, the simulation reference coordinate system S and the robot base coordinate system B; 2) Obtain the calibration relationship T from the camera to the robot base C B The transformation T from the simulation reference to the robot base S B The conversion relationship; 3) The actual target position T in the base coordinate system B 目标实际 =T C B *T C 目标 The position T of the simulation target in the base coordinate system B 目标仿真 =T S B *T S 目标 , calculate the difference between the actual target and the simulation target ΔT = (T B 目标实际 )·(T B 目标仿真 ) -1 ; 4) Each point P in the simulation trajectory S 轨迹 Represents the trajectory point in the simulation reference coordinate system, and the trajectory point is transformed to the corresponding position of the actual target: P B 轨迹调整 =T S B *P S 轨迹 ,Adjust the trajectory points according to the deviation matrix: P B 轨迹调整 =T S B *P S 轨迹 , and finally output the actual trajectory point P in the base coordinate system B 轨迹实际 , for the robot to execute.

Images