Robot motion trail correction control method and device based on digital twinning

Abstract

The invention relates to the technical field of motion control, in particular to a robot motion trail correction control method based on digital twinning. The invention also discloses a method. The motion state data of the robot are collected in real time, the motion track is predicted and corrected based on the digital twin model, the path is dynamically adjusted, it is ensured that the robot accurately follows the preset track in the complex environment, errors are rapidly corrected, and the motion precision and stability are improved. A three-level error correction strategy is adopted, and the correction force is flexibly adjusted according to the deviation distance calculated in real time, first-level correction is suitable for small deviation, second-level correction is suitable for medium deviation, and third-level correction is suitable for large deviation. According to the grading strategy, excessive correction is avoided, and the robustness and flexibility of the system are improved. By establishing a three-layer digital twinborn model, virtual-real combination is realized, an actual motion state is simulated, a future trajectory is predicted, and the response speed to complex environment deviation is increased.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of motion control technology, in particular to a robot motion trajectory correction control method and device based on digital twinning. BACKGROUND

[0002] Robot motion control is an important research field in robotics, mainly focusing on how to control robots to move and position in complex environments through computers. With the rapid development of artificial intelligence and robotics, accurate path planning and real-time motion correction have become important topics for improving the autonomy and adaptability of robots. Digital twinning technology simulates physical systems in virtual space, providing real-time virtual-real combined control strategies for robots, which can effectively improve the stability and accuracy of robot motion in dynamic environments. This technology is widely used in intelligent robots, autonomous driving, drones, automation, and other fields, especially in complex tasks that require high precision and real-time adjustment, such as industrial automation, logistics transportation, and service robots.

[0003] In existing robot motion control technology, although there are control methods based on sensor data collection and path planning, there are still several key problems and deficiencies:

[0004] Firstly, the existing technology lacks real-time motion trajectory correction: the existing technology usually uses simple feedback control methods when correcting the motion trajectory, which is difficult to deal with dynamic changes and error accumulation in complex environments, especially when there is a large deviation, it is often unable to correct in time or not accurate enough. This leads to the robot being unable to accurately follow the preset trajectory in complex environments, affecting the motion accuracy and stability.

[0005] Secondly, the existing technology lacks flexible error correction strategy adjustment: many existing systems fail to flexibly adjust the correction strategy according to the size of the error and the environment. Most systems use a uniform correction method, ignoring the impact of error size on motion trajectory, resulting in over-correction in small deviation cases, increasing system resource consumption, and inability to correct in time in large deviation cases, reducing the adaptability of robots in dynamic environments.

[0006] To address the above problems, it is necessary to propose a robot motion trajectory correction control method and device based on digital twinning. SUMMARY

[0007] The purpose of the present application is to solve the problems in the background art and propose a robot motion trajectory correction control method and device based on digital twinning.

[0008] The purpose of the present application can be achieved through the following technical solutions:

[0009] The application provides a robot motion trajectory correction control method and device based on digital twinning, including a robot motion trajectory correction control method based on digital twinning and a robot motion trajectory correction control device based on digital twinning.

[0010] In a first aspect, the application provides a robot motion trajectory correction control method based on digital twinning, including the following steps:

[0011] Step one, real-time motion state acquisition and synchronous transmission;

[0012] Access the IMU inertial measurement unit, positioning device, encoder, accelerometer, and gyroscope in the robot to acquire real-time motion state data of the robot, including angular velocity, linear velocity, and position coordinates.

[0013] Including: real-time position coordinates (x0, y0, z0), real-time linear velocity (vx, vy, vz), real-time angular velocity (ωx, ωy, ωz), and real-time acceleration (ax, ay, az).

[0014] Wherein x0, y0, and z0 are the x, y, and z components of the real-time position coordinates, respectively;

[0015] Wherein vx, vy, and vz are the linear velocities of the robot along the x, y, and z axis directions, respectively;

[0016] Wherein ωx, ωy, and ωz are the angular velocities of the robot around the x, y, and z axis directions, respectively;

[0017] Wherein ax, ay, and az are the accelerations of the robot along the x, y, and z axis directions, respectively.

[0018] Step two, preset trajectory mapping and discretization processing;

[0019] Retrieve the preset trajectory of the robot, and discretize the continuous preset trajectory of the robot into a preset number of discrete points, and then divide the trajectory into a plurality of trajectory units with a preset number of segments.

[0020] Obtain the preset motion trajectory of the robot in the spatial coordinate system, and obtain its preset motion trajectory equation L = f(x, y, z).

[0021] As a preferred mode of the application, the preset motion trajectory L of the robot is discretized, the continuous preset motion trajectory is equally divided, K discrete points on the preset motion trajectory L are obtained, the number K of discrete points is a preset value, the discrete points obtained after equal division are sequentially numbered, and the number is k; k = 1, 2, 3,..., K.

[0022] As a preferred mode of the present application, the continuous preset motion trajectory L is divided into K-1 trajectory units with discrete points as division nodes, and the end point discrete point of each trajectory unit is regarded as the expected position of the trajectory unit, i.e., the target position finally reached by the robot on the trajectory unit.

[0023] As a preferred mode of the present application, the discrete point coordinates (x_k, y_k, z_k) = (x_1, y_1, z_1), (x_2, y_2, z_2),..., (x_K, y_K, z_K) of each trajectory unit are located.

[0024] Step three, the digital twin model drives and maps the virtual and real trajectories in real time.

[0025] A three-layer digital twin model is established, and the virtual copy of the robot in the three-layer digital twin model reflects the real motion state and future motion state result prediction of the robot in the physical world in real time. The three-layer digital twin model includes a trajectory planning model of the robot, a motion mapping model, and a dynamics prediction model, which respectively complete virtual mapping of the preset motion trajectory of the robot, virtual mapping of the real motion trajectory and motion state, and virtual mapping of driving force and driving torque about the influence of future trajectory.

[0026] The trajectory planning model of the robot is specifically:

[0027] A first three-dimensional space model is established, and the motion trajectory start point coordinates (x_origin, y_origin, z_origin), trajectory end point coordinates (x_end, y_end, z_end), and preset motion trajectory equation L = f(x, y, z) of the robot are located in the first three-dimensional space model.

[0028] The discrete point coordinates (x_k, y_k, z_k) = (x_1, y_1, z_1), (x_2, y_2, z_2),..., (x_K, y_K, z_K) are located and marked in the first three-dimensional space model according to the corresponding coordinates.

[0029] The motion mapping model of the robot is specifically:

[0030] A second three-dimensional space model is established, and the real-time position coordinates (x0, y0, z0) of the robot at each time t are located in the second three-dimensional space model, denoted as the position vector p(t) = [x(t), y(t), z(t)] at time t. T The corresponding real-time position coordinates (x0, y0, z0) are converted into the corresponding historical position coordinates [x(t), y(t), z(t)] at time t, and are input as the historical location coordinates of the robot into the second three-dimensional space model for positioning and marking.

[0031] The linear velocity vector v(t) = [v x (t), v y (t), v z (t)] T , the angular velocity vector ω(t) = [ω x (t), ω y (t), ω z (t)] T and the acceleration vector a(t) = [a x (t), a y (t), a z (t)] T are input to the corresponding positions in the second three-dimensional space model according to the historical position coordinates [x(t), y(t), z(t)] of the robot.

[0032] The position vector p(t) = [x(t), y(t), z(t)] T , where x(t), y(t) and z(t) are respectively the position coordinate components of the robot along the x, y and z axes collected at time t;

[0033] The velocity vector v(t) = [v x (t), v y (t), v z (t)] T , where v x (t), v y (t) and v z (t) are respectively the velocity components of the robot along the x, y and z axes collected at time t;

[0034] The angular velocity vector ω(t) = [ω x (t), ω y (t), ω z (t)] T , where ω x (t), ω x (t) and ω x (t) are respectively the angular velocity components of the robot along the x, y and z axes collected at time t;

[0035] The acceleration vector a(t) = [a x (t), a y (t), a z (t)] T , where a x (t), a x (t) and a x (t) are respectively the acceleration components of the robot along the x, y and z axes collected at time t.

[0036] The dynamic prediction model of the robot comprises a third three-dimensional space model, a position deviation calculation program, a trajectory prediction program and a dynamic calculation program of the robot, which are specifically as follows:

[0037] A third three-dimensional space model is established to construct a parameter matrix of adjustment state of the robot at each time t The elements are all unknown quantities to be solved, and their specific values are obtained by simultaneous solution of the position deviation calculation program, the trajectory prediction program and the dynamic calculation program.

[0038] wherein, and are respectively x, y and z components of the position adjustment value of the robot, representing specific values of the position correction distance along the x, y and z axis directions if the robot returns to the preset motion trajectory from the current position. The position adjustment value component is positive when it represents the correction along the positive direction of the x, y or z axis, and is negative when it represents the correction along the negative direction of the x, y or z axis.

[0039] wherein, and are respectively x, y and z components of the linear velocity adjustment value of the robot, representing specific values of the linear velocity correction along the x, y and z axis directions if the robot returns to the preset motion trajectory from the current position. The linear velocity correction value component is positive when it represents the correction along the positive direction of the x, y or z axis, and is negative when it represents the correction along the negative direction of the x, y or z axis.

[0040] wherein, and are respectively x, y and z components of the angular velocity adjustment value of the robot, representing specific values of the angular velocity correction around the x, y and z axis directions if the robot returns to the preset motion trajectory from the current position. The angular velocity correction value component is positive when it represents the correction in the clockwise direction around the x, y or z axis, and is negative when it represents the correction in the counterclockwise direction around the x, y or z axis.

[0041] wherein, and are respectively x, y and z components of the acceleration adjustment value of the robot, representing specific values of the acceleration correction along the x, y and z axis directions if the robot returns to the preset motion trajectory from the current position. The acceleration correction value component is positive when it represents the correction along the positive direction of the x, y or z axis, and is negative when it represents the correction along the negative direction of the x, y or z axis.

[0042] The position deviation calculation program, the trajectory prediction program and the dynamic calculation program are specifically as follows:

[0043] The position deviation calculation program is specifically as follows:

[0044] The real-time position coordinates (x0, y0, z0) of the robot and a preset motion trajectory equation L=f(x, y, z) are acquired, and the preset motion trajectory equation L=f(x, y, z) is parameterized as L=f[f1(u), f2(u), f1(u)], wherein wherein u is a projection point parameter, wherein f1(u), f2(u), f3(u) are parameterized component functions, respectively representing the variable relationship between x, y and z in the preset motion trajectory equation L=f(x, y, z) and the projection point parameter u. After parameterization, the motion trajectory is converted from a curve defined by three variables x, y and z to a curve defined by a single variable u.

[0045] The projection point parameter u is solved by a preset formula The u is substituted back into L=f[x(u), y(u), z(u)] to obtain the projection point (x^, y^, z^) of the real-time position coordinates (x0, y0, z0) on the preset motion trajectory equation L=f(x, y, z).

[0046] As a preferred mode of the present application, the projection point parameter u is solved by a preset formula The x, y and z components of the position adjustment value of the robot are calculated and

[0047] The trajectory prediction program of the robot is specifically: wherein and are the coordinate values of the robot along the x, y and z coordinate axes at t time after prediction;

[0048] The dynamics operation program of the robot is specifically:

[0049]

[0050] wherein, and F x，y，z (t) = m x a x，y，z (t) is a linear dynamics equation; wherein F x，y，z (t) is the linear driving force output by the motor of the robot, including the linear driving forces F x (t), F y (t) and F z (t) generated by the robot along the x, y and z coordinate axes at t time; wherein m is the preset standard mass of the robot, and a x，y，z (t) includes the acceleration components a x (t), a y (t) and a z (t) of the robot along the x, y and z coordinate axes at t time;

[0051] in, This is the equation of rotational dynamics; where τ x，y，z (t) represents the rotational driving torque output by the robot motor, including the rotational driving torque τ generated by the robot at time t about the x, y, and z axes. x (t), τ y (t) and τ z (t); where I x，y，z The preset rotational inertia of the robot about the x, y, and z axes; where ω x，y，z (t) represents the angular velocity components of the robot around the x, y, and z axes at time t.

[0052] The first, second, and third three-dimensional spatial models share the same coordinate system, including the same origin and the coordinate directions of the x, y, and z axes.

[0053] Step 4: Error correction strategy and control signal generation;

[0054] An error correction strategy is determined by measuring the distance between the robot's real-time position coordinates and the preset trajectory. First-, second-, or third-level error correction signals are matched to ensure optimal adjustment measures are applied for different trajectory deviations. Different reference points are located based on the first-, second-, or third-level error correction signals as the target focus for trajectory correction, and the x, y, and z components of the position adjustment value are calculated. These are then input into the trajectory prediction program and the dynamics calculation program. Finally, linear driving forces and rotational driving torques are generated based on the trajectory prediction program and the dynamics calculation program to complete the precise trajectory correction of the robot.

[0055] Retrieve the robot's real-time position coordinates (x0, y0, z0), real-time linear velocity (vx, vy, vz), and real-time angular velocity (ωx, ωy, ωz).

[0056] Input the robot's real-time position coordinates (x0, y0, z0) into the position deviation calculation program to obtain the x, y, and z components of the robot's position adjustment value. and

[0057] By preset formula Calculate the deviation distance E(t) of the robot at time t.

[0058] As a preferred embodiment of the present invention, an error correction strategy is matched according to the deviation distance E(t). If the deviation distance E(t) is less than the first preset threshold E1, a first-level error correction signal is matched.

[0059] If the deviation distance E(t) is greater than or equal to the first preset threshold E1 and less than the second preset threshold E2, then the second-level error correction signal is matched.

[0060] If the deviation distance E(t) is greater than or equal to the second preset threshold E2, then a level 3 error correction signal is matched.

[0061] Upon detecting the robot's first-level error correction signal, the discrete point (x_k, y_k, z_k) with the minimum distance to the robot's real-time position coordinates (x_k, y_k, z_k) is obtained, and the N1th discrete point (x_k+N1, y_k+N1, z_k+N1) preceding this discrete point (x_k, y_k, z_k) is located. This discrete point (x_k+N1, y_k+N1, z_k+N1) is then used as the projection point (x^, y^, z^) and input into the robot's trajectory prediction program and dynamics calculation program.

[0062] Upon detecting the robot's secondary error correction signal, the discrete point (x_k, y_k, z_k) with the minimum distance to the robot's real-time position coordinates (x_k, y_k, z_k) is obtained, and the N2th discrete point (x_k+N1, y_k+N1, z_k+N1) preceding this discrete point (x_k, y_k, z_k) is located. The discrete point (x_k+N1, y_k+N1, z_k+N1) is then used as the projection point (x^, y^, z^) and input into the robot's trajectory prediction program and dynamics calculation program.

[0063] Once the robot's level 3 error correction signal is detected, the discrete point with the minimum distance to the robot's real-time position coordinates (x0, y0, z0) is immediately located, and the coordinates of the located discrete point are used as projection points (x^, y^, z^) and input into the robot's trajectory prediction program and the robot's dynamics calculation program.

[0064] Wherein, N1 and N2 are preset calibration parameters, N1 is greater than N2 and both N1 and N2 are positive integers.

[0065] After receiving the projection point (x^, y^, z^), the robot's trajectory prediction program calculates the x, y, and z components of the position adjustment value. and And send it to the robot's dynamics calculation program. The robot's dynamics calculation program, through... and Solving for the linear driving force F x (t), F y (t) and F z (t), and solve for the rotational driving torque τ. x (t), τ y (t) and τ z (t).

[0066] linear driving force F x(t), F y (t) and F z (t) and the de-rotation driving torque τ x (t), τ y (t) and τ z (t) and τ are sent to the robot as motor output parameters for trajectory correction, completing the trajectory correction task.

[0067] Every preset time interval, the deviation distance E(t) of the robot is recalculated, and the level of the error correction signal is dynamically updated, so as to adjust the corresponding trajectory correction strategy.

[0068] In a second aspect, the present application provides a robot motion trajectory correction control device based on digital twinning, comprising a data acquisition module, a trajectory planning module, a central calculation module and a signal matching module.

[0069] The data acquisition module is responsible for real-time acquisition of the motion state data of the robot, including position coordinates, linear velocity, angular velocity and acceleration. The motion state data comes from the sensors inside the robot, including the IMU inertial measurement unit, the positioning device and the encoder, ensuring that the real-time motion state of the robot is accurately monitored and synchronously transmitted.

[0070] The trajectory planning module is responsible for generating the preset motion trajectory of the robot and performing discretization processing. The discretized trajectory will be divided into multiple trajectory units, each trajectory unit corresponding to a desired position, so that the robot can accurately control the path according to these discrete points.

[0071] The central calculation module stores the trajectory planning model, motion mapping model and dynamics prediction model of the robot and the first, second and third three-dimensional space models contained therein. It is responsible for processing information from the data acquisition module and the trajectory planning module, and performing calculation tasks contained in the programs in the models, including position deviation calculation, trajectory prediction and dynamics calculation. Real-time receive the position, velocity and other data of the robot, calculate the deviation distance E(t).

[0072] The signal matching module matches the preset correction strategy signal. By parameterizing the trajectory, solving the shortest distance principle, and combining the prediction model, the error correction signal and control instruction are generated. According to the level of the error correction signal, the adjustment intensity of the robot and the discrete points as the correction target are determined, and the correction target is generated. The linear driving force and the rotation driving torque are solved. The linear driving force and the rotation driving torque are sent to the robot as motor output parameters for trajectory correction, completing the trajectory correction task.

[0073] Every preset time interval, the deviation distance E(t) of the robot is recalculated, and the level of the error correction signal is dynamically updated, so as to adjust the corresponding trajectory correction strategy.

[0074] Compared with the prior art, the present application has the following advantages:

[0075] 1、The present application collects real-time motion state data of the robot (such as position coordinates, linear velocity, angular velocity and acceleration), and predicts and corrects the robot motion trajectory based on the digital twin model. Combined with position deviation calculation, trajectory prediction and dynamics operation, the motion path of the robot can be dynamically adjusted to ensure that the robot can accurately follow the preset trajectory in a complex environment, and even when errors occur, the robot can quickly make corrections to improve motion accuracy and stability.

[0076] 2、According to the real-time calculated deviation distance E(t), the present application adjusts the correction strength flexibly according to the error size through a three-level error correction strategy (first, second and third level signals). The first level error is suitable for small deviation, the second level error is suitable for medium deviation, and the third level error is suitable for large deviation. This hierarchical correction strategy can effectively avoid overcorrection, while quickly making corrections when there is a serious deviation from the trajectory, improving the robustness and flexibility of the system, and optimizing resource utilization and motion stability.

[0077] 3、The present application realizes the combination of virtual and real robot motion by establishing a three-level digital twin model, simulates the actual motion state of the robot, and predicts the future motion trajectory. This not only enhances the predictability of the robot's future behavior, but also improves the response speed to motion deviation in complex dynamic environments. Through virtual mapping, motion state mapping and dynamics prediction, the system can optimize and adjust the path in real time, improve the autonomy and adaptability of the robot, and reduce the need for external intervention. BRIEF DESCRIPTION OF DRAWINGS

[0078] In order to facilitate the understanding of those skilled in the art, the present application will be further described below in conjunction with the drawings:

[0079] Figure 1 The method flowchart of the present application;

[0080] Figure 2 The system block diagram of the present application. DETAILED DESCRIPTION

[0081] The technical solutions of the present application will be described below in conjunction with the embodiments. Obviously, the described embodiments are only a part of the embodiments of the present application, not all. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0082] Please refer to Figure 1 The robot motion trajectory correction control method based on digital twin, as shown in the figure, comprises the following steps:

[0083] Step one, real-time motion state acquisition and synchronous transmission;

[0084] Access the IMU inertial measurement unit, positioning device, encoder, accelerometer and gyroscope in the robot, and collect real-time motion state data of the robot including angular velocity, linear velocity, position coordinates.

[0085] Including: real-time position coordinates (x0, y0, z0), real-time linear velocity (vx, vy, vz), real-time angular velocity (ωx, ωy, ωz) and real-time acceleration (ax, ay, az).

[0086] Wherein, x0, y0 and z0 are x, y and z components in real-time position coordinates respectively;

[0087] Wherein, vx, vy and vz are linear velocities of the robot along x, y and z axis directions respectively;

[0088] Wherein, ωx, ωy and ωz are angular velocities of the robot around x, y and z axis directions respectively;

[0089] Wherein, ax, ay and az are accelerations of the robot along x, y and z axis directions respectively.

[0090] Step two, preset trajectory mapping and discretization processing;

[0091] Retrieve the preset trajectory of the robot, and discretize the continuous preset trajectory of the robot into a preset number of discrete points, and then divide the trajectory into a preset number of trajectory units.

[0092] Get the preset motion trajectory of the robot in the space coordinate system, and get its preset motion trajectory equation L=f(x, y, z).

[0093] Further, the preset motion trajectory L of the robot is discretized, and the continuous preset motion trajectory is equally divided to obtain K discrete points on the preset motion trajectory L. The number K of discrete points is a preset value, and the discrete points obtained after equal division are sequentially numbered, and the numbering symbol is k; k=1, 2, 3,..., K.

[0094] Further, taking the discrete point as the division node, the continuous preset motion trajectory L is divided into K-1 trajectory units, and the end discrete point of each trajectory unit is regarded as the expected position of the trajectory unit, that is, the target position of the robot on the trajectory unit.

[0095] Further, locate the discrete point coordinates (x_k, y_k, z_k)=(x_1, y_1, z_1), (x_2, y_2, z_2),..., (x_K, y_K, z_K) of each trajectory unit.

[0096] Step 3: Real-time driving of the digital twin model and mapping of virtual and real trajectories;

[0097] A three-layer digital twin model is established to reflect the robot's actual motion state in the physical world and predict its future motion state in real time through virtual copies of the robot in the three-layer digital twin model. The three-layer digital twin model includes a trajectory planning model, a motion mapping model, and a dynamics prediction model, which respectively complete the virtual mapping of the robot's preset motion trajectory, the virtual mapping of the actual motion trajectory and motion state, and the virtual mapping of the influence of driving force and driving torque on the future trajectory.

[0098] The robot's trajectory planning model is as follows:

[0099] Establish an independent first three-dimensional space model, and locate the starting coordinates (x_origin, y_origin, z_origin), ending coordinates (x_end, y_end, z_end), and preset motion trajectory equation L = f(x, y, z) of the robot's motion trajectory in the first three-dimensional space model.

[0100] The discrete point coordinates (x_k, y_k, z_k) = (x_1, y_1, z_1), (x_2, y_2, z_2), ..., (x_K, y_K, z_K) are located and marked in the first three-dimensional space model according to their corresponding coordinates.

[0101] The robot's motion mapping model is as follows:

[0102] Establish an independent second three-dimensional spatial model, and locate the robot's real-time position coordinates (x0, y0, z0) at each time t in the second three-dimensional spatial model, denoted as the position vector at time t p(t) = [x(t), y(t), z(t)] T The corresponding real-time position coordinates (x0, y0, z0) are converted into the historical position coordinates [x(t), y(t), z(t)] at time t, and then used as the robot's historical positioning coordinates to be input into the second three-dimensional space model for positioning and marking.

[0103] The robot's linear velocity vector v(t) = [v x (t), v y (t), v z (t)] T Angular velocity vector ω(t)=[ω x (t), ω y (t), ω z (t)] T and acceleration vector a(t)=[a x (t), a y(t), a z (t)] T According to the historical position coordinates [x(t), y(t), z(t)] of the robot, the corresponding positions are input into the second three-dimensional space model.

[0104] Position vector p(t) = [x(t), y(t), z(t)] T wherein x(t), y(t) and z(t) are respectively the position coordinate components of the robot along the x, y and z axes at the time t;

[0105] Velocity vector v(t) = [v x (t), v y (t), v z (t)] T wherein v x (t), v y (t) and v z (t) are respectively the velocity components of the robot along the x, y and z axes at the time t;

[0106] Angular velocity vector ω(t) = [ω x (t), ω y (t), ω z (t)] T wherein ω x (t), ω x (t) and ω x (t) are respectively the angular velocity components of the robot along the x, y and z axes at the time t;

[0107] Acceleration vector a(t) = [a x (t), a y (t), a z (t)] T wherein a x (t), a x (t) and a x (t) are respectively the acceleration components of the robot along the x, y and z axes at the time t.

[0108] The dynamic prediction model of the robot comprises a third three-dimensional space model, a position deviation operation program, a trajectory prediction program and a dynamic operation program of the robot, and specifically comprises:

[0109] A separate third three-dimensional space model is established to construct an adjustment state parameter matrix of the robot at each time t The elements in the matrix are all unknowns to be solved, and the specific values thereof are obtained according to the simultaneous solution of the position deviation operation program, the trajectory prediction program and the dynamic operation program.

[0110] wherein, and are respectively the x, y, z components of the position adjustment value of the robot, representing the specific value of the position correction distance along the x, y and z axis directions that the robot needs to complete if it returns to the preset motion trajectory from the current position. The position adjustment value component is positive when it represents the correction along the positive direction of the x, y or z axis, and is negative when it represents the correction along the negative direction of the x, y or z axis;

[0111] wherein, and are respectively the x, y, z components of the linear velocity adjustment value of the robot, representing the specific value of the linear velocity correction along the x, y and z axis directions that the robot needs to complete if it returns to the preset motion trajectory from the current position. The linear velocity correction value component is positive when it represents the correction along the positive direction of the x, y or z axis, and is negative when it represents the correction along the negative direction of the x, y or z axis;

[0112] wherein, and are respectively the x, y, z components of the angular velocity adjustment value of the robot, representing the specific value of the angular velocity correction around the x, y and z axis directions that the robot needs to complete if it returns to the preset motion trajectory from the current position. The angular velocity correction value component is positive when it represents the correction in the clockwise direction around the x, y or z axis, and is negative when it represents the correction in the counterclockwise direction around the x, y or z axis;

[0113] wherein, and are respectively the x, y, z components of the acceleration adjustment value of the robot, representing the specific value of the acceleration correction along the x, y and z axis directions that the robot needs to complete if it returns to the preset motion trajectory from the current position. The acceleration correction value component is positive when it represents the correction along the positive direction of the x, y or z axis, and is negative when it represents the correction along the negative direction of the x, y or z axis;

[0114] The position deviation operation program, trajectory prediction program and dynamics operation program are specifically:

[0115] The position deviation operation program is specifically:

[0116] Obtain the real-time position coordinates (x0, y0, z0) of the robot and the preset motion trajectory equation L = f(x, y, z), and parameterize the preset motion trajectory equation L = f(x, y, z) as L = f[f1(u), f2(u), f1(u)], wherein wherein u is the projection point parameter, and f1(u), f2(u), f3(u) are the component functions after parameterization, respectively representing the variable relationship between x, y and z in the preset motion trajectory equation L = f(x, y, z) and the projection point parameter u. After parameterization, the motion trajectory is converted from a curve defined by three variables x, y and z to a curve defined by a single variable u.

[0117] By preset formula Solve the projection point parameter u, and substitute u back into L = f[x(u), y(u), z(u)], to obtain the projection point (x^, y^, z^) of the real-time position coordinate (x0, y0, z0) on the preset motion trajectory equation L = f(x, y, z).

[0118] Further, by preset formula Calculate the x, y, z components of the position adjustment value of the robot And

[0119] It should be noted that the position deviation operation program is based on the shortest distance principle, that is, the shortest distance principle of a point to a curve. The basic concept of the shortest distance is: given a point and a curve, the distance between the point and its projection point on the curve is the shortest distance from the point to the curve. The mathematical source of the position deviation operation formula is: define a distance function D(t) representing the distance between a point and any point on a curve. Then, take the derivative of the distance function to obtain the preset formula Find the minimum point. The point where the derivative is zero corresponds to the minimum or maximum value of the function, and then find the shortest distance to solve the parameter u.

[0120] The trajectory prediction program of the robot is specifically: Wherein And are the coordinate values of the robot along the x, y, and z coordinate axes at time t after prediction;

[0121] The dynamics operation program of the robot is specifically:

[0122]

[0123] Wherein, And F x，y，z (t) = m x a x，y，z (t) is a linear dynamics equation; wherein F x，y，z (t) is the linear driving force output by the robot motor, including the linear driving forces F x (t), F y (t), and F z (t) generated by the robot along the x, y, and z axis coordinate directions at time t; wherein m is the preset standard mass of the robot, and a x，y，z (t) includes the acceleration components a x (t), a y (t), and a z (t) of the robot along the x, y, and z axis coordinate directions at time t;

[0124] Wherein, is the rotational dynamics equation; where τ x，y，z (t) is the rotational driving torque output by the robot motor, including the rotational driving torque τ x (t), τ y (t) and τ z (t) generated by the robot around the x, y and z axis coordinate directions at time t; where I x，y，z is the preset moment of inertia of the robot around the x, y and z axis directions; where ω x，y，z (t) is the angular velocity component of the robot around the x, y, z axis directions at time t.

[0125] The first three-dimensional space model, the second three-dimensional space model and the third three-dimensional space model have the same coordinate system, including the same coordinate origin and the coordinate directions of the x, y and z axes.

[0126] Step four, error correction strategy and control signal generation;

[0127] The error correction strategy is determined by the distance between the real-time position coordinates of the robot and the preset trajectory, and is matched to the first, second or third error correction signal to achieve that different trajectory deviation values can be matched to the best adjustment measures. Different reference points are positioned according to the first, second or third error correction signal as the target focus of trajectory correction, and the x, y, z components of the position adjustment value are solved. And input them into the trajectory prediction program and the dynamics operation program. Finally, the linear driving force and the rotational driving torque are generated according to the trajectory prediction program and the dynamics operation program, and the accurate trajectory correction of the robot is completed.

[0128] The real-time position coordinates (x0, y0, z0), real-time linear velocity (vx, vy, vz) and real-time angular velocity (ωx, ωy, ωz) of the robot are called.

[0129] The real-time position coordinates (x0, y0, z0) of the robot are input into the position deviation operation program to obtain the x, y, z components of the position adjustment value of the robot and

[0130] The deviation distance E(t) of the robot at time t is calculated by the preset formula

[0131] Further, according to the deviation distance E(t), the error correction strategy is matched, if the deviation distance E(t) is less than the first preset threshold E1, the first error correction signal is matched;

[0132] If the deviation distance E(t) is greater than or equal to the first preset threshold E1 and less than the second preset threshold E2, the second error correction signal is matched;

[0133] ​If the deviation distance E(t) is greater than or equal to the second preset threshold E2, the third-level error correction signal is matched.

[0134] It should be noted that the design of the third-level signal reflects the adjustment strategy adopted by the robot under different error conditions. The first-level error correction signal indicates that the error of the current position is small, and the deviation of the robot from the preset trajectory is not large. At this time, we think that the motion correction of the robot does not need to be adjusted immediately, and the system can choose to ignore the current deviation, allowing the robot to continue moving in the forward direction to avoid resource waste and motion instability caused by excessive correction; the second-level error correction signal indicates that the error has reached a moderate level. At this time, the robot needs to be corrected, but it does not need to be adjusted too drastically immediately. The system will choose to move towards the discrete point with a medium distance, gradually adjust the motion direction, and perform smooth trajectory correction; the third-level error correction signal indicates that the robot has deviated from the preset trajectory seriously, and the deviation is large. At this time, the robot must immediately take corrective measures and as quickly as possible adjust the position to the nearest deviation point.

[0135] When the first-level error correction signal of the robot is identified, the discrete point (x_k, y_k, z_k) with the smallest distance from the real-time position coordinates (x0, y0, z0) of the robot is obtained, and the N1th discrete point (x_k+N1, y_k+N1, z_k+N1) before the discrete point (x_k, y_k, z_k) is located. The discrete point (x_k+N1, y_k+N1, z_k+N1) is input as the projection point (x^, y^, z^) into the trajectory prediction program of the robot and the dynamics calculation program of the robot;

[0136] When the second-level error correction signal of the robot is identified, the discrete point (x_k, y_k, z_k) with the smallest distance from the real-time position coordinates (x0, y0, z0) of the robot is obtained, and the N2th discrete point (x_k+N1, y_k+N1, z_k+N1) before the discrete point (x_k, y_k, z_k) is located. The discrete point (x_k+N1, y_k+N1, z_k+N1) is input as the projection point (x^, y^, z^) into the trajectory prediction program of the robot and the dynamics calculation program of the robot;

[0137] When the third-level error correction signal of the robot is identified, the discrete point with the smallest distance from the real-time position coordinates (x0, y0, z0) of the robot is immediately located, and the located discrete point coordinates are input as the projection point (x^, y^, z^) into the trajectory prediction program of the robot and the dynamics calculation program of the robot;

[0138] Wherein, N1 and N2 are preset calibration parameters, N1 is greater than N2, and N1 and N2 are both positive integers.

[0139] After receiving the projection point (x^, y^, z^), the robot's trajectory prediction program calculates the x, y, and z components of the position adjustment value. and And send it to the robot's dynamics calculation program. The robot's dynamics calculation program, through... and Solving for the linear driving force F x (t), F y (t) and F z (t), and solve for the rotational driving torque τ. x (t), τ y (t) and τ z (t).

[0140] linear driving force F x (t), F y (t) and F z (t) and the solution of the rotational driving torque τ x (t), τ y (t) and τ z (t) is sent to the robot as the motor output parameters for trajectory correction to complete the trajectory correction task.

[0141] Every preset time interval, the robot's deviation distance E(t) is recalculated, and the level of the error correction signal is dynamically updated to adjust the corresponding trajectory correction strategy.

[0142] Please see Figure 2 As shown, the robot motion trajectory correction control device based on digital twin includes a data acquisition module, a trajectory planning module, a central computing module, and a signal matching module.

[0143] The data acquisition module is responsible for collecting the robot's motion state data in real time, including position coordinates, linear velocity, angular velocity, and acceleration. This motion state data comes from sensors inside the robot, including an IMU (Inertial Measurement Unit), a positioning device, and an encoder, ensuring that the robot's real-time motion state is accurately monitored and synchronously transmitted.

[0144] The trajectory planning module is responsible for generating the robot's preset motion trajectory and discretizing it. The discretized trajectory is divided into multiple trajectory units, each corresponding to a desired position, so that the robot can perform precise path control based on these discrete points.

[0145] The central computing module stores the trajectory planning model, the motion mapping model and the dynamics prediction model of the robot and the first, second and third three-dimensional space models contained therein. It is responsible for processing information from the data acquisition module and the trajectory planning module, and performing the calculation tasks contained in the programs in the models, including position deviation calculation, trajectory prediction and dynamics calculation. It receives real-time data such as the position and speed of the robot, and calculates the deviation distance E(t).

[0146] The signal matching module matches the error signal to the preset correction strategy signal. By parameterizing the trajectory, solving the shortest distance principle, and combining the prediction model, an error correction signal and a control instruction are generated. According to the level of the error correction signal, the adjustment intensity of the robot and the discrete points as the correction target are determined, and the correction target is generated. The linear driving force and the rotary driving torque are solved. The linear driving force and the rotary driving torque are sent to the robot as the motor output parameters of the trajectory correction to complete the trajectory correction task.

[0147] It should be understood that the terms "comprise" and "comprising" used in the specification and claims of the present disclosure indicate the presence of the described features, integers, steps, operations, elements, and / or components, but do not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components, and / or groups thereof.

[0148] It should also be understood that the terms used in the specification of the present disclosure are only for the purpose of describing specific embodiments and are not intended to limit the present disclosure. As used in the specification and claims of the present disclosure, the singular forms "a", "an" and "the" are intended to include the plural forms unless the context clearly indicates otherwise. It should be further understood that the term "and / or" used in the specification and claims of the present disclosure means any combination of one or more of the associated listed items and all possible combinations thereof;

[0149] The preferred embodiments of the application disclosed above are only used to help explain the application. The preferred embodiments do not describe all the details and do not limit the application to specific embodiments. Obviously, many modifications and changes can be made according to the content of the present specification. The present specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the application, so that those skilled in the art can well understand and utilize the application. The application is limited only by the claims and their entire scope and equivalents.

Claims

1. A robot motion trajectory correction control method based on digital twinning, characterized by, The method comprises the following steps: Step one, real-time motion state acquisition and synchronous transmission; Access the IMU inertial measurement unit, positioning device, encoder, accelerometer and gyroscope in the robot to acquire real-time motion state data of the robot including angular velocity, linear velocity and position coordinates; Step two, preset trajectory mapping and discretization processing; Retrieve the preset trajectory of the robot and discretize the continuous preset trajectory of the robot into a preset number of discrete points, and then divide the trajectory into a plurality of trajectory units in a preset number of segments; Step three, real-time driving of digital twin model and virtual-real trajectory mapping; A three-layer digital twin model is established, which reflects the real motion state and future motion state result prediction of the robot in the physical world through the virtual copy of the robot in the three-layer digital twin model; the three-layer digital twin model comprises a trajectory planning model, a motion mapping model and a dynamics prediction model of the robot, which respectively complete virtual mapping of the preset motion trajectory of the robot, virtual mapping of the real motion trajectory and motion state, and virtual mapping of driving force, driving torque and future trajectory influence; the first three-dimensional space model, the second three-dimensional space model and the third three-dimensional space model have the same coordinate system, including the same coordinate origin and the coordinate directions of x, y and z axes; Step four, error correction strategy and control signal generation; Determine the error correction strategy according to the distance between the real-time position coordinates of the robot and the preset trajectory, match to the first, second or third error correction signal to realize that different trajectory deviation values can match to the best adjustment measure; locate different reference points as the target focus of trajectory correction according to the first, second or third error correction signal, solve the x, y and z components of the position adjustment value, and input them into the trajectory prediction program and the dynamics calculation program; finally, generate linear driving force and rotary driving torque according to the trajectory prediction program and the dynamics calculation program to complete accurate trajectory correction of the robot. 2.The robot motion trajectory correction control method based on digital twinning according to claim 1, wherein, The acquired real-time motion state data of the robot includes: Real-time position coordinates (x0, y0, z0), real-time linear velocity (vx, vy, vz), real-time angular velocity (ωx, ωy, ωz) and real-time acceleration (ax, ay, az); Wherein, x0, y0 and z0 are the x, y and z components of the real-time position coordinates respectively; vx, vy and vz are the linear velocities of the robot along the x, y and z axis directions respectively; ωx, ωy and ωz are the angular velocities of the robot around the x, y and z axis directions respectively; ax, ay and az are the accelerations of the robot along the x, y and z axis directions respectively. 3.The robot motion trajectory correction control method based on digital twinning according to claim 1, wherein, The specific process of discretizing the continuous preset trajectory of the robot into a preset number of discrete points is as follows: Obtain the preset motion trajectory of the robot in the space coordinate system and obtain its preset motion trajectory equation L=f(x, y, z); Discretize the preset motion trajectory L of the robot, equally divide the continuous preset motion trajectory, obtain K discrete points on the preset motion trajectory L, the number K of discrete points is a preset value, sequentially number the discrete points obtained after equal division, and the numbering symbol is k; k=1, 2, 3,..., K; The continuous preset motion trajectory L is divided into K-1 trajectory units with the discrete points as division nodes, and the end point discrete point of each trajectory unit is regarded as the expected position of the trajectory unit, i.e., the target position finally reached by the robot on the trajectory unit; The coordinates of the discrete points of each trajectory unit are located as (x_k, y_k, z_k)=(x_1, y_1, z_1), (x_2, y_2, z_2),..., (x_K, y_K, z_K). 4.The robot motion trajectory correction control method based on digital twinning according to claim 1, wherein, The trajectory planning model of the robot is specifically: An independent first three-dimensional space model is established, and the start point coordinates (x_origin, y_origin, z_origin), the end point coordinates (x_end, y_end, z_end) of the motion trajectory of the robot and the preset motion trajectory equation L=f(x, y, z) are located in the first three-dimensional space model; The coordinates of the discrete points (x_k, y_k, z_k)=(x_1, y_1, z_1), (x_2, y_2, z_2),..., (x_K, y_K, z_K) are located and marked in the first three-dimensional space model according to the corresponding coordinates. 5.The robot motion trajectory correction control method based on digital twinning according to claim 1, wherein, The motion mapping model of the robot is specifically: establishing a second three-dimensional space model, and positioning the real-time position coordinates (x0, y0, z0) of the robot at each time t in the second three-dimensional space model, denoted as the position vector p(t) = [x(t), y(t), z(t)] at time t T , and converting the corresponding real-time position coordinates (x0, y0, z0) into the corresponding historical position coordinates [x(t), y(t), z(t)] at time t, and inputting the historical positioning coordinates of the robot into the second three-dimensional space model for positioning and marking; The linear velocity vector v(t) = [v x (t), v y (t), v z (t)] T , the angular velocity vector ω(t) = [ω x (t), ω y (t), ω z (t)] T , and the acceleration vector a(t) = [a x (t), a y (t), a z (t)] T are input to the corresponding positions in the second three-dimensional space model according to the historical position coordinates [x(t), y(t), z(t)] of the robot; Position vector p(t) = [x(t), y(t), z(t)] T where x(t), y(t), and z(t) are position coordinate components of the robot in the x, y, z axis directions, respectively, acquired at time t; Velocity vector v(t) = [v x (t), v y (t), v z (t)] T where v x (t), v y (t), and v z (t) are the velocity components of the robot along the x, y, z axes, respectively, at time t Angular velocity vector ω(t) = [ω x (t), ω y (t), ω z (t)] T where ω x (t), ω x (t), and ω x (t) are angular velocity components of the robot along x, y, z axes, respectively, at time t; Acceleration vector a(t) = [a x (t), a y (t), a z (t)] T where a x (t), a x (t), and a x (t) are the acceleration components of the robot along the x, y, z axes, respectively, at time t.

6. The digital-twin-based robot motion trajectory correction control method according to claim 1, characterized in that: The dynamics prediction model of the robot includes a third three-dimensional space model, a position deviation calculation program, a trajectory prediction program and a dynamics calculation program of the robot, which are specifically: A third three-dimensional space model is established to build the adjustment state parameter matrix of the robot at each time t The elements are unknowns to be solved, and their specific values are obtained by simultaneous solving of the position deviation operation program, the trajectory prediction program, and the dynamics operation program. wherein, and are respectively the x, y, z components of the position adjustment value of the robot, representing the specific numerical value of the position correction distance along the x, y and z axis directions that the robot needs to complete if it returns to the preset motion trajectory from the current position; the position adjustment value component is positive when it represents the correction along the positive direction of the x, y or z axis, and is negative when it represents the correction along the negative direction of the x, y or z axis; wherein, and are respectively the x, y, z components of the linear velocity adjustment value of the robot, representing the specific value of the linear velocity correction that the robot needs to complete along the x, y, and z axis directions if it returns to the preset motion trajectory from the current position; the linear velocity correction value component is positive when it represents the correction along the positive direction of the x, y, or z axis, and is negative when it represents the correction along the negative direction of the x, y, or z axis. wherein, and are respectively the x, y, z components of the angular velocity adjustment value of the robot, representing the specific value of the angular velocity correction that the robot needs to complete around the x, y and z axis directions if it returns to the preset motion trajectory from the current position; the angular velocity correction value component is positive when it represents the clockwise direction correction around the x, y or z axis, and is negative when it represents the counterclockwise direction correction around the x, y or z axis; wherein, and are respectively the x, y, z components of the acceleration adjustment value of the robot, representing the specific value of the acceleration correction that the robot needs to complete along the x, y and z axis directions if it returns to the preset motion trajectory from the current position; the acceleration correction value component is positive when it represents the correction along the positive direction of the x, y or z axis, and is negative when it represents the correction along the negative direction of the x, y or z axis. 7.The robot motion trajectory correction control method based on digital twinning according to claim 6, wherein, The position deviation calculation program and the trajectory prediction program are specifically: The position deviation calculation program is specifically: Obtain real-time position coordinates (x0, y0, z0) of the robot and a preset motion trajectory equation L = f(x, y, z), and parameterize the preset motion trajectory equation L = f(x, y, z) as L = f[f1(u), f2(u), f1(u)], wherein Wherein u is a projection point parameter, wherein f1(u), f2(u), f3(u) are component functions after parameterization, respectively representing the variable relationship between x, y and z in the preset motion trajectory equation L = f(x, y, z) and the projection point parameter u; after parameterization, the motion trajectory is converted from a curve defined by three variables x, y and z to a curve defined by a single variable u; By a preset formula Solve the projection point parameter u, and substitute u back into L=f[x(u), y(u), z(u)] to obtain the projection point (x, y, z) of the real-time position coordinate (x0, y0, z0) on the preset motion trajectory equation L=f(x, y, z). by a preset formula x, y, z components of the position adjustment value of the robot and The trajectory prediction program of the robot is specifically: wherein and are the coordinate values of the robot along the x, y and z coordinate axes in the future after time t obtained through prediction.

8. The digital-twin-based robot motion trajectory correction control method according to claim 6, characterized in that: The dynamics computation program of the robot is specifically: wherein, and F x，y，z (t) = m x a x，y，z (t) is a linear dynamics equation; wherein F x，y，z (t) is a linear driving force output by a motor of the robot, including linear driving forces F x (t), F y (t), and F z (t) generated by the robot along x, y, and z coordinate directions at time t; wherein m is a preset standard mass of the robot, and a x，y，z (t) includes acceleration components a x (t), a y (t), and a z (t) of the robot along x, y, and z coordinate directions at time t. wherein, is the rotational dynamics equation; wherein τ x，y，z (t) is the rotational drive torque output by the robot motor, including the rotational drive torques τ x (t), τ y (t) and τ z (t) generated by the robot around the x, y and z axis coordinate directions at time t; wherein I x，y，z is the rotational inertia of the robot around the x, y and z axis directions; wherein ω x，y，z (t) is the angular velocity component of the robot around the x, y, z axis directions at time t. 9.The robot motion trajectory correction control method based on digital twinning of claim 1, wherein, The specific process of the error correction strategy and the control signal generation is: The real-time position coordinates (x0, y0, z0), the real-time linear velocity (vx, vy, vz) and the real-time angular velocity (ωx, ωy, ωz) of the robot are called; The real-time position coordinates (x0, y0, z0) of the robot are input into the position deviation calculation program to obtain the x, y, z components of the position adjustment value of the robot and by a preset formula a deviation distance E(t) of the computer robot at time t; According to the deviation distance E(t), the error correction strategy is matched, if the deviation distance E(t) is less than the first preset threshold E1, the first-level error correction signal is matched; If the deviation distance E(t) is greater than or equal to the first preset threshold E1 and less than the second preset threshold E2, the second-level error correction signal is matched; If the deviation distance E(t) is greater than or equal to the second preset threshold E2, the third-level error correction signal is matched; When the first-level error correction signal of the robot is identified, the discrete point (x_k, y_k, z_k) with the smallest distance from the real-time position coordinates (x0, y0, z0) of the robot is obtained, and the N1th discrete point (x_k+N1, y_k+N1, z_k+N1) before the discrete point (x_k, y_k, z_k) is located; the discrete point (x_k+N1, y_k+N1, z_k+N1) is input as the projection point (x^, y^, z^) into the trajectory prediction program of the robot and the dynamics calculation program of the robot; When the secondary error correction signal of the robot is identified, the discrete point (x_k, y_k, z_k) with the smallest distance from the real-time position coordinates (x0, y0, z0) of the robot is obtained, and the N2th discrete point (x_k+N1, y_k+N1, z_k+N1) before the discrete point (x_k, y_k, z_k) is located; the discrete point (x_k+N1, y_k+N1, z_k+N1) is input as the projection point (x^, y^, z^) into the trajectory prediction program of the robot and the dynamics calculation program of the robot; When the tertiary error correction signal of the robot is identified, the discrete point with the smallest distance from the real-time position coordinates (x0, y0, z0) of the robot is immediately located, and the located discrete point coordinates are input as the projection point (x^, y^, z^) into the trajectory prediction program of the robot and the dynamics calculation program of the robot; Wherein, N1 and N2 are preset calibration parameters, N1 is greater than N2 and N1 and N2 are both positive integers; After receiving the projection point (x^, y^, z^), the robot's trajectory prediction program calculates the x, y, and z components of the position adjustment value. and And send it to the robot's dynamics calculation program; the robot's dynamics calculation program, through... and Solving for the linear driving force F x (t), F y (t) and F z (t), and solve for the rotational driving torque τ. x (t), τ y (t) and τ z (t); The linear driving force F x (t), F y (t) and F z (t) and the de-rotation driving torque τ x (t), τ y (t) and τ z (t) are sent to the robot as the motor output parameters of the trajectory correction to complete the trajectory correction task.

10. A robot motion trajectory correction control device based on digital twinning for implementing the robot motion trajectory correction control method based on digital twinning according to any one of claims 1 to 9, characterized in that, It comprises a data acquisition module, a trajectory planning module, a central calculation module and a signal matching module. The data acquisition module is responsible for real-time acquisition of the motion state data of the robot, including position coordinates, linear velocity, angular velocity and acceleration; the motion state data comes from the sensors inside the robot, including an IMU inertial measurement unit, a positioning device and an encoder, ensuring that the real-time motion state of the robot is accurately monitored and synchronously transmitted; The trajectory planning module is responsible for generating the preset motion trajectory of the robot and performing discretization processing thereon; The discretized trajectory will be divided into multiple trajectory units, each trajectory unit corresponding to an expected position, so that the robot can accurately control the path according to these discrete points; The central calculation module stores the trajectory planning model, motion mapping model and dynamics prediction model of the robot and the first, second and third three-dimensional space models contained therein; it is responsible for processing information from the data acquisition module and the trajectory planning module, and performing calculation tasks contained in the programs in the model, including position deviation calculation, trajectory prediction and dynamics calculation; it receives the position, velocity and other data of the robot in real time, and calculates the deviation distance; The signal matching module matches the preset correction strategy signal; by parameterizing the trajectory, solving the shortest distance principle, and combining the prediction model, the error correction signal and the control instruction are generated; according to the level of the error correction signal, the adjustment intensity of the robot and the discrete point as the correction target are determined, and the correction target is generated; the linear driving force and the rotary driving torque are solved; the linear driving force and the rotary driving torque are sent to the robot as the motor output parameters of the trajectory correction to complete the trajectory correction task; Every preset time interval, the deviation distance of the robot is recalculated, and the level of the error correction signal is dynamically updated, so as to adjust the corresponding trajectory correction strategy.

Images