Path planning method and device for charging robot, storage medium and program product

Abstract

The invention discloses a path planning method and device for a charging robot, a storage medium and a program product. The invention provides a robot path planning method, which comprises the following steps of: designing a track collision avoidance condition, namely estimating a moving track of an obstacle by adopting a Kalman filter, classifying the obstacle in a static state and a dynamic state, and modeling the dynamic obstacle into a mobile 3D (Three Dimensional) ellipsoid to design the track collision avoidance condition of the robot; a dynamic obstacle avoidance optimization objective function construction step: describing a motion track of the robot by adopting a B spline curve, and designing a static obstacle avoidance index, a dynamic obstacle avoidance index, robot motion smoothness and robot motion constraints to construct a robot dynamic obstacle avoidance optimization objective function; and an optimal trajectory obtaining step: minimizing the dynamic obstacle avoidance optimization objective function by adopting an L-BFGS method to obtain the optimal trajectory of the robot at the current moment, 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 in particular 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 of robotic arms, is undergoing unprecedented in-depth research and rapid development. In dynamic and changeable practical application scenarios, the position and state of obstacles change rapidly, which puts forward more stringent path planning requirements for specific types of robots such as charging robots. When performing charging tasks, charging robots must not only accurately plan the path from the starting point to the charging interface, but also must perceive and flexibly avoid dynamic obstacles along the way in real time to ensure the safe and efficient completion of the task.

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

[0004] However, there are still many challenges in enabling the robot to achieve autonomous operation in such a complex and dynamic environment. The primary difficulty lies in the real-time perception of dynamic obstacles. How to ensure perception efficiency while achieving sufficient accuracy is the key for the robot to obtain comprehensive and accurate environmental information in a complex dynamic environment. Secondly, the design of the planner must take into account both efficiency and accuracy. It must not only respond quickly to the dynamic changes of obstacles, but also accurately calculate the motion trajectory of the robot to achieve smooth and efficient movement between moving obstacles. In addition, the motion planning of the robot under highly constrained conditions and the obstacle avoidance strategy of the end effector are the hot spots and difficulties of current research. How to effectively combine the dynamic characteristics of the robot with the real-time information of obstacles to optimize the obstacle avoidance strategy, reduce the risk of collision, and improve the success rate of the mission requires further research. Summary of the invention

[0005] Technical issues The purpose of the present invention is to address the deficiencies in the prior art and provide a charging robot path planning method for dynamic obstacles. According to the Kalman filter's estimation of the obstacle trajectory in a complex environment, the B-spline curve is used to plan the motion trajectory of the charging robot and obtain the optimal trajectory of the charging robot.

[0006] Technical Solution According to a first aspect of the present invention, a path planning method for a charging robot is provided, which comprises: a trajectory avoidance condition design step (S100), wherein the obstacle running trajectory is estimated by adopting a Kalman filter, the obstacles are classified into dynamic obstacles and static obstacles, and the dynamic obstacles are modeled as a moving 3D ellipsoid to design the robot trajectory avoidance condition; a dynamic obstacle avoidance optimization objective function construction step (S200), wherein the robot dynamic obstacle avoidance optimization objective function is constructed by adopting a B-spline curve to describe the robot's motion trajectory, and static obstacle avoidance indicators, dynamic obstacle avoidance indicators, robot motion smoothness, and robot's own motion constraints; and an optimal trajectory acquisition step (S300), wherein the dynamic obstacle avoidance optimization objective function is minimized by adopting an L-BFGS method 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 robot's body state and environmental state at the next moment are obtained, the dynamic obstacle avoidance optimization objective function construction step (S200) is repeated until the robot's obstacle avoidance task is completed.

[0007] 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), extracting the spatial point cloud information into 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 obstacle point cloud clusters, point cloud cluster sequences and Kalman filter lists at corresponding time points: point cloud cluster matching and tracking history update step (S120), 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 the case of association failure, it is assumed that the irrelevant point cloud cluster is a newly appeared object, and a new Kalman filter is created for it; the obstacle state classification step (S130) is performed by comparing the intervals The point cloud of two frames per second is used to obtain the motion state of the point cloud cluster, so as to classify the obstacles into dynamic obstacles and static obstacles; the dynamic environment model establishment step (S140) describes the dynamic obstacles as having a velocity The 3D ellipsoid: , in, , in, 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.

[0008] As a preferred solution, according to the path planning method of the present invention, the obstacle state classification step includes: a KD tree construction step (S1301): A dense, unfiltered point cloud is extracted from the robot coordinate system at the time to construct a KD tree; a global nearest neighbor distance calculation step (S1302) is performed to calculate the current frame in the KD tree. The moment belongs to the point cloud cluster Each point The global nearest neighbor distance ; Obstacle state determination step (S1303), 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 In the case of, the obstacle corresponding to the point cloud cluster is determined to be a static obstacle; the dynamic obstacle occlusion processing step (S1304) is to track 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.

[0009] As a preferred solution, according to the path planning method of the present invention, the dynamic obstacle avoidance optimization objective function construction step (S200) includes: the terminal motion trajectory construction step (S210), 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: , B-spline curve trajectory optimization target construction step (S220), B-spline curve trajectory The optimization goal is: , in, , and is the total weight coefficient of different optimization sub-objectives.

[0010] As a preferred solution, according to the path planning method of the present invention, 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.

[0011] As a preferred solution, according to the path planning method of the present invention, 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.

[0012] As a preferred solution, according to the path planning method of the present invention, the constraint sub-goal ensures that the generated trajectory meets the desired speed limit and acceleration limit, which is expressed by the following formula: , 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.

[0013] 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: , 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.

[0014] 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: , 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: .

[0015] 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), in which 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 is minimized by the L-BFGS method to calculate the robot arm at time and a control instruction input step (S320), inputting the position control instruction obtained in the control instruction calculation step into the robot arm.

[0016] According to a second aspect of the present invention, a path planning device for a charging robot is provided, comprising: a trajectory avoidance condition design unit (100), which estimates the obstacle running trajectory by using a Kalman filter, classifies the obstacles into dynamic obstacles and static obstacles, and models the dynamic obstacles as a moving 3D ellipsoid to design the robot trajectory avoidance condition; a dynamic obstacle avoidance optimization objective function construction unit (200), which describes the robot's motion trajectory by using a B-spline curve, and designs a static obstacle avoidance index, a dynamic obstacle avoidance index, a robot motion smoothness, and a robot's own motion constraint to construct a robot dynamic obstacle avoidance optimization objective function; an optimal trajectory acquisition unit (300), which minimizes the dynamic obstacle avoidance optimization objective function by using an L-BFGS method to obtain the optimal trajectory of the robot at the current moment, and uses the first trajectory point as the current target position of the robot; and a repeated optimization objective function construction unit (400), which repeats the dynamic obstacle avoidance optimization objective function construction when the robot's body state and environmental state at the next moment are obtained, until the robot's obstacle avoidance task is completed.

[0017] According to a third aspect of the present invention, there is provided 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 the first aspect of the present invention.

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

[0019] Beneficial technical effects In the automated charging task that needs to avoid dynamic obstacles, a static and dynamic obstacle estimation method is designed, and a charging robot end trajectory optimization method is proposed. This method is executed in the form of a model predictive controller, which can handle the desired operating effect and contact constraints, and integrate the mathematical description of the dynamic obstacles into the trajectory optimization task to ensure the safe execution of the automated charging task. This technology significantly improves the obstacle avoidance effect in both static and dynamic environments. Compared with other trajectory optimization methods, the trajectory optimization method has better real-time performance, strong versatility, and is easy to use.

[0020] Further features of the present invention will become apparent from the following description of exemplary embodiments with reference to the attached drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 is a flow chart showing a path planning method for a charging robot according to an embodiment of the present invention; Figure 2is a flow chart showing the steps of designing trajectory collision avoidance conditions according to an embodiment of the present invention; Figure 3 is a flow chart showing the obstacle status classification steps according to an embodiment of the present invention; Figure 4 is a flowchart showing the steps of constructing a dynamic obstacle avoidance optimization objective function according to an embodiment of the present invention; Figure 5 is a flow chart showing the steps of obtaining an optimal trajectory according to an embodiment of the present invention; Figure 6 is a schematic diagram showing a software configuration of a path planning device for a charging robot according to an embodiment of the present invention. DETAILED DESCRIPTION

[0022] Exemplary embodiments of the present invention are described in detail below with reference to the accompanying drawings. It should be noted that the relative arrangement of components, numerical expressions and numerical values ​​described in these embodiments do not limit the scope of the present invention unless otherwise specifically stated.

[0023] 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 in the automatic charging system. As an alternative, the automatic charging system can also communicate with a server, and the processor in the server executes the computer program stored in the server and feeds back the program execution results to the automatic charging system in real time.

[0024] In the present invention, the term "unit" may refer to a software environment, a hardware environment, or a combination of software and hardware environments. In a software environment, the term "unit" refers to 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, and when the CPU executes the instructions or programs, the CPU performs 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 specific embodiment, the term "unit" may include mechanical, optical or electrical components, or any combination thereof. The term "unit" may include active (such as transistors) or passive (such as capacitors) components. The term "unit" may include a semiconductor device having a substrate and other material layers with various conductive concentrations. It may include a CPU or a programmable processor that can execute a program stored in a memory to perform a specified function. The term "unit" may include a logic element (such as AND, OR) implemented by a transistor circuit or any other switching circuit. In the combination of software and hardware environment, the term "unit" or "circuit" refers to any combination of software and hardware environment as described above. In addition, the term "element", "component", "part" or "device" may also refer to a "circuit" integrated or not integrated with packaging materials.

[0025] The following will refer to Figure 1-Figure 5 A path planning method for a charging robot according to an embodiment of the present invention is described.

[0026] In step S100, a Kalman filter is used to estimate the trajectory 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 collision avoidance conditions of the charging robot trajectory are designed.

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

[0028] In step S110, a Kalman filter list of obstacle point cloud clusters, point cloud cluster sequences and corresponding time points is constructed. The spatial point cloud information formed by the depth camera perception is extracted using the density-based spatial clustering technology with noise (DBSCAN) to obtain Point cloud cluster , further dividing the point cloud clusters into time intervals Constructed into a point cloud cluster sequence ;for Each point cloud cluster at the moment will be further constructed using the Kalman filter constructed by the conservative motion model. List of Kalman filters at time .

[0029] In step S120, point cloud cluster matching and tracking history update are performed. , first propagate forward Then, a K-neighbor search technique is used to associate the cluster centroids in the current frame to the locations where the Kalman filters are propagated forward. If the association is successful, the point cloud cluster will inherit the tracking history of its associated object. Otherwise, assume that the irrelevant point cloud cluster is a newly appeared object and create a new Kalman filter for it.

[0030] In step S130, the obstacle status is classified. The motion state of the point cloud cluster is obtained by taking two frames of point cloud of one second.

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

[0032] First, in S1301, a KD tree of the point cloud is constructed. Specifically, A dense, unfiltered point cloud is extracted from the charging robot coordinate system at the moment and a KD tree is constructed.

[0033] In S1302, the global nearest neighbor distance of each point cloud point is calculated. Specifically, the KD tree is used to calculate the distance between the current frame and The moment belongs to the point cloud cluster Each point The global nearest neighbor distance .

[0034] In step S1301, each point cloud point is evaluated to determine the obstacle status. Calculate the speed of each point and count The number of point cloud points is recorded as the number of dynamic points of the point cloud cluster. , 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 between two frames changes, only the number of dynamic points appearing in the overlapping area is counted.

[0035] In step S1304, the occlusion caused by dynamic obstacles is processed. Associated to its corresponding point cloud cluster And project it onto the image plane. If the 3D space point The corresponding projection point The depth value is less than the three-dimensional space point The corresponding projection point If the depth is greater than , all point cloud points in the current point cloud cluster can be dynamically determined; otherwise, for point cloud points belonging to other point cloud clusters , first project it onto On the image plane at the moment, the projection rule is as follows: , in, express The camera extrinsic matrix at the moment, represents the intrinsic parameter matrix of the camera, Represents a 3D space point in the camera coordinate system. The vertical projection distance of is greater than the three-dimensional space point The corresponding projection point When the depth value is greater than , the point cloud point is considered to be occluded and no dynamic determination is performed.

[0036] After completing step S130, the process proceeds to step S140. In step S140, a dynamic environment model is established. Specifically, for dynamic obstacles, they are described as having speed. A three-dimensional ellipsoid: , in, , represents the position of the ellipsoid center in three-dimensional space, , and Respectively represent the axis lengths of the ellipsoid in the x, y, and z directions, Represents the expansion radius.

[0037] After completing the design of the collision avoidance conditions of the charging robot trajectory, 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 indicators, dynamic obstacle avoidance indicators, charging robot motion smoothness, and the charging robot's own motion constraints are designed to construct the charging robot dynamic obstacle avoidance optimization objective function. B-spline curve is a curve representation method widely used in mathematics and computer graphics. It is generated by a set of control points through a linear combination of 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 the k-order B-spline curve is: P(u)=∑(n,i=0)PiNi,k(u), Among them, Ni,k(u) is the B-spline basis function, and is calculated by the control points through a specific algorithm.

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

[0039] In step S210, a B-spline based motion trajectory of the charging robot terminal is constructed. Specifically, the B-spline curve trajectory of the charging robot terminal motion is By the curve order , the number is The curve control points and uniform node vector Uniquely determined. For each node interval ,exist The B-spline basis function satisfies the following differential equation: , In step S220, an optimization target of a B-spline curve trajectory is constructed. The optimization goal is specifically: , in, , and is the total weight coefficient of different optimization sub-goals. The optimization goals specifically include smoothness evaluation sub-goals, constraint sub-goals, static obstacle avoidance sub-goals, and dynamic obstacle avoidance sub-goals.

[0040] The above sub-goals are described in detail below.

[0041] 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: , 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.

[0042] The constraint subgoal ensures that the generated trajectory meets the desired speed and acceleration limits, which are 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.

[0043] The static obstacle avoidance subgoal 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.

[0044] The dynamic obstacle avoidance subgoal ensures that the generated trajectory can avoid dynamic obstacles, which is expressed as follows: , in, For the The position, velocity and size of the 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: .

[0045] 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 to obtain the optimal trajectory of the charging robot at the current moment, and the first trajectory point is used as the current target of the charging robot; when the state of the charging robot and the state of the environment at the next moment are obtained, step S220 is repeated.

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

[0047] In S310, the optimal B-spline curve trajectory solved by the dynamic obstacle avoidance optimization objective function of the charging robot constructed in step S200 is minimized by the L-BFGS method to calculate the charging robot arm moment control instructions. In S320, the robot arm joint position control instruction calculated in the above step S310 is input into the charging robot arm; At this moment, return to step S220 until the charging robot arm obstacle avoidance task is completed.

[0048] The following will refer to Figure 6 A path planning device for a charging robot according to an embodiment of the present invention is described.

[0049] like Figure 6 As shown, the path planning device 10 according to the 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 acquisition unit 300 and a repeated optimization objective function construction unit 400.

[0050] The trajectory collision avoidance condition design unit 100 estimates the obstacle trajectory by using a Kalman filter, classifies the obstacles into dynamic obstacles and static obstacles, and models the dynamic obstacles as a moving 3D ellipsoid to design the robot trajectory collision avoidance condition. The dynamic obstacle avoidance optimization objective function construction unit 200 constructs the robot dynamic obstacle avoidance optimization objective function by using a B-spline curve 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. The optimal trajectory acquisition unit 300 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. When the repeated optimization objective function construction unit 400 obtains the robot's body state and environmental state at the next moment, it repeats the dynamic obstacle avoidance optimization objective function construction until the robot's obstacle avoidance task is completed.

[0051] By using the path planning method and path planning device according to the above-mentioned embodiment, in the automated charging task that needs to avoid dynamic obstacles, a static and dynamic obstacle estimation method is designed, a charging robot arm end trajectory optimization method is proposed, and the method is executed in the form of a model predictive controller, which can handle the desired operating effect and contact constraints, and integrate the mathematical description of the dynamic obstacles into the trajectory optimization task to ensure the safe execution of the automatic charging task. This technology significantly improves the obstacle avoidance effect 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 ease of use.

[0052] Other embodiments The embodiments of the present invention may also be implemented by reading out and executing computer executable instructions (e.g., one or more programs) recorded on a storage medium (which may also be more completely referred to as a "non-transitory computer-readable storage medium") to perform one or more functions in the above-mentioned embodiments, and / or a computer of a system or device including one or more circuits (e.g., an application-specific integrated circuit (ASIC)) for performing one or more functions in the above-mentioned embodiments, and the embodiments of the present invention may be implemented by, for example, reading out and executing the computer executable instructions from the storage medium by the computer of the system or device to perform one or more functions in the above-mentioned embodiments, and / or controlling the one or more circuits to perform one or more functions in the above-mentioned embodiments. The computer may include one or more processors (e.g., a central processing unit (CPU), a microprocessing unit (MPU)), and may include a network of separate computers or separate processors to read out 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), a memory of a distributed computing system, an optical disk (such as a compact disc (CD), a digital versatile disc (DVD), or a Blu-ray Disc (BD)™), a flash memory device, a memory card, and the like.

[0053] The embodiments of the present invention may also be implemented by providing software (including a computer program product of a computer program / instructions) that performs the functions of the above-described embodiments to a system or device via a network or various storage media, and a computer (central processing unit (CPU), microprocessing unit (MPU)) of the system or device reads and executes the computer program / instructions.

[0054] While the present invention has been described with reference to exemplary embodiments, it is to be understood that the invention is not limited to the disclosed exemplary embodiments.The scope of the following claims is to be accorded the broadest interpretation so as to encompass all such modifications 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 body 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.

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 The 3D ellipsoid: ， in, , in, 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.

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 5, wherein: The dynamic obstacle avoidance sub-goal ensures that the generated trajectory can avoid dynamic obstacles, which is expressed as follows: ， 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: 。 10. 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.

11. 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 The objective function construction unit is optimized repeatedly. When the robot's main body 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.

12. 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 10.

13. 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 10.

Images