机器人在观测受限下拾取及放置物体的任务规划方法及系统

技术领域

本发明涉及机器人任务规划技术领域，具体地，涉及机器人在观测受限下拾取及放置物体的任务规划方法及系统，用于安排机器人完成任务所需的动作序列，特别是在物体信息不完整的情况下对机器人拾取及放置物体任务的规划。

背景技术

很多情况下机器人需要通过多次拾取及放置不同物体来达到某一任务目标，比如将杂乱物体摆放整齐，或在杂乱物体中寻找目标物体。一般机器人拾放物体的任务规划算法假设完全已知所有物体的信息，包括每个物体的名称和位姿。当机器人只能通过自身观测来获取物体信息时，由于物体互相遮挡，机器人很可能无法观测到全部物体的信息，给规划带来了不确定性。

部分可观测马尔可夫决策过程(POMDP)，是一种描述不确定性下决策过程的通用模型。POMDP规划是在不确定性下计算具有最大期望回报的动作。POMDP规划很难离线一次性计算出完整的策略，一般采用在线规划的方式，即每一步只规划机器人这一步需要执行的动作，执行动作并重新观测后，再规划下一步的动作。广泛使用的在线POMDP规划方法是信念树搜索，目前较先进的信念树搜索算法包括DESPOT和POMCP等。

尽管POMDP规划已成功应用在各行各业，但是应用在机器人物体拾放任务中仍然有不足之处。机器人物体拾放任务中并非每一步所有动作都是可行的，而过去的信念树搜索算法没有考虑这一点，在构建信念树时，每个结点下扩展的动作分枝通常来自整个动作空间。有时候会假设每一步有哪些动作可行是已知的，构建信念树时，每个结点下扩展的动作分枝来自可行的动作。但机器人物体拾放任务中每一步有哪些动作可行并非事先已知，一个拾取或放置物体的动作是否可行，取决于运动规划能否为这个动作找到可行的运动轨迹。

专利文献CN113190012A(申请号：202110506117.7)公开了一种机器人任务自主规划方法及系统。其中，该方法包括基于家庭环境语义知识模型，获得静态物品语义位置和静态物品与动态物品之间的位置关系；基于静态物品语义位置和静态物品与动态物品之间的位置关系，根据混合任务规划器来执行动作规划，直至机器人执行的任务序列完成任务；其中，混合任务规划器在执行动作规划的过程中，首先进行离线任务规划，并确定离线任务规划的动作影响是否为确定型来判断是否继续执行离线任务序列，当离线任务规划的动作影响为不确定型，然后再进行在线动作规划。专利文献CN112131754A(申请号：202011060344.3)公开了一种基于机器人伴随行为模型的扩展POMDP规划方法及系统，包括在标准POMDP规划过程中，当正在执行的任务动作aT的不变式与某一观察动作aO匹配时，将任务动作aT、观察动作aO基于匹配的谓词语句构成伴随关系形成伴随行为模型；在任务动作aT的执行过程中，获取观察动作aO的观察值obs；基于任务动作aT的不变式、观察值obs更新机器人的系统知识库kb；判断知识库kb中的不变式的真值为假是否成立，若成立则触发任务重规划。上述两个专利提供了基于POMDP的机器人任务规划算法，但是没有考虑动作的可行性

专利文献CN112356031A(申请号：202011220903.2)公开了一种基于Kernel采样策略在不确定性环境下的在线规划方法，用于对机器人执行任务时的规划，在该不确定性环境中，表示为POMDP模型的不确定性是制约机器人可靠运行的主因；所述POMDP模型中，机器人可观测自身的部分状态，机器人通过不断的与环境进行交互来获得回报最大的策略；在所述在线规划方法中，处理可观测部分时，把机器人的状态表示为一个信念，记为belief，其属于一个状态的集合，以POMDP算法通过构建信念树的方式执行前向搜索，以此来获得当前信念下的最优策略；所述信念树的每一个节点代表一个信念，父节点与子节点通过行为-观测分支连接；所述POMDP算法是在线POMDP规划算法Kernel-DESPOT。专利文献CN114118441A(申请号：202111401793.4)公开了一种基于高效搜索策略在不确定性环境下的在线规划方法，将机器人的状态视为一个信念，以POMDP算法初始化当前信念的上、下边界后，通过折扣化上下限表示当前信念的全部信息进而执行前向搜索构建信念树，以此获得当前信念下的最优策略；所述信念树的每一个节点代表一个信念，父节点与子节点通过行为-观测分支连接。上述两个专利改进了DESPOT算法，但也没有考虑动作可行性。对于机器人需要拾取和放置物体的任务，如果物体摆放杂乱且拥挤，不考虑动作可行性会导致规划结果不可靠，优化性能较差。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种机器人在观测受限下拾取及放置物体的任务规划方法及系统。

根据本发明提供的一种机器人在观测受限下拾取及放置物体的任务规划方法，包括：

步骤S1：对信念树搜索算法进行改进，包括：建立动作可行概率、基于动作可行概率设置信念树动作分枝扩展以及设置修正回报函数；

步骤S2：利用改进后的信念树搜索算法进行任务与规划和执行。

优选地，所述建立动作可行概率采用：

步骤S1.1：在观测z下，把所有可能可行的动作放入可用动作集合A

步骤S1.2：观测z下动作a∈A

步骤S1.3：将任务中可能出现的物体随机布置，并获得观测z，记录每一个动作a∈A

优选地，所述基于动作可行概率设置信念树动作分枝扩展采用：

对于根结点b

优选地，所述修正回报函数采用：修正回报函数Γ(h(s)，a)≤0，其中，h(s)是从任务开始到状态s经历的动作和观测历史，其根据历史信息对执行的动作进行评价。

优选地，所述步骤S2采用：

步骤S2.1：对当前信念进行更新；

步骤S2.2：利用改进后的信念树搜索算法规划机器人当前需要执行的动作；

步骤S2.3：机器人执行规划的动作，然后获得新的观测；

步骤S2.4：新的观测下对动作可行概率进行更新；如果任务完成，结束任务，否则重复触发步骤S2.1至步骤S2.4。

优选地，所述步骤S2.2采用：在构建信念树时，每个结点下扩展的动作分枝根据改进后的信念树搜索算法确定；在搜索信念树时，回报值是回报函数与修正回报函数之和。

优选地，所述步骤S2.4采用：

设机器人当前观测z下的可用动作集合为A

其中，N＞0是滑动平均的周期，近似看作利用近期N个样本进行概率估计。

根据本发明提供的一种机器人在观测受限下拾取及放置物体的任务规划系统，包括：

模块M1：对信念树搜索算法进行改进，包括：建立动作可行概率、基于动作可行概率设置信念树动作分枝扩展以及设置修正回报函数；

模块M2：利用改进后的信念树搜索算法进行任务与规划和执行。

优选地，所述建立动作可行概率采用：

模块M1.1：在观测z下，把所有可能可行的动作放入可用动作集合A

模块M1.2：观测z下动作a∈A

模块M1.3：将任务中可能出现的物体随机布置，并获得观测z，记录每一个动作a∈A

所述基于动作可行概率设置信念树动作分枝扩展采用：

对于根结点b

所述修正回报函数采用：修正回报函数Γ(h(s)，a)≤0，其中，h(s)是从任务开始到状态s经历的动作和观测历史，其根据历史信息对执行的动作进行评价。

优选地，所述模块M2采用：

模块M2.1：对当前信念进行更新；

模块M2.2：利用改进后的信念树搜索算法规划机器人当前需要执行的动作；

模块M2.3：机器人执行规划的动作，然后获得新的观测；

模块M2.4：新的观测下对动作可行概率进行更新；如果任务完成，结束任务，否则重复触发模块M2.1至模块M2.4；

所述模块M2.2采用：在构建信念树时，每个结点下扩展的动作分枝根据改进后的信念树搜索算法确定；在搜索信念树时，回报值是回报函数与修正回报函数之和；

所述模块M2.4采用：

设机器人当前观测z下的可用动作集合为A

其中，N＞0是滑动平均的周期，近似看作利用近期N个样本进行概率估计。

与现有技术相比，本发明具有如下的有益效果：

1、在利用基于信念树搜索的POMDP在线规划算法规划机器人拾取及放置物体的任务时，构建的信念树上每个结点下扩展的动作分枝都来自父观测分枝下的可行动作集合，充分考虑了动作可行性，保障了规划的可靠性。

2、建立动作可行概率，信念树上除了根结点以外的结点下扩展的动作分枝的可行性通过动作可行概率采样确定，节省了运动规划的调用次数，提高了规划速度。

3、设计了修正回报函数，根据任务执行的历史信息对信念树搜索方向进行实时调整，抑制不希望的搜索方向。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为信念树示例。

图2为规划与执行流程。

图3为实施实例的任务场景。

图4为实施实例的任务目标。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例1

本发明用于在机器人需要拾取及放置物体的任务中，规划机器人每一步该执行什么动作，即规划拾取和放置物体的操作顺序。设机器人只能靠自身观测物体信息，而物体又是杂乱摆放的，可能因互相遮挡无法直接获得所有物体的信息。本发明在采用信念树搜索的POMDP在线规划算法基础上进行了一些改进，使得在规划中充分考虑了动作可行性，提高了规划的可靠性，同时又不明显增加规划的计算量。

本发明的算法要求已知任务过程的POMDP模型。下面对POMDP模型和规划原理进行简要介绍。

离散POMDP模型定义为一个元组(S，A，Z，T，O，R)，S，A，Z分别表示状态空间、动作空间以及观测空间，T，O，R分别表示转移函数、观测函数以及回报函数。每一步，机器人在状态s∈S下执行一个动作a∈A，并获得回报r＝R(s，a)，然后状态以概率T(s′|s，a)转移到s′∈S，机器人以概率O(z′|s′，a)获得新的观测z′∈Z。

在机器人拾取及放置物体的任务中，一般状态定义为所有物体的名称和位姿。观测定义为机器人直接观测到的物体的名称和位姿。动作定义为拾取某一物体，或将物体以某一位姿放置。转移函数和观测函数根据具体任务模型确定，也有一些方法可以学习转移函数和观测函数。回报函数根据优化目标确定，一般希望任务完成得越快越好，每执行一个动作都会花费一定的时间代价，对应一个负的回报。

由于真实状态不可知，机器人需要持续维护一个信念b∈B，表示状态空间中的概率分布，B是所有可能的信念的集合，称为信念空间，b(s)表示信念b中状态为s的概率。每一步机器人都会根据执行的动作a和获得的观测z′将原先信念b更新为b′，更新过程用b′＝τ(b，a，z′)表示。

策略为一个从信念空间到动作空间的映射π：

其中γ∈[0，1]是折扣因子，代表远期回报相对近期回报的重要程度。POMDP规划就是寻找使值函数尽可能大的策略。

一次性计算出完整的策略很难，一般只在每一步通过在线规划计算当前需要执行的动作。广泛使用的在线POMDP规划算法是信念树搜索。每一步以当前信念作为信念树的根结点b

使用Δ(b)表示结点b在信念树上的深度。A

利用信念树搜索最优策略π

从叶结点往上层层递推可获得根结点b

根据本发明提供的一种机器人在观测受限下拾取及放置物体的任务规划方法，包括：

步骤S1：对信念树搜索算法进行改进，包括：建立动作可行概率、基于动作可行概率设置信念树动作分枝扩展以及设置修正回报函数；

步骤S2：利用改进后的信念树搜索算法进行任务与规划和执行。

本发明可与任意采用信念树搜索的POMDP在线规划算法结合，在实施本发明时首先要确定具体选择哪一种信念树搜索算法。同时本发明在规划中充分考虑了动作可行性，一个动作是否可行，取决于运动规划能否为这个动作找到可行的运动轨迹，所以也需要选择适用于任务动作的运动规划算法。

所述建立动作可行概率采用：

在任务过程中并非每一步所有动作都是可行的，本发明定义了可用动作集合和可行动作集合。可用动作集合A

步骤S1.1：设计可用动作集合筛选规则。

根据观测z，有一些动作是明显不可行的，而一些动作是有可能可行的。筛选可用动作集合的规则，就是根据简单的逻辑判断，把那些有可能可行的动作放入可用动作集合A

步骤S1.2：设计动作特征量计算方法。

设σ(z，a)是根据观测z计算出的关于可用动作a∈A

步骤S1.3：估计动作可行概率值。

接下来对动作可行概率的具体值进行估计。将任务中可能出现的物体随机布置，并获得观测z，筛选观测z下可用动作集合A

所述基于动作可行概率设置信念树动作分枝扩展采用：

所有信念树搜索算法都需要构建信念树。信念树上每个结点b下扩展的动作分枝都必须来自此结点的父观测分枝z

在构建信念树时，根结点下扩展的动作分枝代表机器人当前可能要执行的动作，需要严格检查动作可行性。但是其他结点下扩展的动作分枝与机器人实际执行的动作无关，对动作可行性的准确性要求不严格。此时可以利用动作可行概率对动作可行性进行采样。

对于根结点b

所述设计修正回报函数采用：

由于信念树上的动作分枝的可行性不能保证完全准确，有时可能会误导信念树搜索方向。本发明设计了修正回报函数Γ(h(s)，a)≤0，其中h(s)是从任务开始到状态s经历的动作和观测历史。修正回报函数仅用于信念树搜索，不代表实际回报，其根据历史信息对执行的动作进行评价，可人为抑制不希望的搜索方向。

如图2所示，所述步骤S2采用：

步骤S2.1：对当前信念进行更新。如果当前是任务第一步，根据信念树搜索算法提供的信念表示法建立初始信念。如果当前不是任务第一步，根据信念树搜索算法提供的信念更新方法，对当前信念进行更新。

步骤S2.2：用信念树搜索算法规划机器人当前需要执行的动作。在构建信念树时，每个结点下扩展的动作分枝根据步骤3确定。在搜索信念树时，回报值r＝R(s，a)+Γ(h(s)，a)，是回报函数与步骤4设计的修正回报函数之和。

步骤S2.3：机器人执行规划的动作，然后获得新的观测。

步骤S2.4：对动作可行概率进行更新。设机器人当前观测z下的可用动作集合为A

其中N＞0是滑动平均的周期，可以近似看作利用近期N个样本进行概率估计。

步骤S2.5：如果任务完成，结束任务，否则返回步骤5.1。

综上所述，本发明可结合任意采用信念树搜索的在线POMDP规划算法。不同的规划算法可能对信念表示、信念跟新、信念树构建以及信念树搜索方式有不同的特殊处理，但均适用于本发明的内容。

本发明提供的机器人在观测受限下拾取及放置物体的任务规划系统，可以通过本发明提供的机器人在观测受限下拾取及放置物体的任务规划方法中的步骤流程实现。本领域技术人员，可以将所述机器人在观测受限下拾取及放置物体的任务规划方法理解为机器人在观测受限下拾取及放置物体的任务规划系统的一个优选例。

实施例2

实施例2是实施例1的优选例

任务实例场景如图3所示，机器人面前有一个货架，货架支撑面长40cm宽30cm，货架上摆放了6个物体，相机固定在货架前且与货架上的物体处于同一水平面。任务参考坐标系x方向指向机器人前方，y方向指向机器人左侧。所有物体都是直径6cm高12cm的圆柱体，每个物体以不同的名称和颜色区分。机器人可以拾取已知名称和位置的物体，也可以将持有的物体放在货架上或货架外。一开始机器人不知道各物体的位置，任务目标是将所有物体排列成图4所示队形。

首先给出任务的POMDP模型。状态定义为s＝{(o

步骤1：选择信念树搜索算法和运动规划算法。

本实例信念树搜索算法选择DESPOT，采用粒子信念表示法，构建一种基于情景的稀疏信念树，可设定在任意时间内结束搜索。运动规划算法选择RRT-connect，是快速扩展随机树(RRT)的变种，属于基于采样的运动规划方法。

步骤2：建立动作可行概率。

步骤2.1：设计可用动作集合筛选规则。

在观测z＝{(o

步骤2.2：设计动作特征量计算方法。

本实例中拾取动作是否可行，很大程度上与其他物体和要拾取的物体之间的相对位置有关。如果其他物体在要拾取的物体后方，那么几乎没有影响。否则，其他物体和要拾取的物体在y方向的距离，主要决定了机器人拾取物体时被其他物体阻碍的程度。放置是拾取的逆过程，放置动作被其他物体阻碍的程度与在拾取动作中相同。

在观测z＝{(o

在观测z＝{(o

步骤2.3：估计动作可行概率值。

在仿真中随机生成5000个物体布局，在每个布局中对未被遮挡的物体用运动规划检查是否可拾取，并记录对应动作的特征量，计算每个特征量下动作可行的频率，得出动作可行概率估计值如表1所示。在本实例中动作特征量的取值范围为0～15，特征量越大，动作可行的概率越低。

表1

步骤3：设计信念树动作分枝扩展规则。

对于根结点b

步骤4：设计修正回报函数。

由于信念树上大部分动作分枝的可行性是根据动作可行概率采样决定的，不能保证完全准确，可能使机器人进入重复执行某些动作的死胡同。设计修正回报函数

步骤5：任务规划与执行。

步骤5.1：对当前信念进行更新。如果当前是任务第一步，根据DESPOT采用的粒子信念表示法建立初始信念。如果当前不是任务第一步，根据DESPOT采用的粒子滤波方法对当前信念进行更新。

步骤5.2：用DESPOT算法规划机器人当前需要执行的动作。在构建信念树时，每个结点下扩展的动作分枝根据步骤3确定。在搜索信念树时，回报值r＝R(s，a)+Γ(h(s)，a)，是回报函数与步骤4设计的修正回报函数之和。

步骤5.3：机器人执行规划的动作，然后获得新的观测。

步骤5.4：对动作可行概率进行更新。设机器人当前观测z下的可用动作集合为A

其中N＝100是滑动平均的周期。

步骤5.5：如果任务完成，结束任务，否则返回步骤5.1。

需要理解的是，本发明并不局限于上述特定的实施方式，本领域技术人员可以在权利要求的范围内做出各种变化和修改，这并不影响本发明的实质内容。本发明可结合任意采用信念树搜索的在线POMDP规划算法。不同的方法可能对信念表示、信念跟新、信念树构建以及信念树搜索方式有不同的特殊处理，采用不同的信念树搜索算法均可使用本发明算法。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。