Optimization method for whole-body motion planning of mobile mechanical arm

Abstract

The invention provides an optimization method for whole-body motion planning of a mobile manipulator, and relates to the technical field of motion planning of the manipulator, and the method comprises the steps: determining environment features and a position gradient when a joint of the mobile manipulator changes based on a depth image shot by the mobile manipulator; performing strategy planning on the motion state and the motion action of the mobile mechanical arm through an actor-commentator network subjected to reinforcement learning training to obtain an expected speed of an end effector in the mobile mechanical arm; position constraint conditions used for obstacle avoidance of the movable mechanical arm are constructed based on the position gradient, and joint constraint parameters used for constraining the decision variables are constructed; and optimizing the decision variable based on a quadratic programming solver determined by the position constraint condition and the joint constraint parameter to obtain a planning result of the joint speed. According to the method and the device, the problem of limitation in the aspects of planning task completion, environment perception, constraint expression, obstacle avoidance and motion coordination during motion planning of the mobile mechanical arm is solved.

Description

Technical Field

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

[0002] In the field of mobile robot motion planning, existing technologies have many deficiencies in solving the problem of mobile robot motion planning. The traditional staged control method plans the robot arm and chassis separately. Although it is simple and easy to use, it limits the solution space and seriously reduces the efficiency of the system, especially in complex environments where multiple trials and errors are required, making it difficult to complete the task efficiently.

[0003] Among the existing technologies, optimization-based motion planning methods are one of the current research hotspots. This type of method introduces a mathematical optimization model to coordinate the control of a multi-robot degree of freedom system. Although it performs well in terms of accuracy and model utilization, it has limitations in terms of environmental perception and constraint expression capabilities, poor real-time performance, and difficulty in achieving smooth and coordinated obstacle avoidance planning in a dynamic obstacle environment. Motion planning is performed using reinforcement learning methods, but it relies on environmental feature extraction. It can achieve online learning, but it is difficult to generate smooth, continuous, and coordinated motion trajectories in complex scenarios with multiple redundant degrees of freedom. It is often limited by discrete action space and algorithm generalization performance, resulting in unsatisfactory motion planning results. Although hybrid strategies that integrate reinforcement learning and optimization methods have potential, they still have limitations in obstacle avoidance and improving motion coordination. Summary of the invention

[0004] The present invention provides an optimization method for whole-body motion planning of a mobile robot arm, so as to overcome the defects of limitations in planning task completion, environmental perception, constraint expression, obstacle avoidance, and motion coordination when planning the motion of a mobile robot arm.

[0005] The present invention provides an optimization method for whole-body motion planning of a mobile manipulator, comprising: Based on the depth image captured by the mobile robot arm, determine the environmental features and the position gradient when the joints of the mobile robot arm change; The actor-critic network trained by reinforcement learning performs strategic planning on the motion state and motion action of the mobile robot arm to obtain the expected speed of the end effector in the mobile robot arm, wherein the motion state includes the joint angle, the environmental characteristics, and the position deviation between the end effector and the target position, and the motion action includes the motion speed and rotation speed of the end effector in three directions in a three-dimensional rectangular coordinate system; Constructing a position constraint condition for the mobile manipulator to avoid obstacles based on the position gradient, and constructing joint constraint parameters for constraining decision variables, wherein the decision variables include a joint velocity and a relaxation norm of the mobile manipulator, and the decision variables are obtained by weighting the desired velocity through the manipulator Jacobian matrix; The decision variables are optimized 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.

[0006] In some embodiments, the step of determining the environmental features and the position gradient of the mobile mechanical arm when the joints of the mobile mechanical arm change based on the depth image taken by the mobile mechanical arm includes: Calling a convolutional neural network to perform image encoding on the depth image to obtain environmental features; The depth image is converted to obtain a local point cloud map of the environment centered on the mobile robotic arm; The coordinates of the environment point cloud in the local map of the environment point cloud are converted into position coordinates centered on the mobile mechanical arm through a signed distance field, and the distance between the connecting rod and the obstacle under the current joint configuration of the mobile mechanical arm is queried from the position coordinates; The position gradient of the mobile robot arm when the joint changes is determined based on the separation distance.

[0007] In some embodiments, the actor-critic network trained by reinforcement learning performs strategic planning on the motion state and motion action of the mobile robotic arm to obtain the expected speed of the end effector in the mobile robotic arm, including: Construct the maximum entropy of the actor-critic network at the iterative time step of reinforcement learning as the soft-state action return; Using the expected value of the soft state action reward as the soft Q value of the motion strategy, and learning the soft Q value through a soft Bellman operator in a distributed soft policy iteration framework to perform strategy planning training on the actor-critic network; The initial motion state of the mobile robot is input into the trained actor-critic network for motion strategy planning to obtain the optimal motion strategy of the mobile robot. The desired speed of the end effector in the mobile robot arm is determined based on the optimal motion strategy.

[0008] In some embodiments, constructing the maximum entropy of the actor-critic network at the iterative time step of reinforcement learning includes: Constructing a position reward value according to a second-order norm of a position deviation between an end effector of the mobile robot arm and a 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; Constructing a speed reward value based on the difference between the moving speed of the mobile robot arm when the position deviation is generated during the movement of the mobile robot arm and the action speed of the end effector in the mobile robot arm; The position reward value and the speed reward value are weighted by a preset position reward weight and a preset speed reward weight to obtain a weighted position reward value and a weighted speed reward value; According to the shortest distance when the end effector of the mobile robot avoids obstacles and the preset obstacle avoidance distance threshold, an obstacle avoidance reward value is constructed; Construct a time reward value based on the time spent on the mobile robot movement process; The weighted position reward value, the weighted speed reward value, the obstacle avoidance reward value and the time reward value are summed to obtain a total reward value; The policy entropy is calculated based on the total reward value as the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning.

[0009] In some embodiments, the strategy planning training process of the actor-critic network includes: During each iterative time step of learning the soft Q value, the following operations are performed: 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, wherein the first distribution is a distribution obtained by calculating the second distribution by a soft Bellman operator, and the second distribution is a distribution of current soft state action returns output by the critic network in the actor-critic network; When the critic network is used for policy evaluation, the parameters of the actor network and the parameters of the critic network are updated by minimizing the KL divergence.

[0010] In some embodiments, the updating of the parameters of the actor network and the comment 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 time difference error of the current soft state action reward. The calculation process of the update gradient includes: Modeling the second distribution as a Gaussian distribution consisting of a mean of the current soft-state action reward and a variance of the current soft-state action reward; Calculate an update gradient based on the first bound limit distribution, the mean, and the variance; The first boundary restriction distribution is obtained by processing the first distribution through a boundary value restriction function, and the boundary of the boundary value restriction function is determined according to the mean and a preset constraint boundary.

[0011] In some embodiments, the actor-critic network includes two critic networks, and the current distribution of soft state action returns is obtained by calling a Bayesian control function to perform mixed estimation on the distribution of soft state action returns output by the two critic networks when performing policy evaluation. The process of the Bayesian control function performing mixed estimation includes: Call the Bayesian control function to estimate the likelihood of the mean and variance in the distribution of the soft state action return, and obtain the mixed mean and mixed variance; The mixed mean and mixed variance of the two review networks are fused to obtain the mixed soft-state action reward distribution as the distribution of the current soft-state action reward.

[0012] In some embodiments, constructing a position constraint condition for moving the manipulator to avoid obstacles based on the position gradient includes: Constructing the first obstacle avoidance shock absorber parameters according to the position gradient of each link when the joint of the mobile robot arm changes; Constructing a second obstacle avoidance shock absorber parameter according to the distance between each link and the obstacle when the joint of the mobile robot arm changes, and the difference between the distance between the link and the minimum distance of the obstacle; Determining a first velocity damper parameter and a second velocity damper parameter for limiting the position of the joint of the mobile mechanical arm, wherein the first velocity damper parameter is a preset fixed parameter, and the second velocity damper parameter is calculated according to an influence distance and a minimum distance between the joint of the mobile mechanical arm and the singular position; The first speed damper parameter is merged with the first obstacle avoidance damper parameter to obtain a first constraint parameter, and the second speed damper parameter is merged with the second obstacle avoidance damper parameter to obtain a second constraint parameter; The ratio of the second constraint parameter to the first constraint parameter is used as a position constraint condition.

[0013] In some embodiments, the joint constraint parameters include constraint weights and optimization biases, and the construction of the joint constraint parameters for constraining the decision variables includes: The value of the position deviation between the end effector of the mobile robot arm 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; combining the third constraint parameter and the fourth constraint parameter into a target weight for constraining a joint velocity; Acquire a relaxation weight for adjusting the relaxation norm, and construct a constraint weight based on the relaxation weight and the target weight; determining an angle between the mobile mechanical arm and the mobile base, and determining an adjustment gain value of the angle; An optimized bias is constructed according to the adjustment gain value and the maneuverability Jacobian matrix of the mobile robot arm, and the optimized bias is used to minimize the angle when the mobile robot arm moves.

[0014] The present invention also provides an optimization device for whole-body motion planning of a mobile manipulator, comprising: An environmental perception module, which is used to determine environmental features and position gradients when the joints of the mobile robotic arm change based on the depth image taken by the mobile robotic arm; A speed planning module is used to perform strategic planning on the motion state and motion action of the mobile robot arm through an actor-critic network trained by reinforcement learning, so as to obtain a desired speed of an end effector in the mobile robot arm, wherein the motion state includes a joint angle, the environmental characteristics, and a position deviation between the end effector and a target position, and the motion action includes the motion speed and rotation speed of the end effector in three directions in a three-dimensional rectangular coordinate system; A motion planning module, for constructing position constraints for the mobile manipulator to avoid obstacles based on the position gradient, and constructing joint constraint parameters for constraining decision variables, wherein the decision variables include joint velocities and relaxation norms of the mobile manipulator, and the decision variables are obtained by weighting the desired velocity through the manipulator Jacobian matrix; The motion planning module is also used to optimize the decision variables based on the position constraint conditions and the quadratic planning solver determined by the joint constraint parameters to obtain the planning result of the joint speed.

[0015] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, an optimization method for whole-body motion planning for a mobile robotic arm as described in any one of the above is implemented.

[0016] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements any of the above-described optimization methods for whole-body motion planning for a mobile robotic arm.

[0017] The present invention also provides a computer program product, comprising a computer program, wherein when the computer program is executed by a processor, the optimization method for whole-body motion planning for a mobile robotic arm as described in any one of the above is implemented.

[0018] The optimization method for whole-body motion planning of a mobile manipulator provided by the present invention first performs environmental perception and uses depth images to determine environmental features and the position change gradient of the mobile manipulator. On this basis, the desired speed of the end effector is strategically planned through an actor-critic network trained by reinforcement learning, thereby improving the motion planning effect and achieving real-time performance. Furthermore, based on the position constraints, joint speed constraints, and relaxation norm constraints for obstacle avoidance, the desired speed is planned through a quadratic programming solver to obtain the planning results of the joint speed, thereby ensuring the reliability, rapidity, and safety of the mobile manipulator control under complex motion trajectories, and solving the problems in the existing motion planning methods, such as the limitations of the ability to perceive the environment and the ability to express constraints, and the difficulty in achieving smooth and coordinated obstacle avoidance planning through motion planning using a single reinforcement learning. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced one by one below. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

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

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

[0022] Figure 3 It is a structural schematic diagram of the mobile mechanical arm provided by the present invention.

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

[0024] Figure 5 It is a comparative schematic diagram of the value distribution mixture provided by the present invention.

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

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

[0027] Reference numerals: 1: Robotic arm; 2: Joint; 3: Connecting rod; 4: Mobile base; 5: End effector. DETAILED DESCRIPTION

[0028] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0029] The optimization method for whole-body motion planning of a mobile manipulator according to the present invention is described below with reference to the accompanying drawings.

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

[0031] Step 101: Based on the depth image taken by the mobile robotic arm, determine the environmental features and the position gradient when the joints of the mobile robotic arm change.

[0032] The mobile robot needs to perceive the environment during its movement in order to plan the movement speed and avoid obstacles safely. Figure 2 As shown in the figure, in the environmental perception layer, the depth image taken by the mobile robot through the depth camera is first obtained to obtain environmental information. The depth image has two functions. One is to perform point cloud computing and build a local distance map, specifically a local map of the environmental point cloud, which is used to further calculate the position gradient when the joints of the mobile robot change, so as to perform position constraints. The other part is to encode through the convolutional neural network (CNN) encoder to obtain environmental features for speed planning.

[0033] Step 102: Perform strategy planning on the motion state and motion action of the mobile robot arm through the actor-critic network trained by reinforcement learning to obtain the expected speed of the end effector in the mobile robot arm.

[0034] Continue to see Figure 2In the task space speed planning layer, the movement speed of the mobile robot to the target position is planned. The planning process can be designed as a mobile robot end arrival task. Specifically, it can be 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 the joint angle, environmental characteristics, and the position deviation between the end effector and the target position, and the motion action includes the motion speed and rotation speed of the end effector in three directions in the three-dimensional rectangular coordinate system. The Markov decision process needs to plan the motion action under each motion state, so as to determine the expected speed of the end effector in the mobile robot, ensure that the mobile position of the mobile robot reaches the target position as soon as possible, and also need to ensure to avoid collision with obstacles or reach singular positions. Singular positions refer to special positions or nodes that are different from conventional positions or regular positions, which will not be elaborated.

[0035] 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 network (DSAC). Furthermore, the embodiment of the present invention adopts a Bayesian distributed soft actor-critic network (BayesDSAC) for strategy planning. The actor-critic network generally includes an actor network and a comment network, while the BayesDSAC of the embodiment of the present invention includes an actor network and two comment networks. The two comment networks will evaluate the decision of the actor network at the same time and output two value distributions, which are then fused through a Bayesian control function to obtain the corresponding mixed distribution for comprehensive evaluation of the decision of the actor network. The actor-critic network can be trained through reinforcement learning.

[0036] The motion state can be represented based on the robot joint angle and the environmental features obtained by the environmental perception layer. The robot joint angle is used to determine the joint position and implement joint constraints, while the environmental features can be used to help determine the moving position of the mobile robot arm.

[0037] The motion is represented by the speed of the end effector of the mobile robot in its own three-dimensional coordinate system, and the rotation speed relative to the X, Y, and Z axes. After the actor-critic network is trained through reinforcement learning, after inputting the initial motion state of the mobile robot, it can plan the motion state and motion of the mobile robot and obtain the expected speed of the end effector in the mobile robot. , this expected speed also includes the speed of the end effector in the X, Y, and Z axes in its own three-dimensional coordinate system , , , and the rotation speed relative to the X, Y, and Z axes , , , recorded as .

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

[0039] like Figure 2 As shown in the task space speed planning layer, the expected speed of the end effector of the mobile robot is planned by Bayes DSAC After that, it enters the final joint space motion planning layer. The goal of the mobile robot motion planning is to finally determine the joint speed. In the joint space motion planning layer, the main process is to execute the quadratic optimization (Quadratic Programming, QP) process of the signed distance field (Signed DistanceField, SDF) constraint. In this process, the position gradient of the mobile robot arm when the joint changes is first determined according to the signed distance field, and then the position constraint conditions for the mobile robot arm to avoid obstacles are constructed based on the position gradient to constrain the position. In addition, it is necessary to construct joint constraint parameters for constraining decision variables. The decision variable x is the optimization target of the quadratic optimization, including the joint speed and relaxation norm of the mobile robot arm, and the decision variable is the desired speed through the Jacobian matrix of the robot arm. In this way, the process of optimizing the decision variable x is to optimize the expected speed of the end effector of the mobile robot The joint velocity of the mobile robot is the joint velocity to be planned by the joint space motion planning layer, denoted as , the relaxation norm is expressed by the relaxation vector It is used to make the mobile robot better meet the constraints during the movement and improve the control effect.

[0040] According to the above constitution, the position constraint conditions and joint constraint parameters are constructed respectively, thus forming the optimization conditions for the secondary optimization.

[0041] Step 104: The decision variables are optimized 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.

[0042] After the position constraint conditions and joint constraint parameters are constructed in step 103, the quadratic programming solver can be used as optimization conditions to determine the quadratic programming solver. The quadratic programming solver optimizes the decision variable x by executing the QP method to obtain the joint velocity The planning result, the constraints of the quadratic programming solver and its decision variable x can be expressed as follows: (1) (2) (3) (4) The above formula (1) is the solution formula of the quadratic programming solver executing the QP method. represents the constraint weight of the joint constraint parameter, 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 constraints that the decision variable x must satisfy in the quadratic programming solver of formula (1). In the above formula (2), represents the Jacobian matrix of the robot arm, represents the expected speed of the end effector of the mobile robot, x is the decision variable, and formula (3) is the position constraint condition for the mobile robot to avoid obstacles 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 expected speed The lower limit of is usually 0. is the upper limit of the decision variable x, that is, the expected speed The upper limit is determined according to the dynamic performance of the mobile robot arm and represents the maximum limit speed that can be achieved.

[0043] The optimization goal of the quadratic programming solver implementing the QP method is to minimize the joint velocities , while maximizing the maneuverability of the mobile robot. The maneuverability is measured by optimizing the bias C in the above formula (1). The larger the value of C, the better the maneuverability of the mobile robot.

[0044] After optimizing the decision variable x through the QP method, the joint velocity will be obtained The planning results, including the specific joint speeds And the relaxation norm , joint velocity Used as a motion indicator for moving the robot arm, the relaxation norm Used to measure whether the mobile robot meets the motion constraints and whether the mobile robot strictly follows the joint speed during the movement. To perform this, the joint velocities are limited when the mobile robot approaches the target position to ensure that the target position is reached.

[0045] The embodiment of the present invention firstly utilizes environmental perception and strategy planning to improve the effect of motion planning and achieve real-time performance. Furthermore, the desired speed is planned by a quadratic programming solver based on the position constraints, joint speed constraints, and relaxation norm constraints of obstacle avoidance, and the planning result of the joint speed is obtained, thereby ensuring the reliability, rapidity, and safety of the mobile robot control under complex motion trajectories. This solves the problem that the existing motion planning methods have limitations in the ability to perceive the environment and express constraints, and that it is difficult to achieve smooth and coordinated obstacle avoidance planning through motion planning using single reinforcement learning.

[0046] In an embodiment of the present invention, an example structure of a mobile mechanical arm is as follows: Figure 3 As shown, it specifically includes: a robot arm 1, a joint 2, a connecting rod 3, a mobile base 4, and an end effector 5. It can be seen that the robot arm 1 has an end effector 5, but is composed of multiple joints 2 and connecting rods 3, and the joints are connected by connecting rods. Figure 3 Only two to three joints of a robotic arm are shown. In actual implementation, there may be multiple robotic arms installed on the mobile base 4, and the number of joints and connecting rods on each robotic arm may also be multiple, which can be set according to the actual scenario. In the embodiment of the present invention, one robotic arm including 9 joints and 9 connecting rods will be used for illustration.

[0047] The planning of the motion strategy of the mobile robotic arm depends on the environmental perception layer, which is used to constrain its own position. In the specific implementation, a depth camera can be installed on the mobile robotic arm, specifically on a mobile base, and the depth camera can be used to capture a depth image. Then, based on the depth image captured by the mobile robotic arm, the environmental characteristics and the position gradient of the mobile robotic arm when the joints change are determined.

[0048] Specifically, the captured depth images are processed in two parts. The first part overlaps 5 frames of depth images, and then calls the convolutional neural network (CNN) to encode the depth images to obtain environmental features, which are used to construct the motion state of the mobile robotic arm and plan the speed strategy in the task space speed planning layer. It can also be used to support the reinforcement learning process of BayesDSAC.

[0049] The second part is to use an image-to-point cloud algorithm or program software to convert the depth image to obtain a local distance map centered on the mobile robotic arm. This local distance map is the local map of the environmental point cloud. Then, the embodiment of the present invention converts the coordinates of the environmental point cloud in the local map of the environmental point cloud into position coordinates centered on the mobile robotic arm through a signed distance field (SDF). This position coordinate includes but is 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 distance between the link k and the obstacle under the current joint configuration q of the mobile robotic arm can be queried from the position coordinates, which is recorded as .

[0050] Finally, the position gradient of the mobile robot arm joint changes is determined based on the distance. This is also to measure the impact of the change in the mobile robot arm joint on the distance to the obstacle. Here, the mobile robot arm joint position can be fine-tuned first, and the position fine-tuning amount can be recorded. At the same time, the distance to the obstacle before and after the fine-tuning can be recorded. Then, the difference in the distance before and after the fine-tuning can be calculated. Finally, the corresponding position gradient can be obtained by differential calculation based on the difference and the position fine-tuning amount, which can be expressed as , 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 robot to complete the obstacle avoidance task and ensure safety.

[0051] The embodiment of the present invention designs an environmental perception layer when planning the motion of the mobile manipulator, realizes efficient environmental perception, can quickly and accurately obtain information about the surrounding environment, generate reliable environmental features and use them to calculate the position gradient for obstacle avoidance. This efficient perception capability provides a solid foundation for subsequent behavior planning, ensuring that the mobile manipulator can effectively and safely perform dynamic tasks in complex environments.

[0052] In some embodiments, Figure 2 The task space speed planning layer shown in mainly relies on the BayesDSAC strategy planning. In this strategy planning process, the motion state and motion action of the mobile robot are strategically planned through the actor-critic network trained by reinforcement learning to obtain the expected speed of the end effector in the mobile robot. The specific process of strategy planning is described in detail below.

[0053] In the embodiment of the present invention, the strategy planning process of the motion state S and motion action A of the mobile robot can be regarded as a Markov decision process. Specifically, the current motion state of the agent at a certain iteration time step t is , will perform an action with probability , and then enter a new state of motion , can get rewards In return, this probability is called the state transition probability, recorded as The goal of reinforcement learning is to learn a strategy that can perform multiple movements and ensure that the highest reward is obtained after each movement is performed, and finally maximize the expected future cumulative return after all movements are completed.

[0054] In view of this Markov process, the embodiment of the present invention adopts the distributed soft policy iteration (DSPI) framework for training BayesDSAC through reinforcement learning. First, the maximum entropy of the actor-critic network at the iterative time step of reinforcement learning is constructed as the soft state action reward. Here, for each iterative time step t of the actor-critic network at the current state Execution strategy Action Rewards received , enhanced by policy entropy, and then defined in a distributed form under the DSPI framework the accumulated soft-state action reward at the current iteration time step t , the formula is as follows: (5) In the above formula (5), represents the discount factor, represents the entropy coefficient, the value range is (0,1), and t represents the execution strategy time or iteration time step, i represents the execution strategy The time or iteration time step after the action, represents the actor-critic network at each iteration time step t in the current state Execution strategy Action The reward you get when you do it. Indicates that the strategy is not executed at time t The current reward at the time of the action.

[0055] Next, according to the DSPI framework, the soft state action is reported The expected value of is taken as the soft Q value of the motion strategy, denoted as , expressed as: (6) During reinforcement learning training, the soft Q value is learned through the soft Bellman operator in the distributed soft policy iteration framework to train the actor-critic network for strategy planning. The learning process follows the following formula: (7) In the above formula (7), Representation strategy The soft Bellman operator under means that the two variables have the same probability distribution, represents the entropy coefficient, Representation strategy The action to be performed next, t represents the current iteration time step, t+1 represents the next iteration time step, represents the state of the next iteration time step t+1, represents the accumulated soft state action reward 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 reward at the next iteration time step t+1.

[0056] In each time step iteration of the reinforcement training, the actor network in BayesDSAC is used to plan the strategy execution action, and then the evaluation network is used to evaluate the executed action to obtain the corresponding reward value and calculate the accumulated soft state action return. In the DSPI framework, both the actor network and the critic network need to use this accumulated soft state action return, and then use formula (7) to learn the soft Q value, so as to adjust their own parameters. , To update, are the parameters of the actor network, are the parameters of the comment network.

[0057] In the process of performing policy iteration to learn soft Q values, the soft state action return of the target policy is minimized. The Kulbak-Leibler (KL) divergence between the soft state action return Z of the current policy and the soft state action return Z of the current policy is used to update the soft state action return Z of the current policy. It can be regarded as a training label. As the goal of BayesDSAC to learn, it can maximize the expected future cumulative return. The corresponding first distribution is calculated by the soft Bellman operator, and the first distribution will be described below.

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

[0059] As mentioned above, the motion state It includes joint angles, environmental features, and the position deviation between the end effector and the target position. Here, the initial motion state consists of the initial joint angles of the mobile robot. , environmental characteristics and the position deviation between the end effector of the mobile robot and the target position Composition, initial joint angle Environmental characteristics can be measured by instruments. That is, the environmental features calculated by the environmental perception layer are used to construct the initial motion state of the mobile robot, that is, .

[0060] The trained actor-critic network BayesDSAC will plan each step of the movement action for the initial movement state. Each time an action is performed, the movement state will be changed, that is, the joint angle, environmental characteristics, and the position deviation from the target position will be changed, so that the mobile robot can obtain the maximum cumulative soft state action reward, and finally the mobile robot reaches the target position and ends the planning. In this way, a series of movement actions will be obtained as the optimal movement strategy.

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

[0062] In the embodiment of the present invention, in the task space speed planning layer, the motion strategy of the mobile manipulator is reasonably planned through the Bayes DSAC trained by reinforcement learning, wherein the environmental characteristics, joint angles, and position deviations from the target position are planned as motion states during the planning process, so that the mobile manipulator can better integrate into the environmental state during the motion process. The desired speed of the end effector provided in the end lays the foundation for the subsequent determination of the actual joint speed and the generation of continuous, high-quality whole-body control results.

[0063] Furthermore, in the above embodiment, the strategy planning of BayesDSAC is to determine a motion strategy when the accumulated soft-state action reward is the largest, which is consistent with the reward obtained as a reward after executing the action at each time step. Related, this reward value is enhanced to policy entropy. The following describes the process of constructing the maximum entropy of the actor-critic network at the iterative time step of reinforcement learning.

[0064] In this embodiment of the present invention, the reward obtained as a return after executing an action at each iterative time step is , here written as , consisting of multiple parts of rewards.

[0065] First, the position reward value is constructed according to the second-order norm of the position deviation between the end effector of the mobile robot and the target position, denoted as , expressed as: (8) In the above formula (8), represents the position deviation between the end effector of the mobile robot and the target position, Represents the function for calculating the second-order norm. It can be understood that the smaller the position deviation is, the closer to the target position is, and the higher the position reward value is.

[0066] In addition, when the second-order norm of the position deviation is less than the preset position deviation threshold, the moving target reward value is obtained, which is recorded as , expressed as: (9) In the above formula (9), Indicates 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 robot and the target position. The position deviation threshold is used to determine whether the end effector has reached the position it should reach at the iteration 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 has reached the position it should reach and a reward is given. Otherwise, it means that it has not reached the position and no reward is given.

[0067] During the movement of the mobile robot, a corresponding movement speed will be generated. Therefore, the embodiment of the present invention also constructs a speed reward value based on the difference between the movement speed when the position deviation is generated during the movement of the mobile robot and the action speed of the end effector in the mobile robot, which is recorded as Here, the motion speed of the end effector in the mobile robot is the speed at which the action planned by the strategy is executed, denoted as , and the moving speed when position deviation occurs According to the position deviation and joint angles The speed bonus value is obtained by fitting calculation. The calculation formula is as follows: (10) (11) In the above formulas (10) and (11), That is, the moving speed when the position deviation occurs during the movement of the mobile robot. Indicates position deviation, represents the joint angle, Represents the function for velocity fitting calculation based on the end effector position and position deviation. represents the function for calculating the second-order norm, Indicates the speed at which the actions planned by the strategy are executed.

[0068] Here, the position reward and speed reward are generally balanced, because the closer to the target position and the faster the speed are, the more accurate and faster the mobile robot can reach the target position. Therefore, in the embodiment of the present invention, the position reward weight is preset. and speed reward weight , for the position reward value And speed bonus value Weighted to get the weighted position reward value and weighted speed bonus .

[0069] During the movement, the obstacle avoidance of the mobile robot is also taken into consideration, and corresponding rewards need to be given. Here, according to the shortest distance y when the end effector of the mobile robot avoids obstacles and the preset obstacle avoidance distance threshold , construct the obstacle avoidance reward value , expressed as the following formula: (12) Understandably, when , indicating that the mobile robot arm starts to perform obstacle avoidance movement when it has not reached the appropriate obstacle avoidance distance. No reward is given at this time. When , it means that the mobile robot arm has completed the obstacle avoidance within a reasonable obstacle avoidance distance, and the reward value is given according to the shortest distance y when the end effector of the mobile robot arm avoids the obstacle. This means that the mobile robot arm did not avoid obstacles and hit an obstacle, so a penalty value of 25 is given.

[0070] In addition, the embodiment of the present invention is also based on the time spent in the movement process of the mobile robot arm. , the construction time reward value is recorded as , expressed as the following formula: (13) Understandably, the time it takes to move The more there are, the worse the exercise effect is, and the less time reward value is given.

[0071] Finally, the weighted position reward value , moving target reward value , weighted speed bonus value , obstacle avoidance reward value And time bonus value Sum up and get the total reward value , expressed as: (14) 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 As the reward value at the iteration time step t Substituting into the above formula (5), we can get the maximum entropy of the actor-critic network at the iteration time step of reinforcement learning, which is used as the maximum entropy of the strategy at the current iteration time step t. Soft state action return on .

[0072] The embodiment of the present invention, when performing strategy iteration, takes into account multiple aspects such as position, speed, time, obstacle avoidance, moving target, etc., and constructs corresponding reward values ​​one by one as soft state action returns, which can ensure that the planned expected speed can reach the target as soon as possible to avoid collision, thereby improving the motion planning effect.

[0073] The following describes the strategy planning training process of the actor-critic network. This process is actually the process of learning soft Q values ​​in multiple iterative time steps under the DSPI framework. Specifically, in each iterative time step of learning soft Q values, the following operations are performed: On the one hand, when using the actor network in the actor-critic network for policy iteration, the second distribution is updated by minimizing the KL divergence between the first distribution and the second distribution. In the following, Indicates soft state action return distribution.

[0074] The first distribution is the distribution obtained by calculating the second distribution using the soft Bellman operator, that is, the distribution obtained by calculating the distribution of the current soft state action return output by the comment network, that is, The second distribution is the distribution of the current soft state action reward output by the comment network, which is expressed in the above formula (7) During policy iteration, the distribution of the current soft state action return is updated by minimizing the KL divergence of these two distributions, so that the second distribution Closer to , thus completing the learning goal of the current iteration time step. KL divergence is expressed as , the formula is as follows: (15) In the above formula (15), represents the function for calculating KL divergence, represents the parameters of the actor network before parameter update, where represents the parameters of the actor network after parameter update, represents the parameters of the comment network before parameter update, is the current strategy, and B represents the buffer area in the DSPI framework, which is used to store motion actions. area. Represents the execution of a policy in an actor network Movement After that, the distribution of the current soft-state action reward output by the comment network before the parameter update, is the distribution of the current soft state action reward output by the comment network after parameter update, To comment on the soft Bellman operator of the network before parameter update, Indicates based on predict The probability distribution of . Denotes that the current soft state action reward Z obeys the distribution of the current soft state action reward output by the comment network before the actor network updates its parameters .

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

[0076] When the preset iteration time step t or number of iterations is reached, the training is stopped. The trained actor-critic network can plan the motion strategy for the initial motion state of the mobile robot and obtain the optimal motion strategy of the mobile robot.

[0077] The actor-critic network of the embodiment of the present invention helps to achieve the training goal in each policy iteration process by calculating the KL divergence of the distribution of the current soft state action reward and minimizing it, thereby ensuring the training effectiveness of the actor-critic network.

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

[0079] First, in the embodiment of the present invention, the second distribution is modeled as a Gaussian distribution, that is, satisfying , that is, the Gaussian distribution is the mean of the current soft state action return and the variance of the current soft state action return composition.

[0080] When calculating the updated gradient to minimize the KL divergence, the update target is the time difference error of the second distribution. In order to avoid the gradient explosion problem caused by excessive calculation of the square of the time difference error, the embodiment of the present invention uses the edge value restriction function Clip to update the first distribution. After processing, we get the first boundary restricted distribution, denoted as , and the boundary of the boundary value restriction function is based on the mean And the preset constraint boundary b is determined, expressed as: (16) In the above formula (16), Represents the mean of the current soft state action return , which is used to replace , is the boundary restriction function, and b is the preset constraint boundary.

[0081] According to the above formula (6), the embodiment of the present invention reports the soft state action The expected value of is used as the soft Q value of the motion strategy, so the above formula (7) can be rewritten as: (17) In the above formula (17), It means that in formula (7) , For strategy, represents the entropy coefficient, represents the expected value of the soft state action return at iteration time step t+1, Indicates that at the iteration time step t+1, in the strategy The action to be performed is: Indicates that at the iteration time step t+1, in the strategy The state below Indicates that at the iteration time step When commenting on the parameters of the network before updating, Indicates that at the iteration time step is the parameter before the actor network is updated, and the meanings of the other parameters remain unchanged. The parameter explanation can refer to the above formula (7), which will not be repeated here.

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

[0083] Next, the updated gradient is calculated based on the first boundary restriction distribution, the mean, and the variance. According to the above, the first boundary restriction distribution is obtained by processing the first distribution with the boundary restriction function Clip, and the boundary of the boundary restriction function is based on the mean. And the preset constraint boundary b determines the updated gradient of KL divergence , which can be expressed as: (18) In the above formula (18), To restrict the first distribution by the boundary value function Clip Processing is performed to obtain the first boundary restricted distribution, Represents the mean of the current soft state action return , Represents the variance of the current soft state action reward. express The differential of express The differential of .

[0084] In an embodiment of the present invention, the KL divergence is minimized by calculating the updated gradient of the KL divergence, and the distribution of the current soft state action reward is modeled as a Gaussian distribution. At the same time, the boundary of the boundary value constraint function is determined according to the mean in the Gaussian distribution and the preset constraint boundary. Then, the boundary constraint is performed on the first distribution through the boundary value constraint function to prevent the problem of updated gradient explosion when minimizing the KL divergence, effectively improve the stability of reinforcement learning, and ensure the evaluation accuracy of the soft state action reward output by the comment network.

[0085] In the process of actor-critic network training, the existing technical methods still have the problem of underestimation, that is, there may be a problem of excessive underestimation of the soft state action reward by the critic network, which will make the convergence process of strategy optimization in the reinforcement learning process slower and difficult to maximize the cumulative reward. Based on the above scenario, Figure 4 As shown, in an embodiment of the present invention, the actor-critic network uses BayesDSAC, which specifically includes two comment networks, and the current distribution of soft-state action returns is obtained by calling the Bayesian control function to perform a mixed estimation on the distribution of soft-state action returns output by the two comment networks when performing strategy evaluation.

[0086] like Figure 4As shown in the figure, during the policy iteration, the critic network 1 and the critic network 2 evaluate the actions executed by the actor network in the old policy, and output the distribution of the current soft state action return, which are recorded as , as well as , When calculating the update gradient, a hybrid estimation is performed through the Bayesian control function (BCF) to obtain the corresponding hybrid soft-state action reward distribution.

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

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

[0089] In the embodiment of the present invention, the independent distribution output by the two review networks is as well as To calculate the updated gradient, the two independent distributions are fused through the Bayesian control function, the formula is as follows: (19) In the above formula (19), represents the assumption of uniform prior, represents the prior conditional probability distribution, Represents the probability distribution corresponding to two independent distributions. is the calculated posterior conditional probability distribution, represents the parameters of one of the review networks of BayesDSAC, Represents the parameters of another critic network in BayesDSAC.

[0090] Next, we use likelihood estimation to expand the Bayesian formula of the above formula (19), which is expressed as: (20) In the above formula (20), represents the prior conditional probability distribution corresponding to one of the comment networks, represents the prior conditional probability distribution corresponding to another review network, represents the probability distribution of an independent analysis of one of the review network outputs, Represents the probability distribution of another independent analysis of the review network output.

[0091] In the above manner, the embodiment of the present invention calls the Bayesian control function to respectively calculate the mean of the soft state action reward distribution and variance By performing likelihood estimation, we can obtain the mixed mean and mixed variance, thereby integrating the approximate value estimates of the two independent distributions. The calculation process of the mixed mean is as follows: (twenty one) The calculation process of mixed variance is as follows: (twenty two) In the above formulas (21) and (22), and denote the mean and variance of the soft-state action reward distribution of one of the review networks, respectively. and denote the mean and variance of the soft-state action reward distribution of another review network, respectively.

[0092] Finally, the mixed mean and mixed variance of the two review networks are fused to obtain the mixed soft state action return distribution as the distribution of the current soft state action return, where the corresponding mixed Gaussian distribution is denoted as .

[0093] Therefore, the above formula (17) can be further rewritten as: (twenty three) In the above formula (23), That is, The corresponding mixed soft-state action reward, is the value in the above formula (17) The expected value of the corresponding mixed soft-state action reward. The other parameters have the same meaning as the above formula (17). The parameter explanation can refer to formula (17) and will not be repeated here.

[0094] The first distribution when the above formula (18) calculates the updated gradient of KL divergence is It can be rewritten as , expressed as: (twenty four) In the above formula (24), Represents the parameters of the comment network Current soft state action report before update Obeying mixed Gaussian distribution The explanation of other parameters can refer to formula (18) and will not be repeated here.

[0095] Therefore, the above formula (18) can be rewritten as: (25) In the above formula (25), represents the parameters of the i-th review network in BayesDSAC, That is, , That is, , represents the mean of the current soft-state action returns of the i-th comment network, represents the variance of the current soft-state action return of the i-th comment network, Compared with the above formula (24) The same meaning, Compared with the above formula (18) have the same meaning, except that is the updated gradient calculated for the i comment networks. The explanations of the remaining parameters can refer to the above formulas (18) and (24) and will not be repeated here.

[0096] like Figure 4 As shown in Figure 1, by performing a mixed estimation on the distribution of the soft state action returns output by the two review networks, the corresponding mixed Gaussian distribution is obtained, and then the first distribution is calculated based on the mixed Gaussian distribution. and the second distribution The KL divergence of To minimize the KL divergence, in the process update the parameters of the two review networks , After the parameter update is completed, the comment network continues to make comments and outputs the distribution of the corresponding soft state action rewards, which are recorded as , as well as , Next, we use the above formula (20) and formula (21) to calculate the mixed mean and mixed variance , continue to calculate the updated gradient at the next iterative time step.

[0097] For actor networks, such as Figure 4 As shown, at the time step t of the policy iteration, the corresponding action will be executed according to the old policy, and the old policy is recorded as , the comment network evaluates and outputs the distribution of the current soft state action return , and then use the above formula (15) to calculate the KL divergence, and then calculate the updated gradient To minimize the KL divergence. In the process of minimizing the KL divergence, the actor network will adjust the old strategy to form a new strategy. Then the corresponding action is performed again for the new strategy, and the new strategy is recorded as .

[0098] After the comment network updates its parameters, it continues to evaluate the actions of the actor network executing the new strategy and outputs the distribution of the corresponding soft state action rewards. , , , , and then perform mixed estimation to obtain the mixed mean , which is used to evaluate the actions of the actor network in executing the new strategy. The actor network then calculates the mixed mean Continue to update the strategy action, the formula is as follows: (26) In the above formula (26), represents the reward value obtained by the actor network executing the strategy update, It represents the mixed mean output when the comment network updates its parameters and evaluates the actions of the actor network after executing the new strategy. represents the entropy coefficient, Represents the actor network in the strategy The action to be performed.

[0099] Finally, according to Update the parameters of the actor network , thus the parameter update process of the actor network at the current iteration time step t is introduced.

[0100] In the embodiment of the present invention, Bayes DSAC is used as the actor-critic network, and two critic networks are designed to evaluate the actor network during reinforcement learning training. The Bayesian control function is used to mix the value distribution, which improves the estimation quality of the value function and avoids the underestimation of the value caused by the minimum value method used in the prior art, thereby improving the strategy convergence speed. This combination makes the mobile robot arm more powerful in learning complex tasks.

[0101] In some embodiments, the motion planning of the mobile manipulator relies on the QP method of the quadratic programming solver, such as the above formula (1). Before executing this method, various constraints need to be constructed, one of which is the position constraint for the mobile manipulator to avoid obstacles based on the position gradient in the above formula (3). The specific construction process is described below.

[0102] The position constraint condition consists of two parts, namely obstacle avoidance parameters and velocity damper parameters. The obstacle avoidance parameters are used to force the robot's joints to stay away from the collision boundary. In the environmental perception layer, the embodiment of the present invention queries the distance between the link k and the obstacle under the current joint configuration q of the mobile robot through SDF, which is recorded as , and then calculate the position gradient , 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. The position gradient is used here to calculate the first obstacle avoidance parameter.

[0103] On the one hand, the first obstacle avoidance parameter is constructed according to the position gradient of each link when the mobile robot joint changes. , the formula is as follows: (27) In the above formula (27), It represents the position gradient of the distance between the link 9 of the 9th joint and the obstacle under the current joint configuration q. The explanation of other parameters can be referred to , no further details. Represents the dimension of the matrix. R in the following formulas represents the dimension of the matrix, which will not be described one by one later.

[0104] On the other hand, according to the distance between each link and the obstacle when the mobile robot joint changes , distance to connecting rod and minimum distance to obstacles The difference between , the calculation formula is as follows: (28) In the above formula (28), Indicates the distance between the link 9 and the obstacle in the current joint configuration q of the mobile robot. For the explanation of other parameters, refer to , without further elaboration, Indicates the minimum distance between the connecting rod and the obstacle, which is preset based on historical experience.

[0105] The velocity damper parameters are mainly used to limit the joint position, specifically to reduce the joint velocity of the mobile robot so that it slows down when approaching a singular position.

[0106] Here, the first velocity damper parameter and the second velocity damper parameter for limiting the position of the mobile mechanical arm joint are determined. is a preset fixed parameter, expressed as the following formula: (29) The second speed damper parameters It is based on the influence distance between the mobile robot joint and the singular position and minimum distance The calculated influence distance represents the influence range of the singular position on the mobile robot. Within this influence range, the mobile robot needs to control the joint speed. The minimum distance is preset and refers to the limit distance that the mobile robot can approach the singular position. If it is less than this minimum distance, it indicates that the mobile robot is already in the singular position. The second speed damper parameter The calculation formula is as follows: (30) In the above formula (30), represents the minimum distance between the mobile robot joint and the singular position, represents the influence distance between the mobile robot joint and the singular position, i represents the i-th joint, Represents the preset weight parameter.

[0107] Finally, the first velocity damper parameter and the first obstacle avoidance parameters Fusion is performed to obtain the first constraint parameter , the second speed damper parameter and the second obstacle avoidance parameter Fusion is performed to obtain the second constraint parameter The specific fusion method is to physically splice the matrices, which can be expressed as: (31) (32) Finally, the second constraint parameter With the first constraint parameter The ratio of is used as the position constraint, which is the above formula (3).

[0108] 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 obstacle avoidance parameters and speed damper parameters. The obstacle avoidance parameters can enable the mobile manipulator to have good obstacle avoidance performance and improve the movement safety. At the same time, the speed damper parameters can enable the mobile manipulator to reasonably control the speed of the joint when facing a singular position, ensuring that the joint avoids reaching the singular position.

[0109] Furthermore, in the quadratic programming solver, the joint constraint parameters include constraint weights and optimization biases, which are respectively and C. The following describes the specific process of constructing the joint constraint parameters used to constrain the decision variable x.

[0110] For constraint weights In this embodiment of the present invention, the value of the position deviation between the end effector of the mobile robot and the target position is As the third constraint parameter constraining the joint velocity , and the value of the position deviation The reciprocal of is used as the fourth constraint parameter for constraining joint velocity , and then combine the third constraint parameter and the fourth constraint parameter into the target weight for constraining the joint velocity , which can be specifically written as , the combination here is to directly splice the matrices.

[0111] Get the relaxation norm used to adjust The relaxation weight , this relaxation weight is preset, the relaxation norm It is used to measure whether the mobile robot meets the motion constraints, while the relaxation weight is used to measure the degree and effect of meeting the motion constraints. The larger it is, the farther the mobile robot arm is from the target position. At this time, the mobile robot arm can be limitedly free from the motion constraints of the joint speed, and errors are allowed.

[0112] Here, the constraint weight is constructed based on the relaxation weight and the target weight. , the constraint weight is used to control the joint velocity when the mobile robot moves, and adjust the relaxation vector at the same time. The construction process can be expressed as the following formula: (33) In the above formula (33), is the target weight used to constrain the joint velocity , Represents the relaxation norm used to adjust The diag function represents the function of constructing a diagonal matrix.

[0113] The optimized offset C is a value that measures the operating performance of the mobile robot. Here, the angle between the mobile robot and the mobile base is determined first. , and determine the angle The adjustment gain value is determined by the preset adjustment gain weight. To calculate, then according to the adjustment gain value and the manipulability Jacobian matrix of the mobile robot, the optimized bias C is constructed. The optimized bias C is used to make the angle when the mobile robot moves. Minimize, expressed as the following formula: (34) In the above formula (34), represents the manipulability Jacobian matrix of the preset mobile robot, represents the angle between the mobile robot and the mobile base, Adjust gain weights for presets.

[0114] The constraint weights are determined by the above formulas (33) and (34) respectively. After optimizing the bias C, the quadratic programming solver as shown in the above formula (1) can be constructed as the joint constraint parameters to perform the optimization of the decision variable x.

[0115] At the same time, in order to improve the elegance of the mobile robot movement, the embodiment of the present invention uses the robot Jacobian matrix To construct the decision variable x, that is, the above formula (2), but the Jacobian matrix of this robot arm It is obtained by expanding the identity matrix and is expressed as the following formula: (35) In the above formula (35), represents the Jacobian matrix of the robot arm before expansion, Represents the identity matrix of 6×6 dimensions.

[0116] In an embodiment of the present invention, when constructing a quadratic programming solver, joint constraint parameters of a relaxation vector for constraining joint velocity are constructed, so that the movement of the mobile base and the robotic arm can be better moved. At the same time, a method is also constructed for minimizing the angle between the mobile robotic arm and the mobile base when the mobile robotic arm moves, and the direction of the end effector is constrained on the mobile base, thereby enhancing the flexibility of the mobile robotic arm and improving the operability of the mobile robotic arm.

[0117] The optimization device for whole-body motion planning of a mobile robotic arm provided by the present invention is described below. The optimization device for whole-body motion planning of a mobile robotic arm described below and the optimization method for whole-body motion planning of a mobile robotic arm described above can be referenced to each other.

[0118] Figure 6 is a schematic diagram of the structure of the optimization device for whole-body motion planning of a mobile manipulator provided by the present invention, such as Figure 6As shown, the optimization device for whole-body motion planning of a mobile robotic arm 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 environment characteristics and the position gradient of the mobile robot arm when the joint changes based on the depth image taken by the mobile robot arm; the speed planning module 602 is used to perform strategic planning on the motion state and motion action of the mobile robot arm through the actor-critic network trained by reinforcement learning, and obtain the expected speed of the end effector in the mobile robot arm, the motion state includes the joint angle, the environment characteristics and the position deviation between the end effector and the target position, and the motion action includes the motion speed and rotation speed of the end effector in three directions in the three-dimensional rectangular coordinate system; the motion planning module 603 is used to construct the position constraint conditions for the mobile robot arm to avoid obstacles based on the position gradient, and construct the joint constraint parameters for constraining the decision variables, the decision variables include the joint speed and relaxation norm of the mobile robot arm, and the decision variables are obtained by weighting the expected speed by the Jacobian matrix of the robot arm; the motion planning module 603 is also used to optimize the decision variables based on the quadratic planning solver determined by the position constraint conditions and the joint constraint parameters to obtain the planning result of the joint speed.

[0119] It should be noted that the beneficial effects of the optimization device for whole-body motion planning of a mobile robotic arm here and the optimization method for whole-body motion planning of a mobile robotic arm mentioned above can correspond to each other, so the beneficial effects of the optimization device for whole-body motion planning of a mobile robotic arm will not be repeated here.

[0120] Figure 7 An example of a physical structure diagram of an electronic device is shown in FIG. Figure 7As shown, the electronic device may include: a processor (processor) 710 , a communication interface (Communications Interface) 720 , a memory (memory) 730 and a communication bus 740 , wherein the processor 710 , the communication interface 720 , and the memory 730 communicate with each other through the communication bus 740 . The processor 710 can call the logic instructions in the memory 730 to execute an optimization method for whole-body motion planning of a mobile robotic arm, the method comprising: determining environmental features and position gradients when the joints of the mobile robotic arm change based on a depth image taken by the mobile robotic arm; performing strategy planning on the motion state and motion action of the mobile robotic arm through an actor-critic network trained by reinforcement learning to obtain an expected speed of the end effector in the mobile robotic arm, the motion state including the joint angle, the environmental features, and the position deviation between the end effector and the target position, the motion action including the motion speed and rotation speed of the end effector in three directions in a three-dimensional rectangular coordinate system; constructing position constraints for obstacle avoidance of the mobile robotic arm based on the position gradient, and constructing joint constraint parameters for constraining decision variables, the decision variables including the joint speed and relaxation norm of the mobile robotic arm, the decision variables being obtained by weighting the expected speed through the Jacobian matrix of the robotic arm; optimizing the decision variables based on the quadratic programming solver determined by the position constraints and the joint constraint parameters to obtain the planning result of the joint speed.

[0121] In addition, the logic instructions in the above-mentioned memory 730 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on this understanding, the technical solution of the present invention can be essentially or partly embodied in the form of a software product that contributes to the prior art. The computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc. Various media that can store program codes.

[0122] 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 whole-body motion planning of a mobile robotic arm provided by the above methods. The method includes: based on the depth image taken by the mobile robotic arm, determining the environmental characteristics and the position gradient when the joints of the mobile robotic arm change; performing strategic planning on the motion state and motion action of the mobile robotic arm through an actor-critic network trained by reinforcement learning to obtain the expected speed of the end effector in the mobile robotic arm, and the motion state includes the joint angle. The method comprises the steps of: constructing a position constraint condition for a mobile robot to avoid obstacles based on the position gradient, constructing joint constraint parameters for constraining decision variables, and constructing joint constraint parameters for constraining decision variables. The decision variables include the joint velocity and relaxation norm of the mobile robot. The decision variables are obtained by weighting the desired velocity through the Jacobian matrix of the robot. The decision variables are optimized by a quadratic programming solver determined based on the position constraint condition and the joint constraint parameters to obtain a planning result of the joint velocity.

[0123] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which is implemented when the processor executes the optimization method for the whole-body motion planning of a mobile manipulator provided by the above-mentioned methods, the method comprising: determining the environmental characteristics and the position gradient of the mobile manipulator joint when the joint changes based on the depth image taken by the mobile manipulator; performing strategy planning on the motion state and motion action 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 including the joint angle, the environmental characteristics and the position deviation between the end effector and the target position, the motion action including the motion speed and rotation speed of the end effector in three directions in a three-dimensional rectangular coordinate system; constructing a position constraint condition for obstacle avoidance of the mobile manipulator based on the position gradient, and constructing a joint constraint parameter for constraining the decision variable, the decision variable including the joint speed and relaxation norm of the mobile manipulator, the decision variable being obtained by weighting the expected speed by the Jacobian matrix of the manipulator; optimizing the decision variable based on the quadratic programming solver determined by the position constraint condition and the joint constraint parameter to obtain the planning result of the joint speed.

[0124] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

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

[0126] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions 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 robot, characterized in that: include: Based on the depth image captured by the mobile robot arm, determine the environmental features and the position gradient when the joints of the mobile robot arm change; The actor-critic network trained by reinforcement learning performs strategic planning on the motion state and motion action of the mobile robot arm to obtain the expected speed of the end effector in the mobile robot arm, wherein the motion state includes the joint angle, the environmental characteristics, and the position deviation between the end effector and the target position, and the motion action includes the motion speed and rotation speed of the end effector in three directions in a three-dimensional rectangular coordinate system; Constructing a position constraint condition for the mobile manipulator to avoid obstacles based on the position gradient, and constructing joint constraint parameters for constraining decision variables, wherein the decision variables include a joint velocity and a relaxation norm of the mobile manipulator, and the decision variables are obtained by weighting the desired velocity through the manipulator Jacobian matrix; The decision variables are optimized 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.

2. The optimization method for whole-body motion planning of a mobile manipulator according to claim 1, characterized in that: The method of determining the environmental features and the position gradient of the mobile mechanical arm when the joints of the mobile mechanical arm change based on the depth image taken by the mobile mechanical arm includes: Calling a convolutional neural network to perform image encoding on the depth image to obtain environmental features; The depth image is converted to obtain a local point cloud map of the environment centered on the mobile robotic arm; The coordinates of the environment point cloud in the local map of the environment point cloud are converted into position coordinates centered on the mobile mechanical arm through a signed distance field, and the distance between the connecting rod and the obstacle under the current joint configuration of the mobile mechanical arm is queried from the position coordinates; The position gradient of the mobile robot arm when the joint changes is determined based on the separation distance.

3. The optimization method for whole-body motion planning of a mobile manipulator according to claim 1, characterized in that: The actor-critic network trained by reinforcement learning performs strategic planning on the motion state and motion action of the mobile robot arm to obtain the expected speed of the end effector in the mobile robot arm, including: Construct the maximum entropy of the actor-critic network at the iterative time step of reinforcement learning as the soft-state action return; Using the expected value of the soft state action reward as the soft Q value of the motion strategy, and learning the soft Q value through a soft Bellman operator in a distributed soft policy iteration framework to perform strategy planning training on the actor-critic network; The initial motion state of the mobile robot is input into the trained actor-critic network for motion strategy planning to obtain the optimal motion strategy of the mobile robot. The desired speed of the end effector in the mobile robot arm is determined based on the optimal motion strategy.

4. The optimization method for whole-body motion planning of a mobile manipulator according to claim 3, characterized in that: The method of constructing the maximum entropy of the actor-critic network at the iterative time step of reinforcement learning includes: Constructing a position reward value according to a second-order norm of a position deviation between an end effector of the mobile robot arm and a 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; Constructing a speed reward value based on the difference between the moving speed of the mobile robot arm when the position deviation is generated during the movement of the mobile robot arm and the action speed of the end effector in the mobile robot arm; The position reward value and the speed reward value are weighted by a preset position reward weight and a preset speed reward weight to obtain a weighted position reward value and a weighted speed reward value; According to the shortest distance when the end effector of the mobile robot avoids obstacles and the preset obstacle avoidance distance threshold, an obstacle avoidance reward value is constructed; Construct a time reward value based on the time spent on the mobile robot movement process; The weighted position reward value, the weighted speed reward value, the obstacle avoidance reward value and the time reward value are summed to obtain a total reward value; The policy entropy is calculated based on the total reward value as the maximum entropy of the actor-critic network at the iterative time steps of reinforcement learning.

5. The optimization method for whole-body motion planning of a mobile manipulator according to claim 3, characterized in that: The strategy planning training process of the actor-critic network includes: During each iterative time step of learning the soft Q value, the following operations are performed: 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, wherein the first distribution is a distribution obtained by calculating the second distribution by a soft Bellman operator, and the second distribution is a distribution of current soft state action returns output by the critic network in the actor-critic network; When the critic network is used for policy evaluation, the parameters of the actor network and the parameters of the critic network are updated by minimizing the KL divergence.

6. The optimization method for whole-body motion planning of a mobile manipulator according to claim 5, characterized in that: The updating of the parameters of the actor network and the comment 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 time 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 consisting of a mean of the current soft-state action reward and a variance of the current soft-state action reward; Calculate an update gradient based on the first bound limit distribution, the mean, and the variance; The first boundary restriction distribution is obtained by processing the first distribution through a boundary value restriction function, and the boundary of the boundary value restriction function is determined according to the mean and a preset constraint boundary.

7. The optimization method for whole-body motion planning of a mobile manipulator according to claim 6, characterized in that: The actor-critic network includes two critic networks, and the distribution of the current soft state action return is obtained by calling the Bayesian control function to perform mixed estimation on the distribution of the soft state action returns output by the two critic networks when performing strategy evaluation. The process of the Bayesian control function performing mixed estimation includes: Call the Bayesian control function to estimate the likelihood of the mean and variance in the distribution of the soft state action return, and obtain the mixed mean and mixed variance; The mixed mean and mixed variance of the two review networks are fused to obtain the mixed soft-state action reward distribution as the distribution of the current soft-state action reward.

8. The optimization method for whole-body motion planning of a mobile manipulator according to claim 1, characterized in that: The constructing of a position constraint condition for the mobile manipulator to avoid obstacles based on the position gradient includes: Constructing the first obstacle avoidance shock absorber parameters according to the position gradient of each link when the joint of the mobile robot arm changes; Constructing a second obstacle avoidance shock absorber parameter according to the distance between each link and the obstacle when the joint of the mobile robot arm changes, and the difference between the distance between the link and the minimum distance of the obstacle; Determining a first velocity damper parameter and a second velocity damper parameter for limiting the position of the joint of the mobile mechanical arm, wherein the first velocity damper parameter is a preset fixed parameter, and the second velocity damper parameter is calculated according to an influence distance and a minimum distance between the joint of the mobile mechanical arm and the singular position; The first speed damper parameter is merged with the first obstacle avoidance damper parameter to obtain a first constraint parameter, and the second speed damper parameter is merged with the second obstacle avoidance damper parameter to obtain a second constraint parameter; The ratio of the second constraint parameter to the first constraint parameter is used as a position constraint condition.

9. The optimization method for whole-body motion planning of a mobile manipulator according to claim 1, characterized in that: The joint constraint parameters include constraint weights and optimization biases, and the construction of joint constraint parameters for constraining decision variables includes: The value of the position deviation between the end effector of the mobile robot arm 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; combining the third constraint parameter and the fourth constraint parameter into a target weight for constraining a joint velocity; Acquire a relaxation weight for adjusting the relaxation norm, and construct a constraint weight based on the relaxation weight and the target weight; determining an angle between the mobile mechanical arm and the mobile base, and determining an adjustment gain value of the angle; An optimized bias is constructed according to the adjustment gain value and the maneuverability Jacobian matrix of the mobile robot arm, and the optimized bias is used to minimize the angle when the mobile robot arm moves.

10. An optimization device for whole-body motion planning of a mobile robot, characterized in that: include: An environmental perception module, which is used to determine environmental features and position gradients when the joints of the mobile robotic arm change based on the depth image taken by the mobile robotic arm; A speed planning module is used to perform strategic planning on the motion state and motion action of the mobile robot arm through an actor-critic network trained by reinforcement learning, so as to obtain a desired speed of an end effector in the mobile robot arm, wherein the motion state includes a joint angle, the environmental characteristics, and a position deviation between the end effector and a target position, and the motion action includes the motion speed and rotation speed of the end effector in three directions in a three-dimensional rectangular coordinate system; A motion planning module, for constructing position constraints for the mobile manipulator to avoid obstacles based on the position gradient, and constructing joint constraint parameters for constraining decision variables, wherein the decision variables include joint velocities and relaxation norms of the mobile manipulator, and the decision variables are obtained by weighting the desired velocity through the manipulator Jacobian matrix; The motion planning module is also used to optimize the decision variables based on the position constraint conditions and the quadratic planning solver determined by the joint constraint parameters to obtain the planning result of the joint speed.

Images