Path Planning Method and Device for Charging Robot, Storage Medium and Program Product

Abstract

The present invention discloses a path planning method and device for a charging robot, a storage medium, and a program product. A robot path planning method is provided, which includes: a trajectory collision avoidance condition design step of designing a robot trajectory collision avoidance condition by using a Kalman filter to estimate the running trajectory of an obstacle, classifying the obstacles into static and dynamic ones, and modeling the dynamic obstacle as a moving 3D ellipsoid; a dynamic obstacle avoidance optimization objective function construction step of constructing a robot dynamic obstacle avoidance optimization objective function by using a B-spline curve to describe the motion trajectory of the robot and designing a static obstacle avoidance index, a dynamic obstacle avoidance index, the smoothness of the robot motion, and the self-motion constraint of the robot; and an optimal trajectory obtaining step of obtaining the optimal trajectory of the robot at the current moment by minimizing the dynamic obstacle avoidance optimization objective function by using the L-BFGS method, and taking the first trajectory point as the current target position of the robot.

Description

Technical Field

[0001] The present invention relates to the field of industrial robots, and particularly to a path planning method, a path planning device, a storage medium, and a program product for a charging robot. Background Art

[0002] With the increasing maturity and popularity of autonomous navigation technology, the field of robotics, especially the obstacle avoidance planning method for robotic arms, is undergoing unprecedented in-depth research and rapid development. In dynamic and ever-changing practical application scenarios, the positions and states of obstacles change rapidly, which poses more stringent path planning requirements for specific types of robots such as charging robots. When a charging robot executes a charging task, it not only needs to accurately plan the path from the starting point to the charging interface, but also must perceive and flexibly avoid dynamic obstacles along the way to ensure the safe and efficient completion of the task.

[0003] There has been much research on the robot path planning technology for dynamic scenarios (for example, the Chinese patent application published as CN117666583A discloses a robot path planning method and its device, and an electronic device based on an improved potential field; the Chinese patent application published as CN117647976A discloses a motion path planning method, system, device, and medium for a robot).

[0004] However, enabling a robotic arm to achieve autonomous operation in such a complex and dynamic environment still faces many challenges. The primary problem lies in the real-time perception of dynamic obstacles. How to achieve sufficient accuracy while ensuring the perception efficiency is the key for the robotic arm to obtain comprehensive and accurate environmental information in a complex dynamic environment. Secondly, the design of the planner needs to balance efficiency and accuracy. It should not only quickly respond to the dynamic changes of obstacles but also accurately calculate the motion trajectory of the robotic arm to achieve smooth and efficient shuttling among moving obstacles. In addition, the motion planning of the robotic arm under highly constrained conditions and the obstacle avoidance strategy of the end effector are the current research hotspots and difficulties. How to effectively combine the dynamic characteristics of the robotic arm with the real-time information of obstacles to optimize the obstacle avoidance strategy, reduce the collision risk, and improve the task success rate still requires further research. Summary of the Invention

[0005] Technical Problem

[0006] The purpose of the present invention is to provide, in view of the deficiencies of the prior art, a path planning method for a charging robot for dynamic obstacles. According to the estimation of the running trajectory of obstacles in a complex environment by a Kalman filter, a B-spline curve is used to plan the motion trajectory of the charging robot, and the optimal trajectory of the charging robot is obtained.

[0007] Technical Solution

[0008] According to a first aspect of the present invention, there is provided a path planning method for a charging robot, which includes: a trajectory collision avoidance condition design step (S100), where a Kalman filter is used to estimate the running trajectory of obstacles, the obstacles are classified into dynamic obstacles and static obstacles, and the dynamic obstacles are modeled as moving 3D ellipsoids to design the robot trajectory collision avoidance conditions; a dynamic obstacle avoidance optimization objective function construction step (S200), where a B-spline curve is used to describe the motion trajectory of the robot, and a static obstacle avoidance index, a dynamic obstacle avoidance index, the smoothness of the robot motion, and the robot's own motion constraints are designed to construct the robot dynamic obstacle avoidance optimization objective function; and an optimal trajectory obtaining step (S300), where the L-BFGS method is used to minimize the dynamic obstacle avoidance optimization objective function to obtain the optimal trajectory of the robot at the current moment, and the first trajectory point is used as the current target position of the robot; when the body state and environmental state of the robot at the next moment are obtained, the dynamic obstacle avoidance optimization objective function construction step (S200) is repeated until the robot obstacle avoidance task is completed.

[0009] As a preferred solution, according to the path planning method of the present invention, the trajectory collision avoidance condition design step includes: a Kalman filter list construction step (S110), where the spatial point cloud information is extracted into point cloud clusters by using density-based spatial clustering of applications with noise (DBSCAN) technology, and the point cloud clusters are constructed into a point cloud cluster sequence at time intervals . For each point cloud cluster at time, a Kalman filter is used to construct a Kalman filter list at time to construct an obstacle point cloud cluster, a point cloud cluster sequence, and a Kalman filter list corresponding to the time points: a point cloud cluster matching and tracking history update step (S120), for each time, first, all the Kalman filters in are propagated forward, and then the K-nearest neighbor search technology is used to associate the cluster centroid in the current frame with the position propagated forward by the Kalman filter. In the case where the point cloud cluster is successfully associated, the point cloud cluster inherits the tracking history of its associated object . In the case where the point cloud cluster is unsuccessfully associated, it is assumed that the uncorrelated point cloud cluster is a newly emerging object, and a new Kalman filter is created for it; an obstacle state classification step (S130), where the motion state of the point cloud cluster is obtained by comparing the point clouds of two frames separated by seconds to classify the obstacles into dynamic obstacles and static obstacles; a dynamic environment model establishment step (S140), for dynamic obstacles, which are described as having a speed 3D ellipsoid:

[0010] ,

[0011] wherein, ,

[0012] wherein, represents the position of the center of the ellipsoid in three-dimensional space, , and respectively represent the axis lengths of the ellipsoid in the x , y and z directions, represents the expansion radius.

[0013] As a preferred solution, according to the path planning method of the present invention, the obstacle state classification step includes: K-D tree construction step (S1301): constructing a K-D tree by extracting dense, unfiltered point clouds from the robot coordinate system at the moment; global nearest neighbor distance calculation step (S1302), calculating the global nearest neighbor distance of each point in the current frame belonging to the point cloud cluster through the K-D tree; obstacle state determination step (S1303), calculating the speed of each point using , counting the number of point cloud points as the number of dynamic points of the point cloud cluster, and determining the obstacle corresponding to the point cloud cluster as a dynamic obstacle when the number of dynamic points is greater than the threshold , and determining the obstacle corresponding to the point cloud cluster as a static obstacle when the number of dynamic points is less than or equal to the threshold ; dynamic obstacle occlusion processing step (S1304), associating the current point cloud cluster with its corresponding point cloud cluster through tracking history and projecting it onto the image plane. When the depth value of the projection point corresponding to the three-dimensional space point is less than the depth of the projection point corresponding to the three-dimensional space point , all the point cloud points in the current point cloud cluster are subjected to dynamic determination, and when the depth value of the projection point corresponding to the three-dimensional space point is greater than or equal to the depth of the projection point corresponding to the three-dimensional space point , for the point cloud points belonging to other point cloud clusters, project them onto On the image plane at a moment, the projection rule is as follows:

[0014] ,

[0015] wherein, represents the extrinsic parameter matrix of the camera at a moment, represents the intrinsic parameter matrix of the camera, represents a three-dimensional space point in the camera coordinate system. When the vertical projection distance of the three-dimensional space point is greater than the depth value of the projection point corresponding to the three-dimensional space point , the point cloud point is occluded and no dynamic determination is performed.

[0016] As a preferred solution, according to the path planning method of the present invention, the step (S200) of constructing the dynamic obstacle avoidance optimization objective function includes: a step (S210) of constructing the end motion trajectory, which is determined by the curve order , the number of curve control points and the uniform knot vector to determine the B-spline curve trajectory of the robot end motion. For each node interval , there exists order B-spline basis functions, which satisfy the following differential equation:

[0017] ,

[0018] A step (S220) of constructing the optimization objective of the B-spline curve trajectory. The optimization objective of the B-spline curve trajectory is:

[0019] ,

[0020] wherein, , and are the total weight coefficients of different optimization sub-objectives.

[0021] As a preferred solution, according to the path planning method of the present invention, the optimization objective includes: a smoothness evaluation sub-objective, a constraint sub-objective, a static obstacle avoidance sub-objective, and a dynamic obstacle avoidance sub-objective.

[0022] As a preferred solution, according to the path planning method of the present invention, the smoothness evaluation sub-objective evaluates the smoothness of the trajectory according to the acceleration and jerk of the trajectory, and is represented by the following formula:

[0023] ,

[0024] wherein, is the acceleration of the -th control point of the B-spline curve trajectory, is the jerk of the -th control point of the B-spline curve trajectory.

[0025] As a preferred solution, according to the path planning method of the present invention, the constraint sub-goal ensures that the generated trajectory conforms to the desired speed limit and acceleration limit, which is expressed by the following formula:

[0026] ,

[0027] wherein, is the maximum acceleration of the B-spline curve trajectory, is the maximum speed of the B-spline curve trajectory, is an obstacle function or a penalty function.

[0028] As a preferred solution, according to the path planning method of the present invention, the static obstacle avoidance sub-goal ensures that the generated trajectory can avoid static obstacles, which is expressed by the following formula:

[0029] ,

[0030] wherein, is a pair of anchor points and vectors of the B-spline curve trajectory, is the number of pairs of anchor points and vectors of the B-spline curve trajectory.

[0031] As a preferred solution, according to the path planning method of the present invention, the dynamic obstacle avoidance sub-goal ensures that the generated trajectory can avoid dynamic obstacles, which is expressed by the following formula:

[0032] ,

[0033] wherein, is the position, speed and size of the -th 3D ellipsoid of the dynamic obstacle, is the distance between the control point of the B-spline curve trajectory and the center of the obstacle, and is expressed by the following formula:

[0034] .

[0035] As a preferred solution, according to the path planning method of the present invention, the optimal trajectory obtaining step (S300) includes: a control instruction calculation step (S310), which minimizes the optimal B-spline curve trajectory obtained by solving the robot dynamic obstacle avoidance optimization objective function constructed in the dynamic obstacle avoidance optimization objective function construction step through the L-BFGS method, to calculate the control instruction of the robot's manipulator at time ; and a control instruction input step (S320), which inputs the position control instruction obtained in the control instruction calculation step into the robot's manipulator.

[0036] According to a second aspect of the present invention, there is provided a path planning device for a charging robot, which includes: a trajectory collision avoidance condition design unit (100), which estimates the running trajectory of obstacles by using a Kalman filter, classifies the obstacles into dynamic obstacles and static obstacles, and models the dynamic obstacles as moving 3D ellipsoids to design the robot trajectory collision avoidance conditions; a dynamic obstacle avoidance optimization objective function construction unit (200), which constructs a robot dynamic obstacle avoidance optimization objective function by using a B-spline curve to describe the robot's motion trajectory and designing static obstacle avoidance indexes, dynamic obstacle avoidance indexes, robot motion smoothness, and robot self-motion constraints; an optimal trajectory obtaining unit (300), which minimizes the dynamic obstacle avoidance optimization objective function by using the L-BFGS method to obtain the optimal trajectory of the robot at the current moment and takes the first trajectory point as the current target position of the robot; and a repeated optimization objective function construction unit (400), which, when obtaining the body state and environmental state of the robot at the next moment, repeatedly constructs the dynamic obstacle avoidance optimization objective function until the robot obstacle avoidance task is completed.

[0037] According to a third aspect of the present invention, there is provided a non-transitory storage medium that stores a computer program, and when the computer program is executed by a processor, it can implement the path planning method for a charging robot according to the first aspect of the present invention.

[0038] According to a fourth aspect of the present invention, there is provided a computer program product, the computer program product includes computer instructions, and when the computer instructions are executed by a processor, it can implement the path planning method for a charging robot according to the first aspect of the present invention.

[0039] Beneficial technical effects

[0040] In an automated charging task that requires avoiding dynamic obstacles, an estimation method for static and dynamic obstacles is designed, a trajectory optimization method for the end of a charging robotic arm is proposed, and this method is executed in the form of a model predictive controller, which can handle the desired operating effects and contact constraints. The mathematical description of the dynamic obstacles is incorporated into the trajectory optimization task to ensure the safe execution of the automatic charging task. The obstacle avoidance effect of this technology is significantly improved in both static and dynamic environments. Compared with other trajectory optimization methods, the trajectory optimization method has better real-time performance and has the advantages of strong generality and easy use.

[0041] Other features of the present invention will become clear from the following description of exemplary embodiments with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 is a flowchart showing a path planning method for a charging robot according to an embodiment of the present invention;

[0043] Figure 2 is a flowchart showing the design steps of trajectory collision avoidance conditions according to an embodiment of the present invention;

[0044] Figure 3 is a flowchart showing the obstacle state classification steps according to an embodiment of the present invention;

[0045] Figure 4 is a flowchart showing the construction steps of a dynamic obstacle avoidance optimization objective function according to an embodiment of the present invention;

[0046] Figure 5 is a flowchart showing the steps of obtaining an optimal trajectory according to an embodiment of the present invention;

[0047] Figure 6 is a schematic diagram showing the software structure of a path planning device for a charging robot according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0048] The following describes in detail the exemplary embodiments of the present invention with reference to the accompanying drawings. It should be noted that unless otherwise specifically stated, the relative configurations of components, numerical representations, and numerical values described in these embodiments do not limit the scope of the present invention.

[0049] The path planning method for a charging robot of the present invention can be implemented by a processor in an automatic charging system executing a computer program stored in a memory of the automatic charging system. As an alternative, it can also be implemented by the automatic charging system communicating with a server, and a processor in the server executing a computer program stored on the server and real-time feedback the program execution result to the automatic charging system.

[0050] In the present invention, the term "unit" may refer to a software environment, a hardware environment, or a combination of a software and a hardware environment. In a software environment, the term "unit" refers to a functionality, an application, a software module, a function, a routine, a set of instructions, or a program that can be executed by a programmable processor, such as a microprocessor, a central processing unit (CPU), or a specially designed programmable device, or a controller. The 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, a circuit, a component, a physical structure, a system, a module, or a subsystem. According to a specific embodiment, the term "unit" may include mechanical, optical, or electrical components, or any combination thereof. The term "unit" may include active (e.g., transistors) or passive (e.g., capacitors) components. The term "unit" may include a semiconductor device having a substrate and other material layers having various conductive concentrations. It may include a CPU or a programmable processor that can execute a program stored in the memory to perform a specified function. The term "unit" may include logic elements (e.g., AND, OR) implemented by transistor circuits or any other switching circuits. In a combination of a software and a hardware environment, the term "unit" or "circuit" refers to any combination of the software and hardware environments as described above. In addition, the terms "element", "component", "part", or "device" may also refer to a "circuit" integrated or not integrated with a packaging material.

[0051] The following will refer to Figures 1-5 Describe a path planning method for a charging robot according to an embodiment of the present invention.

[0052] In step S100, a Kalman filter is used to estimate the running trajectories of obstacles in a complex environment, the obstacles are classified into dynamic obstacles and static obstacles, the dynamic obstacles are modeled as moving 3D ellipsoids, and the trajectory collision avoidance conditions of the charging robot are designed.

[0053] As Figure 2 shown, step S100 of designing the trajectory collision avoidance conditions of the robot includes the following sub-steps S110 - S140.

[0054] In step S110, an obstacle point cloud cluster, a point cloud cluster sequence, and a list of Kalman filters at corresponding time points are constructed. The spatial point cloud information formed by sensing with a depth camera is extracted into point cloud clusters using density-based spatial clustering of applications with noise (DBSCAN), and further the point cloud clusters are constructed into a point cloud cluster sequence according to a time interval ; for each point cloud cluster at time, a Kalman filter constructed by a conservative motion model is used, and further constructed List of Kalman filters at a moment 。

[0055] In step S120, point cloud cluster matching and tracking history update are performed. For each moment , first, all Kalman filters in are propagated forward, and then the K-nearest neighbor search technique is used to associate the cluster centroid in the current frame with the position propagated forward by the Kalman filter. If the point cloud cluster is successfully associated, the point cloud cluster will inherit the tracking history of its associated object . Otherwise, assume that the unassociated point cloud cluster is a newly emerged object and create a new Kalman filter for it.

[0056] In step S130, the obstacle state is classified. The motion state of the point cloud cluster is obtained by comparing the point clouds of two frames separated by seconds.

[0057] As Figure 3 shown, step S130 of obstacle classification includes the following sub-steps S1301 - S1304.

[0058] First, in S1301, a K-D tree of the point cloud is constructed. Specifically, dense and unfiltered point cloud is extracted from the coordinate system of the charging robot at moment to construct the K-D tree.

[0059] In S1302, the global nearest neighbor distance of each point cloud point is calculated. Specifically, the global nearest neighbor distance of each point belonging to the point cloud cluster in the current frame at is calculated through the K-D tree.

[0060] In step S1301, each point cloud point is evaluated to determine the obstacle state. Specifically, is used to calculate the speed of each point, and the number of point cloud points is counted, denoted as the number of dynamic points of the point cloud cluster. If the number of dynamic points is greater than the threshold , the obstacle corresponding to the point cloud cluster is a dynamic obstacle; otherwise, the obstacle corresponding to the point cloud cluster is a static obstacle. It should be noted that when the field of view changes between two frames, only the number of dynamic points appearing in the overlapping area is counted.

[0061] In step S1304, the occlusion caused by dynamic obstacles is processed. The current point cloud cluster is associated with its corresponding point cloud cluster through the tracking history and projected onto the image plane. If the three-dimensional space point The corresponding projection point has a depth value less than that of the projection point corresponding to the 3D space point The corresponding projection point If the depth is less, all the point cloud points in the current point cloud cluster can be used for dynamic determination; otherwise, for the point cloud points belonging to other point cloud clusters , first project it onto the image plane at the moment. The specific projection rule is as follows:

[0062] ,

[0063] where represents the external camera parameter matrix at the moment , represents the internal camera parameter matrix, represents the 3D space point in the camera coordinate system. When the vertical projection distance of the 3D space point is greater than the depth value of the projection point corresponding to the 3D space point The corresponding projection point , this point cloud point is considered occluded and no dynamic determination is performed.

[0064] After completing step S130, the process proceeds to step S140. In step S140, a dynamic environment model is established. Specifically, for a dynamic obstacle, it is described as a 3D ellipsoid with a velocity :

[0065] ,

[0066] where , represents the position of the center of the ellipsoid in 3D space, , and respectively represent the axis lengths of the ellipsoid in the x, y, and z directions, represents the inflation radius.

[0067] After completing the design of the trajectory collision avoidance conditions for the charging robot, the path planning method proceeds to step S200. In step S200, a B-spline curve is used to describe the motion trajectory of the charging robot, and static obstacle avoidance metrics, dynamic obstacle avoidance metrics, charging robot motion smoothness, and charging robot self-motion constraints are designed to construct an optimization objective function for dynamic obstacle avoidance of the charging robot. A B-spline curve is a curve representation method widely used in mathematics and computer graphics. It is generated by a linear combination of a set of control points through B-spline basis functions and has high flexibility and smoothness. The B-spline curve is represented by a linear combination of B-spline basis functions. Specifically, given n + 1 control points P0, P1, ..., Pn, the expression of a k-th order B-spline curve is:

[0068] P(u)=∑(n,i=0)PiNi,k(u),

[0069] where Ni,k(u) is the B-spline basis function and is calculated from the control points through a specific algorithm.

[0070] As Figure 4 shown, step S200 of constructing the optimization objective function for dynamic obstacle avoidance of the charging robot includes the following sub-steps S210 - S220.

[0071] In step S210, construct the end motion trajectory of the charging robot based on B-spline. Specifically, the B-spline curve trajectory of the end motion of the charging robot is uniquely determined by the curve order , the number of curve control points and the uniform knot vector . For each knot interval , there exists order B-spline basis functions that satisfy the following differential equation:

[0072] ,

[0073] In step S220, construct the optimization objective of the B-spline curve trajectory. The optimization objective of the B-spline curve trajectory is specifically:

[0074] ,

[0075] where , and are the full weight coefficients of different optimization sub-objectives. The optimization objective specifically includes a smoothness evaluation sub-objective, a constraint sub-objective, a static obstacle avoidance sub-objective, and a dynamic obstacle avoidance sub-objective.

[0076] The above sub-goals will be described in detail below.

[0077] The smoothness evaluation sub-goal evaluates the smoothness of the trajectory based on the acceleration and jerk of the trajectory, which is specifically expressed by the following formula:

[0078] ,

[0079] where, is the acceleration of the th control point of the B-spline curve trajectory, is the jerk of the th control point of the B-spline curve trajectory.

[0080] The constraint sub-goal ensures that the generated trajectory conforms to the desired speed limit and acceleration limit, which is specifically expressed by the following formula:

[0081] ,

[0082] where, is the maximum acceleration of the B-spline curve trajectory, is the maximum speed of the B-spline curve trajectory, is the obstacle function or penalty function.

[0083] The static obstacle avoidance sub-goal ensures that the generated trajectory can avoid static obstacles, which is specifically expressed by the following formula:

[0084] ,

[0085] where, is a pair of anchor points and vectors of the B-spline curve trajectory, is the number of pairs of anchor points and vectors of the B-spline curve trajectory.

[0086] The dynamic obstacle avoidance sub-goal ensures that the generated trajectory can avoid dynamic obstacles, which is specifically expressed by the following formula:

[0087] ,

[0088] where, is the position, velocity and size of the th three-dimensional ellipsoid of the dynamic obstacle, is the distance between the control point of the B-spline curve trajectory and the center of the obstacle, which is specifically expressed by the following formula:

[0089] .

[0090] After step S200 of constructing the dynamic obstacle avoidance optimization objective function of the charging robot, the path planning method proceeds to step S300. In S300, the L-BFGS method is used to minimize the dynamic obstacle avoidance optimization objective function of the charging robot, obtaining the optimal trajectory of the charging robot at the current moment, and taking the first trajectory point as the current target of the charging robot; when obtaining the body state and environmental state of the charging robot at the next moment, step S220 is repeatedly executed.

[0091] As Figure 5 shown, the optimal trajectory obtaining step S300 includes sub-steps S310 - S320.

[0092] In S310, the optimal B-spline curve trajectory solved by minimizing the dynamic obstacle avoidance optimization objective function of the charging robot constructed in the above step S200 through the L-BFGS method is used to calculate the control command of the charging robotic arm at the moment of.

[0093] In S320, the robotic arm joint position control command calculated in the above step S310 is input into the charging robotic arm; at the moment, return to step S220 until the obstacle avoidance task of the charging robotic arm is completed.

[0094] Next, a path planning device for a charging robot according to an embodiment of the present invention will be described with reference to Figure 6 the following.

[0095] As Figure 6 shown, the path planning device 10 according to an embodiment of the present invention includes a trajectory collision avoidance condition design unit 100, a dynamic obstacle avoidance optimization objective function construction unit 200, an optimal trajectory obtaining unit 300, and a repeated optimization objective function construction unit 400.

[0096] The trajectory collision avoidance condition design unit 100 estimates the running trajectory of obstacles by using a Kalman filter, classifies the obstacles into dynamic obstacles and static obstacles, and models the dynamic obstacles as moving 3D ellipsoids to design the robot trajectory collision avoidance conditions. The dynamic obstacle avoidance optimization objective function construction unit 200 constructs the dynamic obstacle avoidance optimization objective function of the robot by using B-spline curves to describe the movement trajectory of the robot and designing static obstacle avoidance indexes, dynamic obstacle avoidance indexes, robot movement smoothness, and robot self-movement constraints. The optimal trajectory obtaining unit 300 minimizes the dynamic obstacle avoidance optimization objective function by using the L-BFGS method to obtain the optimal trajectory of the robot at the current moment, and takes the first trajectory point as the current target position of the robot. The repeated optimization objective function construction unit 400 repeatedly constructs the dynamic obstacle avoidance optimization objective function when obtaining the body state and environmental state of the robot at the next moment until the obstacle avoidance task of the robot is completed.

[0097] With the path planning method and path planning device according to the above embodiments, in an automated charging task that needs to avoid dynamic obstacles, an estimation method for static and dynamic obstacles is designed, a method for optimizing the trajectory of the end of the charging robotic arm is proposed, and this method is executed in the form of a model predictive controller, which can handle the desired operating effect and contact constraints, and incorporates the mathematical description of the dynamic obstacle into the trajectory optimization task to ensure the safe execution of the automatic charging task. The obstacle avoidance effect of this technology is significantly improved in both static and dynamic environments. Compared with other trajectory optimization methods, the trajectory optimization method has better real-time performance and has the advantages of strong versatility and easy use.

[0098] Other embodiments

[0099] Embodiments of the present invention can also be implemented by a computer of a system or device that reads and executes computer-executable instructions (such as one or more programs) recorded on a storage medium (which can also be more completely referred to as a "non-transitory computer-readable storage medium") to perform one or more functions of the above embodiments, and / or includes one or more circuits (such as an application specific integrated circuit (ASIC)) for performing one or more functions of the above embodiments. Moreover, embodiments of the present invention can be implemented by a method in which the computer of the system or device, such as by reading and executing the computer-executable instructions from the storage medium, performs one or more functions of the above embodiments, and / or controls the one or more circuits to perform one or more functions of the above embodiments. The computer may include one or more processors (such as a central processing unit (CPU), a microprocessing unit (MPU)), and may include a network of separate computers or 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, for example, one or more of a hard disk, a random access memory (RAM), a read-only memory (ROM), the memory of a distributed computing system, an optical disc (such as a compact disc (CD), a digital versatile disc (DVD), or a Blu-ray Disc (BD)™), a flash device, and a memory card, etc.

[0100] Embodiments of the present invention can also be implemented by the following method, that is, by providing software (including a computer program product of computer programs / instructions) that performs the functions of the above embodiments to a system or device through a network or various storage media, and the computer (central processing unit (CPU), microprocessing unit (MPU)) of the system or device reads and executes the computer program / instructions.

[0101] Although the present invention has been described with reference to exemplary embodiments, it should be understood that the present invention is not limited to the disclosed exemplary embodiments. The scope of the appended claims should be given the broadest interpretation so as to cover all such variations and equivalent structures and functions.

Claims

1. A path planning method for a charging robot, comprising: The trajectory collision avoidance condition design step is to estimate the obstacle trajectory by using a Kalman filter, classify obstacles into dynamic obstacles and static obstacles, and model dynamic obstacles as moving 3D ellipsoids to design the robot trajectory collision avoidance conditions; The steps of constructing the dynamic obstacle avoidance optimization objective function are as follows: the robot's dynamic obstacle avoidance optimization objective function is constructed by using a B-spline curve to describe the robot's motion trajectory, and by designing static obstacle avoidance indicators, dynamic obstacle avoidance indicators, robot motion smoothness, and the robot's own motion constraints; as well as The optimal trajectory acquisition step is to obtain the optimal trajectory of the robot at the current moment by minimizing the dynamic obstacle avoidance optimization objective function using the L-BFGS method, and use the first trajectory point as the current target position of the robot; When the robot's main state and environmental state at the next moment are obtained, the dynamic obstacle avoidance optimization objective function construction steps are repeated until the robot's obstacle avoidance task is completed. Among them, for dynamic obstacles, they are described as having speed The 3D ellipsoid: ， in, , and represents the position of the ellipsoid center in three-dimensional space, , and Respectively represent the ellipsoid in x , y and z The axis length in the direction, represents the expansion radius, Furthermore, in the step of constructing the dynamic obstacle avoidance optimization objective function, the dynamic obstacle avoidance sub-objective ensures that the generated trajectory can avoid dynamic obstacles, which is expressed by the following formula: ， in, For the The position, velocity and size of the 3D ellipsoid of the dynamic obstacle, is the distance between the control point of the B-spline curve trajectory and the obstacle center, and is expressed by the following formula: 。 2. The path planning method according to claim 1, wherein: The design steps of trajectory collision avoidance conditions include: The Kalman filter list construction step extracts the spatial point cloud information using density-based noise-containing spatial clustering technology. Point cloud cluster , and the point cloud clusters are divided into time intervals Constructed as a point cloud cluster sequence ,for For each point cloud cluster at the moment, a Kalman filter is used to construct List of Kalman filters at time , to construct the obstacle point cloud cluster, point cloud cluster sequence and Kalman filter list of corresponding time points: Point cloud cluster matching and tracking history update step, for each moment , first propagate forward Then, the K-neighbor search technique is used to associate the cluster centroid in the current frame to the position where the Kalman filter propagates forward, and the clusters in the point cloud are When the association is successful, the point cloud cluster inherits the tracking history of its associated object. , in the point cloud cluster In case of association failure, the unrelated point cloud clusters are assumed to be newly appeared objects and a new Kalman filter is created for them; The obstacle state classification step is to compare the interval 1 second to obtain the motion state of the point cloud cluster, so as to classify the obstacles into dynamic obstacles and static obstacles; and Steps for building a dynamic environment model: for dynamic obstacles, describe them as objects with speed 3D ellipsoid.

3. The path planning method according to claim 2, wherein: The obstacle status classification step comprises: The KD tree construction step is to Extract a dense, unfiltered point cloud in the robot coordinate system at the moment to construct the KD tree; The global nearest neighbor distance calculation step is to calculate the current frame through the KD tree The moment belongs to the point cloud cluster Each point The global nearest neighbor distance ; Obstacle status determination steps, using Calculate the speed of each point and count The number of point cloud points is used as the number of dynamic points of the point cloud cluster. When the number of dynamic points is greater than the threshold In the case of , the obstacle corresponding to the point cloud cluster is determined as a dynamic obstacle, and when the number of dynamic points is less than or equal to the threshold , determining the obstacle corresponding to the point cloud cluster as a static obstacle; and Dynamic obstacle occlusion processing step, by tracking the history of the current point cloud cluster Associated to its corresponding point cloud cluster And project it onto the image plane, at the three-dimensional space point The corresponding projection point The depth value is less than the three-dimensional space point The corresponding projection point In the case of a depth of , all point cloud points in the current point cloud cluster are dynamically determined, and in the three-dimensional space point The corresponding projection point The depth value is greater than or equal to the three-dimensional space point The corresponding projection point In the case of depth, for point cloud points belonging to other point cloud clusters , projecting it onto On the image plane at the moment, the projection rule is: ， in, express The camera extrinsic matrix at the moment, represents the intrinsic parameter matrix of the camera, Represents a three-dimensional space point in the camera coordinate system. The vertical projection distance of is greater than the three-dimensional space point The corresponding projection point In the case of a depth value of , the point cloud point is occluded and no dynamic determination is performed.

4. The path planning method according to claim 1, wherein: The dynamic obstacle avoidance optimization objective function construction step includes: Steps for constructing the terminal motion trajectory: by the curve order , the number is The curve control points and uniform node vector Determine the B-spline curve trajectory of the robot end motion , for each node interval ,exist The B-spline basis function satisfies the following differential equation: ,as well as B-spline curve trajectory optimization target construction steps, B-spline curve trajectory The optimization goal is: ， in, , and is the total weight coefficient of different optimization sub-objectives.

5. The path planning method according to claim 4, wherein: The optimization objectives include: a smoothness evaluation sub-objective, a constraint sub-objective, a static obstacle avoidance sub-objective, and a dynamic obstacle avoidance sub-objective.

6. The path planning method according to claim 5, wherein: The smoothness evaluation sub-goal evaluates the smoothness of the trajectory according to the acceleration and jerk of the trajectory, which is expressed by the following formula: ， in, is the B-spline curve trajectory The acceleration of the control point, is the B-spline curve trajectory The acceleration of a control point.

7. The path planning method according to claim 5, wherein: The constraint sub-goal ensures that the generated trajectory meets the desired speed limit and acceleration limit, which is expressed as follows: ， in, is the maximum acceleration of the B-spline trajectory, is the maximum velocity of the B-spline curve trajectory, is a barrier function or a penalty function.

8. The path planning method according to claim 5, wherein: The static obstacle avoidance sub-goal ensures that the generated trajectory can avoid static obstacles, which is expressed as follows: ， in, is a pair of anchor points and vectors of the B-spline curve trajectory, is the number of anchor point and vector pairs of the B-spline curve trajectory.

9. The path planning method according to claim 1, wherein: The steps to obtain the optimal trajectory include: The control instruction calculation step minimizes the optimal B-spline curve trajectory solved by the robot dynamic obstacle avoidance optimization objective function constructed in the dynamic obstacle avoidance optimization objective function construction step by using the L-BFGS method to calculate the robot's mechanical arm at time control instructions; and A control instruction input step is to input the position control instruction obtained in the control instruction calculation step into the mechanical arm of the robot.

10. A path planning device for a charging robot, comprising: The trajectory collision avoidance condition design unit estimates the obstacle trajectory by using a Kalman filter, classifies obstacles into dynamic obstacles and static obstacles, and models dynamic obstacles as moving 3D ellipsoids to design the robot trajectory collision avoidance conditions; The dynamic obstacle avoidance optimization objective function construction unit uses B-spline curves to describe the robot's motion trajectory and designs static obstacle avoidance indicators, dynamic obstacle avoidance indicators, robot motion smoothness, and the robot's own motion constraints to construct the robot's dynamic obstacle avoidance optimization objective function; The optimal trajectory acquisition unit obtains the optimal trajectory of the robot at the current moment by minimizing the dynamic obstacle avoidance optimization objective function using the L-BFGS method, and uses the first trajectory point as the current target position of the robot; as well as Repeat the optimization objective function construction unit. When the robot's main state and environmental state at the next moment are obtained, the dynamic obstacle avoidance optimization objective function construction is repeated until the robot's obstacle avoidance task is completed. Among them, for dynamic obstacles, they are described as having speed The 3D ellipsoid: ， in, , and represents the position of the ellipsoid center in three-dimensional space, , and Respectively represent the ellipsoid in x , y and z The axis length in the direction, represents the expansion radius, Furthermore, in the dynamic obstacle avoidance optimization objective function construction unit, the dynamic obstacle avoidance sub-objective ensures that the generated trajectory can avoid dynamic obstacles, which is expressed by the following formula: ， in, For the The position, velocity and size of the 3D ellipsoid of the dynamic obstacle, is the distance between the control point of the B-spline curve trajectory and the obstacle center, and is expressed by the following formula: 。 11. A non-transitory storage medium storing a computer program, which, when executed by a processor, can implement the path planning method for a charging robot according to any one of claims 1 to 9. 12 . A computer program product, comprising computer instructions, which, when executed by a processor, can implement the path planning method for a charging robot according to any one of claims 1 to 9.

Images