Trajectory planning method and device of mechanical arm, storage medium and computer program product

Abstract

The application provides a trajectory planning method and device of a mechanical arm, a storage medium and a computer program product. The trajectory planning method comprises: an information acquisition step of acquiring environmental information around the mechanical arm and state information of the mechanical arm; a path planning step of performing path planning by using a rapid random tree expansion method according to the acquired environmental information and the state information of the mechanical arm, to generate a running path of the mechanical arm, and smoothing and optimizing the running path by using a gradient descent method; and a trajectory control step of converting a control system of the mechanical arm from a nonlinear system to a linear system by using a feedback linearization method, and generating a control instruction for controlling the mechanical arm to track the running path by using a linear quadratic regulator to optimize a control gain of the linear system according to the state information and the environmental information of the mechanical arm. The application dynamically adjusts the control instruction by real-time sensing information, so that the robot can more flexibly and accurately avoid obstacles in a dynamic environment.

Description

Technical Field

[0001] This invention relates to the field of robot path planning technology, and in particular to a method and apparatus for trajectory planning of the robotic arm of a charging robot, a storage medium, and a computer program product. Background Technology

[0002] With the rapid development of robotics technology, the application of autonomous charging robots is gradually increasing, especially in fields such as industrial automation and warehousing logistics. The robotic arm, as a key component of the charging robot, is responsible for precisely manipulating the charging plug and charging interface. However, current technologies still face many challenges in application to complex environments.

[0003] In existing technologies, robot environmental perception and path planning are crucial aspects of the charging process. Common path planning algorithms, such as A* search (a heuristic search algorithm) and Dijkstra's algorithm, while capable of planning reasonable paths in known environments, suffer from poor adaptability and real-time performance when facing unknown or dynamic environments. These algorithms typically assume a static and fully known environment, ignoring potential dynamic obstacles or unforeseen changes. Furthermore, fixed-path planning methods cannot adjust the path in real time according to environmental changes, leading to path deviations or accumulated errors in complex environments.

[0004] To address the shortcomings of existing technologies in path planning, the RRTS (Rapidly-exploring Random Tree Star) algorithm has gained attention in recent years. The RRTS algorithm generates globally optimal paths through random sampling and asymptotic optimization. However, the generated paths are typically coarse-grained and require further optimization to ensure their smoothness and feasibility.

[0005] In terms of control, existing trajectory tracking methods mostly employ traditional PID (Proportional Integral Derivative) controllers or MPC (Model Predictive Control) controllers. However, these methods often exhibit insufficient control accuracy or slow response speed in complex and dynamic environments. Feedback linearization techniques improve the control accuracy of the system by transforming the nonlinear control problem into a linear one. However, in practical applications, dynamic changes in the environment and uncertainties in sensors still place higher demands on control algorithms.

[0006] Currently, Chinese patent application CN202410506695.4 discloses an environment-adaptive joint trajectory planning method for a robotic arm. While this method is adaptive, the multi-objective particle swarm optimization algorithm struggles to converge in complex scenarios like charging. Chinese patent application CN202111215805.4 discloses an adaptive trajectory planning and obstacle avoidance method for robots, using an ant colony algorithm for planning and obstacle avoidance; however, the smoothness of the path generated by this method is difficult to guarantee.

[0007] In summary, existing technologies still have many shortcomings in environmental perception, path planning, and control strategies for adaptive trajectory planning of robotic arms. How to achieve efficient and stable charging operations in dynamic and complex environments remains a pressing issue that needs to be addressed. Summary of the Invention

[0008] To address the aforementioned problems in the prior art, this invention provides a trajectory planning method for dynamically adjusting control commands in a dynamic environment based on real-time perceived environmental information and the state information of the robotic arm itself.

[0009] To achieve the above objectives, according to a first aspect of the present invention, a trajectory planning method for a robotic arm is provided, the trajectory planning method comprising the following steps: an information acquisition step, using at least one sensor to acquire environmental information surrounding the robotic arm and state information of the robotic arm; a path planning step, based on the acquired environmental information and state information of the robotic arm, performing path planning using a fast random tree expansion method to generate a running path for the robotic arm, and using a gradient descent method to smooth and optimize the running path; and a trajectory control step, using a feedback linearization method to convert the control system of the robotic arm from a nonlinear system to a linear system, and using a linear quadratic control controller to optimize the control gain of the linear system based on the state information of the robotic arm and the environmental information, generating control commands for controlling the robotic arm to track the running path.

[0010] As a preferred embodiment, the trajectory planning method for the robotic arm according to the present invention further includes a monitoring step, which monitors the environmental changes around the robotic arm and the execution state of the robotic arm, and uses the acquired environmental change information and the state change information of the robotic arm as updated environmental information and state information, and repeatedly executes the path planning step and / or trajectory control step.

[0011] As a preferred embodiment, in the trajectory planning method of the robotic arm according to the present invention, in the information acquisition step, the environmental information acquired by the at least one sensor includes the location of obstacles and / or the location of the target, and the state information of the robotic arm includes at least one of the following: the actual position, posture, speed, and direction of movement of the robotic arm at the current moment.

[0012] As a preferred embodiment, the trajectory planning method for the robotic arm according to the present invention further includes an information fusion step, wherein after the information acquisition step, when the acquired environmental information includes perception information collected by at least two sensors, the perception information collected by the at least two sensors is fused, and the fused information is used as the environmental information.

[0013] As a preferred embodiment, the trajectory planning method for the robotic arm according to the present invention includes the following path planning steps: a workspace initialization step, which initializes the workspace of the robotic arm according to the environmental information and the state information, defining the starting position, target position, free space, and obstacle area of ​​the robotic arm in the workspace; a random expansion tree initialization step, which uses the starting position as the starting point of the random expansion tree to generate the random expansion tree consisting of a set of nodes composed of multiple tree nodes; and a new node generation step, which obtains random sampling points in the free space, and determines the nearest neighbor sampling point in the set of nodes in the random expansion tree, and sets the nearest neighbor sampling point as the nearest neighbor sampling point. The process involves: a first preset distance extending from the random sampling point to generate a new node; a collision detection step determining whether the extension path between the nearest sampling point and the new node collides with the obstacle area; a random expansion tree update step updating the random expansion tree based on the new node generated in the new node generation step if the collision detection step determines that the extension path does not collide with the obstacle area; an expansion termination step determining whether a preset termination condition is met, stopping the expansion and generating the running path if the condition is met; and a gradient descent optimization step optimizing the running path by repeatedly iterating to minimize the cost function value of the running path.

[0014] As a preferred embodiment, in the trajectory planning method for the robotic arm according to the present invention, in the new node generation step (S230), the sampling formula for the random sampling points is defined as follows: , in, The nearest neighbor sampling point is the one that is closest to the random sampling point. For the random sampling points; Furthermore, the formula for generating the new node is defined as follows: , in, The nearest sampling point, For the random sampling points, For the new node, The first preset distance.

[0015] As a preferred embodiment, in the trajectory planning method for the robotic arm according to the present invention, the collision detection step includes: The interpolation point generation step involves generating multiple discrete intermediate joint angles in the joint space as interpolation points. These multiple interpolation points, along with the nearest neighbor sampling point and the random sampling point, jointly construct the extension path. The interpolation point generation formula is defined as follows: , in, interpolation point For the new node The nearest sampling point The preset number of interpolations; The link pose calculation steps involve calculating the pose of the link corresponding to each interpolation point, wherein the pose calculation formula is defined as follows: , in, For the first The first interpolation point The angle of each joint For the first Transformation matrix of each joint; The detection step generates a detection result by detecting whether each link intersects with the obstacle area; the detection algorithm is defined as follows: , in, The geometric model of each of the links, A geometric model of the obstacle; If there is an intersection, the detection result is that a collision has occurred; if there is no intersection, the detection result is that no collision has occurred.

[0016] As a preferred embodiment, in the trajectory planning method for the robotic arm according to the present invention, the random expansion tree update step includes: The search radius is calculated using the following formula: , in, For the search radius, As a preset constant, For the dimension of space, The number of nodes in the randomly expanded tree; The neighborhood node search step involves taking the new node as the center, determining the neighborhood range based on the search radius and the position of the new node, and searching for multiple neighborhood nodes within the neighborhood range to obtain a set of neighborhood nodes. The parent node determination step involves calculating the total path cost based on the path costs of the neighboring nodes and the path costs from the neighboring nodes to the new node, and selecting the neighboring node with the minimum total path cost as the parent node of the new node. The function for calculating the minimum path cost is defined as follows: , in, This represents the path cost from the neighboring node to the new node. This represents the path cost of the neighboring nodes; The parent node update step involves, for each neighboring node, determining whether the path cost of the corresponding new node is less than the path cost of the neighboring node. If the path cost of the new node is less than the path cost of the neighboring node, the new node is updated to the parent node. The determination formula is defined as follows: , in, The path cost for the neighboring nodes is... The path cost of the new node is... The path cost from the neighboring node to the new node; The node set update step involves updating the random expansion tree by adding the new node and the parent node to the node set of the random expansion tree.

[0017] As a preferred embodiment, in the trajectory planning method for the robotic arm according to the present invention, the gradient descent optimization step includes: The path cost calculation step involves using a path cost function to calculate the path cost of the robotic arm's operating path. The path cost function is defined as follows: , in, This is the operating path of the robotic arm. For path speed, Path acceleration The cost function; The path optimization step involves calculating the gradient of the path cost function with respect to the running path, and iteratively adjusting the running path along the direction of the negative gradient to update the running path, thereby minimizing the cost value of the path cost function. The formula for calculating the gradient is expressed as: , in, For the updated running path The learning rate used to determine the magnitude of path adjustment. The gradient of the path, This represents the path during the k-th iteration.

[0018] As a preferred embodiment, in the trajectory planning method for the robotic arm according to the present invention, in the extended termination step, the preset termination condition includes either of the following two conditions: The distance between any node in the random expansion tree and the target location is less than a second preset distance; the number of updates to the random expansion tree reaches the maximum number of iterations.

[0019] As a preferred embodiment, the trajectory planning method for the robotic arm according to the present invention includes the following trajectory control steps: The state equation construction step involves constructing a state equation representing the motion state of the robotic arm, wherein the state equation is expressed as: , in, Let be the state vector of the robotic arm. To control the input, Represents a nonlinear function. Represents the input matrix; The linearization process involves constructing a feedback linearization control law to transform the state equation into a linear system. The feedback linearization control law is defined as follows: , in, To control the input, For the new control variable, , For the feedback linearization control law, the linear system in the closed-loop case is expressed as: ; The weight calculation steps involve constructing a performance index function that reflects the performance of the control system, and obtaining the state weight matrix and the control weight matrix by minimizing the performance index function; wherein, the performance index function is defined as follows: , in, This is the state weight matrix; To control the weight matrix; The time derivative represents the integral of the performance index function over the time domain; The control gain calculation step involves substituting the state weight matrix and the control weight matrix into the Riccati equation to obtain a symmetric positive definite matrix, and then calculating the control gain based on the symmetric positive definite matrix. The control input optimization step optimizes the control input based on the control gain: , in, For the control input, The control gain is... This is the state vector of the robotic arm; The control command generation step involves taking the optimized control input as the control command and substituting it into the linear system to form a closed-loop system, thereby controlling the motion trajectory of the robotic arm.

[0020] According to a second aspect of the present invention, a trajectory planning device for a robotic arm is provided, the trajectory planning device comprising: an information acquisition unit that acquires environmental information surrounding the robot and state information of the robotic arm using at least one sensor; a path planning unit that performs path planning using a fast stochastic expansion method based on the acquired environmental information and state information of the robotic arm to generate a running path for the robotic arm, and smooths and optimizes the running path using a gradient descent method; and a trajectory control unit that uses a feedback linearization method to convert the control system of the robotic arm from a nonlinear system to a linear system, and optimizes the control gain of the linear system using a linear quadratic control controller based on the state information of the robotic arm and the environmental information, and generates control commands for controlling the robotic arm to track the running path.

[0021] According to a third aspect of the present invention, a non-transitory storage medium is provided, which stores a computer program that, when executed by a processor, enables the trajectory planning method according to the first aspect of the present invention.

[0022] According to a fourth aspect of the present invention, a computer program product is provided, comprising computer instructions that, when executed by a processor, enable the trajectory planning method according to a first aspect of the present invention.

[0023] The beneficial effects of this invention are as follows: The trajectory planning method of the robotic arm in this invention uses the fast random tree expansion method to generate the running path of the robotic arm, and uses the gradient descent method to smooth and optimize the generated path, making the generated running path smoother; In terms of trajectory control of the robotic arm, the control system of the robotic arm is first converted from a nonlinear system to a linear system through the feedback linearization method, and then the linear control method is used to optimize the control gain of the system according to the changes in the environment and the state changes of the robotic arm itself, and dynamically adjust the control command to realize a step-by-step control strategy of nonlinear compensation and linear optimization, taking into account complex dynamics and performance optimization, so that the robotic arm can avoid obstacles more flexibly and accurately in the dynamic environment and reach the specified pose efficiently and stably. Attached Figure Description

[0024] Figure 1 A schematic diagram illustrating an exemplary structure of an automatic charging system for performing a trajectory planning method according to the present invention is shown.

[0025] Figure 2 A flowchart illustrating the trajectory planning method for a robotic arm according to the present invention is provided.

[0026] Figure 3 A flowchart illustrating another trajectory planning method for a robotic arm according to the present invention is shown.

[0027] Figure 4 A flowchart illustrating the path planning steps of the trajectory planning method according to the present invention is provided.

[0028] Figure 5 A flowchart illustrating the collision detection steps of the trajectory planning method according to the present invention is provided.

[0029] Figure 6 A flowchart illustrating the random expansion tree update step of the trajectory planning method according to the present invention is provided.

[0030] Figure 7 A flowchart illustrating the gradient descent optimization steps of the trajectory planning method according to the present invention is provided.

[0031] Figure 8 A flowchart illustrating the trajectory control steps of the trajectory planning method according to the present invention is provided.

[0032] Figure 9 A software structure block diagram of the trajectory planning device for a robotic arm according to the present invention is illustrated. Detailed Implementation

[0033] Exemplary embodiments of the present invention will now be described in detail with reference to the accompanying drawings. It should be noted that, unless otherwise specifically stated, the relative configuration of components, numerical representations, and values ​​described in these embodiments does not limit the scope of the invention.

[0034] The trajectory planning method for the robotic arm of this invention can be applied to an automatic charging system, implemented by a processor in the automatic charging system executing a computer program stored in the system's memory. Alternatively, the automatic charging system can communicate with a server, where a processor executes a computer program stored on the server or in the cloud, and the program execution result is fed back to the automatic charging system in real time.

[0035] In this invention, the term "unit" can refer to a software environment, a hardware environment, or a combination of both. In a software environment, the term "unit" refers to a function, application, software module, feature, routine, set of instructions, or program that can be executed by a programmable processor (such as a microprocessor, central processing unit (CPU), or specially designed programmable device) or controller. Memory contains instructions or programs that, when executed by the CPU, cause the CPU to perform operations corresponding to the unit or function. In a hardware environment, the term "unit" refers to a hardware element, circuit, component, physical structure, system, module, or subsystem. According to a particular embodiment, the term "unit" can include mechanical, optical, or electrical components, or any combination thereof. The term "unit" can include active (e.g., transistors) or passive (e.g., capacitors) components. The term "unit" can include a semiconductor device having a substrate and other material layers having various conductivity concentrations. It can include a CPU or programmable processor that can execute programs stored in memory to perform a specified function. The term "unit" can include logic elements (e.g., AND, OR) implemented by transistor circuitry or any other switching circuitry. In the context of a combination of software and hardware environments, the term "unit" or "circuit" refers to any combination of software and hardware environments as described above. Additionally, the terms "element," "component," "part," or "device" may also refer to a "circuit" integrated with or not integrated with packaging material.

[0036] This invention takes the motion planning and control of a charging robot's arm as an example, applying the trajectory planning method of this invention to control the robotic arm of an automatic charging system. The system architecture of the automatic charging system of this invention is described below with reference to the accompanying drawings.

[0037] [Architecture of the Automatic Charging System of the Invention] The automatic charging system of this invention is designed with a network of main tracks and branch tracks, thereby ensuring that intelligent components can move efficiently and safely to any designated parking space. The main track is closed and runs through the entire parking lot, while the branch tracks extending from the main track lead to each parking space, allowing intelligent components to stay next to the parking space without affecting the passage of the main track.

[0038] The intelligent components of the automatic charging system of this invention include a charging pile and a robotic arm, both of which are capable of autonomous movement on the main track and branch tracks. The charging pile can move to a specific parking space according to scheduling instructions to provide power replenishment for electric vehicles; the robotic arm is responsible for inserting and removing the charging gun from the charging pile, and also has the ability to move to any parking space to ensure automation and seamless connection of the charging process.

[0039] An exemplary structure of the automatic charging system in this invention embodiment can be found in [reference needed]. Figure 1 As shown. Figure 1 The shape of the track of the automatic charging system and its positional relationship with parking spaces are shown. The automatic charging system of this invention includes a main track 1 and branch tracks 2, as well as charging piles and robotic arms capable of autonomously moving on these two tracks. Figure 1 As shown, the main track 1 forms a closed loop and is designed to cover the entire parking lot. Branch tracks 2 extend from the main track 1, with charging parking spaces 3 distributed on both sides of each branch track. The branch tracks 2 allow charging piles and robotic arms to stop and operate at the parking spaces without affecting the smooth flow of the main track 1.

[0040] Although Figure 1 The main track 1 shown is a closed loop, but the invention is not limited to this. The shape of the main track 1 can also be a circle or an ellipse or other closed shapes.

[0041] Furthermore, although the main track and branch tracks of the automatic charging system of the present invention are preferably suspended in the following description to save space and facilitate passage and operation, the present invention is not limited thereto. Depending on the height and layout of the parking lot, the automatic charging system of the present invention can also be applied to ground-mounted tracks with tracks laid on the ground.

[0042] The charging operation process of the automatic charging system of the present invention will be described in detail below.

[0043] [Charging operation process of the automatic charging system] Once an electric vehicle is parked in any parking space within the automatic charging system, the user can send a charging request to the system. The automatic charging system then selects a suitable charging station and robotic arm. Specifically, upon receiving a charging request from an electric vehicle, the automatic charging system selects the most suitable charging station and robotic arm to handle the charging task for that parking space. For example, it may prioritize selecting the charging station and robotic arm that is closest to the parking space and is currently idle. This invention does not limit the method of selecting the charging station and robotic arm.

[0044] Furthermore, the automatic charging system moves the selected charging pile and the robotic arm to the designated parking space. It should be understood that the present invention does not impose a specific order on the arrival of the charging pile and the robotic arm at the designated parking space; they can arrive sequentially or simultaneously.

[0045] After both the charging pile and the robotic arm have moved along the track to the corresponding branch track of the designated parking space, motion commands for the current environment of the robotic arm are generated according to the trajectory planning method of the robotic arm described later. This allows the robotic arm to automatically grasp the charging gun on the charging pile according to the motion command generated by the motion strategy network, insert it into the electric vehicle, and the charging pile begins charging the electric vehicle. Next, until the charging pile is fully charged, the robotic arm moves along the track back to the current parking space, removes the charging gun, and puts it back on the charging pile. Thus, the charging operation is completed, and the charging pile and robotic arm leave the current parking space to perform other charging tasks.

[0046] In the aforementioned automatic charging system, the steps of controlling the robotic arm to automatically grab the charging gun and insert it into the electric vehicle, as well as controlling the robotic arm to put the charging gun back into the charging pile, involve the motion planning and control of the robotic arm. The trajectory planning method used for robotic arm control will be described in detail below.

[0047] [Robotic Arm Trajectory Planning Methods] [Design of Trajectory Planning Algorithm] The trajectory planning method of the robotic arm according to the present invention can be implemented by the processor in the robotic arm reading the application stored in the memory of the robotic arm, or by the robotic arm communicating with the server, and the server executing the application stored on the server and sending the execution result to the robotic arm, or by the server and the robotic arm executing the corresponding application respectively and communicating the execution result with each other.

[0048] Existing control methods suffer from the difficulty of ensuring the trajectory tracking accuracy and response speed of the robotic arm in complex dynamic environments, thus affecting the successful execution of charging tasks. In order to meet the needs of the robotic arm to adapt to uncertain dynamic environments and complete charging tasks, this invention designs a robotic arm trajectory planning method based on real-time perception information. This method can perceive the environmental information around the robotic arm and the state information of the robotic arm itself in real time. Based on the perceived environmental and state information, dynamic path planning is realized, and the robotic arm is controlled to adjust obstacle avoidance actions according to environmental changes, so as to safely and quickly reach the designated pose.

[0049] In this invention, the trajectory planning method for the robotic arm mainly includes an information acquisition step S100, a path planning step S200, and a trajectory control step S300. (See below for reference.) Figure 2 The trajectory planning method for the robotic arm of the present invention will be described.

[0050] like Figure 2 As shown, the information acquisition step S100 is executed first, using at least one sensor to acquire environmental information around the robotic arm and the status information of the robotic arm.

[0051] Environmental information refers to obstacle information in the environment surrounding the robotic arm and the location information of various targets required for the robotic arm to perform the charging task, such as the location of the charging gun and the location of the electric vehicle's charging interface. In this embodiment, at least one of different types of sensors, such as vision sensors, LiDAR, and ultrasonic sensors, can be used to perceive dynamic environmental information. For example, images of the robotic arm's surroundings acquired by a vision sensor can be used as environmental information. Specifically, the vision sensor can be, for example, the RealSense vision sensor developed by Intel, to acquire real-time information about the robot's surrounding environment.

[0052] The state information of a robotic arm refers to information reflecting its actual motion trajectory, such as its current position and posture, speed, and direction of motion. This information can be collected using at least one sensor, such as a vision sensor or an inertial measurement unit (IMU).

[0053] In the path planning step S200, based on the acquired environmental information and the state information of the robotic arm, the fast random tree expansion method is used to plan the path to generate the running path of the robotic arm, and the gradient descent method is used to smooth and optimize the running path.

[0054] In step S100, environmental information and the state information of the robotic arm have been acquired. Further path planning is needed based on the obstacle positions, target positions, and the current pose of the robotic arm in the environmental information. This path avoids obstacles and generates a path that reaches the target position, serving as a reference trajectory for subsequent motion control by the control system. In this embodiment, the Fast Random Tree Expansion Algorithm (RRTS algorithm) is used to generate the path, and gradient descent is used to smooth and optimize it. Through iterative adjustments, the path's cost function value is gradually reduced, resulting in a better path.

[0055] In trajectory control step S300, the control system of the robotic arm is converted from a nonlinear system to a linear system through a feedback linearization method. Based on the state information of the robotic arm and the environmental information, a linear quadratic control controller is used to optimize the control gain of the linear system and generate control commands for controlling the robotic arm to track the running path, thereby realizing the control of the motion trajectory of the robotic arm.

[0056] In terms of trajectory control, this invention employs a feedback linearization method combined with an LQR (linear quadratic regulator) to control the motion trajectory of a robotic arm. The feedback linearization method refers to using nonlinear feedback to offset the nonlinearity of the control system, transforming the robotic arm's nonlinear control system into a linear control system. This transforms the originally complex nonlinear control problem into a linear system control problem, making the linearized system response more predictable and providing a compatibility basis for subsequent linear control methods, thus improving the tracking accuracy and robustness of the robotic arm. Furthermore, the LQR in this invention is responsible for the performance optimization of the linear system. By constructing a performance index function (also known as a cost function), the physical meaning of which is to weigh the cost of the system deviating from a zero state and the control energy consumption, the optimal control gain is calculated by minimizing the value of the performance index function, thereby achieving performance optimization of the control system.

[0057] The trajectory planning method for the robotic arm in this invention uses a fast random tree expansion method to generate the robotic arm's running path, and then uses a gradient descent method to smooth and optimize the generated path, resulting in a smoother running path. Regarding the trajectory control of the robotic arm, a feedback linearization method is first used to convert the robotic arm's control system from a nonlinear system to a linear system. Then, a linear control method is used to optimize the system's control gain based on changes in the environment and the robotic arm's own state, dynamically adjusting control commands to achieve a step-by-step control strategy that combines nonlinear compensation and linear optimization. This approach balances complex dynamics and performance optimization, enabling the charging robot to more flexibly and accurately avoid obstacles and reach the designated pose in dynamic environments, achieving efficient and stable charging operations.

[0058] As an optional implementation method, refer to Figure 3 As shown, the trajectory planning method also includes a monitoring step S400, which monitors the environmental changes around the robotic arm and the execution state of the robotic arm, and uses the acquired environmental change information and the state change information of the robotic arm as updated environmental information and state information, while iterating the execution information acquisition step S100, the path planning step S200 and / or the trajectory control step S300.

[0059] Specifically, the trajectory planning method of this invention is dynamically changing, adjusting the running path, control strategy, or both in real time according to changes in the environment and the state of the robotic arm itself. Compared to traditional fixed path planning, this invention can be applied to more complex dynamic environments. For example, in the scenario of a charging robot, since the location where users park their electric vehicles is not fixed and each user has different parking habits, the location of the charging port will also change. If a static, fixed path planning is used, the robotic arm of the charging robot will be unable to accurately insert the charging gun into the charging port, or even damage the vehicle. In this case, this invention can adjust the running path of the robotic arm by collecting the location of the electric vehicle's charging port in real time, so that it can accurately perform the charging operation.

[0060] The steps of the trajectory planning method in this invention are described in detail below.

[0061] As an optional implementation, in the information acquisition step S100, the environmental information acquired by at least one sensor includes the location of obstacles and / or the location of the target, and the state information of the robotic arm includes at least one of the following: the actual position, posture, speed, and direction of movement of the robotic arm at the current moment.

[0062] Specifically, the sensors used to acquire environmental information can be visual sensors, lidar, or other similar sensors. In this embodiment, a visual sensor is preferred. The visual sensor collects information such as obstacles around the robotic arm, the location of the charging gun, and the location of the charging port, providing basic environmental information for subsequent path planning. Visual sensors can acquire rich information such as images, colors, shapes, and motion trajectories, making them suitable for complex scene analysis and offering high timeliness and flexibility. The robotic arm's state information refers to information reflecting its actual motion trajectory, which may include one or more of the following: the robotic arm's current position and posture, speed, and direction of motion.

[0063] As an optional implementation method, the trajectory planning method also includes: In the information fusion step, after the information acquisition step S100, when the acquired environmental information includes perception information collected by at least two sensors, the perception information collected by at least two sensors is fused, and the fused information is used as environmental information.

[0064] Specifically, in this invention, multiple sensors can be used to perceive environmental information. For example, a visual sensor can be used to collect environmental information, and an IMU can be used to collect the state information of the robotic arm. In this case of multi-sensor data, it is necessary to fuse the multi-sensor data. In this embodiment, a Kalman filter is used to fuse multi-sensor data, reduce the error of single sensor measurement, and improve the reliability and accuracy of environmental perception.

[0065] For example, information such as obstacle location and charging port location is transmitted through a state transition matrix. The specific form of the state transition matrix is ​​as follows: (1) In the above formula (1), This is a rotation matrix used to describe the pose. This is a position vector.

[0066] This invention designs a Kalman filter for multi-sensor data fusion: setting the initial state vector of the system. and the initial covariance matrix The state vector typically includes key variables such as the robot arm's pose (position and orientation) and its velocity. At each time step... Predict the next state and covariance matrix based on the system's state transition model: (2) (3) In formulas (2) and (3) above, It is the state transition matrix, which describes the dynamic model of the system; It is a control input model that describes the input. Impact on the state; It is the process noise covariance matrix, representing the uncertainty of the prediction model.

[0067] When the sensor provides measurement data When using Kalman gain Correct the predicted state: (4) (5) (6) In formulas (4) to (6) above, It is the observation matrix, which maps the state space to the measurement space. It is the measurement noise covariance matrix, representing the uncertainty of the measurement data. It is an identity matrix.

[0068] For data from multiple sensors, this invention jointly represents the data from all sensors into a single observation vector, and then performs a fusion calculation. Specifically, this involves combining the observation matrices of each sensor... and measured values Combine them to construct a comprehensive observation matrix and a comprehensive measurement vector, and then perform the update steps as described above.

[0069] In this invention, the path planning step S200 further includes a workspace initialization step S210, a random expansion tree initialization step S220, a new node generation step S230, a collision detection step S240, a random expansion tree update step S250, an expansion termination step S260, and a gradient descent optimization step S270. (See below for further details.) Figure 4 The path planning step S200 of the present invention will be described.

[0070] In the workspace initialization step S210, the workspace of the robotic arm is initialized based on environmental information and status information. Perform initialization processing in the workspace Define the starting position of the robotic arm. Target location Free space And the area of ​​obstacles. This represents the number of degrees of freedom of the robotic arm.

[0071] In the random expansion tree initialization step S220, the starting position is used as the starting point of the random expansion tree, generating a random expansion tree consisting of a set of multiple tree nodes. The initialization of the random expansion tree includes... , where the node set Includes start position edge set It is an empty set.

[0072] In the new node generation step S230, in free space Obtain a random sampling point And determine the nodes in the set of nodes in the random expansion tree that are related to the random sampling points. The nearest neighbor sampling point and the nearest sampling point Extend a first preset distance to the random sampling point Generate new nodes .

[0073] In the new node generation step S230, the sampling formula for random sampling points is defined as follows: (7) In the above formula (7), The nearest neighbor sampling point is the one that is closest to the random sampling point. These are random sampling points; Furthermore, the formula for generating new nodes is defined as follows: (8) In the above formula (8), For the nearest sampling point, For random sampling points, For the new node, This is the first preset distance.

[0074] In collision detection step S240, it is determined whether the extended path between the nearest sampling point and the new node collides with an obstacle area. If it is determined in collision detection step S240 that the extended path collides with an obstacle area, the process returns to step S230 and re-executes the new node generation step S230; otherwise, if it is determined in collision detection step S240 that the extended path does not collide with an obstacle area, the process proceeds to step S250.

[0075] In the random expansion tree update step S250, the random expansion tree is updated based on the new node generated in the new node generation step S230.

[0076] In the extension termination step S260, it is determined whether the preset termination condition is met, and if the condition is met, the extension is stopped and the running path is generated.

[0077] In the gradient descent optimization step S270, the running path is optimized by repeatedly iterating and adjusting the running path to minimize the cost function value of the running path.

[0078] This invention employs the RRTS algorithm for path planning to generate an initial running path. The RRTS algorithm uses random sampling and asymptotic optimization. It searches for nodes adjacent to a new node in the tree; if the path can be shortened by using the new node, the tree structure is updated. The generated path is then optimized using a gradient descent optimization step to ensure a feasible and shorter path is generated.

[0079] In this invention, the collision detection step S240 further includes an interpolation point generation step S2410, a link pose calculation step S2420, and a detection step S2430. (See below for further details.) Figure 5 The collision detection step S240 of the present invention will be described in detail.

[0080] First, in the interpolation point generation step S2410, multiple discrete intermediate joint angles are generated in the joint space as interpolation points. These multiple interpolation points, along with the nearest neighbor sampling point and random sampling points, jointly construct the extension path. The interpolation point generation formula is defined as follows: (9) In the above formula (9), interpolation point For new nodes Nearest neighbor sampling point This is the preset number of interpolations.

[0081] The collision detection process in step S240 is to check the collision from the nearest sampling point. To the new node Is the path in free space? In this embodiment, a linear interpolation method is used to generate a series of discrete intermediate joint angles in joint space, which are then used to generate multiple checkpoints on the path.

[0082] Next, in the link pose calculation step S2420, the pose of the link corresponding to each interpolation point is calculated, where the pose calculation formula is defined as follows: (10) In the above formula (10), For the first The first interpolation point The angle of each joint For the first The transformation matrix of each joint describes the effect of the joint angle on the link position.

[0083] Detection step S2430 generates a detection result by detecting whether each link intersects with the obstacle area; the detection algorithm is defined as follows: (11) In the above formula (11), The geometric model for each link, This is a geometric model of the obstacle.

[0084] In step S2430a, if there is an intersection, the detection result is that a collision has occurred. In step S2430b, if there is no intersection, the detection result is that no collision has occurred.

[0085] In this embodiment of the invention, a detection algorithm is used to determine the collision status of each link with obstacles in the environment. Assume the geometric model of each link is as follows: The geometric model of the obstacle is Collision detection can be achieved by examining the link geometry model. Is it related to the obstacle geometry model? Intersection is used to determine the location and orientation of the sampled points in the environment, and a bounding box detection algorithm is used to check for intersection. If an intersection exists, it represents the nearest neighbor sampling point. To the new node The path will collide with obstacles, the new node will be unusable, and a new node needs to be generated.

[0086] In this invention, the random expansion tree update step S250 further includes a search radius calculation step S2510, a neighbor node search step S2520, a parent node determination step S2530, a parent node update step S2540, and a node set update step S2550. See below for further details. Figure 6 The random expansion tree update step S250 of the present invention will be described in detail.

[0087] like Figure 6 As shown, in the search radius calculation step S2510, the search radius is calculated according to the following formula: (12) In the above formula (12), The search radius; This is a preset constant; For the dimension of space; This represents the number of nodes in the randomly expanded tree.

[0088] In the neighbor node search step S2520, the neighborhood range is determined with the new node as the center, based on the search radius and the position of the new node, and multiple neighbor nodes are searched within the neighborhood range to obtain a set of neighbor nodes. In the parent node determination step S2530, the total path cost is calculated based on the path cost of neighboring nodes and the path cost from neighboring nodes to the new node. The neighboring node with the minimum total path cost is selected as the parent node of the new node. The function for calculating the minimum path cost is defined as follows: (13) In the above formula (13), This represents the path cost from a neighboring node to the new node; This represents the path cost of neighboring nodes.

[0089] Specifically, after the search is complete, the new node will be... Add nodes to the random expansion tree And calculate the parent node with the minimum total path cost. Add edge set This completes the update of the randomly expanded tree.

[0090] In the parent node update step S2540, for each neighboring node, it is determined whether the path cost of the corresponding new node is less than the path cost of the neighboring nodes. If the path cost of the new node is less than the path cost of the neighboring nodes, proceed to step S2540a and update the new node as the parent node. If the path cost of the new node is not less than the path cost of the neighboring nodes, no update is performed, and the process proceeds directly to step S2550. The determination formula is defined as follows: (14) In the above formula (14), The path cost for neighboring nodes; The path cost for the new node; This represents the path cost from a neighboring node to the new node.

[0091] The purpose of this step is to, for each neighboring node Check if the new node has been passed. Connections can reduce path costs if new nodes are used. To connect the tree, add the new node. Set it as the parent node to further reduce the total path cost. If the above conditions are met, Reset the parent node to and update the edge set. .

[0092] In the node set update step S2550, the random expansion tree is updated by adding the new node and the parent node to the node set of the random expansion tree.

[0093] In this invention, the gradient descent optimization step S270 further includes a path cost calculation step S2710 and a path optimization step S2720. See below for further details. Figure 7 The gradient descent optimization step S270 of the present invention will be described.

[0094] like Figure 7 As shown, in the path cost calculation step S2710, the path cost function is used to calculate the path cost of the robotic arm's running path. The path cost function is defined as follows: (15) In the above formula (15), The running path of the robotic arm Path speed Path acceleration Let be the cost function.

[0095] This embodiment employs gradient descent optimization. The goal of path optimization is to find a path. This minimizes the path cost function. Typically, the path cost function... Factors such as path length, path smoothness, and distance to obstacles can be considered to optimize the path from multiple dimensions and obtain a better running path.

[0096] In path optimization step S2720, the gradient of the path cost function with respect to the running path is calculated, and the running path is updated by iteratively adjusting it along the direction of the negative gradient to minimize the cost value of the path cost function. The formula for calculating the gradient is expressed as follows: (16) In the above formula (16), For the updated running path The learning rate used to determine the magnitude of path adjustment Gradient of the path This represents the path during the k-th iteration.

[0097] Specifically, in order to optimize the path The path cost function needs to be calculated. For path The gradient, i.e. The gradient represents the impact of each path point on the overall cost during adjustment. The path update rule is similar to standard gradient descent, adjusting the path along the negative gradient direction to reduce the cost function value. In the iterative process of randomly expanding the tree, this invention needs to calculate the path cost of the expanded nodes. Through repeated iterations using the path cost function, the path is gradually adjusted, making the cost function value of the path smaller and smaller, thereby obtaining a better path.

[0098] As an optional implementation, in the extended termination step S260, the preset termination condition includes either of the following two conditions: The distance between any node in the randomly expanded tree and the target location is less than the second preset distance; the number of updates to the randomly expanded tree reaches the maximum number of iterations.

[0099] The termination conditions for the random expansion tree update iteration in this invention include: whether the target has been reached and whether the iteration limit has been reached. If any node in the random expansion tree... satisfy ,in The distance threshold is a positive number; updates stop when any node in the randomly expanded tree approaches the target position. Alternatively, updates stop when the randomly expanded tree reaches its maximum number of iterations. If it stops expanding, then the expansion will cease.

[0100] In this invention, the trajectory control step S300 further includes a state equation construction step S310, a linearization processing step S320, a weight calculation step S330, a control gain calculation step S340, a control input optimization step S350, and a control command generation step S360. (See below for further details.) Figure 8 The trajectory control step S300 of the present invention will be described in detail.

[0101] like Figure 8 As shown, firstly, in the state equation construction step S310, a state equation representing the motion state of the robotic arm is constructed. The state equation is expressed as: (17) In the above formula (17), Let be the state vector of the robotic arm. To control the input, Represents a nonlinear function. This represents the input matrix.

[0102] In linearization step S320, the state equations are transformed into a linear system by constructing a feedback linearization control law. The definition of the feedback linearization control law is as follows: (18) In the above formula (18), To control the input, For the new control variable, , For feedback linearization control law, the linear system in the closed-loop case is expressed as: .

[0103] The goal of feedback linearization is to design and This transforms the nonlinear state equation (i.e., Equation 17 above) into a linear system. This simplifies the control design of the robotic arm control system.

[0104] In weight calculation step S330, a performance index function reflecting the performance of the control system is constructed. The state weight matrix and control weight matrix are obtained by minimizing the performance index function. The performance index function is defined as follows: (19) In the above formula (19), The state weight matrix is... , To control the weight matrix, , The time derivative represents the integral of the performance index function over the time domain.

[0105] In the control gain calculation step S340, the state weight matrix and the control weight matrix are substituted into the Riccati equation to obtain a symmetric positive definite matrix, and the control gain is calculated based on the symmetric positive definite matrix.

[0106] In the control input optimization step S350, the control input is optimized based on the control gain: (20) In the above formula (20), To control the input, To control the gain, , Let be the state vector of the robotic arm.

[0107] In the control command generation step S360, the optimized control input is used as a control command and substituted into the linear system to form a closed-loop system to control the motion trajectory of the robotic arm.

[0108] The trajectory execution controller of this invention employs a feedback linearization method, transforming the trajectory tracking problem into a simple linear control problem, thereby improving control accuracy. Simultaneously, based on sensor feedback of environmental changes and robotic arm state changes, LQR-based feedback control is used to optimize control gain, ensuring the stability and response speed of the robotic arm during trajectory tracking. The LQR controller calculates the state feedback matrix and control input in real time, allowing the system to stabilize quickly with minimal energy. Controlling the robotic arm's trajectory using an LQR controller offers the following advantages: the LQR controller optimizes using a linear quadratic objective function, resulting in a closed-form analytical solution that is easy to calculate and implement; the LQR controller exhibits strong robustness to changes and disturbances in system parameters, achieving good control performance under uncertain environments; furthermore, the LQR controller ensures that the system's state trajectory converges to the equilibrium point, demonstrating good stability and convergence.

[0109] In addition, the present invention also provides a trajectory planning device for a robotic arm.

[0110] [Software Structure of the Trajectory Planning Device for the Robotic Arm of the Present Invention] The following is for reference Figure 9 The software structure of the trajectory planning device for the robotic arm of the present invention will be described. For example... Figure 9 As shown, the trajectory planning device 900 for the robotic arm of the present invention includes an information acquisition unit 901, a path planning unit 902, and a trajectory control unit 903. The processing performed by each unit will be described in detail below.

[0111] like Figure 9 As shown, firstly, the information acquisition unit 901 uses at least one sensor to acquire environmental information around the robot and the state information of the robotic arm.

[0112] Environmental information can refer to obstacle information in the environment surrounding the robotic arm, and the location information of various targets required for the robotic arm to perform the charging task, such as the location of the charging gun and the location of the electric vehicle's charging interface. In this embodiment, at least one of different types of sensors, such as vision sensors, lidar, and ultrasonic sensors, can be used to perceive dynamic environmental information. For example, images of the robotic arm's surroundings collected by a vision sensor can be used as environmental information.

[0113] The state information of a robotic arm refers to information reflecting its actual motion trajectory, such as its current position and posture, speed, and direction of motion. This information can be collected using at least one sensor, such as a vision sensor or an IMU (Integrated Measurement Unit).

[0114] Next, the path planning unit 902 uses the fast stochastic expansion method to plan the path based on the acquired environmental information and the state information of the robotic arm, so as to generate the running path of the robotic arm, and uses the gradient descent method to smooth and optimize the running path.

[0115] In this embodiment, after the information acquisition unit 901 acquires environmental information and the state information of the robotic arm, it further performs path planning based on the obstacle positions, target positions, and the current pose of the robotic arm in the environmental information to avoid obstacles in the environment, thereby generating a running path that can reach the target position, which serves as a reference trajectory for subsequent motion control by the control system. This embodiment uses the RRTS algorithm to generate the running path and employs gradient descent to smooth and optimize it. Through repeated iterations, the path is gradually adjusted, making the cost function value of the path smaller and smaller, thus obtaining a better path.

[0116] Then, the trajectory control unit 903 uses a feedback linearization method to convert the control system of the robotic arm from a nonlinear system to a linear system, and uses a linear quadratic control controller to optimize the control gain of the linear system based on the state information of the robotic arm and the environmental information, thereby generating control commands for controlling the robotic arm to track the running path, thus realizing the control of the motion trajectory of the robotic arm.

[0117] In terms of trajectory control, this invention employs a feedback linearization method combined with LQR linear control to control the motion trajectory of a robotic arm. The feedback linearization method refers to using nonlinear feedback to cancel the nonlinearity of the control system, transforming the robotic arm's nonlinear control system into a linear control system. This transforms the originally complex nonlinear control problem into a linear system control problem, making the linearized system response more predictable and providing a compatibility basis for subsequent linear control methods, thus improving the tracking accuracy and robustness of the robotic arm. Furthermore, the LQR function in this invention is responsible for optimizing the performance of the linear system. By constructing a performance index function, which physically represents the trade-off between the system's deviation from zero and the cost of control energy consumption, the optimal control gain is calculated by minimizing the performance index function value, thereby achieving performance optimization of the control system.

[0118] The trajectory planning device of this invention uses a fast random tree expansion method to generate the running path of the robotic arm, and smooths and optimizes the generated path using a gradient descent method, resulting in a smoother running path. In terms of the trajectory control of the robotic arm, the control system of the robotic arm is first converted from a nonlinear system to a linear system through a feedback linearization method. Then, the control gain of the system is optimized according to the changes in the environment and the state changes of the robotic arm itself through a linear control method, and the control commands are dynamically adjusted to realize a step-by-step control strategy of nonlinear compensation and linear optimization. This approach takes into account both complex dynamics and performance optimization, enabling the charging robot to avoid obstacles more flexibly and accurately in dynamic environments and reach the designated pose, thus achieving efficient and stable charging operations.

[0119] [Other Implementation Methods] Embodiments of the invention can also be implemented by a computer that reads and executes computer-executable instructions (e.g., one or more programs) recorded on a storage medium (also more fully referred to as a "non-transitory computer-readable storage medium") to perform one or more functions in the above embodiments, and / or includes one or more circuits (e.g., application-specific integrated circuits (ASICs)) for performing one or more functions in the above embodiments. Furthermore, embodiments of the invention can be implemented using a method by which the computer of the system or device, for example, reads and executes the computer-executable instructions from the storage medium to perform one or more functions in the above embodiments, and / or controls the one or more circuits to perform one or more functions in the above embodiments. The computer may include one or more processors (e.g., a central processing unit (CPU), a microprocessor unit (MPU)) and may include separate computers or a network of separate processors to read and execute the computer-executable instructions. The computer-executable instructions may be provided to the computer, for example, from a network or the storage medium. The storage medium may include one or more of the following: hard disk, random access memory (RAM), read-only memory (ROM), memory of a distributed computing system, optical disc (such as compressed optical disc (CD), digital versatile optical disc (DVD) or Blu-ray disc (BD)™), flash memory device, and memory card.

[0120] While the present invention has been described above with reference to exemplary embodiments, these embodiments are only for illustrating the technical concept and features of the present invention and should not be construed as limiting the scope of protection of the present invention. Any equivalent variations or modifications made in accordance with the spirit and essence of the present invention should be covered within the scope of protection of the present invention.

Claims

1. A trajectory planning method for a robotic arm, characterized in that, The trajectory planning method includes the following steps: Information acquisition step (S100): At least one sensor is used to acquire environmental information around the robotic arm and the state information of the robotic arm. The path planning step (S200) involves using the fast random tree expansion method to generate the running path of the robotic arm based on the acquired environmental information and the state information of the robotic arm, and then using the gradient descent method to smooth and optimize the running path. as well as In the trajectory control step (S300), a feedback linearization method is used to convert the control system of the robotic arm from a nonlinear system to a linear system. Based on the state information of the robotic arm and the environmental information, a linear quadratic control controller is used to optimize the control gain of the linear system, generating control commands for controlling the robotic arm to track the running path. The path planning step (S200) includes: Random expansion tree initialization step (S220): Generate random expansion tree; The new node generation step (S230) generates a new node; and The random expansion tree update step (S250) involves updating the random expansion tree based on the new node. Furthermore, the random expansion tree update step (S250) specifically includes: The search radius calculation step (S2510) calculates the search radius according to the following formula: ， in, For the search radius, As a preset constant, For the dimension of space, The number of nodes in the randomly expanded tree; The neighborhood node search step (S2520) involves determining the neighborhood range based on the new node as the center, the search radius, and the position of the new node, and then searching for multiple neighborhood nodes within the neighborhood range to obtain a set of neighborhood nodes. The parent node determination step (S2530) involves calculating the total path cost based on the path costs of the neighboring nodes and the path costs from the neighboring nodes to the new node, and selecting the neighboring node with the minimum total path cost as the parent node of the new node. The function for calculating the minimum path cost is defined as follows: ， in, This represents the path cost from the neighboring node to the new node. This represents the path cost of the neighboring nodes; The parent node update step (S2540) involves determining, for each neighboring node, whether the path cost of the corresponding new node is less than the path cost of the neighboring node, and updating the new node to the parent node if the path cost of the new node is less than the path cost of the neighboring node. The determination formula is defined as follows: ， in, The path cost for the neighboring nodes is... The path cost of the new node is... The path cost from the neighboring node to the new node; The node set update step (S2550) updates the random expansion tree by adding the new node and the parent node to the node set of the random expansion tree.

2. The trajectory planning method according to claim 1, characterized in that, The trajectory planning method further includes a monitoring step (S400), which monitors the environmental changes around the robotic arm and the execution state of the robotic arm, and uses the acquired environmental change information and the state change information of the robotic arm as updated environmental information and state information, while iterating the execution information acquisition step, path planning step and / or trajectory control step.

3. The trajectory planning method according to claim 1, characterized in that, In the information acquisition step (S100), the environmental information acquired by the at least one sensor includes the location of obstacles and / or the location of the target, and the state information of the robotic arm includes at least one of the following: the actual position, posture, speed, and direction of movement of the robotic arm at the current moment.

4. The trajectory planning method according to claim 1, characterized in that, The trajectory planning method also includes: The information fusion step, after the information acquisition step, involves fusing the acquired environmental information (which includes perception information collected by at least two sensors) with the fused information as the environmental information.

5. The trajectory planning method according to claim 1, characterized in that, The path planning step (S200) further includes: Workspace initialization step (S210): The workspace of the robotic arm is initialized according to the environmental information and the state information, and the starting position, target position, free space and obstacle area of ​​the robotic arm are defined in the workspace. The random expansion tree initialization step (S220) uses the starting position as the starting point of the random expansion tree to generate the random expansion tree, which includes a set of nodes consisting of multiple tree nodes; In the new node generation step (S230), a random sampling point is obtained in the free space, and the nearest neighbor sampling point is determined in the node set in the random expansion tree. The nearest neighbor sampling point is then extended to the random sampling point by a first preset distance to generate the new node. Collision detection step (S240): Determine whether the extended path between the nearest sampling point and the new node collides with the obstacle area; In the random expansion tree update step (S250), if it is determined in the collision detection step that the extended path does not collide with the obstacle area, the random expansion tree is updated according to the new node generated in the new node generation step. The extension termination step (S260) involves determining whether a preset termination condition is met, and stopping the extension if the condition is met, and generating the running path. The gradient descent optimization step (S270) optimizes the running path by repeatedly adjusting the running path to minimize the cost function value of the running path.

6. The trajectory planning method according to claim 5, characterized in that, In the new node generation step (S230), the sampling formula for the random sampling points is defined as follows: ， in, The nearest neighbor sampling point is the one that is closest to the random sampling point. For the random sampling points; Furthermore, the formula for generating the new node is defined as follows: ， in, The nearest sampling point, for The random sampling points, For the new node, This is the first preset distance.

7. The trajectory planning method according to claim 5, characterized in that, The collision detection step (S240) includes: In the interpolation point generation step (S2410), multiple discrete intermediate joint angles are generated in the joint space as interpolation points. These multiple interpolation points, together with the nearest neighbor sampling point and the random sampling point, construct the extension path. The interpolation point generation formula is defined as follows: ， in, interpolation point For the new node The nearest sampling point The preset number of interpolations; The link pose calculation step (S2420) calculates the pose of the link corresponding to each interpolation point, wherein the pose calculation formula is defined as follows: ， in, For the first The first interpolation point The angle of each joint For the first Transformation matrix of each joint; Detection step (S2430) generates a detection result by detecting whether each link intersects with the obstacle region; wherein, the detection algorithm is defined as follows: ， in, The geometric model of each of the links, A geometric model of the obstacle; If there is an intersection, the detection result is that a collision has occurred; if there is no intersection, the detection result is that no collision has occurred.

8. The trajectory planning method according to claim 5, characterized in that, The gradient descent optimization step (S270) includes: The path cost calculation step (S2710) uses a path cost function to calculate the path cost of the robotic arm's running path. The path cost function is defined as follows: ， in, This is the operating path of the robotic arm. For path speed, Path acceleration The cost function; The path optimization step (S2720) involves calculating the gradient of the path cost function with respect to the running path, and iteratively adjusting the running path along the direction of the negative gradient to update the running path, thereby minimizing the cost value of the path cost function. The formula for calculating the gradient is expressed as: ， in, For the updated running path The learning rate used to determine the magnitude of path adjustment. The gradient of the path, This represents the path during the k-th iteration.

9. The trajectory planning method according to claim 5, characterized in that, In the extended termination step (S260), the preset termination condition includes either of the following two conditions: The distance between any node in the random expansion tree and the target location is less than a second preset distance; the number of updates to the random expansion tree reaches the maximum number of iterations.

10. The trajectory planning method according to claim 1, characterized in that, The trajectory control step (S300) includes: State equation construction step (S310): Construct a state equation representing the motion state of the robotic arm, wherein the state equation is expressed as: ， in, Let be the state vector of the robotic arm. To control the input, Represents a nonlinear function. Represents the input matrix; The linearization process (S320) transforms the state equation into a linear system by constructing a feedback linearization control law, which is defined as follows: ， in, To control the input, For the new control variable, , For the feedback linearization control law, the linear system in the closed-loop case is expressed as: ; In the weight calculation step (S330), a performance index function reflecting the performance of the control system is constructed. The state weight matrix and the control weight matrix are obtained by minimizing the performance index function. The performance index function is defined as follows: ， in, This is the state weight matrix; To control the weight matrix; The time derivative represents the integral of the performance index function over the time domain; In the control gain calculation step (S340), the state weight matrix and the control weight matrix are substituted into the Riccati equation to obtain a symmetric positive definite matrix, and the control gain is calculated based on the symmetric positive definite matrix. Control input optimization step (S350): Optimize the control input based on the control gain. ， in, For the control input, The control gain is... This is the state vector of the robotic arm; In the control command generation step (S360), the optimized control input is used as the control command and substituted into the linear system to form a closed-loop system to control the motion trajectory of the robotic arm.

11. A trajectory planning device for a robotic arm, characterized in that, The trajectory planning device is used to execute the trajectory planning method according to any one of claims 1-10, and the trajectory planning device comprises: An information acquisition unit uses at least one sensor to acquire environmental information around the robot and the state information of the robotic arm. The path planning unit, based on the acquired environmental information and the state information of the robotic arm, performs path planning using a fast stochastic expansion method to generate the robotic arm's running path, and then uses gradient descent to smooth and optimize the running path; and The trajectory control unit uses a feedback linearization method to convert the control system of the robotic arm from a nonlinear system to a linear system, and uses a linear quadratic control controller to optimize the control gain of the linear system based on the state information of the robotic arm and the environmental information, thereby generating control commands for controlling the robotic arm to track the running path.

12. A non-transitory storage medium storing a computer program that, when executed by a processor, can implement the trajectory planning method according to any one of claims 1-10.

13. A computer program product comprising computer instructions that, when executed by a processor, enable the trajectory planning method according to any one of claims 1-10.

Images