Collision avoidance system and method for human commanded systems

Information

  • Patent Grant
  • 8898000
  • Patent Number
    8,898,000
  • Date Filed
    Tuesday, November 8, 2011
    13 years ago
  • Date Issued
    Tuesday, November 25, 2014
    10 years ago
Abstract
A method of implementing an optimal avoidance filter for interposing between a human operator issued movement commands and a corresponding machine control system of a movable machine, for the avoidance of collisions with objects. The method: includes inputting a detailed representation of objects in the vicinity of the movable machine; and formulating a hierarchical set of bounding boxes around the objects. The hierarchical set including refinement details depending on the current positional state of the movable machine, with objects closer to the machine having higher levels of refinement details. The method further includes utilizing the resultant hierarchical set as a set of constraints for a mixed integer optimization problem to determine any alterations to the issued movement commands so as to avoid collisions with any objects.
Description
CROSS-REFERENCE TO RELATED APPLICATIONS

This application is the U. S. national phase of PCT/AU2011/001428 filed Nov. 8, 2011. PCT/AU2011/001428 claims priority to Australian patent application 2010904962 filed Nov. 8, 2010. The disclosures of both AU 2010904962 and PCT/AU2011/001428 are hereby incorporated herein by reference.


FIELD OF THE INVENTION

The present invention relates to collision avoidance systems and methods and, in particular, discloses a system and method for a collision avoidance frame work for human commanded systems such as mining shovels or the like.


REFERENCES

Barraquand, J., Langlois, B. & Latombe, J. (1992), ‘Numerical potential-field techniques for robot path planning’, IEEE Transactions On Systems Man And Cybernetics 22(2), 224-241.


Bellingham, J., Richards, A. & How, J. (2002), Receding horizon control of autonomous vehicles, in ‘Proc. of the American Control Conf’.


Bemporad, A. & Moran, M. (1999), ‘Control of systems integrating logic, dynamics, and constraints’, Automatica 35(3), 407-427.


Blanchini, F. (1999), ‘Set invariance in control [survey paper]’, Automatica 35, 1747-1767.


Blanchini, F. & Miani, S. (2008), Set-Theoretic Methods in Control, Systems & Control: Foundations & Applications, Birkhauser, Boston, Basel, Berlin.


Blanchini, F., Pellegrino, F. A. & Visentini, L. (2004), ‘Control of manipulators in a constrained workspace by the means of linked invariant sets’, International Journal of Robust and Nonlinear Control 14, 1185-1205.


Cohen, J., Lin, M. C., Manocha, D. & Ponamgi, K. (1995), I-collide: Am interactive and exact collision detection system for large-scaled environments, in ‘Proceedings of ACM Int. 3D Graphics Conference’, ACM Press, pp. 189-196.


Culligan, K. (2006), Online trajectory planning for uavs using mixed integer programming, Master's thesis, MIT, Aerospace Control Lab.


Daniel, R. & McAree, P. (1998), ‘Fundamental limits of performance for force reflecting teleoperation’, International Journal Of Robotics Research 17(8), 811-830.


Daniel, R. & McAree, P. (2000), ‘Multivariable stability of force-reflecting teleoperation: Structures of finite and infinite zeros’, International Journal Of Robotics Research 19(3), 203-224.


Floudas, C. (1995), Non-linear and Mixed Integer Optimization: Fundamentals and Applications, Topics in Chemical Engineering, Oxford University Press, New York.


Gottschalk, A. S., Lin, M. C. & Mancha, D. (1996), Obbtree: a hierarchical structure for rapid interference detection, in ‘Proceedings of the 23rd annual conference on Computer graphics and interactive techniques’, ACM Press, pp. 171-180.


ILOG (2007), ILOG CPLEX SYSTEM Version 10.2 Users Guide. 49


Kearney, M., Raković, S. & McAree, P. (2009), Optimal cost control correction: A set-theoretic approach, in ‘Proc. European Control Conference [accepted]’.


Khatib, O. (1986), ‘Real-time obstacle avoidance for manipulators and mobile robots’, International Journal of Robotics Research 5(1), 90-98.


Kuwata, Y. (2003), Real-time Trajectory Design for Unmanned Aerial Vehicles using Receding Horizon Control, Masters, MIT.


Kuwata, Y. (2007), Trajectory planning for unmanned vehicles using robust receding horizon control, Phd, MIT.


Kuwata, Y. & How, J. P. (2004), Three dimensional receding horizon control for uays, in ‘AIAA Guidance, Navigation, and Control Conference’, Providence, R.I., USA.


Kuwata, Y., Richards, A., Schouwenaars, T. & How, J. (2007), ‘Distributed robust receding horizon control for multi-vehicle guidance’, IEEE Transactions on Control Systems Technology 15(4), 627-641.


Latombe, J.-C. (1991), Robot motion planning, Kluwer Academic, Boston, Mass.


LaValle, S. M. (2006), Planning Algorithms, Cambridge University Press, Cambridge, U.K. available at http://planning cs uiuc.edu/.


Maciejowski, J. (2002), Predictive control: with constraints, Pearson Education Limited, Harlow, England.


Mayne, D. Q., Rawlings, J. B., Rao, C. V. & Scokaert, P. O. M. (2000), ‘Constrained model predictive control: Stability and optimality’, Automatica 36(6), 789-814.


McAree, P. & Daniel, R. (2000), ‘Stabilizing impacts in force-reflecting teleoperation using distance-to-impact estimates’, International Journal Of Robotics Research 19(4), 349-364.


Mignone, D. (2001), The REALLY BIG Collection of Logic Propositions and Linear Inequalities, Technical report, Automatic Control Lab, ETH Zurich.


Rakovi¶c, S. & Mayne, D. Q. (2005), Robust time optimal obstacle avoidance problem for constrained discrete time systems, in ‘44th IEEE conference on Decision and Control’, IEEE, Seville, Spain, pp. 981-986.


Raković, S. V., Blanchini, F., E. Cruck & Morari, M. (2007), Robust Obstacle Avoidance for Constrained Linear Discrete Time Systems: A Set-theoretic Approach, in ‘IEEE Conference on Decision and Control’.


Raković, S. V. & Mayne, D. Q. (2007), ‘Robust Model Predictive Control for Obstacle Avoidance: Discrete Time Case’, Lecture Notes in Control and Information Sciences (LNCIS) 358, 617-627.


Ren, J., McIsaac, K. & Patel, R. (2006), ‘Modified Newton's method applied to potential field-based navigation for mobile robots’, IEEE Transactions On Robotics 22(2), 384-391.


Richards, A. G. (2002), Trajectory Optimization using Mixed-Integer Linear Programming, Masters, MIT.


Richards, A. G. (2005), Robust Constrained Model Predictive Control, Phd, MIT.


Richards, A., Kuwata, Y. & How, J. (2003), Experimental demonstration of real-time milp control, in ‘AIAA Guidance, Navigation and Control Conference’, Austin, Tex.


Rossiter, J. (2003), Model-based predictive control: a practical approach, CRC Press, Boca Raton, Fla.


Schouwenaars, T. (2006), Safe Trajectory Planning of Autonomous Vehicles, Phd, MIT.


Sheridan, T. (1993), ‘Space teleoperation through time-delay—review and prognosis’, IEEE Transactions On Robotics And Automation 9(5), 592-606.


Slutski, L. (1998), Remote manipulation systems: quality evaluation and improvement, International series on microprocessor-based and intelligent systems engineering, Kluwer Academic, Dordrecht, the Netherlands.


Smith, Z. V. (2008), Algorithms for Collision Hulls and their Applications to Path Planning in Open-Cut Mining, PhD thesis, University of Queensland, Mechanical Engineering [submitted].


Thompson, R., McAree, P., Daniel, R. & Murray, D. (2005), ‘Operator matching during visually aided teleoperation’, Robotics And Autonomous Systems 50(1), 69-80.


BACKGROUND

In any part human operated machinery environment, in industrial and other environments, it is important for the machinery to avoid collisions with other objects. One important example of such an environment is in an open cut mining excavation environment.



FIG. 1 depicts a mining shovel loading a haul truck. This is a common activity in open-cut mining, but one which carries the significant risk of collision between the shovel and the truck. It would be desirable to have a technology that assists operators of earth-moving equipment to avoid such collisions. However, the need for such a technology arises in more or less the same form in several teleoperation contexts including nuclear decommissioning (Thompson et al. 2005, McAree & Daniel 2000, Daniel & McAree 2000, 1998) and space applications (Sheridan 1993). The aim is to filter the operator command so that the operator's intent is realized while avoiding collisions between the slave and obstacles in its workspace. The problem is characterized by (i) the presence of a human-in-the-loop who provides a command reference to the slave manipulator to achieve some defined task; (ii) significant energy associated with motion of the slave, with a high likelihood for damage-causing impacts between it and obstacles within its workspace; (iii) rate and saturation constraints on inputs states and outputs which limit the rate at which energy can be removed from and injected into the slave; (iv) the slave and workspace obstacles having non-convex geometries; and (v) a requirement for the slave to manoeuvre within concavities of obstacles.


Previous relevant work includes potential-field avoidance methods (Khatib 1986), motion planning (Latombe 1991, LaValle 2006), receding horizon trajectory planning (denoted RHTP) (Bellingham et al. 2002, Richards et al. 2003, Kuwata 2007, Kuwata et al. 2007) and set-theoretic control methods (Raković & Mayne 2005, Blanchini et al. 2004, Raković & Mayne 2007, Raković et al. 2007). Potential field obstacle avoidance methods were first explored in Khatib (1986) and have been applied frequently to obstacle avoidance problems, see for example (Latombe 1991, LaValle 2006, Ren et al. 2006, Barraquand et al. 1992). These methods use potential fields around each obstacle to determine planning or control laws that repel the manipulator. The approach, while conceptually attractive, suffers from the drawback that the potential field does not explicitly take account of the dynamics and performance limitations of the manipulator. Careful crafting of the potential field is required to guarantee avoidance and ensure that no alteration occurs in situations where collisions will not occur, such as moving parallel to an obstacle face. Motion planning methods, by way of contrast, seek to find a path from an initial configuration of a robot to a desired configuration avoiding all obstacles en route. These methods are most commonly used in autonomous robotics (Latombe 1991, LaValle 2006). The main differences between the motion planning and avoidance filtering problems is the objective and the available information: the final goal of the robot is known in the motion planning problem, hence the problem is fully specified, while for the avoidance filtering problem future commands are not known, and the objective is to minimize the alteration from the operator's command.


RHTP, for example, calculates the path to the goal configuration using a receding horizon control framework with the property that each time step, the minimum-cost trajectory to the goal configuration is computed and the first action is taken. This control structure allows for changes to the environment and the goal configuration to occur during the operation. RHTP can be implemented for polytopal obstacles, polytopal system constraints and linear (or piecewise afine) dynamics using MIP, see for example (Bellingham et al. 2002, Richards et al. 2003, Kuwata 2007, Kuwata et al. 2007). Set-theoretic control methods (Blanchini & Miani 2008) have also been applied to obstacle avoidance problems. Dynamic programming-based set iterates, for instance, have been used to robustly drive the state to the origin while avoiding obstacles (Rakovic & Mayne 2005), and linked invariant sets have been used to solve the obstacle avoidance with tracking problem (Blanchini et al. 2004). Both of these methods solve variations of the motion planning problem and, as such, are applicable to the avoidance filtering problem (Kearney et al. 2009). Set-theoretic methods were not considered because any change to the environment requires the re-computation of the sets which define the avoidance control laws, restricting these methods to a static environment. This attribute of set theoretic methods are not compatible with the level of detail strategy necessary to represent non-convex obstacle sets.


SUMMARY

It is an object of the present invention to provide an improved collision avoidance framework for human commanded systems.


In accordance with a first aspect of the present invention, there is provided a method of implementing an optimal avoidance filter for interposing between a human operator issued movement commands and a corresponding machine control system of a movable machine, for the avoidance of collisions with objects, the method comprising: (a) inputting a detailed representation of objects in the vicinity of the movable machine; (b) formulating a hierarchical set of bounding boxes around the objects, the hierarchical set including refinement details depending on the current positional state of the movable machine, with objects closer to the machine having higher levels of refinement details; (c) utilising the resultant hierarchical set as a set of constraints for an optimisation problem to determine any alterations to the issued movement commands so as to avoid collisions with any objects.


Preferably the method also includes the steps of: (d) utilising the predicted future motion to update the hierarchical set off bounding boxes. In some embodiments, the step (c) further can comprise the step of: (i) determining a series of alternative alterations to the issued movement commands, and costing the series in term of magnitude of alteration, and utlising a lower cost alternative alteration. The set of bounding boxes are preferably axially aligned.


The steps (a) to (c) are preferably applied in a continuous iterative manner


The hierarchical set of bounding boxes preferably can include representation of non convex objects, in the form of convexities in the hierarchical set.


The step (b) further can preferably comprise, for any particular time step, culling members of the set that are not reachable in the current time step.


In accordance with a further aspect of the present invention, there is provided an optimal avoidance filter for interposing between a human operator issued movement commands and a corresponding machine control system of a movable machine, for the avoidance of collisions with objects, the optimal avoidance filter comprising: First input means for inputting a detailed representation of objects in the vicinity of the movable machine; Hierarchical bounding box determination means for formulating a hierarchical set of bounding boxes around the objects, the hierarchical set including refinement details depending on the current positional state of the movable machine, with objects closer to the machine having higher levels of refinement details; Optimisation means utilising the resultant hierarchical set as a set of constraints for a mixed integer optimisation problem to determine any alterations to the issued movement commands so as to avoid collisions with any objects, and outputting the alterations to the movement commands.





BRIEF DESCRIPTION OF THE DRAWINGS

Benefits and advantages of the present invention will become apparent to those skilled in the art to which this invention relates from the subsequent description of exemplary embodiments and the appended claims, taken in conjunction with the accompanying drawings, in which:



FIG. 1 illustrates an Electric mining shovel loading a haul truck;



FIG. 2 illustrates a Teleoperated system with the Optimal Avoidance Filter (OAF) interposed between master and slave devices. The OAF calculates an additive modification to the operator command, dependant on the state, and the obstacle set;



FIG. 3 illustrates a convex polytopal obstacle (black), made up from intersection of half spaces. The shaded area indicates the feasible region when a bold obstacle avoidance constraint is active (its corresponding custom character=0). The state (black dot), x, is shown to be in the feasible region;



FIG. 4 illustrates a different level of detail representation for a haul truck tray;



FIG. 5 illustrates the construction of an axially-aligned bounding box hierarchy of a 2D non-convex object;



FIG. 6 illustrates an axial-aligned bounding box BVH—based on the example in FIG. 5;



FIG. 7 illustrates examples of minimum covers generated using the nominal trajectory for different state, command input pairs. The nominal trajectory is given by circles and current position by the square;



FIG. 8 illustrates a comparison of implicit and leaf boxes OAF algorithms from four different starting points and constant commands. The trajectories starting at points 1,2 and 3 stop within concavities of the obstacle in the direction commanded by the operator, while the trajectory from point 4 moves along the side of the obstacles before resuming following the command provided by the operator. In all four of these simulations, the trajectories produced by the leaf node OAF and the implicit OAF correspond.



FIG. 9 illustrates nominal trajectory OAF compared to root box and leaf boxes OAFs from four different starting points and constant commands. Trajectories determined using nominal trajectory and leaf nodes OAF, starting from points 1,2 and 3, correspond. The trajectories starting at point 4 diverge due to the ordering of the branching in the MIP solution;



FIG. 10 illustrates the nominal trajectory OAF and leaf boxes OAF trajectories can be seen diverging. The dashed line, indicating the nominal path, shows that at the point of divergence the cost of diverting to the left and right were equal;



FIG. 11 illustrates a comparison of simulation times for the different OAF algorithms and BVH complexities.



FIG. 12 illustrates the simplification of a BVH using Propositions 5.1 and 5.2.



FIG. 13 illustrates three different intersection situations for reachable constraints. Bold lines indicate reachable constraints, while dashed lines represent unreachable constraints.



FIG. 14 illustrates comparison between trajectories generated by unmodified OAF algorithms, and those that use the reachable constraint method to determine constraints. With the exception of situation 4 in (a), which is due to the order of branching in the MIQP solver (as in FIG. 10), all of the trajectories correspond.



FIG. 15 illustrates a Truck tray (left) and dipper (right).



FIG. 16 illustrates a Leaf boxes approximation to the truck tray-dipper obstacle set (256 boxes).



FIG. 17 illustrates a simulation of loading pass using an OAF in the state space.



FIG. 18 illustrates a simulation of loading pass using an OAF.





DETAILED DESCRIPTION

Preferred embodiments of the invention will now be described, by way of example only, with reference to the accompanying drawings.


The preferred embodiment utilises an optimal avoidance filter (or OAF) and it is synthesized using a receding horizon control (RHC) framework in which the control action is determined by predicting the future evolution of the system over a given horizon, optimizing the control sequence over the horizon to obtain the most desirable future system evolution, and applying the first control action in the optimized control sequence (Rossiter 2003, Maciejowski 2002). RHC has two attributes that are advantageous when applied to the avoidance filtering problem. First, the predictive nature of receding horizon control allows the constraints associated with the slave manipulator, e.g. actuator torque and speed constraints, to be explicitly taken into account when determining the control action. Second, the future avoidance of obstacles can be guaranteed, even when the operator's future commands are not known, provided the avoidance filter is recursively feasible (Rossiter 2003). A significant challenge is the representation of obstacles. Abstractly, there exists a collision set Cobs in the configuration space of the slave that is to be manoeuvred around, defined as the set of configurations where the slave intersects with workspace obstacles (or itself). Cobs is well defined mathematically, but difficult to compute. We utilize recent work by Smith (2008) who has presented algorithms for representing Cobs in a form that is suitable for incorporation into a receding horizon control framework. In particular, these algorithms approximate Cobs as a hierarchy of axially-aligned bounding-boxes. The OAF formulation draws an appropriate representation from this hierarchy and expresses the resulting constraints as a family of mixed integer linear inequalities to be satisfied. The OAF is synthesized as a mixed integer program (MIP) using the approximation of Cobs, denoted Ĉobs drawn by the OAF from the hierarchy of axially-aligned bounding boxes. The requirement to run in real-time places restrictions on the level and apportioning of geometric detail in Ĉobs. Intuitively, higher detail is desired in regions where the slave manipulator currently is and is likely to go within the prediction horizon, while the remainder of Ĉobs can be represented more coarsely.


The preferred embodiment is directed to the complimentary questions of (i) how to draw an efficient representation of Cobs from a level of detail representation, at each time step given the current state of the slave manipulator and operator command and (ii) how to embed this level-of-detail within the OAF MIP. Two strategies are examined. The first looks to determine the most appropriate Ĉobs as part of the OAF MIP. The second looks to use a prediction of future motion to determine a level-of-detail approximation that is fit-for-purpose and provide this to the OAF MIP. Both strategies produce similar solutions, but the second is shown to have a significantly lower computational cost. Further reduction in computational cost is achieved by removing those obstacle avoidance constraints than cannot be active on the prediction horizon from the OAF MIP. Restrictions are identified on how Ĉobs can change between samples to ensure that the OAF remains recursively feasible. A simplified simulation example, based on the shovel-truck avoidance problem, is presented to show the applicability of the methods presented to the motivating problem.


The proposed OAF follows a similar structure to RHTP: a framework based on receding horizon control with avoidance constraints represented using mixed integer inequalities, but will differ in that it will calculate an additive modification to the operator's current command (along the lines of the potential field avoidance method), rather than the command to drive the state to a defined goal configuration.


2 Structure of the OAF



FIG. 2 shows schematically a human-operated system made up of

    • A slave manipulator which receives an input to perform a desired task. This slave manipulator may include a pre-existing control system. The inputs and states are subject to constraints. The input is often, though not always, a rate command.
    • An input device, through which a human operator provides a command input to the slave manipulator. Joysticks are a common form of input device and can be quite sophisticated, e.g. in force reaction applications (Slutski 1998)
    • The environment, which contain obstacles whose location and geometry are known. In general, the obstacles have non-convex geometry. It is desired that the slave device does not collide with any of the obstacles in the environment.


The OAF is interposed between the input device and the slave manipulator (as shown in FIG. 2) and computes and additive alteration to the operator reference so that the slave avoids collision with obstacles. The OAF also ensures that the constraints of the slave manipulator are satisfied. The OAF objective function is chosen to ensure that the alteration from the operator command is minimal, although alternative objectives could be chosen within this framework.


2.1 Notation and Definitions






    • Variables are represented using the following convention: spaces (state and input) are represented using capital letters, e.g. X;U. Sets are represented using upper case letter, e.g. P;O. Members of sets and spaces are represented using lower case italics, e.g. x; u; v. Convex polytopes are represented as uppercase characters, e.g. P;O. Problem descriptions (mathematical programs) will use uppercase characters, e.g. P.

    • The slave dynamics are represented using a non-linear, time-invariant discrete-time System:

      χ+=f(χ,u),   (2.1)

      where χεcustom charactern is the current state of the system, uεRm is the current input and χ+is the successor state. The state at time-step k is denoted χk.

    • The slave manipulator has constraints on the states and the inputs, which, in general, are mixed. The admissible set of inputs and states satisfy:

      (χ,u)εcustom charactercustom character×custom character  (2.2)

      where X⊂custom charactern is the set of admissible states and U⊂custom characterm is the set of admissible inputs. The obstacle set custom character⊂X is a mapping from Cobs to the state space, in which it is desired that the state evolution never enters:

      χkcustom character, ∀k=1, 2,  (2.3)





A formal definition of custom character is

custom character:={χεcustom character:Cp(χ)εCobs},   (2.4)

where Cp(.) maps the state space into a configuration of the slave manipulator. Correspondingly, the representation of obstacle Cj within the state space is:

custom characterj={χεcustom character:Cp(χ)εcustom characterj},   (2.5)

and the approximation of each set is given respectively by Ĉj and Ôj . For the examples in this description, several of the states make up the configuration space, hence Cp(χ)=Cpχ where Cp is an appropriately sized matrix. custom characteri is used to represent part of the obstacles set the is represented by a convex polytope, such that custom charactercustom charactericustom characteri

    • XT is a positively invariant set and κ·(:) an associated feedback control law that must meet the following invariance and admissibility conditions (Blanchini 1999):

      ∀χεXT, f(χ,κ(χ))εXT,
      ∀χεXT, (χ,κ(χ))εcustom character.
    • The sequence of inputs generated by the operator, {ũ0, ũ1, . . . ũn} is denoted by ũN. The infinite sequence of future inputs {ũ0, ũ1, . . . } is denoted by ũ. -The OAF algorithm computes a sequence of alterations {v0, v1 . . . vn} is denoted by vn. The infinite sequence, {v0, v1 . . . } is denoted by v.


      2.2 The Optimal Avoidance Filter Algorithm


The OAF algorithm calculates an additive alteration vk, to the command provided by the operator ũk, to determine the filtered system input

ukk+vk,   (2.6)

such that (i) collisions with obstacles are avoided (χk+Icustom character), and (ii) the system constraints are satisfied ((χk+i; uk+icustom character), now and for all future time steps (i>=0). Additionally, the OAF algorithm minimizes the alterations to the operator command by costing the alterations using an appropriate norm. As posed, this problem is acausal, since the future operator input sequence {ũ0, ũ1, . . . } is unknown.


The OAF mathematical program PN(χ, ũ), which is solved online in a receding horizon fashion, accounts for constraints as it is derived from an N-step constrained optimal control problem, and causality is obtained by using an appropriate model to predict future operator inputs. The terminal state of the OAF mathematical program is constrained to enter a collision-free positively invariant set, χT:

χNεXT,   (2.7)

to obtain, using standard results in receding horizon control literature (Mayne et al. 2000, Rossiter 2003), a guaranteed stable (Mayne et al. 2000), and recursively feasible (Rossiter 2003) receding horizon controller. The obstacle avoidance constraints incorporated into the OAF mathematical program are

χkcustom character, ∀k=0, . . . , N,   (2.8)
XTcustom character=∅.   (2.9)


The operator command prediction model used holds the current operator's command constant over the planning horizon, and sets the command input to be zero for k>N:

ũk+ik, i=1, . . . N−1,   (2.10)
ũk+i=0, i≧N.   (2.11)


The invariant set feedback control law is then considered to be an alteration:

vk=κ(χk), ∀k>N.   (2.12)


In this prediction model, the likelihood that the prediction is correct decreases into the future. This attribute can be included in the formulation of PN by discounting the cost function:











v

N
-
1


=

arg







min
v






k
=
0


N
-
1





γ
k





v
k








,




(
2.13
)








where 0≦γ≦1 is the discount factor. The resulting OAF mathematical program PN(χ, ũ), can be posed as:










v

N
-
1


=

arg







min
v






k
=
0


N
-
1





γ
k





v
k











(
2.14
)











x

k
+
1


=

f


(


x
k

,


u
~

+

v
k



)



,







k

=
0

,





,

N
-
1








(
2.15
)











(


x
k

,


u
~

+

v
k



)


𝒫

,







k

=
0

,





,

N
-
1








(
2.16
)











x
k


𝒪

,







k

=
1

,





,

N
-
1








(
2.17
)








x
N



χ
T


,




(
2.18
)








χ
T


𝒪

=


.





(
2.19
)







The OAF algorithm is implemented by at each time step by:

    • 1. Measuring the current state, χk, and the current operator command input, ũk.
    • 2. Solving the OAF mathematical program PN(χ, ũ), to obtain the sequence of alterations, vN−1.
    • 3. Setting the first element of vN−1, to be vk.
    • 4. Sending the filtered input command, ukk.+vk, to the slave device.


      3 The OAF for Convex Polytopal Obstacles


Let the obstacle set, custom character, be composed of NO convex polytopes:









𝒪
=





N
o



j
=
1





O
j

.






(
3.1
)







Each custom characterj can be described as the intersection of Nh (finite) open half-spaces as shown in FIG. 3. That is










O
j

=






N
h



(

O
j

)




j
=
1





{

x


𝕏
:



a

i
,
j

T


x

<

b

i
,
j





}

.






(
3.2
)







Noting that {χ:−ai,jTχ≦−bij} is the complement of {χ: −ai,jTχ<bij}, the obstacle avoidance constraint χ∉Oj, can be represented as:









x







N
h



(

O
j

)




i
=
1





{

x


𝕏
:



-

a

i
,
j

T



x



-

b

i
,
j






}

.






(
3.3
)







Equation 3.3 is non-convex and can also be expressed as a collection of OR (written custom character) constraints:

[−a1jχ≦−b1j]custom character[−a2jχ≦−b2j]custom character . . . custom character[−aNh(Oj)jx≦−bNk(Oj) j].   (3.4)


This structure is exploited in (Richards 2002, Kuwata 2003) where Eqn. 3.4 is transformed into a set of mixed-integer linear inequalities using the so-called big-M method (Bemporad & Morari 1999, Mignone 2001) by introducing a scalar M, such that

custom charactercustom character{102 εcustom charactern:−ai,jTχ≦−bi,j+M}, ∀i,j,   (3.5)

and a binary decision variable (aijk) for each of the half-spaces in Oj. The resulting mixed-integer linear inequalities are:















-

a

i
,
j

T



x




-

b

i
,
j



+

M






α

i
,
j
,
k





,







i

=
1

,





,


N
h



(

O
j

)


,







(
3.6
)











j
=
1



N
h



(

O
j

)





α

i
,
j
,
k







N
h



(

O
j

)


-
1


,




(
3.7
)











α

i
,
j
,
k




{

0
,
1

}


,






1

,





,



N
h



(

O
j

)


.








(
3.8
)








where k represents the prediction time step in PN. When a constraint is active α=0; when inactive α=1. Equation 3.5 ensures that when a constraint is inactive, custom character is a subset of the half-space induced by the constraint. Equation 3.7 ensures that the obstacle avoidance constraints for Oj (Eqns. 3.6 to 3.8) are satisfied by forcing at least one of the avoidance constraints of Oj to be active. If the slave dynamics are linear, its system constraints polytopal, and the obstacle set, O, made up of polytopal obstacles, then the OAF can be posed as the following MIP:















v

N
-
1


=

arg







min
v






k
=
0


N
-
1





γ
k





v
k












(
3.9
)
















x

k
+
1


=


Ax
k

+

B


(



u
~

k

+

v
k


)




,








k

=
0

,





,

N
-
1








(
3.10
)
















(


x
k

,



u
~

k

+

v
k



)


𝒫

,








k

=
0

,





,

N
-
1








(
3.11
)












a
ij



x
k





b

i
,
j


+

M






α

i
,
j
,
k





,










i

=
1

,





,


N
h



(

O
j

)


,







j

=
1

,





,

N
o

,

























k

=
1

,





,

N
-
1






(
3.12
)














i
=
1



N
h



(

O
j

)





α

i
,
j
,
k







N
h



(

O
j

)


-
1


,







j

=
1

,





,

N
o

,







k

=
1

,





,

N
-
1








(
3.13
)













x
N



χ
T


,





(
3.14
)














χ
T


𝒪

=


,





(
3.15
)








given that an appropriate norm is chosen for Eqn. 3.9. The solution of Eqns. 3.9 to 3.15 is NP-hard (Floudas 1995), with a worst-case bound on the computational cost that is exponential in the number of binary decision variables:










(

N
-
1

)






j
=
1


N
o






N
h



(

O
i

)


.






(
3.16
)







Equation 3.16 does not include the additional binary variables required to represent the invariant set obstacle avoidance constraint (Eqn. 3.15), as this depends on the choice of invariant set. This additional number may range from zero, for a fixed invariant set (XTcustom character/custom character), to a number that is arbitrarily large for an invariant set parameterized by χN.


3.1 Using Axial-Aligned Bounding Boxes to Represent Obstacles


In previous work, Kuwata & How (2004), Richards (2005), Richards et al. (2003) have used axially-aligned bounding boxes (abbreviated as ABBs) to represent (or bound) obstacles, or parts thereof. An ABB is represented by the maximum and minimum bounds in each axis of the obstacle:

Bj:=[χmin,j, χmax,j]×[custom charactermin,j, custom charactermax,j]×  (3.17)


The mixed-integer inequalities for the avoidance of an ABB obstacle are given by:









x



x

min
,
j


+

M






α

1
,
j
,
k








(
3.18
)







-
x




-

x

max
,
j



+

M






α

2
,
j
,
k








(
3.19
)






y



y

min
,
j


+

M






α

3
,
j
,
k








(
3.20
)








-
y




-

y

max
,
j



+

M






α

4
,
j
,
k















(
3.21
)










i
=
1


2






N
D





α

i
,
j
,
k






2






N
D


-
1





(
3.22
)








where ND is the number of dimensions in which the obstacle is defined (usually 2D or 3D). 2ND binary variables are required for each ABB-obstacle.


Extension to Non-Convex Obstacles Using Bounding Volume Hierarchies


One strategy that extends the OAF to avoid non-convex obstacles is to convexify them, and avoid the resulting convex representation. The two most common convex representations for non-convex obstacles are the convex decomposition and the convex hull. The convex decomposition represents a non-convex obstacle as a number of convex regions Pi;j, such that custom characterj=∪Pij, ∀j, and the convex hull of an obstacle is the smallest convex set that contains the obstacle. A major downside of using a convex decomposition of the object is that it contains a lot of detail, hence it is computationally expensive representation, while the convex hull representation, although computationally less expensive, does not allow the slave to move within concavities of the non-convex obstacle. Schouwenaars (2006) has modified the convex hull representation to include convex polytopal safe zones within the convex hull that allow movement into the concavities, but this increases the complexity of the representation. Furthermore each of these representations are static, and consequently may not be the most efficient representation in a given situation (as represented by the state, command input pair).


The preferred embodiment utilises a level-of-detail approach for avoiding non-convex obstacles which utilizes representations drawn from bounding volume hierarchies of each obstacle. FIG. 4 illustrates this idea showing several different level of detail representations of a haul truck tray, from coarsest to finest. The appropriate level-of-detail representation of the obstacle set is chosen such that the cost of computing the alteration vk is reduced when compared to using the highest detail representation available, while not significantly changing the resulting alteration. It is necessary to ‘trade-off’ between these two objectives.


4.1 Representation of Non-Convex Obstacles Using Bounding Volume Hierarchies


In prior work, BVH's have been used to determine whether arbitrary geometric models of objects intersect (Gottschalk et al. 1996, Cohen et al. 1995). A BVH is constructed by recursively bounding and partitioning the geometry of an obstacle and storing the resulting bounding volumes in a binary tree (Gottschalk et al. 1996). This construction is initiated by determining an ABB (or another chosen volume) that bounds the entire obstacle. This box is the root box (ABB) of the obstacle. The geometry of the obstacle is then subdivided along the centre of the longest side of the root ABB into two sub-geometries, which are in turn bounded with an ABB and stored in the binary tree. This ‘bound-and-split’ process is recursively applied to the leaf boxes of the BVH until either a minimum geometry size is achieved or further recursion does not improve precision. FIGS. 5 and 6 shows the construction of a BVH for an arbitrary closed 2D obstacle. ABBs are chosen because they are simple and lead to efficient Minkowski sum operations (Smith 2008). BVHs composed of oriented bounding boxes (Gottschalk et al. 1996) could also be used as an alternative level-of-detail representation. A union the ABBs selected from the BVH of a specific obstacle (custom characterj) must be a superset of that obstacle, specifically a cover.


Definition A cover custom characterj is a collection of boxes, B, from the BVH of custom characterj, such that:












𝒪
^

j

=






(

l
,
m

)



I
j





B

l
,
m





𝒪
j



,




(
4.1
)








where l indicates the level of detail (starting with 1 for the root node), and m indicates the node within the level. Bl;m is a particular box within the BVH. The index set, Ij , indicates which boxes from the BVH are included in the cover representing custom characterj. A further requirement is that no superfluous boxes should be included in the cover, i.e. boxes that can be removed where the remnant remains a cover. If this requirement holds, the cover is minimal. A minimal cover of an obstacle is a cover such that if any of the boxes (ABBs) are removed, it is no longer a cover. The non-convex OAF algorithm will choose a minimal cover as the representation for each obstacle, based upon the current state and operator command. The following proposition allows for the synthesis of minimal cover selection algorithms that recurse down the BVH:

    • Proposition 4.1 There is a single member of the minimal cover on each branch of the tree (path from root box to a particular leaf box).


The leaf boxes of the BVH form a partition of the obstacle:







𝒪
j

=




m
=
1


2


N
L

-
1





𝒢


(

B


N
L

,
m


)








where custom character(.), is the geometry that is bounded by a given box, and custom character(BNL;m1)∩custom character(BNL;m2)=∅, ∀m1≠m2. The geometry in each of the leaf boxes is only bounded by its ancestor boxes, i.e. custom character(BNL;m)custom characterBl, ∀l=1, . . . , NL−1 only, where (.) indicates the appropriate ancestor box for each level. These boxes are found on the branch of the tree that goes from the root box to the given leaf box. Hence, to cover the entire obstacle, it is necessary for boxes on each branch of the tree to be included in the cover. If there is more than one box on a branch, then the box that is a descendant of the other box is superfluous and can be removed. The minimal cover can be chosen in two ways: implicitly as part of the optimization solution or explicitly using a static or adaptive rule. Approaches to each are now considered.


4.2 Implicit Non-Convex OAF


In the implicit non-convex OAF algorithm, the entire BVH is included in the OAF MIP and the coarsest minimal cover that is feasible with respect to the optimal trajectory is selected during the optimization. The selection of the minimal cover is incorporated into the OAF MIP by allocating minimal cover-selection binary decision variables δl,m,kε{0,1}, to each box in the BVH that has children, and by adding a minimal cover selection function (logic) for each box, βl,ml,m,k) to the right-hand side of the constraint relaxation inequality (3.22),














i
=
1


2


N
D





α

i
,
j
,
k






2


N
D


-
1
+


β

l
,
m




(

δ
k

)




,




(
4.2
)








where δk is the vector of minimal cover selection binary variables for time k. The minimum cover selection function determines whether the box is in the minimal cover based on δk: If βl,mk)=0, the box is a member of the minimal cover; if βl,mk)≧1, it is not. For βl,mk)=1, the constraint relaxation inequality becomes:














i
=
1


2


N
D





α

i
,
j
,
k





2


N
D



,




(
4.3
)








allowing all of the avoidance constraints for the box to be relaxed to the entire constraint set. The OAF objective function is modified by placing a small cost on the minimal cover binary decision variables such that finer detail will only be selected if a reduction of the trajectory cost (the unmodified objective function) results. The minimal cover selection function for each box is composed of an ancestor minimal cover selection function, βl,mk)≧0 and a descendent minimal cover selection function, βl,mk)≧0, both of which must equal zero if the box is in the minimal cover. The minimal cover selection function becomes:

βl,mk)= βl,mk)+{tilde over (β)}l,mk)   (4.4)


The ancestor component ensures that the box can only be a member of the minimal cover if none of its ancestors are in the minimal cover (by Proposition 4.1). The descendent component determines whether the box, or some of its descendants are part of the minimal cover, given that βl,mk)=0. The descendant minimal cover selection algorithm for boxes with children is given by:

{tilde over (β)}l,mk)=δl,m,k.   (4.5)


The box may be selected for the minimal cover (dependant on the ancestor part of the selection function) if δl,m,k=0, and if δl,m,k=1, Bl,m will be relaxed in favor of its descendants. By Proposition 4.1, βNL,mk)=0 for the leaf boxes of the BVH, since if none of the ancestors of a leaf box are in the minimal cover, then the leaf box must be in the minimal cover. The ancestor minimal cover selection function is given by:













β
_


l
,
m




(

δ
k

)


=




p
=
1


l
-
1




(

1
-

δ

p
,

·

,
k





)



,




(
4.6
)








where (.) indicates the appropriate ancestor box (which can be determined by recursing up the BVH via the parent relationship) of Bl,m. If all of the ancestor boxes of Bl,m are not in the minimal cover (i.e. δp,.k=1; ∀p) βl,mk)=0. and Bl,m may be in the minimal cover. If one of the ancestors of Bl,m is in the minimal cover, βl,mk)≧1 and Bl,m cannot be in the minimal cover. Note βl,mk)=0 for the root box as it has no ancestors. The minimum cover selection functions for the boxes in a BVH with NL detail levels are given by:












β

1
,
1




(

δ
k

)


=

δ

1
,
1
,
k



,




(
4.7
)









β

l
,
m




(

δ
k

)


=


δ

l
,
m
,
k


+




p
=
1


l
-
1




(

1
-

δ

p
,

·

,
k





)




,







m

=
1

,





,

2

l
-
1


,



l

=
2

,





,


N
L

-
1

,




(
4.8
)









β


N
L

,
m




(

δ
k

)


=




p
=
1



N
L

-
1




(

1
-

δ

p
,

·

,
k





)



,



m

=
1

,





,

2


N
L

-
1






(
4.9
)







The OAF objective function (Eqn. 3.9) is modified so that it selects the coarsest minimal cover that is feasible with respect to the minimum cost trajectory. This is achieved by costing the relaxation of a box in favor of its descendants, which is implemented by placing a small cost ε>0, on each of the minimum cover selection binary decision variables. This causes the MIP solver to choose finer detail only if the trajectory cost will be reduced as a result. The modified objective function is:











v

N
-
1


=

arg







min
v






k
=
0


N
-
1




(



γ
k





v
k




+

ε






1
T



δ
k



)





,




(
4.10
)








where 1 is a column vector of ones of an appropriate size, and 0<γ≦1 is the discounted rate.


The implicit version of the finite horizon obstacle avoidance problem (Pimp(x0; , ũN−1.)) is:











v

N
-
1


=

arg







min
v






k
=
0


N
-
1




(



γ
k





v
k




+

ε






1
T



δ
k



)





,




(
4.11
)








x

k
+
1


=


Ax
k

+

B


(



u
~

k

+

v
k


)




,



k

=
0

,





,

N
-
1





(
4.12
)








(


x
k

,



u
~

k

+

v
k



)


𝒫

,



k

=
0

,





,

N
-
1





(
4.13
)









a

i
,
l
,
m




x
k





b

i
,
l
,
m


+

M






α

i
,
l
,
m
,
k





,



i

=
1

,





,

2


N
D


,







m

=
1

,





,

2

l
-
1


,



l

=
1

,





,

N
L

,



k

=
1

,

,

N
-
1





(
4.14
)











i
=
1


2


N
D





α

i
,
l
,
m
,
k






2


N
D


-
1
+


β

l
,
m




(

δ
k

)




,







m

=
1

,





,

2

l
-
1


,



l

=
1

,





,

N
L

,



k

=
1

,





,

N
-
1





(
4.15
)








x
N



χ
T


,




(
4.16
)









χ
T


𝒪

=


,




(
4.17
)








where βl,mk) is given by the appropriate selection function in Eqns. 4.7-4.9. The implicit non-convex OAF algorithm, given by solving Eqns. 4.11-4.17 in a receding horizon fashion, is recursively feasible (Rossiter 2003), as the same minimal cover can always be chosen by the MIP solver at the next time step. A single minimal cover for the entire prediction horizon can be chosen by using only one set of minimal cover selection decision variables and using these for the minimal cover selection functions at each prediction step.


4.3 Explicit Non-Convex OAF


The explicit non-convex OAF algorithm operates by:

    • 1. selecting an appropriate minimal cover from the BVH for each obstacle using a static rule or a adaptive algorithm based on the current state and/or operator command, then
    • 2. solving the OAF for convex polytonal obstacles (Section 3),treating the boxes in the minimal cover(s) as convex obstacles.


A static minimal cover selection rule could be to choose either the finest minimal cover, which is made up of all the leaf boxes in the BVH (denoted leaf boxes OAF), or the minimal cover that requires the least number of binary variables to represent it, i.e. the root box only (denoted root box OAF). A simple adaptive minimal cover selection algorithm would be to switch between the leaf boxes and root box minimal cover representations for an obstacle depending on the current distance to the obstacle. A more sophisticated adaptive minimal cover selection algorithm can be synthesized by examining the structure of the solution of PN. The optimizer selects the minimum-cost feasible trajectory over the prediction horizon as the solution of PN. As the objective function costs deviations from the operator command, the minimum-cost feasible trajectory is likely to be spatially close to the nominal trajectory:


Definition The nominal trajectory {tilde over (T)}N(xk; ũk), is defined as the predicted trajectory over an N-step horizon that will occur if the operator's command is held constant over the N-step horizon (vN-1=0, hence zero-cost):

{tilde over (T)}Nk, ũk)={{tilde over (χ)}k, {tilde over (χ)}k+1, . . . , {tilde over (χ)}k+N},

where ˜{tilde over (χ)}k=xk and {tilde over (χ)}j+1=f({tilde over (χ)}j; ũk).


Hence, an appropriate minimal cover selection rule may be to choose fine detail for the parts of the obstacles that are close to the nominal trajectory and coarse detail for parts of the obstacles far away from the nominal trajectory. As the nominal trajectory is defined for the horizon, a single minimal cover will be used over the prediction horizon. A minimal cover selection rule that apportions fine detail near the nominal trajectory and coarse detail elsewhere can be implemented efficiently by recursing down the BVH of each obstacle. The desired minimal cover (i) will contain the smallest number of ABBs such that any leaf boxes that intersect with the nominal trajectory are included, or (ii) if the trajectory does not intersect with any of the leaf boxes, the coarsest minimal cover that does not intersect with the nominal trajectory will be chosen. The implementation of the minimal cover selection rule involves recursing down each branch of the BVH until a leaf box or a box that does not intersect with the nominal trajectory is found and added to the minimal cover. Further recursion to such a box's children (if any) is halted due to Proposition 4.1. This approach is hereafter called the nominal trajectory minimal cover selection algorithm, see Alg. 1, and the OAF algorithm utilizing this selection rule to select minimal covers for each obstacle is called the nominal cover OAF algorithm.












Algorithm 1: recurseNomTraj(Bl,m, {tilde over (T)}N(xkk))

















Data: ABB: Bl,m; nominal trajectory: {tilde over (T)}N(xk, ũk)



Result: Nominal Trajectory Minimal Cover, custom character



if Bl,m is a leaf node then



 | append Bl,m to custom character



 | custom character  = custom character  + Bl,m;



else



 | if Bl,m ∩ {tilde over (T)}N(xk, ũk) = ∅ then



 |  | append Bl,m to custom character



 |  | custom character  = custom character  + Bl,m;



 | else



 |  | recurse to Children



 └  └ ∀ children to Bl,m : Bl+1,p, recurseNomTraj(custom character  {tilde over (T)}N(xk, ũk));










FIG. 7 presents minimal covers for the obstacle given in FIGS. 5 and 6 that are generated by four different state, operator command pairs using Alg. 1. Each nominal trajectory is represented by the joined circles and the current position is shown by the square. FIG. 7(a) shows the minimal cover when the current position is external to the root box and the nominal trajectory does not intersect with any leaf boxes, i.e. the coarsest minimal cover that does not intersect with the nominal trajectory. The minimal cover produced in FIG. 7(b) includes leaf boxes that the nominal trajectory intersects with, and the minimum amount of boxes required to cover the remainder of the object. FIG. 7(c) shows that the minimal cover is the root box when the slave state is outside the root box and the nominal trajectory does not intersect with the root box. FIG. 7(d) shows a potential drawback to the nominal trajectory minimal cover selection algorithm.


Here the nominal trajectory crosses the centerline of the object and includes fine detail on the opposite side of the obstacle in the minimal cover. The inclusion of finer detail on the opposite side of the obstacle in the minimal cover is unlikely to improve the trajectory, or in particular, reduce the magnitude of the first alteration, compared to a minimal cover that has a coarser representation for the far side of the obstacle. This additional detail will increase the computational cost of solving the resulting MIP.


Algorithm 2 shows the operation of the explicit non-convex OAF algorithm. getMinimalCover( )calls the appropriate static or adaptive rule that chooses the minimal cover for each object at each time step, e.g. all leaf nodes, or the nominal trajectory minimal cover (Alg. 1).












Algorithm 2: Explicit non-convex OAF















Data: current state xk, current operator command ûk, obstacle ABB-Tree


 root node B1.1


Result: operator modification vk



custom character  = get MinimalCover( )



Set up standard OAF problem with custom character  as obstacles


Add custom character  to PN as obstacles


Solve OAF problem


vN−1 ← solution of PN


Select first input modification


Return vk










4.3.1 Recursive Feasibility of Nominal Trajectory Explicit OAF


The constraint set of the nominal trajectory non-convex OAF potentially changes at each time step. As a result, standard recursive feasibility conditions as set down, for example in (Rossiter 2003), do not hold. We now provide conditions under which recursive feasibility holds for changing obstacle constraint sets. The obstacle representation at time k is denoted, custom characterk, and is a superset of the obstacle set custom character. It is assumed that there is a feasible trajectory at time step k,

Tk={χkk+1, . . . , χk+N}∪XT.


A trajectory at time k+1 can be constructed that is a subset of Tk (assuming the deterministic case),

Tk+1={χk+1, χk+2, . . . , χk+N, fk+N, Nk+N))}∪XT.


A limited recursive feasibility condition is presented in the following proposition:


Proposition 4.2 Recursive feasibility holds for a changing obstacle set when the obstacle set is monotonically decreasing, i.e. custom characterk+1custom charactercustom characterk.


Proof: It is given that Tk is feasible with respect to custom characterk, i.e. Tkcustom characterk=∅. Since Tk+1custom characterTk and custom characterk+1custom charactercustom characterk then Tk+1 is feasible with respect to custom characterk+1.


The downside of Proposition 4.2 is that it only allows for the obstacle representation set to be refined; it does not allow for the obstacle representation set to become coarser if the slave moves away from it. This limitation is addressed in Corollary 4.3.


Definition The a posteriorix obstacle set, O(Tk), is defined as the coarsest level of detail obstacle representation set such that custom character(Tk)∩Tk=∅.


Corollary 4.3 Recursive feasibility holds for a changing obstacle set if the obstacle set at time k+1, custom characterk+1, is a subset of custom character(Tk).


Proof: Follows directly from the proof of Proposition 4.2. It is possible to incorporate the recursive feasibility conditions for custom characterk+1 from Corollary 4.3 into the nominal trajectory minimal cover selection algorithm (Alg. 1) by enforcing recursion to the box's children when a box intersects with the solution trajectory from PN at the previous time step. This modification to Alg. 1, restricts the minimal cover at time k+1 to be a subset of custom characterk(Tk).


4.4 Evaluation of Explicit and Implicit Non-Convex OAF Algorithms


The alternative OAF algorithms are evaluated by comparing their performance in terms of computational cost and deviation from the nominal trajectory. The implicit OAF algorithm is evaluated against the leaf boxes OAF algorithm, and the nominal trajectory OAF algorithm is compared to both the leaf boxes and the root box OAF algorithms. The dynamic model used for the comparison simulations is that of a proportionally velocity-controlled point mass in two dimensions. The discretized dynamics (Ts=0.2 s) and constraints for each degree of freedom (chosen along x and y axes) are:











[




q

k
+
1







v

q
,

k
+
1






]

=



[



1


0.0865




0


0.135



]



[




q
k






v

q
,
k





]


+


[



0.1135




0.865



]



u

q
,
k





,




(
4.18
)







q


[


-
10

,
10

]


,


v
q



[


-
1

,
1

]


,


u
q



[


-
1

,
1

]


,






10


(


u
q

-

v
q


)





[


-
1

,
1

]

.






(
4.19
)







The invariant set used in this simulation is the zero-velocity invariant set, which is given, along with its associated terminal feedback control by:

XT={[x,y,vx,vy]Tεcustom character/custom character:vx=0, vy=0},   (4.20)
ux,T=0, uy,T=0.   (4.21)


This invariant set is incorporated in the MIP OAF using the following constraints:

xk+Ncustom character.   (4.22)
vx,k+N=0, vy,k+N=0, ux,k′N=0, uy,k+N=0.   (4.23)


Note that Eqn 4.22 will require obstacle avoidance constraints, analogous to those in remainder of the horizon, to be imposed for the terminal state, e.g. nominal trajectory or implicit avoidance constraints. The obstacle set and BVH for these simulations is the obstacle and BVH given, respectively, in FIGS. 5 and 6. The prediction horizon length is set to 1 sec (N=5). and a 2-norm cost will be placed on the deviation, and the discount rate γ=1. The resulting MIP formulation is a Mixed Integer Quadratic Program (MIQP), which can be solved using CPLEX (ILOG 2007).



FIG. 8 shows that the trajectory produced by the implicit OAF corresponds to the trajectory produced by the leaf box OAF. Since all minimal covers are supersets of leaf box minimal cover, and that the MIQP solver determines the global optimal solution to the MIP, the trajectories correspond due to Proposition 4.4:


Proposition 4.4 Consider two minimal covers: custom character1 and custom character2. If custom character1custom charactercustom character2, then cost of the optimal trajectory that is feasible with respect to custom character1 is less than, or equal to the cost of the optimal trajectory that is feasible with respect to custom character2.


Proof: Consider the set of all trajectories from a given state, χ, that are feasible with respect to the constraints and the minimal cover custom character.

TN(χ,custom character)={TN(χ,uN−1): ∀uN−1εcustom characterN, subject to TN(χ,uN−1)∩custom character=∅}


Since custom character1custom charactercustom character2, all of the trajectories that are feasible with respect to custom character2, are also feasible with respect to custom character1, hence TN(χ, custom character2)custom characterTN(χ, custom character1), ∀χεcustom character. So the cost for the minimum cost trajectory in TN(χ, custom character1) is less than, or equal to the cost for the minimum cost trajectory in TN (χ, custom character2).


This result is dependant on the appropriate choice of ε in Eqn. 4.11. If ε is larger than the cost difference between the leaf node trajectory and one for another minimal cover, then the implicit and leaf node trajectories will not correspond. The least computationally efficient algorithm of either the leaf boxes OAF or the implicit OAF is redundant, as they produce the same trajectory.


The table below shows simulation times (in seconds) for the different forms of the OAF for the different starting points. These simulations were run on an Intel Core 2 Duo E6300 (single core only) with 4 GB of RAM, where the OAF MIQP is solved using CPLEX10.2 (ILOG 2007).



















1
2
3
4
Binary Variables





















Root Box
0.56
0.53
0.66
0.56
 20


Leaf Boxes
15.09
15.13
14.44
12.66
320


Nominal Trajectory
3.67
2.86
3.16
3.37
20-320


Implicit
59
52.71
52.2
55.35
695









The table shows that for the four trajectories considered in FIG. 8, the computation of the leaf boxes trajectory takes approximately 30% of the time taken to compute the implicit OAF trajectory. This comparison renders the implicit OAF formulation redundant. Two reasons for the poor performance of the implicit OAF are that (i) it is computing the best minimal cover in addition to the optimal trajectory, and (ii) the leaf node OAF is a subproblem of the implicit OAF. The nominal trajectory explicit OAF is to be verified by comparing its trajectories against those produced by the leaf boxes OAF and the root box OAF. The nominal trajectory OAF is formulated to produce a lower deviation trajectory than the root node


OAF, and (ii) be more computationally efficient on average than the leaf boxes OAF. The simulations verify this supposition. In three of the four simulations shown in FIG. 9, the trajectories produced by the leaf boxes OAF and the nominal trajectory OAF correspond, while in the fourth, the leaf boxes OAF and the nominal trajectory OAF diverges due to the order of branching in the MIQP solver when the minimum-cost trajectory is not unique (see FIG. 10). Additionally, divergences will generally occur due to a difference between the leaf boxes minimal cover and the nominal trajectory minimal cover, particularly if perturbations to the nominal trajectory would result in an alternate minimal cover. The simulation times in the above Table show that the nominal trajectory OAF is more computationally efficient than the leaf boxes OAF, with simulations taking approximately 20-25% of the computation time for the leaf boxes OAF.


The number of binary variables for the different OAF algorithms is given by the following table:















Number of Binary Variables



















Root Boxes
2NDN



Leaf Boxes
2NDN × 2NL−1



Implicit
N [2ND(2NL − 1) + (2NL−1 − 1)]











FIG. 11 shows how the simulation times of the four different OAF algorithms change with respect to the complexity of the BVH, which is given by the number of levels (NL) within the BVH binary tree. The simulation times of the implicit and leaf boxes OAF increase significantly due to the increase in the number of binary variables. This increase occurs because the number of binary variables required for both formulations are exponential with respect to NL (see Table 2), and the worst case computation cost of an MIP is exponential with respect to the number of binary variables. FIG. 11 also shows that the nominal trajectory OAF does increase, although not by as much as the leaf boxes OAF, and is less costly than the leaf boxes OAF. The root box algorithm is constant as NL has no affect on its runtime.


5 Reducing Computational Cost Using Reachable Sets


The computational cost of the OAF can be further reduced by removing obstacles or parts thereof that are not reachable at a given time-step in the prediction horizon from the MIP. Within the context of the OAF formulations presented in Sections 2 and 4, reachability can be used to (i) simplify the BVH for a given obstacle by removing ABBs and branches of the tree that are not reachable, and (ii) remove polytopal obstacles and constraints that are not reachable at a given prediction step from the OAF MIP. Reachability is defined in terms of the region in the state space X that can be reached in a given time period: the reachable set.


Definition: The one-step reachable set is defined as the set containing all possible successor states for a given state or set of states custom character, i.e.

custom character(custom character)={χ+εcustom charactern:∀uεcustom character, ∀χεcustom character, χ+=f(χ,u)}.   (5.1)


Definition The i-step reachable set is recursively defined as the repeated application of the one-step reachable set:

custom characterx(custom character)=custom character(custom characterx−1(custom character)),   (5.2)

with custom character0(custom character)=custom character.


5.1 Simplification of Bounding Volume Hierarchies Using Reachable Sets


The number of ABBs within a BVH that are considered in a computation can be reduced using reachability. This reduction is performed by (i) culling boxes and branches of the BVH that are not reachable, and (ii) replacing a box with one of its children within the BVH, when only that child is reachable. This BVH simplification strategy relies on the following propositions:


Proposition 5.1 If an ABB within the BVH is not reachable (i.e. it does not intersect with the reachable set), then none of its descendants are reachable.


Proof Let B be a descendant box of B, B∩custom character=∅, and custom character(B) is defined as the geometry that is bounded by B (custom character(b)⊂B). Since B is a descendant of B, then custom character(B)custom charactercustom character(B). By the transitive property of ⊂,custom character(B)∩custom character=∅, hence if B is culled due to being non-reachable, its descendants should also be culled.


Proposition 5.2 If reachable set R intersects a box Blm, and only one of the box's children Bl+1,q1, then box Bl;m can be replaced by its child Bl+1;q within the BVH.


Proof Let Bl+1,q1 and Bl+1;q2 be the two child boxes of Bl;m. Also Bl;m and Bl+1;q1 intersect with reach set, R, while Bl+1;q2 does not. custom charactercustom character(Blm)=custom charactercustom character(Bl+1,q1) since custom character(Bl,m)=custom character(Bl+1,q1)∪custom character(Bl+1,q2) and custom charactercustom character(Bl+1,q2)=∅. Hence, custom charactercustom character(Blm)custom characterBl+1,q1.


Propositions 5.1 and 5.2 can be used together to synthesize an algorithm that simplifies a BVH for a given reachable set custom character by traversing the tree. This recursive algorithm first determines whether the children of a candidate box are reachable, and removes all the non-reachable children and their descendants from the BVH. If only one child remains, it replaces the candidate box in the BVH, and has the recursive algorithm run on it. If more than one child is reachable, the candidate box remains in the tree and recursion proceeds to its reachable children. FIG. 12 shows how this recursive algorithm can be used to simplify a BVH, where the colored-in dots represent the boxes that intersect with reachable set custom character. FIG. 12(a) shows the entire BVH, and FIG. 12(b) shows the simplified BVH. The simplification of the BVH will result in either a reduction in the number of binary variables required to represent an obstacle or an increase in the detail of the representation.


5.2 Reduction of Polytopal Obstacle Constraints Using Reachability


The number of binary variables required to represent custom character at a given prediction time-step can be reduced by culling the obstacles and constraints that cannot be reached from the OAF MIP. This idea is analogous to that set out previously by Kuwata (2003) and Richards et al. (2003), both of whom use approximations of the reachable set of the entire prediction horizon to cull constraints representing obstacles outside this set from the OAF MIP. Culligan (2006) further reduced the number of binary variables in the MIP by including, for each time step in the planning horizon, only the obstacles that could be reached at that time step.


A further new reduction is achieved by including only the constraints that are necessary to represent the part of the obstacle set that intersects with the reach set. Specifically, it reduces the number of constraints (hence, binary variables) required to represent a convex polytopal obstacle, Oj, that is not completely inside the reach set, custom characterk Only constraints that are active for some state within custom characterk/Oj are selected. The constraint, {χ:aijχ≦bij}, is selected if.

custom characterk∩{χ:ai,jχ≦bi,j}≠∅.  (5.3)


The reachable constraints are then represented by the index set,

Ij,k={iε{1,2, . . . , Nh(Oj)}:custom characterk∩{χ: −ai,jχ≦−bi,j}≠∅},  (5.4)

and the induced obstacle is given by










𝒪

j
,
k


=




i




j
,
k







{

x






k



:






-


a

i
,
j



x




-

b

i
,
j





}

.






(
5.5
)







The induced obstacle is a subset of the reach set, custom characterk, and can also be expressed as the intersection of the Oj with custom characterk.


Three possible situations occur if an obstacle Oj intersects with reach set custom characterk:

    • 1. Ojcustom charactercustom characterk (see FIG. 13(a)): Here, all of the half-spaces will be required to represent the obstacle. The formulation of Eqns. 3.6 and 3.7 are used to determine the constraints for Oj. The induced obstacle for time k is given by Ojk=Oj.
    • 2. Ojcustom charactercustom characterk with two or more constraints reachable, i.e. |Ijk|≧2 (see FIG. 13(b)).


The mixed integer linear inequalities for Oj at time k are:












-

a

i
,
j

T



x




-

b

i
,
j



+

M






α

i
,
j
,
k





,



i




j
,
k




,




(
5.6
)











x




j
,
k






α

i
,
j
,
k










j
,
k




-
1


,




(
5.7
)







where |.| indicates the number of elements in the index set.

    • 3. Ojcustom charactercustom characterk with only a single constraint reachable, i.e. |Ijk|=1 (see FIG. 13(c)). No binary variables are required as only a single linear inequality is required to represent Oj in custom characterk. The constraint is

      ai,jTχ≦−bi,j, for iεIj,k.  (5.8)


The number of binary variables that can be removed from the OAF MIP using the reachable constraint method, will depend on the dynamics of the slave, the closeness of the reachable set approximation to the true reachable set, and the geometry of the obstacles. Note that when Ocustom charactercustom characterk∀k=1, . . . , N, there will be no reduction in the number of binary variables; in this situation other OAF algorithms, such as those presented in Sections 2 and 4, can be used.


5.3 Reachable Constraint Method for Axially-Aligned Bounding Boxes


Reachable constraints can be efficiently determined for the case where the obstacle Oj and the reachable set (or its approximation) custom characterk are ABBs, by modifying the standard algorithm for testing whether a pair of ABBs intersect (Cohen et al. 1995). This algorithm determines whether two ABBs intersect involves by projecting the boxes onto each axis, and determining whether the projections overlap. If the projections overlap on all axes, then the ABBs overlap. The modified algorithm stores whether the bounds of the obstacle overlap as boolean variables, Ii;j;k, and, if the obstacle intersects with the reach set, these variables are used to determine the reachable constraint index set Ij,k directly.


Since the only modifications to the standard algorithm are (i) the storage of the Ii,j,k variables, and (ii) the construction of set Ij,k, the modified algorithm requires only a minor increase in computational resources over the standard ABB intersection test algorithm. The operation of the modified intersection algorithm in 2D is presented in












Algorithm 3: Pairwise ABB-intersection algorithm - reachable constraint


method

















Data: ABB reach box or ABB approximation to reach set:



  Rk = [xmin,Rk, xmax,Rk] × [ymin,Rk, ymax,Rk]



Data: ABB obstacle Oj = [xmin,j, xmax,j] × [ymin,j, ymax,j]



Result: Active constraint index set, custom character



begin



 | custom character  = ∅



 | Test projections onto axes for overlap (True/False):



 | rx,min = xmin,Rk ε [xmin,j, xmax,j] , rx,max = xmax,Rk ε [xmin,j, xmax,j]



 | I1,j,k = xmin,j ε [xmin,Rk , xmax,Rk] , I2,j,k =xmax,j ε [xmin,Rk , xmax,Rk]



 | ry,min = ymin,Rkε [ymin,j, ymax,j] , ry,max = ymax,Rkε [ymin,j, ymax,j]



 | I3,j,k = ymin,j ε [ymin,Rk , ymax,Rk] , I4,j,k = ymax,j ε [ymin,Rk , ymax,Rk]



 | Test for intersection (Do the boxes overlap on both axes?):



 | if (rx,mincustom character  rx,maxcustom character  I1,j,kcustom character  I2,j,k) custom character  (ry,mincustom character  ry,maxcustom character  I3,j,kcustom character  I4,j,k) then



 |  | ABB's collide



 |  | for i = 1 to custom character  do



 |  |  | if Ii,j,k = true then



 |  └  └ └ append i to custom character



end










Algorithm 3.


This algorithm can be extended to higher dimensions with only minimal modifications (projecting to the new axis/axes and requiring the additional projections to overlap also for intersection of the ABB).


5.4 Recursive Feasibility for Reachable Constraints


We now show that the reachable constraint method is recursively feasible when applied to (i) a constant obstacle set custom character, and (ii) an obstacle set that can change at each time step according to Corollary 4.3, custom characterk:


Proposition 5.3 Recursive feasibility holds for (i) a constant obstacle set, and (ii) an obstacle set that can change at each time step according to Corollary 4.3, with constraints determined using the reachable constraint method.


Proof Part (ii) is considered first. It is assumed there is a feasible trajectory at time k,

Tk={χk, χk+1, . . . , χk+N}∪XT,  (5.9)

satisfying the following constraints

χk+p+1=fk+p, uk+p), ∀p=0, . . . , (N−1),  (5.10)
k+p, uk+pcustom character, ∀p=0, . . . , (N−1)   (5.11)
χk+pcustom characterk,pk), ∀k=1, . . . , (N−1),  (5.12)
χk+NεXT,  (5.13)
XTcustom characterk,p=∅.  (5.14)

where custom characterk,pk)=custom characterkcustom characterpk). Assuming that Tk can be exactly implemented in the future, it is possible to construct a trajectory at the next time step,

Tk+1={χk+1, χk+2, . . . , χk+N, fk+N, Kk+N))}∪XT,  (5.15)

which is a subset of the trajectory at the previous time step (Tk). In order for recursive feasibility to be established, it is necessary to show that Tk+1 is feasible if Tk is feasible. The dynamic and system constraints (Eqn. 5.10 and 5.11) for Tk+1 are satisfied as it is a subset of the Tk. The constraints in Eqns. 5.13 and 5.14 are satisfied for the Tk+1 due to the invariance of XT.


It remains necessary to show that this trajectory will satisfy the obstacle avoidance constraints induced by the reachable sets from χk+1, that is:

χk+pcustom characterk+1,p−1k+1), ∀p=2, . . . , N  (5.16)


Since custom characterp−1k+1)custom charactercustom characterpk) (as custom characterp−1k+1) is a restriction of custom characterpk), with the additional constraint that the state at time k+1 is χk+1) and custom characterk+1custom charactercustom character(Tk), where custom character(Tk) is the a posteriori obstacle set for time k (by the condition of Corollary 4.3). The induced obstacle sets are related by:

custom characterk+1,p−1k+1)custom charactercustom characterpk)∩custom character(Tk).  (5.17)


It follows from Eqn. 5.17, and the fact that custom characterkcustom charactercustom character(Tk):

χk+pcustom characterk,pk)→χk+pcustom characterk+1, p−1k+1), ∀p.


Hence, all constraints are satisfied for Tk+1, if the constraints, Eqn 5.10 to 5.14, are satisfied for Tk. The recursive feasibility condition given in Eqn. 5.17 also holds trivially when the obstacle set is constant, since Tk is feasible with respect to custom character, so (i) is also satisfied.


5.5 Evaluation of OAF Algorithms Using the Reachable Constraint Method


The leaf boxes and nominal trajectory OAF algorithms, using the reachable constraint method, are evaluated against the corresponding unmodified OAF algorithm. The OAF algorithms uses the reachable constraint method should produce the same trajectory, more computationally efficiently than the corresponding unmodified OAF method. The dynamic model and OAF formulation presented in Section 4.4 are used, again, for these simulations.


The reachable constraint method utilizing ABBs, requires an ABB approximations to the reachable sets can be calculated. These ABBs are calculated using a method similar to the one presented in Culligan (2006). The approximate reachable sets,

custom characterpk)=[χk+p,mink+p,max]×[custom characterk+p,min,custom characterk+p,max], ∀p=1, . . . , N  (5.18)

are calculated by solving the following models:

χk+p,min=fk+p−1, min, umin), ∀p=1, . . . , N,  (5.19)
χk+p,max=fk+p−1,max, umax), ∀p=1, . . . , N,  (5.20)
χk,maxk,mink.  (5.21)

where the maximal and minimal inputs are given by:











u

m





i





n


=

arg







min
u



{



1
T


u


:






u


𝕌

}




,




(
5.22
)







u

m





ax


=

arg







max
u




{



1
T


u


:






u


𝕌

}

.







(
5.23
)







This formulation will produce outer approximation to reach sets for linear systems with system matrices having all positive or zero elements, such as the model presented in Section 4.4.



FIG. 14 and the table below show the comparisons between OAF algorithms using the reachable constraint method and unmodified OAF algorithms FIG. 14 shows that the trajectories for each starting point correspond for both the leaf node OAF and the nominal trajectory OAF, except for Trajectory 4 of the Leaf boxes OAF. This behaviour is due to the ordering of the branching in the MIQP when the minimal-cost trajectory is not unique. Table 3 shows that OAF algorithm using the reachable constraint method to have significantly shorter run times than the corresponding unmodified OAF algorithm. Hence, the reachable constraint method should be used where reachable sets and the resulting reachable constraints can be determined efficiently, e.g. when the reach set and obstacles are represented using ABBs.


The table below shows a simulation time comparison between unmodified OAF algorithms and OAF algorithms using the reachable constraint method. These simulations were run on an Intel Core 2 Duo E6300 (single core only) with 4 GB of RAM, where the OAF MIQP is solved using CPLEX10.2 (ILOG 2007). Times in seconds.


















1
2
3
4





















Leaf Boxes
Unmodified
15.09
15.13
14.44
12.66



Reachable constraints only
3.33
2.75
2.51
3.42


Nominal
Unmodified
3.67
2.86
3.16
3.37


Trajectory
Reachable constraints only
1.49
1.31
1.38
1.65










6. Simulation Example Based on Simplified Mining Shovel-Truck Avoidance Problem


The example considered in this section is that of a simplified cartesian excavator, where an operator loads material into a truck tray (FIG. 15, left) by commanding the velocity of the dipper (FIG. 15, right).


The scenario that is simulated in this section is of an operator making his first loading pass of an empty truck tray with the dipper, but failing to lift out or stop the dipper inside the truck tray. This is modeled as a constant operator command input over the simulation. The nominal trajectory OAF algorithm, using the reachable constraint method, and a lookahead of 1 second (or 5 samples) will be used to avoid collisions. The dynamics and kinematics in this example have been simplified: the motion is pure translation, and each degree of freedom (DOF) has double integrator dynamics with proportional rate feedback. Each DOF is aligned to a cartesian axis. The x DOF has twice the effective inertia, and can travel at twice the velocity of the y DOF and z DOF.


The discrete time model for the system (Δt=0:2s) is










[




x

k
+
1







u

x
,

k
+
1








y

k
+
1







u

y
,

k
+
1








z

k
+
1







u

z
,

k
+
1






]

=


[



1


0


0


0.1264


0


0




0


1


0


0


0.08647


0




0


0


1


0


0


0.08647




0


0


0


0.3679


0


0




0


0


0


0


0.1353


0




0


0


0


0


0


0.1353



]






[




x
k






v

x
,
k







y
k






v

y
,
k







z
k






v

z
,
k





]

+



[



0.07358


0


0




0


0.1135


0




0


0


0.1135




0.6321


0


0




0


0.8647


0




0


0


0.8647



]



[




u

x
,
k







u

y
,
k







u

z
,
k





]


.








(
6.1
)







The corresponding velocity, command and actuator constraints for this system are

vxε[−2,2], vy, vzε[−1,1],  (6.2)
uxε[−2,2], uy, uzε[−1,1],  (6.3)
10(uq−vq)ε[−1,1], q=x,y,z.  (6.4)


Again, the zero-velocity, collision-free invariant set is utilized as the OAF terminal invariant set:

XT={χεcustom character/custom character:vx=0, vy=0, vz=0},   (6.5)

and the associated terminal feedback control is

ux,T=0, uy,T=0, uz,T=0.  (6.6)


The invariant set and associated control law is included in the OAF MIQP using the following constraints:

χk+Ncustom character,  (6.7)
vx,k+N=0, vy,k+N=0, vz,k+N=0,  (6.8)
ux,k+N=0, uy,k+N=0, uz,k+N=0.   (6.9)


Object-object avoidance constraints can be represented using the Minkowski Sum, as the motion of the dipper is pure translation. The Minkowski Sum is defined as the exhaustive sum of two sets, A and B:

custom charactercustom character={a+b:∀aεcustom character, ∀bεcustom character}


The set representing the geometry of the dipper for a given state χ (since the dipper's motion is pure translation) is:

XD(χ)=Cpχ⊕XD  (6.10)

where Cp: custom charactercustom character3 is the projection matrix from the state to the position space (Note that in general, the relationship between the state and position spaces may not be linear, particularly if rotation states are involved), and χDcustom character3 is the set representing the geometry of the dipper when the state is at the origin. Hence, the object-object obstacle avoidance constraint for the cartesian excavator is given by:

(Cpχ⊕XD)∩custom characterT=∅.  (6.11)

where custom characterT custom character3 represents the geometry of the truck tray. Equation 6.11 can be transformed into a point-object constraint using the Minkowski sum:

Cpχ∉[custom character⊕(−XD)]  (6.12)

where −χ={−χ, ∀χεX}.


The level-of-detail point-polytope avoidance constraints are calculated using a method based on the method to determine Minkowski bounding trees, presented in Smith (2008): A BVH of ABBs for the tray is constructed, and the BVH of the obstacle set is found by taking Minkowski sum of the truck tray BVH box-wise with an ABB of the dipper (effectively the root box of the BVH of the dipper). FIG. 16 shows the leaf boxes of the Minkowski Bounding Tree of the dipper-truck tray obstacle set.



FIG. 17 shows the leaf boxes of the BVH representing the state space obstacle and the resulting trajectory (white spheres), while FIG. 18, shows corresponding snapshots of the relative motion of the dipper to the truck tray. Both figures show that the dipper successfully avoids colliding with the shovel.


7 Conclusions


The preferred embodiment provides for an effective OAF, which is interposed between a human operator and the slave manipulator, to assist the operator in avoiding collisions by minimally altering the operator's command. The OAF formulation addresses the challenges inherent in assisting human operators in avoiding obstacles, namely it deals with the non-causal structure of the problem, and accounts for that the dynamics and performance limitations of the system when determining the alteration to the operator's command. The main contribution of the paper though is in incorporating geometric level of detail into the OAF framework to produce a computationally efficient algorithm for avoiding non-convex obstacles. The present results, while simulation-based only, are sufficiently promising to suggest that the OAF can work in practice for a suitable application.


Interpretation

Reference throughout this specification to “one embodiment” or “an embodiment” means that a particular feature, structure or characteristic described in connection with the embodiment is included in at least one embodiment of the present invention. Thus, appearances of the phrases “in one embodiment” or “in an embodiment” in various places throughout this specification are not necessarily all referring to the same embodiment, but may. Furthermore, the particular features, structures or characteristics may be combined in any suitable manner, as would be apparent to one of ordinary skill in the art from this disclosure, in one or more embodiments.


Similarly it should be appreciated that in the above description of exemplary embodiments of the invention, various features of the invention are sometimes grouped together in a single embodiment, figure, or description thereof for the purpose of streamlining the disclosure and aiding in the understanding of one or more of the various inventive aspects. This method of disclosure, however, is not to be interpreted as reflecting an intention that the claimed invention requires more features than are expressly recited in each claim. Rather, as the following claims reflect, inventive aspects lie in less than all features of a single foregoing disclosed embodiment. Thus, the claims following the Detailed Description are hereby expressly incorporated into this Detailed Description, with each claim standing on its own as a separate embodiment of this invention.


Furthermore, while some embodiments described herein include some but not other features included in other embodiments, combinations of features of different embodiments are meant to be within the scope of the invention, and form different embodiments, as would be understood by those in the art. For example, in the following claims, any of the claimed embodiments can be used in any combination.


Furthermore, some of the embodiments are described herein as a method or combination of elements of a method that can be implemented by a processor of a computer system or by other means of carrying out the function. Thus, a processor with the necessary instructions for carrying out such a method or element of a method forms a means for carrying out the method or element of a method. Furthermore, an element described herein of an apparatus embodiment is an example of a means for carrying out the function performed by the element for the purpose of carrying out the invention.


In the description provided herein, numerous specific details are set forth. However, it is understood that embodiments of the invention may be practiced without these specific details. In other instances, well-known methods, structures and techniques have not been shown in detail in order not to obscure an understanding of this description.


As used herein, unless otherwise specified the use of the ordinal adjectives “first”, “second”, “third”, etc., to describe a common object, merely indicate that different instances of like objects are being referred to, and are not intended to imply that the objects so described must be in a given sequence, either temporally, spatially, in ranking, or in any other manner


In the claims below and the description herein, any one of the terms comprising, comprised of or which comprises is an open term that means including at least the elements/features that follow, but not excluding others. Thus, the term comprising, when used in the claims, should not be interpreted as being limitative to the means or elements or steps listed thereafter. For example, the scope of the expression a device comprising A and B should not be limited to devices consisting only of elements A and B. Any one of the terms including or which includes or that includes as used herein is also an open term that also means including at least the elements/features that follow the term, but not excluding others. Thus, including is synonymous with and means comprising.


Similarly, it is to be noticed that the term coupled, when used in the claims, should not be interpreted as being limitative to direct connections only. The terms “coupled” and “connected,” along with their derivatives, may be used. It should be understood that these terms are not intended as synonyms for each other. Thus, the scope of the expression a device A coupled to a device B should not be limited to devices or systems wherein an output of device A is directly connected to an input of device B. It means that there exists a path between an output of A and an input of B which may be a path including other devices or means. “Coupled” may mean that two or more elements are either in direct physical or electrical contact, or that two or more elements are not in direct contact with each other but yet still co-operate or interact with each other.


Although the present invention has been described with particular reference to certain preferred embodiments thereof, variations and modifications of the present invention can be effected within the spirit and scope of the following claims.

Claims
  • 1. A method of implementing an optimal avoidance filter for interposing between movement commands issued by a human operator and a machine control system of a movable machine operated by the human operator, for the avoidance of collisions with objects, the method comprising: (a) inputting a detailed representation of the objects in a vicinity of the movable machine;(b) formulating a hierarchical set of bounding boxes around the objects, the hierarchical set including refinement details depending on a current positional state of the movable machine, with the objects closer to the movable machine having higher levels of refinement details; and(c) utilizing, by the processor, the hierarchical set as a set of constraints for an optimization program to determine any alterations to the issued movement commands so as to avoid collisions with the objects.
  • 2. The method as claimed in claim 1 wherein the set of constraints comprise mixed integer constraints and the optimization program comprises a mixed integer optimization program.
  • 3. The method as claimed in claim 1 further comprising: (d) utilizing a the predicted future motion to update the hierarchical set.
  • 4. The method as claimed in claim 1 wherein step (c) further comprises the step of: determining a series of alternative alterations to the issued movement commands, and costing the series in terms of a magnitude of alteration, and utilizing a lower cost alternative alteration.
  • 5. The method as claimed in claim 1 wherein steps (a) to (c) are applied in a continuous iterative manner.
  • 6. The method as claimed in claim 1 wherein the hierarchical set of bounding boxes are axially aligned.
  • 7. The method as claimed in claim 1 wherein the hierarchical set of bounding boxes includes representation of non convex objects, in a form of convexities in the hierarchical set.
  • 8. The method as claimed in claim 1 wherein step (b) further comprises, for any particular time interval, culling members of the hierarchical set that are not reachable in a current time interval.
Priority Claims (1)
Number Date Country Kind
2010904962 Nov 2010 AU national
PCT Information
Filing Document Filing Date Country Kind 371c Date
PCT/AU2011/001428 11/8/2011 WO 00 5/6/2013
Publishing Document Publishing Date Country Kind
WO2012/061874 5/18/2012 WO A
US Referenced Citations (4)
Number Name Date Kind
6317691 Narayan et al. Nov 2001 B1
20040212676 Mathes et al. Oct 2004 A1
20060184294 Ma et al. Aug 2006 A1
20100094520 Zagorski Apr 2010 A1
Non-Patent Literature Citations (1)
Entry
PCT Search Report and Written Opinion for PCT/AU2011/001428, completed Mar. 7, 2012.
Related Publications (1)
Number Date Country
20130231855 A1 Sep 2013 US