Collision avoidance method and device for kinematic structure, and robot system

Abstract

A method for collision avoidance of a kinematic structure is provided. The method includes receiving pose data indicating a desired pose of the kinematic structure and time data indicating an update interval to be used to control the kinematic structure. The method further includes, for each update interval, performing a constrained optimization process on the inverse kinematics of the desired pose according to the time data to obtain one or more kinematic chain parameters for a collision-free state of the kinematic structure. The method further includes generating control data for controlling the kinematic structure based on the one or more kinematic chain parameters.

Description

[Technical Field]

[0001] The present disclosure relates to collision avoidance for kinematic structures. In particular, examples of the present disclosure relate to methods and apparatuses, robotic systems, non-transitory machine-readable media, and programs for collision avoidance for kinematic structures. [Background technology]

[0002] Kinematic structures, such as robots and computer-animated characters, are often applied in dynamic environments with both stationary and dynamic objects and / or may perform time-varying and unpredictable tasks. To ensure the free movement and / or safety of the kinematic structures in such applications, collisions, i.e., unintended contact, must be avoided. Collision avoidance can be achieved by finding a trajectory for the kinematic structure that globally connects the starting configuration and the final target configuration in a collision-free manner. However, due to the high computational complexity involved in such trajectory planning, practice has shown that such trajectory planning approaches are not suitable for dynamic environments and / or time-critical applications.

[0003] Therefore, there is a need to improve collision avoidance for kinematic structures. Summary of the Invention [Means for solving the problem]

[0004] This need is met by a method for collision avoidance of a kinematic structure, a device for controlling a kinematic structure, a robot system, a non-transitory machine-readable medium, and a program according to the independent claims. Advantageous embodiments are defined in the dependent claims.

[0005] According to a first aspect, the present disclosure provides a method for collision avoidance of a kinematic structure. The method includes receiving pose data indicating a desired pose of the kinematic structure and time data indicating an update interval used to control the kinematic structure. The method further includes, for each update interval, performing a constrained optimization process on the inverse kinematics of the desired pose according to the time data to obtain one or more kinematic chain parameters for a collision-free state of the kinematic structure. The method further includes generating control data for controlling the kinematic structure based on the one or more kinematic chain parameters.

[0006] According to a second aspect, the present disclosure provides an apparatus for collision avoidance of a kinematic structure. The apparatus includes an interface circuit configured to receive pose data indicating a desired pose of the kinematic structure and time data indicating an update interval used to control the kinematic structure. The apparatus further includes a processing circuit configured to perform, for each update interval, a constrained optimization process on the inverse kinematics of the desired pose according to the time data to obtain one or more kinematic chain parameters for a collision-free state of the kinematic structure. The processing circuit is further configured to generate control data for controlling the kinematic structure based on the one or more kinematic chain parameters.

[0007] According to a third aspect, the present disclosure provides a system including an apparatus according to the second aspect and a robotic apparatus configured to operate based on the control data.

[0008] According to a fourth aspect, the present disclosure provides a non-transitory machine-readable medium storing a program having program code for performing the method according to the first aspect, when the program can be run on a processor or programmable hardware.

[0009] According to a fifth aspect, the present disclosure provides a program having a program code for performing the method according to the first aspect when the program can be run on a processor or programmable hardware.

[0010] Some examples of apparatus and / or methods will now be described, by way of example only, with reference to the accompanying figures. [Brief explanation of the drawings]

[0011] [Figure 1] 1 illustrates an exemplary apparatus for collision avoidance of a kinematic structure. [Figure 2] 1 shows an example of self-collision avoidance of a kinematic structure in a system. [Figure 3] 1 shows an example of obstacle collision avoidance of the kinematic structure in the system. [Figure 4] 10 graphically illustrates an exemplary effect of the number of conflicting links on the number of conflicting pairs. [Figure 5] 10 is a graph illustrating an exemplary collision avoidance cost function. [Figure 6] 10 illustrates an exemplary effect of the number of conflicting links on the number of conflicting pairs for obstacle collision avoidance. [Figure 7] 1 illustrates a flowchart of an exemplary method for collision avoidance for a kinematic structure. DETAILED DESCRIPTION OF THE INVENTION

[0012] Some examples are described in more detail with reference to the accompanying drawings. However, other possible examples are not limited to the features of these embodiments described in detail. Other examples may include not only feature variations, but also feature equivalents and alternatives. Furthermore, the terms used herein to describe particular examples should not be construed as limiting further possible examples.

[0013] Throughout the description of the figures, the same or similar reference numbers refer to the same or similar elements and / or features, which may be implemented in the same or modified form while providing the same or similar function. Also, the thickness of lines, layers and / or regions in the figures may be exaggerated for clarity.

[0014] Where two elements A and B are combined using "or", this is understood to disclose all possible combinations, i.e. A only, B only and A and B, unless expressly defined otherwise in individual cases. As alternative expressions for the same combination, "at least one of A and B" or "A and / or B" can be used. This equally applies to combinations of more than two elements.

[0015] Where singular forms such as "a," "an," "the," etc. are used and the use of only a single element is not explicitly or implicitly defined as required, further examples may also use multiple elements to implement the same function. Where a function is described below as being implemented using multiple elements, further examples may implement the same function using a single element or a single processing entity. It will be further understood that when the terms "include," "including," "comprise," and / or "comprising" are used, they are intended to describe the presence of specified features, integers, steps, operations, processes, elements, components, and / or groups thereof, and do not exclude the presence or addition of one or more other features, integers, steps, operations, processes, elements, components, and / or groups thereof.

[0016] FIG. 1 illustrates an exemplary apparatus 100 for collision avoidance of a kinematic structure 200. The kinematic structure 200 can be any type of controllable kinematic device or system that includes or is formed by a kinematic chain, such as a robotic device or system, an animated character used in computer animation or gaming, or the like. This makes the kinematic structure 200 applicable, for example, to industrial environments such as production and logistics, or computer animation or gaming environments. The kinematic chain of the kinematic structure 200 can be configurable, manipulable, and / or controllable to provide or realize a movement, pose, or the like of the kinematic structure 200. For example, the kinematic chain may include one or more of links, joints, actuators, manipulators, or the like, which may be controlled individually or simultaneously. In some examples, the kinematic chain may form, at least in part, the skeleton, or the like, of a computer animation or gaming character that can be configured, manipulated, and / or controlled. However, it should be noted that the kinematic structure 200 is not limited to the aforementioned examples. In general, the apparatus 100 is configured to determine a collision-free state for the kinematic structure 200, particularly its pose, with respect to itself, i.e., for self-collision or joint collision avoidance, and / or with respect to the environment to which the kinematic structure 200 is applied, i.e., for obstacle collision avoidance. In at least some examples, the apparatus 100 is configured to control the kinematic structure 200 according to the determined collision-free state.

[0017] The apparatus 100 includes at least an interface circuit 110 and a processing circuit 120. The processing circuit 120 is operatively connected to the interface circuit 110. The interface circuit 110 is configured to receive pose data 111 indicating a desired pose of the kinematic structure 200 and time data 112 indicating an update interval used to control the kinematic structure 200. The pose data 111 may be received from any suitable data source, such as a controller that may be part of the apparatus 100 or that together with the apparatus 100 forms a system, such as a robotic system, of which the apparatus 100 may be a part. As used herein, a pose may preferably be understood to represent the position and orientation of the kinematic structure 200 in three dimensions or space. The pose may be configured to follow or address a task to be performed by the kinematic structure 200. For example, the task to be performed may include any type of movement in an environment or space, interaction with or manipulation of another object, such as a tool, production material, another computer-animated character, etc., where neither the pose nor the task is limited herein, and / or any other kinematic operation. As used herein, the update interval may be received from any type of timer configured to indicate system time. The update interval may also be referred to as a calculation interval, a time step, a system frequency, etc. For example, the update interval may be a calculation interval, a frequency, etc. at which the kinematic structure 200 and / or a system to which the kinematic structure 200 is applied, such as a robotic system, a computer animation, or a game environment, is updated, e.g., manipulated, or controlled. Optionally, the interface circuit 110 may be configured to receive additional input data. For example, the interface circuit 110 may be configured to receive one or more of information regarding the environment of the kinematic structure 200, the current state of the kinematic structure 200, and the current state of one or more obstacles associated with the kinematic structure 200. Such information may be obtained from one or more sensors, etc.

[0018] Processing circuitry 120 is configured to receive and process attitude data 111 and time data 112. Furthermore, processing circuitry 120 may be configured to receive and process information regarding the environment of kinematic structure 200, the current state of kinematic structure 200, and the current state of one or more obstacles associated with kinematic structure 200, for example, to determine the relative velocity between kinematic structure 200 and one or more obstacles, or other environment-related information. For example, processing circuitry 120 may be a single dedicated processor, a single shared processor, or multiple individual processors, some or all of which may be shared, digital signal processor (DSP) hardware, an application specific integrated circuit (ASIC), a neuromorphic processor, or a field programmable gate array (FPGA). Processing circuitry 120 may optionally be operatively connected to read-only memory (ROM), random access memory (RAM), and / or non-volatile memory, for example, for storing software. Optionally, the processing circuitry 120 may be operatively connected to a network controller that communicates over a network to remotely control the kinematic structure 200. Further optionally, the apparatus 100 may include additional circuitry. It may include a circuit.

[0019] In particular, processing circuitry 120 is configured to perform a constrained optimization process on the inverse kinematics of a desired pose obtained or determined based on pose data 111 according to time data 112 to obtain, for each update interval, one or more kinematic chain parameters for a collision-free state of kinematic structure 200. To this end, processing circuitry 120 may include or utilize a solver coupled to the update interval. Further below, with respect to equations used, the update interval will be denoted by Δt.

[0020] As used herein, a collision-free state of the kinematic structure 200 may refer to any state or configuration of the kinematic structure 200 in which a minimum distance between elements in the kinematic chain is maintained relative to one another to avoid unintended contact, also referred to as self-collision avoidance and / or joint collision avoidance, and / or between the kinematic structure 200 or elements of its kinematic chain and one or more obstacles, i.e., other static or dynamic objects in the environment, also referred to as obstacle collision avoidance. One or more kinematic chain parameters may refer to any parameters related to collision avoidance measures, particularly the control of the kinematic chain, that can be determined and set for motion. For example, in the case of a robotic device (see, e.g., FIGS. 2 and 3 ), this may refer to one or more kinematic chain parameters, e.g., joints, actuators, etc. Also, in the case of a robotic device, the kinematic chain may include one or more links, joints, etc., that can be operated via one or more actuators.

[0021] Furthermore, as used herein, inverse kinematics may be understood as a computational process of determining, calculating, etc., one or more variable kinematic chain parameters configured to place one or more elements of a kinematic chain, such as links, joints, manipulators, or the skeleton of an animation or game character, in a predetermined position and orientation, i.e., pose, relative to the start of the kinematic chain. In the case of a robotic device, the start of the kinematic chain may be the base of the robotic device.

[0022] An optimization process, as used herein, may be understood as a computational process that determines, e.g., finds, a preferred, e.g., best, or generally optimized, solution from a set of feasible alternatives for a collision-free state of the kinematic structure 200. For example, the problem underlying the optimization process may be formulated according to the following mathematical equation:

[0023]

number

[0024] where P denotes a matrix representing one or more objectives, and q T where ∇ denotes a vector representing one or more objectives, G denotes an (inequality) constraint matrix, h denotes an (inequality) constraint vector, A denotes an (equality) constraint matrix, b denotes an (equality) constraint vector, and "dotted q" denotes joint velocities. Thus, the problem is formulated as a convex optimization utilizing exclusively convex cost functions and constraints, resulting in fast computation. In at least some examples, the optimization process for inverse kinematics utilizes quadratic programming utilizing quadratic and linear cost functions. However, it should be noted that the present disclosure is not limited to the above examples. Any other suitable method for formulating the problem may be utilized as well. In general, processing circuitry 120 is configured to determine kinematic chain parameters and / or states of kinematic structure 200 that do not violate one or more constraints for or associated with a collision-free state, in order to obtain optimized one or more kinematic chain parameters by performing an optimization process.

[0025] This allows the inverse kinematics to be solved at the level of joint angular velocities due to the convex relationship between the joint angular velocities and the spatial velocities at any point on the kinematic structure 200 according to the following equation:

[0026]

number

[0027] where "x" denotes the task space velocity, J(q) denotes the Jacobian matrix, and "q" denotes the joint velocity. Note, however, that the present disclosure is not limited to the above example. Any other suitable method for solving the inverse kinematics may be utilized as well.

[0028] For example, the optimization process may include defining and / or solving multiple objectives for or related to at least one or more of self-collision avoidance and / or joint collision avoidance and / or obstacle collision avoidance to obtain optimized one or more kinematic chain parameters for a collision-free state. For example, the optimization process may utilize multiple, i.e., one or more, cost functions and / or constraints to define and / or solve multiple objectives. As an example, the number of objectives of the optimization problem underlying the optimization process performed by processing circuit 120 may be formulated according to the following mathematical equation:

[0029]

number

[0030] where each objective is an independent matrix P i and / or independent vector q i T and / or an independent matrix may be generated. i and q i T The indices are shown to mean: t: (last link or end effector) tracking, sc: self-collision avoidance and / or joint collision avoidance, OC: Obstacle collision avoidance, acc: acceleration and / or motion smoothness, j: joint limit.

[0031] These indices or abbreviations for tracking t, self and / or joint collision avoidance sc, obstacle collision avoidance, acceleration or motion smoothness acc, and joint constraint j are used throughout this disclosure where appropriate. Note that for simple collision avoidance, the terms and / or equations indicated by the indices sc and / or oc are sufficient, and the additional terms and / or equations indicated by t, acc, and / or j are optional. The matrix P i and vector q iT The individual outputs of are the final P matrix and q matrix of the global optimization problem according to Eq. T The individual constraint matrices G may be combined, e.g., added, to create a vector. i and vector h i may also be combined, e.g., added or stacked. Constraining the optimization process may include, for example, utilizing one or more constraints that separate the permissible, collision-free space of the kinematic structure 200 from the non-collision-free, forbidden space of the kinematic structure 200. This is i and q i T , denoted by the above indices sc, i.e., self-collision avoidance, and oc, i.e., obstacle-collision avoidance. Furthermore, one or more constraints may relate to one or more hardware limitations of the kinematic structure 200, such as joint limits, joint angle limits, joint velocity limits, joint acceleration limits, acceleration limits, jerk limits, etc. This is because P i and q i T In at least some examples, one or more constraints may relate to the smoothness of motion desired for operating the kinematic structure 200, e.g., taking into account acceleration limits, jerk limits, etc. This is i and q i T In general, one or more constraints can set boundaries within which the complete state space is available for determining one or more kinematic chain parameters optimized through inverse kinematics.

[0032] Further, processing circuitry 120 is configured to generate the above-mentioned control data 125 for controlling kinematic structure 200 based on the determined one or more kinematic chain parameters. To this end, apparatus 100 may include a data interface to kinematic structure 200 and / or an associated robotic system, computer animation environment or engine, game environment or engine, etc. Control data 125 may be configured to control the motion of kinematic structure 200 under collision avoidance, for example, by controlling corresponding actuators, joints, links, etc. of kinematic structure 200 or an animation or game engine.

[0033] By coupling the optimization-based inverse kinematics to the update interval Δt indicated by the time data 112 and performing its full calculation at each update interval, the kinematic structure 200 can be controlled under collision avoidance even for dynamic tasks and / or in unstructured and unpredictable environments, such as one or more dynamic obstacles (e.g., obstacle 300 in FIG. 2 or FIG. 3 ). Performing the constrained optimization process requires only a small computational effort. Furthermore, by performing the constrained optimization process on the inverse kinematics of a desired pose, the kinematic structure 200 can be controlled to operate at feasibility constraints based on the resulting one or more kinematic chain parameters.

[0034] A more detailed example of the proposed collision avoidance is provided below with reference to FIGS. 2 and 3 , which illustrate a system 1 including apparatus 100 and an exemplary kinematic structure 200. In these examples, kinematic structure 200 is formed as a robotic device including a base (not shown), a first link 210, a second link 220, a third link 230, and an end effector 240, i.e., the last or most distal link from the base of kinematic structure 200. Kinematic structure 200, i.e., the robotic device, includes a number of joints 250 connecting adjacent links 210, 220, 230, and 240 to one another. While kinematic structure 200 according to FIGS. 2 and 3 is a six-axis robotic device, it should be noted that the kinematic structure 200 described herein is not limited thereto, and the proposed collision avoidance can also be applied to robotic devices having other configurations. Furthermore, as described herein, the proposed collision avoidance can also be applied to computer animations or games, and kinematic structure 200 can be a character in a computer animation or game, or the like.

[0035] It should be noted that FIGS. 2 and 3 illustrate the kinematic structure 200 as a collision representation, which can optionally be utilized as a simplified representation, model, etc., of the kinematic structure 200 to minimize computational effort for collision avoidance. To this end, the collision representation of the kinematic structure 200 may include, for example, multiple objects whose distances can be calculated with little computational effort. Optionally, information regarding the gradient of the closest distances from, to, and / or between objects may be determined. For example, the objects may be provided by one or more models of links and / or primitive shapes, e.g., 3D models such as mesh files, that enable efficient distance calculations. By way of example, FIGS. 2 and 3 illustrate a collision representation of four objects, each of whose links 210, 220, 230, and 240 forms one object. However, utilizing a collision representation is not required, and FIGS. 2 and 3 merely serve to illustrate the proposed collision avoidance.

[0036] 2 illustrates an exemplary kinematic structure 200 formed as a robotic device that can be at least partially controlled by apparatus 100. In FIG. 2, arrows indicate three link pair combinations scl, sc2, and sc3, based on which an example of self-collision avoidance, which may include joint collision avoidance, for kinematic structure 200 is described below.

[0037] In general, processing circuitry 120 is configured to penalize the proximity of elements, e.g., links, relative to one another in the kinematic chain of kinematic structure 200 in the optimization process to find one or more optimized kinematic chain parameters. In other words, processing circuitry 120 may be configured to determine one or more kinematic chain parameters for a self-collision-free state, which may be constituted by a collision-free state to be determined for kinematic structure 200 in the optimization process.

[0038] As used herein, the matrix P i and vector q i T The index sc in is related to self-collision avoidance. Self-collisions can occur between links 210, 220, 230, 240, resulting in a large number of link-pair combinations. Accordingly, processing circuitry 120 may be configured to avoid self-collisions between two consecutive links 210, 220, 230, 240 by determining and / or setting respective joint limit constraints in the optimization process. For further link-pair combinations scl, sc2, and sc3, the optimization problem may include constraints according to the following mathematical formula:

[0039]

number

[0040] where d is the current distance and d safety denotes the safety distance, and k scdenotes the self-collision feasibility scalar, Δt denotes the update interval as provided by the time data 124, and "q" denotes the joint velocity. This self-collision avoidance constraint limits how much the minimum distance between any two links, e.g., links 210, 220, 230, and 240, is allowed to change within the update interval Δt. The minimum distance between each link pair can be defined by the points P1 and P2 that are closest to each other for each link pair. The distance traveled within the update interval Δt by applying any joint velocity at the start of the update interval Δt is described by the right-hand side of Inequality 4. The safety distance d safety By defining the distance as d, we can limit this distance to the distance itself, and safety Based on this, processing circuit 120 is configured to establish self-collision avoidance. The feasibility of the aforementioned self-collision constraint is verified when the distance d is less than the safe distance d safety A feasibility scalar k that defines how quickly we are willing to reach sc ≧1, thereby indicating how much joint acceleration is applied to comply with this boundary. However, it should be noted that the present disclosure is not limited to the above example. Other suitable methods for self-collision may also be utilized. It should be noted that the number of further link pair combinations scl, sc2, and sc3, which may also be referred to as the number of collision pairs, may be determined, for example, by the following formula: where n P denotes the number of collision pairs, and n L denotes the number of links in the kinematic structure 200.

[0041]

number

[0042] In Fig. 4, the horizontal axis represents the number of collision links n L The vertical axis represents the number of collision pairs n P As a graph showing the number of collision pairs n P The number of collision links for L This shows the impact of

[0043] Referring again to FIG. 2, by comparing Equation 4 with the optimization problem according to Equation 1, the following equation is obtained for self-collision avoidance:

[0044]

number

[0045] This constraint can provide a feasible self-collision avoidance. However, any cost function can be utilized to minimize the bouncing maneuver of the kinematic structure 200 according to the following equation:

[0046]

number

[0047] where w sc denotes the weight of the self-collision avoidance cost function. Equation 6 is based on the logarithm of the distance, which makes use of the time derivative of the logarithm to create a dependency on the joint angular velocity.

[0048] 5 illustrates the collision avoidance cost function as a graph where the horizontal axis represents distance and the vertical axis represents cost. Referring to FIG. 5, note that configurations with small distances incur exponentially increasing costs, while configurations with large distances incur similarly small costs. The cost function according to Equation 6 penalizes proximity and / or motivates the kinematic structure 200 not to move near the boundaries defined by the constraints of Equation 4, potentially resulting in smoother collision avoidance maneuvers.

[0049] Referring again to FIG. 2, in at least some examples, cost function Equation 6 may be incorporated into the optimization process according to the following equation, in which the gradient of the distance with respect to the joint angle may be determined:

[0050]

number

[0051] The above formula may determine the gradient of the distance with respect to the joint angle. However, in at least some embodiments, a more efficient method for determining the gradient may be utilized according to the following formula:

[0052]

number

[0053] where P1 denotes the first closest point, P2 denotes the second closest point, and J P1 denotes the Jacobian of P1, and J p2 denotes the Jacobian of P2. Note that it is not necessary to subtract all Jacobians. Rather, we can consider only the joint i that can cause relative motion between P1 and P2, i.e., the joint between P1 and P2 in the kinematic chain, distinguishing between revolute and prismatic joint j, according to the following formula:

[0054]

number

[0055] Optionally, by utilizing Equation 9, the determination of the distance gradient can be reduced to just one cross product for the revolute joint between P1 and P2. For prismatic joints, the calculation of the cross product between the joints can even be omitted since the distance gradient can be derived directly from the forward kinematics. Processing circuitry 120 may also be configured to perform the calculation between the joints.

[0056] feasibility scalar k sc can be determined in several ways, for example, the feasibility scalar k sc and / or the feasibility scalar k sc The constant minimum value of can be determined according to the following formula:

[0057]

number

[0058] Alternatively or additionally to Equation 10, the feasibility scalar k sc The value of may be determined without determining the geometric relationship of Equation 10. For example, the feasibility scalar k sc The value of can be determined according to the following formula:

[0059]

number

[0060] Equation 11 considers the highest rate at which the shortest distance can be reduced. The feasibility scalar k sc 11, the joint velocities q can be determined to avoid self-collisions whose implementation is practically feasible for the kinematic structure 200.

[0061] The feasibility scalar k determined according to Equation 11 sc It should be noted that, since k is a constant, at least in certain circumstances or scenarios, the motion of the kinematic structure 200 may be evaluated for the worst case scenario and therefore be overly conservative. Thus, in at least some instances, the feasibility scalar k sc can be determined, e.g., calculated, online by utilizing the actual joint velocities "superscript q" instead of the global worst-case scenario "maximum superscript q" and / or "minimum superscript q". For example, the feasibility scalar k sc An alternative method for determining σ may utilize the following formula:

[0062]

number

[0063] feasibility scalar k scAnother alternative method of determining σ can be done empirically in a simulation.

[0064] 3 illustrates an exemplary kinematic structure 200 formed as a robotic device that can be at least partially controlled by the apparatus 100. In FIG. 3, arrows indicate three link-obstacle combinations oc1, oc2, and oc3 with respect to an exemplary obstacle 300 present in the environment of the kinematic structure 200, based on which an example of obstacle collision avoidance for the kinematic structure 200 will be described below. Each of the link-obstacle combinations oc1, oc2, and oc3 is associated with a collision path between the respective link 220, 230, and 240 and the obstacle 300. It should be noted that obstacle collision avoidance can be performed with respect to one or more stationary and / or dynamic obstacles 300. It should be further noted that the processing circuit 120 can be configured to determine one or more kinematic chain parameters for obstacle collision avoidance states, which can be constituted by the collision avoidance state to be determined for the kinematic structure 200 in the optimization process. In at least some examples, the collision-free state of the kinematic structure 200 to be determined may comprise self-collision avoidance and / or joint collision avoidance and / or obstacle collision avoidance.

[0065] In general, the processing circuitry 120 is configured to penalize the proximity of the kinematic chains of the kinematic structure 200 in the optimization process to obtain an optimized kinematic chain parameter or parameters. For example, this can be expressed by the following mathematical formula:

[0066]

number

[0067] where w ocdenotes the weights of the obstacle collision avoidance cost function. Note that Equations 13 and 14 for obstacle collision avoidance correspond, at least in large part, to Equations 6 and 7 above for self-collision avoidance, and will not be repeated here. Similar to the above for self-collision avoidance, the optimization problem for obstacle collision avoidance may include one or more constraints according to the following mathematical formula:

[0068]

number

[0069] where d0 denotes the current distance to the obstacle 300, and k oc denotes the obstacle collision feasibility scalar. These one or more constraints correspond, at least to a large extent, to the self-collision constraints according to Equations 4 and 5 above. Therefore, reference is made to the above description of self-collision, and the description will not be repeated here.

[0070] It should be noted that obstacle collision avoidance differs from self-collision avoidance in that, for example, the position P and velocity v of the obstacle, i.e., obstacle 300, cannot be influenced by the kinematic structure 200 itself, e.g., its joint angles, etc. Furthermore, it should be noted that when avoiding a collision with an obstacle, e.g., obstacle 300 in FIG. 3 , constraint Equation 15, and optionally cost function Equation 13, are added to the optimization problem in the optimization process for many and all link-obstacle pairs ocl, oc2, oc3 between the kinematic structure 200 and the obstacle 300, excluding the static base links. Only collisions influenced by the joint angles of the kinematic structure 200 may be avoided by the motion of the kinematic structure 200. However, this does not necessarily apply to the static base links of the kinematic structure 200, as they do not need to be considered. Therefore, the number of collision pairs n P For example, the formula n P =n L -1, and n L indicates the number of links.

[0071] In Fig. 6, the horizontal axis represents the number of collision links n L The vertical axis represents the number of collision pairs n P As a graph showing the number of collision links n for obstacle collision avoidance, L is the number of collision pairs n P 3, for obstacle collision avoidance, three constraints, and optionally a cost function, would be added to the optimization problem in the optimization process performed by processing circuitry 120.

[0072] Referring again to FIG. 3, according to the above explanation, the gradient of the distance with respect to the joint angle has only one Jacobian term, namely, with respect to the closest point of the kinematic structure 200, which can be expressed according to the following mathematical formula:

[0073]

number

[0074] where P r denotes the closest point on the kinematic structure 200, and P o indicates the closest point on the obstacle, and J Pr HA P r Denote the Jacobian of v Po is the P on the obstacle, e.g., the obstacle 300 in FIG. o It should be noted that information data regarding an obstacle, e.g., obstacle 300, may be received by processing circuit 120 via interface circuit 110 from any suitable data source, such as one or more sensors configured to indicate the current state of kinematic structure 200 and / or the current state of obstacle 300. Based on such information, processing circuit 120 determines P o , v Po etc. Furthermore, it is noted that the above example may enable collision avoidance with respect to both static and dynamic, e.g., moving, obstacles present in the environment of the kinematic structure 200.

[0075] Feasibility scalar k for obstacle collision avoidanceoc To determine the feasibility scalar for self-collision avoidance, k sc See above for determining the feasibility scalar k of obstacle collision avoidance oc. oc can be determined in a similar or identical manner, and the description thereof will not be repeated here.

[0076] 2 and 3, the optimization process performed by processing circuitry 120 described above can be modified or extended in many ways.

[0077] For example, the optimization process performed by processing circuitry 120 may include tracking the last link of the kinematic chain of kinematic structure 200. In the case of a robotic device such as that shown in Figures 2 and 3, the last link, which is the most distal link from the base of kinematic structure 200, may also be referred to as an end effector, such as end effector 240 shown in Figures 2 and 3. As noted above, P i Indices t and q of i T refers to the tracking of such last link or end effector, which may be any objective and / or constraint in the optimization process performed by processing circuitry 120. For example, tracking of the last link or end effector, e.g., end effector 240 of FIGS. 2 and 3, may be performed according to the following mathematical formula:

[0078]

number

[0079] Here, "superscript x d ” denotes the desired final link or end effector velocity, and k t denotes the proportional gain, and x d denotes the desired last link or end effector pose, x denotes the actual last link or end effector pose, J denotes the Jacobian of the last link or end effector, and W Tdenotes the weighting matrix. Equation 20 represents a quadratic cost function of the error function to be minimized, allowing accurate tracking of the last link or end effector without prohibiting path deviations necessary for collision avoidance. In other words, this cost function includes the error in the velocity of the last link or end effector, and a proportional gain k t > 0. In other words, tracking the last link involves penalizing deviations from one or more of the desired last link velocity and the desired last link or end effector pose.

[0080] In at least some instances, this equation can be expanded, isolating all terms that depend on joint velocities, and compared with Equation 1 above to obtain two cost function terms for the purpose of tracking the last link or end effector according to the following mathematical expressions:

[0081]

number

[0082] This maximum is reached for infinitely large values ​​of "q", which may result in undesirable behavior of the kinematic structure 200. Joint optimization of both terms may result in the desired behavior of minimizing the tracking error of the last link or end effector.

[0083] In a further example, the optimization process performed by processing circuitry 120 can take into account hardware limitations inherent in kinematic structure 200. For example, in the case of a robotic device such as that shown in Figures 2 and 3, kinematic structure 200 may have hardware limitations including, for example, one or more of joint angle limits, joint position limits, joint velocity limits, joint acceleration limits, etc. Thus, the optimization process can optionally consider feasibility while providing self-collision avoidance and / or obstacle collision avoidance.

[0084] Hardware limitations can be respected, for example, by utilizing upper and lower bounds on the joint velocities in the optimization process. i index and q i T where acc denotes acceleration and j denotes joint constraints, which may be any objective and / or constraint in the optimization process.

[0085] As an example, the joint angle limits and joint acceleration limits can first be converted to joint velocities according to the following formula:

[0086]

number

[0087] Essentially, joint angle limits can be treated at least similarly to the collision constraints described above. Instead of the distance between objects, the remaining joint movement in each direction can be determined and converted to allowed joint velocities by the update interval Δt, for example according to the following formula:

[0088]

number

[0089] feasibility scalar k j has a similar or identical meaning as described above for collision avoidance and may be determined, for example, according to the following equation:

[0090]

number

[0091] Alternatively, following the same reasoning as above with respect to Equation 12, the feasibility scalar k j may also be determined according to the following formula:

[0092]

number

[0093] Furthermore, joint angular acceleration limits can be converted to allowable joint velocities by applying the inverse derivative with an appropriate selected order. For example, for second order derivatives, translations can be determined according to the following formula:

[0094]

number

[0095] Furthermore, by inserting lower and upper acceleration limits, rearranging terms and solving for "dotted superscript q(t)", the joint angular velocity limits can be provided according to the following equation:

[0096]

number

[0097] Note that with two velocities for each of the upper and lower bounds of the joint angular position and joint angular acceleration constraints, and the actual constraint for the joint angular velocity, all the constraint velocities can be compared to each other to determine which one is passed to the optimization problem. For example, the upper and lower bounds can be determined according to the following equations:

[0098]

number

[0099] For the lower / upper bounds, we can first consider the threat and select the maximum / minimum speeds among all the minimum / maximum speeds. By passing these to the optimization problem, we can ensure that all limits are always respected. These values ​​are used as bounds on the optimization variables in Equation 23 and Equation 24 above.

[0100] The same procedure may be performed for jerk to account for limitations in actuator dynamics. Instead of taking the first derivative as shown for acceleration in Equation 32, the second backward derivative of the joint angular velocity can be determined.

[0101] In a further example, the optimization process performed by processing circuitry 120 may consider smoothness of movement for controlling kinematic structure 200. For example, smoothness of movement may refer to smoothly controlling the movement of links 210, 220, 230, and 240 of the kinematic structure according to Figures 2 and 3 while providing self-collision avoidance and / or joint collision avoidance and / or obstacle collision avoidance. Thus, in at least some examples, it may be desirable in the optimization process to penalize high acceleration values ​​and / or encourage smooth motion according to the following mathematical formula:

[0102]

number

[0103] where W acc represents a weighting matrix for motion smoothness, and "dotted superscript q(t)" represents the current joint velocity. This formula can be extended to provide two cost terms that can filter potential spikes in joint acceleration, for example, according to the following formula:

[0104]

number

[0105] As explained above, Pi and q i T The index acc in refers to the smoothness of such motion. Note that smoothness of motion is not essential for self-collision avoidance and / or obstacle collision avoidance, but may optionally be considered in the optimization process performed by processing circuit 200.

[0106] To further emphasize the above-mentioned collision avoidance, FIG. 7 illustrates a flowchart of a method 400 for collision avoidance for a kinematic structure. The method includes receiving pose data 410 indicating a desired pose of the kinematic structure and time data indicating an update interval to be used to control the kinematic structure. The method further includes, for each update interval, performing a constrained optimization process 420 on the inverse kinematics of the desired pose according to the time data to obtain one or more kinematic chain parameters for or associated with a collision-free state of the kinematic structure. The method further includes generating control data 430 for controlling the kinematic structure based on the one or more kinematic chain parameters.

[0107] By coupling optimization-based inverse kinematics to update intervals indicated by time data and performing its full calculation at each update interval, the method 400 can enable control of a kinematic structure under collision avoidance even in dynamic tasks and / or in unstructured and unpredictable environments, such as those with one or more dynamic obstacles. Performing the constrained optimization process requires only a small computational effort. Furthermore, by performing the constrained optimization process on the inverse kinematics of a desired pose, the kinematic structure can be controlled to operate within feasibility constraints based on the resulting one or more kinematic chain parameters.

[0108] Details and aspects of method 400 are described in relation to the proposed technology or one or more examples described above (e.g., FIGS. 1-7). Method 400 may include one or more additional optional features corresponding to one or more aspects of the proposed technology or one or more examples described above.

[0109] The following examples relate to further embodiments. (1) A method for collision avoidance of a kinematic structure, comprising: receiving pose data indicative of a desired pose of the kinematic structure and time data indicative of an update interval to be used to control the kinematic structure; for each update interval, performing a constrained optimization process on the inverse kinematics of the desired pose according to the time data to obtain one or more kinematic chain parameters for a collision-free state of the kinematic structure; generating control data for controlling the kinematic structure based on the one or more kinematic chain parameters. (2) The method of (1), wherein the optimization process utilizes one or more constraints that separate the allowed collision-free space of the kinematic structure from the prohibited collision-free space of the kinematic structure. (3) The method of (1) or (2), wherein the optimization process includes solving multiple objectives for one or more of self-collision avoidance, joint collision avoidance, and obstacle collision avoidance to obtain optimized one or more kinematic chain parameters. (4) The method of (3), wherein the optimization process utilizes multiple cost functions and / or constraints to solve multiple objectives. (5) A method according to any one of (1) to (4), wherein the optimization process includes imposing a penalty on determining such kinematic chain parameters and / or states of the kinematic structure that do not violate one or more constraints for a collision-free state in order to obtain the optimized one or more kinematic chain parameters. (6) A method according to any one of (1) to (5), wherein the optimization process includes penalizing the proximity of elements in the kinematic chain of the kinematic structure relative to each other to obtain one or more optimized kinematic chain parameters. (7) The method of (6), wherein penalizing proximity includes limiting the maximum value by which the shortest distance between elements in the kinematic chain is allowed to change within each update interval to obtain optimized one or more kinematic chain parameters. (8) A method according to any one of (1) to (7), wherein the optimization process includes penalizing the proximity of the kinematic structure to an obstacle to obtain an optimized kinematic chain parameter or parameters. (9) The method of (8), wherein penalizing proximity includes limiting the maximum value by which the shortest distance between the kinematic structure and the obstacle is allowed to change within each update interval while taking into account one or more of the movement and direction of the obstacle. (10) A method according to any one of (1) to (9), wherein the optimization process includes imposing a penalty on such kinematic chain parameters and / or states of the kinematic structure that violate one or more constraints due to hardware limitations of the kinematic structure in order to obtain the optimized one or more kinematic chain parameters. (11) The method of (10), wherein the one or more constraints for the hardware limitations include one or more of a joint angle limit, a joint position limit, a joint velocity limit, and a joint acceleration limit. (12) A method according to any one of (1) to (11), wherein the optimization process includes imposing a penalty on such kinematic chain parameters and / or states of the kinematic structure that violate one or more constraints for acceleration limits of the kinematic chains of the kinematic structure in order to obtain optimized one or more kinematic chain parameters. (13) The method of (12), wherein the optimization process includes limiting the acceleration of one or more elements in the kinematic chain of the kinematic structure. (14) A method according to any one of (1) to (13), wherein the optimization process includes tracking the last link of the kinematic chain of the kinematic structure. (15) The method of (14), wherein tracking the last link includes penalizing deviations from one or more of a desired last link velocity and a desired last link pose. (16) A method according to any one of (1) to (15), wherein the method uses quadratic programming to obtain the one or more kinematic chain parameters for each update interval. (17) The method according to any one of (1) to (16), wherein the kinematic structure is a robotic device or part thereof. (18) A method according to any one of (1) to (16), wherein the kinematic structure is a computer animation object. (19) An apparatus for controlling a kinematic structure, comprising: an interface circuit configured to receive pose data indicative of a desired pose of the kinematic structure and time data indicative of an update interval to be used to control the kinematic structure; for each update interval, performing a constrained optimization process on the inverse kinematics of the desired pose according to the time data to obtain one or more kinematic chain parameters for a collision-free state of the kinematic structure; and a processing circuit configured to generate control data for controlling the kinematic structure based on the one or more kinematic chain parameters. (20) The apparatus of (19), wherein the processing circuitry is configured to execute an inverse kinematics solver to output the one or more kinematic chain parameters for each update interval. (21) The device according to any one of (19) and (20), a robotic device configured to operate based on the control data. (22) A non-transitory machine-readable medium storing a program having program code for performing the method according to any one of (1) to (18) when the program is executed on a processor or programmable hardware. (23) A program having a program code for performing the method according to any one of (1) to (18) when the program is executed on a processor or programmable hardware.

[0110] Aspects and features described in connection with a particular one of the foregoing embodiments may be combined with one or more further embodiments to replace identical or similar features of the further embodiments or to introduce additional features into the further embodiments.

[0111] The embodiments may also be or relate to a (computer) program comprising program code for performing one or more of the above-described methods when the program is run on a computer, processor, or other programmable hardware component. Accordingly, the steps, operations, or processes of different ones of the above-described methods may also be performed by a programmed computer, processor, or other programmable hardware component. The embodiments may also cover a machine, a processor, or a program storage device, such as a computer-readable digital data storage medium, that encodes and / or includes machine-executable, processor-executable, or computer-executable programs and instructions. The program storage device may be or include, for example, a digital storage device, a magnetic storage medium, such as a magnetic disk or magnetic tape, a hard disk drive, or an optically readable digital data storage medium. Other examples may include a computer, a processor, a control unit, a (field) programmable logic array ((F)PLA), (F)PGA), a graphics processor unit (GPU), an ASIC, an integrated circuit (IC), or a system-on-chip (SoC) system programmed to perform the above-described method steps.

[0112] Furthermore, it is understood that the disclosure of several steps, processes, operations, or functions disclosed in this specification or claims should not be construed to imply that these operations are necessarily order dependent unless explicitly stated in individual cases or unless required for technical reasons. Thus, the foregoing description does not limit the execution of several steps or functions to a particular order. Furthermore, in further embodiments, a step, function, process, or operation may be included in and / or divided into several sub-steps, functions, processes, or operations.

[0113] When aspects are described in the context of an apparatus or system, these aspects should also be understood as descriptions of the corresponding method. For example, a block, device, or functional aspect of a device or system may correspond to a feature, such as a method step, of a corresponding method. Thus, aspects described in the context of a method should also be understood as descriptions of the corresponding block, corresponding element, characteristic or functional feature of the corresponding device or corresponding system.

[0114] The following claims are incorporated into the detailed description, with each claim standing on its own as a separate example. It should be noted that, although a dependent claim in the claims implies a specific combination with one or more other claims, other examples may include combining a dependent claim with the features of other dependent or independent claims. Such combinations are expressly suggested herein unless it is stated that a specific combination is not intended in a particular case. Furthermore, features of a claim are intended to be included in other independent claims, even if that claim is not directly defined as dependent on those other independent claims.

Claims

1. 1. A method for collision avoidance of a kinematic structure, comprising: receiving pose data indicative of a desired pose of the kinematic structure and time data indicative of an update interval to be used to control the kinematic structure; for each update interval, performing a constrained optimization process on the inverse kinematics of the desired pose according to the time data to obtain one or more kinematic chain parameters for a collision-free state of the kinematic structure; generating control data for controlling the kinematic structure based on the one or more kinematic chain parameters.

2. The method of claim 1 , wherein the optimization process utilizes one or more constraints that separate an allowed collision-free space of the kinematic structure from a prohibited collision-free space of the kinematic structure.

3. 10. The method of claim 1, wherein the optimization process includes solving multiple objectives for one or more of self-collision avoidance, joint collision avoidance, and obstacle collision avoidance to obtain optimized one or more kinematic chain parameters.

4. The method of claim 3 , wherein the optimization process utilizes multiple cost functions and / or constraints to solve multiple objectives.

5. The method of claim 1 , wherein the optimization process includes penalizing determining such kinematic chain parameters and / or states of the kinematic structure that do not violate one or more constraints for a collision-free state in order to obtain the optimized one or more kinematic chain parameters.

6. 2. The method of claim 1, wherein the optimization process includes penalizing the proximity of elements in a kinematic chain of the kinematic structure relative to one another to obtain optimized one or more kinematic chain parameters.

7. The method of claim 1 , wherein the optimization process includes penalizing the proximity of the kinematic structures to obtain an optimized kinematic chain parameter or parameters.

8. 2. The method of claim 1, wherein the optimization process includes imposing a penalty on such kinematic chain parameters and / or states of the kinematic structure that violate one or more constraints due to hardware limitations of the kinematic structure in order to obtain the optimized one or more kinematic chain parameters.

9. The method of claim 8 , wherein the one or more constraints for the hardware limits include one or more of a joint angle limit, a joint position limit, a joint velocity limit, and a joint acceleration limit.

10. 2. The method of claim 1, wherein the optimization process comprises imposing a penalty on such kinematic chain parameters and / or states of the kinematic structure that violate one or more constraints for acceleration limits of the kinematic chains of the kinematic structure in order to obtain the optimized one or more kinematic chain parameters.

11. The method of claim 10 , wherein the optimization process includes limiting the acceleration of one or more elements in the kinematic chain of the kinematic structure.

12. The method of claim 1 , wherein the optimization process includes tracking the last link of a kinematic chain of the kinematic structure.

13. The method of claim 1 , wherein tracking the last link comprises penalizing deviations from one or more of a desired last link velocity and a desired last link pose.

14. The method of claim 1 , wherein the method utilizes quadratic programming to obtain the one or more kinematic chain parameters for each update interval.

15. The method of claim 1 , wherein the kinematic structure is a robotic device or part thereof.

16. 1. An apparatus for controlling a kinematic structure, comprising: an interface circuit configured to receive pose data indicative of a desired pose of the kinematic structure and time data indicative of an update interval to be used to control the kinematic structure; for each update interval, performing a constrained optimization process on the inverse kinematics of the desired pose according to the time data to obtain one or more kinematic chain parameters for a collision-free state of the kinematic structure; and a processing circuit configured to generate control data for controlling the kinematic structure based on the one or more kinematic chain parameters.

17. The apparatus of claim 16 , wherein the processing circuitry is configured to execute an inverse kinematics solver to output the one or more kinematic chain parameters for each update interval.

18. 17. An apparatus according to claim 16; a robotic device configured to operate based on the control data.

19. A non-transitory machine-readable medium storing a program having a program code for performing the method of claim 1 when said program is executed on a processor or programmable hardware.

20. 10. A program having a program code for performing the method according to claim 1 when the program is run on a processor or programmable hardware.

Images