Robot dynamics identification and excitation trajectory planning method and system under geometric and physical mixed constraints

Abstract

The present invention belongs to the field of robot dynamics parameter identification, and discloses a robot dynamics identification excitation trajectory planning method under geometric and physical mixed constraints, the method comprising: (1) describing in the robot DH coordinate system according to the structure of the robot arm and selecting characteristic points and characteristic radius on the robot arm; (2) extending obstacles in the robot arm workspace outward and simplifying them into a cuboid, and describing them in the world coordinate system of the robot arm; (3) calculating the characteristic circle at the characteristic point and the simplified obstacle interference distance according to the posture of the robot arm; (4) selecting a safe distance to design an interference function, and obtaining the interference function value corresponding to the interference distance; (5) calculating the interference function values ​​under different postures, and taking the maximum value or average value as the interference function of the trajectory; (6) calculating multiple non-interference trajectories under physical constraints, and obtaining a non-interference trajectory set; (7) taking the sum of the excitation trajectory condition number and the trajectory interference function as the objective function, and optimizing and calculating the non-interference excitation trajectory under physical constraints. Through the present invention, the excitation trajectory planning under the geometric and physical constraints of the robot arm is realized.

Description

Technical Field

[0001] The present invention belongs to but is not limited to the field of robot dynamics parameter identification, and in particular relates to a robot dynamics identification excitation trajectory planning method and system under geometric and physical mixed constraints. Background Art

[0002] Excitation trajectory planning is part of the dynamic parameter identification process. Its main function is to stimulate the dynamic characteristics of the parameters to improve the identification accuracy of the dynamic parameters. In order to fully stimulate the dynamic characteristics of the parameters, the robot arm often needs to perform excitation motion in a large range of space. When there are obstacles in the robot's workspace, running the excitation trajectory can easily cause interference and collision between the robot arm and the obstacle. At the same time, the excitation trajectory itself also needs to meet certain physical constraints, such as head and tail smoothness constraints and position, velocity and acceleration limit constraints. How to simultaneously meet physical constraints and geometric constraints of obstacles when planning the excitation trajectory is of great significance to the identification of dynamic parameters.

[0003] In the past, the limiting effect of obstacles is usually not directly considered in the planning of incentive trajectories. Instead, only physical limit constraints and physical boundary constraints are considered to complete the optimization of incentive trajectories in the absence of obstacles. Then, obstacles are considered to screen the non-interference trajectory through trajectory simulation through simulation software, and usually only the ground is considered as an obstacle during simulation. For manipulators with multiple obstacles or large obstacles in the workspace, the above methods often cannot directly obtain the non-interference incentive trajectory. Therefore, modeling is carried out for obstacles in the workspace, and geometric constraints are imposed on the trajectory considering the interference between the manipulator and the obstacles under the incentive trajectory. The incentive trajectory with geometric and physical mixed constraints is obtained, which is of great significance for the dynamic parameter identification of manipulators with obstacles in the actual working environment.

[0004] The technical problems existing in the existing technology in terms of incentive trajectory planning mainly include the following points:

[0005] 1) Lack of methods to directly consider obstacle constraints: In previous incentive trajectory planning, planning is usually performed without obstacles, and then the non-interference trajectory is screened through simulation software. This method does not directly consider the existence of obstacles in the planning process, resulting in low efficiency and failure to obtain a completely non-interference trajectory.

[0006] 2) Inadequate handling of multiple obstacles and large obstacles: When there are multiple obstacles or large obstacles in the workspace, existing planning methods often find it difficult to directly obtain non-interference incentive trajectories. This increases the difficulty and risk in practical applications.

[0007] 3) Lack of processing methods for geometric and physical mixed constraints: Existing trajectory planning methods usually only consider physical limit constraints and physical boundary constraints, such as head and tail smoothness constraints, position, velocity and acceleration limits, etc., but do not incorporate the geometric constraints of obstacles into the planning process. This results in the need to frequently adjust the trajectory to meet all constraints in actual operations, reducing work efficiency.

[0008] Technical problems caused by industrial applications.

[0009] 1. Application of robotic arms in automated manufacturing

[0010] Problem description: In automated manufacturing, robotic arms need to perform multiple tasks in complex working environments, such as welding, assembly, and handling. Existing trajectory planning methods make it difficult to directly consider obstacles during the planning process, resulting in low planning efficiency and failure to ensure the safety of the robotic arm in actual operation.

[0011] Influence:

[0012] Reduced production efficiency: The trajectory needs to be adjusted repeatedly to avoid collisions, which increases the production cycle.

[0013] Increased safety risks: Failure to fully consider obstacles can easily lead to collisions between the robot arm and equipment or workpieces, causing equipment damage and production accidents.

[0014] Increased maintenance costs: Frequent trajectory adjustments and collision risks increase the cost of equipment maintenance and replacement.

[0015] 2. Application of surgical robots in the medical field

[0016] Problem description: In minimally invasive surgery, surgical robots need to operate precisely inside the patient's body, avoiding important organs and blood vessels. Existing trajectory planning methods cannot directly consider the complex geometric constraints in the body during the planning process, resulting in low efficiency of surgical robot operations.

[0017] Influence:

[0018] Prolonged surgery time: The robot’s trajectory needs to be frequently adjusted to avoid obstacles, which prolongs the surgery time and increases the risk to the patient.

[0019] Reduced surgical accuracy: The trajectory adjustment process affects surgical accuracy and increases the risk of surgical failure.

[0020] Increased operational difficulty: Doctors need to constantly monitor and adjust the robot's operating trajectory, which increases the complexity and difficulty of the operation. Summary of the invention

[0021] In order to solve the problem that the excitation trajectory of a robot arm running in the presence of obstacles is prone to interference, the present invention provides a method that takes obstacles into consideration in the excitation trajectory planning. The interference measure of the excitation trajectory is calculated by simplifying the geometric shapes of the obstacles and the robot arm in different postures. The interference measure and the condition number of the observation matrix are considered and optimized in combination with physical constraints to obtain the excitation trajectory with geometric and physical mixed constraints.

[0022] The present invention is implemented as follows: a robot dynamics identification excitation trajectory planning method under geometric and physical mixed constraints, the method comprising:

[0023] (1) According to the structure of the robot arm and the feature point p selected on the robot arm m and characteristic radius r m , which is expressed in the robot DH coordinate system as n p m , combined with the DH coordinate system transformation, the coordinates of the feature points in the world coordinate system can be obtained 0 p m :

[0024]

[0025] Where: t represents time, n p m Represents the description of the mth feature point of the robot arm in the coordinate system {n}, represents the homogeneous transformation matrix from coordinate system {0} to coordinate system {n}, 0 p m Represents the description of the mth feature point of the robot arm in the coordinate system {0};

[0026] The feature point is fixed on the joint n, that is, n p m is a fixed value, selected by the robot arm structure;

[0027] (2) The obstacles in the robot's workspace are extended outward to be simplified into a cuboid. After extension, the edges of the cuboid are parallel to the {0} axis of the coordinate system. The information of the simplified j-th cuboid is recorded as O j , described in the world coordinate system of the robot, we have:

[0028]

[0029] in: Indicates obstacle O j The maximum and minimum values ​​of the x-axis projection of the coordinate system {0}, Indicates obstacle O j The maximum and minimum values ​​of the y-axis projection of the coordinate system {0}, Indicates obstacle O jThe maximum and minimum values ​​of the z-axis projection of the coordinate system {0};

[0030] (3) Calculate the distance between the characteristic circle at the characteristic point and the simplified obstacle according to the robot arm posture. The coordinates of the characteristic point are:

[0031]

[0032] in: 0 x m , 0 y m , 0 z m is the characteristic point p of the robot arm at time t m Coordinates in the {0} coordinate system.

[0033] Feature point p m Distance to simplified obstacle O j The distance L mj The calculation is done as follows. First, the distance between the point and the obstacle projection is calculated:

[0034]

[0035] Where: Lx mj Represents the feature point p m With obstacles O j The distance projected on the x-axis of the {0} coordinate system, Ly mj Indicates the distance of its projection on the y-axis, Lz mj Represents the distance of its projection on the z-axis;

[0036] Then calculate the distance from the point to the simplified obstacle:

[0037]

[0038] Where: L mj Represents the feature point p m With obstacles O j If the feature point is inside the obstacle, then L mj A negative value indicates interference, and externally the distance is calculated based on the projection distance, which simplifies the distance calculation process;

[0039] So the distance between the characteristic circle and the obstacle is expressed as: L mj -r m ;

[0040] (4) Select the safety distance to design the interference function g(x). g(x) should satisfy the requirement of monotony and be non-increasing, and be 0 when the distance is greater than the safety distance. As a further preferred embodiment, the specific g(x) is selected as follows:

[0041]

[0042] Where: d s is the safety distance, x is the distance between the robot arm and the obstacle, g(x) increases rapidly as x decreases when x is less than zero, increases slowly when x is between the safety distance and 0, and increases slowly when x is greater than d. s When g(x) is zero, the larger the g(x), the more serious the interference. When g(x) is used as the objective function for subsequent optimization, it is helpful to avoid the interference.

[0043] (5) Discretize the excitation trajectory θ(t) according to equal time intervals Δt to obtain different postures θ i , the excitation trajectory uses a finite Fourier series:

[0044]

[0045] Where: θ k is the position of joint k, t is the time, ω f is the fundamental frequency of the Fourier series, H is the order of the finite Fourier series, and are respectively called the Fourier coefficients of the i-th order sine and cosine terms of joint k, θ k,0 It is called the position bias term;

[0046] Discretize the above excitation trajectory as follows:

[0047]

[0048] θ i =[θ1(iΔt),θ2(iΔt),θ3(iΔt),θ4(iΔt),θ5(iΔt),θ6(iΔt)],

[0049] Where: θ i represents the six joint angles of the i-th position after discretization, T is the period of the finite Fourier series, and Δt is the selected discrete time interval;

[0050] (6) Calculate the posture transformation matrix under different postures

[0051]

[0052] in, The calculation method is:

[0053]

[0054] Where: d n , a n , α nis the DH parameter, joint distance d n Defined as n-1 Axis to X n The distance along the Z axis n-1 The direction of the axis is positive; the connecting rod length is a n Defined as Z n-1 Axis to Z n The distance along the X axis n The direction of the axis is positive; the connecting rod torsion angle α n Defined as Z n-1 Axis to Z n The rotation angle around the X axis n The positive rotation of the axis is positive, and α is specified n ∈(-π,π];

[0055] (7) Calculate the interference distance between all feature points p and obstacle O at posture i (time iΔt) from (3) to obtain posture θ i Simplified calculation method of the interference distance L between the lower robot arm and the obstacle:

[0056]

[0057] Where L mj represents the distance between the mth feature point and the jth obstacle at time iΔt, r m represents the radius of the characteristic circle at the mth characteristic point, min{·} is the minimum value function, and L i It represents the minimum value of the distance between all characteristic circles on the manipulator and the simplified obstacle, which can represent the interference between the manipulator and the obstacle in this posture;

[0058] (8) Substitute the L calculated in (7) i Substitute the designed interference function g(x) into the calculation of all posture interference function values ​​g(L i ), and calculate the interference function of the trajectory with its maximum value or average value. Preferably, a method for calculating the interference function of a trajectory is:

[0059]

[0060] Where: G(Γ) is the interference function of the trajectory, Γ∈R (2NH+N)×1 is the excitation trajectory coefficient to be determined, g(L(t)) is the interference function between the manipulator and the obstacle at time t, and max{·} is the maximum value function.

[0061] (9) Taking the trajectory interference function as the objective function, the non-interference trajectory is optimized under the physical boundary constraints and physical limit constraints:

[0062]

[0063] Among them: The excitation trajectory coefficient to be determined is recorded as Γ∈R (2NH+N) ×1, N represents the total number of joints, θ k,max and θ k,min They represent the maximum and minimum motion ranges allowed for joint k, and are the functions of velocity and acceleration with respect to time, and They represent the velocity and acceleration limits of joint k respectively;

[0064] (10) Repeat step (9) multiple times to calculate and obtain multiple non-interference trajectories to form a non-interference trajectory set;

[0065] (11) Taking the sum of the excitation trajectory condition number and the trajectory interference function as the objective function and the non-interference trajectory set as the initial data set, a heuristic optimization algorithm is used to optimize the calculation of the non-interference excitation trajectory under the physical boundary constraint and the physical limit constraint:

[0066]

[0067] Where: H b represents the observation matrix corresponding to the dynamic parameters in the joint dynamics model, and the cond(·) function represents the condition number calculation function;

[0068] When interference occurs, the magnitude of G(L) is much lower than that of cond(H b ) is slightly larger, so the optimized trajectory is preferentially a non-interference trajectory; when there is no interference, G(L) is small or 0, and the optimized trajectory can give full play to the excitation characteristics of the excitation trajectory.

[0069] Furthermore, in step (1), the feature point is selected at the center position of the joint of the robot arm and the center position where the robot arm connecting rod turns, and the corresponding feature radius is slightly larger than the radius of the circumscribed circle of the cross section of the robot arm at the feature point.

[0070] Furthermore, in step (2), the cuboid is a circumscribed cuboid of the obstacle, and its side length is parallel to the world coordinate system {0}.

[0071] Furthermore, in step (5), the time interval Δt=0.01T.

[0072] Another object of the present invention is to provide a computer device, which includes a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of the robot dynamics identification and excitation trajectory planning method under the mixed constraints of geometry and physics.

[0073] Another object of the present invention is to provide a computer-readable storage medium storing a computer program, which, when executed by a processor, enables the processor to execute the steps of the robot dynamics identification and excitation trajectory planning method under the geometric and physical mixed constraints.

[0074] Another object of the present invention is to provide a robot dynamics identification and excitation trajectory planning system under geometric and physical mixed constraints, comprising the following modules:

[0075] Feature point selection module: used to select feature points and feature radius on the robot arm according to the robot arm structure, and obtain the coordinates of the feature points in the world coordinate system through DH coordinate system transformation;

[0076] Obstacle simplification module: used to extend the obstacles in the robot's workspace outward and simplify them into cuboids, and describe the simplified obstacle information in the world coordinate system;

[0077] Distance calculation module: used to calculate the distance between the characteristic circle at the feature point and the simplified obstacle according to the robot arm posture;

[0078] Interference function design module: used to select the safety distance and design the interference function to ensure that the interference function is monotonically non-increasing and is 0 when the distance is greater than the safety distance.

[0079] Furthermore, it also includes:

[0080] Excitation trajectory discretization module: used to discretize the excitation trajectory at equal time intervals to obtain different postures. The excitation trajectory is represented by a finite Fourier series.

[0081] Posture transformation calculation module: used to calculate the posture transformation matrix under different postures.

[0082] Furthermore, it also includes:

[0083] Interference distance calculation module: used to calculate the interference distance between all feature points and obstacles, and obtain the interference distance between the robot arm and the obstacle;

[0084] Interference function calculation module: used to bring the calculated interference distance into the interference function to calculate the interference function values ​​of all postures, and calculate the interference function of the trajectory with the maximum value or average value.

[0085] Furthermore, it also includes:

[0086] Trajectory optimization module: It is used to optimize and solve the non-interference trajectory under the physical boundary constraints and physical limit constraints with the trajectory interference function as the objective function, and repeatedly calculate to obtain multiple non-interference trajectories to form a non-interference trajectory set;

[0087] Heuristic optimization module: It is used to optimize the calculation of non-interference excitation trajectory by using the sum of the excitation trajectory condition number and the trajectory interference function as the objective function and the non-interference trajectory set as the initial data set, and adopts a heuristic optimization algorithm under the physical boundary constraints and physical limit constraints.

[0088] In combination with the above technical solutions and the technical problems solved, the advantages and positive effects of the technical solutions to be protected by the present invention are as follows:

[0089] First, the present invention simplifies the robot arm into a plurality of spheres and the obstacles in the workspace into cuboids, which can reflect the characteristics of the robot arm and obstacles in actual situations while simplifying the calculation amount for interference.

[0090] The present invention adopts the following steps to calculate the interference distance between a sphere and a cuboid: first, the distance from a point to the cuboid is calculated through the distance between the feature point of the sphere center and the projection of the cuboid, and then the distance between the sphere and the cuboid is further calculated, thereby avoiding a large number of classification calculations for the positions of the sphere and the cuboid in three-dimensional space.

[0091] Since the optimization calculation of the condition number of the observation matrix is ​​time-consuming, the present invention first optimizes and solves multiple interference-free trajectories as the initial data set without considering the condition number of the observation matrix, and then performs secondary optimization solution considering the condition number and interference function. Compared with directly optimizing and solving the interference-free low condition number excitation trajectory, the optimization solution time is shortened.

[0092] The present invention combines geometric constraints with physical constraints by taking the trajectory interference measurement value as a part of the objective function to realize the excitation trajectory planning under the geometric and physical mixed constraints.

[0093] Second, the technical solution of the present invention fills the technical gap in the industry at home and abroad:

[0094] Previous excitation trajectory planning directly considered the geometric constraints of obstacles, and lacked effective means for excitation trajectory planning under complex working conditions. The robot arm excitation trajectory planning method under geometric and physical mixed constraints provided by the present invention fills the gap that obstacle constraints cannot be directly considered when planning the robot arm excitation trajectory.

[0095] The technical solution of the present invention solves the technical problems that people have been eager to solve but have never been able to solve successfully:

[0096] For a complex robotic arm and its working environment, how to simplify the description of the interference situation during its movement has always been a technical problem that people have been eager to solve but have never succeeded in achieving. The present invention simplifies the geometric features of the robotic arm and obstacles, performs point-to-surface collision calculations under time discreteness, and realizes simplified modeling of actual working conditions and mathematical measurement calculations of interference situations.

[0097] Third, technical problems of existing technologies and significant technological advances.

[0098] Technical problems of existing technologies:

[0099] High complexity of trajectory planning: In the existing technology, the trajectory planning of the robot arm requires a lot of calculations in complex environments, and it is difficult to calculate a safe trajectory without interference in real time.

[0100] Insufficient safety: Traditional methods are prone to collisions between the robotic arm and obstacles in complex environments, posing a safety hazard.

[0101] Low optimization efficiency: Existing methods are inefficient in the trajectory optimization process and have difficulty finding the optimal solution under physical and geometric constraints.

[0102] Significant technological advancements:

[0103] Simplified distance calculation: By simplifying obstacles into cuboids, the complexity of distance calculation is greatly reduced and the efficiency of trajectory planning is improved.

[0104] Efficient interference evaluation: Designing a monotonically non-increasing interference function can quickly evaluate the interference between feature points and obstacles and quantify the degree of interference.

[0105] High optimization solution efficiency: Through the heuristic optimization algorithm, the non-interference trajectory is efficiently solved under the constraints of physical boundaries and physical limits, which significantly improves the efficiency of trajectory optimization.

[0106] Comprehensive optimization goal: In the final optimization process, the excitation trajectory condition number and trajectory interference function are comprehensively considered to ensure the safety of the trajectory and the excitation effect.

[0107] Safety enhancement: The non-interference trajectory is calculated repeatedly, and a non-interference trajectory set is constructed, from which the optimal trajectory is selected to ensure the safe operation of the robot arm in complex environments.

[0108] The present invention solves the problems of high complexity, insufficient safety and low optimization efficiency in the existing technology through systematic parameters, algorithms and mathematical models. The method of geometric and physical hybrid constraints is adopted to realize efficient, safe and accurate trajectory planning of the robot arm in complex environments, achieving significant technological progress. BRIEF DESCRIPTION OF THE DRAWINGS

[0109] Figure 1 It is a flow chart of robot arm excitation trajectory planning under geometric and physical mixed constraints provided by an embodiment of the present invention;

[0110] Figure 2 is a schematic diagram of a mechanical arm joint simplified as a characteristic ball provided by an embodiment of the present invention;

[0111] Figure 3is a simplified schematic diagram of obstacles in the working space of a robot arm provided by an embodiment of the present invention;

[0112] Figure 4 is an interference function diagram of a structure provided by an embodiment of the present invention;

[0113] Figure 5 It is an excitation trajectory diagram under the planning geometric and physical mixed constraints provided by an embodiment of the present invention;

[0114] Figure 6 It is a schematic diagram of the variation of the joint angle of the excitation trajectory over time under the physical geometric constraints provided by an embodiment of the present invention;

[0115] Figure 7 It is a schematic diagram of the variation of the joint angular velocity of the excitation trajectory with time under the physical geometric constraints provided by an embodiment of the present invention;

[0116] Figure 8 It is a schematic diagram of the variation of the joint angular acceleration of the excitation trajectory with time under the physical geometric constraints provided by an embodiment of the present invention;

[0117] Fig. 9 It is a schematic diagram of the interference of excitation trajectories under physical geometric constraints provided by an embodiment of the present invention;

[0118] Fig.10 This is a schematic diagram of the interference between the excitation trajectory and the obstacle generated by using common physical constraints without using the method of the present invention;

[0119] Fig.11 It is a diagram of the convergence of the condition number for optimizing the excitation trajectory with and without considering obstacles. DETAILED DESCRIPTION

[0120] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0121] Technical problems of existing technologies in industrial applications.

[0122] 1. Robot path planning in automated manufacturing

[0123] Technical issues:

[0124] Inefficiency: In a manufacturing workshop, a robot arm needs to operate between multiple devices and workpieces. If the trajectory planning does not take obstacle constraints into account, the planning efficiency is low and multiple adjustments are required, which reduces production efficiency.

[0125] Safety hazards: When working in a complex environment, if the trajectory planning does not take obstacle constraints into consideration, collisions are likely to occur, posing a safety hazard.

[0126] The solution of the present invention:

[0127] By directly considering the geometric constraints of obstacles in the planning process, the efficiency and reliability of trajectory planning are improved.

[0128] By optimizing the trajectory, the robot arm can operate safely and efficiently in complex environments, reduce the risk of collision and improve production efficiency.

[0129] 2. Path planning in medical surgical robots

[0130] Technical issues:

[0131] High precision requirements: In minimally invasive surgery, surgical robots need to operate precisely in a small space. Existing methods make it difficult to directly consider obstacles such as human organs and blood vessels in the planning stage, resulting in frequent trajectory adjustments, which affects the efficiency and effectiveness of the surgery.

[0132] High operational risk: If the trajectory during surgery does not take obstacles into consideration, the robot may accidentally touch important organs and blood vessels, resulting in a high surgical risk.

[0133] The solution of the present invention:

[0134] The geometric constraints of organs and blood vessels in the human body are directly considered during the planning process to optimize the operation trajectory of the surgical robot and improve surgical accuracy.

[0135] By optimizing the algorithm, we can ensure that the surgical robot avoids collisions during operation, reduce surgical risks and improve the success rate of surgery.

[0136] The present invention adopts an incentive trajectory planning method under geometric and physical mixed constraints, directly considers the geometric constraints of obstacles in the planning process, improves the efficiency and accuracy of trajectory planning, and solves the following main problems in the prior art:

[0137] 1) Improve planning efficiency: Directly consider the geometric constraints of obstacles, avoid multiple adjustments and simulations, and significantly improve trajectory planning efficiency.

[0138] 2) Enhanced safety: By optimizing trajectory planning, collisions can be avoided in complex environments and potential safety hazards can be reduced, which significantly improves operational safety and reliability, especially in high-risk applications such as automated manufacturing and medical surgery.

[0139] 3) Improve operational accuracy: In high-precision applications such as medical surgery, obstacles in the human body are directly considered to improve the operational accuracy and success rate of surgical robots.

[0140] By solving the above technical problems, the present invention has brought significant technological progress in industrial applications such as automated manufacturing and medical surgery, and improved the efficiency, safety and accuracy of the overall system.

[0141] Example 1: Robot path planning in automated manufacturing

[0142] In modern manufacturing plants, robotic arms are widely used in tasks such as welding, assembly, and handling. Due to the complex manufacturing environment, robotic arms need to avoid various obstacles in the workspace, such as other equipment, workpieces, and personnel, to ensure that tasks are completed efficiently and safely.

[0143] The robot dynamics identification-inspired trajectory planning method under geometric and physical mixed constraints of the present invention can be used to optimize the path planning of a robot arm in a manufacturing workshop. In such an environment, the robot arm needs to perform a variety of tasks while avoiding collisions with obstacles in the environment.

[0144] 1. Feature point selection:

[0145] Feature points are selected at the key joints and end effector of the robot arm, and the geometric coordinates of these feature points are measured and recorded.

[0146] 2. Obstacle modeling:

[0147] The equipment and workpieces in the workshop are simplified into rectangular obstacles, and the coordinate information of each rectangular obstacle is recorded.

[0148] 3. Trajectory planning:

[0149] The excitation trajectory of the robot arm is represented by Fourier series and discretized.

[0150] The distances between feature points and obstacles in different postures are calculated, and interference functions are designed to evaluate the safety of the trajectory.

[0151] 4. Optimize trajectory:

[0152] Under physical boundaries and speed and acceleration constraints, the robot's excitation trajectory is optimized to ensure an interference-free path.

[0153] 5. Path execution:

[0154] The optimized trajectory is loaded into the control system of the robot arm to perform the manufacturing task.

[0155] Advantages:

[0156] The movement safety of the robot arm is improved and collision with equipment and workpieces is avoided.

[0157] The motion path of the robotic arm is optimized to improve task execution efficiency.

[0158] It ensures the stable operation of the robotic arm in complex environments and reduces human intervention.

[0159] Example 2: Path planning in a medical surgical robot

[0160] In minimally invasive surgery, medical surgical robots need to operate precisely in the narrow space of the human body. During the operation, the robot needs to avoid important organs and blood vessels to ensure the safety and success of the operation.

[0161] The method of the present invention can be used to optimize the path planning of a medical surgical robot in minimally invasive surgery, ensuring that the robot moves and operates accurately and safely during the operation.

[0162] Implementation steps:

[0163] 1. Feature point selection:

[0164] Feature points are selected at the joints of the surgical robot and the ends of the surgical tools, and the geometric coordinates of these feature points are measured and recorded.

[0165] 2. Obstacle modeling:

[0166] The patient's main organs and blood vessels are simplified into rectangular obstacles, and the coordinate information of each rectangular obstacle is recorded.

[0167] 3. Trajectory planning:

[0168] The excitation trajectory of the surgical robot is represented by Fourier series and discretized.

[0169] The distances between feature points and obstacles in different postures are calculated, and interference functions are designed to evaluate the safety of the trajectory.

[0170] 4. Optimize trajectory:

[0171] Under physical boundaries and speed and acceleration constraints, the excitation trajectory of the surgical robot is optimized to ensure an interference-free path.

[0172] 5. Path execution:

[0173] The optimized trajectory is loaded into the control system of the surgical robot to perform the surgical operation.

[0174] Advantages:

[0175] The operational safety of the surgical robot is improved and collision with important organs and blood vessels is avoided.

[0176] The motion path of the surgical robot is optimized, improving surgical accuracy and efficiency.

[0177] It ensures the stable operation of the surgical robot in complex surgical environments and reduces surgical risks and complications.

[0178] Through the above two specific industrial application examples, the method of the present invention has demonstrated significant technical progress and advantages in practical applications, and can effectively improve the safety and efficiency of the robot system.

[0179] The present invention provides an example of geometric-physical hybrid constraint-inspired trajectory planning for a collaborative robot in a multi-obstacle environment, which is further described below in conjunction with the accompanying drawings.

[0180] like Figure 1 As shown, an embodiment of the present invention provides a method for planning a robot arm excitation trajectory under a mixed constraint of geometry and physics, which specifically includes the following steps:

[0181] (1) According to the structure of the robot arm and the feature point p selected on the robot arm m and characteristic radius r m , which is expressed in the robot DH coordinate system as n p m , combined with the DH coordinate system transformation, the coordinates of the feature points in the world coordinate system can be obtained 0 p m :

[0182]

[0183] in: n p m Represents the description of the mth feature point of the robot arm in the coordinate system {n}, represents the homogeneous transformation matrix from coordinate system {0} to coordinate system {n}, 0 p m Represents the description of the mth feature point of the robot arm in the coordinate system {0}.

[0184] The feature point is fixed on the joint n, that is, n p m It is a fixed value, selected by the robot arm structure.

[0185] like Figure 2 As shown, the characteristic circle composed of the characteristic points and characteristic vectors of joints 1 and 4 of the collaborative robot arm is shown in the figure.

[0186] (2) The obstacles in the robot's workspace are extended outward to be simplified into a cuboid. After extension, the edges of the cuboid are parallel to the {0} axis of the coordinate system. The information of the simplified j-th cuboid is recorded as O j , described in the world coordinate system of the robot, we have:

[0187]

[0188] in: Indicates obstacle O jThe maximum and minimum values ​​of the x-axis projection of the coordinate system {0}, Indicates obstacle O j The maximum and minimum values ​​of the y-axis projection of the coordinate system {0}, Indicates obstacle O j The maximum and minimum values ​​of the z-projection of coordinate system {0}.

[0189] like Figure 3 As shown in the figure, the simplified obstacles in the robot arm and its workspace in this example are shown in the figure. There are a total of 6 obstacles, which are simplified into six rectangular blocks. If the obstacles are not considered and the excitation trajectory planning is directly performed, interference is very likely to occur.

[0190] (3) Calculate the distance between the characteristic circle at the characteristic point and the simplified obstacle according to the robot arm posture. The coordinates of the characteristic point are:

[0191] 0 P m (t) = [ 0 x m (t), 0 y m (t), 0 z m (t),1] T

[0192] in: 0 x m , 0 y m , 0 z m is the characteristic point p of the robot m Coordinates in the {0} coordinate system.

[0193] Feature point p m Distance to simplified obstacle O j The distance L mj The calculation is done as follows. First, the distance between the point and the obstacle projection is calculated:

[0194]

[0195] Where: Lx mj Represents the feature point p m With obstacles O j The distance projected on the x-axis of the {0} coordinate system, Ly mj Indicates the distance of its projection on the y-axis, Lz mj Represents the distance of its projection on the z-axis;

[0196] Then calculate the distance from the point to the simplified obstacle

[0197]

[0198] Where: L mj Represents the feature point p m With obstacles O j If the feature point is inside the obstacle, then L mj A negative value indicates interference occurs, and externally it is calculated based on the projection distance, which simplifies the distance calculation process.

[0199] So the distance between the characteristic circle and the obstacle is expressed as: L mj -r m .

[0200] (4) Select the safety distance to design the interference function g(x). g(x) should satisfy the requirement of monotony and be non-increasing, and be 0 when the distance is greater than the safety distance. As a further preferred embodiment, the specific g(x) is selected as follows:

[0201]

[0202] Where: d s is the safe distance, x is the distance between the objects, g(x) increases rapidly as x decreases when x is less than zero, increases slowly when x is between the safe distance and 0, and increases slowly when x is greater than d. s When g(x) is zero, the larger the g(x), the more serious the interference. When g(x) is subsequently optimized as the objective function, it is helpful to avoid interference.

[0203] like Figure 4 The figure shows the g(x) image selected in the example. The safety distance is 5 cm, which is in line with the actual situation.

[0204] (5) Discretize the excitation trajectory θ(t) according to equal time intervals Δt to obtain different postures θ i , the excitation trajectory uses a finite Fourier series:

[0205]

[0206] where θ k is the position of joint k, t is the time, ω f is the fundamental frequency of the Fourier series, H is the order of the finite Fourier series, and are respectively called the Fourier coefficients of the i-th order sine and cosine terms of joint k, θ k,0 It is called the position bias term.

[0207] Discretize the above excitation trajectory as follows:

[0208]

[0209] θ i=[θ1(iΔt),θ2(iΔt),θ3(iΔt),θ4(iΔt),θ5(iΔt),θ6(iΔt)],

[0210] where θ i represents the six joint angles of the i-th posture after discretization, T is the period of the finite Fourier series, and Δt is the selected discrete time interval.

[0211] Preferably, the time interval Δt=0.01T.

[0212] (6) Calculate the posture transformation matrix under different postures

[0213]

[0214] in, The calculation method is:

[0215]

[0216] Among them, d n , a n , α n is the DH parameter, joint distance d n Defined as n-1 Axis to X n The distance along the Z axis n-1 The direction of the axis is positive; the connecting rod length is a n Defined as Z n-1 Axis to Z n The distance along the X axis n The direction of the axis is positive; the connecting rod torsion angle α n Defined as Z n-1 Axis to Z n The rotation angle around the X axis n The positive rotation of the axis is positive, and α is specified n ∈(-π,π].

[0217] (7) Calculate the interference distance between all feature points p and obstacle O at posture i (time iΔt) from (3) to obtain posture θ i Simplified calculation method of the interference distance L between the lower robot arm and the obstacle:

[0218]

[0219] Where L mj represents the distance between the mth feature point and the jth obstacle at time iΔt, r m represents the radius of the characteristic circle at the mth characteristic point, min{·} is the minimum function, L iIt represents the minimum value of the distance between all characteristic circles on the robot and the simplified obstacle, which can represent the interference between the robot and the obstacle in this posture.

[0220] (8) Substitute the L calculated in (7) i Substitute the designed interference function g(x) into the calculation of all posture interference function values ​​g(L i ), and calculate the interference function of the trajectory with its maximum value or average value. Preferably, a method for calculating the interference function of a trajectory is:

[0221]

[0222] Where: G(Γ) is the interference function of the trajectory, Γ∈R (2NH+N)×1 is the excitation trajectory coefficient to be determined, g(L(t)) is the interference function between the manipulator and the obstacle at time t, and max{·} is the maximum value function.

[0223] (9) Taking the trajectory interference function as the objective function, the non-interference trajectory is optimized under the physical boundary constraints and physical limit constraints:

[0224]

[0225] Among them, the excitation trajectory coefficient to be determined is recorded as Γ∈R (2NH+N)×1 , N represents the total number of joints, θ k,max and θ k,min They represent the maximum and minimum motion ranges allowed for joint k, and are the functions of velocity and acceleration with respect to time, and They represent the velocity and acceleration limits of joint k respectively.

[0226] (10) Repeat step (9) multiple times to obtain multiple non-interference trajectories to form a non-interference trajectory set.

[0227] (11) Taking the sum of the excitation trajectory condition number and the trajectory interference function as the objective function and the non-interference trajectory set as the initial data set, a heuristic optimization algorithm is used to optimize the calculation of the non-interference excitation trajectory under the physical boundary constraint and the physical limit constraint:

[0228]

[0229] Among them, H b It represents the observation matrix corresponding to the dynamic parameters in the joint dynamics model, and the cond(·) function represents the condition number calculation function.

[0230] When interference occurs, the magnitude of G(L) is much lower than that of cond(H b) is slightly larger, so the optimized trajectory is preferentially a non-interference trajectory; when there is no interference, G(L) is small or 0, and the optimized trajectory can give full play to the excitation characteristics of the excitation trajectory.

[0231] like Figure 5 As shown, after optimization, the excitation trajectory does not interfere with obstacles and satisfies the geometric constraints;

[0232] like Figures 6 to 8 The figure shows the change of joint angle, angular velocity and angular acceleration with time of the optimized excitation trajectory. It can be seen that the physical constraints are met.

[0233] The final optimized excitation trajectory has a lower condition number, such as Fig.11 shown.

[0234] An embodiment of the present invention provides a computer device, which includes a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the steps of the robot dynamics identification excitation trajectory planning method under the geometric and physical mixed constraints.

[0235] An embodiment of the present invention provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the processor executes the steps of the robot dynamics identification and excitation trajectory planning method under the geometric and physical mixed constraints.

[0236] The robot dynamics identification and excitation trajectory planning method under the mixed constraints of geometry and physics of the present invention has a wide range of application fields and related products, including but not limited to:

[0237] Industrial robots: Application in automated production lines helps robots perform efficient non-interference trajectory calculation and task execution in complex environments, ensuring safety and accuracy.

[0238] Service robots: In service robots in homes, hospitals, hotels and other places, service safety is improved by optimizing trajectory planning.

[0239] Human-machine collaboration: In collaborative robots, trajectory optimization is achieved during human-machine collaborative operations, and trajectory planning is performed considering collision modeling between humans and robotic arms to ensure safe interaction between humans and machines.

[0240] Education and Research: In the field of robotics research and education, provide an effective physical geometry constraint excitation trajectory for the research and teaching of robot dynamics, and promote the development of related technologies.

[0241] like Fig. 9The figure shows a schematic diagram of the interference of the excitation trajectory under the physical geometric constraints provided by an embodiment of the present invention, wherein every 100 points on the horizontal axis represent the change in the distance of a characteristic ball to the obstacle over time. There are 9 characteristic balls in total. It can be seen from the figure that the obtained excitation trajectory makes the distances between the 9 characteristic balls and the three obstacles greater than zero, indicating that the planned excitation trajectory is collision-free.

[0242] Run the stimulus trace in a simulation environment to verify that Figure 5 As shown, it can be seen that the robotic arm did not collide with the obstacle.

[0243] Comparative experiments such as Fig.10 As shown in the figure, under the same working condition, if the geometric constraints of the obstacle are not considered for excitation trajectory optimization, the distance between the joint characteristic ball and the obstacle will be less than 0, indicating that a collision has occurred.

[0244] The iteration is performed under the same teaching and learning heuristic algorithm with comprehensive consideration of physical geometric constraints and only considering physical constraints. The iterative convergence diagram is shown in Fig.11 As shown in the figure, in both cases, only considering the physical constraints will result in a lower condition number, but both methods converge equally, which shows that the incentive trajectory planning method under the physical geometric constraints proposed in this method can also converge well. The lower condition number of the incentive trajectory is due to the large number of spatial obstacles that impose greater restrictions on the trajectory.

[0245] It should be noted that the embodiments of the present invention can be implemented by hardware, software, or a combination of software and hardware. The hardware part can be implemented using dedicated logic; the software part can be stored in a memory and executed by an appropriate instruction execution system, such as a microprocessor or dedicated design hardware. A person of ordinary skill in the art will appreciate that the above-mentioned devices and methods can be implemented using computer executable instructions and / or contained in a processor control code, such as a carrier medium such as a disk, CD or DVDROM, a programmable memory such as a read-only memory (firmware), or a data carrier such as an optical or electronic signal carrier. Such code is provided on the carrier medium. The device and its modules of the present invention can be implemented by hardware circuits such as very large-scale integrated circuits or gate arrays, semiconductors such as logic chips, transistors, etc., or programmable hardware devices such as field programmable gate arrays, programmable logic devices, etc., can also be implemented by software executed by various types of processors, and can also be implemented by a combination of the above-mentioned hardware circuits and software, such as firmware.

[0246] The above description is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any modifications, equivalent substitutions and improvements made by any technician familiar with the technical field within the technical scope disclosed by the present invention and within the spirit and principle of the present invention should be covered by the protection scope of the present invention.

Claims

1. A robot dynamics identification and excitation trajectory planning method under geometric and physical mixed constraints, characterized in that: The method includes: (1) According to the structure of the robot arm and the feature points and feature radius on the robot arm, they are expressed as in the robot arm DH coordinate system, and the coordinates of the feature points in the world coordinate system are obtained by DH coordinate system transformation; (2) Extend the obstacles in the robot's workspace outward and simplify them into a cuboid, and describe the simplified obstacle information in the world coordinate system; (3) Calculate the distance between the characteristic circle at the characteristic point and the simplified obstacle according to the robot arm posture; (4) Select the safety distance to design the interference function. The interference function should be monotonically non-increasing and be 0 when the distance is greater than the safety distance. (5) The excitation trajectory is discretized at equal time intervals to obtain different postures, and the excitation trajectory is represented by a finite Fourier series; (6) Calculate the posture transformation matrix under different postures; (7) Calculate the interference distance between all feature points and obstacles to obtain the interference distance between the robot arm and the obstacle; (8) Substitute the calculated interference distance into the interference function to calculate the interference function values ​​of all postures, and calculate the interference function of the trajectory with the maximum value or average value; (9) Taking the trajectory interference function as the objective function, optimize and solve the non-interference trajectory under the physical boundary constraints and physical limit constraints; (10) Repeat step (9) multiple times to calculate and obtain multiple non-interference trajectories to form a non-interference trajectory set; (11) The sum of the excitation trajectory condition number and the trajectory interference function is taken as the optimization objective function, and the non-interference trajectory set is taken as the initial data set. A heuristic optimization algorithm is used to optimize the calculation of the non-interference excitation trajectory under the physical boundary constraints and physical limit constraints.

2. The robot dynamics identification and excitation trajectory planning method under geometric and physical mixed constraints according to claim 1 is characterized by: In the step (1), the feature point is selected at the center position of the joint of the robot arm and the center position where the robot arm connecting rod turns, and the corresponding feature radius is slightly larger than the radius of the circumscribed circle of the cross section of the robot arm at the feature point.

3. The robot dynamics identification and excitation trajectory planning method under the mixed constraints of geometry and physics according to claim 1 is characterized by: In the step (2), the cuboid is a circumscribed cuboid of the obstacle, and its side length is parallel to the world coordinate system {0}.

4. The robot dynamics identification and excitation trajectory planning method under geometric and physical mixed constraints according to claim 1 is characterized by: In step (4), the trajectory interference function is specifically selected as follows: Where: d s is the safety distance, x is the distance between the robot arm and the obstacle, g(x) increases rapidly as x decreases when x is less than zero, increases slowly when x is between the safety distance and 0, and increases slowly when x is greater than d. s When g(x) is zero, the larger the g(x), the more serious the interference. When g(x) is subsequently optimized as part of the optimization objective function, it is helpful to avoid interference.

5. The robot dynamics identification and excitation trajectory planning method under the mixed constraints of geometry and physics according to claim 1 is characterized by: In step (5), the excitation trajectory is discretized as follows: Among them, q k represents the six joint angles of the kth posture after discretization, T is the period of the Fourier series, and Δt is the selected discrete time interval.

6. The robot dynamics identification and excitation trajectory planning method under the mixed constraints of geometry and physics according to claim 1 is characterized by: In step (11), the optimization objective function is: Among them, Y(q) is the observation matrix corresponding to the dynamic parameters in the joint dynamics model, and cond(Y(q)) is the condition number calculation function.

7. A robot dynamics identification and excitation trajectory planning system under mixed geometric and physical constraints, characterized in that: Includes the following modules: Feature point selection module: used to select feature points and feature radius on the robot arm according to the robot arm structure, and obtain the coordinates of the feature points in the world coordinate system through DH coordinate system transformation; Obstacle simplification module: used to extend the obstacles in the robot's workspace outward and simplify them into cuboids, and describe the simplified obstacle information in the world coordinate system; Distance calculation module: used to calculate the distance between the characteristic circle at the feature point and the simplified obstacle according to the robot arm posture; Interference function design module: used to select the safety distance and design the interference function, ensuring that the interference function is monotonically non-increasing and is 0 when the distance is greater than the safety distance; Excitation trajectory discretization module: used to discretize the excitation trajectory at equal time intervals to obtain different postures. The excitation trajectory is represented by a finite Fourier series. Posture transformation calculation module: used to calculate the posture transformation matrix under different postures; Interference distance calculation module: used to calculate the interference distance between all feature points and obstacles, and obtain the interference distance between the robot arm and the obstacle; Interference function calculation module: used to bring the calculated interference distance into the interference function to calculate the interference function values ​​of all postures, and calculate the interference function of the trajectory with the maximum value or average value; Trajectory optimization module: It is used to optimize and solve the non-interference trajectory under the physical boundary constraints and physical limit constraints with the trajectory interference function as the objective function, and repeatedly calculate to obtain multiple non-interference trajectories to form a non-interference trajectory set; Heuristic optimization module: It is used to optimize the calculation of non-interference excitation trajectory by using the sum of the excitation trajectory condition number and the trajectory interference function as the objective function and the non-interference trajectory set as the initial data set, and adopts a heuristic optimization algorithm under the physical boundary constraints and physical limit constraints.

Images