A time scaling system and method for robot arm trajectory planning

Abstract

The present invention discloses a time scaling system and method for robot arm trajectory planning. The system includes: a Simulink model, an external torque observer model and a time generator; the Simulink model is used to obtain the original timestamp and, in combination 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 expected torque and expected angular velocity of the robot arm based on the timestamp and torque signal of the trajectory scale; the time generator is used to obtain a scaling factor based on the expected torque and expected angular velocity, so as to adjust the movement speed of the robot arm under interference of different degrees. The present invention can 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 collisions, and uses feedback from the contact force sensor to scale or transform the expected position, velocity and acceleration.

Description

Technical Field

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

[0002] Manipulator trajectory planning is a core technology in manipulator control. Its purpose is to generate the motion trajectory of each manipulator joint based on a given task, enabling the manipulator to complete the task safely and efficiently. Traditional trajectory planning methods typically use polynomial interpolation to represent the manipulator's trajectory as a function of time. However, in practical applications, the manipulator's trajectory can be affected by factors such as the environment and the manipulator's own state, resulting in deviations from the desired trajectory and even collisions. Summary of the Invention

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

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

[0005] A time scaling system for robot arm trajectory planning, 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 manipulator, obtain the timestamp of the trajectory after scaling;

[0007] The external torque observer model is used to obtain the desired torque and desired angular velocity of the robotic arm based on the timestamp and torque signal of the 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 manipulator 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 a 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 a 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 robotic 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 robotic 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 trajectory scaled timestamp and torque signal combined with the external torque observer model, obtain the desired torque and desired angular velocity;

[0021] S3. Based on the expected torque and 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.

[0022] Furthermore, in step S1, the original timestamp is obtained based on the Simulink model, and the timestamp of the trajectory after 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 the trajectory scaled in combination with the external torque observation model, specifically:

[0027] Use the expected motion trajectory module to obtain the expected motion trajectory according to the current position p of the robot arm 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 manipulator are filtered by a low-pass filter, and the state of the manipulator is updated by the expected torque and the torque signal of the manipulator respectively to obtain the expected torque and the expected angular velocity of the manipulator.

[0030] Furthermore, the desired torque is obtained based on the desired motion trajectory based on the inverse dynamics model, 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 force 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 expected angular velocity of the func_fs function 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 (ψ), in order to adjust the movement speed of the robot arm 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 moderate interference;

[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 determined by k.

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

[0045] By using Simulink models and trajectory scaling technology, the present invention can more accurately control the motion trajectory of the robotic arm, thereby improving the accuracy of task execution; automatically adjust the motion speed of the robotic arm according to the degree of external interference, so that the robotic arm can maintain stable performance in different working environments; by monitoring and processing the torque signal in real time, the robotic arm can quickly respond to changes in the external torque, thereby dynamically adjusting the motion of the robotic 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 robotic arm and the environment, thereby reducing or avoiding damage caused by collisions, and use the feedback of the contact force sensor to scale or transform the expected position, speed and acceleration, so that the robotic arm can automatically decelerate or stop when it contacts an obstacle, thereby protecting the robotic 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 following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are merely embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any creative work.

[0047] The following further describes the time scaling system and method for robot trajectory planning in the present invention with reference to the accompanying drawings;

[0048] Figure 1 Schematic diagram of the Simulink model in the time scaling system for robot trajectory planning provided by the present invention;

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

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

[0051] The following embodiments of the present invention are described in further detail with reference to 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 with reference to 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 manipulator, obtain the timestamp of the trajectory after scaling;

[0055] The external torque observer model is used to obtain the desired torque and desired angular velocity of the robotic arm based on the timestamp and torque signal of the 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 manipulator 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 a 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 that the robot arm can stabilize at 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 a 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 robotic 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 robotic 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 trajectory scaled timestamp and torque signal combined with the external torque observer model, obtain the desired torque and desired angular velocity;

[0070] S3. Based on the expected torque and 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.

[0071] In step S1, the original timestamp is obtained based on the Simulink model, and the timestamp of the trajectory after scaling is obtained based on the torque signal of the manipulator, 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 scaled by combining the external torque observation model, specifically:

[0079] Use the expected motion trajectory module to obtain the expected motion trajectory according to the current position p of the robot arm 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 manipulator are filtered by a low-pass filter, and the state of the manipulator is updated by the expected torque and the torque signal of the manipulator respectively to obtain the expected torque and the expected angular velocity of the manipulator.

[0082] The desired torque is obtained based on the desired motion trajectory based on the inverse dynamics model, 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 force 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 desired motion trajectory at the previous timestamp.

[0086] 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 expected angular velocity of the func_fs function 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 (ψ), in order to adjust the movement speed of the robot arm 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 moderate interference;

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

[0097] The above description of the disclosed embodiments is intended to enable one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily 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 is not limited to the embodiments shown herein but is intended to conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A time scaling system for robot trajectory planning, characterized in that: The system includes: 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 manipulator, obtain the timestamp of the trajectory after scaling; The Simulink model includes: a clock module with a switch, a trajectory scaling module and a delay module; The clock module with a switch is used to receive current time information and obtain an original timestamp; The delay module is used to delay the torque signal of the robotic arm to obtain a delayed torque signal; The trajectory scaling module is used to process the original timestamp and the delayed torque signal to obtain a timestamp after trajectory scaling; The external torque observer model is used to obtain the desired torque and desired angular velocity of the manipulator according to the timestamp and torque signal after trajectory scaling; 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 of the robot arm and the timestamp after trajectory scaling; The inverse dynamics model is used to obtain the desired torque of the robotic arm according to the desired motion trajectory; The low-pass filter is used to filter the torque signal and the desired torque of the robotic arm respectively, and update the state of the robotic arm according to the desired torque and the torque signal of the robotic arm to obtain the desired torque and the desired angular velocity of the robotic arm; 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 manipulator under different degrees of interference.

2. The robot arm trajectory planning time scaling system according to claim 1, characterized in that: The desired motion trajectory includes: the position, velocity and acceleration of the robotic arm.

3. A time scaling method for robot trajectory planning, applied to the time scaling system for robot trajectory planning according to any one of claims 1-2, characterized in that: The following steps are involved: S1. Based on the Simulink model, the original timestamp is obtained, and combined with the torque signal of the robot arm, the timestamp of the trajectory after scaling is obtained, specifically: Use the clock module with a switch to receive the current time information and obtain the original timestamp; The delay module is used to delay the torque signal of the robot arm 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; S2. Based on the timestamp and torque signal after trajectory scaling and combined with the external torque observer model, the desired torque and desired angular velocity of the manipulator are obtained, specifically: Use the expected motion trajectory module to obtain the expected motion trajectory based on the current position of the robot arm and the timestamp after trajectory scaling; The inverse dynamics model is used to obtain the desired torque of the manipulator according to the desired motion trajectory; The torque signal and the desired torque of the manipulator are filtered respectively by a low-pass filter, and the state of the manipulator is updated according to the desired torque and the torque signal of the manipulator to obtain the desired torque and the desired angular velocity of the manipulator; S3. Based on the expected torque and expected angular velocity combined with the time generator model, a scaling factor is obtained to adjust the movement speed of the manipulator under different degrees of interference.

4. The time scaling method for robot arm trajectory planning according to claim 3, characterized in that: The inverse dynamics model is used to obtain the desired torque of the robotic arm according to the desired motion trajectory, specifically: ， in, 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 force effect matrix, and G is the gravity matrix.

5. The time scaling method for robot arm trajectory planning according to claim 3, characterized in that: In S3, based on the expected torque and expected angular velocity combined with the time generator model, a scaling factor is obtained to adjust the movement speed of the manipulator under different degrees of interference, specifically: Based on the expected angular velocity of the func_fs function Perform normalization: ， According to the sensitivity constant Calculating intermediate variables , Represents 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 is defined in each interval , in order to achieve the adjustment of the robot arm movement speed at different levels, the scaling factor The expression is as follows: ， In the above formula, when Time: Adjust the movement speed of the robot arm so that it moves slowly under small disturbances; when When: the robot arm stops moving under medium interference; when When: Make the robot arm move in the opposite direction under large disturbance; when When: Make the robot arm move in the reverse direction at full speed determined by k.

Images