Joint space trajectory planning method for robot arm based on model prediction path integral

Abstract

The application belongs to the technical field of mechanical arm joint space trajectory planning, and aims to provide a mechanical arm joint space trajectory planning method based on model prediction path integral, so as to realize mechanical arm joint space trajectory optimization planning. The method comprises the following steps: firstly, the joint configuration of the mechanical arm is utilized, the configuration symbolic distance function (CSDF) is utilized according to the configuration of the mechanical arm and the point cloud of the scene, and the estimation value of the distance and gradient of the whole mechanical arm body to the nearby obstacle is obtained; then, a fast, online and global trajectory generator is constructed based on the model prediction path integral method, the initial and target mechanical arm configurations are received, the collision cost of the current mechanical arm configuration space is estimated according to the estimation value of the distance and gradient of the whole mechanical arm body to the nearby obstacle, and a globally optimal trajectory is generated for the mechanical arm to execute. The application is mainly applied to mechanical arm design and manufacturing occasions.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm joint spatial trajectory planning technology, specifically involving rapid calculation of spatial distances of robotic arm configurations and methods for generating spatial trajectories of robotic arm joints. In particular, it relates to a robotic arm joint spatial trajectory planning method based on model-predicted path integrals. Background Technology

[0002] Robotic arms are the most widely used automated mechanical devices in the field of robotics. As complex systems characterized by high precision, multiple inputs and outputs, high nonlinearity, and strong coupling, their unique operational flexibility has led to their widespread use in performing various tasks. They have played a significant role in fields such as industrial manufacturing, medical treatment, entertainment services, military, semiconductor manufacturing, and space exploration, becoming an indispensable part of completing complex tasks in industry. Motion planning is one of the necessary conditions for the normal operation of robotic arms. Excellent planning algorithms will make the operation of robotic arms smoother and more efficient, meeting the needs of various task scenarios. Therefore, in order to obtain better working performance, motion planning for such systems has always been one of the key areas of robotics research. Developing efficient motion planning algorithms to generate safe and smooth motion trajectories is crucial for robotic arms to complete tasks efficiently.

[0003] Trajectory planning for robotic arms can be categorized into two types based on the object of planning: joint space trajectory planning and Cartesian space trajectory planning. Joint space trajectory planning obtains the desired trajectory by establishing a function of joint angle variables with respect to time. This method has low computational cost and avoids situations where no solution exists, but it cannot directly control the trajectory of the end effector, making it less than ideal in practical operation. Cartesian space trajectory planning, on the other hand, first determines the trajectory of the end effector and then obtains the relationship between the corresponding joint variables and time variables through inverse kinematics. This method requires extensive inverse kinematics calculations, resulting in a large computational load and potentially leading to singular joint angles, making it impossible to plan an executable trajectory. Both methods have their advantages and disadvantages, and the choice for robotic arm trajectory planning needs to be made based on the specific circumstances. Classical motion planning algorithms, such as the artificial potential field method and sampling-based and bio-inspired heuristics, have been widely used to solve motion planning problems. However, most of these methods fail in highly dynamic, high-dimensional configuration spaces, suffer from high computational cost and slow convergence speed, and are not conducive to real-time implementation. Extensive research has been conducted by scholars both domestically and internationally on the trajectory planning problem for robotic arms. In 2017, Nguyen et al. proposed a fast heuristic planning method to enhance real-time control of robotic arms, but the planned trajectory accuracy was low. Also in 2017, Walch et al. used quadratic spline curves for interpolation planning, calculating the parabolic trajectory, and the generated trajectory satisfied the constraints. In 2019, Li et al. restricted joint motion velocity and acceleration to reduce joint jitter, proposing a smooth linear trajectory planning algorithm. In 2018, Huang et al. proposed an acceleration-optimal trajectory planning method, first using fifth-order B-splines for trajectory interpolation in joint space, resulting in trajectories that satisfied continuity and kinematic constraints. In 2021, Ma et al. formulated time-optimal trajectory planning as a convex optimization problem, considering both acceleration and torque constraints, effectively suppressing acceleration abrupt changes and thus making the velocity curve smoother.

[0004] In summary, scholars both domestically and internationally have conducted fruitful research on the trajectory planning problem for robotic arms, achieving abundant research results. Despite significant progress in trajectory planning, existing planning methods for reactive robotic arm motion struggle to provide reliable solutions in unknown environments. Traditional planning methods, such as the artificial potential field method or the navigation function method, can typically update the trajectory quickly to avoid obstacles; however, because these methods rely on gradient calculation, they are susceptible to local minima and cannot obtain the optimal trajectory solution. Therefore, this invention proposes a trajectory planning method based on model-predicted path integrals. This method samples the joint space of the robotic arm, guiding it away from local minima caused by obstacles, thus achieving safe, smooth, and rapid generation of the robotic arm's joint space trajectory. Summary of the Invention

[0005] To overcome the shortcomings of existing technologies, this invention aims to propose a robotic arm joint space trajectory planning method based on Model Predictive Path Integral (MPPI) to achieve optimized planning of robotic arm joint space trajectories. The technical solution adopted by this invention is a robotic arm joint space trajectory planning method based on MPPI, with the following steps: First, using the joint configuration of the robotic arm, based on the configuration of the robotic arm and the point cloud of the scene, the estimated values ​​of the distance and gradient of the entire robotic arm body to nearby obstacles are obtained using the Configuration Signed Distance Function (CSDF). Then, based on the MPPI method, a fast, online, and global trajectory generator is constructed. This generator receives the starting and target robotic arm configurations, estimates the collision cost in the current robotic arm configuration space based on the estimated values ​​of the distance and gradient of the entire robotic arm body to nearby obstacles, and generates a globally optimal trajectory for the robotic arm to execute.

[0006] The specific steps are as follows:

[0007] Part 1: Methods for Calculating Spatial Distances in Robotic Arm Configurations

[0008] The method of generating control points on the surface of the robotic arm determines whether the robotic arm will collide with the surrounding environment by calculating the configuration space symbolic distance function between the generated control points and the point cloud of the surrounding environment. At the same time, the obtained data can be provided to the planner based on the model prediction path integral in the second part to generate a collision-free trajectory.

[0009] The point cloud is composed of sampling points from the surface triangular mesh of an object with a known geometric shape and point clouds acquired by a depth camera. The acquired point cloud is first downsampled and denoised to reduce the number of points and increase their accuracy. The processed point cloud is then unified into a base coordinate system through coordinate transformation, resulting in a point cloud {s} representing the current working scene of the robotic arm, consisting of S points. k ∈R 3} k＝1,…,S

[0010] The current configuration q of the robotic arm i Given the values ​​{θ1, θ2, θ3, θ4, θ5, θ6, θ7}, where θ1-θ7 are the angle values ​​of the seven joints, the current pose of the robotic arm is uniquely determined. To quickly calculate the CSDF value of the current pose, some points need to be sampled on the surface of the robotic arm. The distance between the robotic arm and surrounding obstacles is indirectly estimated by using the sampled points. First, a set of points coinciding with the joints of the robotic arm are selected. The pose of the given robotic arm configuration in the workspace is calculated using GPU-accelerated forward kinematics. Then, linear interpolation is performed between the positions of the selected points to obtain the current configuration q. i A set of C sampling points {cj (q i )∈R 3} j＝1,...,C If a robotic arm is used to perform a grasping task, obstacle avoidance by the end effector is crucial during the grasping process. Therefore, if the geometry of the object to be grasped is known, samples are taken from the object's surface. After obtaining the sampling points, a forward kinematics transformation is performed, and these points are added to the sampling points on the robotic arm itself. This method yields the robotic arm surface sampling points {c}. j (q i )∈R 3} j＝1,...,C and work scene point cloud {s k ∈R 3} k＝1,...S After this, the distance between the robotic arm and the environmental point cloud is estimated. The estimation steps are as follows:

[0011] First, define any point x∈R in the space. 3 The symbolic distance function value (SDF) is:

[0012]

[0013] Where, ρ∈R + It is a very small normal value, ensuring the smoothness of the entire set at the zero point;

[0014] Then, the CSDF module is used to estimate the minimum distance between the current robotic arm configuration and the environmental point cloud. Within the robotic arm's workspace, given a configuration q... i The sampling points {c} on the surface of the robotic arm are obtained under the condition :={θ1,θ2,θ3,θ4,θ5,θ6,θ7}. j (q i )∈R 3} j＝1,...,C The configurational sign distance function is defined as:

[0015]

[0016] Where r∈R + This is a safety threshold. This module can obtain the distance query between the current robotic arm configuration and the environmental point cloud and provide it to the planner for trajectory generation. A positive CSDF indicates that the current robotic arm configuration will not collide with the environment, while a negative CSDF indicates that the robotic arm will collide with the environment.

[0017] The second part describes the method for generating the trajectory in the joint space of the robotic arm. Based on the distance between the robotic arm configuration and the environmental point cloud obtained from the CSDF module, a trajectory generation method based on MPPI is used to solve the trajectory in the joint space of the robotic arm without collision. The dynamic model of the robotic arm is used to predict the dynamics of the robotic arm in the future. Multiple control input sequences are generated by sampling, and these sequences are used to predict multiple future state trajectories. Each sampled trajectory is evaluated according to a certain cost function. The trajectory with lower weight / higher cost has less impact on the final control input. Then, the trajectories are weighted and averaged to obtain the final control input. In each iteration, the new sampled trajectory is adjusted based on the result of the previous iteration, thereby gradually optimizing the control input sequence and approaching the optimal solution.

[0018] The MPPI model is a discrete-time continuous-state system q t+1 =q t +e t q t and q t+1 Let e ​​represent the configuration of the robotic arm at times t and t+1, respectively. t ~N(d) t ,Σ),e t This indicates that the mean is d. t Given a Gaussian distributed sequence with covariance Σ, in each iteration, the control domain is H and the nominal configuration is q0,...,q H The corresponding nominal displacements d0,...,d H-1 M displacement sequences E were obtained by sampling. j :={e j,0 ,...,e j,H-1},j=1,...,M, by performing forward calculations on the system model using the sampled results, the rolling cost C(E) of each displacement sequence is obtained. j Then, the M rolling costs are assigned certain weights to obtain their weighted average, and finally the displacement sequence obtained at time t is obtained.

[0019]

[0020] Where α d e is a smoothing parameter j,t E represents j The value at time t, and the definitions of other relevant parameters are as follows:

[0021]

[0022]

[0023] Where λ > 0 is a smoothing coefficient;

[0024] After each iteration, the displacement sequence is calculated by weighting. Update the current nominal configuration The updated configuration will be sent to the trajectory actuator to drive the robotic arm to move along the planned trajectory;

[0025] For the rolling cost C(E) j It can be divided into path constraints C. r (E j ) and breaking constraint C terminal (E j The specific forms of each constraint are as follows:

[0026] (1) Path constraint C r (E j )

[0027] Path constraint C r (E j It is mainly used to penalize the total length C of the trajectory. lenth And the collision between the robotic arm and the environment C coll Defined as C r (E j ):=C lenth (E j )+C coll (E j ), where C lenth (E j C represents the total length of the currently sampled trajectory of the robotic arm. coll (E j () is a distance-related value, and the mathematical definitions of its components are as follows:

[0028]

[0029]

[0030] Where, ω lenth ,ω coll >0 is the adjustment coefficient, and the collision cost C coll (E j ) Calculated by the CSDF module in Part 1:

[0031]

[0032] Where δ is the distance threshold, and when the distance value is less than or equal to δ, C coll (q j,t ,e j,t The distance is defined as a constant value of 1; conversely, the larger the distance value, the lower the probability of the robotic arm colliding with the environmental point cloud. coll (qj,t ,e j,t The smaller the value, the better;

[0033] (2) Breaking constraint C terminal (E j )

[0034] The purpose of the break constraint is to penalize the distance between the robotic arm and the final target point. The break constraint is defined as the robotic arm configuration q obtained by the model output from the currently sampled control sequence. j,H With target configuration q goal The distance is mathematically defined as follows:

[0035] C terminal (E j ):=ω terminal ||q j,H -q goal || (9)

[0036] Where, ω terminal This is the adjustment coefficient.

[0037] Trajectory execution is achieved through a joint trajectory controller in ROS. The controller, based on the target position, velocity, and acceleration of the specified joint, optimizes and generates a smoother trajectory. It interpolates over time at each given path point, enabling the robotic arm to perform continuous, smooth, and collision-free motion along these estimates. It also enables simultaneous control of multiple joints of the robotic arm, thereby improving the robot's performance and efficiency in various tasks.

[0038] The features and beneficial effects of this invention are:

[0039] This invention is of great significance to the research on joint space trajectory planning algorithms for multi-degree-of-freedom robotic arms. When facing unknown, multi-obstacle environments, this invention can achieve fully autonomous collision distance estimation and globally optimal trajectory generation for the robotic arm, and the generated trajectory is safe and efficient. This invention has high theoretical and engineering value, and its advantages are summarized as follows:

[0040] (1) This invention proposes a collision distance estimation algorithm for robotic arms based on the configuration sign distance function. Traditional bounding box collision detection algorithms use simple bounding box models to approximate the geometry of the robotic arm and perform collision detection by detecting the intersection relationships between bounding boxes. Although simple and efficient, this approach cannot handle complex geometries and contact constraints. Compared to traditional algorithms, the method proposed in this invention is based on scene point clouds and sampling points of the robotic arm, achieving fast and accurate collision detection results.

[0041] (2) This invention designs a multi-degree-of-freedom robotic arm trajectory generation algorithm based on model prediction path integral, which can generate the globally optimal robotic arm joint space trajectory. It integrates perception constraints and motion optimization objectives, and samples multiple control sequences around the nominal sequence and uses the system model for propagation. During the propagation process, according to the pre-set cost function, the costs of the sampled multiple control sequences are weighted and the weighted average value is obtained, which is used to generate the nominal control sequence for the next iteration and the optimal trajectory generated in the current iteration, thus ensuring the safety, speed and efficiency of the multi-degree-of-freedom robotic arm in the motion planning process.

[0042] (3) The program framework of the method proposed in this invention can be processed by CPU and GPU at the same time, which can maximize the use of the onboard computer containing these two processors, save computing resources for other programs (such as capture tasks) that need to be run to complete other tasks in actual work scenarios, ensure the running frequency of the planner, and ensure the real-time performance of planning. Attached image description:

[0043] Figure 1 : Overall structure diagram of robotic arm joint spatial trajectory planning based on model-predicted path integral.

[0044] Figure 2 : Structure diagram of a trajectory generator based on model-predicted path integral.

[0045] Figure 3 : Schematic diagram of surface sampling point generation for robotic arm.

[0046] Figure 4 : A schematic diagram of a virtual robotic arm working in the Gazebo simulation environment.

[0047] Figure 5 The trajectory of a virtual robotic arm in the Gazebo simulation environment. Detailed Implementation

[0048] This invention aims to propose a robotic arm joint space trajectory planning method based on Model Predictive Path Integral (MPPI). In the proposed method, a trajectory generator based on MPPI guides a globally optimal path, and trajectory commands are executed through a joint trajectory controller, generating real-time, safe, and smooth motion while satisfying constraints. First, to enable the robotic arm to plan a collision-free trajectory, this invention proposes a Configuration Signed Distance Function (CSDF) module, which directly uses the joint configuration of the robotic arm for calculation. This module receives the robot's configuration and the point cloud of the scene, and outputs the estimated distances and gradients from the entire robotic arm body to nearby obstacles. CSDF is used for fast collision checking by the trajectory generator; this module uses GPU acceleration to quickly calculate the distance between the current robotic arm configuration and obstacles. Second, based on the MPPI method, this invention constructs a fast, online, and global trajectory generator. It receives the initial and target robotic arm configurations, estimates the collision cost in the current robotic arm configuration space using the CSDF module, and generates a globally optimal trajectory for the robotic arm to execute. Furthermore, through a joint trajectory controller based on the Robot Operating System (ROS), instructions are executed. This module sends smooth, collision-free joint space trajectory instructions to the robotic arm, which then moves along the planned joint space trajectory. This approach can be used in various task scenarios and has significant application value. The specific steps are as follows:

[0049] Part 1: Spatial Distance Calculation Method for Robotic Arm Configuration: When planning the joint spatial trajectory of a robotic arm, it is necessary to consider the distance between the robotic body and surrounding obstacles in order to generate a collision-free path and avoid collisions with the surrounding environment during movement.

[0050] As a type of multi-jointed serial robot, the robotic arm is composed of links and joints connected sequentially from its base, making it difficult to simplify into a single point mass. To calculate the distance between the robotic arm and surrounding obstacles, surface processing of the robotic arm is necessary. This invention proposes a method for generating control points on the surface of the robotic arm. By calculating the configurational space sign distance function between the generated control points and the surrounding environment's point cloud, it is determined whether the robotic arm will collide with the environment. Simultaneously, the obtained data can be provided to a planner based on model-predicted path integrals in the second part to generate collision-free trajectories.

[0051] For robotic arm systems equipped with depth camera sensors constrained by limited field of view, this invention proposes a configuration space distance calculation method based on point clouds. For obstacle avoidance purposes, the robotic arm's working scene is considered as a large point cloud. This point cloud is composed of sampling points from surface triangular meshes of objects with known geometric shapes and point clouds acquired by the depth camera. The acquired point cloud is first downsampled and denoised to reduce the number of points and increase their accuracy, thereby improving computational efficiency and making trajectory generation more efficient. The processed point cloud is then unified into a base coordinate system through coordinate transformation, resulting in a point cloud {s} representing the current working scene of the robotic arm, consisting of S points. k ∈R 3} k＝1,...,S

[0052] Assume the current configuration of the robotic arm is q i Given the values ​​:={θ1,θ2,θ3,θ4,θ5,θ6,θ7} (where θ1-θ7 are the angle values ​​of the seven joints), the current pose of the robotic arm can be uniquely determined. To quickly calculate the CSDF value of the current pose, some points need to be sampled on the surface of the robotic arm. The distance between the robotic arm and surrounding obstacles is indirectly estimated using these sampled points. First, a set of points coinciding with the joints of the robotic arm is selected, and the pose of the given robotic arm configuration in the workspace is calculated using GPU-accelerated forward kinematics. Then, linear interpolation is performed between the positions of the selected points to obtain the current configuration q. i A set of C sampling points {c j (q i )∈R 3} j＝1,...,C If a robotic arm is used to perform tasks such as grasping, obstacle avoidance by the end effector is crucial during the grasping process. Therefore, if the geometry of the object to be grasped is known, samples can be taken from the object's surface. After obtaining the sampling points, a forward kinematics transformation is performed, and these points are added to the sampling points on the robotic arm itself. The above method is used to obtain the sampling points {c} on the robotic arm surface. j (q i )∈R 3} j＝1,...,C and work scene point cloud {s k ∈R 3} k＝1,...S After that, the distance between the robotic arm and the environmental point cloud is estimated.

[0053] First, define any point x∈R in the space. 3 The Signed Distance Function (SDF) value is:

[0054]

[0055] Where, ρ∈R+ It is a very small normal value, ensuring the smoothness of the entire set at the zero point. Parallel processing via GPU allows for rapid querying of the distance between any point in space and the environment point cloud.

[0056] Then, the CSDF module is used to estimate the minimum distance between the current robotic arm configuration and the environmental point cloud. Within the robotic arm's workspace, given a configuration q... i The sampling points {c} on the surface of the robotic arm are obtained under the condition :={θ1,θ2,θ3,θ4,θ5,θ6,θ7}. j (q i )∈R 3} j＝1,...,C The configurational sign distance function is defined as:

[0057]

[0058] Where r∈R + This is the safety threshold. This module can obtain the distance query between the current robotic arm configuration and the environmental point cloud and provide it to the planner for trajectory generation. A positive CSDF indicates that the current robotic arm configuration will not collide with the environment, while a negative CSDF indicates that the robotic arm will collide with the environment.

[0059] The second part describes a method for generating the trajectory in the joint space of the robotic arm. Based on the distance between the robotic arm configuration and the environmental point cloud obtained from the CSDF module, this invention proposes a trajectory generation method based on MPPI to solve for collision-free trajectories in the joint space of the robotic arm. The core idea of ​​this method is to use the system's dynamic model to predict the system dynamics over a future period. Multiple control input sequences are generated through sampling, and these sequences are used to predict multiple future state trajectories. Each sampled trajectory is evaluated according to a certain cost function; trajectories with lower weights (higher costs) have less impact on the final control input. The trajectories are then weighted and averaged to obtain the final control input. In each iteration, the new sampled trajectory is adjusted based on the results of the previous iteration, thereby gradually optimizing the control input sequence and approaching the optimal solution. Compared to traditional methods, this method can be used in scenarios with high real-time requirements. Through extensive random sampling, it is less prone to getting trapped in local optima, has some global optimization potential, and can handle complex nonlinear systems.

[0060] The MPPI model used in this invention is a discrete-time continuous-state system q t+1 =q t +e t q t and q t+1 Let e ​​represent the configuration of the robotic arm at times t and t+1, respectively. t ~N(d) t ,Σ),e t This indicates that the mean is d.t The sequence is a Gaussian distribution with covariance Σ. Based on the core idea of ​​MPPI, in each iteration, given the control domain H and the nominal configuration q0,...,q... H The corresponding nominal displacements d0,...,d H-1 M displacement sequences E were obtained by sampling. j :={e j,0 ,...,e j,H-1},j=1,...,M. The rolling cost C(E) of each displacement sequence is obtained by forward calculation of the sampled results through the system model. j Then, the M rolling costs are assigned certain weights to obtain their weighted average, and finally the displacement sequence obtained at time t is obtained.

[0061]

[0062] Where α d e is a smoothing parameter j,t E represents j The value at time t, and the definitions of other relevant parameters are as follows:

[0063]

[0064]

[0065] Where λ > 0 is a smoothing coefficient.

[0066] After each iteration, the displacement sequence is calculated by weighting. Update the current nominal configuration The updated configuration will be sent to the trajectory actuator to drive the robotic arm to move along the planned trajectory.

[0067] For the rolling cost C(E) j It can be divided into path constraints C. r (E j ) and breaking constraint C terminal (E j The specific forms of each constraint are as follows:

[0068] (1) Path constraint C r (E j )

[0069] Path constraint C r (E j It is mainly used to penalize the total length C of the trajectory. lenth And the collision between the robotic arm and the environment C coll Defined as C r (E j):=C lenth (E j )+C coll (E j ), where C lenth (E j C represents the total length of the currently sampled trajectory of the robotic arm. coll (E j () is a distance-related value, and the mathematical definitions of its components are as follows:

[0070]

[0071]

[0072] Where, ω lenth ,ω coll >0 is the adjustment coefficient, and the collision cost C coll (E j ) Calculated by the CSDF module in Part 1:

[0073]

[0074] Where δ is the distance threshold, and when the distance value is less than or equal to δ, C coll (q j,t ,e j,t The distance is defined as a constant value of 1; conversely, the larger the distance value, the lower the probability of the robotic arm colliding with the environmental point cloud. coll (q j,t ,e j,t The smaller the value, the better.

[0075] (2) Breaking constraint C terminal (E j )

[0076] The purpose of the break constraint is to penalize the distance between the robotic arm and the final target point. The break constraint is defined as the robotic arm configuration q obtained by the model output from the currently sampled control sequence. j,H With target configuration q goal The distance is mathematically defined as follows:

[0077] C terminal (E j ):=ω terminal ||q j,H -q goal || (18)

[0078] Where, ω terminal This is the adjustment coefficient.

[0079] This invention uses MPPI as a globally optimal trajectory generator to plan the joint space trajectory of a robotic arm. The planned path points are then executed by a controller. The generated collision-free joint space trajectory is received and executed through a joint trajectory controller in ROS. The controller interpolates at each given path point over time, resulting in continuous, smooth, and collision-free movement of the robotic arm. The ROS-based joint trajectory controller accepts the target position, velocity, and acceleration of specified joints to optimize and generate a smoother trajectory, instructing the robotic arm to move along these estimates. Using the joint trajectory controller, multiple joints of the robotic arm can be controlled simultaneously, thereby improving the robot's performance and efficiency in various tasks. When configuring the controller, the parameters of the joint trajectory controller need to be defined in the ROS controller configuration file, including the controlled joint name, type, PID control parameters, etc. Since this algorithm is verified in the Gazebo simulation environment, the relevant hardware interface files also need to be configured. Finally, the simulation and controller are started using the relevant startup files to ensure correct connection between them and the robotic arm's hardware interface.

[0080] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0081] This invention proposes a method for generating the trajectory of a multi-degree-of-freedom robotic arm based on model-predicted path integrals, which mainly consists of a simulation environment and an algorithm.

[0082] The simulation environment was built using Gazebo. Gazebo is a powerful 3D robot simulation software with excellent physical simulation performance. Its main features include: (1) Realistic dynamic simulation: It supports a variety of high-performance physics engines, such as DART, ODE, SimBody, Bullet, etc.; (2) Realistic 3D visualization effects: Gazebo uses OGRE (an open-source graphics rendering engine), which can achieve good visual effects such as light, shadow, and materials. (3) Sensors and plugins: It supports a variety of virtual sensors such as depth cameras, and allows users to design and build sensors, robot models, and environmental controls independently through API plugins.

[0083] This invention utilizes a simulation environment built with Gazebo, which is a virtual robotic arm working scenario. The algorithm mainly consists of two parts: calculation of spatial distances in the multi-degree-of-freedom robotic arm configuration and generation of spatial trajectories for the robotic arm joints. These will be described in detail below:

[0084] The overall structure diagram of the multi-degree-of-freedom robotic arm trajectory generation method based on model-predicted path integral is attached. Figure 1As shown. All algorithms in this invention are written in Python and developed under the ROS robot operating system on a Linux operating system. The method designed in this invention mainly includes the following two parts:

[0085] Part 1: Spatial Distance Calculation Method for Robotic Arm Configuration: When planning the joint spatial trajectory of a robotic arm, it is necessary to consider the distance between the robotic body and surrounding obstacles in order to generate a collision-free path and avoid collisions with the surrounding environment during movement.

[0086] As a type of multi-jointed serial robot, the robotic arm is composed of links and joints connected sequentially from its base, making it difficult to simplify into a single point mass. To calculate the distance between the robotic arm and surrounding obstacles, surface processing of the robotic arm is necessary. This invention proposes a method for generating control points on the surface of the robotic arm. By calculating the configurational space sign distance function between the generated control points and the surrounding environment's point cloud, it is determined whether the robotic arm will collide with the environment. Simultaneously, the obtained data can be provided to a planner based on model-predicted path integrals in the second part to generate collision-free trajectories.

[0087] For robotic arm systems equipped with depth camera sensors constrained by limited field of view, this invention proposes a configuration space distance calculation method based on point clouds. For obstacle avoidance purposes, the robotic arm's working scene is considered as a large point cloud. This point cloud is composed of sampling points from surface triangular meshes of objects with known geometric shapes and point clouds acquired by the depth camera. The acquired point cloud is first downsampled and denoised to reduce the number of points and increase their accuracy, thereby improving computational efficiency and making trajectory generation more efficient. The processed point cloud is then unified into a base coordinate system through coordinate transformation, resulting in a point cloud {s} representing the current working scene of the robotic arm, consisting of S points. k ∈R 3} k＝1,...,S

[0088] Assume the current configuration of the robotic arm is q i Given the values ​​:={θ1,θ2,θ3,θ4,θ5,θ6,θ7} (where θ1-θ7 are the angle values ​​of the seven joints), the current pose of the robotic arm can be uniquely determined. To quickly calculate the CSDF value of the current pose, some points need to be sampled on the surface of the robotic arm. The distance between the robotic arm and surrounding obstacles is indirectly estimated using these sampled points. First, a set of points coinciding with the joints of the robotic arm is selected, and the pose of the given robotic arm configuration in the workspace is calculated using GPU-accelerated forward kinematics. Then, linear interpolation is performed between the positions of the selected points to obtain the current configuration q. i A set of C sampling points {c j (q i )∈R 3}j＝1,...,C As attached Figure 3 As shown. If the robotic arm is used to perform tasks such as grasping, obstacle avoidance by the end effector is crucial during the grasping process. Therefore, if the geometry of the object to be grasped is known, samples can be taken from the object's surface. After obtaining the sampling points, a forward kinematics transformation is performed, and these points are added to the sampling points on the robotic arm body. The above method is used to obtain the sampling points {c} on the robotic arm surface. j (q i )∈R 3} j＝1,...,C and work scene point cloud {s k ∈R 3} k＝1,...S After that, the distance between the robotic arm and the environmental point cloud is estimated.

[0089] First, define any point x∈R in the space. 3 The symbolic distance function value (SDF) is:

[0090]

[0091] Where, ρ∈R + It is a very small normal value, ensuring the smoothness of the entire set at the zero point. Parallel processing via GPU can quickly complete the distance query between a point in space and the environment point cloud.

[0092] Then, the CSDF module is used to estimate the minimum distance between the current robot arm configuration and the environment point cloud. Within the robot arm's workspace, given a configuration q... i The sampling points {c} on the surface of the robotic arm are obtained under the condition :={θ1,θ2,θ3,θ4,θ5,θ6,θ7}. j (q i )∈R 3} j＝1,...,C The configurational sign distance function is defined as:

[0093]

[0094] Where r∈R + This is a safety threshold. This module can obtain the distance query between the current robotic arm configuration and the environmental point cloud and provide it to the planner for trajectory generation. A positive CSDF indicates that the current robotic arm configuration will not collide with the environment, while a negative CSDF indicates that the robotic arm will collide with the environment.

[0095] The second part describes a method for generating the trajectory in the joint space of the robotic arm. Based on the distance between the robotic arm configuration and the environmental point cloud obtained from the CSDF module, this invention proposes a trajectory generation method based on MPPI to solve for collision-free trajectories in the joint space of the robotic arm. The core idea of ​​this method is to use the system's dynamic model to predict the system dynamics over a future period. Multiple control input sequences are generated through sampling, and these sequences are used to predict multiple future state trajectories. Each sampled trajectory is evaluated according to a certain cost function; trajectories with lower weights (higher costs) have less impact on the final control input. The trajectories are then weighted and averaged to obtain the final control input. In each iteration, the new sampled trajectory is adjusted based on the results of the previous iteration, thereby gradually optimizing the control input sequence and approaching the optimal solution. Compared to traditional methods, this method can be used in scenarios with high real-time requirements. Through extensive random sampling, it is less prone to getting trapped in local optima, has some global optimization potential, and can handle complex nonlinear systems.

[0096] The MPPI model used in this invention is a discrete-time continuous-state system q t+1 =q t +e t q t and q t+1 Let e ​​represent the configuration of the robotic arm at times t and t+1, respectively. t ~N(d) t ,Σ),e t This indicates that the mean is d. t The sequence is a Gaussian distribution with covariance Σ. Based on the core idea of ​​MPPI, in each iteration, given the control domain H and the nominal configuration q0,...,q... H The corresponding nominal displacements d0,...,d H-1 M displacement sequences E were obtained by sampling. j :={e j,0 ,...,e j,H-1},j=1,...,M. The rolling cost C(E) of each displacement sequence is obtained by forward calculation of the sampled results through the system model. j Then, the M rolling costs are assigned certain weights to obtain their weighted average, and finally the displacement sequence obtained at time t is obtained.

[0097]

[0098] Where α d e is a smoothing parameter j,t E represents j The value at time t, and the definitions of other relevant parameters are as follows:

[0099]

[0100]

[0101] Where λ > 0 is a smoothing coefficient.

[0102] After each iteration, the displacement sequence is calculated by weighting. Update the current nominal configuration The updated configuration will be sent to the trajectory actuator to drive the robotic arm to move along the planned trajectory.

[0103] For the rolling cost C(E) j It can be divided into path constraints C. r (E j ) and breaking constraint C terminal (E j The specific forms of each constraint are as follows:

[0104] (1) Path constraint C r (E j )

[0105] Path constraint C r (E j It is mainly used to penalize the total length C of the trajectory. lenth And the collision between the robotic arm and the environment C coll Defined as C r (E j ):=C lenth (E j )+C coll (E j ), where C lenth (E j C represents the total length of the currently sampled trajectory of the robotic arm. coll (E j () is a distance-related value, and the mathematical definitions of its components are as follows:

[0106]

[0107]

[0108] Where, ω lenth ,ω coll >0 is the adjustment coefficient, and the collision cost C coll (E j ) Calculated by the CSDF module in Part 1:

[0109]

[0110] Where δ is the distance threshold, and when the distance value is less than or equal to δ, Ccoll (q j,t ,e j,t The distance is defined as a constant value of 1; conversely, the larger the distance value, the lower the probability of the robotic arm colliding with the environmental point cloud. coll (q j,t ,e j,t The smaller the value, the better.

[0111] (2) Breaking constraint C terminal (E j )

[0112] The purpose of the break constraint is to penalize the distance between the robotic arm and the final target point. The break constraint is defined as the robotic arm configuration q obtained by the model output from the currently sampled control sequence. j,H With target configuration q goal The distance is mathematically defined as follows:

[0113] C terminal (E j ):=ω terminal ||q j,H -q goal || (27)

[0114] Where, ω terminal This is the adjustment coefficient.

[0115] Based on the above two parts, a virtual robotic arm working scene is built in Gazebo, and the relevant hardware interface files are configured to ensure correct connection with the robotic arm's hardware interface files. The working scene is shown in the attached figure. Figure 4 As shown in the figure. Based on the generated collision-free trajectory in joint space, the trajectory is executed by a joint trajectory controller. The controller interpolates the time at each given path point, thereby enabling the robotic arm to perform continuous, smooth, and collision-free movement. The planned trajectory is shown in the attached figure. Figure 5 As shown.

[0116] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for planning the joint space trajectory of a robotic arm based on model-predicted path integrals, characterized by the following steps: The process is as follows: First, utilizing the joint configuration of the robotic arm, and based on the configuration and the point cloud of the scene, the distance and gradient estimates from the entire robotic arm to nearby obstacles are obtained using the configuration signed distance function. Then, based on the model prediction path integral method, a fast, online, global trajectory generator is constructed. This generator receives the initial and target robotic arm configurations, estimates the collision cost in the current robotic arm configuration space based on the estimated distances and gradients from the entire robotic arm to nearby obstacles, and generates a globally optimal trajectory for the robotic arm to execute. The specific steps are as follows: Part 1: Methods for Calculating Spatial Distances in Robotic Arm Configurations The method of generating control points on the surface of the robotic arm determines whether the robotic arm will collide with the surrounding environment by calculating the configuration space symbolic distance function between the generated control points and the point cloud of the surrounding environment. At the same time, the obtained data can be provided to the planner based on the model prediction path integral in the second part to generate a collision-free trajectory. The point cloud is composed of sampling points from the surface triangular mesh of an object with a known geometric shape and point clouds acquired by a depth camera. The acquired point cloud is first downsampled and denoised to reduce the number of points and increase the accuracy of the points. The processed point cloud is then unified into a base coordinate system through coordinate transformation to obtain a point cloud of the robotic arm's current working scene consisting of S points. Current robotic arm configuration It is known that, among which The angle values ​​of the seven joints are used to uniquely determine the current pose of the robotic arm. To quickly calculate the CSDF value of the current pose, some points need to be sampled on the surface of the robotic arm. The distance between the robotic arm and surrounding obstacles is indirectly estimated by using the sampled points. First, a set of points coinciding with the joints of the robotic arm are selected. The pose of the given robotic arm configuration in the workspace is calculated using GPU-accelerated forward kinematics. Then, linear interpolation is performed between the positions of the selected points to obtain the current configuration. A set of C sampling points If a robotic arm is used to perform a grasping task, obstacle avoidance by the end effector is crucial during the grasping process. Therefore, if the geometry of the object to be grasped is known, samples are taken from the object's surface. After obtaining the sampling points, a forward kinematics transformation is performed, and these points are added to the sampling points on the robotic arm itself. This method is used to obtain the surface sampling points of the robotic arm. Point clouds in work scenarios After this, the distance between the robotic arm and the environmental point cloud is estimated. The estimation steps are as follows: First, define any point in the space. The symbolic distance function value (SDF) is: in, It is a very small normal value, ensuring the smoothness of the entire set at the zero point; Then, the CSDF module is used to estimate the minimum distance between the current robotic arm configuration and the environmental point cloud. Within the robotic arm's workspace, given a configuration... The sampling points on the surface of the robotic arm are obtained below. The configurational sign distance function is defined as: in This is a safety threshold. This module can obtain the distance query between the current robotic arm configuration and the environmental point cloud and provide it to the planner for trajectory generation. This indicates that the current configuration of the robotic arm will not collide with the environment; conversely, this indicates that the robotic arm will collide with the environment. The second part describes the method for generating the trajectory in the joint space of the robotic arm. Based on the distance between the robotic arm configuration and the environmental point cloud obtained from the CSDF module, a trajectory generation method based on MPPI is used to solve the trajectory in the joint space of the robotic arm without collision. The dynamic model of the robotic arm is used to predict the dynamics of the robotic arm in the future. Multiple control input sequences are generated by sampling, and these sequences are used to predict multiple future state trajectories. Each sampled trajectory is evaluated according to a certain cost function. The trajectory with lower weight / higher cost has less impact on the final control input. Then, the trajectories are weighted and averaged to obtain the final control input. In each iteration, the new sampled trajectory is adjusted based on the result of the previous iteration, thereby gradually optimizing the control input sequence and approaching the optimal solution. The MPPI model is a discrete-time continuous-state system. , and These represent the robotic arm in and The configuration of time, , Indicates that it follows the mean. covariance is The Gaussian distribution sequence, in each iteration, is given a control region of... and nominal configuration Corresponding nominal displacement M displacement sequences were obtained by sampling. The rolling cost of each displacement sequence is obtained by forward calculation of the sampling results through the system model. Then, the M rolling costs are assigned certain weights to obtain their weighted average, and finally the displacement sequence obtained at time t is obtained. : in As a smoothing parameter, express The value at time t, and the definitions of other relevant parameters are as follows: in, It is a smoothing coefficient; After each iteration, the displacement sequence is calculated by weighting. Update the current nominal configuration The updated configuration will be sent to the trajectory actuator to drive the robotic arm to move according to the planned trajectory; For rolling costs It can be divided into path constraints. and breakage constraint The specific forms of each constraint are as follows: (1) Path constraints Path constraints Primarily used to penalize the total length of the trajectory And the collision between the robotic arm and the environment Defined as ,in This indicates the total length of the currently sampled trajectory of the robotic arm. It is a distance-related value, and the mathematical definitions of its components are as follows: in, To adjust the coefficient, collision cost Calculated by the CSDF module in Part 1: in This is a distance threshold; when the distance value is less than or equal to... hour, Defined as a constant value of 1, conversely, the larger the distance value, the lower the probability of the robotic arm colliding with the environmental point cloud. The smaller the value, the better; (2) Breaking constraint The purpose of the break constraint is to penalize the distance between the robotic arm and the final target point. The break constraint is defined as the robotic arm configuration obtained by the model output from the currently sampled control sequence. With target configuration The distance is mathematically defined as follows: in, This is the adjustment coefficient.

2. The robotic arm joint space trajectory planning method based on model prediction path integral as described in claim 1, characterized in that, Trajectory execution is achieved through a joint trajectory controller in ROS. The controller, based on the target position, velocity, and acceleration of the specified joint, optimizes and generates a smoother trajectory. It interpolates over time at each given path point, enabling the robotic arm to perform continuous, smooth, and collision-free motion along these estimates. It also enables simultaneous control of multiple joints of the robotic arm, thereby improving the robot's performance and efficiency in various tasks.

Images