A system for robot motion planning and control algorithm improved by algebraic algorithm

Abstract

The application relates to the technical field of robot control, in particular to a robot motion planning and control algorithm system improved in an algebraic algorithm, which comprises a parameter acquisition module, a residual error calculation module, a potential energy analysis module, an instruction point updating module and a control execution module. According to the application, the dynamic residual error between actual motion and a theoretical model is calculated by collecting joint angle and torque signals in real time, and an algebraic potential energy field capable of quantitatively describing motion stability risk is innovatively constructed based on the residual error; then, the potential energy size of each instruction point, the potential energy gradient change between adjacent points and the key torque change rate residual error are taken as core features and input into a Bayesian judgment model to accurately perform posterior probability evaluation on the reliability of each instruction point, so that low-potential energy points are used to replace potential unstable instruction points in a planning track in a forward-looking manner, and finally an optimized track with smoothness and safety is generated before motion execution.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a robot motion planning and control algorithm system improved by algebraic algorithms. Background Technology

[0002] Robot control technology is an important branch of automation and intelligent manufacturing. It mainly studies how to achieve precise control of robot motion, posture and operation process through sensing, computing and execution.

[0003] Among them, the robot motion planning and control algorithm system refers to the system that determines the trajectory path of the robotic arm or mobile platform through motion planning algorithm before the robot performs a task, and uses control algorithm to drive each actuator to move according to the planned path.

[0004] Existing robot motion planning and control algorithm systems rely heavily on idealized dynamic models to determine motion trajectories during the planning phase. This approach ignores the differences in dynamic characteristics caused by non-ideal factors such as friction between mechanical structures, transmission backlash, and dynamic load changes in actual operation. Furthermore, its control process merely drives the actuator to passively track the preset path, lacking a mechanism for predicting and evaluating the potential motion stability risks of each command point on the planned path in the real physical environment. This results in the final generated trajectory potentially vibrating or being impacted by unforeseen dynamic disturbances during actual execution, thereby affecting the robot's motion accuracy and operational stability, and significantly reducing the overall reliability of the system. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of existing technologies and propose an algebraic algorithm-based robot motion planning and control algorithm system.

[0006] To achieve the above objectives, the present invention adopts the following technical solution: an algebraic algorithm-improved robot motion planning and control algorithm system, the system comprising:

[0007] The parameter acquisition module acquires the robot joint angle signal sequence and robot joint torque signal sequence as the robot moves over time, calculates the robot joint motion rate and robot joint torque change rate, and generates the robot's initial motion parameter set;

[0008] The residual calculation module compares the robot joint motion rate and robot joint torque change rate in the initial motion parameter set of the robot with their respective preset theoretical rate residuals to generate a robot residual stability parameter set.

[0009] The potential energy analysis module acquires the robot joint position command points, determines the robot algebraic potential energy value for each command point based on the robot residual stability parameter set and the robot initial motion parameter set, and generates a robot algebraic potential energy feature set.

[0010] The command point update module inputs the robot algebraic potential energy feature set and the robot residual stability parameter set into the Bayesian decision model, calculates the posterior probability of the reliability of the robot joint position command points, and filters command points according to the posterior probability to obtain the updated command point set.

[0011] The control execution module constructs the robot motion control trajectory based on the updated instruction point set, thereby obtaining the robot motion control instruction set.

[0012] The present invention is improved in that the initial motion parameter set of the robot includes robot joint angle change information, robot joint motion rate information, and robot joint torque change rate information; the robot residual stability parameter set includes robot joint motion rate residual data, robot joint torque change rate residual data, and robot residual trend smoothing results; the robot algebraic potential energy feature set includes robot algebraic potential energy value, potential energy gradient direction angle difference, and potential energy feature parameters; the updated command point set includes command point pose data, command point confidence data, and replacement command point identifiers; and the robot motion control command set includes control trajectory sequence, target joint angle command, and target torque command.

[0013] The present invention is improved in that the parameter acquisition module includes:

[0014] The motion data acquisition submodule collects robot joint angle signal sequences and robot joint torque signal sequences through joint angle sensors and torque sensors, assigns a corresponding timestamp to each signal sequence, and combines the robot joint angle signal sequences and robot joint torque signal sequences with timestamps to establish a real-time motion state dataset for the robot.

[0015] The motion rate calculation submodule performs time difference calculation on robot joint angle values ​​at adjacent timestamps in the robot's real-time motion state dataset to obtain the robot joint motion rate, and performs time difference calculation on robot joint torque values ​​at adjacent timestamps to obtain the robot joint torque change rate, generating joint dynamic change rate information.

[0016] The motion parameter generation submodule constructs an initial motion parameter set for the robot based on the robot joint angle signal sequence and robot joint torque signal sequence in the robot's real-time motion state dataset, combined with the robot joint motion rate and robot joint torque change rate in the joint dynamic change rate information.

[0017] The present invention is improved in that the residual calculation module includes:

[0018] The initial residual calculation submodule extracts the robot joint motion rate and robot joint torque change rate from the robot initial motion parameter set, compares them with the preset robot theoretical joint motion rate and robot theoretical joint torque change rate, obtains the difference between the two, and generates a robot algebraic constraint residual set.

[0019] The residual trend analysis submodule calculates the trend of each residual within a set sliding time window based on the robot algebraic constraint residual set, compares the residual trend with a preset trend fluctuation threshold, filters residual data that exceed the trend fluctuation threshold, and obtains abnormal residual trend items.

[0020] The residual correction submodule identifies the residual data marked by the abnormal residual trend term in the robot algebraic constraint residual set, performs state estimation and correction smoothing on the marked residual data, and integrates the smoothed residual data with other data in the robot algebraic constraint residual set to generate a robot residual stability parameter set.

[0021] The present invention is improved in that the potential energy analysis module includes:

[0022] The command point generation submodule acquires the target pose data of the robot end effector's workspace, and calls the joint angle signal sequence in the robot's initial motion parameter set and the target pose data of the robot end effector's workspace. It generates a smooth intermediate path point by performing interpolation between the two, which serves as the robot joint position command point.

[0023] The algebraic potential energy determination submodule performs joint algebraic operations on the robot joint motion rate and robot joint torque change rate in the robot residual stability parameter set and the robot initial motion parameter set for each robot joint position command point, and performs gradient direction analysis on the operation results to determine the robot algebraic potential energy value and gradient direction of each command point, and obtains the potential energy gradient information of the command point.

[0024] The potential energy feature generation submodule calculates the potential energy gradient direction angle difference between adjacent command points based on the potential energy gradient information of the command points, and integrates the robot algebraic potential energy value of each command point with the calculated potential energy gradient direction angle difference to generate a robot algebraic potential energy feature set.

[0025] The present invention has an improvement, wherein the process of performing gradient direction analysis on the calculation result is specifically as follows:

[0026] Establish a joint coordinate system with the angle of each joint of the robot as the coordinate axis;

[0027] In the joint coordinate system, calculate the rate of change of the result of the joint algebraic operation relative to each joint angle coordinate axis;

[0028] Combine the rate of change corresponding to all joint angle coordinate axes as components of a multidimensional vector;

[0029] The multidimensional vector is used to determine the gradient direction.

[0030] The present invention is improved in that the instruction point update module includes:

[0031] The posterior probability calculation submodule inputs the robot algebraic potential energy feature set and the robot residual stability parameter set into the Bayesian decision model to perform posterior probability calculation, obtains the motion reliability posterior probability value corresponding to each robot joint position command point, and establishes a robot command point confidence table based on the posterior probability value.

[0032] The command point filtering submodule compares the posterior probability value of each command point recorded in the robot command point confidence table with a preset confidence threshold one by one to determine the confidence level of the command point's movement, marks command points that are below the confidence threshold, and generates a list of command points to be replaced.

[0033] The instruction point set update submodule, for the list of instruction points to be replaced, calls the robot algebraic potential energy feature set, selects replacement points based on the potential energy difference between adjacent instruction points in the list, and merges all the original instruction points that are retained with all the replacement points to obtain the updated instruction point set.

[0034] The present invention is improved in that the process of determining the reliability of the motion of the command point is specifically as follows:

[0035] The posterior probability value of motion reliability corresponding to each robot joint position command point recorded in the robot command point confidence table is compared with the preset confidence threshold.

[0036] When the posterior probability value of motion reliability is greater than or equal to the preset confidence threshold, the corresponding robot joint position command point is determined as a reliable command point and directly retained.

[0037] When the posterior probability value of motion reliability is less than the preset confidence threshold, the corresponding robot joint position command point is determined as an untrusted command point, and all untrusted command points are uniformly marked.

[0038] The present invention is improved in that the control execution module includes:

[0039] The motion trajectory construction submodule connects each instruction point of the updated instruction point set in a preset motion space to obtain the robot motion control trajectory.

[0040] The instruction interpolation calculation submodule analyzes the target joint angle and target joint torque instructions required to be achieved at each discrete moment during the robot's motion control trajectory, and obtains the target instruction at each moment.

[0041] The instruction set integration submodule arranges all target joint angle and target joint torque instruction data for each moment in the order of execution time to form a robot motion control instruction set.

[0042] The present invention is improved in that the process of connecting each instruction point in the updated instruction point set in a preset motion space is specifically as follows:

[0043] The rotation angle range and maximum angular velocity of each joint of the robot are obtained as physical limit parameters, and the three-dimensional spatial coordinate data of static obstacles in the robot's working environment are collected.

[0044] The robot joint space constraint boundary is established based on the physical limit parameters, and the three-dimensional spatial coordinate data of the static obstacle is mapped to the robot joint space through inverse kinematics calculation to generate the obstacle region in the joint space.

[0045] Combine all areas within the robot's joint space constraint boundary that are not occupied by obstacles, and define the combined area as the preset motion space.

[0046] Compared with the prior art, the advantages and positive effects of the present invention are as follows:

[0047] In this invention, the dynamic residual between the actual motion and the theoretical model is calculated by real-time acquisition of joint angle and torque signals. Based on the residual, an algebraic potential energy field that can quantify the motion stability risk is innovatively constructed. Then, the potential energy magnitude of each command point, the potential energy gradient change between adjacent points, and the key torque change rate residual are used as core features and input into a Bayesian decision model to accurately evaluate the reliability of each command point posterior probability. This allows for the proactive identification and replacement of potentially unstable command points in the planned trajectory with low potential energy points. Finally, an optimized trajectory that balances smoothness and safety is generated before motion execution, significantly improving the motion control accuracy and overall operational stability of the robot in complex dynamic environments. Attached Figure Description

[0048] Figure 1 This is a system module diagram of the present invention;

[0049] Figure 2 This is a system framework diagram of the present invention;

[0050] Figure 3 This is a schematic diagram of the parameter acquisition module process of the present invention;

[0051] Figure 4 This is a schematic diagram of the residual calculation module flow of the present invention;

[0052] Figure 5 This is a schematic diagram of the potential energy analysis module flow of the present invention;

[0053] Figure 6 This is a schematic diagram of the instruction point update module process of the present invention;

[0054] Figure 7 This is a schematic diagram of the control execution module flow of the present invention. Detailed Implementation

[0055] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0056] Please see Figure 1 This invention provides a technical solution: an algebraic algorithm-improved robot motion planning and control algorithm system, the system comprising:

[0057] The parameter acquisition module acquires the robot joint angle signal sequence and robot joint torque signal sequence as the robot moves over time, calculates the robot joint motion rate and robot joint torque change rate, and generates the robot's initial motion parameter set;

[0058] The residual calculation module compares the robot joint motion rate and robot joint torque change rate in the robot's initial motion parameter set with their respective preset theoretical rate residuals to generate a robot residual stability parameter set.

[0059] The potential energy analysis module acquires the robot joint position command points, determines the robot algebraic potential energy value for each command point based on the robot residual stability parameter set and the robot initial motion parameter set, and generates the robot algebraic potential energy feature set.

[0060] The command point update module inputs the robot's algebraic potential energy feature set and the robot's residual stability parameter set into the Bayesian decision model, calculates the posterior probability of the reliability of the robot's joint position command points, and filters command points based on the posterior probability to obtain the updated command point set.

[0061] The control execution module constructs the robot motion control trajectory based on the updated instruction point set, and obtains the robot motion control instruction set.

[0062] The robot's initial motion parameter set includes information on robot joint angle changes, robot joint motion rates, and robot joint torque change rates; the robot residual stability parameter set includes robot joint motion rate residual data, robot joint torque change rate residual data, and robot residual trend smoothing results; the robot algebraic potential energy feature set includes robot algebraic potential energy values, potential energy gradient direction angle differences, and potential energy feature parameters; the updated command point set includes command point pose data, command point confidence data, and replacement command point identifiers; and the robot motion control command set includes control trajectory sequences, target joint angle commands, and target torque commands.

[0063] Please see Figure 2 and Figure 3 The parameter acquisition module includes:

[0064] The motion data acquisition submodule collects robot joint angle signal sequences and robot joint torque signal sequences through joint angle sensors and torque sensors, assigns a corresponding timestamp to each signal sequence, and combines the robot joint angle signal sequences and robot joint torque signal sequences with timestamps to establish a real-time motion state dataset for the robot.

[0065] Each joint of the six-axis robotic arm (using the second joint as an example here) will be equipped with a joint angle sensor and a torque sensor. The joint angle sensor uses an absolute encoder, and the torque sensor is a strain gauge type. At the start of the robot's welding task, data acquisition begins at a frequency of 100 Hz. At the first timepoint of 0.01 seconds, the angle sensor of the second joint records a joint angle value of 0.5236 radians, and simultaneously, the torque sensor mounted on the same joint records a joint torque value of 15.5 Nm. A 0.01-second timestamp is appended to these two signal values. At the second timepoint of 0.02 seconds, the angle sensor of the second joint records a reading of 0.5250 radians, and the torque sensor records a reading of 15.8 Nm; again, a 0.02-second timestamp is appended to these two signal values. This process is continuously executed; for example, at the Nth timepoint (N × 0.01 seconds), the corresponding joint angle signal value and joint torque signal value are acquired and timestamped. Subsequently, the robot's second joint angle and torque signal values ​​collected at each timestamp are combined. For example, the angle value of 0.5236 radians at 0.01 seconds is combined with the torque value of 15.5 Nm to form one data pair, and the angle value of 0.5250 radians at 0.02 seconds is combined with the torque value of 15.8 Nm to form another data pair. This data collection and combination process is repeated for all six joints. All data pairs generated at consecutive timestamps are then compiled into a sequence set in chronological order. This set constitutes the robot's real-time motion state dataset.

[0066] The motion rate calculation submodule performs time difference calculation on robot joint angle values ​​at adjacent timestamps in the robot's real-time motion state dataset to obtain the robot joint motion rate, and performs time difference calculation on robot joint torque values ​​at adjacent timestamps to obtain the robot joint torque change rate, generating joint dynamic change rate information.

[0067] Taking the aforementioned second joint as an example, the angle value of 0.5236 radians at 0.01 seconds and the angle value of 0.5250 radians at 0.02 seconds are extracted. The specific action of performing time difference calculation is to subtract the angle value of the previous moment from the angle value of the later moment, and then divide the difference by the time interval between the two moments. The calculation example is: (0.5250-0.5236) radians / (0.02-0.01) seconds, which gives the joint movement rate of the robot's second joint at 0.02 seconds as 0.14 radians / second. Similarly, time difference calculation is performed on the robot joint torque values. The torque value of 15.5 Nm at 0.01 seconds and the torque value of 15.8 Nm at 0.02 seconds are extracted. The calculation process is as follows: (15.8-15.5) Nm / (0.02-0.01) sec, yielding a joint torque change rate of 30 Nm / sec for the robot's second joint at time 0.02. This calculation is repeated for each pair of adjacent timestamps in the robot's real-time motion state dataset, generating a joint motion rate value and a joint torque change rate value for each joint at each discrete moment (except the initial moment). The set of joint motion rates and joint torque change rates for all joints at all discrete moments constitutes the joint dynamic change rate information.

[0068] The motion parameter generation submodule constructs the initial motion parameter set of the robot based on the robot joint angle signal sequence and robot joint torque signal sequence in the robot's real-time motion state dataset, combined with the robot joint motion rate and robot joint torque change rate in the joint dynamic change rate information.

[0069] The raw data acquired at a specific timestamp is integrated with the rate of change data calculated at the same timestamp. Taking the data of the second joint at 0.02 seconds as an example, the robot's real-time motion state dataset provides a joint angle signal of 0.5250 radians and a joint torque signal of 15.8 Nm. The joint dynamic rate of change information provides a joint motion rate of 0.14 radians / second and a joint torque rate of change of 30 Nm / second at the same time. These four data points—0.5250 radians, 15.8 Nm, 0.14 radians / second, and 30 Nm / second—are combined into a single data record, which is then associated with the timestamp of 0.02 seconds. This data combination operation is performed on all six joints of the robot at all discrete timestamps of the calculated rates. Finally, the combined data records of all joints at all timestamps are aggregated to form a structured dataset, which is the robot's initial motion parameter set.

[0070] Please see Figure 2 and Figure 4 The residual calculation module includes:

[0071] The initial residual calculation submodule extracts the robot joint motion rate and robot joint torque change rate from the robot's initial motion parameter set, compares them with the preset theoretical robot joint motion rate and robot joint torque change rate, obtains the difference between the two, and generates a robot algebraic constraint residual set.

[0072] From the robot's initial motion parameter set, the robot joint motion rate and joint torque change rate at each discrete time stamp are extracted for each joint. Taking the data of the second joint at time stamp 0.02 seconds as an example, the extracted joint motion rate is 0.14 radians / second, and the joint torque change rate is 30 Nm / second. Simultaneously, preset theoretical robot joint motion rates and theoretical joint torque change rates are invoked. These two theoretical values ​​are theoretical outputs calculated by the model based on the robot's ideal dynamics model, without considering non-ideal factors such as friction, backlash, and load changes, after inputting the same joint position and velocity commands. The extracted actual rates are compared with the theoretical rates to obtain the difference. Specifically, the theoretical joint motion rate is subtracted from the actual measured joint motion rate; for example, 0.14 radians / second - 0.15 radians / second, resulting in a difference of -0.01 radians / second. Similarly, subtracting the theoretical rate of change of joint torque from the actual calculated rate of change (e.g., 30 Nm / s - 25 Nm / s in the example) yields a difference of 5 Nm / s in the rate of change of joint torque. This process is repeated for each data record in the robot's initial motion parameter set, generating a set of difference data for each timestamp of each joint. The collection of all these difference data constitutes the robot's algebraic constraint residual set.

[0073] The residual trend analysis submodule, based on the robot algebra constraint residual set, calculates the trend of change of each residual within a set sliding time window, compares the residual trend with the preset trend fluctuation threshold, filters residual data that exceed the trend fluctuation threshold, and obtains abnormal residual trend items.

[0074] Based on the robot's algebraically constrained residual set, the trend of change for each residual is calculated within a set sliding time window. The size of the sliding time window is set to 5, based on statistical analysis of the duration of transient noise signals in historical data, selecting a window length that can cover the typical noise pulse width without excessively delaying anomaly detection. Taking the joint torque change rate residual of the second joint as an example, the residual values ​​from time point t1 to t5 are 5.0, 5.2, 5.3, 6.5, and 5.6 (all in Newton-meters per second). The specific process for calculating the residual trend is to perform linear regression analysis on the five residual values ​​within the window to obtain the slope of the fitted line, which is the trend. In this example, the calculated slope is 0.2. Subsequently, the calculated residual trend is compared with a preset trend fluctuation threshold. The trend fluctuation threshold is set based on statistical analysis of a large amount of historical residual data collected under normal robot operating conditions. Specifically, residual data under standard welding tasks are collected, and the trend of change for each set of data within a sliding window of the same size is calculated to form a trend value sample set. Calculate the standard deviation of all trend values ​​in the sample set, assuming the standard deviation of the residual trend of the joint torque change rate is 0.08. Set the trend fluctuation threshold to three times this standard deviation, i.e., 0.08 × 3 = 0.24. This setting aims to identify abnormal changes that exceed the normal statistical fluctuation range. The comparison process is as follows: compare the currently calculated residual change trend of 0.2 with the threshold of 0.24. Because 0.2 is less than 0.24, the residual change trend within this window is judged as normal. If the trend calculated in another window is 0.3, since 0.3 is greater than 0.24, the residual data corresponding to that window will be filtered out and marked as an abnormal residual trend item.

[0075] The residual correction submodule identifies residual data marked by abnormal residual trend terms in the robot algebraic constraint residual set, performs state estimation and correction smoothing on the marked residual data, and integrates the smoothed residual data with other data in the robot algebraic constraint residual set to generate a robot residual stability parameter set.

[0076] Identify residual data marked by outlier residual trend terms in the robot's algebraic constraint residual set. Assume that in the torque change rate residual sequence of the second joint, the fourth data point, 6.5 Nm / s (corresponding to time t4), is marked due to the trend analysis results of its sliding window. Perform state estimation and correction smoothing on the marked residual data. Specifically, instead of directly using the marked data point 6.5, estimate a substitute value using its adjacent unmarked normal data points. The smoothing method used here is to take the arithmetic mean of the two adjacent normal points. That is, use the residual value 5.3 at time t3 and the residual value 5.6 at time t5 for calculation. The calculation process for the substitute value is: (5.3 + 5.6) / 2 = 5.45 Nm / s. Replace the original outlier value 6.5 with the calculated substitute value 5.45. The smoothed residual data (i.e., the sequence after replacing 6.5 with 5.45) is integrated with other unprocessed residual data deemed normal in the robot algebraic constraint residual set. For example, the torque change rate residual sequence of the second joint is updated to 5.0, 5.2, 5.3, 5.45, 5.6. This correction process is repeated for all marked anomalous residual data points, and the final dataset is the robot residual stability parameter set.

[0077] Please see Figure 2 and Figure 5 The potential energy analysis module includes:

[0078] The command point generation submodule acquires the target pose data of the robot end effector's workspace, and calls the joint angle signal sequence in the robot's initial motion parameter set and the target pose data of the robot end effector's workspace. It generates smooth intermediate path points by performing interpolation between the two, which serve as robot joint position command points.

[0079] First, the target pose data of the robot's end effector (e.g., a welding torch) in the workspace is acquired. This data consists of a Cartesian coordinate point and a pose quaternion; for example, the target point's coordinates are (500, 300, 200) mm, and the pose is (0.707, 0, 0.707, 0). Simultaneously, the current joint angle signal sequence from the robot's initial motion parameter set is retrieved, assuming the current angles of the six joints are (0.5, 0.2, -0.1, 0.8, 0.4, 0.6) radians. Through inverse kinematics calculation, the target pose is converted into the corresponding target joint angle sequence, for example, (0.6, 0.3, -0.15, 0.85, 0.42, 0.61) radians. Next, interpolation is performed between the current joint angle sequence and the target joint angle sequence. The specific interpolation process uses a fifth-order polynomial interpolation method to generate 10 intermediate path points between the two joint angle sequences. Taking the first joint as an example, between the initial angle of 0.5 radians and the target angle of 0.6 radians, ten smooth transition angle values ​​are calculated using a fifth-order polynomial. For example, the first intermediate point is 0.51 radians, the second is 0.525 radians, and so on until the tenth point is close to 0.6 radians. This interpolation operation is performed on all six joints, generating ten sets of intermediate path points containing the angle values ​​of the six joints. These generated smooth intermediate path points collectively serve as the robot's joint position command points.

[0080] The algebraic potential energy determination submodule performs joint algebraic operations on the robot joint motion rate and robot joint torque change rate in the robot residual stability parameter set and the robot initial motion parameter set for each robot joint position command point, and performs gradient direction analysis on the operation results to determine the robot algebraic potential energy value and gradient direction of each command point, and obtains the potential energy gradient information of the command point.

[0081] The specific process of performing gradient direction analysis on the calculation results is as follows:

[0082] Establish a joint coordinate system with the angle of each joint of the robot as the coordinate axis;

[0083] In the joint coordinate system, calculate the rate of change of the result of the joint algebraic operation relative to each joint angle coordinate axis;

[0084] Combine the rate of change corresponding to all joint angle coordinate axes as components of a multidimensional vector;

[0085] Determine the gradient direction using a multidimensional vector;

[0086] For each robot joint position command point generated by the command point generation submodule, taking the first intermediate path point (joint angles of 0.51, 0.21, -0.105, 0.805, 0.402, 0.601) as an example, the calculation involves the residual data in the robot residual stability parameter set that is closest to the motion state (e.g., velocity residual -0.01 radians / second, torque change rate residual 5.45 Nm / second), and the robot joint motion rate (0.14 radians / second) and robot joint torque change rate (30 Nm / second) corresponding to the robot initial motion parameter set. The specific definition of the joint algebra operation is: weighted summation of the absolute values ​​of each residual and the absolute values ​​of each rate. The weight coefficients are set based on the prior assessment of the influence of each parameter on motion stability; the parameter with a greater influence on stability has a larger weight value. For example, the joint torque change rate residual directly reflects the anomaly of dynamic force, and its weight (0.5) is higher than that of the motion rate itself (0.2). The calculation example is: 1.0 × |-0.01| + 0.5 × |5.45| + 0.2 × |0.14| + 0.1 × |30| = 0.01 + 2.725 + 0.028 + 3.0 = 5.763. This result, 5.763, is the robot's algebraic potential energy value at this command point. Gradient direction analysis is then performed on the calculation result. First, a six-dimensional joint coordinate system is established with the robot's six joint angles as coordinate axes. Next, in the joint coordinate system, the rate of change of the joint algebraic operation result relative to each joint angle coordinate axis is calculated, i.e., the partial derivative of the algebraic potential energy value with respect to each joint angle is calculated. This is achieved by slightly perturbing each joint angle value, recalculating the algebraic potential energy value, and obtaining the rate of change. For example, the calculated rate of change (partial derivative) components are (0.1, -0.05, 0.02, 0.0, -0.08, 0.03). These six rate of change components are combined into a gradient direction vector. In robot motion planning, this gradient direction vector points in the direction of the fastest increase in algebraic potential energy in joint space, reflecting the direction of increasing motion instability risk. This process is repeated for each command point to obtain the potential energy gradient information for all command points.

[0087] The potential energy feature generation submodule calculates the potential energy gradient direction angle difference between adjacent command points based on the potential energy gradient information of the command points, and integrates the robot algebraic potential energy value of each command point with the calculated potential energy gradient direction angle difference to generate a robot algebraic potential energy feature set.

[0088] Based on the potential energy gradient information at the command points, taking the first and second command points as examples, the gradient direction vector of the first command point is (0.1, -0.05, 0.02, 0.0, -0.08, 0.03), and the gradient direction vector of the second command point is (0.12, -0.04, 0.025, 0.0, -0.09, 0.033). The cosine of the angle between these two multidimensional vectors is calculated, and then the angle difference is obtained using the inverse cosine function. The calculation process for the angle difference is as follows: first, calculate the dot product of the two vectors; then, calculate the magnitudes of the two vectors separately; finally, divide the dot product by the product of the two magnitudes to obtain the cosine of the angle, ultimately finding an angle difference of 5.1 degrees. The robot's algebraic potential energy value at each command point is integrated with the calculated angle difference between it and the subsequent command point. Taking the aforementioned first command point as an example, its algebraic potential energy value is 5.763, and the calculated angle difference is 5.1 degrees. Combine these two values ​​into a feature pair (5.763, 5.1). Repeat this calculation and integration process for all adjacent command point pairs to generate a feature pair for all command points except the last one. The set of these feature pairs ultimately constitutes the robot's algebraic potential energy feature set.

[0089] Please see Figure 2 and Figure 6 The instruction point update module includes:

[0090] The posterior probability calculation submodule inputs the robot's algebraic potential energy feature set and the robot's residual stability parameter set into the Bayesian decision model to perform posterior probability calculation, obtains the motion reliability posterior probability value corresponding to each robot joint position command point, and establishes a robot command point confidence table based on the posterior probability value.

[0091] The robot's algebraic potential energy feature set and robot residual stability parameter set are input into the Bayesian decision model. For a specific robot joint position command point, the robot's algebraic potential energy value and the angle difference between the potential energy gradient direction are extracted from the robot's algebraic potential energy feature set, and the corrected joint torque change rate residual value corresponding to the timestamp is extracted from the robot's residual stability parameter set. These three data points together constitute a multidimensional observation vector. The Bayesian decision model calculates the posterior probability that the command point belongs to a certain motion reliability category through the following operation: ;

[0092] in, The observation vector, in the current robot motion control scenario, is a multi-dimensional vector representing the core basis for evaluating the reliability of a joint position command to be executed. This vector consists of three key indicators calculated in previous steps: the robot's algebraic potential energy value, the potential energy gradient direction angle difference, and the corrected joint torque change rate residual. The values ​​of these three indicators being "large" or "small" are relative; their judgment criterion is not a simple fixed threshold, but rather determined by the model through... The learned statistical distribution patterns for different categories. Specifically: for the "reliable" category ( The model learns from a large amount of historical data that the values ​​of these three indicators are usually distributed within a low range. When a new observation vector... When the numerical characteristics of the value closely match the statistical distribution of this "reliable" interval (i.e., If the value is relatively high, it is more likely to be judged as reliable. Conversely, for the "unreliable" category (…), it is more likely to be judged as reliable. The model learns that its index values ​​are often significantly higher. If The numerical characteristics fall into the high-frequency range of the "unreliable" category (i.e., If the value is relatively high, then it is more likely to be judged as unreliable.

[0093] Motion reliability category. In the current scenario, it represents the reliability assessment result for a command point. There are two categories: The "reliable" state indicates that the robot's movement is smooth and as expected when executing the command point; This indicates an "unreliable" state, meaning that executing this command point may result in abnormal behavior such as vibration, impact, or deviation from the trajectory.

[0094] Posterior probability. It represents the probability after obtaining a specific observation vector. Then, the calculated instruction points belong to the category. The probability. This is the basis for the final decision, for example... It is the confidence level at which the instruction point is judged as "reliable".

[0095] Prior probability. It represents the likelihood, based on historical experience, that a command point belongs to a certain category before any specific observation is made. The inherent probability. This value is determined by statistically analyzing a large amount of historical robot operation data, showing the overall proportion of "reliable" and "unreliable" command points. For example, if 95% of the command points historically were reliable, then... Let's set it to 0.95.

[0096] Class conditional probability, also known as likelihood. It represents the probability that a given instruction point actually belongs to a given category. Given a category (e.g., known to be a "reliable" point), the current specific feature vector is observed. How likely is it? This probability value is provided by a Gaussian mixture probability density model that has been pre-trained using a large amount of labeled historical data.

[0097] The total number of categories. In this binary classification problem, The value is 2 ("reliable" and "unreliable").

[0098] Category index. It is a variable used to refer to a specific category, when... When the formula calculates the probability of a "reliable" state, it calculates the probability of a "reliable" state. At that time, the probability of an "unreliable" state is calculated.

[0099] : Summation index. It is used to calculate the sum in the denominator, iterating through all possible categories (from 1 to 1). ).

[0100] Marginal probability. This term is the sum of the numerators for all classes, serving as a normalization constant to ensure that the sum of the posterior probabilities for all possible classes equals 1.

[0101] Taking a single instruction point as an example, the likelihood probability of its observation vector x (after normalization) in the model is... The likelihood probability is 0.8. It is 0.15.

[0102] 1. Calculate the "reliable" state ( The numerator of )

[0103] ;

[0104] 2. Calculate the "unreliable" state ( The numerator of )

[0105] ;

[0106] 3. Calculate the normalized denominator:

[0107] ;

[0108] 4. Calculate the final posterior probability that the instruction point belongs to the "reliable" state:

[0109] .

[0110] This calculation is repeated for each robot joint position command point to obtain the posterior probability value of motion reliability corresponding to each command point, and a robot command point confidence table is established based on these posterior probability values.

[0111] The command point filtering submodule compares the posterior probability value of each command point recorded in the robot command point confidence table with the preset confidence threshold one by one to determine the confidence level of the command point's motion. Command points below the confidence threshold are marked uniformly, and a list of command points to be replaced is generated.

[0112] The process of determining the reliability of the motion at the command point is as follows:

[0113] The posterior probability value of motion reliability corresponding to each robot joint position command point recorded in the robot command point confidence table is compared with the preset confidence threshold.

[0114] When the posterior probability value of motion reliability is greater than or equal to the preset confidence threshold, the corresponding robot joint position command point is determined as a reliable command point and directly retained.

[0115] When the posterior probability value of motion reliability is less than the preset confidence threshold, the corresponding robot joint position command point is determined as an untrusted command point, and all untrusted command points are uniformly marked.

[0116] The posterior probability value of each command point recorded in the robot command point confidence table is compared one by one with a preset confidence threshold. The confidence threshold is set based on the analysis of robot operation failure cases. During the experimental phase, all command points that caused motion stoppage or severe vibration were recorded, and the posterior probability values ​​of these command points were examined. It was found that the posterior probability values ​​corresponding to all failure cases were below 0.90, while the probability values ​​of most success cases were above this value. To leave a safety margin, the confidence threshold was set to 0.90. The process of judging the motion reliability of command points is as follows: the posterior probability value of motion reliability corresponding to each robot joint position command point recorded in the robot command point confidence table is compared with the preset confidence threshold of 0.90. For example, the posterior probability value of the first command point is 0.990. Since 0.990 is greater than or equal to 0.90, this command point is judged as a reliable command point and directly retained. Assume that the calculated posterior probability value of the fifth command point is 0.85. Since 0.85 is less than 0.90, this instruction point is determined to be an untrusted instruction point. All instruction points determined to be untrusted, such as the fifth instruction point, are marked uniformly. After comparing all instruction points, the lists of all marked untrusted instruction points are compiled to generate a list of instruction points to be replaced.

[0117] The instruction point set update submodule, for the list of instruction points to be replaced, calls the robot algebraic potential energy feature set, selects replacement points based on the potential energy difference between adjacent instruction points in the list, and merges all the original instruction points and all replacement points to obtain the updated instruction point set;

[0118] Suppose the list contains a fifth command point. The robot's algebraic potential energy feature set is then invoked to find the potential energy information of the fifth command point and its adjacent fourth and sixth command points. A replacement point is selected based on the potential energy difference between adjacent command points in the list. Specifically, the algebraic potential energy values ​​of the fourth and sixth command points are compared. Assume the potential energy value of the fourth command point is 4.2, and the potential energy value of the sixth command point is 4.5. Because the potential energy value of the fourth command point (4.2) is lower than that of the sixth command point (4.5), the fourth command point with the lower potential energy is selected as the replacement point for the fifth command point. This selection is based on the fact that lower algebraic potential energy usually corresponds to a more stable motion state that is closer to the theoretical model. Therefore, the original fifth command point is removed from the command sequence and replaced with a copy of the fourth command point. All retained original reliable command points (such as the first to fourth, and sixth to tenth command points) are merged with all selected replacement points (such as the point replacing the fifth command point) in chronological order. The merged command point sequence is the updated command point set.

[0119] Please see Figure 2 and Figure 7 The control execution module includes:

[0120] The motion trajectory construction submodule connects each command point in the updated command point set in a preset motion space to obtain the robot's motion control trajectory.

[0121] The process of connecting each command point in the updated command point set within the preset motion space is as follows:

[0122] The rotation angle range and maximum angular velocity of each joint of the robot are obtained as physical limit parameters, and the three-dimensional spatial coordinate data of static obstacles in the robot's working environment are collected.

[0123] The robot joint space constraint boundary is established based on the physical limit parameters, and the three-dimensional spatial coordinate data of the static obstacle is mapped to the robot joint space through inverse kinematics calculation to generate the obstacle region in the joint space.

[0124] Combine all areas within the robot's joint space constraint boundary that are not occupied by obstacles, and define the combined area as the preset motion space.

[0125] First, the physical limit parameters of each joint of the robot are obtained, derived from the specifications provided by the robot manufacturer. Taking the second joint as an example, its rotation angle range is -1.92 radians to 2.09 radians, and its maximum angular velocity is 2.5 radians / second. Simultaneously, the three-dimensional spatial coordinate data of static obstacles in the robot's operating environment are collected. For example, if there is a fixed device next to the robot, its vertex coordinates in the robot's base coordinate system are collected and constructed into a three-dimensional model. Based on the physical limit parameters, the constraint boundaries of the robot's joint space are established. For the second joint, its range of motion is [-1.92, 2.09]. This process is performed on all six joints, forming a six-dimensional hypercube constraint space. Subsequently, the three-dimensional spatial coordinate data of the static obstacles are calculated using inverse kinematics and mapped to the robot's joint space. This means calculating all possible combinations of joint angles when the robot's end effector touches an obstacle. These combinations form one or more irregular regions in the joint space, which are the obstacle regions within the joint space. For example, after mapping, it was found that when the angle of the second joint is within the range of [1.5, 1.7] radians, the robot arm will collide with the adjacent device regardless of the configuration of other joints. Therefore, [1.5, 1.7] becomes an obstacle region for the second joint. All areas within the robot joint space constraint boundary that are not occupied by obstacle regions are combined. For the second joint, its available motion space is [-1.92, 1.5) and (1.7, 2.09). The available spaces of all joints are combined, and this combined safe area is determined as the preset motion space. Finally, each command point in the updated command point set is connected within the aforementioned preset motion space. The connection method is to perform linear interpolation between adjacent command points to form segmented straight paths. These path segments are connected end-to-end, ultimately resulting in a complete robot motion control trajectory.

[0126] The instruction interpolation calculation submodule analyzes the target joint angle and target joint torque instructions required at each discrete moment during the robot's motion control trajectory to obtain the target instruction at each moment.

[0127] The servo cycle of the robot controller is determined by its hardware specifications, for example, 2 milliseconds. The first segment of the trajectory connects the first and second updated command points. Assume the joint angle coordinates of these two points are the angles of the first and second command points, respectively. The total execution time of this segment is calculated based on the distance between the two points and the maximum permissible speed, for example, 100 milliseconds. At the first 2-millisecond moment, the target joint angle that the robot needs to reach is calculated using linear interpolation. The calculation method is: first command point angle + (second command point angle - first command point angle) × (2 / 100). Simultaneously, using the robot's dynamic model, based on this interpolated target position, velocity, and acceleration, the target joint torque required to drive the joint to this state is calculated in reverse. This interpolation and calculation is performed for all six joints. At the next 2-millisecond moment (i.e., a total time of 4 milliseconds), the interpolation calculation is performed again, and the corresponding target torque is calculated. This process is repeated throughout the entire 100 milliseconds until the second updated command point is reached. Then, the entire process is repeated for the next segment. In this way, a set of instructions containing six target joint angles and six target joint torques is parsed for each servo cycle.

[0128] The instruction set integration submodule arranges all target joint angle and target joint torque instruction data for each moment according to the execution time sequence to form a robot motion control instruction set.

[0129] Create a data list or queue. At timestamp 0.000, send no instructions or send instructions to maintain the current state. At timestamp 0.002, store the calculated first set of target instructions (containing six target joint angle values ​​and six target joint torque values) into the list. At timestamp 0.004, store the calculated second set of target instructions into the list. And so on, along the entire motion control trajectory, adding the target instruction data calculated for each servo cycle to the end of the list in strict chronological order. Once all interpolation points for the entire trajectory have been calculated, this complete list, containing all target joint angle and torque instruction data arranged chronologically from start to finish, ultimately forms the robot motion control instruction set. This instruction set is ultimately used to drive the servo motors of each joint of the robot, enabling it to execute tasks accurately and smoothly along the planned trajectory.

[0130] The above are merely preferred embodiments of the present invention and are not intended to limit the present invention in any other way. Any person skilled in the art may make changes or modifications to the above-disclosed technical content to create equivalent embodiments that can be applied to other fields. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A robot motion planning and control algorithm system improved by algebraic algorithms, characterized in that: The system includes: The parameter acquisition module acquires the robot joint angle signal sequence and robot joint torque signal sequence as the robot moves over time, calculates the robot joint motion rate and robot joint torque change rate, and generates the robot's initial motion parameter set; The residual calculation module compares the robot joint motion rate and robot joint torque change rate in the initial motion parameter set of the robot with their respective preset theoretical rate residuals to generate a robot residual stability parameter set. The potential energy analysis module acquires the robot joint position command points, determines the robot algebraic potential energy value for each command point based on the robot residual stability parameter set and the robot initial motion parameter set, and generates a robot algebraic potential energy feature set. The command point update module inputs the robot algebraic potential energy feature set and the robot residual stability parameter set into the Bayesian decision model, calculates the posterior probability of the reliability of the robot joint position command points, and filters command points according to the posterior probability to obtain the updated command point set. The control execution module constructs the robot motion control trajectory based on the updated instruction point set, and obtains the robot motion control instruction set. The potential energy analysis module includes: The command point generation submodule acquires the target pose data of the robot end effector's workspace, and calls the joint angle signal sequence in the robot's initial motion parameter set and the target pose data of the robot end effector's workspace. It generates a smooth intermediate path point by performing interpolation between the two, which serves as the robot joint position command point. The algebraic potential energy determination submodule performs joint algebraic operations on the robot joint motion rate and robot joint torque change rate in the robot residual stability parameter set and the robot initial motion parameter set for each robot joint position command point, and performs gradient direction analysis on the operation results to determine the robot algebraic potential energy value and gradient direction of each command point, and obtains the potential energy gradient information of the command point. The potential energy feature generation submodule calculates the potential energy gradient direction angle difference between adjacent command points based on the potential energy gradient information of the command points, and integrates the robot algebraic potential energy value of each command point with the calculated potential energy gradient direction angle difference to generate a robot algebraic potential energy feature set.

2. The robot motion planning and control algorithm system improved by algebraic algorithm according to claim 1, characterized in that: The robot's initial motion parameter set includes robot joint angle change information, robot joint motion rate information, and robot joint torque change rate information; the robot residual stability parameter set includes robot joint motion rate residual data, robot joint torque change rate residual data, and robot residual trend smoothing results; the robot algebraic potential energy feature set includes robot algebraic potential energy values, potential energy gradient direction angle difference, and potential energy feature parameters; the updated command point set includes command point pose data, command point confidence data, and replacement command point identifiers; the robot motion control command set includes control trajectory sequences, target joint angle commands, and target torque commands.

3. The robot motion planning and control algorithm system improved by algebraic algorithm according to claim 1, characterized in that: The parameter acquisition module includes: The motion data acquisition submodule collects robot joint angle signal sequences and robot joint torque signal sequences through joint angle sensors and torque sensors, assigns a corresponding timestamp to each signal sequence, and combines the robot joint angle signal sequences and robot joint torque signal sequences with timestamps to establish a real-time motion state dataset for the robot. The motion rate calculation submodule performs time difference calculation on robot joint angle values ​​at adjacent timestamps in the robot's real-time motion state dataset to obtain the robot joint motion rate, and performs time difference calculation on robot joint torque values ​​at adjacent timestamps to obtain the robot joint torque change rate, generating joint dynamic change rate information. The motion parameter generation submodule constructs an initial motion parameter set for the robot based on the robot joint angle signal sequence and robot joint torque signal sequence in the robot's real-time motion state dataset, combined with the robot joint motion rate and robot joint torque change rate in the joint dynamic change rate information.

4. The robot motion planning and control algorithm system improved by algebraic algorithm according to claim 1, characterized in that: The residual calculation module includes: The initial residual calculation submodule extracts the robot joint motion rate and robot joint torque change rate from the robot initial motion parameter set, compares them with the preset robot theoretical joint motion rate and robot theoretical joint torque change rate, obtains the difference between the two, and generates a robot algebraic constraint residual set. The residual trend analysis submodule calculates the trend of each residual within a set sliding time window based on the robot algebraic constraint residual set, compares the residual trend with a preset trend fluctuation threshold, filters residual data that exceed the trend fluctuation threshold, and obtains abnormal residual trend items. The residual correction submodule identifies the residual data marked by the abnormal residual trend term in the robot algebraic constraint residual set, performs state estimation and correction smoothing on the marked residual data, and integrates the smoothed residual data with other data in the robot algebraic constraint residual set to generate a robot residual stability parameter set.

5. The robot motion planning and control algorithm system improved by algebraic algorithm according to claim 1, characterized in that: The specific process of performing gradient direction analysis on the calculation result is as follows: Establish a joint coordinate system with the angle of each joint of the robot as the coordinate axis; In the joint coordinate system, calculate the rate of change of the result of the joint algebraic operation relative to each joint angle coordinate axis; Combine the rate of change corresponding to all joint angle coordinate axes as components of a multidimensional vector; The multidimensional vector is used to determine the gradient direction.

6. The robot motion planning and control algorithm system improved by algebraic algorithm according to claim 1, characterized in that: The instruction point update module includes: The posterior probability calculation submodule inputs the robot algebraic potential energy feature set and the robot residual stability parameter set into the Bayesian decision model to perform posterior probability calculation, obtains the motion reliability posterior probability value corresponding to each robot joint position command point, and establishes a robot command point confidence table based on the posterior probability value. The command point filtering submodule compares the posterior probability value of each command point recorded in the robot command point confidence table with a preset confidence threshold one by one to determine the confidence level of the command point's movement, marks command points that are below the confidence threshold, and generates a list of command points to be replaced. The instruction point set update submodule, for the list of instruction points to be replaced, calls the robot algebraic potential energy feature set, selects replacement points based on the potential energy difference between adjacent instruction points in the list, and merges all the original instruction points that are retained with all the replacement points to obtain the updated instruction point set.

7. The robot motion planning and control algorithm system improved by algebraic algorithm according to claim 6, characterized in that: The process of determining the reliability of the motion at the command point is as follows: The posterior probability value of motion reliability corresponding to each robot joint position command point recorded in the robot command point confidence table is compared with the preset confidence threshold. When the posterior probability value of motion reliability is greater than or equal to the preset confidence threshold, the corresponding robot joint position command point is determined as a reliable command point and directly retained. When the posterior probability value of motion reliability is less than the preset confidence threshold, the corresponding robot joint position command point is determined as an untrusted command point, and all untrusted command points are uniformly marked.

8. The robot motion planning and control algorithm system improved by algebraic algorithm according to claim 1, characterized in that: The control execution module includes: The motion trajectory construction submodule connects each instruction point of the updated instruction point set in a preset motion space to obtain the robot motion control trajectory. The instruction interpolation calculation submodule analyzes the target joint angle and target joint torque instructions required to be achieved at each discrete moment during the robot's motion control trajectory, and obtains the target instruction at each moment. The instruction set integration submodule arranges all target joint angle and target joint torque instruction data for each moment in the order of execution time to form a robot motion control instruction set.

9. The robot motion planning and control algorithm system improved by algebraic algorithm according to claim 8, characterized in that: The process of connecting each command point in the updated command point set in the preset motion space is as follows: The rotation angle range and maximum angular velocity of each joint of the robot are obtained as physical limit parameters, and the three-dimensional spatial coordinate data of static obstacles in the robot's working environment are collected. The robot joint space constraint boundary is established based on the physical limit parameters, and the three-dimensional spatial coordinate data of the static obstacle is mapped to the robot joint space through inverse kinematics calculation to generate the obstacle region in the joint space. Combine all areas within the robot's joint space constraint boundary that are not occupied by obstacles, and define the combined area as the preset motion space.

Images