Optimization Method for Whole-Body Motion Planning of Mobile Manipulators

Abstract

The present invention provides an optimization method for the whole-body motion planning of a mobile manipulator, which relates to the technical field of manipulator motion planning. The method includes: determining environmental features and position gradients when the joints of the mobile manipulator change based on the depth images captured by the mobile manipulator; performing policy planning on the motion states and motion actions of the mobile manipulator through an actor-critic network trained by reinforcement learning to obtain the expected velocity of the end effector in the mobile manipulator; constructing position constraint conditions for obstacle avoidance of the mobile manipulator based on the position gradients, and constructing joint constraint parameters for constraining decision variables; optimizing the decision variables through a quadratic programming solver determined based on the position constraint conditions and joint constraint parameters to obtain the planning result of joint velocities. Through the present application, the problems of limitations in planning tasks, environmental perception, constraint expression, obstacle avoidance, and motion coordination during the motion planning of the mobile manipulator are overcome.

Description

Technical Field

[0001] The present invention relates to the technical field of robotic arm motion planning, and particularly to an optimization method for full-body motion planning of a mobile robotic arm. Background Art

[0002] In the field of mobile robotic arm motion planning, there are many deficiencies in the existing technologies for solving the problem of mobile robotic arm motion planning. The traditional phased control method plans the robotic arm and the chassis separately. Although it is simple and easy to use, it limits the solution space and seriously reduces the system efficiency. Especially in complex environments, it requires multiple trials and errors and is difficult to complete tasks efficiently.

[0003] In the prior art, the optimization-based motion planning method is one of the current research hotspots. This type of method coordinates the control of a multi-robotic-arm degree-of-freedom system by introducing a mathematical optimization model. Although it performs well in terms of accuracy and utilization of the model, it has limitations in environmental perception ability and constraint expression ability, poor real-time performance, and it is difficult to achieve smooth and coordinated obstacle avoidance planning in a dynamic obstacle environment. Motion planning by means of reinforcement learning depends on environmental feature extraction and can achieve online learning. In complex scenarios with multiple redundant degrees of freedom, it is difficult to generate smooth, continuous, and coordinated motion trajectories and is often limited by the discrete action space and algorithm generalization performance, resulting in unsatisfactory motion planning effects. The hybrid strategy integrating reinforcement learning and optimization methods has potential, but there are still limitations in obstacle avoidance and improving motion coordination. Summary of the Invention

[0004] The present invention provides an optimization method for full-body motion planning of a mobile robotic arm to overcome the limitations in aspects such as planning task completion, environmental perception, constraint expression, obstacle avoidance, and motion coordination during the motion planning of the mobile robotic arm.

[0005] The present invention provides an optimization method for full-body motion planning of a mobile robotic arm, including:

[0006] Determining environmental features and position gradients when the joints of the mobile robotic arm change based on the depth image captured by the mobile robotic arm;

[0007] Performing policy planning on the motion state and motion actions of the mobile robotic arm through an actor-critic network trained by reinforcement learning to obtain the expected velocity of the end effector in the mobile robotic arm. The motion state includes joint angles, the environmental features, and the position deviation between the end effector and the target position. The motion actions include the motion velocities and rotational velocity of the end effector in three directions in a three-dimensional Cartesian coordinate system;

[0008] Construct position constraint conditions for obstacle avoidance of a mobile manipulator based on the position gradient, and construct joint constraint parameters for constraining decision variables, where the decision variables include the joint velocity and relaxation norm of the mobile manipulator, and the decision variables are obtained by weighting the desired velocity through the manipulator Jacobian matrix;

[0009] Optimize the decision variables using a quadratic programming solver determined based on the position constraint conditions and the joint constraint parameters to obtain the planning result of the joint velocity.

[0010] In some embodiments, determining the environmental features and the position gradient when the joints of the mobile manipulator change based on the depth image captured by the mobile manipulator includes:

[0011] Invoke a convolutional neural network to perform image encoding on the depth image to obtain environmental features;

[0012] Perform conversion processing on the depth image to obtain a local environmental point cloud map centered on the mobile manipulator;

[0013] Convert the coordinates of the environmental point cloud in the local environmental point cloud map to position coordinates centered on the mobile manipulator through a signed distance field, and query the distance between the link and the obstacle under the current joint configuration of the mobile manipulator from the position coordinates;

[0014] Determine the position gradient when the joints of the mobile manipulator change based on the separation distance.

[0015] In some embodiments, using the actor-critic network trained by reinforcement learning to perform policy planning on the motion state and motion actions of the mobile manipulator to obtain the desired velocity of the end effector in the mobile manipulator includes:

[0016] Construct the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning as the soft state-action return;

[0017] Take the expected value of the soft state-action return as the soft Q-value of the motion policy, and learn the soft Q-value through the soft Bellman operator in the distributed soft policy iteration framework to perform policy planning training on the actor-critic network;

[0018] Input the initial motion state of the mobile manipulator into the trained actor-critic network for motion policy planning to obtain the optimal motion policy of the mobile manipulator;

[0019] Determine the desired velocity of the end effector in the mobile manipulator based on the optimal motion policy.

[0020] In some embodiments, constructing the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning includes:

[0021] Constructing a position reward value according to the second-order norm of the position deviation between the end effector of the mobile manipulator and the target position, and obtaining a moving target reward value when the second-order norm of the position deviation is less than a preset position deviation threshold;

[0022] Constructing a speed reward value based on the difference between the moving speed when the position deviation is generated during the movement of the mobile manipulator and the action speed of the end effector in the mobile manipulator;

[0023] Weighting the position reward value and the speed reward value through preset position reward weights and speed reward weights to obtain a weighted position reward value and a weighted speed reward value;

[0024] Constructing an obstacle avoidance reward value according to the closest distance when the end effector of the mobile manipulator avoids obstacles and a preset obstacle avoidance distance threshold;

[0025] Constructing a time reward value based on the time spent during the movement of the mobile manipulator;

[0026] Summing up the weighted position reward value, the weighted speed reward value, the obstacle avoidance reward value and the time reward value to obtain a total reward value;

[0027] Calculating the policy entropy based on the total reward value as the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning.

[0028] In some embodiments, the policy planning training process of the actor-critic network includes:

[0029] During each iterative time step of learning the soft Q value, perform the following operations:

[0030] When performing policy iteration using the actor network in the actor-critic network, update the second distribution by minimizing the KL divergence between the first distribution and the second distribution, where the first distribution is the distribution obtained by calculating the second distribution using the soft Bellman operator, and the second distribution is the distribution of the current soft state-action return output by the critic network in the actor-critic network;

[0031] When performing policy evaluation using the critic network, update the parameters of the actor network and the parameters of the critic network by minimizing the KL divergence.

[0032] In some embodiments, updating the parameters of the actor network and the parameters of the critic network by minimizing the KL divergence is achieved by calculating the update gradient of the KL divergence and setting the update target of the update gradient to the temporal difference error of the current soft state action return. The calculation process of the update gradient includes:

[0033] Model the second distribution as a Gaussian distribution, which consists of the mean of the current soft state action return and the variance of the current soft state action return;

[0034] Calculate the update gradient based on the first boundary-limited distribution, the mean, and the variance;

[0035] Among them, the first boundary-limited distribution is obtained by processing the first distribution through a boundary value limiting function, and the boundary of the boundary value limiting function is determined according to the mean and a preset constraint boundary.

[0036] In some embodiments, there are two critic networks in the actor-critic network. The distribution of the current soft state action return is obtained by calling a Bayesian control function to perform a mixed estimation on the distributions of the soft state action returns output by the two critic networks during policy evaluation. The process of the Bayesian control function performing the mixed estimation includes:

[0037] Call the Bayesian control function to perform likelihood estimation on the mean and variance in the distribution of the soft state action return respectively to obtain a mixed mean and a mixed variance;

[0038] Fuse the mixed means and mixed variances of the two critic networks to obtain a mixed soft state action return distribution as the distribution of the current soft state action return.

[0039] In some embodiments, constructing the position constraint condition for the mobile manipulator to avoid obstacles based on the position gradient includes:

[0040] Construct the first obstacle avoidance shock absorber parameter according to the position gradient of each link when the joints of the mobile manipulator change;

[0041] Construct the second obstacle avoidance shock absorber parameter according to the distance between each link and the obstacle when the joints of the mobile manipulator change and the difference between the distance of the link and the minimum distance to the obstacle;

[0042] Determine the first speed shock absorber parameter and the second speed shock absorber parameter for restricting the joint positions of the mobile manipulator. The first speed shock absorber parameter is a preset fixed parameter, and the second speed shock absorber parameter is calculated according to the influence distance and the minimum distance between the joints of the mobile manipulator and the singular position;

[0043] Fuse the first speed shock absorber parameters and the first obstacle avoidance shock absorber parameters to obtain a first constraint parameter, and fuse the second speed shock absorber parameters and the second obstacle avoidance shock absorber parameters to obtain a second constraint parameter;

[0044] Use the ratio of the second constraint parameter to the first constraint parameter as the position constraint condition.

[0045] In some embodiments, the joint constraint parameters include a constraint weight and an optimization bias. The construction of joint constraint parameters for constraining decision variables includes:

[0046] Use the value of the position deviation between the end effector of the mobile manipulator and the target position as a third constraint parameter for constraining the joint velocity, and use the reciprocal of the value of the position deviation as a fourth constraint parameter for constraining the joint velocity;

[0047] Combine the third constraint parameter and the fourth constraint parameter into a target weight for constraining the joint velocity;

[0048] Obtain a relaxation weight for adjusting the relaxation norm, and construct a constraint weight based on the relaxation weight and the target weight;

[0049] Determine the angle between the mobile manipulator and the mobile base, and determine the adjustment gain value of the angle;

[0050] Construct an optimization bias based on the adjustment gain value and the manipulative Jacobian matrix of the mobile manipulator. The optimization bias is used to minimize the angle when the mobile manipulator moves.

[0051] The present invention also provides an optimization device for the full-body motion planning of a mobile manipulator, including:

[0052] An environment perception module, configured to determine environmental features and position gradients when the joints of the mobile manipulator change based on the depth image captured by the mobile manipulator;

[0053] A speed planning module, configured to perform policy planning on the motion state and motion actions of the mobile manipulator through an actor-critic network trained by reinforcement learning to obtain the expected speed of the end effector in the mobile manipulator. The motion state includes joint angles, the environmental features, and the position deviation between the end effector and the target position, and the motion actions include the motion speeds and rotational speeds of the end effector in three directions in a three-dimensional rectangular coordinate system;

[0054] A motion planning module, configured to construct position constraint conditions for obstacle avoidance of a mobile manipulator based on the position gradient, and construct joint constraint parameters for constraining decision variables, where the decision variables include the joint velocity and the relaxation norm of the mobile manipulator, and the decision variables are obtained by weighting the desired velocity through a manipulator Jacobian matrix;

[0055] The motion planning module is further configured to optimize the decision variables by a quadratic programming solver determined based on the position constraint conditions and the joint constraint parameters to obtain a planning result of the joint velocity.

[0056] The present invention further provides an electronic device, including a memory, a processor, and a computer program stored on the memory and executable on the processor, where when the processor executes the computer program, the optimization method for full-body motion planning of a mobile manipulator as described in any one of the above is implemented.

[0057] The present invention further provides a non-transitory computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the optimization method for full-body motion planning of a mobile manipulator as described in any one of the above is implemented.

[0058] The present invention further provides a computer program product, including a computer program, and when the computer program is executed by a processor, the optimization method for full-body motion planning of a mobile manipulator as described in any one of the above is implemented.

[0059] The optimization method for full-body motion planning of a mobile manipulator provided by the present invention first performs environmental perception, and uses a depth image to determine environmental features and the position change gradient of the mobile manipulator. On this basis, a policy for the desired velocity of the end effector is planned through an actor-critic network trained by reinforcement learning, improving the motion planning effect and having real-time performance. Further, for position constraints, joint velocity constraints, and relaxation norm constraints in obstacle avoidance, the desired velocity is planned through a quadratic programming solver to obtain a planning result of the joint velocity, ensuring the reliability, rapidity, and safety of the control of the mobile manipulator under complex motion trajectories, and solving the problems in the prior art motion planning methods that the environmental perception ability and constraint expression ability have limitations, and it is difficult to achieve smooth and coordinated obstacle avoidance planning through single reinforcement learning. Description of the Drawings

[0060] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the accompanying drawings required for use in the description of the embodiments or the prior art one by one. Obviously, the accompanying drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0061] Figure 1 It is a schematic flow chart of the optimization method for full-body motion planning of a mobile manipulator provided by the present invention.

[0062] Figure 2 It is a schematic diagram of the principle of the optimization method for full-body motion planning of a mobile manipulator provided by the present invention.

[0063] Figure 3 It is a schematic structural diagram of the mobile manipulator provided by the present invention.

[0064] Figure 4 It is a schematic diagram of the principle of reinforcement learning training of the actor-critic network provided by the present invention.

[0065] Figure 5 It is a comparison schematic diagram of the mixture of value distributions provided by the present invention.

[0066] Figure 6 It is a schematic structural diagram of the optimization device for full-body motion planning of a mobile manipulator provided by the present invention.

[0067] Figure 7 It is a schematic structural diagram of the electronic device provided by the present invention.

[0068] Reference numerals:

[0069] 1: Manipulator; 2: Joint; 3: Link; 4: Mobile base; 5: End effector. Detailed implementation manners

[0070] To make the objectives, technical solutions and advantages of the present invention clearer, the technical solutions in the present invention will be clearly and completely described below with reference to the accompanying drawings in the present invention. Obviously, the described embodiments are some, but not all, of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments in the present invention without creative efforts shall fall within the protection scope of the present invention.

[0071] The optimization method for full-body motion planning of a mobile manipulator according to the present invention will be described below with reference to the accompanying drawings.

[0072] Figure 1 It is one of the schematic flow charts of the optimization method for full-body motion planning of a mobile manipulator provided by the present invention. As Figure 1 shown, the method includes the following steps 101 to 104.

[0073] Step 101, based on the depth image captured by the mobile manipulator, determine the environmental features and the position gradient when the joints of the mobile manipulator change.

[0074] During the movement of the mobile manipulator, environmental perception is required for motion speed planning and safe obstacle avoidance. As Figure 2 shown, in the environmental perception layer, first, the depth image captured by the depth camera of the mobile manipulator is obtained to acquire environmental information. The depth image has two functions. One is to perform point cloud calculation to construct a local distance map, specifically the local map of the environmental point cloud, which is used for subsequent further calculation of the position gradient when the joints of the mobile manipulator change, for position constraint. The other part is to be encoded by a convolutional neural network (CNN) encoder to obtain environmental features for speed planning.

[0075] Step 102: Use the actor-critic network trained by reinforcement learning to perform policy planning on the motion state and motion actions of the mobile manipulator to obtain the expected speed of the end effector in the mobile manipulator.

[0076] Continue to refer to Figure 2 , in the task space speed planning layer, the motion speed of the mobile manipulator moving to the target position is planned. The planning process can be designed as a task for the end of the mobile manipulator, specifically regarded as a Markov decision process for the motion state S and motion action A, that is, to determine the motion state and the executed motion action at each iteration time step t. The motion state includes joint angles, environmental features, and the position deviation between the end effector and the target position, while the motion action includes the motion speeds and rotational speed of the end effector in three directions in the three-dimensional rectangular coordinate system. The Markov decision process is to plan the motion actions in each motion state to determine the expected speed of the end effector in the mobile manipulator, ensure that the moving position of the mobile manipulator reaches the target position as soon as possible, and in addition, it is necessary to ensure avoiding collisions with obstacles or reaching singular positions. A singular position refers to a special position or node different from the normal position or regular position, which will not be elaborated.

[0077] Specifically, the Markov decision process can be implemented by an actor-critic network. In the implementation of the present invention, the actor-critic network can be a Distributed Soft Actor-Critic (DSAC). Further, the embodiment of the present invention adopts a Bayesian Distributed Soft Actor-Critic (BayesDSAC) for policy planning. The actor-critic network generally includes an actor network and a critic network, while the BayesDSAC in the embodiment of the present invention includes an actor network and two critic networks. The two critic networks will simultaneously evaluate the decision of the actor network and output two value distributions, and then fuse them through a Bayesian control function to obtain the corresponding mixed distribution for comprehensively evaluating the decision of the actor network. And the actor-critic network can be trained by reinforcement learning.

[0078] The motion state can be represented according to the robot joint angles and combined with the environmental features obtained by the environmental perception layer. The robot joint angles are used to determine the joint positions and implement joint constraints, while the environmental features can be used to help determine the moving position of the mobile manipulator.

[0079] The motion actions are represented by the velocities of the end effector of the mobile manipulator along the X, Y, and Z axes and the rotational velocities relative to the X, Y, and Z axes in its own three-dimensional coordinate system. After the actor-critic network is trained through reinforcement learning, after inputting the initial motion state of the mobile manipulator, it can perform policy planning on the motion state and motion actions of the mobile manipulator to obtain the desired velocities of the end effector in the mobile manipulator. This desired velocity also includes the velocities of the end effector along the X, Y, and Z axes in its own three-dimensional coordinate system. 、 、 and the rotational velocities relative to the X, Y, and Z axes. 、 、 which is denoted as .

[0080] Step 103: Construct position constraint conditions for the mobile manipulator to avoid obstacles based on the position gradient, and construct joint constraint parameters for constraining decision variables.

[0081] As Figure 2 shown, in the task space velocity planning layer, the desired velocity of the end effector of the mobile manipulator is planned through BayesDSAC. After that, it enters the final joint space motion planning layer. The goal of the mobile manipulator motion planning is to finally determine the joint velocities. In the joint space motion planning layer, mainly the quadratic programming (QP) process of the signed distance field (SDF) constraint is executed. In this process, first, the position gradient when the mobile manipulator joints change is determined according to the signed distance field, and then position constraint conditions for the mobile manipulator to avoid obstacles are constructed based on the position gradient to constrain the position. In addition, joint constraint parameters for constraining decision variables need to be constructed. The decision variable x is the optimization objective of the quadratic programming, including the joint velocities and relaxation norms of the mobile manipulator, and the decision variable is obtained by weighting the desired velocity through the manipulator Jacobian matrix. Thus, the process of optimizing the decision variable x is the process of optimizing the desired velocity of the end effector of the mobile manipulator. The joint velocities of the mobile manipulator are the joint velocities to be planned in the joint space motion planning layer, denoted as , and the relaxation norm is represented by the relaxation vector It is used to enable the mobile manipulator to better meet the constraints during movement and improve the control effect.

[0082] According to the above components, the position constraint conditions and joint constraint parameters are respectively constructed, and the optimization conditions for quadratic optimization are formed.

[0083] Step 104: Optimize the decision variables using the quadratic programming solver determined based on the position constraint conditions and joint constraint parameters to obtain the planned result of the joint velocity.

[0084] After constructing the position constraint conditions and joint constraint parameters through Step 103, the quadratic programming solver can be determined as the optimization condition. The quadratic programming solver completes the optimization of the decision variable x by executing the QP method to obtain the planned result of the joint velocity. The constraint conditions of the quadratic programming solver and its decision variable x can be expressed by the following formulas:

[0085] (1)

[0086] (2)

[0087] (3)

[0088] (4)

[0089] The above formula (1) is the solution formula for the quadratic programming solver to execute the QP method. represents the constraint weight in the joint constraint parameters, C represents the optimization bias, T represents the transpose of the matrix, and x is the decision variable. The above formulas (2), (3), and (4) are all the constraint conditions that the decision variable x in the quadratic programming solver of formula (1) needs to satisfy. In the above formula (2), represents the manipulator Jacobian matrix. represents the desired velocity of the end effector of the mobile manipulator, x is the decision variable, and formula (3) is the position constraint condition for obstacle avoidance of the mobile manipulator constructed based on the position gradient. and are the first constraint parameter and the second constraint parameter of the constraint condition respectively. In formula (4), is the lower limit of the decision variable x, that is, the lower limit of the desired velocity , generally taking the value of 0. is the upper limit of the decision variable x, that is, the upper limit of the desired velocity , and this upper limit needs to be determined according to the dynamic performance of the mobile manipulator, representing the maximum limit velocity that can be achieved.

[0090] The optimization objective of the quadratic programming solver to execute the QP method is to minimize the joint velocity Meanwhile, maximize the manipulation performance of the mobile manipulator. The manipulation performance is measured by the optimized offset C in the above formula (1). The larger the value of C, the better the manipulation performance of the mobile manipulator.

[0091] After optimizing the decision variable x through the QP method, the joint velocities will be obtained as the planning result, including specific joint velocities and the relaxation norm The joint velocities are used as the motion indicators of the mobile manipulator, and the relaxation norm is used to measure whether the mobile manipulator satisfies the motion constraints, and to judge whether the mobile manipulator strictly follows the joint velocities during the motion process. When the mobile manipulator approaches the target position, the joint velocities are restricted to ensure reaching the target position.

[0092] In the embodiment of the present invention, first, environmental perception and strategy planning are utilized to improve the motion planning effect and have real-time performance. Further, for the position constraints, joint velocity constraints, and relaxation norm constraints in obstacle avoidance, the desired velocity is planned through a quadratic programming solver to obtain the planning result of the joint velocities, ensuring the reliability, rapidity, and safety of the control of the mobile manipulator under complex motion trajectories, and solving the problems in the existing motion planning methods that there are limitations in the environmental perception ability and constraint expression ability, and it is difficult to achieve smooth and coordinated obstacle avoidance planning through single reinforcement learning.

[0093] In the embodiment of the present invention, an example structure of the mobile manipulator is as Figure 3 shown, specifically including: manipulator 1, joints 2, linkages 3, mobile base 4, and end effector 5. It can be seen that the manipulator 1 has an end effector 5, but consists of multiple joints 2 and linkages 3, and the joints are connected by linkages. Figure 3 Only two to three joints of one manipulator are shown in

[0094] In actual implementation, there can be multiple manipulators installed on the mobile base 4, and the number of joints and linkages on each manipulator can also be multiple, which can be set according to the actual scenario. In the embodiment of the present invention, one manipulator including 9 joints and 9 linkages will be used for illustration.

[0095] Specifically, the captured depth image is processed in two parts. In the first part, 5 depth images are overlapped, and then a convolutional neural network (CNN) is called to perform image encoding on the depth image to obtain environmental features, which are used to constitute the motion state of the mobile manipulator. The speed strategy is planned in the task space speed planning layer and can also be used to support the reinforcement learning process of BayesDSAC.

[0096] In the second part, an algorithm or program software for converting images to point clouds is used to process the conversion of the depth image to obtain a local distance map centered on the mobile manipulator, which is the local map of the environmental point cloud. Then, in the embodiment of the present invention, the coordinates of the environmental point cloud in the local map of the environmental point cloud are converted into position coordinates centered on the mobile manipulator through the signed distance field (SDF). The position coordinates include, but are not limited to, the position coordinates of each link k under the joint configuration q. The joint configuration q includes the number of joints and the current position of each joint. Since there must be obstacles in the captured depth image, the separation distance between the link k and the obstacle under the current joint configuration q of the mobile manipulator can be queried from the position coordinates, denoted as .

[0097] Finally, the position gradient when the joints of the mobile manipulator change is determined based on the separation distance. This is also to measure the impact on the distance from the obstacle when the joints of the mobile manipulator change. Here, the joint position of the mobile manipulator can be fine-tuned first, and the position fine-tuning amount is recorded. At the same time, the separation distances from the obstacle before and after the fine-tuning are recorded respectively. Then, the difference between the separation distances before and after the fine-tuning is calculated. Finally, differential calculation is performed based on the difference and the position fine-tuning amount to obtain the corresponding position gradient, denoted as , where q represents the current joint configuration, i represents the i-th joint, and k represents the k-th link. Through this position gradient, position constraints can be performed during the movement of the mobile manipulator to complete the obstacle avoidance task and ensure safety.

[0098] In the embodiment of the present invention, an environmental perception layer is designed during the motion planning of the mobile manipulator to achieve efficient environmental perception, quickly and accurately obtain surrounding environmental information, generate reliable environmental features, and use them to calculate the position gradient for obstacle avoidance. This efficient perception ability provides a solid foundation for subsequent behavior planning, ensuring that the mobile manipulator can effectively and safely perform dynamic tasks in a complex environment.

[0099] In some embodiments, Figure 2 the task space speed planning layer shown in mainly relies on the policy planning of BayesDSAC. During this policy planning process, the motion state and motion actions of the mobile manipulator are policy-planned through an actor-critic network trained by reinforcement learning to obtain the expected speed of the end effector in the mobile manipulator. The specific process of the policy planning is introduced below.

[0100] In the embodiments of the present invention, the strategy planning process of the motion state S and motion actions A of the mobile manipulator can be regarded as a Markov decision process. Specifically, the current motion state of the intelligent agent at a certain iterative time step t will probabilistically execute an action and then enter a new motion state and can obtain a reward As a return, this probability is called the state transition probability, denoted as . The goal of reinforcement learning is to learn a policy that can execute multiple motion actions and ensure the highest reward is obtained after each motion action is executed. Finally, when all motion actions are completed, the expected future cumulative return can be maximized.

[0101] For this Markov process, in the embodiments of the present invention, when training BayesDSAC through reinforcement learning, a distributed soft policy iteration (DSPI) framework is used for training. First, the maximum entropy of the actor-critic network at the iterative time step of reinforcement learning is constructed as the soft state-action return. Here, for each iterative time step t of the actor-critic network in the current state while executing the policy the action obtained, the reward is enhanced by the policy entropy, and then in the DSPI framework, the soft state-action return accumulated at the current iterative time step t is defined in a distributed form, and the formula is as follows:

[0102] (5)

[0103] In the above formula (5), represents the discount factor, represents the entropy coefficient, and its value range is (0, 1). t represents the time or iterative time step for executing the policy , and i represents the time or iterative time step after executing the action of the policy . represents the reward obtained when the actor-critic network executes the action of the policy at each iterative time step t in the current state . represents the current reward when the policy action is not executed at time t.

[0104] Next, according to the DSPI framework, the soft state-action return The expected value is used as the soft Q-value of the motion strategy and is denoted as , which is expressed as:

[0105] (6)

[0106] When performing reinforcement learning training, the soft Q-value is learned through the soft Bellman operator in the distributed soft policy iteration framework to conduct policy planning training on the actor-critic network. The learning process follows the following formula:

[0107] (7)

[0108] In the above formula (7), represents the soft Bellman operator under the policy , indicates that two variables have the same probability distribution, represents the entropy coefficient, represents the policy under which the action is executed. t represents the current iteration time step, and t + 1 represents the next iteration time step. represents the state at the next iteration time step t + 1, represents the cumulative soft state-action return at the current iteration time step t, represents the discount factor, r represents the reward value obtained at the current iteration time step t, represents the cumulative soft state-action return at the next iteration time step t + 1.

[0109] In each time step iteration process of the reinforcement training, the actor network in BayesDSAC is used to plan the policy execution action, and then the evaluation network evaluates the executed action to obtain the corresponding reward value and calculates the cumulative soft state-action return. Under the DSPI framework, both the actor network and the critic network need to rely on this cumulative soft state-action return, and then use formula (7) to learn the soft Q-value, so as to update their own parameters 、 . is the parameter of the actor network, is the parameter of the critic network.

[0110] In the process of performing policy iteration to learn the soft Q-value, by minimizing the Kullback-Leibler (KL) divergence between the soft state-action return of the target policy and the soft state-action return Z of the current policy, the soft state-action return Z of the current policy is updated. Among them, can be regarded as a training label, which is the target to be learned by BayesDSAC and can obtain the maximum expected future cumulative return, The corresponding first distribution is obtained by calculating through the soft Bellman operator, and the first distribution will be described in detail below.

[0111] By setting the corresponding iteration time step t or the number of iterations as the constraint condition for reinforcement learning training, after the actor-critic network is trained, the initial motion state of the mobile manipulator is input into the trained actor-critic network for motion strategy planning to obtain the optimal motion strategy of the mobile manipulator.

[0112] As described above, the motion state consists of joint angles, environmental features, and the position deviation between the end effector and the target position. Here, the initial motion state is composed of the initial joint angles of the mobile manipulator , environmental features and the position deviation between the end effector of the mobile manipulator and the target position . The initial joint angles can be obtained by instrument measurement, and the environmental features are the environmental features calculated by the environmental perception layer. Here, they are used to construct the initial motion state of the mobile manipulator, that is .

[0113] The trained actor-critic network BayesDSAC will plan the motion actions for each step according to the initial motion state. Each time an action is executed, the motion state will be changed, that is, the joint angles, environmental features, and the position deviation from the target position will be changed, so that the mobile manipulator can obtain the maximum cumulative soft state action reward, and finally the mobile manipulator reaches the target position to end the planning. In this way, a series of motion actions will be obtained as the optimal motion strategy.

[0114] Finally, based on the optimal motion strategy, the expected speed of the end effector in the mobile manipulator is determined, that is, according to the moving distance after executing the motion action at each iteration time step and the time spent on executing the motion action, the expected speed of the end effector in the mobile manipulator is determined, that is .

[0115] In the embodiment of the present invention, in the task space speed planning layer, the BayesDSAC trained by reinforcement learning reasonably plans the motion strategy of the mobile manipulator. Among them, during the planning process, the environmental features, joint angles, and the position deviation from the target position are used as the motion state for planning, which can enable the mobile manipulator to better integrate into the environmental state during the motion process. The finally provided expected speed of the end effector lays a foundation for determining the actual joint speed subsequently and generating continuous and high-quality whole-body control results.

[0116] Further, in the above embodiment, the policy planning of BayesDSAC is to determine a motion policy when the cumulative soft state-action return is maximized, which is related to the reward obtained as a return after performing an action at each time step. This reward value is enhanced as the policy entropy. The process of constructing the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning is introduced below. The reward obtained as a return after performing an action at each iterative time step in the embodiment of the present invention is

[0117] denoted as here, and it is composed of rewards in multiple parts.

[0118] First, a position reward value is constructed according to the second-order norm of the position deviation between the end effector of the mobile manipulator and the target position, denoted as and expressed as:

[0119] (8)

[0120] In the above formula (8), represents the position deviation between the end effector of the mobile manipulator and the target position, represents the function for calculating the second-order norm. It can be understood that the smaller the position deviation, the closer to the target position, and the higher the obtained position reward value.

[0121] In addition, when the second-order norm of the position deviation is less than a preset position deviation threshold, a moving target reward value is obtained, denoted as and expressed as:

[0122] (9)

[0123] In the above formula (9), represents the preset position deviation threshold, represents the function for calculating the second-order norm, represents the position deviation between the end effector of the mobile manipulator and the target position. The position deviation threshold is used to determine whether the end effector reaches the position where it should be at the iterative time step t. If the second-order norm of the position deviation is less than the position deviation threshold, it means that the end effector reaches the position where it should be, and a reward is given; otherwise, it means that it has not reached, and no reward is given.

[0124] During the movement of the mobile manipulator, a corresponding moving speed will be generated. Therefore, in the embodiment of the present invention, a speed reward value is also constructed based on the difference between the moving speed when a position deviation occurs during the movement of the mobile manipulator and the action speed of the end effector in the mobile manipulator, denoted as . Here, the action speed of the end effector in the mobile manipulator is the speed when performing the action of the policy planning, denoted as​ and the moving speed when there is a position deviation is then obtained by fitting calculation based on the position deviation and the joint angles The speed reward value is calculated by the following formula:

[0125] (10)

[0126] (11)

[0127] In the above formulas (10) and (11), is the moving speed when there is a position deviation during the movement of the mobile manipulator, represents the position deviation, represents the joint angle, represents a function for speed fitting calculation based on the end effector position and the position deviation. represents a function for calculating the second-order norm, represents the speed when performing the action of policy planning.

[0128] Here, the position reward and the speed reward generally need to be balanced. This is because getting closer to the target position and having a faster speed are both beneficial for the mobile manipulator to reach the target position accurately and quickly. Therefore, in the embodiments of the present invention, through the preset position reward weight and the speed reward weight , the position reward value and the speed reward value are weighted to obtain the weighted position reward value and the weighted speed reward value .

[0129] During the movement process, considering the obstacle avoidance situation of the mobile manipulator, corresponding rewards need to be given. Here, according to the closest distance y when the end effector of the mobile manipulator avoids obstacles and the preset obstacle avoidance distance threshold , an obstacle avoidance reward value is constructed, which is expressed by the following formula:

[0130] (12)

[0131] It can be understood that when , it means that the mobile manipulator starts to perform obstacle avoidance movement before reaching the appropriate obstacle avoidance distance, and no reward is given at this time. When , it means that the mobile manipulator completes the obstacle avoidance within a reasonable obstacle avoidance distance, and then a reward value is given according to the closest distance y when the end effector of the mobile manipulator avoids obstacles. And when It is stated that if the mobile robotic arm fails to avoid obstacles and hits an obstacle, a penalty value of 25 is given.

[0132] In addition, an embodiment of the present invention also constructs a time reward value based on the time taken during the movement of the mobile robotic arm , denoted as , and is expressed by the following formula:

[0133] (13)

[0134] It can be understood that the more time taken for the movement, the worse the movement effect, and thus the less time reward value is given.

[0135] Finally, the weighted position reward value , the movement target reward value , the weighted speed reward value , the obstacle avoidance reward value , and the time reward value are summed up to obtain the total reward value , which is expressed as:

[0136] (14)

[0137] Finally, the policy entropy is calculated based on the total reward value as the maximum entropy of the actor-critic network at the iterative time step of reinforcement learning. Here, the total reward value is used as the reward value at the iterative time step t and substituted into the above formula (5), and the maximum entropy of the actor-critic network at the iterative time step of reinforcement learning can be obtained, which is used as the soft state-action return at the current iterative time step t under the policy .

[0138] In an embodiment of the present invention, when performing policy iteration, multiple aspects such as position, speed, time, obstacle avoidance, and movement target are considered, and corresponding reward values are constructed one by one as the soft state-action return, which can ensure that the planned desired speed can reach the target as soon as possible while avoiding collisions and improve the motion planning effect.

[0139] The following introduces the policy planning training process of the actor-critic network. This process is actually a process of learning the soft Q-value at multiple iterative time steps under the DSPI framework. Specifically, in each iterative time step of learning the soft Q-value, the following operations are performed: On the one hand, when performing policy iteration using the actor network in the actor-critic network, the second distribution is updated by minimizing the KL divergence between the first distribution and the second distribution. In the following text, represents the distribution of the soft state-action return .

[0140] The first distribution is the distribution obtained by calculating the soft Bellman operator for the second distribution, that is, the distribution obtained by calculating the current soft state-action return distribution output by the critic network, which is the in the above formula (7). The second distribution is the current soft state-action return distribution output by the critic network, that is, the in the above formula (7). During policy iteration, the KL divergence between these two distributions is minimized to update the current soft state-action return distribution, making the second distribution closer to , thus completing the learning objective for the current iteration time step. The KL divergence is expressed as and the formula is as follows:

[0141] (15)

[0142] In the above formula (15), represents the function for calculating the KL divergence, represents the parameters of the actor network before parameter update. Here, represents the parameters of the actor network after parameter update, represents the parameters of the critic network before parameter update, is the current policy, B represents the buffer in the DSPI framework, that is, the area for storing motion actions . represents the current soft state-action return distribution output by the critic network before parameter update after the actor network executes the motion action under the policy , is the current soft state-action return distribution output by the critic network after parameter update, is the soft Bellman operator of the critic network before parameter update, represents predicting according to the probability distribution. represents that the current soft state-action return Z follows the current soft state-action return distribution output by the critic network before the actor network updates its parameters.

[0143] On the other hand, when using the critic network in the actor-critic network for policy evaluation, the parameters of the actor network and the parameters of the critic network are updated by minimizing the KL divergence. Here, minimizing the KL divergence is achieved by updating the parameters of the actor network and the parameters of the critic network , thus completing the training process of the actor-critic network.

[0144] When the preset iteration time step \(t\) or the number of iterations is reached, the training stops. The trained actor-critic network can then perform motion strategy planning for the initial motion state of the mobile manipulator to obtain the optimal motion strategy of the mobile manipulator.

[0145] In the actor-critic network of the embodiments of the present invention, by calculating and minimizing the KL divergence of the distribution of the current soft state-action return, it helps to achieve the training objective during each policy iteration, ensuring the training effectiveness of the actor-critic network.

[0146] In some embodiments, the training process of the actor-critic network is realized by minimizing the KL divergence. Further, updating the parameters of the actor network and the parameters of the critic network by minimizing the KL divergence is achieved by calculating the update gradient of the KL divergence and setting the update target of the update gradient to the temporal difference error of the current soft state-action return. The calculation process of the update gradient is introduced below.

[0147] First, in the embodiments of the present invention, the second distribution is modeled as a Gaussian distribution, that is, it satisfies , that is to say, the Gaussian distribution consists of the mean of the current soft state-action return and the variance of the current soft state-action return.

[0148] When calculating the update gradient to minimize the KL divergence, the update target is the temporal difference error of the second distribution. To avoid the problem of gradient explosion caused by excessive calculation of the square of the temporal difference error, in the embodiments of the present invention, the first distribution is processed by the boundary value limiting function Clip to obtain the first boundary-limited distribution, denoted as , and the boundary of the boundary value limiting function is determined according to the mean and the preset constraint boundary \(b\), expressed as:

[0149] (16)

[0150] In the above formula (16), represents the expected value of the mean of the current soft state-action return, used to replace , is the boundary limiting function, and \(b\) is the preset constraint boundary.

[0151] According to the above formula (6), in the embodiments of the present invention, the expected value of the soft state-action return is used as the soft Q-value of the motion strategy. Thus, the above formula (7) can be rewritten as:

[0152] (17)

[0153] In the above formula (17), represents in formula (7), is the policy, represents the entropy coefficient, represents the expected value of the soft state action return at the iterative time step t + 1, represents the action executed under the policy at the iterative time step t + 1, represents the state under the policy at the iterative time step t + 1, represents the parameters of the critic network before update at the iterative time step , represents the parameters of the actor network before update at the iterative time step . The meanings of the remaining parameters remain unchanged. For parameter explanations, refer to the above formula (7), and no redundant elaboration will be provided here.

[0154] Therefore, when actually calculating the divergence through formula (15), it is calculated using the expected value of , and when calculating the update gradient, it satisfies , which represents the mean of the current soft state action return. The purpose of this is to reduce the instability of the gradient update related to the mean caused by the random TD error return, and using the expected value can improve the estimation accuracy of the soft state action return.

[0155] Next, calculate the update gradient based on the first boundary - restricted distribution, the mean, and the variance. According to the foregoing, the first boundary - restricted distribution is obtained by processing the first distribution through the boundary - value restriction function Clip, and the boundaries of the boundary - value restriction function are determined according to the mean and the preset constraint boundary b. Thus, the update gradient of the KL divergence can be expressed as:

[0156] (18)

[0157] In the above formula (18), is the first boundary - restricted distribution obtained by processing the first distribution through the boundary - value restriction function Clip, represents the mean of the current soft state action return, represents the variance of the current soft state action return. represents 's differential, represents 's differential.

[0158] In the embodiments of the present invention, the KL divergence is minimized by calculating the update gradient of the KL divergence, and the distribution of the current soft state action return is modeled as a Gaussian distribution. Meanwhile, the boundary of the boundary value constraint function is determined according to the mean value in the Gaussian distribution and the preset constraint boundary, and then the first distribution is boundary-constrained by the boundary value constraint function to prevent the problem of update gradient explosion when minimizing the KL divergence, effectively improving the stability of reinforcement learning and ensuring the evaluation accuracy of the soft state action return output by the critic network.

[0159] During the training process of the actor-critic network, the existing technology method still has the problem of value underestimation, that is, there may be a problem of over-underestimation of the soft state action return by the critic network, which will make the convergence process of policy optimization in the reinforcement learning process slower and it is difficult to maximize the cumulative return. Based on the above scenario, as Figure 4 shown, in the embodiments of the present invention, the actor-critic network uses BayesDSAC, which specifically includes two critic networks. The distribution of the current soft state action return is obtained by mixing and estimating the distributions of the soft state action returns output by the two critic networks when performing policy evaluation by calling the Bayesian control function.

[0160] As Figure 4 shown, during policy iteration, critic network 1 and critic network 2 respectively evaluate the actions performed by the actor network under the old policy, and output the distributions of the current soft state action returns, which are respectively denoted as 、 and 、 . When calculating the update gradient, a mixed estimate is performed through the Bayesian control function (BCF) to obtain the corresponding mixed soft state action return distribution.

[0161] The following introduces the process of mixed estimation by the Bayesian control function. First, the Bayesian control function is called to perform likelihood estimation on the mean value and variance in the soft state action return distribution respectively to obtain the mixed mean value and mixed variance.

[0162] The Bayesian control function can be used to fuse and estimate the results of two value distributions. For example, as Figure 5 shown, for any two value distributions and , through the fusion estimation by the Bayesian control function, the composite estimation can effectively avoid underestimation and prevent overestimation.

[0163] In the embodiments of the present invention, the update gradient is calculated through the independent distributions and output by the two critic networks. These two independent distributions are fused by the Bayesian control function, and the formula is as follows:

[0164] (19)

[0165] In the above formula (19), represents the assumption of a uniform prior, represents the prior conditional probability distribution, represents the probability distributions corresponding to two independent distributions. is the calculated posterior conditional probability distribution, represents the parameters of one of the critic networks in BayesDSAC, represents the parameters of the other critic network in BayesDSAC.

[0166] Next, likelihood estimation is used to expand the Bayesian formula of the above formula (19), expressed as:

[0167] (20)

[0168] In the above formula (20), represents the prior conditional probability distribution corresponding to one of the critic networks, represents the prior conditional probability distribution corresponding to the other critic network, represents the probability distribution of the independent analysis output by one of the critic networks, represents the probability distribution of the independent analysis output by the other critic network.

[0169] In the above manner, the embodiments of the present invention call the Bayesian control function to respectively perform likelihood estimation on the mean and variance in the soft state action return distribution, obtain the mixed mean and the mixed variance, and thus fuse the approximate value estimations of the two independent distributions. The calculation process of the mixed mean is as follows:

[0170] (21)

[0171] The calculation process of the mixed variance is as follows:

[0172] (22)

[0173] In the above formulas (21) and (22), and respectively represent the mean and variance in the soft state action return distribution of one of the critic networks, and respectively represent the mean and variance in the soft state action return distribution of the other critic network.

[0174] Finally, the mixed mean and mixed variance of the two critic networks are fused to obtain the mixed soft state-action return distribution, which serves as the distribution of the current soft state-action return. The corresponding mixture Gaussian distribution is denoted as .

[0175] Thus, the above formula (17) can be further rewritten as:

[0176] (23)

[0177] In the above formula (23), is the mixed soft state-action return corresponding to in the above formula (17), is the expected value of the mixed soft state-action return corresponding to in the above formula (17). The meanings of the remaining parameters are the same as those in the above formula (17), and the parameter explanations can all refer to formula (17), so they will not be elaborated here.

[0178] For the first distribution when calculating the update gradient of the KL divergence in the above formula (18), it can be rewritten as , expressed as:

[0179] (24)

[0180] In the above formula (24), represents that before the parameters of the critic network are updated, the current soft state-action return follows the mixture Gaussian distribution . The explanations of the remaining parameters can refer to formula (18), so they will not be elaborated here.

[0181] Therefore, the above formula (18) can be rewritten as:

[0182] (25)

[0183] In the above formula (25), represents the parameters of the i-th critic network in BayesDSAC, is in the above formula (18), is in the above formula (18), represents the mean of the current soft state-action return of the i-th critic network, represents the variance of the current soft state-action return of the i-th critic network, has the same meaning as in the above formula (24), has the same meaning as has the same meaning, the difference is that is the updated gradient calculated for i comment networks. For the explanations of the remaining parameters, please refer to the above formulas (18) and (24), which will not be elaborated here.

[0184] As Figure 4 shown, by performing a mixture estimation on the distributions of the soft state-action returns output by the two comment networks, the corresponding mixture Gaussian distribution is obtained, and then the first distribution and the second distribution are calculated for the KL divergence, and the corresponding updated gradient is calculated to minimize the KL divergence. During this process, the parameters of the two comment networks are updated 、 . After the parameter update of the comment network is completed, it continues to make evaluations and outputs the distributions of the corresponding soft state-action returns, which are denoted as 、 and 、 . Next, the mixture mean and the mixture variance are calculated respectively using the above formulas (20) and (21), and the updated gradient is calculated at the next iteration time step.

[0185] For the actor network, as Figure 4 shown, at the time step t of policy iteration, the corresponding actions will be executed according to the old policy, and the old policy is denoted as . After the comment network makes an evaluation, it outputs the current distribution of the soft state-action returns , and then the KL divergence is calculated using the above formula (15), and further the updated gradient is calculated to minimize the KL divergence. During the process of minimizing the KL divergence, the actor network will adjust the old policy to form a new policy. Then, the corresponding actions are executed again for the new policy, and the new policy is denoted as .

[0186] After the comment network updates its parameters, it continues to make evaluations on the actions of the actor network executing the new policy and outputs the distributions of the corresponding soft state-action returns 、 、 、 , and then a mixture estimation is performed to obtain the mixture mean , which is used to evaluate the actions of the actor network executing the new policy. At this time, the actor network continues to update the policy actions according to the mixture mean , and the formula is as follows:

[0187] (26)

[0188] In the above formula (26), represents the reward value obtained by the actor network performing policy update, represents the mixed mean output when the critic network evaluates the action of the new policy executed by the actor network after updating the parameters, represents the entropy coefficient, represents the action executed by the actor network under the policy ).

[0189] Finally, according to the parameters of the actor network are updated , thus completing the introduction of the parameter update process of the actor network at the current iteration time step t.

[0190] In the embodiment of the present invention, BayesDSAC is used as the actor-critic network, and two critic networks are designed to evaluate the actor network during the reinforcement learning training. By mixing the value distributions through the Bayesian control function, the estimation quality of the value function is improved, and the problem of value underestimation caused by using the minimum value method in the prior art is avoided, thereby improving the policy convergence speed. This combination makes the learning ability of the mobile manipulator more powerful under complex tasks.

[0191] In some embodiments, the motion planning of the mobile manipulator depends on the QP method of the quadratic programming solver, such as the above formula (1). Before execution, various constraint conditions need to be constructed, and one of them is the position constraint condition for obstacle avoidance of the mobile manipulator constructed based on the position gradient in the above formula (3). The following describes the specific construction process.

[0192] The position constraint condition consists of two parts, namely the obstacle avoidance parameter and the velocity shock absorber parameter. The obstacle avoidance parameter is used to force the joints of the robot away from the collision boundary. In the environment perception layer, in the embodiment of the present invention, the distance between link k and the obstacle under the current joint configuration q of the mobile manipulator is queried through SDF, denoted as , and then the position gradient is calculated. This position gradient represents how joint i affects the distance between link k and the obstacle, forming a repulsive vector field in the joint space. Here, the position gradient is used to calculate the first obstacle avoidance parameter.

[0193] On the one hand, according to the position gradient of each link when the joints of the mobile manipulator change, the first obstacle avoidance parameter is constructed, and the formula is as follows:

[0194] (27)

[0195] In the above formula (27), represents the position gradient of the distance between link 9 of the 9th joint and the obstacle under the current joint configuration q. For the explanations of the remaining parameters, please refer to , which will not be elaborated here. represents the dimension of the matrix. In the following formulas, R always represents the dimension of the matrix and will not be elaborated one by one later.

[0196] On the other hand, according to the distance between each link and the obstacle when the joints of the mobile manipulator change , the difference between the distance to the link and the minimum distance to the obstacle , a second obstacle avoidance parameter is constructed, and the calculation formula is as follows:

[0197] (28)

[0198] In the above formula (28), represents the distance between link 9 and the obstacle under the current joint configuration q of the mobile manipulator. For the explanations of the remaining parameters, please refer to , which will not be elaborated here. represents the minimum distance between the link distance and the obstacle, which is preset according to historical experience.

[0199] Regarding the speed damper parameters, they are mainly used to limit the joint positions, specifically to reduce the joint speed of the mobile manipulator so that it decelerates when approaching the singular position.

[0200] Here, the first speed damper parameter and the second speed damper parameter for limiting the joint positions of the mobile manipulator are determined. Among them, the first speed damper parameter is a preset fixed parameter, expressed as the following formula:

[0201] (29)

[0202] And the second speed damper parameter is calculated according to the influence distance between the joints of the mobile manipulator and the singular position and the minimum distance. The influence distance characterizes the influence range of the singular position on the mobile manipulator. Within this influence range, the mobile manipulator needs to control the joint speed. The minimum distance is preset and refers to the limit distance that the mobile manipulator can approach the singular position. If it is less than this minimum distance, it indicates that the mobile manipulator is already in the singular position. The calculation formula for the second speed damper parameter is as follows:

[0203] (30)

[0204] In the above formula (30), represents the minimum distance between the joints of the mobile manipulator and the singular position, represents the influence distance between the joints of the mobile manipulator and the singular position, and i represents the i-th joint, represents a preset weight parameter.

[0205] Finally, the first velocity shock absorber parameter is fused with the first obstacle avoidance parameter to obtain the first constraint parameter , and the second velocity shock absorber parameter is fused with the second obstacle avoidance parameter to obtain the second constraint parameter . The specific fusion method is to perform physical splicing of matrices, which are respectively represented as:

[0206] (31)

[0207] (32)

[0208] Finally, the ratio of the second constraint parameter to the first constraint parameter is used as the position constraint condition, that is, the above formula (3).

[0209] In the embodiment of the present invention, when planning the joint speed of the mobile manipulator, the constraint conditions of the joint speed are constructed by designing the obstacle avoidance parameter and the speed shock absorber parameter. The obstacle avoidance parameter can enable the mobile manipulator to have good obstacle avoidance performance and improve the motion safety. At the same time, the speed shock absorber parameter can enable the mobile manipulator to reasonably control the joint speed when facing the singular position, ensuring that the joints avoid reaching the singular position.

[0210] Furthermore, in the quadratic programming solver, the joint constraint parameters include a constraint weight and an optimization bias, which are respectively and C in the above formula (1). The following introduces the specific process of constructing the joint constraint parameters for constraining the decision variable x.

[0211] Regarding the constraint weight , in the embodiment of the present invention, the value of the position deviation between the end effector of the mobile manipulator and the target position is used as the third constraint parameter for constraining the joint speed, and the reciprocal of the value of the position deviation is used as the fourth constraint parameter for constraining the joint speed. Then, the third constraint parameter and the fourth constraint parameter are combined into the target weight for constraining the joint speed, which can be specifically denoted as , the combination here is to directly splice matrices.

[0212] Obtain the relaxation weight for adjusting the relaxation norm of , this relaxation weight is preset, and the relaxation norm is used to measure whether the mobile manipulator satisfies the motion constraints, while the relaxation weight measures the degree and effect of satisfying the motion constraints. When the position deviation between the end effector of the mobile manipulator and the target position is larger, it means that the mobile manipulator is farther away from the target position. At this time, the mobile manipulator can be limitedly unrestricted by the motion constraints of the joint speed and allow errors to occur.

[0213] Here, the constraint weight is constructed based on the relaxation weight and the target weight , and the constraint weight is used to control the joint speed during the movement of the mobile manipulator and adjust the relaxation vector at the same time. The construction process can be expressed by the following formula:

[0214] (33)

[0215] In the above formula (33), is the target weight for constraining the joint speed , represents the relaxation weight for adjusting the relaxation norm , and the diag function represents the function for constructing a diagonal matrix.

[0216] For the optimization bias C, it is a value that measures the operating performance of the mobile manipulator. Here, first determine the angle between the mobile manipulator and the mobile base, and determine the adjustment gain value of the angle . The adjustment gain value is specifically calculated through the preset adjustment gain weight . Next, based on the adjustment gain value and the manipulability Jacobian matrix of the mobile manipulator, the optimization bias C is constructed. The optimization bias C is used to minimize the angle during the movement of the mobile manipulator, which is expressed by the following formula:

[0217] (34)

[0218] In the above formula (34), represents the preset manipulability Jacobian matrix of the mobile manipulator, represents the angle between the mobile manipulator and the mobile base, is the preset adjustment gain weight.

[0219] Through the above formula (33) and formula (34), the constraint weight After optimizing the bias C, it can be used as a joint constraint parameter to construct the quadratic programming solver shown in the above formula (1) to perform the optimization of the decision variable x.

[0220] Meanwhile, in order to improve the elegance of the movement of the mobile manipulator in the embodiments of the present invention, the Jacobian matrix of the manipulator is used to construct the decision variable x, that is, the above formula (2). However, this Jacobian matrix of the manipulator is obtained by expanding the identity matrix and is expressed as the following formula:

[0221] (35)

[0222] In the above formula (35), represents the Jacobian matrix of the manipulator before expansion, represents the identity matrix with a dimension of 6×6.

[0223] In the embodiments of the present invention, when constructing the quadratic programming solver, joint constraint parameters for constructing a relaxation vector for constraining the joint speed are constructed, so that the movement of the base and the manipulator can be better moved. At the same time, a parameter is also constructed to minimize the angle between the mobile manipulator and the mobile base during the movement of the mobile manipulator, and the direction of the end effector is constrained on the mobile base, thereby enhancing the flexibility of the mobile manipulator and improving the operability of the mobile manipulator.

[0224] Next, an optimization device for the whole-body motion planning of a mobile manipulator provided by the present invention will be described. The optimization device for the whole-body motion planning of a mobile manipulator described below can be correspondingly referred to the optimization method for the whole-body motion planning of a mobile manipulator described above.

[0225] Figure 6 is a schematic structural diagram of an optimization device for the whole-body motion planning of a mobile manipulator provided by the present invention, as Figure 6As shown in the figure, the optimization device for the full-body motion planning of a mobile manipulator includes an environment perception module 601, a speed planning module 602, and a motion planning module 603. Specifically, the environment perception module 601 is used to determine the environmental features and the position gradient when the joints of the mobile manipulator change based on the depth image captured by the mobile manipulator; the speed planning module 602 is used to perform policy planning on the motion state and motion actions of the mobile manipulator through an actor-critic network trained by reinforcement learning to obtain the expected speed of the end effector in the mobile manipulator, where the motion state includes joint angles, the environmental features, and the position deviation between the end effector and the target position, and the motion actions include the motion speeds and rotational speed of the end effector in three directions in a three-dimensional rectangular coordinate system; the motion planning module 603 is used to construct position constraint conditions for obstacle avoidance of the mobile manipulator based on the position gradient, and construct joint constraint parameters for constraining decision variables, where the decision variables include the joint speed and relaxation norm of the mobile manipulator, and the decision variables are obtained by weighting the expected speed through the manipulator Jacobian matrix; the motion planning module 603 is further used to optimize the decision variables through a quadratic programming solver determined based on the position constraint conditions and the joint constraint parameters to obtain the planning result of the joint speed.

[0226] It should be noted that the beneficial effects of the optimization device for the full-body motion planning of a mobile manipulator here can correspond to those of the optimization method for the full-body motion planning of a mobile manipulator in the above text. Therefore, the beneficial effects of the optimization device for the full-body motion planning of a mobile manipulator will not be elaborated here.

[0227] Figure 7 The schematic diagram of the physical structure of an electronic device is exemplified, as Figure 7As shown, the electronic device may include: a processor 710, a communications interface 720, a memory 730, and a communication bus 740. Among them, the processor 710, the communications interface 720, and the memory 730 complete communication with each other through the communication bus 740. The processor 710 may call the logical instructions in the memory 730 to execute an optimization method for full-body motion planning of a mobile manipulator. The method includes: determining environmental features and position gradients when the joints of the mobile manipulator change based on the depth image captured by the mobile manipulator; performing policy planning on the motion state and motion actions of the mobile manipulator through an actor-critic network trained by reinforcement learning to obtain the desired speed of the end effector in the mobile manipulator. The motion state includes joint angles, the environmental features, and the position deviation between the end effector and the target position. The motion actions include the motion speeds and rotational speed of the end effector in three directions in a three-dimensional rectangular coordinate system; constructing position constraint conditions for obstacle avoidance of the mobile manipulator based on the position gradients, and constructing joint constraint parameters for constraining decision variables. The decision variables include the joint speed and relaxation norm of the mobile manipulator, and the decision variables are obtained by weighting the desired speed through the manipulator Jacobian matrix; optimizing the decision variables through a quadratic programming solver determined based on the position constraint conditions and the joint constraint parameters to obtain the planning result of the joint speed.

[0228] In addition, when the logical instructions in the above-mentioned memory 730 can be implemented in the form of software functional units and sold or used as an independent product, they can be stored in a computer-readable storage medium. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: various media such as USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical discs that can store program codes.

[0229] On the other hand, the present invention also provides a computer program product, which includes a computer program. The computer program can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the optimization method for full-body motion planning of a mobile manipulator provided by the above-mentioned various methods. The method includes: determining environmental features and position gradients when the joints of the mobile manipulator change based on the depth image captured by the mobile manipulator; performing policy planning on the motion state and motion actions of the mobile manipulator through an actor-critic network trained by reinforcement learning to obtain the desired speed of the end effector in the mobile manipulator. The motion state includes joint angles, the environmental features, and the position deviation between the end effector and the target position. The motion actions include the motion speeds and rotational speed of the end effector in three directions in a three-dimensional rectangular coordinate system; constructing position constraint conditions for obstacle avoidance of the mobile manipulator based on the position gradients, and constructing joint constraint parameters for constraining decision variables. The decision variables include the joint speed and relaxation norm of the mobile manipulator, and the decision variables are obtained by weighting the desired speed through the manipulator Jacobian matrix; optimizing the decision variables through a quadratic programming solver determined based on the position constraint conditions and the joint constraint parameters to obtain the planning result of the joint speed.

[0230] In yet another aspect, the present invention also provides a non-transitory computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, it implements the optimization method for full-body motion planning of a mobile manipulator provided by the above-mentioned various methods. The method includes: determining environmental features and position gradients when the joints of the mobile manipulator change based on the depth image captured by the mobile manipulator; performing policy planning on the motion state and motion actions of the mobile manipulator through an actor-critic network trained by reinforcement learning to obtain the desired speed of the end effector in the mobile manipulator. The motion state includes joint angles, the environmental features, and the position deviation between the end effector and the target position. The motion actions include the motion speeds and rotational speed of the end effector in three directions in a three-dimensional rectangular coordinate system; constructing position constraint conditions for obstacle avoidance of the mobile manipulator based on the position gradients, and constructing joint constraint parameters for constraining decision variables. The decision variables include the joint speed and relaxation norm of the mobile manipulator, and the decision variables are obtained by weighting the desired speed through the manipulator Jacobian matrix; optimizing the decision variables through a quadratic programming solver determined based on the position constraint conditions and the joint constraint parameters to obtain the planning result of the joint speed.

[0231] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. A person of ordinary skill in the art can understand and implement it without creative labor.

[0232] Through the description of the above embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on such an understanding, the essence of the above technical solution or the part that contributes to the prior art can be embodied in the form of a software product. The computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0233] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments or perform equivalent replacements for some of the technical features. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An optimization method for whole-body motion planning of a mobile manipulator, characterized in that, Including: Based on the depth image captured by the mobile manipulator, determine the environmental features and the position gradient when the joints of the mobile manipulator change; The actor-critic network trained by reinforcement learning performs policy planning on the motion state and motion actions of the mobile manipulator to obtain the expected velocity of the end effector in the mobile manipulator. The motion state includes joint angles, the environmental features, and the position deviation between the end effector and the target position. The motion actions include the motion velocities and rotational velocity of the end effector in three directions in the three-dimensional Cartesian coordinate system; Based on the position gradient, construct position constraint conditions for the mobile manipulator to avoid obstacles, and construct joint constraint parameters for constraining decision variables. The decision variables include the joint velocity and relaxation norm of the mobile manipulator, and the decision variables are obtained by weighting the expected velocity through the manipulator Jacobian matrix; Optimize the decision variables by using a quadratic programming solver determined based on the position constraint conditions and the joint constraint parameters to obtain the planning result of the joint velocity; The actor-critic network trained by reinforcement learning performs policy planning on the motion state and motion actions of the mobile manipulator to obtain the expected velocity of the end effector in the mobile manipulator, including: Construct the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning as the soft state-action return; Take the expected value of the soft state-action return as the soft Q-value of the motion policy, and learn the soft Q-value through the soft Bellman operator in the distributed soft policy iteration framework to perform policy planning training on the actor-critic network; Input the initial motion state of the mobile manipulator into the trained actor-critic network for motion policy planning to obtain the optimal motion policy of the mobile manipulator; Determine the expected velocity of the end effector in the mobile manipulator based on the optimal motion policy; The constructing the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning includes: Construct a position reward value according to the second-order norm of the position deviation between the end effector of the mobile manipulator and the target position, and obtain a moving target reward value when the second-order norm of the position deviation is less than a preset position deviation threshold; Construct a velocity reward value based on the difference between the moving velocity when the position deviation is generated during the motion of the mobile manipulator and the action velocity of the end effector in the mobile manipulator; Weight the position reward value and the velocity reward value through preset position reward weights and velocity reward weights to obtain a weighted position reward value and a weighted velocity reward value; Construct an obstacle avoidance reward value according to the closest distance when the end effector of the mobile manipulator avoids obstacles and a preset obstacle avoidance distance threshold; Construct a time reward value based on the time spent during the motion of the mobile manipulator; Sum up the weighted position reward value, the weighted velocity reward value, the obstacle avoidance reward value, and the time reward value to obtain a total reward value; Calculate the policy entropy based on the total reward value as the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning.

2. The optimization method for whole-body motion planning of a mobile manipulator according to claim 1, characterized in that Determining environmental features and position gradients when the joints of the mobile manipulator change based on the depth image captured by the mobile manipulator includes: Invoking a convolutional neural network to perform image encoding on the depth image to obtain environmental features; Performing conversion processing on the depth image to obtain a local environmental point cloud map centered on the mobile manipulator; Converting the coordinates of the environmental point cloud in the local environmental point cloud map into position coordinates centered on the mobile manipulator through a signed distance field, and querying the separation distance between the link and the obstacle under the current joint configuration of the mobile manipulator from the position coordinates; Determining the position gradient when the joints of the mobile manipulator change based on the separation distance.

3. The optimization method for whole-body motion planning of a mobile manipulator according to claim 1, wherein The policy planning training process of the actor-critic network includes: During each iteration time step of learning the soft Q value, perform the following operations: When performing policy iteration using the actor network in the actor-critic network, update the second distribution by minimizing the KL divergence between the first distribution and the second distribution, where the first distribution is the distribution obtained by calculating the second distribution using the soft Bellman operator, and the second distribution is the distribution of the current soft state-action return output by the critic network in the actor-critic network; When performing policy evaluation using the critic network, update the parameters of the actor network and the parameters of the critic network by minimizing the KL divergence.

4. The optimization method for whole-body motion planning of a mobile manipulator according to claim 3, characterized in that Updating the parameters of the actor network and the parameters of the critic network by minimizing the KL divergence is achieved by calculating the update gradient of the KL divergence and setting the update target of the update gradient to the temporal difference error of the current soft state-action return. The calculation process of the update gradient includes: Modeling the second distribution as a Gaussian distribution, which consists of the mean of the current soft state-action return and the variance of the current soft state-action return; Calculating the update gradient based on the first boundary-limited distribution, the mean, and the variance; Wherein, the first boundary-limited distribution is obtained by processing the first distribution through a boundary value limiting function, and the boundary of the boundary value limiting function is determined according to the mean and a preset constraint boundary.

5. The optimization method for full-body motion planning of a mobile manipulator according to claim 4, wherein There are two critic networks in the actor-critic network. The distribution of the current soft state-action return is obtained by mixing and estimating the distributions of the soft state-action returns output by the two critic networks during policy evaluation by invoking a Bayesian control function. The process of the Bayesian control function for mixing and estimating includes: Invoking the Bayesian control function to perform likelihood estimation on the mean and variance in the distribution of the soft state-action return respectively to obtain a mixed mean and a mixed variance; Fusing the mixed mean and the mixed variance of the two critic networks to obtain a mixed soft state-action return distribution as the distribution of the current soft state-action return.

6. The optimization method for full-body motion planning of a mobile manipulator according to claim 1, characterized in that Constructing position constraint conditions for obstacle avoidance of the mobile manipulator based on the position gradient includes: Constructing first obstacle avoidance shock absorber parameters according to the position gradient of each link when the joints of the mobile manipulator change; Construct the second obstacle avoidance shock absorber parameters according to the distance between each link and the obstacle when the mobile manipulator joint changes, and the difference between the distance to the link and the minimum distance to the obstacle. Determine the first speed shock absorber parameter and the second speed shock absorber parameter for restricting the joint position of the mobile manipulator. The first speed shock absorber parameter is a preset fixed parameter, and the second speed shock absorber parameter is calculated according to the influence distance and the minimum distance between the mobile manipulator joint and the singular position. Fuse the first speed shock absorber parameter with the first obstacle avoidance shock absorber parameter to obtain a first constraint parameter, and fuse the second speed shock absorber parameter with the second obstacle avoidance shock absorber parameter to obtain a second constraint parameter. Use the ratio of the second constraint parameter to the first constraint parameter as the position constraint condition.

7. The optimization method for full-body motion planning of a mobile manipulator according to claim 1, characterized in that, The joint constraint parameter includes a constraint weight and an optimization bias. Constructing the joint constraint parameter for constraining the decision variable includes: Use the value of the position deviation between the end effector of the mobile manipulator and the target position as the third constraint parameter for constraining the joint speed, and use the reciprocal of the value of the position deviation as the fourth constraint parameter for constraining the joint speed. Combine the third constraint parameter and the fourth constraint parameter into a target weight for constraining the joint speed. Obtain a relaxation weight for adjusting the relaxation norm, and construct a constraint weight based on the relaxation weight and the target weight. Determine the angle between the mobile manipulator and the mobile base, and determine the adjustment gain value of the angle. Construct an optimization bias based on the adjustment gain value and the manipulative Jacobian matrix of the mobile manipulator. The optimization bias is used to minimize the angle when the mobile manipulator moves.

8. An optimization device for whole-body motion planning of a mobile manipulator, characterized in that, Include: An environment perception module for determining environmental features and the position gradient when the mobile manipulator joint changes based on the depth image captured by the mobile manipulator. A speed planning module for performing policy planning on the motion state and motion actions of the mobile manipulator through an actor-critic network trained by reinforcement learning to obtain the expected speed of the end effector in the mobile manipulator. The motion state includes joint angles, the environmental features, and the position deviation between the end effector and the target position. The motion actions include the motion speeds and the rotation speed of the end effector in three directions in a three-dimensional rectangular coordinate system. A motion planning module for constructing a position constraint condition for the mobile manipulator to avoid obstacles based on the position gradient, and constructing joint constraint parameters for constraining the decision variable. The decision variable includes the joint speed and the relaxation norm of the mobile manipulator, and the decision variable is obtained by weighting the expected speed through the manipulator Jacobian matrix. The motion planning module is further configured to optimize the decision variable through a quadratic programming solver determined by the position constraint condition and the joint constraint parameter to obtain the planning result of the joint speed. Performing policy planning on the motion state and motion actions of the mobile manipulator through an actor-critic network trained by reinforcement learning to obtain the expected speed of the end effector in the mobile manipulator includes: Construct the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning as the soft state-action return; Use the expected value of the soft state-action return as the soft Q-value of the motion policy, and learn the soft Q-value through the soft Bellman operator in the distributed soft policy iteration framework to perform policy planning training on the actor-critic network; Input the initial motion state of the mobile manipulator into the trained actor-critic network for motion policy planning to obtain the optimal motion policy of the mobile manipulator; Determine the expected velocity of the end effector in the mobile manipulator based on the optimal motion policy; The construction of the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning includes: Construct a position reward value according to the second-order norm of the position deviation between the end effector of the mobile manipulator and the target position, and obtain a moving target reward value when the second-order norm of the position deviation is less than a preset position deviation threshold; Construct a velocity reward value based on the difference between the moving velocity when the position deviation is generated during the motion of the mobile manipulator and the action velocity of the end effector in the mobile manipulator; Weight the position reward value and the velocity reward value through preset position reward weights and velocity reward weights to obtain a weighted position reward value and a weighted velocity reward value; Construct an obstacle avoidance reward value according to the closest distance when the end effector of the mobile manipulator avoids obstacles and a preset obstacle avoidance distance threshold; Construct a time reward value based on the time spent during the motion of the mobile manipulator; Sum up the weighted position reward value, the weighted velocity reward value, the obstacle avoidance reward value, and the time reward value to obtain a total reward value; Calculate the policy entropy based on the total reward value as the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning.

Images