Mechanical arm trajectory planning time scaling system and method

Abstract

The invention discloses a mechanical arm trajectory planning time scaling system and method. The system comprises a Simulink model, an external torque observer model and a time generator. The Simulink model is used for obtaining an original timestamp and obtaining a timestamp after track scaling in combination with a torque signal of the mechanical arm; the external torque observer model is used for obtaining an expected torque and an expected angular velocity of the mechanical arm according to a timestamp of track scaling and a torque signal; the time generator is used for obtaining a scaling factor according to the expected torque and the expected angular speed, and adjusting the motion speed of the mechanical arm with interference at different degrees, the time stamp of the track can be dynamically adjusted according to the contact force between the end effector of the mechanical arm and the environment, so that damage caused by collision is reduced or avoided, and the stability of the mechanical arm is improved. And the expected position, speed and acceleration are scaled or transformed by using the feedback of the contact force sensor.

Description

Technical Field

[0001] The present invention relates to the technical field of robot safety trajectory tracking control, and more specifically to a robot arm trajectory planning time scaling system and method. Background Art

[0002] Robotic arm trajectory planning is one of the core technologies of robotic arm control. Its purpose is to generate the motion trajectory of each joint of the robotic arm according to the given task requirements, so that the robotic arm can complete the task safely and efficiently. The traditional trajectory planning method usually uses polynomial interpolation to express the motion trajectory of the robotic arm as a function of time; however, in practical applications, the motion trajectory of the robotic arm may be affected by environmental factors, the state of the robotic arm itself, and other factors, resulting in deviations between the motion trajectory of the robotic arm and the expected trajectory, or even collisions. Summary of the invention

[0003] The object of the present invention is to provide a robot arm trajectory planning time scaling system and method, which dynamically adjusts the timestamp of the trajectory according to the contact force between the robot arm end effector and the environment, thereby reducing or avoiding damage caused by collision.

[0004] To achieve the above object, the present invention provides the following technical solutions:

[0005] A robot arm trajectory planning time scaling system, the system comprising: a Simulink model, an external torque observer model and a time generator;

[0006] The Simulink model is used to obtain the original timestamp, and combined with the torque signal of the robot arm, obtain the timestamp of the trajectory after scaling;

[0007] The external torque observer model is used to obtain the desired torque and the desired angular velocity of the robot arm according to the timestamp and torque signal of trajectory scaling;

[0008] The time generator is used to obtain a scaling factor according to the expected torque and the expected angular velocity, so as to adjust the movement speed of the robot arm under different degrees of interference.

[0009] Furthermore, the Simulink model includes: a clock module with a switch, a trajectory scaling module and a delay module;

[0010] The clock module with switch is used to receive current time information and obtain an original timestamp;

[0011] The delay module is used to delay the torque signal to obtain a delayed torque signal;

[0012] The trajectory scaling module is used to process the original timestamp and the delayed torque signal to obtain the timestamp after trajectory scaling.

[0013] Furthermore, the external torque observer model includes: an expected motion trajectory module, an inverse dynamics model and a low-pass filter;

[0014] The expected motion trajectory module is used to obtain the expected motion trajectory according to the current position p of the robot arm and the timestamp after trajectory scaling;

[0015] The inverse dynamics model is used to obtain the desired torque of the robot arm according to the desired motion trajectory;

[0016] The low-pass filter is used to filter the torque signal and the expected torque of the robotic arm respectively, and update the state of the robotic arm through the expected torque and the torque signal of the robotic arm to obtain the expected torque and expected angular velocity of the robotic arm.

[0017] Furthermore, the desired motion trajectory includes: the position p, velocity qd and acceleration dqd of the robot arm.

[0018] The present invention also provides a method for time scaling of robot arm trajectory planning, comprising the following steps:

[0019] S1. Get the original timestamp based on the Simulink model and combine it with the torque signal of the robot arm to get the timestamp of the trajectory after scaling.

[0020] S2, based on the timestamp and torque signal of trajectory scaling combined with the external torque observer model, obtain the expected torque and expected angular velocity;

[0021] S3. Based on the expected torque and expected angular velocity combined with the time generator model, the scaling factor is obtained to adjust the movement speed of the robot arm under different degrees of interference.

[0022] Furthermore, the original timestamp is obtained based on the Simulink model in step S1, and the timestamp after trajectory scaling is obtained based on the torque signal of the manipulator, specifically:

[0023] Use the clock module with a switch to receive the current time information and obtain the original timestamp;

[0024] The torque signal is delayed by using a delay module to obtain a delayed torque signal;

[0025] The trajectory scaling module is used to process the original timestamp and the delayed torque signal to obtain the timestamp after trajectory scaling.

[0026] Furthermore, in step S2, the external torque is obtained based on the original timestamp and the timestamp of trajectory scaling combined with the external torque observation model, specifically:

[0027] The expected motion trajectory module is used to obtain the expected motion trajectory according to the current position p of the robot and the timestamp after trajectory scaling;

[0028] The inverse dynamics model is used to obtain the desired torque according to the desired motion trajectory;

[0029] The torque signal and the expected torque of the robot arm are filtered by a low-pass filter, and the state of the robot arm is updated by the expected torque and the torque signal of the robot arm respectively to obtain the expected torque and the expected angular velocity of the robot arm.

[0030] Furthermore, the desired torque is obtained based on the inverse dynamics model according to the desired motion trajectory, specifically:

[0031]

[0032] Among them, q d is the desired joint angle, is the desired angular velocity, is the desired joint angular acceleration, M is the mass matrix, C is the Coriolis and centrifugal effect matrix, and G is the gravity matrix.

[0033] Furthermore, in step S3, based on the expected torque and the expected angular velocity combined with the time generator model, a scaling factor is obtained to adjust the movement speed of the robot arm under different degrees of interference, specifically:

[0034] Based on the func_fs function, the expected angular velocity Perform normalization:

[0035]

[0036] The intermediate variable ψ is calculated based on the sensitivity constant α, where ψ represents the external torque. and the desired angular velocity The angle between them is as follows:

[0037]

[0038] According to the dead zone constant Γ and the reverse motion gain constant k, the value range of ψ is divided into four intervals, and the scaling factor f is defined in each interval s (ψ) to adjust the robot arm movement speed at different levels, the scaling factor f s The expression of (ψ) is as follows:

[0039]

[0040] In the above formula, when 0≤ψ<1: adjust the movement speed of the robot arm so that it moves slowly under small disturbance;

[0041] When 1≤ψ≤1+Γ: stop the robot arm from moving under medium disturbance;

[0042] When 1+Γ<ψ≤2+Γ: make the robot arm move in the opposite direction under large disturbance;

[0043] When 2+Γ<ψ: make the robot arm move in the reverse direction at full speed at the speed determined by k.

[0044] According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

[0045] The present invention can more accurately control the motion trajectory of the robot arm by using the Simulink model and trajectory scaling technology, thereby improving the accuracy of task execution; automatically adjust the motion speed of the robot arm according to the degree of external interference, so that the robot arm can maintain stable performance in different working environments; by real-time monitoring and processing of torque signals, the robot arm can quickly respond to changes in external torque, thereby dynamically adjusting the movement of the robot arm and improving the flexibility and adaptability of the system; dynamically adjust the timestamp of the trajectory according to the contact force between the end effector of the robot arm and the environment, thereby reducing or avoiding damage caused by collision, and use the feedback of the contact force sensor to scale or transform the expected position, speed and acceleration, so that the robot arm can automatically decelerate or stop when it contacts an obstacle, thereby protecting the robot arm and the obstacle. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0047] The following is a further description of the robot arm trajectory planning time scaling system and method in the present invention in conjunction with the accompanying drawings;

[0048] Figure 1 It is a schematic diagram of a Simulink model in the robot arm trajectory planning time scaling system provided by the present invention;

[0049] Figure 2 Schematic diagram of an external torque observer in the robot arm trajectory planning time scaling system provided by the present invention;

[0050] Figure 3It is a schematic diagram of a time generator in the robot arm trajectory planning time scaling system provided by the present invention. DETAILED DESCRIPTION

[0051] The specific implementation of the present invention is further described in detail below in conjunction with the accompanying drawings and examples. The following examples are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

[0052] In order to better understand the purpose, structure and function of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings.

[0053] The present invention provides a time scaling system for robot trajectory planning, the system comprising: a Simulink model, an external torque observer model and a time generator;

[0054] The Simulink model is used to obtain the original timestamp, and combined with the torque signal of the robot arm, obtain the timestamp of the trajectory after scaling;

[0055] The external torque observer model is used to obtain the desired torque and the desired angular velocity of the robot arm according to the timestamp and torque signal of trajectory scaling;

[0056] The time generator is used to obtain a scaling factor according to the expected torque and the expected angular velocity, so as to adjust the movement speed of the robot arm under different degrees of interference.

[0057] like Figure 1 As shown, the Simulink model includes: a clock module with a switch, a trajectory scaling module and a delay module;

[0058] The clock module with switch is used to receive current time information and obtain an original timestamp;

[0059] It should be noted that the Clock with Switch module in the present invention is used to discard the simulation time of tStart seconds before the simulation, so as to facilitate the robot arm to stabilize to the starting point.

[0060] The delay module is used to delay the torque signal to obtain a delayed torque signal;

[0061] The trajectory scaling module is used to process the original timestamp and the delayed torque signal to obtain the timestamp after trajectory scaling.

[0062] like Figure 2 As shown, the external torque observer model includes: an expected motion trajectory module, an inverse dynamics model and a low-pass filter;

[0063] The expected motion trajectory module is used to obtain the expected motion trajectory according to the current position p of the robot arm and the timestamp after trajectory scaling;

[0064] The inverse dynamics model is used to obtain the desired torque of the robot arm according to the desired motion trajectory;

[0065] The low-pass filter is used to filter the torque signal and the expected torque of the robotic arm respectively, and update the state of the robotic arm through the expected torque and the torque signal of the robotic arm to obtain the expected torque and expected angular velocity of the robotic arm.

[0066] The desired motion trajectory includes: the position p, velocity qd and acceleration dqd of the robot arm.

[0067] The present invention also provides a method for time scaling of robot arm trajectory planning, comprising the following steps:

[0068] S1. Get the original timestamp based on the Simulink model and combine it with the torque signal of the robot arm to get the timestamp of the trajectory after scaling.

[0069] S2, based on the timestamp and torque signal of trajectory scaling combined with the external torque observer model, obtain the expected torque and expected angular velocity;

[0070] S3. Based on the expected torque and expected angular velocity combined with the time generator model, the scaling factor is obtained to adjust the movement speed of the robot arm under different degrees of interference.

[0071] The original timestamp is obtained based on the Simulink model in step S1, and the timestamp after trajectory scaling is obtained based on the torque signal of the robot arm, specifically:

[0072] Use the clock module with a switch to receive the current time information and obtain the original timestamp;

[0073] The torque signal is delayed by using a delay module to obtain a delayed torque signal;

[0074] The trajectory scaling module is used to process the original timestamp and the delayed torque signal to obtain the timestamp after trajectory scaling.

[0075] It should be noted that: i-1 and Δt are known quantities stored internally during simulation. The scaled timestamp t is calculated by the following formula: i :

[0076] t i =t i-1 +Δt·s i

[0077] Among them, ti is the scaled timestamp, t i-1 is the timestamp of the previous step, Δt is the fixed time step when there is no scaling, and s i is the scaling factor.

[0078] In step S2, the external torque is obtained based on the original timestamp and the timestamp of the trajectory scaling combined with the external torque observation model, specifically:

[0079] The expected motion trajectory module is used to obtain the expected motion trajectory according to the current position p of the robot and the timestamp after trajectory scaling;

[0080] The inverse dynamics model is used to obtain the desired torque according to the desired motion trajectory;

[0081] The torque signal and the expected torque of the robot arm are filtered by a low-pass filter, and the state of the robot arm is updated by the expected torque and the torque signal of the robot arm respectively to obtain the expected torque and the expected angular velocity of the robot arm.

[0082] The desired torque is obtained based on the inverse dynamics model according to the desired motion trajectory, specifically:

[0083]

[0084] Among them, q d is the desired joint angle, is the desired angular velocity, is the desired joint angular acceleration, M is the mass matrix, C is the Coriolis and centrifugal effect matrix, and G is the gravity matrix.

[0085] It should be noted that in the external torque observation model, it is necessary to estimate the external torque To calculate f s (), where τ mea is the torque measured from the Simscape model, is the expected torque of the expected motion trajectory at the previous timestamp.

[0086] The external torque calculated by the external torque observer module and the desired angular velocity Input to the time generator. The time generator mainly uses the func_fs function to calculate the scaling factor f s ()parameter,

[0087] like Figure 3 As shown, in step S3, based on the expected torque and the expected angular velocity combined with the time generator model, a scaling factor is obtained to adjust the movement speed of the robot arm under different degrees of interference, specifically:

[0088] Based on the func_fs function, the expected angular velocity Perform normalization:

[0089]

[0090] The intermediate variable ψ is calculated based on the sensitivity constant α, where ψ represents the external torque. and the desired angular velocity The angle between them is as follows:

[0091]

[0092] According to the dead zone constant Γ and the reverse motion gain constant k, the value range of ψ is divided into four intervals, and the scaling factor f is defined in each interval s (ψ) to adjust the robot arm movement speed at different levels, the scaling factor f s The expression of (ψ) is as follows:

[0093]

[0094] In the above formula, when 0≤ψ<1: adjust the movement speed of the robot arm so that it moves slowly under small disturbance;

[0095] When 1≤ψ≤1+Γ: stop the robot arm from moving under medium disturbance;

[0096] When 1+Γ<ψ≤2+Γ: make the robot arm move in the opposite direction under large disturbance.

[0097] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A robot arm trajectory planning time scaling system, characterized in that: The system comprises: a Simulink model, an external torque observer model and a time generator; The Simulink model is used to obtain the original timestamp, and combined with the torque signal of the robot arm, obtain the timestamp of the trajectory after scaling; The external torque observer model is used to obtain the desired torque and the desired angular velocity of the robot arm according to the timestamp and torque signal of trajectory scaling; The time generator is used to obtain a scaling factor according to the expected torque and the expected angular velocity, so as to adjust the movement speed of the robot arm under different degrees of interference.

2. The robot arm trajectory planning time scaling system according to claim 1, characterized in that: The Simulink model includes: a clock module with a switch, a trajectory scaling module and a delay module; The clock module with switch is used to receive current time information and obtain an original timestamp; The delay module is used to delay the torque signal to obtain a delayed torque signal; The trajectory scaling module is used to process the original timestamp and the delayed torque signal to obtain the timestamp after trajectory scaling.

3. The robot arm trajectory planning time scaling system according to claim 2, characterized in that: The external torque observer model includes: an expected motion trajectory module, an inverse dynamics model and a low-pass filter; The expected motion trajectory module is used to obtain the expected motion trajectory according to the current position p of the robot arm and the timestamp after trajectory scaling; The inverse dynamics model is used to obtain the desired torque of the robot arm according to the desired motion trajectory; The low-pass filter is used to filter the torque signal and the expected torque of the robotic arm respectively, and update the state of the robotic arm through the expected torque and the torque signal of the robotic arm to obtain the expected torque and expected angular velocity of the robotic arm.

4. The robot arm trajectory planning time scaling system according to claim 3, characterized in that: The desired motion trajectory includes: the position p, velocity qd and acceleration dqd of the robot arm.

5. A robot arm trajectory planning time scaling method, applied to any robot arm trajectory planning time scaling system of claims 1-4, characterized in that: The following steps are involved: S1. Get the original timestamp based on the Simulink model and combine it with the torque signal of the robot arm to get the timestamp of the trajectory after scaling. S2, based on the timestamp and torque signal of trajectory scaling combined with the external torque observer model, obtain the expected torque and expected angular velocity; S3. Based on the expected torque and expected angular velocity combined with the time generator model, the scaling factor is obtained to adjust the movement speed of the robot arm under different degrees of interference.

6. The robot arm trajectory planning time scaling method according to claim 5, characterized in that: The original timestamp is obtained based on the Simulink model in step S1, and the timestamp after trajectory scaling is obtained based on the torque signal of the robot arm, specifically: Use the clock module with a switch to receive the current time information and obtain the original timestamp; The torque signal is delayed by using a delay module to obtain a delayed torque signal; The trajectory scaling module is used to process the original timestamp and the delayed torque signal to obtain the timestamp after trajectory scaling.

7. The method for time scaling of robot trajectory planning according to claim 4, characterized in that: In step S2, the external torque is obtained based on the original timestamp and the timestamp of the trajectory scaling combined with the external torque observation model, specifically: The expected motion trajectory module is used to obtain the expected motion trajectory according to the current position p of the robot and the timestamp after trajectory scaling; The inverse dynamics model is used to obtain the desired torque according to the desired motion trajectory; The torque signal and the expected torque of the robot arm are filtered by a low-pass filter, and the state of the robot arm is updated by the expected torque and the torque signal of the robot arm respectively to obtain the expected torque and the expected angular velocity of the robot arm.

8. The method for time scaling of robot trajectory planning according to claim 7, characterized in that: The desired torque is obtained based on the inverse dynamics model according to the desired motion trajectory, specifically: Among them, q d is the desired joint angle, is the desired angular velocity, is the desired joint angular acceleration, M is the mass matrix, C is the Coriolis and centrifugal effect matrix, and G is the gravity matrix.

9. The method for time scaling of robot trajectory planning according to claim 8, characterized in that: In step S3, based on the expected torque and the expected angular velocity combined with the time generator model, a scaling factor is obtained to adjust the movement speed of the robot arm under different degrees of interference, specifically: Based on the func_fs function, the expected angular velocity Perform normalization: The intermediate variable ψ is calculated based on the sensitivity constant α, where ψ represents the external torque. and the desired angular velocity The angle between them is as follows: According to the dead zone constant Γ and the reverse motion gain constant k, the value range of ψ is divided into four intervals, and the scaling factor f is defined in each interval s (ψ) to adjust the robot arm movement speed at different levels, the scaling factor f s The expression of (ψ) is as follows: In the above formula, when 0≤ψ<1: adjust the movement speed of the robot arm so that it moves slowly under small disturbance; When 1≤ψ≤1+Γ: stop the robot arm from moving under medium disturbance; When 1+Γ<ψ≤2+Γ: make the robot arm move in the opposite direction under large disturbance; When 2+Γ<ψ: make the robot arm move in the reverse direction at full speed at the speed determined by k.

Images