人群环境下基于MPC动态博弈的移动机器人局部规划方法技术

本发明专利技术提供了一种人群环境下基于MPC动态博弈的移动机器人局部规划方法，包括：以机器人和周围行人为参与者建立MPC框架下的动态博弈模型；对动态博弈模型进行线性化近似，得到近似动态博弈模型，利用极大值原理求解近似模型的最优性条件；基于最优性条件设计逆最优控制算法，利用逆最优控制算法从真实行人轨迹中辨识出指引行人决策的权重参数；将辨识出的近似动态博弈模型作为局部规划器，结合机器人定位和行人检测算法，实现动态人群环境下机器人的局部规划控制。本发明专利技术能够使移动机器人在动态人群环境下具有很好的快速到达目标点、避让行人、节省能量等局部规划控制效果。节省能量等局部规划控制效果。节省能量等局部规划控制效果。

【技术实现步骤摘要】
人群环境下基于MPC动态博弈的移动机器人局部规划方法


[0001]本专利技术涉及机器人
，具体地，涉及一种人群环境下基于MPC动态博弈的移动机器人局部规划方法。

技术介绍

[0002]随着移动机器人技术的发展，越来越多的机器人深入到人类的日常生活中，完成有益于人类的服务工作，比如医疗服务机器人(智能轮椅)、餐饮配送机器人、室外巡逻机器人等。这些移动机器人的应用场景往往存在大量动态行人，行人的存在给机器人局部路径规划带来巨大挑战，比如不易预测的行人运动导致避障失败。因此，如何提高移动机器人对动态人群环境的适应性，提高局部避障能力，是移动机器人领域的关键技术。
[0003]对于动态人群环境下机器人局部规划问题，国内外研究者也做了大量研究，可大致分为三类：
[0004]一是反应式局部规划。在早期研究中，研究者们设计了反应式局部规划算法，该算法将行人视为一般的动态障碍物，如Paolo等人[1](Paolo Fiorini and Zvi Shiller.Motion planning in dynamic environments using velocity obstacles.The International Journal of Robotics Research,17(7):760
–
772,1998.)设计的速度障碍算法就可以实现对动态障碍物比较好的实时避障效果。这种局部规划方式能保证安全性，因为它考虑了所有潜在的障碍物。但是由于它是对实时检测到的动态障碍物进行避障，因此只专注到短期的下一步规划，缺少长期规划。
[0005]二是预测式局部规划。预测式局部规划会先预测行人的未来轨迹，再基于预测的未来轨迹进行合理决策，从而能成功规划出在长期情况下安全且舒适的路径。在低密度人群环境中，这种规划方式有很好表现，但是当人群密度增大时，机器人通常会发生停滞现象，直到人群散开，也就是常见的机器人冻结问题(Freezing Robot Problem)，具体可参考Peter等人[2](Peter Trautman and Andreas Krause.Unfreezing the robot:Navigation in dense,interacting crowds.In Intelligent Robots and Systems(IROS),2010IEEE/RSJ International Conference on,pages 797
–
803.IEEE,2010.)的工作。
[0006]三是合作式局部规划。冻结问题的出现是由于之前的局部规划方式将所有人单独来看待，忽略了人与人、人与机器人之间的交互作用。对此，当前研究趋势是合作式局部规划。为了提高避障性能，一部分研究者们将人与人、人与机器人交互作用建成数学模型，比如常见的社会力模型、动态博弈模型等。公开号为CN111752276A的中国专利技术专利基于社会力模型，使机器人产生符合行人预期的局部避障行为。另一部分研究者借鉴学习的思想，从真实行人轨迹中学习交互作用，并用于规划中，比如强化学习算法等。公开号为CN115185281A的中国专利技术专利采用强化学习的方法，考虑了机器人与行人、行人与行人的关联特征，预测行人未来轨迹再进行规划避障。前者直接建模的方式涉及权重设计，手动设计的方式大多需要丰富的经验为依据，调试比较麻烦繁琐；后者直接学习的方法缺少一定
的可解释性，且往往泛化能力不足。
[0007]对此，可以将直接建模与直接学习相结合。首先基于最优控制和动态博弈的理论，假设机器人和行人在环境中始终以理性方式进行最优规划决策，将机器人自身和局部观测到的行人建立成动态博弈模型，进而求解纳什均衡条件下机器人最优控制量。同时利用逆最优控制理论，从真实行人轨迹中离线辨识出动态博弈模型中代价函数权重参数，使建立的模型中的权重设计更符合人类规范准则。
[0008]基于前人的工作，本专利技术创新性地提出了一种能在人群环境下，基于MPC动态博弈的移动机器人局部规划算法，该算法可以应用于载人智能轮椅、餐饮配送机器人在车站、餐厅等行人众多的工作场景下的路径规划任务。

技术实现思路

[0009]针对现有技术中的缺陷，本专利技术的目的是提供一种人群环境下基于MPC动态博弈的移动机器人局部规划方法。
[0010]根据本专利技术提供的一种人群环境下基于MPC动态博弈的移动机器人局部规划方法，包括：
[0011]建立模型步骤：以机器人和周围行人为参与者建立MPC框架下的动态博弈模型；
[0012]近似模型步骤：对动态博弈模型进行线性化近似，得到近似动态博弈模型，利用极大值原理求解近似模型的最优性条件；
[0013]辨识模型步骤：基于最优性条件设计逆最优控制算法，利用逆最优控制算法从真实行人轨迹中辨识出指引行人决策的权重参数；
[0014]局部规划步骤：将辨识出的近似动态博弈模型作为局部规划器，结合机器人定位和行人检测算法，实现动态人群环境下机器人的局部规划控制。
[0015]优选地，所述建立模型步骤包括：
[0016]设计状态方程步骤：分别建立反映行人和机器人运动状态的质点运动学方程；
[0017]设计代价函数步骤：根据行人的运动决策设计对应的目标项、交互项和控制项，由各自权重系数联合共同组成代价函数；
[0018]设计动态博弈步骤：根据状态方程和代价函数，设计智能体的最优控制问题，联合所有智能体的最优控制问题得到动态博弈模型，采用MPC框架得到MPC动态博弈模型，所述智能体为行人或移动机器人。
[0019]优选地，针对具有非完整约束特性的移动机器人，采用反馈线性化方法，将机器人的独轮车运动学方程转换为机器人外一点的质点运动学方程。
[0020]优选地，所述设计动态博弈步骤包括：
[0021]针对每一个智能体，将其决策过程构建成最优控制问题，设计其状态方程和代价函数；
[0022]在每个采样时刻，根据实时检测得到的行人位置和实时定位得到的移动机器人位置，在线同时求解各智能体由实时更新的代价函数和状态方程组成的有限时域开环最优控制问题，并将得到的最优控制序列的第一个元素作用于各智能体；在下一个采样时刻，重复求解操作，用新的测量值作为下一时刻计算多智能体最优控制的初始条件，刷新动态博弈问题并重新求解。
[0023]优选地，所述近似模型步骤包括：
[0024]定义近似模型步骤：在每个智能体的最优控制问题求解过程中，针对代价函数中的交互项具有的非线性特性，对代价函数在初始条件处进行泰勒展开，近似成线性二次型形式，得到近似MPC
‑
LQ动态博弈问题；
[0025]定义最优性条件步骤：在MPC框架下，在每个采样时刻，所有智能体都更新各自最优控制的问题，并联合组成动态博弈问题；求解每个智能体各自的最优控制问题，得到每个智能体的最优解，该最优解满足在其他智能体最优的情况下，各智能体不能单方面改变自己的决策而使代价更本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种人群环境下基于MPC动态博弈的移动机器人局部规划方法，其特征在于，包括：建立模型步骤：以机器人和周围行人为参与者建立MPC框架下的动态博弈模型；近似模型步骤：对动态博弈模型进行线性化近似，得到近似动态博弈模型，利用极大值原理求解近似模型的最优性条件；辨识模型步骤：基于最优性条件设计逆最优控制算法，利用逆最优控制算法从真实行人轨迹中辨识出指引行人决策的权重参数；局部规划步骤：将辨识出的近似动态博弈模型作为局部规划器，结合机器人定位和行人检测算法，实现动态人群环境下机器人的局部规划控制。2.根据权利要求1所述的人群环境下基于MPC动态博弈的移动机器人局部规划方法，其特征在于：所述建立模型步骤包括：设计状态方程步骤：分别建立反映行人和机器人运动状态的质点运动学方程；设计代价函数步骤：根据行人的运动决策设计对应的目标项、交互项和控制项，由各自权重系数联合共同组成代价函数；设计动态博弈步骤：根据状态方程和代价函数，设计智能体的最优控制问题，联合所有智能体的最优控制问题得到动态博弈模型，采用MPC框架得到MPC动态博弈模型，所述智能体为行人或移动机器人。3.根据权利要求2所述的人群环境下基于MPC动态博弈的移动机器人局部规划方法，其特征在于：针对具有非完整约束特性的移动机器人，采用反馈线性化方法，将机器人的独轮车运动学方程转换为机器人外一点的质点运动学方程。4.根据权利要求2所述的人群环境下基于MPC动态博弈的移动机器人局部规划方法，其特征在于，所述设计动态博弈步骤包括：针对每一个智能体，将其决策过程构建成最优控制问题，设计其状态方程和代价函数；在每个采样时刻，根据实时检测得到的行人位置和实时定位得到的移动机器人位置，在线同时求解各智能体由实时更新的代价函数和状态方程组成的有限时域开环最优控制问题，并将得到的最优控制序列的第一个元素作用于各智能体；在下一个采样时刻，重复求解操作，用新的测量值作为下一时刻计算多智能体最优控制的初始条件，刷新动态博弈问题并重新求解。5.根据权利要求1所述的人群环境下基于MPC动态博弈的移动机器人局部规划方法，其特征在于，所述近似模型步骤包括：定义近似模型步骤：在每个智能体的最优控制问题求解过程中，针对代价函数中的交互项具有的非线性特性，对代价函数在初始条件处进行泰勒展开，近似成线性二次型形式，得到近似MPC
‑
LQ动态博弈问题；定义最优性条件步骤：在MPC框架下，在每个采样时刻，所有智能体都更新各自最优控制的问题，并联合组成动态博弈问题；求解每个智能体各自的最优控制问题，得到每个智能体的最优解，该最优解满足在其他智能体最优的情况下，各智能体不能单方面改变自己的决策而使代价更小，即纳什均衡定义；通过同时求解所有智能体各自的最优控制问题得到动态博弈问题纳什均衡意义下的最优性条件；对于近似MPC
‑
LQ动态博弈问题，采用庞特里亚金极大值原理，得到矩阵等式形式的最优性条件；通过求解该矩阵等式，可以得到所有智能体MPC预测周期内的最优控制以及最优轨迹。
6.根据权利要求1所述的人群环境下基于MPC动态博弈的移动机器人局部规划方法...

【专利技术属性】
技术研发人员：张晗，虞关凤，王景川，陈卫东，张心良，
申请(专利权)人：采埃孚中国投资有限公司，
类型：发明
国别省市：