Robot motion trajectory planning and control method and device based on multi-channel fusion, storage medium and program product

Abstract

Disclosed are a robot motion trajectory planning and control method and device based on multi-path fusion, a storage medium and a program product, the method comprising: determining emotional information according to the current joint angle and current joint angular velocity of the robot's mechanical arm, the current position and current velocity of the obstacle, and collision information, wherein the emotional information represents the degree of stress or fear caused by the obstacle approaching and colliding with the robot; based on the emotional information, selecting a fast path or a slow path to obtain the robot's motion information, wherein the fast path simulates the function of the striatum to control the robot to achieve rapid emergency obstacle avoidance, and the slow path simulates the function of the prefrontal lobe to achieve motion trajectory planning of the robot to a target position; controlling the robot's mechanical arm to move for a first predetermined time period according to the motion information; determining whether the robot's mechanical arm has reached the target position; and returning to the step of determining the emotional information in response to the robot's mechanical arm not reaching the target position.

Description

Technical Field

[0001] The present disclosure generally relates to the field of robotics, and more specifically, to a robot motion trajectory planning and control method and device based on multi-path fusion, a storage medium, and a program product. Background Art

[0002] At present, robots are being used more and more widely in all aspects of human production and life. The improvement of high-level human-machine collaboration enables robots to share the same workspace with humans and carry out safe and coordinated activities in industrial and indoor environments. This requires robots to have high adaptability to tasks and environments, to be able to operate adaptively according to tasks, and to flexibly adjust movements in complex and dynamic environments to ensure that they avoid collisions with other objects. The effective improvement of robots' adaptability in complex environments is inseparable from the support of collision detection based on multimodal sensors, the redundancy of the robot's degree of freedom, and other design features. On the other hand, it also requires the help of robot motion decision algorithms. This requires robots to carry out effective motion planning in complex environments, dynamically adjust their motion decisions and actions according to environmental changes, and deal with unpredictable obstacles such as human hands and moving parts that invade and threaten their workspace. Improving the collision avoidance ability of motion decision algorithms to support robots to quickly adapt and avoid collisions in complex unstructured environments containing static and dynamic obstacles, and make flexible motion decisions to carry out safe movements, operations, and human-machine interactions is a huge challenge. Summary of the invention

[0003] The embodiments of the present disclosure provide a robot motion trajectory planning and control method and device based on multi-path fusion, a storage medium and a program product, which can effectively solve the problem that robots in the prior art cannot adapt well to dynamic environments.

[0004] In a general aspect, a robot motion trajectory planning and control method based on multi-path fusion is provided, comprising: determining emotional information based on current joint angles and current joint angular velocities of the robot's mechanical arm, the current position and current velocity of an obstacle, and collision information, wherein the emotional information represents the degree of stress or fear caused by the obstacle approaching and colliding with the robot; based on the emotional information, selecting a fast pathway or a slow pathway to obtain the robot's motion information, wherein the fast pathway simulates the function of the striatum to control the robot to achieve rapid emergency obstacle avoidance, and the slow pathway simulates the function of the prefrontal lobe to achieve motion trajectory planning of the robot to a target position; controlling the robot's mechanical arm to move for a first predetermined time period according to the motion information; determining whether the robot's mechanical arm has reached the target position; and in response to the robot's mechanical arm not reaching the target position, returning to the step of determining the emotional information.

[0005] Optionally, emotion information is determined based on the current joint angle and current joint angular velocity of the robot's robotic arm, the current position and current velocity of the obstacle, and collision information, including: inputting the current joint angle and current joint angular velocity of the robot's robotic arm, the current position and current velocity of the obstacle, and collision information into an emotion model to obtain emotion information, wherein the emotion model is trained based on a predetermined loss function, and the predetermined loss function is determined based on the output of the emotion model, the distance from the robot to the obstacle, and the collision information.

[0006] Optionally, based on the emotional information, a fast path or a slow path is selected to obtain the robot's motion information, including: when the emotional information is less than a first threshold, the slow path is selected to obtain the robot's motion information; when the emotional information is greater than or equal to the first threshold, the fast path is selected to obtain the robot's motion information.

[0007] Optionally, a fast path is selected to obtain the motion information of the robot, including: inputting the current joint angle, the current joint angular velocity, the target position, the minimum distance between the robot and the obstacle, and the two endpoints corresponding to the minimum distance into the fast path model to obtain the motion information of the robot, wherein one of the two endpoints is on the robot and the other of the two endpoints is on the obstacle, and the fast path model is trained by the following reward function:

[0008]

[0009] Among them, r s represents the cumulative reward value without collision in each decision cycle, r g Represents the distance between the end of the robotic arm and the target position, r a Indicates the robot's motion range, r o Depending on the minimum distance between the robot and the obstacle and the collision information, r p A penalty term that represents the robot's actions that exceed the robot's constraints.

[0010] Optionally, a slow path is selected to obtain the motion information of the robot, including: performing the following processing for each node in the first path: when the distance between the current node and the first beacon exceeds a first predetermined step length, extending the second predetermined step length in the direction of the first beacon from the current node as the starting point to obtain the first node, and inserting the first node into the first path, wherein the first path is the path for the robot to reach the target position, the first beacon is the first original node passed from the current node along the first path, and the second predetermined step length is smaller than the first predetermined step length; determining the first path after the node is inserted as the second path; obtaining an augmented path based on the second path and the current position of the robot; obtaining the motion trajectory of the robot based on the spline trajectory corresponding to the augmented path; and determining the motion information of the robot based on the motion trajectory.

[0011] Optionally, the first path is obtained in the following manner: a sampling point is obtained using a sampling strategy determined based on emotion information; a second node on the search tree of the joint space that is closest to the sampling point is obtained, wherein the joint space is the range space of the robot's joint angles, and the joint space includes the obstacle space; starting from the second node, a third predetermined step is extended in the direction of the sampling point to obtain a third node; when the third node does not fall in the obstacle space, at least one fourth node on the search tree is obtained that is within a first preset distance from the third node; a node with the shortest path length with the third node among at least one fourth node is obtained as the parent node of the third node; based on the parent node, the third node is inserted into the search tree; for each first predetermined node in the search tree that is within a second preset distance from the third node, when the first predetermined node regards the third node as a parent node When the path length of the first predetermined node is less than the first original path length of the first predetermined node, the third node is used as the parent node of the first predetermined node, wherein the first original path length is the path length from the first predetermined node to the root node through the original parent node; for each second predetermined node within the third preset distance from the root node in the search tree, when the corresponding path length when the second predetermined node regards the root node as the parent node is less than the second original path length of the second predetermined node, the root node is used as the parent node of the second predetermined node, wherein the second original path length is the path length from the second predetermined node to the root node through the original parent node; when the time from the first acquisition of the sampling point to the current time does not reach the second predetermined time length, return to the step of acquiring the sampling point; based on the adjusted search tree, obtain the first path for the robot to reach the target position within the second predetermined time length.

[0012] Optionally, sampling is performed based on a sampling strategy determined based on emotional information to obtain sampling points, including: obtaining sampling points with a first probability in the following manner: obtaining the output of a strategy function in a fast path model, and determining the output as a sampling point, wherein the first probability is determined based on the emotional information; or obtaining sampling points with a predetermined probability in the following manner: in response to the predetermined probability being a preset second probability, taking the target position as a sampling point; in response to the predetermined probability being a preset third probability, determining a hyperellipsoid with the robot's current position and target position as foci, uniformly sampling in the union of the hyperellipsoid and the joint space of the obstacle removal space to obtain sampling points; in response to the predetermined probability being a preset fourth probability, uniformly sampling in the joint space of the obstacle removal space to obtain sampling points; wherein the sum of the second probability, the third probability and the fourth probability is 1.

[0013] In another general aspect, a robot motion trajectory planning and control device based on multi-path fusion is provided, comprising: a determination unit, configured to determine emotional information based on a current joint angle and a current joint angular velocity of a robot's mechanical arm, a current position and a current velocity of an obstacle, and collision information, wherein the emotional information represents the degree of stress or fear caused by the obstacle approaching and colliding with the robot; a selection unit, configured to select a fast path or a slow path based on the emotional information to obtain the robot's motion information, wherein the fast path simulates the function of the striatum to control the robot to achieve rapid emergency obstacle avoidance, and the slow path simulates the function of the prefrontal lobe to achieve motion trajectory planning of the robot to a target position; a control unit, configured to control the robot's mechanical arm to move for a first predetermined time period according to the motion information; a judgment unit, configured to determine whether the robot's mechanical arm has reached the target position; and a response unit, configured to return to the step of determining the emotional information in response to the robot's mechanical arm not reaching the target position.

[0014] Optionally, the determination unit is also configured to input the current joint angle and current joint angular velocity of the robot's robotic arm, the current position and current velocity of the obstacle, and collision information into the emotion model to obtain emotion information, wherein the emotion model is trained based on a predetermined loss function, and the predetermined loss function is determined based on the output of the emotion model, the distance from the robot to the obstacle, and the collision information.

[0015] Optionally, the selection unit is further configured to select a slow path to obtain the robot's motion information when the emotion information is less than a first threshold; and select a fast path to obtain the robot's motion information when the emotion information is greater than or equal to the first threshold.

[0016] Optionally, the selection unit is further configured to input the current joint angle, the current joint angular velocity, the target position, the minimum distance between the robot and the obstacle, and the two endpoints corresponding to the minimum distance into the fast path model to obtain the action information of the robot, wherein one of the two endpoints is on the robot and the other of the two endpoints is on the obstacle, and the fast path model is trained by the following reward function:

[0017]

[0018] Among them, r s represents the cumulative reward value without collision in each decision cycle, r g Represents the distance between the end of the robotic arm and the target position, r a Indicates the robot's motion range, r o Depending on the minimum distance between the robot and the obstacle and the collision information, r p A penalty term that represents the robot's actions that exceed the robot's constraints.

[0019] Optionally, the selection unit is also configured to perform the following processing for each node in the first path: when the distance between the current node and the first beacon exceeds a first predetermined step length, extending the second predetermined step length in the direction of the first beacon from the current node as the starting point to obtain the first node, and inserting the first node into the first path, wherein the first path is the path for the robot to reach the target position, the first beacon is the first original node passed from the current node along the first path, and the second predetermined step length is smaller than the first predetermined step length; determining the first path after the node is inserted as the second path; obtaining an augmented path based on the second path and the current position of the robot; obtaining the motion trajectory of the robot based on the spline trajectory corresponding to the augmented path; and determining the robot's motion information based on the motion trajectory.

[0020] Optionally, the selection unit is further configured to obtain the first path in the following manner: obtain a sampling point using a sampling strategy determined based on emotion information; obtain a second node on the search tree of the joint space that is closest to the sampling point, wherein the joint space is the range space of the robot's joint angles, and the joint space includes the obstacle space; extend a third predetermined step length in the direction of the sampling point starting from the second node to obtain a third node; when the third node does not fall in the obstacle space, obtain at least one fourth node on the search tree that is within a first preset distance from the third node; obtain a node with the shortest path length with the third node from at least one fourth node as the parent node of the third node; based on the parent node, insert the third node into the search tree; for each first predetermined node in the search tree that is within the second preset distance from the third node, treat the third node as a parent at the first predetermined node. When the path length corresponding to the node is less than the first original path length of the first predetermined node, the third node is used as the parent node of the first predetermined node, wherein the first original path length is the path length from the first predetermined node to the root node through the original parent node; for each second predetermined node within the third preset distance from the root node in the search tree, when the corresponding path length when the second predetermined node regards the root node as the parent node is less than the second original path length of the second predetermined node, the root node is used as the parent node of the second predetermined node, wherein the second original path length is the path length from the second predetermined node to the root node through the original parent node; when the time from the first acquisition of the sampling point to the current time does not reach the second predetermined time length, return to the step of acquiring the sampling point; based on the adjusted search tree, obtain the first path for the robot to reach the target position within the second predetermined time length.

[0021] Optionally, the selection unit is further configured to obtain sampling points with a first probability in the following manner: obtain the output of the strategy function in the fast path model, and determine the output as the sampling point, wherein the first probability is determined based on the emotional information; or, obtain sampling points with a predetermined probability in the following manner: in response to the predetermined probability being a preset second probability, taking the target position as the sampling point; in response to the predetermined probability being a preset third probability, determining a super ellipsoid with the robot's current position and target position as the focus, and uniformly sampling in the union of the super ellipsoid and the joint space of the obstacle removal space to obtain the sampling point; in response to the predetermined probability being a preset fourth probability, uniformly sampling in the joint space of the obstacle removal space to obtain the sampling point; wherein the sum of the second probability, the third probability and the fourth probability is 1.

[0022] In another general aspect, a computer-readable storage medium storing instructions is provided, wherein when the instructions are executed by at least one computing device, the at least one computing device is prompted to execute any of the above-mentioned robot motion trajectory planning and control methods based on multi-path fusion.

[0023] In another general aspect, a system is provided comprising at least one computing device and at least one storage device storing instructions, wherein the instructions, when executed by the at least one computing device, cause the at least one computing device to execute any of the above-described multi-path fusion-based robot motion trajectory planning and control methods.

[0024] In another general aspect, a computer program product is provided, comprising computer instructions, which, when executed by a processor, implement any of the above-mentioned robot motion trajectory planning and control methods based on multi-path fusion.

[0025] According to the robot motion trajectory planning and control method and device based on multi-path fusion, storage medium and program product of the embodiment of the present disclosure, by imitating the multi-path fusion motion decision-making mechanism in which the prefrontal lobe, amygdala and striatum play a major role, that is, by controlling the selection of fast or slow paths to obtain the robot's motion information through emotional information, the robot can make long-term plans and make rational decisions under safe conditions, and make quick decisions under dangerous conditions to avoid dynamic obstacles urgently, thereby enhancing the adaptability of the robot's motion decision-making in an environment with dynamic obstacles. Therefore, through the present disclosure, the problem that the existing technology robots cannot adapt well to dynamic environments can be effectively solved.

[0026] Additional aspects and / or advantages of the present general inventive concept will be set forth in part in the following description and in part will be apparent from the description or may be learned through practice of the present general inventive concept. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] The above and other objects and features of the embodiments of the present disclosure will become more apparent through the following description in conjunction with the accompanying drawings showing the embodiments, in which:

[0028] Figure 1 is a system schematic diagram showing a robot motion trajectory planning and control method based on multi-path fusion according to an embodiment of the present disclosure;

[0029] Figure 2 is a flow chart showing a robot motion trajectory planning and control method based on multi-path fusion according to an embodiment of the present disclosure;

[0030] Figure 3 is a schematic diagram showing an emotion model of an embodiment of the present disclosure;

[0031] Figure 4 is a schematic diagram showing a path of an embodiment of the present disclosure;

[0032] Figure 5 is a block diagram showing a robot motion trajectory planning and control device based on multi-path fusion according to an embodiment of the present disclosure. DETAILED DESCRIPTION

[0033] The following specific embodiments are provided to help the reader obtain a comprehensive understanding of the methods, devices and / or systems described herein. However, after understanding the disclosure of the present application, various changes, modifications and equivalents of the methods, devices and / or systems described herein will be clear. For example, the order of operations described herein is only an example and is not limited to those orders set forth herein, but can be changed as will be clear after understanding the disclosure of the present application, except for operations that must occur in a specific order. In addition, for greater clarity and simplicity, the description of features known in the art may be omitted.

[0034] The features described herein can be implemented in different forms and should not be construed as being limited to the examples described herein. Rather, the examples described herein have been provided to illustrate only some of the many possible ways to implement the methods, devices, and / or systems described herein, which will be clear after understanding the disclosure of the present application.

[0035] As used herein, the term "and / or" includes any one of the associated listed items and any combination of any two or more.

[0036] Although terms such as "first", "second", and "third" may be used herein to describe various members, components, regions, layers, or portions, these members, components, regions, layers, or portions should not be limited by these terms. Instead, these terms are only used to distinguish one member, component, region, layer, or portion from another member, component, region, layer, or portion. Therefore, without departing from the teachings of the examples described herein, the first member, first component, first region, first layer, or first portion referred to in the examples may also be referred to as the second member, second component, second region, second layer, or second portion.

[0037] In the specification, when an element (such as a layer, a region, or a substrate) is described as being “on”, “connected to”, or “coupled to” another element, the element may be directly “on”, “connected to”, or “coupled to” another element, or one or more other elements may be present therebetween. Conversely, when an element is described as being “directly on”, “directly connected to”, or “directly coupled to” another element, there may be no other elements present therebetween.

[0038] The terms used herein are only used to describe various examples and are not intended to limit the disclosure. Unless the context clearly indicates otherwise, the singular form is also intended to include the plural form. The terms "comprise", "include" and "have" indicate the presence of the described features, quantities, operations, components, elements and / or combinations thereof, but do not exclude the presence or addition of one or more other features, quantities, operations, components, elements and / or combinations thereof.

[0039] Unless otherwise defined, all terms (including technical and scientific terms) used herein have the same meaning as those generally understood by a person of ordinary skill in the art to which the present disclosure belongs after understanding the present disclosure. Unless explicitly defined as such herein, terms (such as those defined in a general dictionary) should be interpreted as having a meaning consistent with their meaning in the context of the relevant art and the present disclosure, and should not be interpreted in an idealized or overly formal manner.

[0040] Furthermore, in the description of examples, when it is considered that a detailed description of a well-known related structure or function would cause vague interpretation of the present disclosure, such a detailed description will be omitted.

[0041] The high adaptability of human motor decision-making in complex environments deserves attention. Neuroscience research shows that this is related to the multi-pathway mechanism of the human brain. The prefrontal cortex (PFC), striatum and amygdala in the human brain, as a whole, coordinate with each other to form the neural circuit of motor decision-making. The prefrontal lobe plays an important role in information analysis, reasoning and planning, and dominates the deliberate, rational and analytical decision-making process. The decision-making pathway played by the prefrontal lobe is regarded as the slow pathway of motor decision-making. In contrast, the striatum is crucial for the control of habitual actions, dominates the process of acquiring and applying motor skills from experience and intuition, and plays an important role in the fast pathway of motor decision-making. The selection and switching mechanism of the fast pathway and the slow pathway are regulated by the emotional signals related to stress and fear related to the amygdala. This multi-pathway fusion motor decision-making mechanism of the human brain enables humans to adapt to dynamic environments excellently, which is of great reference significance for the decision-making of robots in dynamic and complex environments.

[0042] Inspired by the multi-pathway mechanism of human brain movement decision-making, this paper integrates the slow planning and fast action of the robot, such as Figure 1 As shown, this is consistent with the current research on the multi-pathway mechanism of human brain motor decision-making. Specifically, the present disclosure adopts a meticulous but relatively slow global planning method (i.e., the method corresponding to the slow pathway) and a fast-responding but less stable model-free reinforcement learning method to train an action model (i.e., the fast pathway model), respectively imitating the rational planning and habitual actions of the prefrontal cortex and striatum in motor decision-making, aiming to solve the problem of goal orientation and dynamic obstacle avoidance in complex dynamic and static obstacle environments. In addition, the present disclosure also imitates the emotion regulation function of the amygdala to regulate the function of the pathway, integrating the two pathways (fast pathway and slow pathway) into a coordinated whole to enhance the adaptability of the robot in complex dynamic environments.

[0043] The core of the method proposed in the present disclosure consists of three parts: a fast pathway, a slow pathway, and a control module to simulate the fast and slow pathways of the brain and their regulatory functions. After the decision-making part receives the state representation from the sensory cortex, it projects it to the fast pathway and the slow pathway respectively. The fast pathway is implemented based on the SAC (Soft Actor-Critic) method, which simulates the striatum to avoid obstacles with habitual human actions in emergency situations; the slow pathway is implemented based on the improved RT-RRT* real-time sampling planning method, simulating the working memory and planning ability of the human prefrontal lobe, and is composed of real-time sampling path planning and trajectory planning to achieve global planning functions; the control module simulates the emotional function of the amygdala to regulate the two pathways, and outputs corresponding actions to the motor cortex according to the decision-making mode to drive the actuator to move.

[0044] The robot motion trajectory planning and control method and device based on multi-path fusion, computer-readable storage medium and program product disclosed in the present invention are described in detail below in conjunction with the accompanying drawings.

[0045] This paper proposes a robot motion trajectory planning and control method based on multi-path fusion. Figure 2 is a flow chart showing a robot motion trajectory planning and control method based on multi-path fusion according to an embodiment of the present disclosure. Figure 2 The robot motion trajectory planning and control method based on multi-path fusion includes the following steps:

[0046] In step S201, emotional information is determined based on the current joint angle and current joint angular velocity of the robot's robotic arm, the current position and current velocity of the obstacle, and collision information, wherein the emotional information represents the degree of stress or fear caused by the obstacle approaching and colliding with the robot.

[0047] As an example, emotional information is generated based on the states of the robot and the obstacle, and the emotional information represents the degree of stress or fear caused by the approach and collision of the obstacle, so as to simulate the process of fear triggering to switch the planning state to the habitual state and fear subsiding to switch the habitual state to the planning state, that is, the fast path or the slow path can be selected according to the emotional information to obtain the robot's action information.

[0048] According to an embodiment of the present disclosure, determining emotional information based on the current joint angle and current joint angular velocity of the robot's mechanical arm, the current position and current velocity of the obstacle, and collision information may include: inputting the current joint angle and current joint angular velocity of the robot's mechanical arm, the current position and current velocity of the obstacle, and collision information into an emotional model to obtain emotional information, wherein the emotional model is trained based on a predetermined loss function, and the predetermined loss function is determined based on the output of the emotional model, the distance from the robot to the obstacle, and the collision information. Through this embodiment, emotional information can be easily and quickly acquired using a trained emotional model.

[0049] As an example, the indicative function of the collision event can be used to represent the collision information, that is, information indicating whether a collision event between the robot and the obstacle occurs. For example, if a collision event occurs, the indicative function is 1; if no collision event occurs, the indicative function is 0.

[0050] As an example, the present disclosure proposes a brain-inspired regulation mechanism to achieve a smooth transition between the two motion decision states of the fast pathway and the slow pathway. Analogous to the fast and slow pathway mechanisms of human brain decision-making, the robot's motion decision-making process can be described as a finite state machine (FSM), and the fast pathway and the slow pathway are regarded as two of its states. A simple method is to set a threshold according to the kinematic characteristics of the obstacle to switch states, but the state transition caused by this method is abrupt, which is inconsistent with the human decision-making regulation process. Inspired by the role of the human brain's emotional mechanism in the regulation of fast and slow pathway switching, the present disclosure simulates the structure and mechanism of the amygdala, generates emotional signals according to the states of the robot and the obstacle, simulates the process of fear triggering to switch the planning to the habit state, and the fear subsidence to switch the habit state to the planning state, thereby achieving smooth switching of the two pathways.

[0051] Specifically, the emotion model of this embodiment can be a forward neural network EmoNet that simulates the structure of the amygdala, such as Figure 3 As shown. The weights of the excitatory full connection and the inhibitory full connection can be restricted to positive and negative values ​​respectively, and e1 and e2 are the emotional thresholds at which the slow pathway begins to be guided by the fast pathway and switches to the fast pathway. The input of EmoNet can include conditional stimuli and unconditioned stimulus US:I collision , where q and Respectively represent the joint angle and joint angular velocity of the robot's manipulator, that is, the position and velocity of the robot, Pdyo and are the position and velocity of the dynamic obstacle in the task space, I collision is an indicative function of collision events, which makes EmoNet suitable for dynamic obstacles that are being observed or appear suddenly.

[0052] The goal of EmoNet is to generate emotional information e related to obstacle collision based on input information. Therefore, the loss function of EmoNet can be defined as follows:

[0053]

[0054] Among them, θ e is the neuron connection weight parameter of EmoNet, e d Indicates the distance to the dynamic obstacle d dyo Impact on emotional output, I collision represents the characteristic function of the collision event, w e is the weight, e d Defined as d dyo The decreasing function is as follows:

[0055]

[0056] where d f is the reference distance to the obstacle when the emotion information is e2.

[0057] In step S202, based on the emotion information, a fast path or a slow path is selected to obtain the action information of the robot. Among them, the fast path simulates the function of the striatum to control the robot to achieve fast emergency obstacle avoidance, and the slow path simulates the function of the prefrontal lobe to achieve the motion trajectory planning of the robot to the target position.

[0058] According to an embodiment of the present disclosure, selecting a fast path or a slow path to obtain the action information of the robot based on the emotion information may include: when the emotion information is less than the first threshold, selecting the slow path to obtain the action information of the robot; when the emotion information is greater than or equal to the first threshold, selecting the fast path to obtain the action information of the robot. Through this embodiment, the switching of the fast path and the slow path based on emotion control is realized.

[0059] As an example, when the above first threshold is e2, when e < e2, select the slow path to obtain the action information of the robot, that is, the robot performs slow path planning; when e > e2 and equal, select the fast path to obtain the action information of the robot, that is, the robot uses the fast path to perform habitual actions.

[0060] As an example, when e < e2, select the slow path to obtain the action information of the robot, that is, the robot performs slow path planning. A second threshold, that is, e1, can be further introduced. When e < e1, the robot performs complete slow path planning independent of the fast path; when e1 < e < e2, the robot performs slow path planning under the guidance of the fast path.

[0061] As an example, considering the continuity of the emotion signal, e is further updated in the form of soft update, as shown in the following formula:

[0062] e t+1 = τ e e t +(1 - τ e )e, (3)

[0063] where τ e is the soft update rate.

[0064] According to an embodiment of the present disclosure, selecting the fast path to obtain the action information of the robot may include: inputting the current joint angle, the current joint angular velocity, the target position, the minimum distance between the robot and the obstacle, and the two end points corresponding to the minimum distance into the fast path model to obtain the action information of the robot. Among them, one of the two end points is on the robot, and the other of the two end points is on the obstacle. The fast path model is trained through the following reward function:

[0065]

[0066] Among them, w represents the weight vector, r s represents the cumulative reward value without collision in each decision cycle, r g Represents the distance between the end of the robotic arm and the target position, r a Indicates the robot's motion range, r o Depending on the minimum distance between the robot and the obstacle and the collision information, r p A penalty term that represents the robot's actions that exceed the robot's constraints.

[0067] Through the above embodiments, the trained fast-path model can obtain the robot's motion information conveniently and quickly.

[0068] Specifically, the fast path aims to imitate the habitual decision-making process of the striatum to achieve obstacle avoidance in emergency situations. In view of the evidence of the relationship between the striatal decision-making mechanism and reinforcement learning, this embodiment uses a model-free reinforcement learning method to implement this function. Considering the advantages of the SAC algorithm such as insensitivity to hyperparameters and loose task requirements, it helps to enhance the adaptability of the robot in a dynamic obstacle environment, so it is selected as the algorithm of the fast path method. The goal of training is to enable the agent (the tool carrying the fast path model) to learn to reach the target position while avoiding dynamic obstacles. It should be noted that the agent can be trained offline before the task to meet the real-time requirements of habitual actions.

[0069] As an example, the neural network structure of the fast-path model of this embodiment is similar to the common reinforcement learning implementation. Multiple parameterized neural networks are used in the fast-path model to estimate the policy function π and the action value function Q. Specifically, the fast-path neural network structure of this embodiment can be composed of five parameterized neural networks: two current Q networks and two target Q networks contained in the Critic network, and the Actor network (the network contains a network, namely itself, which is the carrier of the policy function π). For example, in order to reduce overfitting, the fast-path model contains two current Q networks (composed of θ i parameterization, i = 1, 2), the smaller value of the two outputs is used for gradient calculation in training; moreover, the fast path model also includes two target Q networks, the weight of the target Q network is based on the current Q network weight θ i And the update rate τ are soft updated to enhance the stability of the training process, that is,

[0070]

[0071] Furthermore, the fast-path model also includes an Actor network, which consists of an input layer, two hidden layers, and two output layers, where the Rectified Linear Unit (ReLU) activation function is used in the hidden layer. Since the strategy is represented by a Gaussian distribution, the information output by the output layer of the Actor network uses the hyperbolic tangent function (tanh) and the exponential function as activation functions to obtain the mean and standard deviation of the action distribution. Similarly, the Critic network also uses a similar structure with two hidden layers. The output layer represents the action value Q, which is the target of fast-path training optimization and is equivalent to the loss function of the training strategy function. Finally, the experience replay pool is used for training sampling. and batch processing to ensure the independence of the samples used in each update step.

[0072] As an example, the input of the fast path model, i.e., the state space, can be defined. Generally speaking, in addition to robot dynamics, the realization of collision avoidance also depends on the robot's perception of the environment and obstacles. This may require robot vision to calculate the obstacle information and environmental information required for collision avoidance, or to collect local environmental information and obstacle information through ranging methods (such as ultrasound or radar) to calculate the obstacle distance information required for collision avoidance. In the fast path, it is reasonable to assume that the robot can dynamically access the obstacle position and the distance between it and the obstacle. Therefore, the state space It can be defined as:

[0073]

[0074] Among them, q represents the joint angle of the robot's mechanical arm, represents the joint angular velocity of the robot's manipulator, p goal represents the target position, d o represents the minimum distance between the robot and the obstacle, p rd Indicates the end with the minimum distance, which is located on the robot, p od represents the other end of the minimum distance, and the endpoint is located on the obstacle. It should be noted that the elements in the state space are three-dimensional space variables.

[0075] As an example, we can define the space corresponding to the output of the Actor network in the fast path model, that is, the action space. Including the robot's joint angular velocity, defined as

[0076]

[0077] According to the action space, the robot's action can be further obtained, that is, u n+1 =u n +aT cAmong them, T c is the control cycle, that is, the duration for the robot to perform an action.

[0078] As an example, a suitable reward function can be designed to enable the robot to learn to reach the target position and avoid obstacles. The fast path model can design a reward function as follows:

[0079]

[0080] Among them, w represents the weight vector; r s represents the cumulative reward value without collision in each decision cycle; r g Represents the distance between the end of the robotic arm and the target position; r a Indicates the robot's range of motion; r o Depends on o And collision information (such as whether there is a collision) and d o Negative correlation; r p It represents the penalty term for the robot's action exceeding the robot's constraints, specifically:

[0081] r g =-||p ee -p goal || 2 , (9)

[0082] r a =-‖a t || 2 , (10)

[0083]

[0084]

[0085] It should be noted that for obstacle collisions during training, the fast path model introduces a penalty r when a collision occurs. c , which can effectively promote the learning of obstacle avoidance strategies. For the constraints introduced by robot dynamics, the fast path model uses the penalty term r p Instead of considering hard constraints (such as not allowing the robot to approach the obstacle within a certain distance), the obstacle may approach the robot faster than the maximum speed allowed for the robot to move. In this case, the hard constraint may make the problem infeasible, and the robot may stop and cause a collision. With the soft penalty of this embodiment, the robot will move away from unpredictable dynamic obstacles as quickly as possible.

[0086] According to an embodiment of the present disclosure, selecting a slow path to obtain the robot's motion information may include: performing the following processing for each node in the first path: when the distance between the current node and the first beacon exceeds the first predetermined step length, extending the second predetermined step length in the direction of the first beacon from the current node as the starting point to obtain the first node, and inserting the first node into the first path, wherein the first path is the path for the robot to reach the target position, the first beacon is the first original node passed along the first path from the current starting point, and the second predetermined step length is less than the first predetermined step length; determining the first path after the node is inserted as the second path; obtaining an augmented path based on the second path and the current position of the robot; obtaining the motion trajectory of the robot based on the spline trajectory corresponding to the augmented path; and determining the robot's motion information based on the motion trajectory. Through this embodiment, when the interval between two original nodes is too long, one or more new nodes are inserted, and the robot's motion information is obtained based on the first path after the node is inserted, thereby avoiding the problem of too long interval between the two original nodes in the first path and low real-time availability.

[0087] As an example, the beacon is the original node in the first path, and the newly inserted node does not belong to the beacon.

[0088] As an example, the second path may be obtained by, but not limited to, the following method: Figure 4 As shown, the first path of this embodiment is assumed to be Figure 4 The path shown by the dashed line in the figure shows that the first path will cause the robot to deviate from the original path (solid line), which may affect the switching of the root node. Moreover, since the redundant nodes on the original path are removed, the interval between beacons in the first path may exceed the step length ρ. s Therefore, after planning the first path, starting from the first node of the first path, determine whether the distance between the node and the corresponding first beacon exceeds the first predetermined step length. If so, use x s =STEER(x root , x1) calculates the first node x s , and serves as a new Smart node, where x1 is the first beacon, x root Right now Figure 4 The starting point in the calculation is the current node; if x s Not in the tree In the example, x s Insert into tree As the robot's next moving target, it is also convenient to make corresponding decisions later. A more suitable path is obtained, where the space corresponding to x and its meaning will be discussed in detail later and will not be discussed here. In an environment with static obstacles, this operation ensures the real-time availability of the search tree while maintaining the planning result of RT-RRT*.

[0089] It should be noted that the slow pathway is designed to simulate the role of the prefrontal cortex (PFC) in the rational planning of brain motor decision-making, and to achieve long-term and global planning. Relevant neuroscience evidence shows that the planning function of PFC is achieved through tree search. Inspired by this mechanism, this disclosure applies a planning method based on RRT (Rapidly-exploring Random Tree) sampling to imitate the planning function of PFC, and also uses the search tree structure to optimize decision costs and planning. The planning of the slow path adopts a hierarchical planning strategy. First, the path σ is obtained through a real-time sampling planning method. p (will be described in detail later), and then the trajectory planning method is used to obtain the trajectory σ(s).

[0090] As an example, the trajectory acquisition process will be described below. After the second path is acquired, the broken line path formed by its direct connection can enable the robot to complete the task, but it is not smooth enough and may cause a sudden change in the robot's action. It is not suitable for direct output to the robot for execution. Therefore, this embodiment also introduces trajectory planning based on path points in the joint space. Some methods directly use splines and other methods in sampling-based planning methods to obtain more detailed planning and solve action sequences, but the computational cost of calculating splines between two non-specific points is relatively high. In the slow path, the present disclosure obtains the second path. Finally, the Time-Optimal Path Parameterization based on Reachability Analysis (TOPP-RA) method on the spline trajectory is used to plan the robot's trajectory, as follows:

[0091] Generally, when a robot wants to reach the target position, it may need to make multiple decisions to finally reach the target position, and each decision has a duration, which is called a decision cycle. In each decision cycle, the slow path will obtain an augmented path based on the second path. Then, trajectory planning is performed based on the augmenting path, where the elements in the augmenting path are represented as connect The spline trajectory can be expressed as follows

[0092] σ i (s) = h i0 +h i1s+h i2 s 2 +h ia s 3 , s∈[s i-1 ,s i ], i = 1, 2, ..., N pa . (13)

[0093] Among them, h il is the m-dimensional parameter, l = 0, 1, 2, 3, is a monotonically increasing sequence defined in [0, 1] and is determined by the following formula

[0094]

[0095] This helps the robot to move smoothly along the planned path. and 0) are known, therefore, the spline parameter h il It can be determined by interpolation conditions, first-order continuity conditions, second-order continuity conditions and fixed boundary conditions, that is,

[0096]

[0097] Thus, the trajectory shape σ(s), which is the motion trajectory of the robot, can be obtained.

[0098] As an example, after obtaining the robot's motion trajectory, the robot's action information will be determined in the following way. The slow path uses the TOPP-RA method to solve the action sequence {u} for the trajectory shape σ(s). TOPP-RA recursively calculates the reachable and controllable set of discrete positions on the path, and then forward selects actions to construct the optimal state and control, thereby achieving time-optimal trajectory planning on the specified path. First, in Based on the linear interval monotonically increasing sequence on [0, 1], the trajectory shape σ(s) is processed to divide the trajectory into multiple segments to obtain multiple trajectory points, and the number of trajectory points is obtained as follows:

[0099]

[0100] The number of trajectory points is proportional to the trajectory length of σ, k ra It should be noted that it takes the same time to reach each trajectory point, that is, the control period T c , that is, each decision cycle contains multiple control cycles, so as to control the robot motion according to the trajectory points, and after a decision cycle, the trajectory needs to be re-determined. Then, based on the TOPP-RA method, s is first calculated by backward traversal i The corresponding controllable set:

[0101]

[0102] in, Represents a controllable set, that is, a sequence of actions Can reach The state collection. Represents a single-step set, referring to a state set The state in can be achieved by one step u i Next, based on the TOPP-RA method, we solve the optimization problem about {u} through forward traversal to obtain the optimal action sequence:

[0103]

[0104]

[0105] Among them, Δ i =s i+1 -s i ,Ω i Represents an action state pair (u i , x i ) which takes into account the kinematic and dynamic constraints of the robot.

[0106] Then, the parameterized action sequence {u} (i.e., the action information in this embodiment) planned along the trajectory σ in each decision cycle is returned based on the TOPP-RA method. In each control cycle of a decision cycle, the action corresponding to the sampling time in {u} can be selected and executed on the robot. Compared with the spline-based trajectory planning for unspecified nodes in the sampling-based planning method, this embodiment significantly reduces the computational overhead and ensures satisfactory real-time performance.

[0107] It should be noted that the planning process in the slow path is in the planning state space In which, Set as the robot's joint space Although the joint space has a higher dimensionality compared to the task space, which increases the complexity of the planning task, joint space planning significantly reduces the computational burden of inverse kinematics calculations and helps avoid singular pose problems during robot motion. Therefore, the main steps of the slow path are performed in the joint space. In this case, obstacles are transformed into the joint space to enable collision detection. It should be noted that the obstacle space It not only contains the mapping of obstacle area to joint space, but also the robot joint configuration that collides with obstacles, i.e.

[0108]

[0109] in, and They are the robot and the obstacle in the task space under the x-pose (robot's joint angle) In the representation.

[0110] According to an embodiment of the present disclosure, a first path is obtained in the following manner: a sampling point is obtained using a sampling strategy determined based on emotion information; a second node on a search tree of a joint space that is closest to the sampling point is obtained, wherein the joint space is the range space of the joint angles of the robot, and the joint space includes an obstacle space; a third node is obtained by extending a third predetermined step length in the direction of the sampling point starting from the second node; when the third node does not fall in the obstacle space, at least one fourth node on the search tree that is within a first preset distance from the third node is obtained; a node with the smallest path length with the third node among at least one fourth node is obtained as the parent node of the third node; based on the parent node, the third node is inserted into the search tree; for each first predetermined node in the search tree that is within a second preset distance from the third node, when the first predetermined node regards the third node as a parent node When the corresponding path length is less than the first original path length of the first predetermined node, the third node is used as the parent node of the first predetermined node, wherein the first original path length is the path length from the first predetermined node to the root node through the original parent node; for each second predetermined node within a third preset distance from the root node in the search tree, when the corresponding path length when the second predetermined node regards the root node as a parent node is less than the second original path length of the second predetermined node, the root node is used as the parent node of the second predetermined node, wherein the second original path length is the path length from the second predetermined node to the root node through the original parent node; when the time from the first acquisition of the sampling point to the current time does not reach the second predetermined time length, return to the step of acquiring the sampling point; based on the adjusted search tree, obtain the first path for the robot to reach the target position within the second predetermined time length.

[0111] By obtaining the first path through this embodiment, path redundancy can be reduced by converting curves into straight lines, which is beneficial to trajectory planning and robot action execution.

[0112] The first original path length is the path length of the path from the first predetermined node to the root node of the search tree through its original parent node, and the second original path length is the path length of the path from the second predetermined node to the root node of the search tree through its original parent node. The second predetermined duration is a decision cycle, which includes multiple control cycles, that is, multiple first predetermined durations. The third predetermined step length can be set as needed.

[0113] As an example, since the activity of PFC increases with the planning requirements of breadth-first search (BFS) of future paths during the planning reconfiguration, the present disclosure introduces the RT-RRT* method of implementing search tree rewiring in a BFS manner to characterize the activity of PFC. The planning of the slow path adopts a hierarchical planning strategy. First, the path σ is obtained by a real-time sampling planning method. p (Second path), and then the trajectory planning method is used to derive the trajectory σ(s). The trajectory has been discussed above and will not be discussed in detail here.

[0114] RT-RRT* inherits the probabilistic completeness and asymptotic convergence of the RRT algorithm, and has a strong real-time advantage in robot motion decision-making; however, its initial solution is not friendly to the robot, and the optimization convergence speed is slow. In the real-time planning task of robot motion decision-making, due to the redundancy and roughness of the path it generates, it is almost impossible for the robot to execute directly. Therefore, the present disclosure introduces the Smart method in the slow path to optimize the path. Specifically, the Smart method attempts to connect the later points on the path to the earlier nodes on the path as conflict-free as possible, and reduces the path redundancy by turning the curve into a straight line. The original path retained on the Smart path ( Figure 4 The inflection point of the solid line is called the beacon ( Figure 4 The hollow circle in the figure), the first path is obtained based on the beacon ( Figure 4 RT-RRT* with Smart method smart The planned path can significantly reduce redundancy compared to RT-RRT*, which is beneficial to trajectory planning and robot action execution.

[0115] It should be noted that the real-time planning capability of RT-RRT* comes from the rewiring of the search tree in a dynamic environment. smart The tree structure is not changed directly, as this would destroy the original search tree structure and make it conflict with the rewiring step. Instead, RT-RRT* smart Only change the search tree The path output is that only the new path is sent to the robot. At the same time, considering that the positions of the robot and obstacles are changing during real-time planning, RT-RRT* smart Keep RT-RRT* generated This means that real-time sampling planning only applies the Smart method when generating path planning results.

[0116] As an example, the following describes the solution process of the second and fourth nodes. In the RT-RRT* algorithm, the NEAREST step after sampling (the step of solving the second node) and the NEAR step in the INSERTNODE and REWIRE processes (the step of solving the fourth node) all need to solve the Nearest Neighbor Search (NNS) problem, which is a key part affecting the speed of RRT algorithms. In the process of performing a neighbor query on x, the traditional approach is to traverse the node set in the tree. Filter out the distance from x to the nearest N (set value) node. However, when When the scale is expanded, this step will lead to higher time complexity. In order to speed up the NNS process, the present disclosure uses a hierarchical navigable small world method (HNSW) in the slow path, which can reduce the time complexity from O(N) to O(logN), thereby improving the speed and real-time performance of the algorithm.

[0117] HNSW uses a hierarchical structure to quickly solve the NNS of the target node and constructs an additional graph structure with a series of level hierarchies, each level representing an abstraction of the sample set. HNSW allows the search process to proceed and jump between different levels, thereby improving the search efficiency, so that it can maintain a fast query speed even in high-dimensional state spaces such as robot joint spaces. Compared with nearest neighbor search methods such as KD-Tree and Annoy, HNSW allows new nodes to be inserted into the generated graph structure, which is beneficial to the INSERTNODE step (the step of inserting the third node into the search tree) in RRT algorithms. In addition, when the search time is limited, HNSW can also return an approximate nearest neighbor search with acceptable accuracy instead of a search failure. In the present disclosure, HNSW is mainly used in the following two places, namely formula (20) and formula (21):

[0118]

[0119] Among them, the search number k = 1 means to find the sampling point x rand The closest node (i.e. the second node) of

[0120]

[0121] Among them, search k = k N Indicates that at most the distance to the third node x is found new Less than ρ N (first preset distance) k N nodes.

[0122] As an example, in each decision cycle, the RT-RRT* algorithm calculates the planned path for the current environment (i.e. the second path). If the target point x goal (i.e. target location) has been If searched, Return to the target point x goal ; otherwise, Return to x p The path, where x p Determined by the following formula:

[0123]

[0124] Among them, λ is the weight term, which controls the cost of the planned path c(x p ) and from x p The distance to the target point.

[0125] According to an embodiment of the present disclosure, sampling is performed based on a sampling strategy determined based on emotion information to obtain sampling points, which may include: obtaining sampling points with a first probability in the following manner: obtaining the output of the strategy function in the fast path model, and determining the output as the sampling point, wherein the first probability is determined based on a first threshold; or obtaining sampling points with a predetermined probability in the following manner: in response to the predetermined probability being a preset second probability, taking the target position as the sampling point; in response to the predetermined probability being a preset third probability, determining a hyperellipsoid with the current position and the target position of the robot as the focus, uniformly sampling in the union of the hyperellipsoid and the joint space of the obstacle removal space to obtain sampling points; in response to the predetermined probability being a preset fourth probability, uniformly sampling in the joint space of the obstacle removal space to obtain sampling points; wherein the sum of the second probability, the third probability and the fourth probability is 1.

[0126] Through this embodiment, relatively excellent sampling points can be obtained and the sampling points can be enriched.

[0127] As an example, the sampling strategy is a key component of the sampling-based planning method. Inspired by the mechanism of striatum guiding prefrontal planning in the brain motor decision neural circuit, this disclosure introduces a new sampling method, which uses the output of the SACActor model to guide the sampling of slow pathway planning. Specifically, defined in The sampling function f on can be described as follows:

[0128]

[0129]

[0130] Among them, p f (first probability), p g (second probability) and pe (Third probability) represents the probability parameter of the corresponding sampling method, and the others correspond to the fourth probability. fast (·) represents the sampling distribution regulated by the habitual motor decision model of the fast pathway, with probability p f Affected by the emotional information generated by the emotional model, the emotional model has been introduced above and will not be discussed here. r (·) represents a sampling strategy independent of the fast path, where I(·) represents the indicative function and I(x goal ) indicates selecting the target point x goal As the output of f, U(·) represents the uniform distribution function.

[0131] Specifically, f is first calculated according to p f Determine whether the sampling process is guided by the fast-path Actor model. Otherwise, one of the following three sampling strategies will be selected based on probability:

[0132] a) With probability p g Select the following sampling strategy: f returns x directly goal , that is, the target point is This strategy will help guide the search tree Grow towards the target point in the STEER step.

[0133] b) with probability p e Select the following sampling strategy: f selects x a 、x goal The hyperellipsoid with the focus exist uniform sampling, where the transverse diameter a of the hyperellipsoid e and conjugate diameter b e They are

[0134]

[0135] This sampling strategy is shown to be able to speed up σ while ensuring the optimal cost. p The convergence rate of The joint space with the obstacle space removed is called the movable space;

[0136] c) In other cases, f is drawn from the entire movable space Uniform sampling in .

[0137] As an example, the following introduces p f , the emotional information e output by the emotional model can adjust the sampling strategy f in the slow path fast The implementation probability p f , as shown below:

[0138]

[0139] Among them, P fm represents the maximum probability at e = e2, and p f increases as e increases, causing the robot to show a tendency to plan along the direction of habitual actions. e2 is the first threshold mentioned in the above embodiment.

[0140] When e1 < e < e2, it indicates that the robot may face potential threats brought by dynamic obstacles. Therefore, the robot should focus more attention on the current planned path, that is, the sampling of the slow-path planning needs to be guided by the output of the fast-path SAC Actor model. Here, the design of f fast is introduced as shown in the following formula:

[0141]

[0142] Among them, x f is the reference planning state sampled along the current planned trajectory σ, and the distance from x a does not exceed ρ f . s f represents the corresponding speed of x f on σ and the state of the current environment. f fast (·) returns a sampling point starting from x f and moving along the action direction of π(s f ) with a step size of ρ s . The selection radius ρ f of the reference planning state is adjusted by the emotion e as shown in the following formula:

[0143]

[0144] Among them, ρ f decreases as e increases, indicating that as the emotion strengthens, f fast tends to focus on trajectory points closer to x a along σ. This enables the robot to pay more attention to the situation around the current position after the obstacle approaches and the obstacle avoidance action ends, thereby accelerating the optimization and correction of the local planning. ρ f0 is a custom radius parameter.

[0145] As an example, under the above adjustment mechanism, the motion decision of the robot can be divided into three cases: when e < e1, the slow path is selected to obtain the motion information of the robot, that is, the robot performs slow path planning; when e1 < e < e2, the robot performs slow path planning under the guidance of the fast path; when e > e2 and when they are equal, the fast path is selected to obtain the motion information of the robot, that is, the robot uses the fast path to perform habitual actions.

[0146] In step S203, control the robot's manipulator to move for a first predetermined duration according to the motion information.

[0147] As an example, the above first predetermined duration is one control cycle in the above embodiment. It should be noted that in the slow path, one decision cycle (i.e., the second predetermined duration) corresponds to one motion trajectory, and this motion trajectory will be divided into multiple segments to obtain multiple trajectory points, and the same duration is used to reach each trajectory point, that is, the control cycle T c , in other words, the speed to reach each trajectory point is also consistent, that is, the trajectory distance between the current trajectory point and the next trajectory point and T c The execution speed can be determined, that is, the joint angular velocity. And in each control cycle, the action corresponding to the sampling time in the action information {u} can be selected and executed on the robot. It should also be noted that the emotion information and the selection of the fast and slow paths need to be determined in each control cycle, but the trajectory of the slow path is planned according to one decision cycle, that is, the trajectory planned by the slow path contains the action information of one decision cycle, that is, it contains the action information corresponding to multiple control cycles.

[0148] As an example, assume that one decision cycle contains the action information of 4 control cycles. After the end of the first control cycle of the slow path, the emotion information is determined. After determining the emotion information, it is determined that the fast path is selected in the second control cycle, then the action information is determined through the fast path, and this action information is executed within the second control cycle; after the end of the second control cycle, continue to the third control cycle, and the emotion information also needs to be determined. After determining the emotion information, it is determined that the slow path is selected in the third control cycle. At this time, the slow path needs to re-plan the trajectory of one decision cycle, rather than using the action information of the fourth control cycle of the previous decision cycle, because the trajectory has definitely changed after passing through the fast path once, so when passing through the slow path again, a trajectory of one decision cycle needs to be re-planned.

[0149] In step S204, determine whether the robot's manipulator reaches the target position.

[0150] In step S205, in response to the fact that the robot's manipulator does not reach the target position, return to the step of determining the emotion information.

[0151] As an example, after the robot has moved for a predetermined time but has not reached the target position, it can return to the emotion determination step and perform the next operation for a predetermined time until the robot's mechanical arm reaches the target position.

[0152] The following is a description of the human-inspired robot multi-path fusion motion decision-making method in a dynamic obstacle environment proposed in the present disclosure in conjunction with the method flow in Table 1:

[0153] Table 1 Method flow

[0154]

[0155]

[0156] The slow path process in the above method is described in Table 2 below.

[0157] Table 2 Slow path process

[0158]

[0159] Among them, the 5th row in Table 2 indicates that each time steps 6-14 are completed, the remaining time of the decision cycle is checked, that is, when the time from the first acquisition of the sampling point to the current time does not reach the predetermined time; the 13th row indicates whether the path length of the nodes near the third node is shorter than that of the third node. If so, the third node is used as the parent node of the corresponding node; the 14th row indicates whether the path length of the nodes near the root node is shorter than that of the root node. If so, the root node is used as the parent node of the corresponding node; the 15th row indicates that the first path is determined based on the search tree, and the 18th row indicates that the first node is inserted into the search tree, which is equivalent to inserting the first path.

[0160] In summary, the key steps in the above process can be as follows:

[0161] Step S1: Build and train a striatum-inspired fast-pathway model;

[0162] Step S2: Build and train an amygdala-inspired emotion model;

[0163] Step S3: Set the robot's target position x goal , obstacle space and the robot's initial state x init ;

[0164] Step S4: Update the current position, state space, obstacle space and target position of the input robot of the method flow, which are represented by the following symbols respectively:

[0165] Step S5: The emotion model calculates the emotion information e based on the above information t, and select the fast path or slow path to obtain the robot's motion information;

[0166] Step S6: If the fast path decision is selected, the fast path model is used to obtain the action information {u};

[0167] Step S7: If the slow path decision is selected, the slow path rational planning is adopted to obtain the action information {u};

[0168] Step S8: Control the robot to move for a control period T according to the action information {u} d (i.e. the first scheduled duration);

[0169] Step S9: Repeat steps S4 to S8 until the robot's mechanical arm reaches the target position.

[0170] Figure 5 is a block diagram showing a robot motion trajectory planning and control device based on multi-path fusion according to an embodiment of the present disclosure, such as Figure 5 As shown, the device includes a determination unit 50 , a selection unit 52 , a control unit 54 , a judgment unit 56 and a response unit 58 .

[0171] The determination unit 50 is configured to determine the emotional information according to the current joint angle and the current joint angular velocity of the robot's mechanical arm, the current position and the current velocity of the obstacle, and the collision information, wherein the emotional information represents the degree of stress or fear caused by the obstacle approaching and colliding with the robot; the selection unit 52 is configured to select a fast path or a slow path to obtain the robot's motion information based on the emotional information, wherein the fast path simulates the function of the striatum to control the robot to achieve rapid emergency obstacle avoidance, and the slow path simulates the function of the prefrontal lobe to achieve motion trajectory planning of the robot to the target position; the control unit 54 is configured to control the robot's mechanical arm to move for a first predetermined time period according to the motion information; the judgment unit 56 is configured to determine whether the robot's mechanical arm has reached the target position; the response unit 58 is configured to return to the step of determining the emotional information in response to the robot's mechanical arm not reaching the target position.

[0172] According to an embodiment of the present disclosure, the determination unit 50 is also configured to input the current joint angle and current joint angular velocity of the robot's robotic arm, the current position and current velocity of the obstacle, and collision information into the emotion model to obtain emotion information, wherein the emotion model is trained based on a predetermined loss function, and the predetermined loss function is determined based on the output of the emotion model, the distance from the robot to the obstacle, and the collision information.

[0173] According to an embodiment of the present disclosure, the selection unit 52 is further configured to select a slow path to obtain the robot's motion information when the emotion information is less than a first threshold; and to select a fast path to obtain the robot's motion information when the emotion information is greater than or equal to the first threshold.

[0174] According to an embodiment of the present disclosure, the selection unit 52 is further configured to input the current joint angle, the current joint angular velocity, the target position, the minimum distance between the robot and the obstacle, and the two endpoints corresponding to the minimum distance into the fast path model to obtain the action information of the robot, wherein one of the two endpoints is on the robot and the other of the two endpoints is on the obstacle, and the fast path model is trained by the following reward function:

[0175]

[0176] Among them, r s represents the cumulative reward value without collision in each decision cycle, r g Represents the distance between the end of the robotic arm and the target position, r a Indicates the robot's motion range, r o Depending on the minimum distance between the robot and the obstacle and the collision information, r p A penalty term that represents the robot's actions that exceed the robot's constraints.

[0177] According to an embodiment of the present disclosure, the selection unit 52 is also configured to perform the following processing for each node in the first path: when the distance between the current node and the first beacon exceeds a first predetermined step length, the first node is obtained by extending the second predetermined step length in the direction of the first beacon from the current node as the starting point, and the first node is inserted into the first path, wherein the first path is the path for the robot to reach the target position, the first beacon is the first original node passed from the current node along the first path, and the second predetermined step length is smaller than the first predetermined step length; the first path after the node is inserted is determined as the second path; based on the second path and the current position of the robot, an augmented path is obtained; based on the spline trajectory corresponding to the augmented path, the motion trajectory of the robot is obtained; based on the motion trajectory, the action information of the robot is determined.

[0178] According to an embodiment of the present disclosure, the selection unit 52 is further configured to obtain the first path in the following manner: obtain a sampling point using a sampling strategy determined based on emotion information; obtain a second node on the search tree of the joint space that is closest to the sampling point, wherein the joint space is the range space of the robot's joint angles, and the joint space includes the obstacle space; extend a third predetermined step length in the direction of the sampling point starting from the second node to obtain a third node; when the third node does not fall in the obstacle space, obtain at least one fourth node on the search tree that is within a first preset distance from the third node; obtain a node with the smallest path length with the third node from at least one fourth node as the parent node of the third node; based on the parent node, insert the third node into the search tree; for each first predetermined node in the search tree that is within the second preset distance from the third node, insert the third node into the search tree at the first predetermined node When the path length corresponding to the point when the second predetermined node is regarded as the parent node is less than the first original path length of the first predetermined node, the third node is taken as the parent node of the first predetermined node, wherein the first original path length is the path length from the first predetermined node to the root node through the original parent node; for each second predetermined node within the third preset distance from the root node in the search tree, when the path length corresponding to the second predetermined node when the root node is regarded as the parent node is less than the second original path length of the second predetermined node, the root node is taken as the parent node of the second predetermined node, wherein the second original path length is the path length from the second predetermined node to the root node through the original parent node; when the time from the first acquisition of the sampling point to the current time does not reach the second predetermined time length, return to the step of acquiring the sampling point; based on the adjusted search tree, obtain the first path for the robot to reach the target position within the second predetermined time length.

[0179] According to an embodiment of the present disclosure, the selection unit 52 is also configured to obtain sampling points with a first probability in the following manner: obtain the output of the strategy function in the fast path model, and determine the output as the sampling point, wherein the first probability is determined based on the emotion information; or, obtain sampling points with a predetermined probability in the following manner: in response to the predetermined probability being a preset second probability, take the target position as the sampling point; in response to the predetermined probability being a preset third probability, determine a hyperellipsoid with the current position and the target position of the robot as the focus, and uniformly sample in the union of the hyperellipsoid and the joint space of the obstacle removal space to obtain the sampling point; in response to the predetermined probability being a preset fourth probability, uniformly sample in the joint space of the obstacle removal space to obtain the sampling point; wherein the sum of the second probability, the third probability and the fourth probability is 1.

[0180] According to an embodiment of the present disclosure, a computer-readable storage medium storing instructions is provided, wherein, when the instructions are executed by at least one computing device, the at least one computing device is prompted to execute a robot motion trajectory planning and control method based on multi-path fusion as in any of the above embodiments.

[0181] According to an embodiment of the present disclosure, a system is provided comprising at least one computing device and at least one storage device storing instructions, wherein the instructions, when executed by the at least one computing device, prompt the at least one computing device to execute a robot motion trajectory planning and control method based on multi-path fusion as in any of the above embodiments.

[0182] Although some embodiments of the present disclosure have been shown and described, it will be appreciated by those skilled in the art that modifications may be made to the embodiments without departing from the principles and spirit of the present disclosure, the scope of which is defined by the claims and their equivalents.

Claims

1. A robot motion trajectory planning and control method based on multi-channel fusion, characterized in that: include: Determining emotional information according to a current joint angle and a current joint angular velocity of a mechanical arm of the robot, a current position and a current velocity of an obstacle, and collision information, wherein the emotional information indicates a degree of stress or fear caused by the obstacle approaching and colliding with the robot; Based on the emotion information, a fast path or a slow path is selected to obtain the motion information of the robot, wherein the fast path simulates the function of the striatum to control the robot to achieve rapid emergency obstacle avoidance, and the slow path simulates the function of the prefrontal lobe to achieve motion trajectory planning of the robot to a target position; Controlling the robot's mechanical arm to move for a first predetermined time period according to the action information; Determining whether the robot arm reaches the target position; In response to the robot arm not reaching the target position, returning to the step of determining emotion information; Wherein, determining the emotion information according to the current joint angle and the current joint angular velocity of the robot's mechanical arm, the current position and the current velocity of the obstacle, and the collision information includes: The current joint angle and current joint angular velocity of the robot's robotic arm, the current position and current velocity of the obstacle, and collision information are input into the emotion model to obtain the emotion information, wherein the emotion model is trained based on a predetermined loss function, and the predetermined loss function is determined based on the output of the emotion model, the distance from the robot to the obstacle, and the collision information, and the emotion model is a forward neural network EmoNet that simulates the amygdala structure.

2. The method according to claim 1, characterized in that Based on the emotion information, selecting a fast path or a slow path to obtain the action information of the robot includes: When the emotion information is less than a first threshold, selecting the slow path to obtain the action information of the robot; When the emotion information is greater than or equal to the first threshold, the fast path is selected to obtain the action information of the robot.

3. The method according to claim 2, characterized in that The selecting the fast path to obtain the action information of the robot includes: The current joint angle, the current joint angular velocity, the target position, the minimum distance between the robot and the obstacle, and the two endpoints corresponding to the minimum distance are input into the fast path model to obtain the action information of the robot, wherein one of the two endpoints is on the robot and the other of the two endpoints is on the obstacle, and the fast path model is trained by the following reward function: Among them, represents the cumulative reward value of no collision in each decision cycle, represents the distance between the end of the robot arm and the target position, represents the movement amplitude of the robot, depends on the minimum distance between the robot and the obstacle and the collision information, and represents the penalty item that the robot's movement exceeds the robot constraint.

4. The method according to claim 2, characterized in that The selecting the slow path to obtain the motion information of the robot includes: The following processing is performed for each node in the first path: when the distance between the current node and the first beacon exceeds a first predetermined step length, the first node is obtained by extending the second predetermined step length in the direction of the first beacon from the current node as the starting point, and the first node is inserted into the first path, wherein the first path is the path for the robot to reach the target position, the first beacon is the first original node passed from the current node along the first path, and the second predetermined step length is smaller than the first predetermined step length; Determine the first path after the node is inserted as the second path; Obtaining an augmented path based on the second path and the current position of the robot; Based on the spline trajectory corresponding to the augmented path, obtaining the motion trajectory of the robot; Based on the motion trajectory, the motion information of the robot is determined.

5. The method according to claim 4, characterized in that The first path is obtained by: Acquiring sampling points using a sampling strategy determined based on the emotion information; Acquire a second node on a search tree of a joint space that is closest to the sampling point, wherein the joint space is a range space of a joint angle of the robot, and the joint space includes an obstacle space; Taking the second node as a starting point, extending a third predetermined step length in the direction of the sampling point to obtain a third node; When the third node does not fall in the obstacle space, obtaining at least one fourth node on the search tree that is within a first preset distance from the third node; Obtaining a node with the shortest path length to the third node from among the at least one fourth node as a parent node of the third node; Based on the parent node, inserting the third node into the search tree; For each first predetermined node in the search tree that is within a second preset distance from the third node, when the corresponding path length when the first predetermined node regards the third node as a parent node is less than a first original path length of the first predetermined node, the third node is regarded as a parent node of the first predetermined node, wherein the first original path length is a path length from the first predetermined node to the root node through the original parent node; For each second predetermined node within a third preset distance from the root node in the search tree, when the corresponding path length when the second predetermined node regards the root node as a parent node is less than a second original path length of the second predetermined node, the root node is regarded as the parent node of the second predetermined node, wherein the second original path length is the path length from the second predetermined node to the root node through the original parent node; If the time from the first acquisition of the sampling point to the current time does not reach the second predetermined time, return to the step of acquiring the sampling point; Based on the adjusted search tree, a first path for the robot to reach the target position within the second predetermined time period is obtained.

6. The method according to claim 5, characterized in that The sampling strategy determined based on the emotion information is used to perform sampling to obtain sampling points, including: Acquiring a sampling point with a first probability in the following manner: acquiring an output of a policy function in the fast-path model, and determining the output as the sampling point, wherein the first probability is determined based on the emotion information; or, The sampling points are obtained with a predetermined probability in the following way: In response to the predetermined probability being a preset second probability, taking the target position as the sampling point; In response to the predetermined probability being a preset third probability, determining a super ellipsoid with the current position and the target position of the robot as foci, uniformly sampling in the union of the super ellipsoid and the joint space with the obstacle space removed, to obtain the sampling point; In response to the predetermined probability being a preset fourth probability, uniformly sampling in the joint space after removing the obstacle space to obtain the sampling points; The sum of the second probability, the third probability and the fourth probability is 1.

7. A robot motion trajectory planning and control device based on multi-channel fusion, characterized in that: include: a determination unit configured to determine emotion information based on a current joint angle and a current joint angular velocity of a robot arm, a current position and a current velocity of an obstacle, and collision information, wherein the emotion information indicates a degree of stress or fear caused by the obstacle approaching and colliding with the robot; A selection unit is configured to select a fast path or a slow path to obtain the action information of the robot based on the emotion information, wherein the fast path simulates the function of the striatum to control the robot to achieve fast emergency obstacle avoidance, and the slow path simulates the function of the prefrontal lobe to achieve motion trajectory planning of the robot to a target position; a control unit configured to control the robot's mechanical arm to move for a first predetermined time period according to the action information; a judgment unit, configured to determine whether the mechanical arm of the robot reaches the target position; a response unit configured to return to the step of determining emotion information in response to the robot arm not reaching the target position; Wherein, the determination unit is also configured to input the current joint angle and current joint angular velocity of the robot's robotic arm, the current position and current velocity of the obstacle, and collision information into the emotion model to obtain the emotion information, wherein the emotion model is trained based on a predetermined loss function, and the predetermined loss function is determined based on the output of the emotion model, the distance from the robot to the obstacle and the collision information, and the emotion model is a forward neural network EmoNet that simulates the amygdala structure.

8. A computer-readable storage medium storing instructions, characterized in that: When the instructions are executed by at least one computing device, the at least one computing device is prompted to execute the robot motion trajectory planning and control method based on multi-path fusion as claimed in any one of claims 1 to 6.

9. A system comprising at least one computing device and at least one storage device storing instructions, characterized in that: When the instructions are executed by the at least one computing device, the at least one computing device is prompted to execute the robot motion trajectory planning and control method based on multi-path fusion as described in any one of claims 1 to 6.

10. A computer program product comprising computer instructions, characterized in that: When the computer instructions are executed by the processor, the robot motion trajectory planning and control method based on multi-path fusion as described in any one of claims 1 to 6 is implemented.

Images