CN103329182A - 用于人工指令系统的碰撞避免系统和方法 - Google Patents

Abstract

一种实现最优避免过滤单元的方法，该最优避免过滤单元介于人工操作员发出的运动指令和可移动机器的对应机器控制系统之间，用于避免与物体的碰撞，所述方法包括：(a)输入在可移动机器附近的物体的详细表示；(b)将该物体周围的包围盒的层次集合公式化，所述层次集合包括取决于可移动机器的当前位置状态的细化细节，物体越接近该机器具有越高级别的细化细节；(c)将得到的层次集合用作混合整数最优化问题的约束集合，以确定对发出的运动指令的任何变更，以便避免同任何物体的碰撞。

Description

用于人工指令系统的碰撞避免系统和方法

技术领域

本发明涉及碰撞避免系统和方法，尤其是，本发明公开了一种用于诸如矿用挖掘机等的人工指令系统的碰撞避免框架（frame work）的系统和方法。

参考文献

Barraquand,J.,Langlois,B.和Latombe,J.(1992),`Numerical potential-fieldtechniques for robot path planning',IEEE Transactions On Systems Man And Cybernetics22(2),224-241.

Bellingham,J.,Richards,A.和How,J.(2002),Receding horizon control ofautonomous vehicles,`Proc.of the American Control Conf'.

Bemporad,A.和Morari,M.(1999),`Control of systems integrating logic,dynamics,and constraints',Automatica35(3),407-427.

Blanchini,F.(1999),`Set invariance in control[survey paper]',Automatica35,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 aconstrained workspace by the means of linked invariant sets',International Journal of Robustand Nonlinear Control14,1185-1205.

Cohen,J.,,Lin,M.C.,Manocha,D.和Ponamgi,K.(1995),I-collide:Am interactiveand exact collision detection system for large-scaled environments,`Proceedings of ACM Int.3D Graphics Conference',ACM Press,pp.189-196.

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

Daniel,R.和McAree,P.(1998),`Fundamental limits of performance for forcereflecting teleoperation',International Journal Of Robotics Research17(8),811-830.

Daniel,R.和McAree,P.(2000),`Multivariable stability of force-reflectingteleoperation:Structures of finite and infinite zeros',International Journal Of RoboticsResearch19(3),203-224.

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

Gottschalk,A.S.,Lin,M.C.和Manocha,D.(1996),Obbtree:a hierarchical structurefor rapid interference detection,in`Proceedings of the23rd annual conference on Computergraphics and interactive techniques',ACM Press,pp.171-180.

ILOG(2007),ILOG CPLEX SYSTEM Version10.2Users Guide.49

Kearney,M.,

S.和McAree,P.(2009),Optimal cost control correction:Aset-theoretic approach,`Proc.European Control Conference[accepted]'.

Khatib,O.(1986),`Real-time obstacle avoidance for manipulators and mobile robots',International Journal of Robotics Research5(1),90-98.

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

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

Kuwata,Y.和How,J.P.(2004),Three dimensional receding horizon control for uavs,`AIAA Guidance,Navigation,and Control Conference',Providence,Rhode Island,USA.

Kuwata,Y.,Richards,A.,Schouwenaars,T.和How,J.(2007),`Distributed robustreceding horizon control for multi-vehicle guidance',IEEE Transactions on Control SystemsTechnology15(4),627-641.

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

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),`Constrainedmodel predictive control:Stability and optimality',Automatica 36(6),789-814.

McAree,P.和Daniel,R.(2000),`Stabilizing impacts in force-reflecting teleoperationusing distance-to-impact estimates',International Journal Of Robotics Research19(4),349-364.

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

S.和Mayne,D.Q.(2005),Robust time optimal obstacle avoidance problemfor constrained discrete time systems,`44th IEEE conference on Decision and Control',IEEE,Seville,Spain,pp.981-986.

S.V.,Blanchini,F.,E.Cruck和Morari,M.(2007),Robust ObstacleAvoidance for Constrained Linear Discrete Time Systems:A Set-theoretic Approach,in`IEEE Conference on Decision and Control'.

S.V.和Mayne,D.Q.(2007),`Robust Model Predictive Control for ObstacleAvoidance: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 topotential field-based navigation for mobile robots',IEEE Transactions On Robotics22(2),384-391.

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

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

Richards,A.,Kuwata,Y.和How,J.(2003),Experimental demonstration of real-timemilp control,`AIAA Guidance,Navigation and Control Conference',Austin,TX.

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

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 Automation9(5),592-606.

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

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

Thompson,R.,McAree,P.,Daniel,R.和Murray,D.(2005),`Operator matchingduring visually aided teleoperation',Robotics And Autonomous Systems50(1),69-80.

背景技术

在工业和其他环境中，在人工操作的机械环境的任何部分中，避免机械与其他物体的碰撞是很重要的。这种环境的一个重要的例子是露天采矿挖掘环境。

图1描述了给拖运卡车装载的矿用挖掘机。这在露天采矿中是常见的活动，但这是冒着挖掘机和卡车之间碰撞的巨大风险的活动。因而希望有一种技术帮助运土装备的操作员避免这种碰撞。然而，在包括核设施退役(Thompson等2005,McAree和Daniel2000,Daniel和McAree2000,1998)和空间应用(Sheridan1993)的一些遥操作背景下，对这种技术的需求以几乎相同的形式出现。目的是过滤操作员指令，以便在避免工作空间内从动设备和障碍物之间的碰撞的同时实现操作员的意图。该问题有以下几种特征：(i)存在人工在现场操作（human-in-the-loop），其向从动机械手提供指令参考以完成某些规定的任务；(ii)与从动装置（slave）的运动相关的巨大能量，该能量对于在其工作空间内从动装置和障碍物之间的引起损害的冲击具有高可能性；(iii)对输入状态和输出的速率和饱和约束，其限制了能量自从动装置中移出和向从动装置内注入的速率；(iv)从动设备和工作空间障碍物具有非凸面几何形状；和(v)对从动装置在障碍物的凹面内移动的要求。

之前的相关工作包括势场避免方法(Khatib1986),运动规划(Latombe1991,LaValle2006)，滚动时域轨迹规划（即RHTP）(Bellingham等，2002,Richards等2003,Kuwata2007,Kuwata等2007)和集论控制方法(

&Mayne2005,Blanchini等2004,

&Mayne2007,

等2007)。势场障碍物避免方法首先是由Khatib(1986)探索的，并且已被频繁应用于障碍物避免问题，例如参见(Latombe1991,LaValle2006,Ren等2006,Barraquand等1992)。这些方法利用每个障碍物周围的势场来确定驱开机械手的规划或控制法则。该方法尽管在概念上有吸引力，但有缺点，势场不能明确的考虑到机械手的动力学和性能限制。为保证避免和确保在碰撞不发生的情况下不发生变更，如平行于障碍物面移动的情况下，需要谨慎的制作(crafting)势场。作为对照，运动规划方法力图找到从机器人的初始配置到避免途中所有障碍物的理想配置的路径。这些方法在自控机器人学中是最常用的方法(Latombe1991,LaValle2006)。运动规划和避免过滤问题之间的主要区别是目标和可利用的信息：在运动规划问题中机器人的最终目的是已知的，因此该问题是完全指定的，而对于避免过滤问题未来指令是未知的，其目标是使来自于操作员指令的变更最小化。

RHTP例如使用滚动时域控制框架来计算到目的配置的路径，该滚动时域控制框架具有在每一个时间步长(time step)计算到达目的配置的最小成本轨迹并采取第一步行动的属性。该控制结构考虑到在运行期间环境和目的配置发生变化。使用MIP，RHTP可被实现用于多面体障碍物，多面体系统约束和线性的（或分段仿射的）动力学，例如参见(Bellingham等2002,Richards等2003,Kuwata2007,Kuwata等2007)。集论控制方法（Blanchini和Miani2008）也已被应用于障碍物避免问题。例如，基于动态编程的集合迭代已被用于在避免障碍物时强力驱动该状态到原点（Rakovic和Mayne 2005），并且连接的不变集通过追踪问题已被用于解决障碍物避免（Blanchini等2004）。这两种方法均可解决运动规划问题的变化并且，同样地，均适用于避免过滤问题（Kearney等.2009）。集论方法由于环境的任何变化均需要重新计算定义避免控制法则的集合，将这些方法限制于静态环境，而不被考虑。集论方法的这种属性与描述非凸障碍物集合所必需的细节级别策略不相匹配。

发明内容

本发明的目的是提供一种用于人工指令系统的改进的碰撞避免框架。

根据本发明的第一方面，提供了一种实现用于实现最优避免过滤单元的方法，该最优避免过滤单元介于人工操作者发出的运动指令和可移动机器的对应机器控制系统之间，用于避免与物体的碰撞，该方法包括：(a)输入在可移动机器附近的物体的详细表示；(b)将物体周围的包围盒的层次集合公式化，该层次集合包括取决于可移动机器的当前位置状态的细化细节，物体越接近该机器，具有越高级别的细化细节；和(c)将得到的层次集合用作最优化问题的约束集合，以确定对发出的运动指令的任何变更以便避免同任何物体的碰撞。

优选的，该方法还包括步骤：(d)使用预测的未来运动以更新包围盒外的层次集合。在某些实施例中，步骤(c)进一步可包含步骤：(i)确定对发出的运动指令的一系列可替代变更，并按照变更的幅度计算该系列的成本，以及使用较低成本的可替代变更。包围盒集合优选为轴向对准的。

优选的，步骤(a)到步骤(c)以连续迭代的方式应用。

包围盒的层次集合优选地可包括非凸面物体的形式为层次集合内凸面的表示。

步骤(b)进一步可优选地包括，对于任一特定的时间步长，剔除(cull)该集合中在当前时间步长不可达的成员。

根据本发明更进一步的方面，提供一种最优避免过滤单元，其介于人工操作者发出的运动指令和可移动机器的对应机器控制系统之间，用于避免与物体的碰撞，该最优避免过滤单元包括：第一输入装置，用于输入可移动机器附近的物体的详细表示；层次包围盒的确定装置，用于将物体周围的包围盒的层次集合公式化，该层次集合包括取决于可移动机器的当前位置状态的细化细节，物体越接近机器，细化细节的级别就越高；最优化装置，使用得到的层次集合作为混合的整数最优化问题的约束的集合，以确定对发出的运动指令的任何变更以便避免与任何物体的碰撞，和输出所述对运动指令的变更。

附图说明

从随后的示例性实施例的描述和所附的权利要求书中，并结合附图，本发明的益处和优势对于本发明所涉及领域的技术人员将会变得显而易见。

图1图解说明给拖运卡车装载的电动矿用挖掘机；

图2图解说明了具有介于主动设备和从动设备之间的最优避免过滤单元(OAF)的遥操作系统。该OAF依据状态和障碍物集合，计算对操作者指令的加法修正(additivemodification)；

图3图解说明由多个半空间的交集组成的凸面多面体障碍物（黑色）。阴影区域表明当醒目障碍物避免约束是活动的时(其对应□＝0)的可行域。状态（黑色点）x被示出将位于可行域内；

图4图解说明对于拖运卡车货盘的不同级别的细节表示；

图5图解说明2D非凸面物体的轴向对准包围盒层次的构造；

图6图解说明基于图5示例的轴向对准的包围盒BVH；

图7图解说明对于不同状态，指令输入对使用标称轨迹而产生的最小覆盖的示例。标称轨迹由圆给出而当前位置由方块给出；

图8图解说明对来自四个不同的起始点和恒定指令的隐含的和叶型盒(leafboxes)OAF算法的比较。起始于点1，点2和点3的轨迹，以操作员指令的方向，停止于障碍物的凹面内，而起始于点4的轨迹在重新开始（resume）之前遵循操作员给出的指令沿着障碍物的侧面运动。在所有这四个仿真中，由叶节点OAF和隐含OAF产生的轨迹对应；

图9图解说明了与来自四个不同的起始点和恒定指令的根型盒（root box）OAF和叶型盒OAF相比较的标称轨迹OAF。使用标称轨迹和叶型节点OAF，起始于点1，点2和点3确定的轨迹对应。由于在MIP方案中分支的排序，起始于点4的轨迹发散；

图10图解说明标称轨迹OAF和叶型盒OAF轨迹可被看到发散。表明标称路径的虚线示出了在发散点向左右方向转向的成本是相同的；

图11图解说明对于不同OAF算法和BVH复杂性的仿真时间的比较。

图12图解说明使用命题5.1和5.2的BVH的简化。

图13图解说明对于可达约束的三种不同交叉情况。粗线表示可达约束，虚线表示不可达约束。

图14图解说明由未修正OAF算法产生的轨迹和使用可达约束方法来确定约束的轨迹的比较。除了(a)中的起因于MIQP求解器（如图10中）分支的排序的情况4之外，所有的轨迹均对应。

图15图解说明卡车货盘（左）和铲斗（右）。

图16图解说明近似卡车货盘-铲斗障碍物集合（256盒）的叶型盒。

图17图解说明在状态空间内使用OAF的装载通道的仿真。

图18图解说明使用OAF的装载通道的仿真。

具体实施方式

现在借助示例并参考附图，描述本发明的优选实施例。

优选实施例使用最优避免过滤单元（或OAF），并且它是使用滚动时域控制（RHC）框架合成，在滚动时域控制框架中，控制动作通过以下步骤确定，预测给定时界的系统的未来进化，最优化该时界的控制序列以得到最期望的未来系统进化，和在最优化的控制序列中应用第一控制动作(Rossiter2003,Maciejowski2002)。当用于避免过滤问题时，RHC具有两个有利的属性。第一，滚动时域控制的预测本性允许与从动机械手相关联的约束，如驱动器转矩和速度约束，在确定控制动作时被明确考虑。第二，即使当操作员未来指令未知时，倘若避免过滤单元是递归可行的（Rossiter2003），障碍物的未来避免可被保证。巨大的挑战是障碍物的表示。理论上，在将被四处调动的从动装置的配置空间内存在碰撞集合C_obs，被定义为其中从动装置与工作空间障碍物（或它自己）相交的配置的集合。C_obs在算术上被很好地定义，但难以计算。我们利用Smith(2008)目前的工作成果，Smith已经提出以适合于并入滚动时域控制框架的形式表示C_obs的算法。尤其，这些算法将C_obs近似为轴向对准的包围盒的层次。OAF公式化从该层次绘制了合适的表示，并将得到的约束表达为混合整数线性不等式族是令人满意的。使用C_obs的近似值，OAF被合成为混合整数程序(MIP)，该近似值记为

其由OAF从轴向对准的包围盒的层次绘制。对实时运行的要求为

中几何细节的等级和划分设置了限制。直观地，在从动机械手在预测时界内当前所处和可能要去的区域内更高的细节所期望的，而的剩余部分可被更粗略的表示。

优选实施例针对额外问题：(i)在给定从动机械手的当前状态和操作员指令的每一时间步长，如何从详细表示的级别来绘制C_obs的有效表示，和(ii)如何将该细节级别嵌入到OAF MIP中。两种策略被检查。第一种有赖于确定最合适的

作为OAF MIP的一部分。第二种有赖于使用未来运动的预测来确定适于目的的细节级别近似值，并将其提供给OAF MIP。两种策略产生相似的方案，但是第二种被证明具有非常低的计算成本。计算成本的进一步降低是通过去除对来自OAF MIP的预测时界不能有积极作用的障碍物避免约束来实现的。识别出对于如何在样品之间变化以确保OAF依然递归地可行的限制。为示出提供给激励问题的方法的适用性，给出了基于挖掘机卡车避免问题的简化仿真示例。

提出的OAF具有与RHTP相似的结构：具有混合整数不等式表示的避免约束的基于滚动时域控制的框架，但两者的区别在于它会计算对操作员当前指令的加法修正（沿着势场避免方法的路线），而不是将状态驱动到限定的目标配置的指令。

2.OAF的结构

图2概要示出了人工操作系统，由下列部分构成：

-接收输入以执行期望任务的从动机械手。该从动机械手可包括预先存在的控制系统。输入和状态受制于约束。输入通常是，尽管不总是，速率指令。

-输入设备，通过该输入设备人工操作员向从动机械手提供指令输入。操纵杆是输入设备的常见形式，并且可以是非常复杂的，如在作用力反应应用中(Slutski1998)。

-环境，其包括位置和几何形状已知的障碍物。通常，障碍物具有非凸面几何形状。从动设备不与环境中任何障碍物碰撞是期望的。

OAF介于输入设备和从动机械手（如图2所示）之间，并计算对操作员参考的加法变更，以便从动设备避免与障碍物碰撞。OAF也确保了从动机械手的约束被满足。OAF目标函数被选择用来确保来自操作员指令的变更是最小的，尽管在框架内可选择替代的目标。

2.1记法和定义

-变量是使用以下约定来表示：空间（状态和输入）使用大写字母来表示，如X;U。集合是用大写字母来表示，如P;O。集合和空间的成员是用小写字母的斜体来表示，如x;u;v。凸面的多面体是用大写字符（uppercase characters）来表示，如P;O。问题描述（数字程序）将使用大写字符如P。

-从动设备动力学使用非线性，时间不变量离散时间系统来表示：

x⁺=f(x,u),    (2.1)

其中，是系统的当前状态，

是当前输入，x⁺是后继状态。在时间步长k的状态表示为x_k。

-从动机械手对状态和输入具有约束，通常该约束是混合的。输入和状态的容许集满足：

其中，

是可容许状态的集合，

是可容许输入的集合。障碍物集合

是从C_obs到其中期望状态进化永不输入的状态空间的映射：

的正式定义为：

其中，C_p(.)将状态空间映射进入从动机械手的配置中。对应地，在状态空间内障碍物C_j的表示为：

并且每个集合的近似值分别由

和

给出。在本说明书的示例中，若干状态组成配置空间，因此C_p(x)=C_px，其中C_p是恰当大小的矩阵，

被用于表示由凸面多面体表示的障碍物集合的一部分，使得

-X_T是正不变集而相关联的回馈控制法则，该法则必须满足以下不变式和可采纳条件(Blanchini1999)：

-由操作员产生的输入序列

由

来表示。未来输入的无穷序列

由

来表示。-OAF算法计算变更的序列{v₀,v₁…v_n}由v_n来表示。无穷序列{v₀,v₁…}由v_∞来表示。

2.2最佳避免过滤单元算法

OAF算法计算对由操作员提供的指令

的加法变更v_k，用以确定过滤的系统输入

$u_k={\tilde{u}}_k+v_k,---\left(2.6\right)$

使得现在和对于所有未来时间步长(i>=0)，(i)与障碍物的碰撞被避免

和(ii)系统约束被满足

此外，OAF算法通过使用恰当的范数计算变更成本将对操作员指令变更最小化。如所提出的,由于未来操作员输入序列

是未知的，这个问题是非因果关系的（acausal）。

由于该OAF数学程序来源于N-步约束的最佳控制问题，且因果关系可通过使用恰当模型来预测未来操作员输入而得到，因此以滚动时域方式在线解决的OAF数学程序导致了约束。OAF数学程序的终点状态被约束为输入无碰撞正不变集

以便使用滚动时域控制文献(Mayne et al.2000,Rossiter2003)中的标准结果得到有保证的稳定的(Mayne et al.2000)且递归可行的(Rossiter2003)的滚动时域控制器。并入OAF数学程序中的障碍物避免约束是：

使用的操作员指令预测模型在整个规划时域期间保持当前的操作员指令保持恒定，并对于k>N设置指令输入为零：

${\tilde{u}}_{k+i}={\tilde{u}}_k,i=1,\·\·\·N-1,---\left(2.10\right)$

${\tilde{u}}_{k+i}=0,i\≥N.---\left(2.11\right)$

不变集反馈控制法则则被考虑为是变更：

$\mathrm{vk}=k\left(x_k\right),\∀k>N.---\left(2.12\right)$

在本预测模型中，预测正确的可能性在未来降低。该属性可通过折扣成本函数被包含在P_N的公式表示中：

$v_{N-1}=\arg \underset{v}{\min }\underset{k=0}{\overset{N-1}{\mathrm{\Σ}}}{\mathrm{\γ}}^k\left|\right|v_k\left|\right|,---\left(2.13\right)$

其中，0≤γ≤1is是折扣系数。得到的OAF数学程序

可形成为：

$v_{N-1}=\arg \underset{v}{\min }\underset{k=0}{\overset{N-1}{\mathrm{\Σ}}}{\mathrm{\γ}}^k\left|\right|v_k|---\left(2.14\right)$

$x_{k+1}=f(x_k,\tilde{u}+v_k),\∀k=0,\·\·\·,N-1---\left(2.15\right)$

在每一时间步长中，OAF算法可由以下步骤实现：

1）测量当前状态x_k,和当前操作员指令输入

2）求解OAF数学程序

以得到变更序列v_N-1。

3）设置v_N-1的第一元素为v_k。

4）将过滤的输入指令

发送至从动设备。

3.用于凸面多面体障碍物的OAF

假设障碍物集合

由N_O凸面多面体组成：

如图3所示，每一个

可被描述为N_h(有限)开放的半空间的交集。即：

注意 $\{x:{-a}_{i,j}^{T}x\&le;{-b}_{\mathrm{ij}}\}$ 是 $\{x:{-a}_{i,j}^{T}x<{-b}_{\mathrm{ij}}\}$ 的补码，障碍物避免约束

可被表示为：

方程式3.3是非凸面的并且还可被表示为OR（写作V）约束的群：

该结构由(Richards2002,Kuwata2003)开发，其中方程式3.4被转换为混合整数线性不等式集合，通过引入标量M使用所谓的big-M方法(Bemporad&Morari1999,Mignone2001)，如此

和对于内的每一个半空间使用二元判定变量(α_ijk)。所得到的混合整数线性不等式是：

$\begin{array}{cc}{-a}_{i,j}^{T}x\&le;{-b}_{i,j}+M_{{\mathrm{\&alpha;}}_{i,j,k}},& \&ForAll;i=1,\&CenterDot;\&CenterDot;\&CenterDot;,N_h\left(O_j\right),\end{array}---\left(3.6\right)$

$\underset{j=1}{\overset{N_h\left(O_j\right)}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_{i,j,k}\&le;N_h\left(O_j\right)-1,---\left(3.7\right)$

$\begin{array}{cc}{\mathrm{\&alpha;}}_{i,j,k}\&Element;\left\{\mathrm{0,1}\right\},& \&ForAll;1,\&CenterDot;\&CenterDot;\&CenterDot;,N_h\left(O_j\right).---\left(3.8\right)\end{array}$

其中，k代表P_N内的预测时间步长。当约束是活跃时α=0；当约束是不活跃时α=1。方程式3.5确保当约束是不活跃时，

是约束诱导的半空间的子集。方程式3.7确保通过强制O_j的避免约束中的至少一个是活跃的满足对O_j的障碍物避免约束（方程式3.6至3.8）。假如从动设备的动力学是线性的，其系统约束多面体和由多面体障碍物构成的障碍物集合O，则OAF可被表示为如下MIP：

$v_{N-1}=\arg \underset{v}{\min }\underset{k=0}{\overset{N-1}{\mathrm{\&Sigma;}}}{\mathrm{\&gamma;}}^k\left|\right|v_k\left|\right|---\left(3.9\right)$

$x_{k+1}={\mathrm{Ax}}_k+B({\tilde{u}}_k+v_k),\&ForAll;k=0,\&CenterDot;\&CenterDot;\&CenterDot;,N-1---\left(3.10\right)$

$\begin{array}{cc}{a}_{i,j}{x}_k\&le;b_{i,j}+{\mathrm{M\&alpha;}}_{i,j,k},& \&ForAll;i=1,\&CenterDot;\&CenterDot;\&CenterDot;,N_h\left(O_j\right),\&ForAll;j\end{array},=1,\&CenterDot;\&CenterDot;\&CenterDot;,\mathrm{No},$

$\&ForAll;k=1,...,N-1---\left(3.12\right)$

$\begin{array}{cc}\underset{i=1}{\overset{N_h}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_{i,j,k}\&le;N_h\left(O_j\right)-1,& \&ForAll;j=1,\&CenterDot;\&CenterDot;\&CenterDot;,N_O,\&ForAll;k=1,\&CenterDot;\&CenterDot;\&CenterDot;,N-1---\left(3.13\right)\end{array}$

假定为方程式3.9选择恰当的范数。方程式3.9至3.15的解是NP-hard(Floudas1995)，该方法具有对二元判定变量数目的指数分布的计算成本的最坏情况边界：

$(N-1)\underset{j=1}{\overset{N_O}{\mathrm{\&Sigma;}}}{N}_h\left(O_i\right).---\left(3.16\right)$

方程式3.16不包含表示不变集障碍物避免约束（方程式3.15）所需的附加二元变量，因为这依赖于不变集的选择。该附加数目范围可从对于固定不变集

的零，至对于由x_N参数化的不变集的任意大的数目。

3.1使用轴向对准的包围盒来表示障碍物

之前的工作中，Kuwata和How(2004),Richards(2005),Richards等(2003)已经使用轴向对准的包围盒（缩写为ABB）来表示（或界定）障碍物，或其零部件。ABB由在障碍物的每一轴内的最大和最小边界来表示：

B_j:=[x_min,j,x_max,j]×[y_min,j,y_max,j]×····    (3.17)

对于ABB障碍物避免的混合整数不等式由以下给出：

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)

…

$\underset{i=1}{\overset{{2N}_D}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_{i,j,k}\&le;{2N}_D-1---\left(3.22\right)$

其中，N_D是其中定义了障碍物的维度数（通常2D或3D）。对于每个ABB-障碍物，需要2N_D二元变量。

使用包围体层次（bounding volume hierarchies）延伸至非凸面障碍物

延伸OAF以避免非凸面障碍物的一个策略是将其凸面化，并避免得到的凸面表示。对于非凸面障碍物的两个最常用的凸面表示是凸分解和凸包。凸分解将非凸障碍物表示为大量的凸形区域Pi;j，因此

障碍物的凸包是包含该障碍物的最小的凸集合。使用物体的凸分解的主要不足是，它包含许多细节，因此是运算量大的表示，而凸包表示，尽管运算量较小，不允许从动设备在非凸障碍物的凹面内运动。Schouwenaars(2006)已经修改了凸包表示使其包括凸包内允许运动进入凹面的凸面多面体安全区域，但是这增加了表示的复杂度。进一步地，这些表示中的每一个都是静态的，因而在给定的情况下（由状态，指令输入对表示）可能不是最有效的表示。

优选的实施例使用细节级别（level-of-detail）途径用于避免非凸面障碍物，该途径使用从每个障碍物的包围体层次提取的表示。图4图解说明该想法，示出拖运卡车货盘的若干不同级别的细节表示，从最粗略的至最细致的。选择障碍物集合的恰当的细节级别表示，以便当与使用可用的最高细节表示相比时，计算变更vk的成本会降低，而没有显著地改变得到的变更。需要在两个目标之间进行“权衡。

4.1使用包围体层次对非凸面障碍物的表示

在现有技术中，BVH's已经被用于确定物体的任意几何模型(Gottschalk等人1996,Cohen等人1995)是否交叉。BVH通过递归地限界和划分障碍物的几何形状并将得到的包围体储存在二元树中构造的(Gottschalk et al.1996)。该构造由确定限界整个障碍物的ABB（或另一个选择的体积）开始的。这个盒是障碍物的根型盒（ABB）。然后，障碍物的几何形状沿着根ABB的最长侧的中心被细分成为两个亚几何形状，该亚几何形状又由ABB限界并储存在二元树中。该“限界-分割(bound-and-split)”过程被递归地应用到BVH的叶型盒，直至实现最小的几何尺寸或者进一步的递归不会提高精密度为止。图5和图6展示了任意封闭的2D障碍物的BVH的构造。ABB可被选择，由于它们简单且能导致有效的闵科夫斯基和操作（Minkowski sum operation）(Smith2008)。由有向包围盒(Gottschalk等人1996)组成的BVH也能够被用作可替代的细节级别表示。选自特定障碍物

的BVH的ABB的并集必须是该障碍物的超集，特别地是一覆盖。

定义A覆盖是来自

的BVH的盒B的群，如此

其中l表示细节的级别（从对于根节点以1开始），m表示该级别内的节点。Bl;m是BVH内的特定的盒。索引集I_j表示来自BVH的哪个盒被包含在表示

的覆盖内。进一步的要求是不应该有多余的盒包含在该覆盖内，即可被移除而剩余部分仍保持是覆盖的盒。如果该要求得以保持，覆盖是最小的。障碍物的最小覆盖即是，假如任何盒(ABB)被移动其将不再是覆盖的覆盖。非凸面OAF算法将基于当前状态和操作员指令选择最小覆盖作为每个障碍物的表示。以下命题考虑到向下递归BVH的最小覆盖选择算法的合成。

命题4.1。该树的每一分支（从根型盒到特定叶型盒）上存在最小覆盖的单个成员。

BVH的叶型盒形成障碍物的一部分：

其中，

是由给定盒限界的几何形状，并且

每一个叶型盒的几何形状仅仅由其先辈盒进行限界，即，仅仅

其中(.)表示对于每一级别的恰当的先辈盒。这些盒可在树的从根型盒到给定的叶型盒的分支上找到。因此，为了覆盖整个障碍物，树的每一个分支上的盒都包括在该覆盖内是必需的。假如在一个分支上有多于一个的盒，则是其他盒的子孙的盒是多余的并可被去除。最小覆盖可用两种方式选择：隐含地作为最优解的一部分或明确地使用静态的或自适应法则。现在对于每一情形的方法均被考虑。

4.2隐含非凸面OAF

在隐含非凸面OAF算法中，整个BVH被包含在OAF MIP中，并且关于最优轨迹的最粗略的最小覆盖在最优化期间被选择。通过分配最小覆盖选择二元判定变量δ_l,m,k∈{0,1}到具有子代的BVH内每一个盒，并通过向约束松弛不等式（3.22）的右手侧添加用于每一个盒β_l,m(δ_l,m,k)的最小覆盖选择函数（逻辑），将最小覆盖的选择并入OAF MIP内，

$\underset{i=1}{\overset{{2N}_D}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_{i,j,k}\&le;{2N}_D-1+{\mathrm{\&beta;}}_{l,m}\left({\mathrm{\&delta;}}_k\right),---\left(4.2\right)$

其中δ_k是对时间k的最小覆盖选择二元变量的矢量。最小覆盖选择函数基于δ_k确定该盒是否在最小覆盖内：假如β_l,m(δ_k)=0，该盒是最小覆盖的成员；假如β_l,m(δ_k)≥1，则该盒不是最小覆盖的成员。对于β_l,m(δ_k)=1，约束松弛不等式成为：

$\underset{i=1}{\overset{{2N}_D}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_{i,j,k}\&le;{2N}_D,---\left(4.3\right)$

允许对该盒的所有避免约束被松弛至整个约束集。通过对最小覆盖二元判定变量设置小成本修改OAF目标函数，使得只在得到轨迹成本（未修改的目标函数）降低结果的情况下选择更细化的细节。对于每一个盒的最小覆盖选择函数由先辈最小覆盖选择函数β_l,m(δ_k)≥0和子孙最小覆盖选择函数

组成，如果该盒是在最小覆盖内两者必须都等于零。最小覆盖选择函数变成：

${\mathrm{\&beta;}}_{l,m}\left({\mathrm{\&delta;}}_k\right)={\stackrel{-}{\mathrm{\&beta;}}}_{l,m}\left({\mathrm{\&delta;}}_k\right)+{\widetilde{\mathrm{\&beta;}}}_{l,m}\left({\mathrm{\&delta;}}_k\right).---\left(4.4\right)$

先辈成分确保了只有在其先辈均不在该最小覆盖内的情况下该盒才是最小覆盖的成员（由命题4.1）。子孙成分确定了假如

则该盒或者其一些子孙是否是最小覆盖的一部分。对于具有子代的盒的子孙最小覆盖选择算法由以下给出：

${\widetilde{\mathrm{\&beta;}}}_{l,m}\left({\mathrm{\&delta;}}_k\right)={\mathrm{\&delta;}}_{l,m,k}.---\left(4.5\right)$

假如δ_l,m,k=0，该盒可被选择用于最小覆盖（取决于选择函数的先辈部分），并且假如δ_l,m,k=1,B_l,m将会松弛以有利于其子孙。通过命题4.1，对于BVH的叶型盒

既然假如叶型盒的先辈均没有在最小覆盖内，则该叶型盒一定在最小覆盖内。先辈最小覆盖选择函数由以下给出

${\stackrel{-}{\mathrm{\&beta;}}}_{l,m}\left({\mathrm{\&delta;}}_k\right)=\underset{p=1}{\overset{l-1}{\mathrm{\&Sigma;}}}(1-{\mathrm{\&delta;}}_{p,\&CenterDot;,k}),---\left(4.6\right)$

其中，(.)表示B_l,m的恰当的先辈盒（其可通过父母关系对BVH向上递归而被确定）。假如B_l,m的所有先辈盒均不在最小覆盖内（即δ_p,.k=1;

），

并且B_l,m可能在最小覆盖内。假如B_l,m的先辈之一在最小覆盖内，

且B_l,m不能在最小覆盖内。注意，由于根型盒没有先辈，则对于根型盒

对于BVH内的具有N_L细节级别的盒的最小覆盖选择函数由以下给出：

β_1,1(δ_k)=δ_1,1,k,    (4.7)

$\begin{array}{cc}{\mathrm{\&beta;}}_{l,m}\left({\mathrm{\&delta;}}_k\right)={\mathrm{\&delta;}}_{l,m,k}+\underset{p=1}{\overset{l-1}{\mathrm{\&Sigma;}}}(1-{\mathrm{\&delta;}}_{p,\&CenterDot;,k}),& \&ForAll;m=1,\&CenterDot;\&CenterDot;\&CenterDot;{,2}^{l-1},\&ForAll;l=2,\&CenterDot;\&CenterDot;\&CenterDot;,N_L-1,---\left(4.8\right)\end{array}$

$\begin{array}{cc}{\mathrm{\&beta;}}_{M_{L,m}}\left({\mathrm{\&delta;}}_k\right)=\underset{p=1}{\overset{N_L-1}{\mathrm{\&Sigma;}}}(1-{\mathrm{\&delta;}}_{p,\&CenterDot;,k}),& \&ForAll;m=1,\&CenterDot;\&CenterDot;\&CenterDot;,2^{N_L-1}---\left(4.9\right)\end{array}$

OAF目标函数（方程式3.9）被修改，以便其选择关于最低成本轨迹可行的最粗略的最小覆盖。这是通过通过核算盒松弛的成本以利于其子孙实现的，这通过对最小覆盖选择二元判定变量的每一个设置低成本ε>0实现的。这使得只要轨迹成本作为结果被降低，MIP求解器会选择更细化的细节。

$v_{N-1}=\arg \underset{V}{\min }\underset{k=0}{\overset{N-1}{\mathrm{\&Sigma;}}}\left({\mathrm{\&gamma;}}^k\right|\left|v_k\right||+\&Element;1^T{\mathrm{\&delta;}}_k),---\left(4.10\right)$

其中1是恰当尺寸那些的列向量，而0<γ≤1是折扣率。

有限时域障碍物避免问题(Pimp(x₀;

))的隐含版本是：

$v_{N-1}=\arg \underset{V}{\min }\underset{k=0}{\overset{N-1}{\mathrm{\&Sigma;}}}\left({\mathrm{\&gamma;}}^k\right|\left|v_k\right||+\&Element;1^T{\mathrm{\&delta;}}_k),---\left(4.11\right)$

$\begin{array}{cc}{x}_{k+1}={\mathrm{Ax}}_k+B({\tilde{u}}_k+v_k),& \&ForAll;k=0,\&CenterDot;\&CenterDot;\&CenterDot;,N-1---\left(4.12\right)\end{array}$

$\begin{array}{cc}{a}_{i,l,m}{x}_k\&le;b_{i,l,m}+{\mathrm{M\&alpha;}}_{i,l,m,k},& \&ForAll;i=1,\&CenterDot;\&CenterDot;\&CenterDot;,{2N}_D,\&ForAll;m=1,\&CenterDot;\&CenterDot;\&CenterDot;,2^{l-1},\end{array}$

$\&ForAll;l=1,...,N_L,\&ForAll;k=1,...,N-1---\left(4.14\right)$

$\begin{array}{cc}\underset{i=1}{\overset{{2N}_D}{\mathrm{\&Sigma;}}}{\mathrm{\&alpha;}}_{i,l,m,k}\&le;{2N}_D-1+{\mathrm{\&beta;}}_{l,m}\left({\mathrm{\&delta;}}_k\right),& \&ForAll;m=1,...,2^{l-1},\&ForAll;l=1,...,N_L,\end{array}$

$\&ForAll;k=1,...,N-1---\left(4.15\right)$

其中，β_l,m(δ_k)由方程式4.7-4.9中的恰当的选择函数给出。由于相同的最小覆盖通常由MIP求解器在下一个时间步长选择，通过以滚动时域方式求解方程式4.11-4.17而给出的隐含的非凸面OAF算法，是递归可行的(Rossiter2003)。通过仅使用最小覆盖选择判定变量的一个集合并在每一预测步将这些用于单个最小覆盖选择函数，可以选择用于整个预测时界的该单个最小覆盖。

4.3明确的非凸面OAF

明确的非凸面OAF算法通过以下步骤而运行：

1)基于当前状态和/或操作员指令，使用静态规则或自适应算法，从BVH为每一障碍物选择恰当的最小覆盖，然后

2)求解凸面多面体障碍物（部分3）的OAF，将最小覆盖内的盒处理为凸面障碍物。

静态最小覆盖选择规则可用于选择由BVH内的所有叶型盒（表示为叶型盒OAF）组成的最精细的最小覆盖，或者需要最少数目的二元变量来表示的最小覆盖，即仅仅根型盒（表示为根型盒OAF）。简单的自适应最小覆盖选择算法将取决于距障碍物的当前距离在用于障碍物的叶型盒最小覆盖表示和根型盒最小覆盖表示之间变换。更复杂的自适应最小覆盖选择算法则可通过检查PN解的结构被合成。优化器在预测时界期间选择最低成本可行轨迹作为PN的解。由于目标函数成本与操作员指令有偏差，最低成本可行轨迹有可能在空间上接近标称轨迹：

定义标称轨迹

被定义为在N-步时界期间的预测轨迹，如果操作员指令在N-步时界(vN-1=0，因此零成本)期间保持恒定，该标称轨迹将会出现：

${\tilde{T}}_N(x_k,{\tilde{u}}_k)=\{{\tilde{x}}_k,{\tilde{x}}_{k+1},...,{\tilde{x}}_{k+N}\},$

其中， $~\tilde{x}k=\mathrm{xk}$ 而 $\tilde{x}j+1=f(\tilde{x}j;\tilde{u}k).$

因此，恰当的最小覆盖选择规则可用于对障碍物接近标称轨迹的部分选择精细细节，而对障碍物远离标称轨迹的部分选择粗略细节。由于标称轨迹被定义用于时界，单个最小覆盖将在预测时界期间使用。在标称轨迹附近分配精细细节和在其他地方分配粗略细节的最小覆盖选择规则可通过对每一障碍物的BVH向下递归而被有效执行。期望的最小覆盖(i)将包含最小数目的ABB，这样可包括任何与标称轨迹相交的叶型盒，或(ii)假如该轨迹不与任何叶型盒相交，将会选择不与标称轨迹相交的最粗略最小覆盖。最小覆盖选择规则的执行包括对BVH的每一分支向下递归直至叶型盒或不与标称轨迹相交的盒被发现并被添加到最小覆盖。由于命题4.1，对该盒的子代（假如有的话）的进一步递归停止。该方法以下被称为标称轨迹最小覆盖选择法则，见算法1（Alg.1），而利用该选择规则来选择对于每一障碍物的最小覆盖的OAF算法被称为标称覆盖OAF算法。

图7展示了对于图5和图6给出的障碍物的最小覆盖，其由四个不同的状态，操作员指令对使用Alg.1产生。每一标称轨迹由连在一起的圆表示，而当前位置由方框表示。图7(a)示出了当当前位置与位于根型盒之外且标称轨迹不与任何叶型盒相交时的最小覆盖，即不与标称轨迹相交的最粗略的最小覆盖。图7(b)产生的最小覆盖包括标称轨迹与其相交的叶型盒，和覆盖物体的剩余部分所需的最小数量的盒。图7(c)示出了当从动状态在根型盒外面并且标称轨迹不与根型盒相交时最小覆盖是根型盒。图7(d)示出了标称轨迹最小覆盖选择算法的潜在的缺陷。

这里，标称轨迹与物体的中心线交叉，并包括在最小覆盖内障碍物的相对侧的精细细节。与具有对于障碍物远侧的较粗略表示的最小覆盖相比，包括在最小覆盖内障碍物的相对侧的较精细细节不太可能改善轨迹，或者尤其是，降低了第一变更的幅度。附加的细节将提高求解MIP的运算成本。

算法2示出了明确的非凸面OAF算法的运行。getMinimalCover()调用恰当的静态或自适应规则，该规则在每一时间步长选择每一物体的最小覆盖，如所有的叶节点，或者标称轨迹最小覆盖（Alg.1）

4.3.1标称轨迹明确OAF的递归可行性

在每一时间步长，标称轨迹非凸面OAF的约束集合潜在地变化。因此，例如在(Rossiter2003)中制定的标准递归可行性条件不能保持。我们现在提供一些条件，在该条件下递归可行性适用于改变障碍物约束集合。在时间k的障碍物表示被定义为

并且是障碍物集合

的超集。假设在时间步长k有可行的轨迹：

在时间步长k+1的轨迹可被构建为是T_k的子集（假设确定性的情况）

在以下命题中提出有限递归可行性条件：

命题4.2递归可行性适用于变化的障碍物集合，当障碍物集合单调递减，即

时。

证明：假定T_k关于

是可行的，即

由于

且

则T_k+1关于

是可行的。

命题4.2的不足之处是它仅考虑到障碍物表示集合被加细；它没有考虑到假如从动设备远离它运动，则障碍物表示集合会变得更加简略。该局限在推论4.3中得以解决。

定义后验障碍物集合（posteriorix obstacle set）O(Tk)被定义为细节障碍物表示集合的最简略级别，这样

推论4.3递归可行性适用于变化的障碍物集合，假如时间k+1的障碍物集合

是的子集。

证明：直接遵循来自于命题4.2的证明。当一盒与来自前一次时间步长的P_N的解轨迹相交时，通过强制递归到盒的子代，将来自于推论4.3的用于的递归可行性条件并入标称轨迹最小覆盖选择算法（Alg.1）是可能的。对Alg.1的这种修改限制了在时间k +1的最小覆盖将是

的子集。

4.4明确的和隐含的非凸面OAF算法的评估

通过比较可替代的OAF算法在计算成本和与标称轨迹的偏差方面的性能对他们进行评估。隐含的OAF算法比照叶型盒OAF算法被评估，标称轨迹OAF算法与叶型盒和根型盒OAF算法两者相比较。用于比较仿真的动态模型是在二维的成比例地速率控制的质点质量。对于自由度的每一度（沿着x和y轴选择），离散动力学(T_s=0.2s)和约束是

$\left[\begin{array}{c}\mathrm{qk}+1\\ v_{q,k+1}\end{array}\right]=\left[\begin{array}{cc}1& 0.0865\\ 0& 0.135\end{array}\right]\left[\begin{array}{c}\mathrm{qk}\\ v_{q,k}\end{array}\right]+\left[\begin{array}{c}0.1135\\ 0.865\end{array}\right]u_{q,k}.---\left(4.18\right)$

$q\&Element;[-\mathrm{10,10}],v_q\&Element;[-\mathrm{1,1}],u_q\&Element;[-\mathrm{1,1}],10(u_q-v_q)\&Element;[-\mathrm{1,1}].---\left(4.19\right)$

用在该仿真中的不变集是零速率不变集，该不变集沿着其相关的终端反馈控制由以下给出：

u_x,T=0,u_y,T=0.    (4.21)

使用下列约束，该不变集被并入MIP OAF：

v_x,k+N=0,v_y,k+N=0,u_x,k+N=0,u_y,k+N=0.    (4.23)

注意方程式4.22将要求对终端状态施加障碍物避免约束，类似于时界的剩余部分中的约束，例如标称轨迹或隐含的避免约束。对于该仿真的障碍物集合和BVH是图5和图6分别给出的障碍物和BVH。预测时界长度设为1秒(N=5)。对偏差设置2-范数成本，和折扣率γ=1。得到的MIP公式是混合整数二次方程程序（MIQP），可使用CPLEX(ILOG2007)对其求解。

图8示出了由隐含OAF产生的轨迹对应于叶型盒OAF产生的轨迹。由于所有的最小覆盖都是叶型盒最小覆盖的超集，且MIQP求解器确定了对于MIP的全局最优解，则这些轨迹由于命题4而对应：

命题4.4考虑两种最小覆盖：

和

假如

则关于

可行的最优轨迹的成本小于或等于关于

可行的最优轨迹的成本。

证明：考虑来自于给定状态x的所有轨迹的集合，这些轨迹关于约束和最小覆盖

是可行的：

由于

则关于

可行的所有轨迹也关于

可行，因此， 所以，

中的最小成本轨迹的成本小于或等于

中的最小成本轨迹的成本。

结果依赖于方程式4.11中ε的恰当选择。假如ε大于叶节点轨迹和另一个最小覆盖的叶节点轨迹之间的成本差异，则隐含的轨迹和叶节点轨迹将不对应。叶型盒OAF或者隐含OAF的最小计算效率算法是冗余的，由于它们产生相同的轨迹。

下表示出了对于不同起始点的不同形式OAF的仿真时间（以秒为单位）。该仿真运行于具有4GB RAM的Intel Core2Duo E6300（仅为单核）上，其中OAF MIQP使用CPLEX10.2(ILOG2007)进行求解的。

该表示出了对于图8中考虑的四种轨迹，叶型盒轨迹的计算用了计算隐含OAF轨迹所用时间的约30%的时间。该比较呈现了隐含OAF公式是冗余的。隐含OAF性能不佳的两个理由为：(i)除了最佳轨迹之外，它还计算最佳的最小覆盖，和(ii)叶节点OAF是隐含OAF的子问题。通过将标称轨迹明确OAF轨迹与叶型盒OAF和根型盒OAF产生的轨迹相比较，标称轨迹明确OAF将被验证。标称轨迹OAF被公式化以产生相比于根节点低的偏差轨迹。

相比于叶型盒OAF，OAF和(ii)的平均计算效率将更高。仿真验证了该推测。在图9所示的四个仿真中的三个中，叶型盒OAF和标称轨迹OAF产生的轨迹对应，然而在第四个中，当最小成本轨迹不是唯一时（见图10），由于MIPQ求解器中的分支的顺序，叶型盒OAF和标称轨迹OAF偏离。此外，发散性通常由于叶型盒最小覆盖和标称轨迹最小覆盖之间的差异而出现，尤其是假如对标称轨迹的扰动将导致替代的最小覆盖。上述表格中的仿真时间示出了标称轨迹OAF比叶型盒OAF计算效率更高，模拟用了叶型盒OAF的计算时间的约20%-25%的计算时间。

对于不同OAF算法的二元变量的数目由下表给出：

图11示出了四个不同OAF算法的仿真时间如何关于BVH的复杂度而变化，该复杂度由BVH二元树内的级别数(N_L)给出。隐含和叶型盒OAF的仿真时间由于二元变量数目的增加而显著增加。该增加的发生是由于两个公式化所需的二元变量的数目关于N_L(见表2)是指数的，且MIP的最坏情况计算成本关于二元变量的数目是指数的。图11也示出了标称轨迹OAF确实提高，尽管没有叶型盒OAF的提高幅度大，且比叶型盒OAF的成本小。由于N_L对其运行时间没有影响，根型盒算法是恒定的。

5.使用可达集降低计算成本

OAF的计算成本可通过这样的步骤进一步降低，从MIP移除障碍物或其在在预测时界内的给定的时间步长不可达的部分。在部分2和部分4示出的OAF公式的文本中，可达性可被用于(i)通过移除ABB和不可达的树分支，简化给定障碍物的BVH，和(ii)从OAF MIP移除在给定的预测步不可达的多面体障碍物和约束。依据在给定的时间段可以达到的状态空间X内的区域定义可达性。

定义：一步可达集被定义为包含给定状态的所有可能后继状态的集合或状态的集合，即：

定义：i-步可达集合被递归地定义为一步可达集合的重复应用：

其中

5.1使用可达集包围体层次的简化

使用可达性，可降低计算中被考虑的BVH内的ABB的数目。该降低是通过以下完成的：(i)剔除不可达的BVH的盒和分支，和(ii)仅当盒的多个子代之一是可达的时，用BVH内的该子代置换一盒。该BVH简化策略依赖于以下命题：

命题5.1：假如BVH内的ABB是不可达的（即不与可达集相交），则其子孙没有一个是可达的。

证明：假设

是B的子孙盒，

被定义为由

限界的几何形状。由于B是B的子孙，则

通过

的传递性，

因此假如B由于其为非可达的而被剔除，则其子孙也应被剔除。

命题5.2：假如可达集R与盒B_lm以及盒子代B_l+1,q1中的仅一个子代相交，则盒B_l;m可由BVH内的其子代B_l+1;q所替代。

证明：假设B_l+1,q1和B_l+1;q2是B_l;m的两个子代盒。并且B_l;m和B_l+1;q1与可达集R相交，而B_l+1;q2不相交。因为

且

因此，

命题5.1和5.2可被共同用于合成一种算法，该算法可通过横移树而简化给定可达集

的BVH。该递归算法第一确定候选盒的子代是否可达，并从BVH移除所有的不可达的子代及其后继。假如只有一个子代剩余，则它替换BVH内的候选盒，且对其运行递归算法。假如多于一个子代是可达的，候选盒保留在树内并继续对其可达的子代递归。图12示出了该递归算法如何被用于简化BVH，其中有色的点代表与可达集

相交的盒。图12(a)示出整个BVH，图12(b)示出简化的BVH。BVH的简化将导致用于表示障碍物所需的二元变量数目的减少或者该表示的细节的增加。

5.2使用可达性降低多面体障碍物约束

通过从OAF MIP剔除不可达的障碍物和约束，在给定预测时间步长代表

所需的二元变量的数目可被降低。这种想法类似于由Kuwata(2003)和Richards等人(2003)之前提出的想法，他们二人使用整个预测时界的可达集的近似值来来从OAF MIP剔除表示该集合之外的障碍物的约束。Culligan(2006)通过对于规划时界内每一时间步长，包含只在该时间步长可达的障碍物，而进一步降低MIP内二元变量的数目。

通过仅包括代表与障碍物集合中与可达集相交的部分所必需的约束，可实现进一步的新的降低。特别地，其降低了代表凸面多面体障碍物O_j所需的约束的数目（因此，二元变量），该凸面多面体障碍物不完全在可达集

的内部。只有对于在

内的某些状态活跃的约束被选择。假如以下情况，则约束{x:a_ijx≤b_ij}被选择：

然后，可达约束通过以下索引集表示：

且诱导的障碍物由以下给出

诱导的障碍物是可达集

的子集，并且还可被表达为O_j和

的交。

如果障碍物O_j与可达集相交，则出现三种可能的情况：

1．

（见图13(a)）：此处，需要所有的半空间来表示障碍物。方程式3.6和3.7的公式用于确定O_j的约束。时间k的诱导的障碍物由O_jk=O_j给出。

2.

其中两个或者更多的约束可达，即|I_jk|≥2（见图13(b)）。

在时间k对于O_j的混合整数线性不等式为：

其中|·|表明索引集中元素的数目。

3．

其中仅仅单个约束可达，即|I_jk|=1（见图13(c)）。由于仅需要单个线性不等式来表示O_j在

内，则不需要二元变量。该约束为：

使用可达约束方法可从OAF MIP移除的二元变量的数目，将取决于从动设备的动力学，可达集合接近真实的可达集合的接近度，和障碍物的几何形状。注意，当

时，二元变量的数目将不再降低；在这种情况下，其他的OAF算法例如部分2和部分4所示出的，可被使用。

5.3用于轴向对准的包围盒的可达约束方法

对于障碍物O_j和可达集（或者其近似值）

是ABB的情况，可达约束可通过修改用于测试一对ABB是否相交(Cohen等人1995)的标准算法而被有效确定。该算法通过投射这些盒到每个轴上并确定该投射是否重叠来确定两个ABB是否涉及相交。如果投射在所说有轴上重叠，则ABB重叠。修改的算法储存障碍物的边界是否作为布林变量I_i;j;k重叠，和假如障碍物与可达集相交，这些变量被用于直接地确定可达约束索引集I_j,k。

由于对于标准算法的修改仅为(i)I_i,j,k变量的储存，和(ii)集合I_j,k的约束，修改的算法相比于标准的ABB交集测试算法在计算资源上仅需要较小的提高。修改的求交算法在2D的运行在算法3中示出：

仅通过极小的修改，该算法可被延伸到更高的维度（投射到新的坐标轴并对于ABB的交集要求额外的投射重叠）。

5.4对于可达约束的递归可行性

我们现在示出可达约束方法在应用于情况时是递归可行的，(i)恒定的障碍物集合和(ii)可在每一时间步长依据推论4.3而变化的障碍物集合

命题5.3递归可行性适用于(i)恒定的障碍物集合，和(ii)可在每一时间步长依据推论4.3变化的障碍物集合，其中约束使用约束可达方法确定。

证明部分(ii)被首先考虑。假设在时间k存在可行轨迹：

满足以下约束

$\begin{array}{cc}{x}_{k+p+1}=f(x_{k+p},u_{k+p}),& \&ForAll;p=0,...,(N-1),---\left(5.10\right)\end{array}$

其中

假设在未来T_k可被精确执行，则有可能构建下一个时间步长的轨迹，

该轨迹是在前时间步长(T_k)的轨迹的子集。为建立递归可行性，有必要示出如果T_k是可行的，则T_k+1也是可行的。由于T_k+1是T_k的子集，则动力学约束和系统约束（方程式5.10和5.11）对T_k+1适用。由于χ_T的不变形，则方程式5.13和5.14中的约束对T_k+1适用。

示出该轨迹满足来自于x_k+1的可达集诱导的障碍物避免约束仍是必要的，即：

由于

是

的限定，且有在时间k+1的状态为x_k+1的附加约束）和

其中

是时间k的后验的障碍物集合（以推论4.3为条件）。诱导的障碍物集合有如下关系：

由方程式5.17，和事实其遵循：

因此，如果方程式5.10至5.14的约束对于T_k均适用，则所有的约束对T_k+1均适用。当障碍物集合是恒定时，方程式5.17给出的递归可行性条件保持不重要，由于T_k关于

是可行的，因此(i)也满足。

5.5使用可达约束方法对OAF算法的评估

使用可达约束方法，叶型盒和标称轨迹OAF算法可与对应的未修改的OAF算法进行评估。与对应的未修改的OAF方法相比，使用可达约束方法的OAF算法应当产生相同的轨迹，且计算效率更高。在部分4.4展示的动力学模型和OAF公式再一次用于这些仿真。

使用ABB的可达约束方法，要求对可达集的ABB近似值可被计算。可使用类似于Culligan(2006)提出的方法的方法来计算这些ABB。，近似可达集

可通过求解下列模型来计算：

$x_{k+p,\min }=f(x_{k+p-1,\min },u_{\min }),\&ForAll;p=1,...,N,---\left(5.19\right)$

$x_{k+p,\max }=f(x_{k+p-1,\max },u_{\max }),\&ForAll;p=1,...,N,---\left(5.20\right)$

x_k,max=x_k,min=x_k.    (5.21)

其中最大输入和最小输入由以下给出：

该公式将产生对线性系统的可达集的外逼近，该线性系统的系统矩阵具有全正元素或零元素，例如部分4.4提出的模型。

图14和下表示出了使用可达约束方法的OAF算法和未修改的OAF算法之间的比较。图14示出了除叶型盒OAF的轨迹4之外，每个起始点的轨迹对于叶节点OAF和标称轨迹OAF对应。该行为是由于当最小成本轨迹不是唯一时，MIPQ内分支的排序。表3示出了使用可达约束方法的OAF算法的运行时间明显短于对应的未修改OAF算法。因此，可达约束方法应当被用于可达集和得到的可达约束可被有效确定的情况，如当使用ABB表示可达集和障碍物时。

下表示出了未修改的OAF算法和使用可达约束方法的OAF算法之间的仿真时间比较。这些仿真在RAM为4GB的Intel Core2Duo E6300（仅为单芯）上运行，其中OAF MIQP使用CPLEX10.2(ILOG2007)求解。时间以秒为单位。

6.基于简化的矿用电铲-卡车避免问题的仿真示例

本部分考虑的示例是简化的笛卡尔挖掘机，其中指令铲斗（图15，右方）的速率操作员将材料装载在卡车货盘（图15，左方）中。

本部分模拟的情景是操作员用铲斗对空的卡车货盘进行其首次装载的装载通道，但是未能举起或者铲斗停止在卡车货盘内。这被模型化为在仿真过程中恒定的操作员指令输入。使用可达约束方法的标称轨迹OAF算法，和1秒（或5个样品）的前瞻将被用于避免碰撞。在本示例中的动力学和运动力学已被简化：运动为纯平移，且每一自由度（DOF）具有成比例的速率反馈的二重积分动力学。每一DOF均与笛卡尔轴对准。x DOF具有两倍的有效惯性，且可以y DOF和z DOF速度的两倍行进。

对该系统的离散时间模型Δt=0:2s)为：

$\left[\begin{array}{c}{x}_{k+1}\\ v_{x,k+1}\\ y_{k+1}\\ v_{y,k+1}\\ z_{k+1}\\ v_{z,k+1}\end{array}\right]=\left[\begin{array}{cccccc}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\end{array}\right]\left[\begin{array}{c}{x}_k\\ v_{x,k}\\ y_k\\ v_{y,k}\\ z_k\\ v_{z,k}\end{array}\right]+\begin{array}{c}\\ \\ \end{array}\left[\begin{array}{ccc}0.07358& 0& 0\\ 0& 0.1135& 0\\ 0& 0& 0.1135\\ 0.6321& 0& 0\\ 0& 0.8647& 0\\ 0& 0& 0.8647\end{array}\right]\left[\begin{array}{c}{u}_{x,k}\\ u_{y,k}\\ u_{z,k}\end{array}\right].---\left(6.1\right)$

该系统的对应的速度，指令和驱动器约束是

v_x∈[-2,2],v_y,v_z∈[-1,1],   (6.2)

u_x∈[-2,2],u_y,u_z∈[-1,1],    (6.3)

10(u_q-v_q)∈[-1,1],q=x,y,z.    (6.4)

再一次，零速度，无碰撞不变集被用作OAF终端不变集：

相关的终端反馈控制是:

u_x,T=0,u_y,T=0,u_z,T=0.    (6.6)

使用下列约束将不变集和相关控制法则包括在OAF MIQP内：

v_x,k+N=0,v_y,k+N=0,v_z,k+N=0,    (6.8)

u_x,k+N=0,u_y,k+N=0,u_z,k+N=0.(6.9)

由于铲斗的运动是纯平移，则可使用闵科夫斯基和（Minkowski Sum）来表示物体-物体避免约束。闵科夫斯基和被定义为两个集合A和B的穷举总和：

表示给定状态x（由于铲斗的运动为纯平移）的铲斗的几何形状的集合为：

其中

是从状态到位置空间（注意通常，状态和位置空间之间的关系可能不是线性的，尤其是如果涉及旋转状态的话）的投射矩阵，且

是表示状态在原点时的铲斗几何形状的集合。因此，用于笛卡尔挖掘机的物体-物体障碍物避免约束被给出：

其中

表示卡车货盘的几何形状。使用闵科夫斯基和，方程式6.11可被转换为点-物体（point-object）约束：

其中， $-x=\left\{\begin{array}{ccc}-x,\&ForAll;x& \&Element;& x\end{array}\right\}.$

使用以Smith (2008)提出的以确定闵科夫斯基边界树的方法为基础的方法，细节级别的点-多面体（point-polytope）避免约束被计算：构建用于货盘的ABB的BVH，并且通过以铲斗的ABB（有效地铲斗BVH的根型盒）获取卡车货盘BVH盒方式（box-wise）的闵科夫斯基和，找到障碍物集合的BVH。图16示出了铲斗-卡车货盘障碍物集合的闵可夫斯基边界树的叶型盒。

图17示出了表示状态空间障碍物和得到的轨迹（白色球体）的BVH的叶型盒，而图18示出了铲斗和卡车货盘相对运动的对应快照。两图均示出了铲斗成功避开了同铲的碰撞。

7.结论

优选实施例提供了介于人工操作者和从动机械手之间的有效OAF，以便通过最低限度地改变操作者指令来帮助操作者避免碰撞。OAF公式提供了在帮助人工操作者避免障碍物方面固有的挑战，即处理问题的非因果关系的结构，并对当确定改变操作者指令时考虑系统的动力学和性能限制。本论文的主要贡献是将细节的几何级别并入OAF框架，以产生用于避免非凸面障碍物的计算效率高的算法。当前的结果，尽管只是基于仿真，是十分有前景的，它启发了OAF可在实际中用于恰当的应用。

解释

整个说明书所提及的“一个实施例”或“实施例”意思是结合该实施例所描述的特定特征、结构或特性被包括在本发明的至少一个实施例中。因此，在整个说明书的不同地方出现的短语“在一个实施例中”或“在实施例中”不一定都是指同一个实施例。此外，所述特定的特征、结构或特性可以按任何恰当的方式在一个或多个实施例中组合，本领域普通技术人员根据本公开应当清楚这一点。

类似地，应当理解，在本发明的上述示例性的实施例中，为了使本发明公开内容流畅并有助于理解各个发明方面中的一个或多个方面，本发明的多个特征有时是组合在单个实施例、图或其描述中。但是，公开内容中本发明的方法不应被解释为放映了本发明，所请求保护的发明要有比每一权利要求中所明确记载的特征更多的特征。相反，如所附权利要求所反映的，本发明的各个方面可依赖少于前面所公开的单个实施例的所有特征。从而，附在详细描述之后的权利要求被明确并入该详细描述中，每个权利要求单独作为本发明的独立的实施例。

进一步地，尽管这里描述的一些实施例包括其他实施例中的一些特征，但不包括其他实施例中的各其他特征，但是本领域技术人员应当理解的是，不同实施例的特征的组合意图落在本发明的范围之内，且形成不同的实施例。例如，在所附权利要求中，任何要求保护的实施例可被用于任何组合中。

进一步的，在本文一些实施例被描述为方法或者可由计算机系统的处理器或执行功能的其他装置实现的方法的元素的组合。因此，具有用于执行这样的方法或方法的元素的所需指示的处理器形成了执行该方法或方法的元素的装置。进一步地，在此描述的装置实施例的元素是为了执行本发明的目的执行由元素所执行的功能的装置的示例。

在本文的描述中，给出了大量的具体细节。然而，可以理解的是在没有这些具体细节的情况下，本发明的实施例也可以实现。在其他例子中，为了不影响对本说明书的理解，没有详细示出众所周知的方法、结构和技术。

除非另外指明，否则本文使用顺序形容词“第一”，“第二”，“第三”等来描述常见物体只是表示所引用的相似物体的不同示例，并且不希望隐含所描述的物体在时间上、空间上、级别上或者在任何其他方式上处于给定的顺序。

在下面的权利要求中和本文的描述中，术语“包括”、“由构成”或“其包括”的任一个都是开放式术语，意思是至少包括后述的元素/特征，但不排斥其他的。因此，当用于权利要求中时，术语“包括”不应被解释为局限于其后所列出的手段或元素或步骤。例如，以下表述“设备A包括A和B”的范围不应当被限制为设备仅仅由元件A和B构成。这里所使用的术语“包括”或者“其包括”中的任一个也都是开放式的术语，也表示至少包括该术语之后的元素/特征，但是不排除其他特征。因而，所有的“包括”都是同义的。

类似地，要注意当在权利要求中使用术语“耦合”时，该术语不应被解释为仅限于直接连接。因为术语“耦合”和“连接”以及它们的派生词都可以使用。应该理解的是，这些术语并不意味着它们彼此是同义词。因而，表述“耦合到设备B的设备A”的范围不应当被限制为其中设备A的输出被直接连接到设备B的输入的设备或系统。着意味着在A的输出和B的输入之间存在路径，该路径可以是包括其他设备或装置的路径。“耦合”意味着两个或多个元件或者直接物理接触或者电接触，或者两个或多个元件彼此没有直接接触但是依旧彼此配合或相互作用。

虽然特别参考某些优选实施例对本发明进行了描述，但是在下述权利要求的精神和范围内，可实现对本发明的改变和修改。

Claims (11)

1.一种实现最优避免过滤单元的方法，该最优避免过滤单元介于人工操作员发出的运动指令和可移动机器的对应机器控制系统之间，用于避免与物体的碰撞，所述方法包括：

(a)输入在可移动机器附近的物体的详细表示；

(b)将该物体周围的包围盒的层次集合公式化，所述层次集合包括取决于可移动机器的当前位置状态的细化细节，物体越接近该机器具有越高级别的细化细节；和

(c)将得到的层次集合用作最优化程序的约束集合，以确定对发出的运动指令的任何变更，以便避免同任何物体的碰撞。

2.根据权利要求1所述的方法，其中所述约束集合包括混合整数约束，而所述最优化程序包括混合整数最优化程序。

3.根据权利要求1所述的方法，进一步包括步骤：

(d)使用预测的未来运动来更新包围盒外的层次集合。

4.根据权利要求1所述的方法，其中所述步骤(c)进一步包括步骤：

(i)确定对发出的运动指令的一系列可替代变更，并按照变更的幅度计算该系列的成本，以及使用较低成本的可替代变更。

5.根据任一前述权利要求所述的方法，其中所述步骤(a)至(c)以连续迭代的方式应用。

6.根据任一前述权利要求所述的方法，其中，所述包围盒的集合是轴向对准的。

7.根据任一前述权利要求所述的方法，其中，所述包围盒的层次集合包括非凸面物体的形式为层次集合内凸面的表示。

8.根据任一前述权利要求所述的方法，其中，所述步骤(b)进一步包括，对于任一特定的时间步长，剔除该集合中在当前时间步长不可达的成员。

9.一种最优避免过滤单元，介于人工操作员发出的运动指令和可移动机器的对应机器控制系统之间，用于避免与物体的碰撞，该最优避免过滤单元包括：

第一输入装置，用于输入在可移动机器附近的物体的详细表示；

层次包围盒确定装置，用于将该物体周围的包围盒的层次集合公式化，所述层次集合包括取决于可移动机器当前位置状态的细化细节，物体越接近该机器具有越高级别的细化细节；和

最优化装置，用于将得到的层次集合用作混合整数最优化问题的约束集合，以确定发出的运动指令的任何变更，以便避免同任何物体的碰撞，和输出所述对运动指令的变更。

10.一种实现最优避免过滤单元的方法，大体如以上参照附图所述。

11.一种最优避免过滤单元，大体如上参照附图所述。

Images