Robot path planning method using static and dynamic collision avoidance in uncertain environments

Abstract

The present invention relates to robot path planning. At a first time instance, depth information of a plurality of obstacles in the robot's environment is obtained. A static distance map is generated based on the depth information. A path for the robot is calculated based on the static distance map. At a second time instance, depth information of one or more obstacles is obtained. A dynamic distance map is generated based on the one or more obstacles, wherein for each obstacle that satisfies a condition: a vibration range of the obstacle is calculated based on the position of the obstacle and the static distance map, and the obstacle is classified as a dynamic obstacle or a static obstacle based on a criterion associated with the vibration range. A repulsion velocity of the robot is calculated based on the dynamic distance map to avoid the dynamic obstacle.

Description

Technical Field

[0001] The present disclosure relates to systems and methods for robotic path planning with obstacle avoidance. In particular, the present disclosure relates to methods for planning the movement of a robotic arm while avoiding static or dynamic obstacles in an uncertain environment. Background Art

[0002] Robot path planning methods try to find a trajectory for the robot to move from an initial position to a target position. For a robot arm with multiple joints, the degrees of freedom (DOF) of the motion is equal to the number of joints. Therefore, for each move, there are as many possible actions as the number of joints. Typically, it may be required that the motion is smooth, the trajectory length is short, there are no singular points in the path, and obstacles are avoided. Existing path planning methods can be classified into two types: grid-based path planning and artificial potential field-based path planning. Grid-based path planning is generally a global path planning method that uses prior environmental information to create the best possible path, while artificial potential field-based path planning is generally a local path planning method that recalculates the path to replace the initial plan to avoid obstacles whose positions change relative to the robot over time. These obstacles may be referred to as dynamic obstacles in the following text. In a grid-based path planning method, the continuous space of joint angles can be discretized into a high-dimensional grid. Path planning can be performed in a grid of joint angle space. In the path planning method based on artificial potential field, the minimum Euclidean distance between the robot and the obstacle in Cartesian space can be determined using the Gilbert-Johnson-Keerthi (GJK) algorithm. The repulsive speed can be generated by processing the Euclidean distance and used to activate the robot arm to avoid collision with the obstacle.

[0003] Grid-based path planning methods such as the A* algorithm explore all possible moves in a grid by minimizing a cost function. The resulting path is unique. However, the planning process can be slow because all neighborhoods in the grid may need to be explored. Therefore, if an obstacle is already close to the robot arm and the movement is fast, such planning methods may not guarantee real-time adjustment of the trajectory to avoid collision with the moving obstacle. Artificial potential field-based path planning methods explore all obstacles within a safe distance margin from the robot arm and generate a repulsion velocity based on the minimum distance to each obstacle to move away from the obstacle. The velocity generation process is fast. However, the robot may be trapped in the middle of an obstacle because the individual repulsion velocities of each obstacle can cancel each other, resulting in a combined zero repulsion velocity, i.e., no motion of the robot.

[0004] Therefore, it is highly desirable to provide a flexible path planning method that can consider both static environment and dynamic obstacles and react to moving obstacles and re-plan the path immediately. Summary of the invention

[0005] The teachings disclosed herein relate to methods, systems, and programming for robot path planning. One aspect of the present disclosure provides a method implemented on a machine for robot path planning, the machine having at least one processor, storage, and a communication platform capable of connecting to a network. The method includes the following steps: at a first time instance, obtaining depth information of multiple obstacles in the robot's environment; generating a static distance map based on the depth information of the multiple obstacles; according to a model, calculating a path for the robot based on the static distance map; at a second time instance, obtaining depth information of one or more obstacles in the robot's environment; generating a dynamic distance map based on one or more obstacles; for each obstacle that meets the conditions in the one or more obstacles: calculating the vibration range of the obstacle based on the position of the obstacle and the static distance map, and classifying the obstacle as one of a dynamic obstacle and a static obstacle based on a criterion associated with the vibration range; and calculating the robot's repulsion velocity based on the dynamic distance map, the repulsion velocity corresponding to the speed at which the robot deviates from the path to avoid each dynamic obstacle in the dynamic obstacles.

[0006] According to one aspect of the present disclosure, a system for robot path planning is provided. The system includes a processor configured to: obtain depth information of multiple obstacles in the robot's environment at a first time instance; generate a static distance map based on the depth information of the multiple obstacles; calculate a path for the robot based on the static distance map according to a model; obtain depth information of one or more obstacles in the robot's environment at a second time instance; generate a dynamic distance map based on one or more obstacles; for each obstacle that meets a condition in the one or more obstacles: calculate a vibration range of the obstacle based on the position of the obstacle and the static distance map, and classify the obstacle into one of a dynamic obstacle and a static obstacle based on a criterion associated with the vibration range; and calculate a repulsion speed of the robot based on the dynamic distance map, the repulsion speed corresponding to the speed at which the robot deviates from the path to avoid each dynamic obstacle in the dynamic obstacles.

[0007] Other concepts relate to software for implementing the present teachings. A software product that complies with this concept includes at least one machine-readable non-transient medium and information carried by the medium. The information carried by the medium may be executable program code data, parameters associated with the executable program code, and / or information related to a user, request, content, or other additional information.

[0008] By one aspect of the present disclosure, a non-transitory machine-readable medium having information for robot path planning recorded thereon is provided, wherein the information, when read by a machine, causes the machine to perform the following steps: at a first time instance, obtain depth information of multiple obstacles in the robot's environment; generate a static distance map based on the depth information of the multiple obstacles; calculate a path for the robot based on the static distance map according to a model; at a second time instance, obtain depth information of one or more obstacles in the robot's environment; generate a dynamic distance map based on the one or more obstacles; for each obstacle in the one or more obstacles that meets a condition: calculate a vibration range of the obstacle based on the position of the obstacle and the static distance map, and classify the obstacle as one of a dynamic obstacle and a static obstacle based on a criterion associated with the vibration range; and calculate a repulsion speed of the robot based on the dynamic distance map, the repulsion speed corresponding to a speed at which the robot deviates from the path to avoid each of the dynamic obstacles.

[0009] Additional advantages and novel features will be set forth in part in the description that follows, and in part will become apparent to those skilled in the art upon examination of the following and accompanying drawings, or may be learned by production or operation of the examples. The advantages of the present teachings may be realized and obtained by practicing or using the various aspects of the methods, tools, and combinations described in the detailed examples discussed below. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] The various aspects of the present disclosure described herein are further described according to exemplary embodiments. These exemplary embodiments will be described in detail with reference to the accompanying drawings. These embodiments are non-limiting exemplary embodiments, in which the same reference numerals represent similar structures throughout the several views of the accompanying drawings, and in which:

[0011] Figure 1 depicts an exemplary system diagram for robot path planning and collision avoidance;

[0012] Figure 2 depicts an exemplary flow chart for robot path planning and collision avoidance;

[0013] Figure 3 depicts an exemplary flow chart for static range map based robot path planning;

[0014] Figure 4 An example of finding a robot representation point is depicted;

[0015] Figure 5 depicts an exemplary flow chart for generating repulsion velocities;

[0016] Figure 6 depicts an exemplary flow chart for generating escape velocity;

[0017] Figure 7 depicts an exemplary flow chart for switching control between path planning and repulsion velocity generation;

[0018] Figure 8 An exemplary flow chart for classifying dynamic obstacle points is depicted; and

[0019] Fig. 9 The architecture of a computer that can be used to implement a special purpose system incorporating the present teachings is depicted. DETAILED DESCRIPTION

[0020] The present disclosure relates to methods and systems for performing robot path planning while avoiding obstacles. Specifically, the present disclosure relates to systems and methods for planning a path for a robot arm having multiple joints while avoiding obstacles in an uncertain environment. An uncertain environment is one in which objects in the environment may change their positions. There may be new objects moving into the environment, or existing objects moving out of the environment. The robot arm described herein is an arm of a robot having multiple segments. The posture (i.e., position and orientation) of the robot arm can be determined by multiple values ​​(e.g., angle values), each value corresponding to an angle formed between consecutive operable segments of the robot arm.

[0021] Figure 1 An exemplary system 100 for facilitating robot path planning (i.e., robot arm path planning) according to an embodiment of the present teachings is shown. The system 100 includes a depth camera 102, a depth acquisition unit 104, a target pose 106, an initial pose 108, a static distance map generation unit 110, a path planning unit 112 based on a static distance map, a dynamic distance map generation unit 114, a repulsion velocity generation unit 116 based on a dynamic distance map, a trap escape unit 118 based on obstacle positions, a control switching unit 120 based on obstacle vibrations, and a robot joint velocity generation unit 122. The output of the joint velocity generation unit is the real-time joint velocity 124 of the robot.

[0022] As described above, according to one embodiment of the present disclosure, a robot arm may have multiple joints. Specifically, the robot arm may include multiple segments or links, wherein the connection between adjacent segments or links is called a joint. A typical robot may have 6 or 7 joints, which means that the robot may have 6 or 7 degrees of freedom (DOF) in movement. The links of the robot are rigid bodies. The representation of the robot arm may include, but is not limited to, a computer-aided design (CAD) model, a basic shape (i.e., a cylinder, a circle), and a limited number of points on the surface of the robot. The surface points used to represent the robot arm are called robot representation points. The initial pose (i.e., position and orientation) 108 and the target pose 106 of the robot may be characterized by the angles formed between the joints (referred to herein as joint angles). It must be understood that the robot arm may include an end effector (i.e., a device or tool connected to the end of the robot arm). The target pose 106 of the joint angle may be calculated from the pose (position and orientation) of the desired end effector based on the robot inverse kinematics. Please note that the initial pose does not necessarily refer only to the fixed starting pose of the robot. It can also refer to any position during the robot motion according to the planned path when a dynamically moving obstacle is detected and the robot motion needs to be replanned. The robot position where the replanning is made will be considered the new initial pose, as it is the starting position for the new trajectory planning. The same applies to the target pose. The target pose can refer to any new target position when it is detected that the target is moving, and therefore replanning is required.

[0023] The structural information of the robot working environment can be perceived by the depth measurement device 102, which includes but is not limited to a depth camera, a stereo vision system, and a laser ranging sensor. The depth acquisition unit 104 can acquire the structural information in the form of depth data. Specifically, objects in the environment can be perceived as a finite number of points, which have a numerical value associated with them, which is the distance or "depth" from the origin of the depth measurement device along the view direction (z direction). The depth acquisition unit can use the depth data to calculate the complete three-dimensional (3D) coordinates (xyz) of the point in the coordinate system of the depth measurement device. After proper calibration, the 3D measurement can be converted to the robot coordinate system. The 3D information can be used to generate a static distance map by the static distance map generation unit 110 and a dynamic distance map by the dynamic distance map generation unit 114. The static distance map can store the minimum distance from any 3D point in the robot space to a static object (referred to as a static obstacle), while the dynamic distance map can store the minimum distance from any 3D point in the robot space to a moving obstacle (hereinafter also referred to as a dynamic obstacle). A static distance map may be generated at the beginning of path planning (e.g., based on depth information acquired at a first instance), while a dynamic distance map may be generated, for example, in a continuous manner at a high frequency (i.e., once per second) in subsequent time instances. The details of how static and dynamic distance maps may be generated are not within the scope of the present invention and are therefore not described herein. Obstacles with a fixed position or a small range of back and forth motion (referred to herein as vibration) may be perceived as static obstacles, while other moving obstacles may be perceived as dynamic obstacles. The vibration range of an obstacle refers to the range of position change. For example, a patient lying on an operating table may be considered to vibrate (or move) within a certain range due to breathing. Due to the small range of motion, such patients may be perceived as static obstacles rather than dynamic obstacles. The details of classifying obstacles into two types will be described in a later portion of this disclosure.

[0024] The path planning unit 112 based on the static distance map can plan a collision-free robot trajectory from the initial posture 108 to the target posture 106. Robot path planning can generally be performed in a discrete space of joint angles. In the discretized joint angle space, the joint angle configuration can also be referred to as a node in a grid of joint angles. Therefore, the initial posture can be referred to as an initial node, and the target posture can be referred to as a target node. The path planning problem is to find a path from an initial node in a grid to a target node in a grid while meeting certain criteria. The path planning unit 112 based on the static distance map can be based on a general grid-based path planning algorithm, such as RRT and A*. The found path can be further smoothed by a path smoothing algorithm to generate a smooth trajectory. The path smoothing algorithm can be an interpolation-based or optimization-based algorithm. As described below, the path planning unit 112 based on the static distance map can be referred to as a path planning unit.

[0025] In parallel with the path planning unit 112, the repulsion speed generation unit 116 based on the dynamic distance map can generate a repulsion speed corresponding to the current posture of the robot when a dynamic obstacle is detected. As mentioned above, when replanning is performed, the current posture may become a new initial posture. In the following, the repulsion speed generation unit 116 based on the dynamic distance map may be referred to as the repulsion speed generation unit. The trap escape unit 118 based on the obstacle position can generate an escape speed greater than the repulsion speed to help the robot escape from the state trapped by the obstacle. The control switching unit 120 based on the obstacle vibration can determine the vibration range of the obstacle point and switch between the path planning unit 112 and the repulsion speed generation unit 116. The details of the path planning unit 112 and the repulsion speed generation unit 116 will be described later in this disclosure. The robot joint speed generation unit 122 can generate real-time robot joint speed based on whether the input is a joint space trajectory (output of 112) or a repulsion speed (output of 116).

[0026] Figure 2An exemplary flow chart for robot path planning and collision avoidance according to one embodiment of the present teaching is shown. At step 202, an initial pose and a target pose of the robot may be obtained. At step 204, depth information of the environment may be obtained by the depth camera 102. The depth information may be converted into 3D structural information of the environment. At step 206, a static distance map may be generated for static obstacles based on the depth information. This may correspond to a time instance at the start. At step 208, a collision-free robot trajectory may be obtained by applying a path planning algorithm based on a static distance map. An embodiment of the path planning algorithm will be described below in the present disclosure. At step 210, a dynamic distance map may be generated. This may correspond to a time instance after the start. The dynamic distance map may be generated at a rate proportional to the speed of movement of the dynamic obstacle, so that the dynamic distance map may capture the distance information between the robot and the dynamic obstacle at an acceptable speed. At step 212, the robot's repulsion velocity may be calculated by using the minimum distance between the robot and the dynamic obstacle point based on the dynamic distance map. At step 214, the relative position between the robot and the obstacle can be used to calculate the escape velocity to help the robot escape from the state of being trapped by the obstacle. When the sum of the repulsive velocities generated by each obstacle point is zero, the robot cannot leave or escape from the obstacle to avoid collision, and the robot is said to be trapped by the obstacle. At step 216, the vibration range of the obstacle point can be determined and used by the control switching unit 120 based on the obstacle vibration to switch between the path planning unit and the repulsive velocity generation unit. At step 218, the real-time joint velocity of the robot can be generated based on the trajectory planning algorithm. Trajectory planning is to find a continuous trajectory of motion rather than a discrete position obtained by path planning. The input of the robot joint velocity generation unit 122 can be a combination of joint space trajectory and repulsive joint velocity. The trajectory planning algorithm can be an algorithm based on a polynomial or a spline.

[0027] Figure 3 An exemplary flowchart of robot path planning based on the static distance map in step 208 is shown. At step 302, the initial node may be set as the current node. At step 304, the neighborhood nodes N = {N1, N2, ..., N K}, where K is the number of neighborhood nodes. The current node can be set as the parent node of the neighborhood node, and the neighborhood node can be set as the child node of the current node. The parent node of the initial node and the child node of the target node can be set to empty. During path planning, if the target node becomes one of the neighborhood nodes N of the node, the planning is completed, and the robot trajectory can be obtained by backtracking from the target node through the parent node until the initial node is reached. At step 306, the path planning search starts from the first neighborhood node N1. At step 308, the number of robot representation points can be calculated based on the minimum distance between the parent node and the obstacle. Intuitively, the use of a dynamic number of representation points can be explained as follows. When the robot is far away from an obstacle, the number of robot representation points may be less, so that collision detection can be performed on a smaller number of points on the robot to reduce planning time. As the robot approaches an obstacle, the number of robot representation points may increase, so that collision detection can become more accurate to allow the robot to pass through narrow paths between obstacles. The representation points can be evenly distributed along the surface of the robot or follow a specific distribution pattern. The details of obtaining robot representation points based on the number of points will be explained later. For the initial node, the minimum distance may not have been obtained before. Therefore, the number of representation points can be set to a predefined maximum value n 最大 The maximum value n can be obtained by adding the maximum number of representation points for each robot link 最大 The maximum number of representation points for each robot link can be expressed as a function of the robot link length as follows:

[0028] n l =L(l)

[0029] Where l is the length of the robot link, and L(l) is a function of l. An exemplary embodiment of a linear function may take the form

[0030]

[0031] where σ is the resolution of the robot links, which can be determined based on the application. Intuitively, if the robot workspace is surrounded by obstacles, then the resolution can be small so that the robot’s representation points are dense enough to perform accurate collision detection and path planning.

[0032] For nodes other than the initial node, the number of representation points can be expressed as a function of the minimum distance from the parent node to the obstacle as follows:

[0033] n=w(d p )

[0034] where d p is the minimum distance from the robot representation point at the parent node to the obstacle, and w(d p ) is dp The distance d p can be defined as the Euclidean distance in Cartesian space, and the function w(c n ) can be the distance d p A linear or parabolic function of . An exemplary embodiment of a linear function may take the following form:

[0035]

[0036] where n 最大 is the maximum number of robot representation points, and d 最大 is the maximum reachability of the robot, i.e. the maximum reachable distance.

[0037] At step 310, the minimum distance between each representation point and the obstacle can be obtained from the static distance map. The threshold t can be determined and expressed as a function of the minimum distance from the robot representation point to the obstacle as follows:

[0038] t=T(d)

[0039] Where d is the minimum distance between the robot and the static obstacle obtained at step 310, and T(d) is a function of d. The function T(d) can be a linear or parabolic function of the distance d. An exemplary embodiment of a linear function can take the following form:

[0040]

[0041] where d 最大 As mentioned above.

[0042] At step 312, it is checked whether the obtained minimum distance is below a threshold value t. Intuitively, when the threshold value is small, the robot can be very close to the obstacle surface. Therefore, if the minimum distance is greater than the threshold value, the neighborhood node can be put into the neighborhood list at step 314 for further inspection later. Otherwise, the current node can be discarded (implying a high collision risk), and at step 316, the neighborhood index can be increased by 1 to go to the next neighborhood node.

[0043] At step 318, check whether the current node has reached the last adjacent node N. K. If not, the above process can be repeated. Otherwise, check whether the target node is in the neighborhood list at step 320. If the target node is not in the neighborhood list, the cost can be calculated based on the cost function of each neighborhood node in the list. The neighborhood node with the minimum cost can be selected from the neighborhood list and set as the current node at step 322 to repeat the path planning process. By one embodiment, the details of the cost function are described below. Assume that the trajectory includes N nodes, 1, 2, ..., N. At any intermediate node n, the cost from the starting node to the nth node can be represented by g(n). The cost g(n) can be defined as the Euclidean distance between the joint angle at the initial node (i.e., the configuration of the end effector) and the joint angle at the nth node. Another cost that provides a cost estimate from the nth node to the target node (N) can be represented by h(n). The cost h(n) can be defined as the Euclidean distance between the joint angle at the nth node and the joint angle at the node N (target node). The total cost function f(n) at the nth node can take the following form:

[0044] f(n)=g(n)+h(n)

[0045] If the target node is in the neighborhood list as checked at step 320 , the robot trajectory can be obtained by tracing back from the target node through the parent nodes to the initial node at step 324 .

[0046] Figure 4 An example of finding robot representation points is shown (step 308). In the accompanying drawings, a cylinder represents a connecting rod of a robot arm. Assume that the number of representation points is 12 and they are evenly distributed. One embodiment of the distribution is to place one-third of the representation points on the upper surface of the connecting rod, one-third of the representation points on the lower surface of the connecting rod, and one-third of the representation points on the side surface of the connecting rod. On the upper surface of the connecting rod, there may be four representation points p1 (402), p2 (404), p3 (406), and p4 (408) evenly distributed at 90° intervals. Similarly, there may be four corresponding points on the lower surface. For example, point p5 (410) corresponds to point p1 (402). The representation points on the side can be determined by dividing the length of the cylinder by a positive number (here 2). Therefore, there may be 4 points on the side surface (P8, P9, P10, and a point that is not visible behind the cylinder). All of the above points can be used as robot representation points.

[0047] Figure 5An exemplary process for step 212 for generating a repulsive speed based on a dynamic distance map is shown. At step 502, a robot representation point may be obtained in the same manner as described above. At step 504, a first robot representation point may be selected. At step 506, a minimum distance between the first robot representation point and an obstacle may be obtained from the dynamic distance map. Obstacles in the dynamic distance map may be based on the present disclosure. Figure 8 The dynamic obstacle classification described in . At step 508, it can be checked whether the minimum distance is less than the safety margin s. If not, then at step 518, the robot representation point index can be increased by 1 to go to the next point. Otherwise, at step 510, the repulsion velocity at the current representation point can be calculated based on the artificial potential field (APF) algorithm. Assume p r is the current representation point, p o is the nearest obstacle point. The direction of the repulsive velocity can be determined from p o to p r A vector whose magnitude v can be in the following form:

[0048]

[0049] where s is the safety margin (the minimum distance allowed between the robot and the obstacle), and d is p r and p o The Euclidean distance between . The indicated point within the safety margin s will be called the critical point of the robot. The point on the obstacle closest to the critical point of the robot is called the critical point of the obstacle. Multiple repulsion velocities corresponding to multiple critical points of the robot may be generated.

[0050] At step 512, the robot rejection joint velocity may be calculated according to the above rejection velocity, as follows:

[0051]

[0052] Where v is the repulsive velocity, and V(v) is a function of v. An exemplary embodiment of the function V(v) may take the following form:

[0053] V(v)=J -1 v

[0054] where J is the partial Jacobian matrix of the robot at the current representation point.

[0055] At step 514, if the robot rejection joint velocity is greater than the maximum safety velocity v s , the robot rejection joint speed can be reduced. The safe speed of the robot arm can be called the maximum speed of the robot arm allowed for the application. The matrix can be determined The joint velocity can be calculated based on the amplitude and the safe velocity vs Recalculate to:

[0056]

[0057] where v 最大 is the maximum amplitude, and R is v 最大 and v s Function R(v 最大 ,v s ) can be the amplitude v 最大 and safe speed v s A linear or parabolic function of . The function R(v 最大 ,v s ) may take the following forms:

[0058]

[0059] At step 516, the joint velocities may be saved in list L. At step 518, the robot representation point index may be increased by 1 to go to the next point. At step 520, it is checked whether the next point has reached the final point. If not, the above process may be repeated. Otherwise, at step 522, the final robot joint velocity is calculated by adding the repelling joint velocities in list L. The robot final joint velocity may be expressed as a function of the individual joint velocities in list L as follows:

[0060]

[0061] Where n is the total number of joint velocities in the list L.

[0062] At step 524, check the final robot joint speed If not, then at step 528, the joint velocity may be sent to the robot joint velocity generation unit 122. Otherwise, at step 526, the escape joint velocity may be calculated so that the robot may be able to escape from the obstacle trap.

[0063] Figure 6An exemplary process for calculating the escape velocity in step 526 is shown. When the sum of all joint velocities in the list L is zero, an obstacle trap may occur. In such a case, the robot may be trapped in the middle of multiple obstacles and cannot avoid colliding with them. At step 602, the safety margin s may be increased to include more obstacles near the robot. The amount of increase may be determined empirically. At step 604, obstacles within the safety margin may be identified. At step 606, a repulsion velocity for avoiding new obstacles may be calculated. This will be used as an escape velocity for escaping the obstacle trap. At step 608, as described above, an escape joint velocity may be calculated based on the repulsion velocity. At step 610, it may be checked whether the escape joint velocity is zero. If not, then at step 612, the escape joint velocity may be sent to the joint velocity generation unit 122. Otherwise, the above process may be repeated by increasing the safety margin (step 602) until a non-zero escape velocity is found.

[0064] Figure 7 An exemplary flow of step 216 for controlling switching based on the vibration range of the obstacle is shown. At step 702, obstacle points within the robot safety margin can be obtained based on the dynamic distance map, and the obstacle points can be classified as static obstacle points or dynamic obstacle points based on the vibration range of the obstacle points. The details of the classification will be described later. At step 704, it is checked whether there are dynamic obstacle points in the robot workspace. If not, the path planning unit can generate a robot trajectory at step 706. At step 708, the robot can follow the planned trajectory. At step 710, it can be checked whether the robot is in the target posture. If so, the robot can stop at step 712. Otherwise, if any dynamic obstacle is within the robot safety margin, it may be necessary to check again. Please note that the presence of dynamic objects depends on time, which means that new dynamic objects may appear at any time. If there are any dynamic obstacles, a repulsion velocity can be generated at step 714. At step 716, the robot moves away at the repulsion velocity so that the dynamic obstacle is outside the safety margin. At step 718, it is checked whether the velocity of the dynamic obstacle is zero. If not, the process of checking whether there is a dynamic obstacle may be repeated. Otherwise, the repulsion speed may be set to zero at step 720 until the dynamic obstacle moves again. The speed of the dynamic obstacle may be checked via the dynamic distance map. If the minimum distance to the dynamic obstacle does not change or changes only within a small range, the speed of the dynamic obstacle may be considered zero.

[0065] Figure 8An exemplary flow of step 702 for classifying obstacle points within the robot safety margin is shown. At step 802, the dynamic distance map may be updated. At step 804, a maximum number of robot representation points may be generated as described above. At step 806, detection of dynamic obstacle points may start from the first robot representation point (the order may be random). At step 808, obstacle points within the robot safety margin may be obtained. This may be performed by reading a value from the dynamic distance map at the robot representation point location and checking the value against the safety margin. At step 810, the first obstacle point is picked up. At step 812, the minimum distance in the static distance map may be obtained at the location of the obstacle point. This minimum distance may be considered as the vibration range of the obstacle point. Intuitively, this minimum distance reflects the possible range of motion of the vibrating obstacle. At step 814, it is checked whether the vibration range is less than a threshold. The threshold may be the maximum possible vibration range of a static obstacle. The maximum vibration range of a static obstacle may be determined based on experiments. If the vibration range is greater than the threshold, the obstacle point can be classified as a dynamic obstacle point and placed in the dynamic point list at step 816. Otherwise, at step 818, check whether the obstacle point has reached the last obstacle point. If not, at step 820, the obstacle point index can be increased by 1, and the above process can be repeated. At step 822, check whether the representation point has reached the last representation point. If not, at step 824, the robot representation point index can be increased by 1, and the above process can be repeated. Otherwise, a dynamic point list can be obtained at step 826. If the list is not empty, it can be used to generate the robot repulsion velocity, such as Figure 7 Depicted.

[0066] Intuitively, the classification can be explained as follows. If an obstacle point vibrates within a small range at a fixed position (such as a person's breathing), it can be perceived as a static obstacle point rather than a dynamic obstacle point, so that no repulsive velocity is generated to move the robot away from the obstacle. Making the robot move according to the patient's breathing may negatively affect the stability of the surgery.

[0067] Fig. 9900 is an illustrative diagram of an exemplary computer system architecture according to various embodiments of this teaching. Such a special-purpose system in conjunction with this teaching has a functional block diagram of a hardware platform including a user interface element. Computer 900 can be a general-purpose computer or a special-purpose computer. Both can be used to implement a special-purpose system for this teaching. Receiver 900 can be used to implement any component described herein. For example, this teaching can be implemented on a computer such as computer 900 via the hardware, software program, firmware, or a combination thereof of the computer. Although only one such computer is shown for convenience, the computer functions related to this teaching described herein can be implemented on several similar platforms in a distributed manner to distribute the processing load.

[0068] The computer 900 may include, for example, a communication port 950 that is connected to and from a network connected thereto to facilitate data communications. The computer 900 also includes a central processing unit (CPU) 920 in the form of one or more processors for executing program instructions. The exemplary computer platform may also include an internal communication bus 910, different forms of program storage and data storage (e.g., disk 970, read-only memory (ROM) 930 or random access memory (RAM) 940), various data files for processing and / or communication by the computer 900, and possible program instructions executed by the CPU 920. The computer 900 may also include an I / O component 960 that supports input / output streams between the computer and other components in the computer (such as, user interface element 980). The computer 900 may also receive programming and data via network communications.

[0069] Thus, aspects of the present teachings as outlined above may be embodied in programming. The programmatic aspects of the technology may be considered to be a "product" or "article of manufacture" typically in the form of executable code and / or associated data executed on or implemented in some type of machine-readable medium. Tangible, non-transitory "storage" type media include any or all of memory or other storage for a computer, processor, or the like, or its associated modules (such as various semiconductor memories, tape drives, disk drives, etc.) that can provide storage for software programming at any time.

[0070] All or part of the software may sometimes be communicated over a network, such as the Internet or various other telecommunication networks. Such communications, for example, may enable software to be loaded from one computer or processor to another, for example, from a server or host computer of a robotic motion system to a computing environment or other system(s) of similar functionality related to path planning, in a hardware platform. Therefore, another type of medium that may carry software elements includes light waves, radio waves, and electromagnetic waves, such as across physical interfaces between local devices, through wired and optical ground networks, and through various air links. The physical elements that carry these waves, such as wired or wireless links, optical links, or the like, may also be considered as the medium that carries the software. As used herein, unless limited to tangible "storage" media, terms such as computer or machine "readable media" refer to any medium that participates in providing instructions to a processor for execution.

[0071] Thus, machine-readable media may take many forms, including but not limited to tangible storage media, carrier media, or physical transmission media. Non-volatile storage media include, for example, optical or magnetic disks that may be used to implement the system shown in the accompanying drawings or any of the components of the system, such as any of the storage devices or the like in any (multiple) computers. Volatile storage media include dynamic memory, such as the main memory of such a computer platform. Tangible transmission media include coaxial cables; copper wire and optical fiber, including the wires that form a bus within a computer system. Carrier transmission media may take the form of electrical or electromagnetic signals, or acoustic or light waves, such as those generated during radio frequency (RF) and infrared (IR) data communications. Thus, common forms of computer readable media include, for example: a floppy disk, a flexible disk, a hard disk, a magnetic tape, any other magnetic medium, a CD-ROM, a DVD or DVD-ROM, any other optical medium, a punched card stock tape, any other physical storage medium with a pattern of holes, a RAM, a PROM and EPROM, a FLASH-EPROM, any other memory chip or cassette, a carrier wave that transports data or instructions, a cable or link that transports such a carrier wave, or any other medium from which a computer can read programming code and / or data. Many of these forms of computer readable media may be involved in carrying one or more sequences of one or more instructions to a physical processor for execution.

[0072] Those skilled in the art will recognize that the teachings herein are amenable to various modifications and / or enhancements. For example, while the implementation of the various components described above may be embodied in a hardware device, it may also be implemented as a software-only solution—e.g., installed on an existing server. Additionally, the robotic motion planning system as disclosed herein may be implemented as firmware, a firmware / software combination, a firmware / hardware combination, or a hardware / firmware / software combination.

[0073] Although the foregoing has described what is considered to constitute the present teaching and / or other examples, it should be understood that various modifications may be made thereto, and the subject matter disclosed herein may be implemented in various forms and examples, and the teachings may be applied to many applications, only some of which have been described herein. The appended claims are intended to claim any and all applications, modifications, and variations that fall within the true scope of the present teachings.

Claims

1. A method implemented on a machine for performing robot path planning, the machine having at least one processor, storage, and a communication platform capable of connecting to a network, the method comprising: At a first time instance, obtaining depth information of a plurality of obstacles in an environment of the robot; generating a static distance map based on the depth information of the plurality of obstacles, the static distance map identifying a minimum distance from any 3D point in the environment to a static obstacle within the plurality of obstacles; calculating, according to the model, a path for the robot based on the static range map; At a second time instance, obtaining depth information of one or more obstacles in an environment of the robot; generating a dynamic distance map based on the one or more obstacles, the dynamic distance map identifying a minimum distance from any 3D point in the robot space to each of the one or more obstacles in motion; For each obstacle of the one or more obstacles that is positioned within a safety margin relative to the robot, performing the following operations, wherein the safety margin is determined based on the dynamic distance map: calculating a vibration range of the obstacle based on the location of the obstacle and the static distance map, the vibration range indicating a minimum distance from the location to a static obstacle within the environment, and classifying the obstacle as one of a dynamic obstacle and a static obstacle based on criteria associated with the vibration range; as well as A repulsion velocity of the robot is calculated based on the dynamic distance map, the repulsion velocity corresponding to a velocity at which the robot deviates from the path to avoid each of the dynamic obstacles.

2. The method according to claim 1, characterized in that In response to the vibration range of the obstacle being greater than a threshold, the obstacle is classified as a dynamic obstacle, and In response to the vibration range being less than the threshold, the obstacle is classified as a static obstacle.

3. The method according to claim 1, characterized in that The robot is a robot arm comprising a plurality of segments, and wherein the repulsive velocity of the robot is calculated relative to each obstacle of the one or more obstacles that is positioned within a safety margin relative to the robot.

4. The method according to claim 3, characterized in that The repulsion velocity of the robot relative to a specific obstacle is calculated based on calculating an individual repulsion velocity of each segment of the robot arm, the individual repulsion velocities being calculated based on the distance of the segment from the specific obstacle.

5. The method of claim 4, further comprising: obtaining the repulsion velocity of the robot as a sum of the respective repulsion velocities of the plurality of segments; determining whether the repulsion speed of the robot satisfies a speed condition; as well as In response to the repulsion velocity of the robot satisfying the velocity condition, an escape velocity of the robot is calculated.

6. The method according to claim 5, characterized in that The velocity condition corresponds to the repulsive velocity of the robot being zero.

7. A non-transitory machine-readable medium having recorded thereon information for robot path planning, characterized in that: The information, when read by a machine, causes the machine to perform the following steps: At a first time instance, obtaining depth information of a plurality of obstacles in an environment of the robot; generating a static distance map based on the depth information of the plurality of obstacles, the static distance map identifying a minimum distance from any 3D point in the environment to a static obstacle within the plurality of obstacles; calculating, according to the model, a path for the robot based on the static distance map; At a second time instance, obtaining depth information of one or more obstacles in an environment of the robot; generating a dynamic distance map based on the one or more obstacles, the dynamic distance map identifying a minimum distance from any 3D point in the robot space to each of the one or more obstacles in motion; For each obstacle of the one or more obstacles that is positioned within a safety margin relative to the robot, performing the following operations, wherein the safety margin is determined based on the dynamic distance map: calculating a vibration range of the obstacle based on the location of the obstacle and the static distance map, the vibration range indicating a minimum distance from the location to a static obstacle within the environment, and classifying the obstacle as one of a dynamic obstacle and a static obstacle based on criteria associated with the vibration range; as well as A repulsion velocity of the robot is calculated based on the dynamic distance map, the repulsion velocity corresponding to a velocity at which the robot deviates from the path to avoid each of the dynamic obstacles.

8. The medium according to claim 7, characterized in that In response to the vibration range of the obstacle being greater than a threshold, the obstacle is classified as a dynamic obstacle, and In response to the vibration range being less than the threshold, the obstacle is classified as a static obstacle.

9. The medium according to claim 7, wherein The robot is a robot arm comprising a plurality of segments, and wherein the repulsive velocity of the robot is calculated relative to each obstacle of the one or more obstacles that is positioned within a safety margin relative to the robot.

10. The medium according to claim 9, wherein The repulsion velocity of the robot relative to a specific obstacle is calculated based on calculating an individual repulsion velocity of each segment of the robot arm, the individual repulsion velocities being calculated based on the distance of the segment from the specific obstacle.

11. The medium of claim 10, further comprising: obtaining the repulsion velocity of the robot as a sum of the respective repulsion velocities of the plurality of segments; determining whether the repulsion speed of the robot satisfies a speed condition; as well as In response to the repulsion velocity of the robot satisfying the velocity condition, an escape velocity of the robot is calculated.

12. The medium according to claim 11, wherein The velocity condition corresponds to the repulsive velocity of the robot being zero.

13. A system for robot path planning, comprising: A processor, the processor being configured to: At a first time instance, obtaining depth information of a plurality of obstacles in an environment of the robot; generating a static distance map based on the depth information of the plurality of obstacles, the static distance map identifying a minimum distance from any 3D point in the environment to a static obstacle within the plurality of obstacles; calculating, according to the model, a path for the robot based on the static range map; At a second time instance, obtaining depth information of one or more obstacles in an environment of the robot; generating a dynamic distance map based on the one or more obstacles, the dynamic distance map identifying a minimum distance from any 3D point in the robot space to each of the one or more obstacles in motion; For each obstacle of the one or more obstacles that is positioned within a safety margin relative to the robot, performing the following operations, wherein the safety margin is determined based on the dynamic distance map: calculating a vibration range of the obstacle based on the location of the obstacle and the static distance map, the vibration range indicating a minimum distance from the location to a static obstacle within the environment, and classifying the obstacles as one of dynamic obstacles and static obstacles based on criteria associated with the vibration range; and calculating a repulsion speed of the robot based on the dynamic distance map, the repulsion speed corresponding to a speed at which the robot deviates from the path to avoid each of the dynamic obstacles.

14. The system of claim 13, wherein: In response to the vibration range of the obstacle being greater than a threshold, the obstacle is classified as a dynamic obstacle, and In response to the vibration range being less than the threshold, the obstacle is classified as a static obstacle.

15. The system of claim 13, wherein: The robot is a robot arm comprising a plurality of segments, and wherein the repulsive velocity of the robot is calculated relative to each obstacle of the one or more obstacles that is positioned within a safety margin relative to the robot.

16. The system of claim 15, wherein: The repulsion velocity of the robot relative to a specific obstacle is calculated based on calculating an individual repulsion velocity of each segment of the robot arm, the individual repulsion velocities being calculated based on the distance of the segment from the specific obstacle.

17. The system of claim 16, wherein: The processor is further configured to: obtaining the repulsion velocity of the robot as a sum of the respective repulsion velocities of the plurality of segments; determining whether the repulsion speed of the robot satisfies a speed condition; as well as In response to the repulsion velocity of the robot satisfying the velocity condition, an escape velocity of the robot is calculated.

Images