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

Abstract

The application provides a motion planning method and device of a mechanical arm, a storage medium and a computer program product. The motion planning method comprises: an information acquisition step of acquiring environment information around the mechanical arm and motion information of the mechanical arm; a positioning step of obtaining current pose information of the mechanical arm according to the acquired environment information and motion information; a local map construction step of constructing a local grid map according to the environment information and the current pose information of the mechanical arm; an action instruction generation step of inputting the current pose information, the local grid map and body perception information of the mechanical arm into a pre-constructed action policy network, and generating an action instruction through the action policy network; and an updating step of generating evaluation information of the action of the mechanical arm through an evaluation network, and updating the action policy network and the evaluation network according to the evaluation information. The application dynamically plans a motion path of a robot according to perception information of a dynamic environment, so that the robot safely and quickly reaches a target pose.

Description

Technical Field

[0001] This invention relates to a method for motion planning and control of robots in complex environments, and more particularly to a motion planning method and apparatus, storage medium and computer program product for a rechargeable robot's robotic arm. Background Technology

[0002] Motion planning for robots is a crucial issue in autonomous unmanned systems, involving how to plan safe and rapid task execution strategies using sensors in unknown environments. For example, in an automated charging scenario, the robotic arm of a charging robot needs to manipulate the charging plug to accurately insert it into the vehicle's charging port, relying solely on vision to reach the designated pose and complete the charging task in a cluttered environment.

[0003] Without accurate environmental perception and sufficient obstacle information, the robotic arm may collide and damage the equipment during the task; similarly, without a reasonable motion strategy for the robotic arm, the charging task cannot be completed safely and quickly.

[0004] Existing motion planning algorithms perform path planning within a known environment, ignoring the uncertainties of real-world scenarios. For example, in dynamic environments, the location of the charging port can change due to different car models, different parking postures or positions within the same parking space, or varying environments within the parking space. Some existing technologies also pre-set fixed trajectories, failing to perform real-time motion planning and potentially leading to cumulative errors. Therefore, the accuracy of existing robotic arm motion planning methods in dynamic environments is still insufficient.

[0005] As mentioned earlier, existing motion planning algorithms all plan a global path in a known environment and perform path tracking control. While this method can effectively avoid obstacles in a known environment, it struggles in complex application environments. For example, different users' parking habits lead to variations in parking posture, and different vehicle models have different charging port locations. In such cases, using a preset fixed path or a path planned based on the known environment can cause the charging robot to fail to accurately insert the charging plug into the vehicle's charging port. Therefore, how to plan paths in real time based on the dynamic environment and accurately execute tasks in uncertain and complex environments is a problem that urgently needs to be solved. Summary of the Invention

[0006] To address the aforementioned problems in the prior art, this invention provides a motion planning method that dynamically plans the motion trajectory of a robotic arm in a dynamic environment based on real-time perceived environmental information and the robotic arm's own motion information.

[0007] To achieve the above objectives, according to a first aspect of the present invention, a motion planning method for a robotic arm is provided, the motion planning method comprising the following steps: The information acquisition step involves acquiring environmental information around the robotic arm and motion information of the robotic arm through one or more sensors installed on the robotic arm. The positioning step involves obtaining the current pose information of the robotic arm based on the acquired environmental information and motion information. The local map construction step involves constructing a local grid map containing obstacle information around the robotic arm and the current pose of the robotic arm based on the environmental information and the current pose of the robotic arm. The motion instruction generation step involves inputting the current pose information, the local grid map, and the body perception information of the robotic arm into a pre-constructed motion strategy network, and generating motion instructions through the motion strategy network. These motion instructions are used to guide the movements of the robotic arm. The update step involves generating evaluation information for the robotic arm's movements using an evaluation network, and then updating the motion strategy network and the evaluation network based on the evaluation information.

[0008] As a preferred embodiment, in the information acquisition step, the one or more sensors include an image sensor and an inertial measurement unit; the information acquisition step includes: The image sensor acquires image information around the robotic arm as the environmental information, and the inertial measurement unit acquires inertial data of the robotic arm as the motion information.

[0009] As a preferred embodiment, the positioning step includes the following: The preprocessing step involves extracting features from the image information to obtain feature point tracking data, and pre-integrating the motion information to obtain the pose, velocity, and rotation angle at the current moment. Simultaneously, the pre-integration increment, pre-integration covariance matrix, and Jacobian matrix between adjacent frames are calculated as pre-integration data. The initialization step involves calculating the relative pose between adjacent frames based on the feature point tracking data, and aligning the relative pose with the pre-integrated data to obtain the initial pose in world coordinates. The local optimization step involves using a sliding window optimization method to locally optimize the initial pose, thereby obtaining a local pose. The loop closure detection step involves matching the current frame with historical keyframes and selecting frames that match the historical keyframes as loop closure constraints based on the matching relationship. The global optimization step involves performing global optimization on the local pose based on the closure constraint to obtain the global pose.

[0010] As a preferred embodiment, the local map construction step includes: The depth image of the current frame acquired by the image acquisition device; Based on the depth information of each two-dimensional pixel in the depth image, the two-dimensional pixels are projected into three-dimensional space to obtain the local raster map; wherein, the projection formula is defined as follows: , , in, Here, u represents the camera intrinsic parameters; v represents the pixel x-coordinate; Z represents the depth of each pixel obtained from the depth image; and X and Y represent the x-coordinate and y-coordinate after projection into 3D space, respectively.

[0011] As a preferred embodiment, in the update step, the evaluation network rewards the robotic arm's actions based on a reward function, which is defined as the evaluation information as follows: , in, This represents a position reward function, used to reward the end effector of the robotic arm for approaching a specified position in Cartesian space; This represents the attitude reward function, which rewards the end effector of the robotic arm for reaching a specified attitude; This represents the collision reward function, used to penalize collisions between the robotic arm and the environment; This is the weighting coefficient for location-based rewards; The weighting coefficient for posture rewards; This is the collision penalty coefficient; The location reward function The posture reward function and the collision reward function The definitions are as follows: , , , in, The target posture is the specified posture that the robotic arm needs to reach. The attitude of the end effector of the robotic arm at time t, i.e., the current attitude information; Represents the L2 norm, It represents the difference between two quaternions.

[0012] As a preferred embodiment, in the action instruction generation step, the action policy network generates the action instruction by calculating a policy function that maximizes the cumulative reward, and the policy function is defined as follows: , in, This is the reward decay factor; The policy function to be optimized; The reward obtained by the action policy network at time t; In strategy The expected value of all possible trajectories.

[0013] As a preferred embodiment, the update step includes an action policy network update step, which includes: The policy gradient calculation step involves using the policy gradient algorithm to calculate the gradient of the optimization index with respect to the network parameters of the action policy network. The policy gradient calculation formula is defined as follows: , in, J represents the state; J represents the optimization index. For gradient operators; This is the reward decay factor; The total number of samples; These are the network parameters of the action policy network; The network parameters of the evaluation network; For policy gradient; This represents a state value estimate; The reward obtained at time t; The action policy network parameter update step involves updating the network parameters of the action policy network using gradient descent. The gradient calculation formula for the network parameters of the action policy network is as follows: , in, These are the network parameters of the action policy network; The network parameters of the action policy network The update step size.

[0014] As a preferred embodiment, the update step further includes an evaluation network update step, which includes: The loss function construction steps are as follows: Based on the temporal difference learning method, the loss function of the evaluation network's value function is defined as follows: , in, State; The loss function; The reward obtained at time t; This is the reward decay factor; This represents a state value estimate; The network parameters of the evaluation network; The value gradient calculation step involves calculating the loss function with respect to the network parameters of the evaluation network. The gradient of value is defined by the following formula: , in, For gradient operators; State; This represents a state value estimate; The reward obtained at time t; The evaluation network parameter update step involves updating the network parameters of the evaluation network based on the gradient. The gradient calculation formula for the network parameters of the evaluation network is as follows: , in, The network parameters of the evaluation network The update step size.

[0015] According to a second aspect of the present invention, a motion planning device for a robotic arm is provided, the motion planning device comprising the following steps: The information acquisition unit is configured to acquire environmental information around the robotic arm and motion information of the robotic arm through one or more sensors mounted on the robotic arm; The positioning unit obtains the current pose information of the robotic arm based on the acquired environmental information and motion information; The local map building unit is configured to build a local grid map containing obstacle information around the robotic arm and the current pose of the robotic arm based on the environmental information and the current pose of the robotic arm. The motion instruction generation unit is configured to input the current pose information, the local grid map, and the body perception information of the robotic arm into a pre-constructed motion strategy network, and generate motion instructions through the motion strategy network, the motion instructions being used to guide the movements of the robotic arm; The update unit is configured to generate evaluation information for the robotic arm's movements through an evaluation network, and update the motion strategy network and the evaluation network based on the evaluation information.

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

[0017] 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 motion planning method for a robotic arm according to a first aspect of the present invention.

[0018] The beneficial effects of this invention are as follows: The motion planning method of this invention allows the robot to interact extensively with its environment in a simulated environment, thereby perceiving the environmental information around the robot and the robot's own motion information in real time in a dynamic environment. Through reinforcement learning using this real-time collected perception information, the motion policy network is trained and updated, and then deployed to run in an automatic charging system. Based on the perception information of the dynamic environment, the robot's motion path is dynamically planned, enabling the robot to reach the target pose safely and quickly, and to complete the charging task more accurately, providing electric vehicle users with a more efficient, convenient, and safe charging service. Attached Figure Description

[0019] Figure 1 A schematic diagram illustrating an exemplary structure of an automatic charging system for implementing a motion planning method for a robotic arm according to the present invention is shown.

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

[0021] Figure 3 A flowchart illustrating the positioning step in the motion planning method for a robotic arm according to the present invention is provided.

[0022] Figure 4 A schematic diagram illustrating an exemplary structure of a motion planning device for a robotic arm according to the present invention is shown. Detailed Implementation

[0023] 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.

[0024] 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.

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

[0026] [Architecture of the Automatic Charging System of the Invention] The automatic charging system of the present invention includes a track network consisting of a main track and branch tracks covering all parking spaces in a parking lot, and intelligent agent components capable of moving on the track network, thereby ensuring that the intelligent agent components can move efficiently and safely to any designated parking space. The main track can be in a closed state to run through the entire parking lot, while the branch tracks extending from the main track can reach each parking space, allowing the intelligent agent components to stop beside the parking space without obstructing the passage of the main track.

[0027] 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.

[0028] 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.

[0029] 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.

[0030] 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.

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

[0032] [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 selects a suitable suspended track charging station and a suspended track robotic arm. Specifically, after receiving a charging request from an electric vehicle, the automatic charging system selects the most suitable suspended track charging station and suspended track robotic arm to undertake the charging task for that parking space. For example, it prioritizes selecting the suspended track charging station and suspended track robotic arm that is closest to the parking space and is currently idle. This invention does not limit the method of selecting the suspended track charging station and suspended track robotic arm.

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

[0034] After both the suspended track-mounted charging station and the suspended track-mounted 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 motion planning method for the robotic arm, as described later. This causes the suspended track-mounted robotic arm to automatically grasp the charging gun on the suspended track-mounted charging station based on the motion commands generated by the motion strategy network, insert it into the electric vehicle, and the suspended track-mounted charging station begins charging the electric vehicle. Next, until the charging station is fully charged, the suspended track-mounted robotic arm moves along the track back to the current parking space, removes the charging gun, and places it back on the charging station. Thus, the charging operation is completed, and the charging station and robotic arm leave the current parking space to perform other charging tasks.

[0035] 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, and 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 motion planning method used for robotic arm control will be described in detail below.

[0036] [Motion Planning Methods for Robotic Arms] [Design of Motion Planning Algorithms] The motion planning method for the robotic arm of the present invention can be applied to an automatic charging system. This is achieved by a processor in the automatic charging system (e.g., a charging station) or the robotic arm executing a computer program stored in the memory of the automatic charging system or the robotic arm. Alternatively, the automatic charging system or the robotic arm can communicate with a server, where a processor in the server executes a computer program stored on the server or in the cloud and feeds back the program execution results to the automatic charging system in real time.

[0037] To meet the needs of a suspended track-mounted robotic arm to adapt to uncertain dynamic environments and complete charging tasks, this invention discloses a robotic arm motion planning method based on real-time sensing information. This method can sense the environmental information around the robotic arm and the robotic arm's own motion information in real time. Based on the sensed environmental and motion information, dynamic action commands are generated to control the suspended track-mounted robotic arm to perform obstacle avoidance actions, thereby achieving safe and rapid arrival at the designated position.

[0038] Reinforcement learning typically refers to an agent learning an optimal policy in a Markov process to maximize cumulative rewards by interacting with the environment, performing actions, and receiving feedback (usually rewards or punishments). The concepts involved include the agent's state, actions, and rewards.

[0039] For ease of understanding, some of the technical terms used in this invention are explained below.

[0040] An intelligent agent is a proxy capable of perceiving its environment and taking actions to achieve specific goals. It can be software, hardware, or a system, possessing autonomy, adaptability, and interactivity. An intelligent agent perceives changes in the environment (e.g., through sensors or data input), makes judgments and decisions based on its learned knowledge and algorithms, and then executes actions to influence the environment or achieve predetermined goals.

[0041] The state of the agent: the state of the robotic arm during movement includes the current position of the end effector. ,attitude Position of the target pose ,attitude The current angles of each joint of the robotic arm Current local raster map The position of the end effector is represented using coordinates in the world coordinate system, and the attitude is represented using quaternions in the world coordinate system. The local grid map is at the current position. The surrounding grid state is represented by a three-dimensional tensor, which decomposes the environment into a series of discrete grids. Each grid has a value, which contains two basic types of information: coordinates and whether it is an obstacle. The environmental information is represented by the probability value of each grid being occupied. The value of each element is one of three types: -1, 0, and 1. -1 represents an unknown state, 0 represents an unoccupied state, and 1 represents an occupied state.

[0042] Actions of an agent: Decisions made by an agent that affect its interaction with the environment, such as the increments of the joints of the robotic arm in this embodiment of the invention. .

[0043] Reward for an agent: The reward that a robotic arm receives by interacting with its environment. The reward reflects the desirability of an action performed in a given state.

[0044] The agent's value function is a function that estimates the expected cumulative reward that the agent can obtain, starting from a given state and following a specific policy.

[0045] The intelligent agent involved in this invention is a suspended track-mounted robotic arm that independently interacts with the environment within an automatically charging system created on it. It utilizes environmental feedback rewards to improve its strategy and achieve higher cumulative rewards. Furthermore, through a pre-set reward function, the agent continuously refines its strategy using environmental feedback rewards.

[0046] In this invention, the motion planning method for the robotic arm mainly includes an information acquisition step, a positioning step, a local map construction step, a motion command generation step, and an update step. See below for reference. Figure 2 The motion planning method for the robotic arm of the present invention will be described.

[0047] like Figure 2 As shown, firstly, in the information acquisition step S100, environmental information around the robotic arm and motion information of the robotic arm are acquired through one or more sensors installed on the robotic arm.

[0048] The sensors in this embodiment of the invention may include vision sensors, such as cameras or other image acquisition devices with image acquisition capabilities, to acquire target images around the robotic arm as environmental information. The target images include information about obstacles in the surrounding environment, the location of the charging gun, and the location of the electric vehicle's charging interface. The sensors may also include inertial measurement units (IMUs) to measure parameters such as the robotic arm's acceleration, angular velocity, and tilt angle as motion information. Besides using vision sensors to perceive environmental information, other sensing devices such as lidar can also be used to acquire environmental information.

[0049] Before implementing the motion planning method for the robotic arm, a simulation environment for the charging robot in a complex environment is first built, creating a physical simulation model of the charging robot (six-DOF robotic arm). A vision sensor for sensing environmental information is installed on the end effector of the robotic arm, and the intrinsic parameters of the camera are set according to the actual equipment. An IMU is installed on the camera, and the extrinsic parameters of the camera and IMU are set according to the actual equipment. In the simulation environment, obstacles of different shapes, such as rectangles and circles, are randomly set to simulate obstacles in a real complex environment. After the simulation environment is built, the information acquisition step S100 is executed to acquire environmental information around the robotic arm and the motion information of the robotic arm.

[0050] Further, the positioning step S200 is executed to obtain the current pose information of the robotic arm based on the acquired environmental information and motion information.

[0051] Step S200 is the visual localization process of the robotic arm's end effector: Based on the image information acquired by the visual sensor and the motion information acquired by the IMU, a visual inertial odometry (VIO) is constructed. The VIOMA is used to estimate the pose of the end effector in the world coordinate system, so as to facilitate the creation of a local map by combining it with depth images later.

[0052] Simultaneous localization and mapping (SLAM) technology is frequently used for autonomous navigation in robots. The localization and mapping data provided by the SLAM module helps the robot obtain its own state estimation and information about obstacles in the environment. Visual-inertial odometry (VIO) is one type of SLAM technology; it estimates the robot's state by fusing visual information with inertial measurement units. Given the camera pose, a local map can be constructed using depth information.

[0053] In the local map construction step S300, a local grid map containing obstacle information around the robotic arm and the current pose of the robotic arm is constructed based on environmental information and the current pose information of the robotic arm.

[0054] Step S300 is used to construct a local grid map. In this embodiment, the visual sensor is a binocular camera. The binocular camera obtains a depth image in the current frame. Based on the depth information on the two-dimensional pixels, it projects the points on a whole light ray into three-dimensional space to update the local grid map. The local grid map can be used for motion trajectory planning in the next step.

[0055] In step S400, the current pose information, local grid map and the robot arm's body perception information are input into a pre-constructed motion strategy network, and motion commands are generated through the motion strategy network to guide the robot arm's movements.

[0056] Step S400 is a motion planning algorithm based on reinforcement learning. Based on the current pose information of the robotic arm obtained in step S200 (i.e., the end effector pose), the local grid map obtained in step S300, and the robotic arm's own proprioceptive information (e.g., the angles of each joint), reinforcement learning outputs the current action policy through a neural network model. In reinforcement learning, the neural network model of this invention adopts an Actor-Critic structure. The Actor is the action policy network, used to receive environmental information and output corresponding action variables, such as the increments of each joint of the robotic arm.

[0057] In update step S500, evaluation information is generated for the robotic arm's movements through the evaluation network, and the motion strategy network and evaluation network are updated based on the evaluation information.

[0058] Specifically, the evaluation network is a Critic network, which is used to output action instructions in step S400 to guide the robotic arm to complete the corresponding actions. After that, the quality of the robotic arm's actions is judged based on the current state of the robotic arm, i.e., evaluation information is generated. The evaluation information guides the Actor-Critic structure to update the network parameters, especially to help the Actor network update its strategy, so that the Actor network can continuously optimize the action strategy.

[0059] In summary, the motion planning method of this invention allows the robot to interact extensively with its environment in a simulated environment, thereby perceiving the environmental information around the robot and the robot's own motion information in real time in a dynamic environment. Through reinforcement learning using this real-time collected perception information, the motion policy network is trained and updated, and then deployed to run in an automatic charging system. Based on the perception information of the dynamic environment, the robot's motion path is dynamically planned, enabling the robot to reach the target pose safely and quickly, and complete the charging task more accurately, providing electric vehicle users with a more efficient, convenient, and safe charging service.

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

[0061] As an optional implementation, in the information acquisition step S100, one or more sensors include an image sensor and an inertial measurement unit; the information acquisition step includes: Image sensors collect image information about the area around the robotic arm as environmental information, and an inertial measurement unit collects inertial data of the robotic arm as motion information.

[0062] As mentioned earlier, this invention can acquire image information through a visual sensor and motion information through an IMU. Compared with traditional positioning methods, visual positioning has the advantages of high accuracy and high stability. Visual positioning can perceive environmental changes in real time and quickly and accurately locate the target. This characteristic enables robots to navigate autonomously and avoid obstacles in complex environments, greatly improving the safety and reliability of operations.

[0063] In this invention, the localization step S200 further includes a preprocessing step, an initialization step, a local optimization step, a loop closure detection step, and a global optimization step. See below for reference. Figure 3 The positioning step S200 of the present invention will be described.

[0064] like Figure 3 As shown, in the preprocessing step S201, feature extraction is performed on the image information to obtain feature point tracking data, and pre-integration is performed on the motion information to obtain the pose, velocity and rotation angle at the current moment. At the same time, the pre-integration increment between adjacent frames, the covariance matrix of the pre-integration and the Jacobian matrix are calculated as pre-integration data.

[0065] Step S201 involves preprocessing the image and IMU data: For the image, feature points are extracted, and optical flow tracking is performed using the KLT pyramid to prepare for solving the camera pose. For the IMU data, the IMU data is pre-integrated to obtain the pose, velocity, and rotation angle at the current time. At the same time, the pre-integration increment between adjacent frames, as well as the pre-integration covariance matrix and Jacobian matrix, are calculated.

[0066] In initialization step S202, the relative pose between adjacent frames is calculated based on the feature point tracking data, and the relative pose is aligned with the pre-integrated data to obtain the initial pose in world coordinates.

[0067] Step S202 first performs vision-only initialization, calculates the relative pose of the camera, and then aligns with IMU pre-integration to solve for the initialization parameters.

[0068] In the local optimization step S203, the initial pose is locally optimized using the sliding window optimization method to obtain the local pose.

[0069] Specifically, local nonlinear optimization involves optimizing visual constraints and IMU constraints within a common objective function. This local optimization means optimizing only the variables within the current frame and the previous n frames. Local nonlinear optimization outputs a more accurate local pose.

[0070] In the loop closure detection step S204, the current frame is matched with historical keyframes, and the frames that match the historical keyframes are selected as loop closure constraints based on the matching relationship.

[0071] Loop closure detection saves previously detected keyframes from the image. When returning to the same location, it uses feature point matching to determine if the frame has already been visited. The keyframes mentioned earlier are selected camera frames that can be recorded but avoid redundancy. The selection criteria for keyframes are either a displacement between the current frame and the previous frame exceeding a certain threshold or a number of matched feature points being less than a certain threshold.

[0072] In the global optimization step S205, the local pose is optimized globally based on the closure constraint to obtain the global pose.

[0073] Global optimization involves using visual constraints, IMU constraints, and loop closure constraints obtained from loop closure detection to perform nonlinear optimization when loop closures occur, outputting a more accurate global pose based on local optimization.

[0074] Furthermore, the local map construction step S300 includes: The depth image of the current frame is acquired through an image acquisition device. The image sensor can be a stereo camera, similar to human eyes, which calculates depth using two captured images (color RGB or grayscale). Specifically, the stereo camera is first calibrated to obtain its intrinsic and extrinsic parameters and homography matrix. Then, the original images are corrected based on the calibration results, ensuring the two corrected images are on the same plane and parallel to each other. Pixel matching is performed on the two corrected images. The depth of each pixel is calculated based on the matching results, thus obtaining the depth image.

[0075] Based on the depth information of each two-dimensional pixel in the depth image, the two-dimensional pixels are projected into three-dimensional space to obtain a local raster map; the projection formula is defined as follows: (1) (2) in, is the camera intrinsic parameter of the binocular camera; u is the pixel x-coordinate; v is the pixel y-coordinate; Z is the depth of each pixel obtained from the depth image; X and Y represent the x-coordinate and y-coordinate after projection into 3D space, respectively.

[0076] When a robot enters a new environment, it is unaware of indoor obstacle information. Therefore, the robot needs to traverse the entire environment, detect the location of obstacles, find their corresponding index values ​​in the grid map, and modify the corresponding grid values. Free grids are assigned a value of 0 for grids that do not contain obstacles, while obstacle grids are assigned a value of 1 for grids that contain obstacles. This updates the obstacle information in the local grid map.

[0077] The embodiments of the present invention utilize the visual information perceived in real time by the image acquisition device to fully extract effective obstacle information. Even in unknown environments, autonomous obstacle avoidance planning can be achieved by constructing a map. By randomly initializing the environment in the simulation, the policy network can be generalized in real-world scenarios.

[0078] [Reward Function Construction] To enable the robotic arm to quickly, flexibly, and safely reach the designated pose to complete the charging task, a reasonable reward function needs to be set for the reinforcement learning. In update step S500, the evaluation network rewards the robotic arm's actions based on the reward function, which serves as evaluation information. The reward function is defined as follows: (3) in, This represents the position reward function, used to reward the end effector of the robotic arm for approaching a specified position in Cartesian space; This represents the attitude reward function, used to reward the end effector of the robotic arm for reaching a specified attitude; This represents the collision reward function, used to penalize collisions between the robotic arm and the environment; This is the weighting coefficient for location-based rewards; The weighting coefficient for posture rewards; This is the collision penalty coefficient; Location reward function Posture reward function and collision reward function The definitions are as follows: (4) (5) (6) in, The target posture is the specified posture that the robotic arm needs to reach. This represents the attitude of the end effector of the robotic arm at time t, i.e., the current attitude information. Represents the L2 norm, It represents the difference between two quaternions.

[0079] This invention constructs a reward function based on multiple dimensions, including position, posture, and collision. It evaluates the robotic arm's actions in the current environment from different perspectives, comprehensively assesses the current action, and provides corresponding rewards, thereby continuously optimizing the action strategy of the policy network. The reward function is an immediate feedback signal from the environment to the agent after performing an action in the current state. It is a scalar value that directly quantifies the "goodness" or "badness" of the current action, guiding the agent to learn and make decisions towards the desired goal. By accumulating these immediate rewards, the agent can optimize its strategy.

[0080] [Strategy Function Construction] In the action instruction generation step S400, the action policy network generates action instructions by calculating a policy function that maximizes the cumulative reward. The policy function is defined as follows: (7) in, This is the reward decay factor; The policy function to be optimized; The reward obtained by the action policy network at time t; In strategy The expected value of all possible trajectories.

[0081] Specifically, the policy function that maximizes cumulative reward can be viewed as maximizing the expected value of the value function. The goal is to find the optimal strategy by adjusting the strategy parameters to maximize the cumulative reward, thereby maximizing the expected value. This makes the agent more inclined to choose actions that bring higher cumulative rewards.

[0082] In this embodiment, the reinforcement learning neural network model includes two parts: an Actor network and a Critic network. In reinforcement learning, the Actor network is... This indicates that action a is output in state s. for Given the network parameters, the Actor network needs to interact with the environment and learn a better policy using policy gradients under the guidance of the Critic network's value function. The Critic network is represented as... Let represent the value function in state s, and let represent the expected cumulative reward following policy π starting from state s. The Critic network learns this value function from data collected through interactions with the environment via the Actor network. This value function is used to determine which actions are good and which are not in the current state, thus helping the Actor network update its policy. The Critic network's value function represents the agent's expectation of long-term cumulative reward, helping the agent plan for the long term and weigh immediate rewards against future gains.

[0083] As mentioned earlier, the reinforcement learning method of this invention introduces a reward function, which is an immediate feedback from the environment to the quality of an action. The policy function in the Actor network determines the action policy based on the reward calculated by the reward function, thereby outputting the optimal action policy. The reward function is the driving force for policy optimization.

[0084] [Action Strategy Network Update] Update step S500 includes an action policy network update step, which involves updating the network parameters of the Actor network. To perform an update, the action policy network update steps include: The policy gradient calculation steps involve using the policy gradient algorithm to calculate the gradient of the optimization index with respect to the network parameters of the action-policy network. The formula for calculating the policy gradient is defined as follows: (8) in, J represents the state; J represents the optimization index. For gradient operators; This is the reward decay factor; The total number of samples; These are the network parameters of the action policy network; To evaluate the network parameters; For policy gradient; This represents a state value estimate; Let t be the reward obtained at time t.

[0085] The action policy network parameter update steps involve using gradient descent to update the network parameters. The gradient calculation formula for the action policy network parameters is as follows: (9) in, These are the network parameters of the action policy network; Network parameters for the action policy network The update step size.

[0086] Calculated using the policy gradient formula Afterwards, according to Update the network parameters of the action policy network In this invention, the Actor network is updated using the policy gradient principle. The policy gradient algorithm introduces temporal difference error to guide the Actor network in updating its gradient, which can reduce variance and improve robustness.

[0087] [Evaluation of network update] Update step S500 also includes an evaluation network update step, namely, evaluating the network parameters of the Critic network. To perform an update, the evaluation steps for network updates include: The steps for constructing the loss function are as follows: Based on the temporal difference learning method, a loss function is defined to evaluate the value function of the network. (10) Where s represents the state; The loss function; The reward obtained at time t; This is the reward decay factor; This represents a state value estimate; To evaluate the network parameters; The value gradient calculation step involves calculating the loss function with respect to the network parameters of the evaluation network. The gradient of value is defined by the following formula: (11) in, For gradient operators; s State; This represents a state value estimate; The reward obtained at time t; The evaluation network parameter update steps involve updating the network parameters based on the gradient. The gradient calculation formula for the network parameters is as follows: (12) in, To evaluate the network parameters The update step size.

[0088] In terms of neural network model structure, this invention consists of three types: a data preprocessing network, an evaluation network, and a policy network. The evaluation network is part of the Critic network, the policy network is part of the Actor network, and the data preprocessing network is shared by both the Actor and Critic networks. The data preprocessing network comprises a Convolutional Neural Network (CNN) and a Multilayer Perceptron (MLP). The CNN processes the local grid map, extracting features from the downsampled grid map as a 3D tensor using the CNN. The MLP processes other states. The outputs of both are fused into a single vector, which is then input to the Critic and Actor networks respectively. The Critic network outputs a value function, and the Actor network outputs an action policy. During training, to prevent backpropagation between the two parts from causing unstable parameter updates, the parameters of the preprocessing network are updated only using the gradients of the Critic network, and the gradients of the Actor network are used only to update the policy network.

[0089] When collecting data, reinforcement learning uses the current Actor network to interact with the environment, obtaining N trajectories for the agent before training. This invention uses a round-based system to collect motion trajectories, with each round ending under three conditions: the agent's interaction time reaches its maximum value; the agent collides with the environment; or the agent reaches a designated pose.

[0090] The motion planning method of this invention acquires real-time perceived environmental and motion information, simulates extensive interactions between the robot and the environment, and uses reinforcement learning to train a more effective policy network. The policy network is then deployed in a real system, and a specified pose is set, allowing the robot to safely and quickly reach the charging point and complete the charging task.

[0091] The robotic arm of this invention uses a neural network model for real-time optimized control, changing its control strategy in real time based on changes in visual information. This allows for adaptive adjustment to the environment; even if the environment changes, the robotic arm can dynamically adjust its motion strategy, offering greater practicality and flexibility compared to existing static path planning. Furthermore, the neural network model only requires forward computation, resulting in better real-time performance.

[0092] It should be noted that applying this motion planning method to the motion planning of the robotic arm of a charging robot is only one implementation method. It can also be applied to other types of robots, such as cleaning robots, restaurant service robots, hotel service robots, medical robots, etc. The motion planning method of this invention does not limit the application field of robots.

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

[0094] [The motion planning device for the robotic arm of the present invention] like Figure 4 As shown, the motion planning device 400 of the present invention includes an information acquisition unit 401, a positioning unit 402, a local map construction unit 403, a motion command generation unit 404, and an update unit 405. Each unit will be described in detail below.

[0095] The information acquisition unit 401 is configured to acquire environmental information around the robotic arm and motion information of the robotic arm through one or more sensors mounted on the robotic arm.

[0096] The sensors in this embodiment of the invention may include vision sensors, such as cameras or other image acquisition devices with image acquisition capabilities, to acquire target images around the robotic arm as environmental information. The target images include information about obstacles in the surrounding environment, the location of the charging gun, and the location of the electric vehicle's charging interface. The sensors may also include an IMU (Induction Unit) to measure parameters such as the robotic arm's acceleration, angular velocity, and tilt angle as motion information. Besides using vision sensors to perceive environmental information, other sensing devices such as lidar can also be used to acquire environmental information.

[0097] Before implementing the motion planning method for the robotic arm, a simulation environment for the charging robot in a complex environment is first built, creating a physical simulation model of the charging robot (six-DOF robotic arm). A vision sensor for sensing environmental information is installed on the end effector of the robotic arm, and the intrinsic parameters of the camera are set according to the actual equipment. An IMU is installed on the camera, and the extrinsic parameters of the camera and IMU are set according to the actual equipment. In the simulation environment, obstacles of different shapes, such as rectangles and circles, are randomly set to simulate obstacles in a real complex environment. After the simulation environment is built, the information acquisition unit 401 acquires the environmental information around the robotic arm and the motion information of the robotic arm.

[0098] The positioning unit 402 obtains the current pose information of the robotic arm based on the acquired environmental and motion information.

[0099] In this embodiment, the positioning unit 402 uses a visual positioning method to locate the end effector of the robotic arm: a visual inertial odometry (VIO) is constructed based on image information acquired by the visual sensor and motion information acquired by the IMU. The VIO is used to estimate the pose of the end effector in the world coordinate system, so that a local map can be created later by combining it with depth images. Visual inertial odometry (VIO) is a type of SLAM technology that estimates the robot's state by fusing visual information with an inertial measurement unit. Once the camera pose is known, a local map can be constructed using depth information.

[0100] The local map building unit 403 is configured to build a local grid map containing obstacle information around the robotic arm and the current pose of the robotic arm based on environmental information and the current pose information of the robotic arm.

[0101] Specifically, the local map construction unit 403 is used to construct a local grid map. In this embodiment, the visual sensor is a binocular camera. The binocular camera obtains a depth image in the current frame and projects the points on a whole line of light onto the three-dimensional space based on the depth information on the two-dimensional pixels, thereby updating the local grid map. The local grid map can be used for motion trajectory planning in the next step.

[0102] The motion instruction generation unit 404 is configured to input the current pose information, local grid map and the robot arm's body perception information into a pre-constructed motion policy network, and generate motion instructions through the motion policy network. The motion instructions are used to guide the robot arm's movements.

[0103] The motion command generation unit 404 uses a motion planning algorithm based on reinforcement learning. It calculates the current pose information of the robotic arm (i.e., the end effector pose) obtained by the positioning unit 402, the local grid map constructed by the local map construction unit 403, and the robotic arm's own proprioceptive information (e.g., the angles of each joint). Reinforcement learning outputs the current motion strategy through a neural network model. In reinforcement learning, the neural network model of this invention adopts an Actor-Critic structure. The Actor is the motion policy network, used to receive environmental information and output corresponding motion variables, such as the increments of each joint of the robotic arm.

[0104] The update unit 405 is configured to generate evaluation information for the robotic arm's movements through the evaluation network, and update the motion strategy network and the evaluation network based on the evaluation information.

[0105] Specifically, the evaluation network is a Critic network, which is used to judge the quality of the robotic arm's execution of the action based on the current state of the robotic arm after the action instruction generation unit 404 outputs the action instruction and guides the robotic arm to complete the corresponding action. That is, it generates evaluation information and guides the Actor-Critic structure to update the network parameters based on the evaluation information. In particular, it helps the Actor network to update the strategy, so that the Actor network can continuously optimize the action strategy.

[0106] In summary, the motion planning device of this invention allows the robot to interact extensively with its environment in a simulated environment, thereby perceiving the environmental information around the robot and the robot's own motion information in real time in a dynamic environment. Through reinforcement learning using this real-time collected perception information, the motion strategy network is trained and updated, and then deployed to run in an automatic charging system. Based on the perception information of the dynamic environment, the robot's motion path is dynamically planned, enabling the robot to reach the target pose safely and quickly, and complete the charging task more accurately, providing electric vehicle users with a more efficient, convenient, and safe charging service.

[0107] [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.

[0108] 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 motion planning method for a robotic arm, characterized in that, The motion planning method includes the following steps: The information acquisition step involves acquiring environmental information around the robotic arm and motion information of the robotic arm through one or more sensors installed on the robotic arm. The positioning step involves obtaining the current pose information of the robotic arm based on the acquired environmental information and motion information. The local map construction step involves constructing a local grid map containing obstacle information around the robotic arm and the current pose of the robotic arm based on the environmental information and the current pose of the robotic arm. The motion instruction generation step involves inputting the current pose information, the local grid map, and the body perception information of the robotic arm into a pre-constructed motion strategy network, and generating motion instructions through the motion strategy network. These motion instructions are used to guide the movements of the robotic arm. The update step involves generating evaluation information for the robotic arm's movements using an evaluation network, and then updating the motion strategy network and the evaluation network based on this evaluation information. In the update step, the evaluation network rewards the robotic arm's actions based on a reward function, which serves as the evaluation information. The reward function is defined as follows: ， in, This represents a position reward function, used to reward the end effector of the robotic arm for approaching a specified position in Cartesian space; This represents the attitude reward function, which rewards the end effector of the robotic arm for reaching a specified attitude; This represents the collision reward function, used to penalize collisions between the robotic arm and the environment; This is the weighting coefficient for location-based rewards; The weighting coefficient for posture rewards; This is the collision penalty coefficient; The location reward function The posture reward function and the collision reward function The definitions are as follows: ， ， ， in, The target posture is the specified posture that the robotic arm needs to reach. The attitude of the end effector of the robotic arm at time t, i.e., the current attitude information; Represents the L2 norm, It represents the difference between two quaternions.

2. The motion planning method according to claim 1, characterized in that, In the information acquisition step, the one or more sensors include an image sensor and an inertial measurement unit; the information acquisition step includes: The image sensor acquires image information around the robotic arm as the environmental information, and the inertial measurement unit acquires inertial data of the robotic arm as the motion information.

3. The motion planning method according to claim 2, characterized in that, The positioning steps include the following: The preprocessing step involves extracting features from the image information to obtain feature point tracking data, and pre-integrating the motion information to obtain the pose, velocity, and rotation angle at the current moment. Simultaneously, the pre-integration increment, pre-integration covariance matrix, and Jacobian matrix between adjacent frames are calculated as pre-integration data. The initialization step involves calculating the relative pose between adjacent frames based on the feature point tracking data, and aligning the relative pose with the pre-integrated data to obtain the initial pose in world coordinates. The local optimization step involves using a sliding window optimization method to locally optimize the initial pose, thereby obtaining a local pose. The loop closure detection step involves matching the current frame with historical keyframes and selecting frames that match the historical keyframes as loop closure constraints based on the matching relationship. The global optimization step involves performing global optimization on the local pose based on the closure constraint to obtain the global pose.

4. The motion planning method according to claim 2, characterized in that, The local map construction steps include: The depth image of the current frame acquired by the image acquisition device; Based on the depth information of each two-dimensional pixel in the depth image, the two-dimensional pixels are projected into three-dimensional space to obtain a local raster map; the projection formula is defined as follows: ， ， in, Here, u represents the camera intrinsic parameters; v represents the pixel x-coordinate; Z represents the depth of each pixel obtained from the depth image; and X and Y represent the x-coordinate and y-coordinate after projection into 3D space, respectively.

5. The motion planning method according to claim 1, characterized in that, In the action instruction generation step, the action policy network generates the action instruction by calculating a policy function that maximizes the cumulative reward. The policy function is defined as follows: ， in, This is the reward decay factor; The policy function to be optimized; The reward obtained by the action policy network at time t; In strategy The expected value of all possible trajectories.

6. The motion planning method according to claim 1, characterized in that, The update step includes an action policy network update step, which includes: The policy gradient calculation step involves using the policy gradient algorithm to calculate the gradient of the optimization index with respect to the network parameters of the action policy network. The policy gradient calculation formula is defined as follows: ， in, s J represents the state; J represents the optimization index. For gradient operators; This is the reward decay factor; The total number of samples; These are the network parameters of the action policy network; The network parameters of the evaluation network; For policy gradient; This represents a state value estimate; The reward obtained at time t; The action policy network parameter update step involves updating the network parameters of the action policy network using gradient descent. The gradient calculation formula for the network parameters of the action policy network is as follows: ， in, These are the network parameters of the action policy network; The network parameters of the action policy network The update step size.

7. The motion planning method according to claim 1, characterized in that, The update step further includes an evaluation of the network update step, which includes: The loss function construction steps are as follows: Based on the temporal difference learning method, the loss function of the evaluation network's value function is defined as follows: ， in, s State; The loss function; The reward obtained at time t; This is the reward decay factor; This represents a state value estimate; The network parameters of the evaluation network; The value gradient calculation step involves calculating the loss function with respect to the network parameters of the evaluation network. The gradient of value is defined by the following formula: ， in, For gradient operators; s State; This represents a state value estimate; The reward obtained at time t; The evaluation network parameter update step involves updating the network parameters of the evaluation network based on the gradient. The gradient calculation formula for the network parameters of the evaluation network is as follows: ， in, The network parameters of the evaluation network The update step size.

8. A motion planning device for a robotic arm, characterized in that, The motion planning device is used to execute the motion planning method according to any one of claims 1-7, which includes the following steps: The information acquisition unit is configured to acquire environmental information around the robotic arm and motion information of the robotic arm through one or more sensors mounted on the robotic arm; The positioning unit obtains the current pose information of the robotic arm based on the acquired environmental information and motion information; The local map building unit is configured to build a local grid map containing obstacle information around the robotic arm and the current pose of the robotic arm based on the environmental information and the current pose of the robotic arm. The motion instruction generation unit is configured to input the current pose information, the local grid map, and the body perception information of the robotic arm into a pre-constructed motion strategy network, and generate motion instructions through the motion strategy network, the motion instructions being used to guide the movements of the robotic arm; The update unit is configured to generate evaluation information for the robotic arm's movements through an evaluation network, and update the motion strategy network and the evaluation network based on the evaluation information.

9. A non-transitory storage medium storing a computer program that, when executed by a processor, enables the implementation of the motion planning method for a robotic arm according to any one of claims 1-7.

10. A computer program product comprising computer instructions that, when executed by a processor, enable the implementation of the motion planning method for a robotic arm according to any one of claims 1-7.

Images