Industrial motion control method and system based on embedded system

Abstract

The present invention relates to the field of robot industrial motion control technology, and discloses an embedded-based industrial motion control method and system. First, singularity identification of a robot mechanical arm is performed, and the matrix of the mechanical arm is determined, and the speed of the mechanical arm end effector is described, including the relationship between the linear velocity and the angular velocity and the joint velocity, which is the mechanical arm joint angle; the condition number is calculated, and the singularity is detected, and the degree of closeness to singularity is evaluated by calculating the condition number of the matrix, and the singularity is processed by pseudo-inverse; dynamic path planning is performed, and a model of the current workspace is constructed through the monitored mechanical arm sensor data; the optimal path is calculated, the execution path is selected, and the path planning is performed. The present invention significantly improves the performance of the industrial motion control system, so that it can operate efficiently, safely and reliably in a complex industrial environment, and improves the performance, reliability and adaptability of the system.

Description

Technical Field

[0001] The present invention relates to the technical field of robot industrial motion control, and in particular to an embedded-based industrial motion control method and system. Background Art

[0002] Controlling the motion of a robot arm through an attitude singularity, also known as a singularity or singularity, actually refers to a special case encountered in robot kinematics. When the robot arm is in certain positions and attitudes, the configuration of its joints will cause the loss of directional control ability of one or more degrees of freedom. In this case, we say that the robot arm has encountered a singularity.

[0003] Kinematic analysis: The kinematics of the robot arm is divided into forward kinematics and inverse kinematics. Forward kinematics is to calculate the position and posture of the end effector based on the given joint angles; while inverse kinematics is to solve the required joint angles from the given end effector position and posture. Inverse kinematics is particularly important when dealing with attitude singularity problems, because it is necessary to find the appropriate joint angles to avoid singularities.

[0004] Unpredictable behavior near singularities: When the robot approaches a singularity, a small change in input can lead to a large change in the end effector position, causing unstable behavior. In some cases, even using methods such as pseudo-inverse to approximate the inverse kinematics can result in unnatural movements or sudden changes in velocity.

[0005] Increased difficulty in path planning: Path planning needs to avoid singular points, which increases the complexity of the algorithm and the requirements for environmental perception. Especially in dynamic environments, plans need to be adjusted quickly to cope with emerging situations while staying away from singular points.

[0006] Therefore, to address the above problems, embedded industrial motion control methods and systems are needed. Summary of the invention

[0007] The purpose of the present invention is to provide an embedded-based industrial motion control method and system. The present invention significantly improves the performance of the industrial motion control system by comprehensively using sensor data, data processing algorithms, path planning strategies and control methods, enabling it to operate efficiently, safely and reliably in a complex industrial environment, thereby improving the performance, reliability and adaptability of the system. The present invention is implemented as follows:

[0008] The present invention provides an embedded industrial motion control method, which is specifically performed according to the following steps:

[0009] S1: Identify the singular points of the robot arm. First, determine the matrix of the robot arm. , and describe the relationship between the velocity of the end effector of the robot arm, including the linear velocity, angular velocity and joint velocity, as shown in formula (1);

[0010] Formula (1)

[0011] in, is the velocity vector of the end effector of the robot arm, is the joint velocity vector, is the joint angle of the robot arm;

[0012] S2: Calculate the condition number and detect singular points. Specifically, the condition number of the matrix is ​​calculated to evaluate its degree of closeness to singularity. The condition number is defined as the ratio of the maximum singular value to the minimum singular value, as shown in formula (2).

[0013] Formula (2)

[0014] in, is the condition number;

[0015] S3: setting a threshold for the condition number and detecting whether the condition number exceeds the threshold. When it is detected that the condition number exceeds the preset threshold, the singular point is processed by pseudo-inverse;

[0016] Please follow the steps below:

[0017] S3.1: When the condition number exceeds the preset threshold, the pseudo-inverse is calculated by decomposing the matrix The pseudo-inverse ; Through singular value decomposition as formula (3.1)-formula (3.2);

[0018] Formula (3.1)

[0019] in, and is an orthogonal matrix, is a diagonal matrix containing The singular values ​​of

[0020] Formula (3.2)

[0021] in, yes The pseudo-inverse of is calculated by taking the inverse of the diagonal elements and transposing them;

[0022] S3.2: Using the Pseudo-Inverse Approximate solution of robot arm joint velocity , as shown in formula (3.3);

[0023] Formula (3.3)

[0024] in, is the velocity vector at the end of the robot arm;

[0025] S3.3: Through the objective function and auxiliary vector Optimize joint configuration, including minimizing joint speed and reducing energy consumption, as shown in formula (3.4);

[0026] Formula (3.4)

[0027] in, is the identity matrix, is the null space projection matrix;

[0028] S3.4: In discrete time steps Next, the joint velocity is integrated Convert to joint angle , as shown in formula (3.5);

[0029] Formula (3.5).

[0030] S4: Perform dynamic path planning. Build a model of the current workspace by monitoring the robot sensor data and obstacle data in the space, including the location and shape of obstacles and trajectory prediction of dynamic objects. The specific steps are as follows:

[0031] S4.1: Use sensors including lidar, cameras, and ultrasonic sensors to collect obstacle information in the workspace, continuously track dynamic objects, and predict their future positions; for moving obstacles, use historical trajectory data and motion models to predict their future positions, as shown in formula (4.1);

[0032] Formula (4.1)

[0033] in, is the state vector, including position and velocity, To control the input data, is the process noise, and is the system matrix.

[0034] S4.2: Filter and smooth the raw sensor data, and extract the geometric features of the obstacle from the processed data, including the bounding box and shape outline;

[0035] First, the raw data is continuously collected from each sensor, including the point cloud data provided by the lidar and the image frames provided by the camera; the above data is filtered, as shown in formula (4.2);

[0036] Formula (4.2)

[0037] in, is the data status value; is the Kalman gain, is the measured value, is the observation matrix;

[0038] Then the filtered data is cleaned of data noise, as shown in formula (4.3);

[0039] Formula (4.3)

[0040] in, is the window size, is the input signal, is the output signal;

[0041] Then, data feature extraction is performed, and edge detection is performed on the image data obtained by the camera to identify the outline of the object;

[0042] For point cloud data, use the RANSAC algorithm or least squares method to fit the bounding box or shape of the obstacle, as shown in formula (4.4);

[0043] Formula (4.4)

[0044] in, and They are rotation and translation transformations, and is the corresponding point cloud data.

[0045] S4.3: Integrate the extracted obstacle information into a unified map representation, where the map is a raster map, a point cloud map, or a topological map.

[0046] S5: Calculate the best path, select the execution path, and execute path planning.

[0047] Please follow the steps below:

[0048] S5.1: Define the cost function based on path length or time; as in formula (5.1);

[0049] Formula (5.1)

[0050] in, is the distance between two points, and are adjacent nodes on the path;

[0051] S5.2: Avoid approaching singular points by inserting a penalty term of the matrix condition number; wherein the penalty term first calculates the matrix condition number by singular value decomposition; as shown in formula (5.2);

[0052] Formula (5.2)

[0053] in, and are the maximum and minimum singular values ​​of the Jacobian matrix, respectively;

[0054] Then, the penalty term is specifically set by setting a threshold and setting a condition number threshold ,when When , it is judged that the point is close to a singular point; in the path planning process, if the Jacobian matrix condition number corresponding to a node exceeds the threshold, the corresponding penalty term is added;

[0055] S5.3: combining the energy consumption value and smoothness in the path planning process in step S5.2, combining all cost items to form a total cost function;

[0056] S5.4: Compute the Jacobian matrix for a given joint angle , calculate the Jacobian matrix ; As formula (5.4);

[0057] Formula (5.4)

[0058] in, It is the mapping function from the robot joint angle to the end effector position.

[0059] Furthermore, in the above data feature extraction, the image is first smoothed using a filter, as shown in formula (6);

[0060] Formula (6)

[0061] in, is the standard deviation;

[0062] Then calculate the place where the pixel value changes most dramatically in the image, that is, the potential edge; as shown in formula (6.1);

[0063] Formula (6.1)

[0064] in, and They are the gradients of the image in the horizontal and vertical directions respectively;

[0065] Then, the edges are refined to keep only the local maximum points; each pixel is compared with its two neighbors along the gradient direction, and if the current pixel is not a local maximum, it is set to zero;

[0066] Then by setting two thresholds, the low threshold and high threshold ;

[0067] Determine if the gradient strength of a pixel is higher than , it is considered a strong edge. If the gradient strength of a pixel is between and , and is connected to a strong edge, it is considered a weak edge;

[0068] Then perform fitting, count the number of inliers, and define a distance threshold , determine whether a point belongs to the fitted model;

[0069] Calculate the distance from all points to the fitted model, if the distance is less than , then the point is considered to be an interior point, and the number of interior points is counted ;

[0070] Repeat the above steps and record the model with the largest number of inliers until the best model is found to identify image features.

[0071] Furthermore, after calculating the Jacobian matrix, path planning is directly performed, the starting point and the target point are set, and the data structure of the path planning algorithm is initialized;

[0072] Randomly sample new points, expand the tree and check for collisions, and use the total cost function to evaluate the quality of the new path;

[0073] And starting from the starting point, expand the nodes according to the heuristic function until reaching the target point, and also use the total cost function to evaluate the path; find a safe path from the starting point to the target point, and output it as the final result. If there are still potential singular points on the path, repeat the above steps until a suitable path is found;

[0074] And continuously adjust the path online, first continuously collect new environmental data from lidar and cameras; then integrate the new obstacle information into the existing map representation, and update the raster map, point cloud map or topological map; as shown in formula (8.1);

[0075] Formula (8.1)

[0076] in, is the grid state, is the measured value, is the position of the robotic arm;

[0077] Compare the current map model with the previously stored map model to detect new obstacles or removed obstacles; track dynamic objects, and for moving obstacles, predict their future positions according to formula (8.2) and update this information on the map, as shown in formula (8.2);

[0078] Formula (8.2)

[0079] in, is the state transition matrix, is the control input matrix, is the control vector;

[0080] Then check whether the current path collides with the newly appeared obstacle, including singular point detection, recalculate the Jacobian matrix condition number corresponding to each point on the current path, and determine whether there is a situation close to a singular point;

[0081] And perform cost evaluation, re-evaluate the cost of the current path based on the new environmental information, including path length, energy consumption, and smoothness data;

[0082] Then the path is replanned. If it is found that the path has a collision risk or potential singularity, or there is a better path choice, the replanning process is triggered;

[0083] Re-plan the path according to the current situation; redefine the cost function, combine the new environmental information, and redefine the total cost function including the singularity penalty term and other cost factors;

[0084] Perform path planning, run the selected path planning algorithm, generate a new safe and feasible path, smooth the newly generated path; and perform speed control, adjust the speed of the robot arm to ensure a smooth transition from the old path to the new path.

[0085] Furthermore, the present invention provides an embedded-based industrial motion control system, which uses a high-performance computing unit, including an embedded processor or FPGA, and can quickly process a large amount of sensor data to ensure real-time performance.

[0086] Online path adjustment: the system can quickly adjust the path according to environmental changes to ensure timely response in dynamic environments;

[0087] Specifically, it includes a sensor module for collecting environmental data, including data from lidar, cameras, and ultrasonic sensors;

[0088] Data preprocessing module, which filters and smoothes the raw sensor data to remove noise and outliers;

[0089] Feature extraction module, which extracts geometric features from preprocessed data, including edge computing;

[0090] The environment modeling module integrates the extracted features into a unified map representation, including a raster map, a point cloud map, or a topological map;

[0091] Trajectory prediction module, for dynamic objects, uses historical trajectory data to predict their future positions;

[0092] The path planning module generates a safe path from the starting point to the target point based on the current environment model and task requirements;

[0093] The control and execution module converts the planned path into specific joint angles or motor commands and sends them to the actuators;

[0094] The monitoring and feedback module monitors the system status in real time and provides feedback to adjust the control strategy.

[0095] Furthermore, the present invention provides a computer-storable medium, wherein the computer-readable storage medium includes an embedded processing system and a stored program, and when the embedded system control program runs, the device where the computer-readable storage medium is located is controlled to execute any of the embedded-based industrial motion control methods described above.

[0096] Compared with the prior art, the present invention has the following beneficial effects:

[0097] (1) By comprehensively using sensor data, data processing algorithms, path planning strategies and control methods, the performance of industrial motion control systems has been significantly improved, enabling them to operate efficiently, safely and reliably in complex industrial environments, improving the performance, reliability and adaptability of the system.

[0098] (2) Through the pseudo-inverse method, the system can effectively avoid singular points and avoid unstable behavior caused by singular points. By integrating multiple types of sensors (lidar or cameras), the system can perceive the environment more comprehensively and improve its adaptability to different application scenarios.

[0099] (3) The system can monitor environmental changes in real time and avoid new obstacles by replanning the path, thereby improving the robustness and safety of the system. BRIEF DESCRIPTION OF THE DRAWINGS

[0100] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments will be briefly introduced below. It is understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0101] Figure 1 is a flow chart of the method of the present invention;

[0102] Figure 2 is a speed planning diagram in an embodiment of the present invention;

[0103] Figure 3 is the environmental modeling diagram of the present invention;

[0104] Figure 4 is a joint position planning diagram of the robot arm of the present invention;

[0105] Figure 5 It is a simulation diagram of the angles of the starting point and the end point of the robot arm joint of the present invention. DETAILED DESCRIPTION

[0106] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, 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 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 making creative work belong to the scope of protection of the present invention. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the invention claimed for protection, but is only for the selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work belong to the scope of protection of the present invention.

[0107] See also Figure 1-Figure 5 ,The present invention provides an industrial motion control method based on embedded system;

[0108] Please follow the steps below:

[0109] S1: Identify the singular points of the robot arm. First, determine the matrix of the robot arm. , and describe the relationship between the velocity of the end effector of the robot arm, including the linear velocity, angular velocity and joint velocity, as shown in formula (1);

[0110] Formula (1)

[0111] in, is the velocity vector of the end effector of the robot arm, is the joint velocity vector, is the joint angle of the robot arm;

[0112] S2: Calculate the condition number and detect singular points. Specifically, the condition number of the matrix is ​​calculated to evaluate its degree of closeness to singularity. The condition number is defined as the ratio of the maximum singular value to the minimum singular value, as shown in formula (2).

[0113] Formula (2)

[0114] in, is the condition number;

[0115] S3: setting a threshold for the condition number and detecting whether the condition number exceeds the threshold. When it is detected that the condition number exceeds the preset threshold, the singular point is processed by pseudo-inverse;

[0116] Please follow the steps below:

[0117] S3.1: When the condition number exceeds the preset threshold, the pseudo-inverse is calculated by decomposing the matrix The pseudo-inverse ; Through singular value decomposition as formula (3.1)-formula (3.2);

[0118] Formula (3.1)

[0119] in, and is an orthogonal matrix, is a diagonal matrix containing The singular values ​​of

[0120] Formula (3.2)

[0121] in, yes The pseudo-inverse of is calculated by taking the inverse of the diagonal elements and transposing them;

[0122] S3.2: Using the Pseudo-Inverse Approximate solution of robot arm joint velocity , as shown in formula (3.3);

[0123] Formula (3.3)

[0124] in, is the velocity vector at the end of the robot arm;

[0125] S3.3: Through the objective function and auxiliary vector Optimize joint configuration, including minimizing joint speed and reducing energy consumption, as shown in formula (3.4);

[0126] Formula (3.4)

[0127] in, is the identity matrix, is the null space projection matrix;

[0128] S3.4: In discrete time steps Next, the joint velocity is integrated Convert to joint angle , as shown in formula (3.5);

[0129] Formula (3.5).

[0130] S4: Perform dynamic path planning. Build a model of the current workspace by monitoring the robot sensor data and obstacle data in the space, including the location and shape of obstacles and trajectory prediction of dynamic objects. The specific steps are as follows:

[0131] S4.1: Use sensors including lidar, cameras, and ultrasonic sensors to collect obstacle information in the workspace, continuously track dynamic objects, and predict their future positions; for moving obstacles, use historical trajectory data and motion models to predict their future positions, as shown in formula (4.1);

[0132] Formula (4.1)

[0133] in, is the state vector, including position and velocity, To control the input data, is the process noise, and is the system matrix.

[0134] S4.2: Filter and smooth the raw sensor data, and extract the geometric features of the obstacle from the processed data, including the bounding box and shape outline;

[0135] First, the raw data is continuously collected from each sensor, including the point cloud data provided by the lidar and the image frames provided by the camera; the above data is filtered, as shown in formula (4.2);

[0136] Formula (4.2)

[0137] in, is the data status value; is the Kalman gain, is the measured value, is the observation matrix;

[0138] Then the filtered data is cleaned of data noise, as shown in formula (4.3);

[0139] Formula (4.3)

[0140] in, is the window size, is the input signal, is the output signal;

[0141] Then, data feature extraction is performed, and edge detection is performed on the image data obtained by the camera to identify the outline of the object;

[0142] For point cloud data, use the RANSAC algorithm or least squares method to fit the bounding box or shape of the obstacle, as shown in formula (4.4);

[0143] Formula (4.4)

[0144] in, and They are rotation and translation transformations, and is the corresponding point cloud data.

[0145] S4.3: Integrate the extracted obstacle information into a unified map representation, where the map is a raster map, a point cloud map, or a topological map.

[0146] S5: Calculate the best path, select the execution path, and execute path planning.

[0147] Please follow the steps below:

[0148] S5.1: Define the cost function based on path length or time; as in formula (5.1);

[0149] Formula (5.1)

[0150] in, is the distance between two points, and are adjacent nodes on the path;

[0151] S5.2: Avoid approaching singular points by inserting a penalty term of the matrix condition number; wherein the penalty term first calculates the matrix condition number by singular value decomposition; as shown in formula (5.2);

[0152] Formula (5.2)

[0153] in, and are the maximum and minimum singular values ​​of the Jacobian matrix, respectively;

[0154] Then, the penalty term is specifically set by setting a threshold and setting a condition number threshold ,when When , it is judged that the point is close to a singular point; in the path planning process, if the Jacobian matrix condition number corresponding to a node exceeds the threshold, the corresponding penalty term is added;

[0155] S5.3: combining the energy consumption value and smoothness in the path planning process in step S5.2, combining all cost items to form a total cost function;

[0156] S5.4: Compute the Jacobian matrix for a given joint angle , calculate the Jacobian matrix ; As formula (5.4);

[0157] Formula (5.4)

[0158] in, It is the mapping function from the robot joint angle to the end effector position.

[0159] In this embodiment, in the above data feature extraction, the image is firstly smoothed using a filter, as shown in formula (6);

[0160] Formula (6)

[0161] in, is the standard deviation;

[0162] Then calculate the place where the pixel value changes most dramatically in the image, that is, the potential edge; as shown in formula (6.1);

[0163] Formula (6.1)

[0164] in, and They are the gradients of the image in the horizontal and vertical directions respectively;

[0165] Then, the edges are refined to keep only the local maximum points; each pixel is compared with its two neighbors along the gradient direction, and if the current pixel is not a local maximum, it is set to zero;

[0166] Then by setting two thresholds, the low threshold and high threshold ;

[0167] Determine if the gradient strength of a pixel is higher than , it is considered a strong edge. If the gradient strength of a pixel is between and , and is connected to a strong edge, it is considered a weak edge;

[0168] Then perform fitting, count the number of inliers, and define a distance threshold , determine whether a point belongs to the fitted model;

[0169] Calculate the distance from all points to the fitted model, if the distance is less than , then the point is considered to be an interior point, and the number of interior points is counted ;

[0170] Repeat the above steps and record the model with the largest number of inliers until the best model is found to identify image features.

[0171] In this embodiment, after calculating the Jacobian matrix, path planning is directly performed, the starting point and the target point are set, and the data structure of the path planning algorithm is initialized;

[0172] Randomly sample new points, expand the tree and check for collisions, and use the total cost function to evaluate the quality of the new path;

[0173] And starting from the starting point, expand the nodes according to the heuristic function until reaching the target point, and also use the total cost function to evaluate the path; find a safe path from the starting point to the target point, and output it as the final result. If there are still potential singular points on the path, repeat the above steps until a suitable path is found;

[0174] And continuously adjust the path online, first continuously collect new environmental data from lidar and cameras; then integrate the new obstacle information into the existing map representation, and update the raster map, point cloud map or topological map; as shown in formula (8.1);

[0175] Formula (8.1)

[0176] in, is the grid state, is the measured value, is the position of the robotic arm;

[0177] Compare the current map model with the previously stored map model to detect new obstacles or removed obstacles; track dynamic objects, and for moving obstacles, predict their future positions according to formula (8.2) and update this information on the map, as shown in formula (8.2);

[0178] Formula (8.2)

[0179] in, is the state transition matrix, is the control input matrix, is the control vector;

[0180] Then check whether the current path collides with the newly appeared obstacle, including singular point detection, recalculate the Jacobian matrix condition number corresponding to each point on the current path, and determine whether there is a situation close to a singular point;

[0181] And perform cost evaluation, re-evaluate the cost of the current path based on the new environmental information, including path length, energy consumption, and smoothness data;

[0182] Then the path is replanned. If it is found that the path has a collision risk or potential singularity, or there is a better path choice, the replanning process is triggered;

[0183] Re-plan the path according to the current situation; redefine the cost function, combine the new environmental information, and redefine the total cost function including the singularity penalty term and other cost factors;

[0184] Perform path planning, run the selected path planning algorithm, generate a new safe and feasible path, smooth the newly generated path; and perform speed control, adjust the speed of the robot arm to ensure a smooth transition from the old path to the new path.

[0185] In this embodiment, the method is used for processing. For example, an image is collected by a camera, the image contains static obstacles and dynamic obstacles, the image resolution is 640x480 pixels, and a set of point cloud data is collected by a laser sensor. Each point represents a distance value. The point cloud data format is an XYZ coordinate system, with a total of 5000 points.

[0186] First, preprocess the data and calculate the kernel size: 3x3, standard deviation ; Then filter and estimate the initial state: ;

[0187] Observation matrix: H = IH = I (identity matrix);

[0188] Process noise covariance: Q =0.1 I；

[0189] Observation noise covariance: R =0.01 I ;

[0190] Then perform feature extraction and edge detection:

[0191] Gradient strength threshold: ; High threshold ;

[0192] Number of extracted edge points: about 1000 points

[0193] Then calculate the plane model parameters:

[0194] Plane model parameters: ax+by+cz+d=0ax+by+cz+d=0;

[0195] Number of internal points: about 3000 points

[0196] In this embodiment, environmental modeling is then performed, specifically through a grid map;

[0197] Grid unit size: 10cmx10cm; map size: 10mx10m

[0198] Number of grids occupied: about 3000;

[0199] Then perform dynamic object trajectory prediction:

[0200] Prediction time window: 5 seconds

[0201] Predicted positions: [(x1, y1), (x2, y2), ... ] (one position per second)

[0202] In this embodiment, path planning is performed again, and the cost function is first defined:

[0203] Path length weight: ;

[0204] Singularity penalty weight: ;

[0205] Energy consumption weight: ;

[0206] Smoothness Weight: ;

[0207] Then perform path planning, with the maximum number of iterations: 1000 times;

[0208] Generate path nodes: (about 20 nodes);

[0209] Control and execution, inverse kinematics solution:

[0210] The joint speed is ;

[0211] The joint angle is ;

[0212] The control command is ;

[0213] The motor speed command is ;

[0214] In this embodiment, the present invention provides an embedded-based industrial motion control system, which uses a high-performance computing unit, including an embedded processor or FPGA, and can quickly process a large amount of sensor data to ensure real-time performance.

[0215] Online path adjustment: the system can quickly adjust the path according to environmental changes to ensure timely response in dynamic environments;

[0216] Specifically, it includes a sensor module for collecting environmental data, including data from lidar, cameras, and ultrasonic sensors;

[0217] Data preprocessing module, which filters and smoothes the raw sensor data to remove noise and outliers;

[0218] Feature extraction module, which extracts geometric features from preprocessed data, including edge computing;

[0219] The environment modeling module integrates the extracted features into a unified map representation, including a raster map, a point cloud map, or a topological map;

[0220] Trajectory prediction module, for dynamic objects, uses historical trajectory data to predict their future positions;

[0221] The path planning module generates a safe path from the starting point to the target point based on the current environment model and task requirements;

[0222] The control and execution module converts the planned path into specific joint angles or motor commands and sends them to the actuators;

[0223] The monitoring and feedback module monitors the system status in real time and provides feedback to adjust the control strategy.

[0224] Furthermore, the present invention provides a computer-storable medium, wherein the computer-readable storage medium includes an embedded processing system and a stored program, and when the embedded system control program runs, the device where the computer-readable storage medium is located is controlled to execute any of the embedded-based industrial motion control methods described above.

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

Claims

1. An embedded industrial motion control method, characterized in that: Please follow the steps below: S1: Identify the singular points of the robot arm. First, determine the matrix of the robot arm. , and describe the relationship between the velocity of the end effector of the robot arm, including the linear velocity, angular velocity and joint velocity, as shown in formula (1); Formula (1) in, is the velocity vector of the end effector of the robot arm, is the joint velocity vector, is the joint angle of the robot arm; S2: Calculate the condition number and detect singular points. Specifically, the condition number of the matrix is ​​calculated to evaluate its degree of closeness to singularity. The condition number is defined as the ratio of the maximum singular value to the minimum singular value, as shown in formula (2). Formula (2) in, is the condition number; S3: setting a threshold for the condition number and detecting whether the condition number exceeds the threshold. When it is detected that the condition number exceeds the preset threshold, the singular point is processed by pseudo-inverse. Please follow the steps below: S3.1: When the condition number exceeds the preset threshold, the pseudo-inverse is calculated by decomposing the matrix The pseudo-inverse ; Through singular value decomposition as formula (3.1)-formula (3.2); Formula (3.1) in, and is an orthogonal matrix, is a diagonal matrix containing The singular values ​​of Formula (3.2) in, yes The pseudo-inverse of is calculated by taking the inverse of the diagonal elements and transposing them; S3.2: Using the Pseudo-Inverse Approximate solution of robot arm joint velocity , as shown in formula (3.3); Formula (3.3) in, is the velocity vector at the end of the robot arm; S3.3: Through the objective function and auxiliary vector Optimize joint configuration, including minimizing joint speed and reducing energy consumption, as shown in formula (3.4); Formula (3.4) in, is the identity matrix, is the null space projection matrix; S3.4: In discrete time steps Next, the joint velocity is integrated Convert to joint angle , as shown in formula (3.5); Formula (3.5); S4: Perform dynamic path planning, build a model of the current workspace through the monitored robot arm sensor data and obstacle data in the space, including the location and shape of obstacles and trajectory prediction of dynamic objects; specifically, follow the following steps: S4.1: Use sensors including lidar, cameras, and ultrasonic sensors to collect obstacle information in the workspace, continuously track dynamic objects, and predict their future positions; for moving obstacles, use historical trajectory data and motion models to predict their future positions, as shown in formula (4.1); Formula (4.1) in, is the state vector, including position and velocity, To control the input data, is the process noise, and is the system matrix; S4.2: Filter and smooth the raw sensor data, and extract the geometric features of the obstacle from the processed data, including the bounding box and shape outline; First, the raw data is continuously collected from each sensor, including the point cloud data provided by the lidar and the image frames provided by the camera; the above data is filtered, as shown in formula (4.2); Formula (4.2) in, is the data status value; is the Kalman gain, is the measured value, is the observation matrix; Then the filtered data is cleaned of data noise, as shown in formula (4.3); Formula (4.3) in, is the window size, is the input signal, is the output signal; Then, data feature extraction is performed, and edge detection is performed on the image data obtained by the camera to identify the outline of the object; For point cloud data, use the RANSAC algorithm or least squares method to fit the bounding box or shape of the obstacle, as shown in formula (4.4); Formula (4.4) in, and They are rotation and translation transformations, and is the corresponding point cloud data; S4.3: Integrate the extracted obstacle information into a unified map representation, wherein the map is a raster map, a point cloud map, or a topological map; S5: Calculate the best path, select the execution path, and execute path planning. Please follow the steps below: S5.1: Define the cost function based on path length or time; as in formula (5.1); Formula (5.1) in, is the distance between two points, and are adjacent nodes on the path; S5.2: Avoid approaching singular points by inserting a penalty term of the matrix condition number; wherein the penalty term first calculates the matrix condition number by singular value decomposition; as shown in formula (5.2); Formula (5.2) in, and are the maximum and minimum singular values ​​of the Jacobian matrix, respectively; Then, the penalty term is specifically set by setting a threshold and setting a condition number threshold ,when When , it is considered to be close to a singular point; in the path planning process, if the Jacobian matrix condition number corresponding to a node exceeds the threshold, the corresponding penalty term is added; S5.3: combining the energy consumption value and smoothness in the path planning process in step S5.2, and combining all cost items to form a total cost function; S5.4: Compute the Jacobian matrix for a given joint angle , calculate the Jacobian matrix ; As formula (5.4); Formula (5.4) in, It is the mapping function from the joint angle of the robot arm to the end execution position; In data feature extraction, the image is first smoothed using a filter, as shown in formula (6); Formula (6) in, is the standard deviation; Then calculate the place where the pixel value changes most dramatically in the image, that is, the potential edge; as shown in formula (6.1); Formula (6.1) in, and They are the gradients of the image in the horizontal and vertical directions respectively; Then, the edges are refined to keep only the local maximum points; each pixel is compared with its two neighbors along the gradient direction, and if the current pixel is not a local maximum, it is set to zero; Then by setting two thresholds, the low threshold and high threshold ; Determine if the gradient strength of a pixel is higher than , it is considered a strong edge. If the gradient strength of a pixel is between and , and is connected to a strong edge, it is considered a weak edge; Then perform fitting, count the number of inliers, and define a distance threshold , determine whether a point belongs to the fitted model; Calculate the distance of all points to the fitted model, if the distance is less than , then the point is considered to be an interior point, and the number of interior points is counted ; Repeat the above steps and record the model with the largest number of inliers until the best model is found to identify image features.

2. The embedded-based industrial motion control method according to claim 1, characterized in that: After calculating the Jacobian matrix, directly execute path planning, set the starting point and target point, and initialize the data structure of the path planning algorithm; Randomly sample new points, expand the tree and check for collisions, and use the total cost function to evaluate the quality of the new path; And starting from the starting point, expand the nodes according to the heuristic function until reaching the target point, and also use the total cost function to evaluate the path; find a safe path from the starting point to the target point, and output it as the final result. If there are still potential singular points on the path, repeat the above steps until a suitable path is found; And continuously adjust the path online, first continuously collect new environmental data from lidar and cameras; then integrate the new obstacle information into the existing map representation, and update the raster map, point cloud map or topological map; as shown in formula (8.1); Formula (8.1) in, is the grid state, is the measured value, is the position of the robotic arm; Compare the current map model with the previously stored map model to detect new obstacles or removed obstacles; track dynamic objects, and for moving obstacles, predict their future positions according to formula (8.2) and update this information on the map, as shown in formula (8.2); Formula (8.2) in, is the state transition matrix, is the control input matrix, is the control vector; Then check whether the current path collides with the newly appeared obstacle, including singular point detection, recalculate the Jacobian matrix condition number corresponding to each point on the current path, and determine whether there is a situation close to a singular point; And perform cost evaluation, re-evaluate the cost of the current path based on the new environmental information, including path length, energy consumption, and smoothness data; Then the path is replanned. If it is found that the path has a collision risk or potential singularity, or there is a better path choice, the replanning process is triggered; Re-plan the path according to the current situation; redefine the cost function, combine the new environmental information, and redefine the total cost function including the singularity penalty term; Perform path planning, run the selected path planning algorithm, generate a new safe and feasible path, smooth the newly generated path; and perform speed control, adjust the speed of the robot arm to ensure a smooth transition from the old path to the new path.

3. A computer-readable storage medium, characterized in that: The computer-readable storage medium includes an embedded processing system and a stored program. When the embedded processing system control program runs, the device where the computer-readable storage medium is located is controlled to execute the embedded-based industrial motion control method according to any one of claims 1-2.

Images