SBMPO：基于采样的机器人轨迹规划模型预测优化及其代码版本控制系统

软件影响10（2021）100159原始软件出版物SBMPO：基于采样的机器人轨迹规划模型预测优化Mario Harpera，Mr. P.，Camilo Ordonezb，Emmanuel Collinsca美利坚合众国犹他州洛根犹他州立大学bCenter for Intelligent Systems，Controls and Robotics（CISCOR），FAMU-FSU College of Engineering，Tallahassee，FL，32310，United States of America路易斯维尔自动化和机器人研究所（LARRI），路易斯维尔大学，Lousville，KY 40292，美国A R T I C L E I N F O保留字：运动规划模型预测控制优化Kinodynamic规划A B标准基于采样的模型预测控制（SBMPO）是一种新的非线性预测控制（NMPC）方法，它可以利用动态模型进行运动规划。该工具也非常适合解决传统的MPC问题，并已在机器人、任务调度、资源管理、燃烧过程和一般优化等各种情况下进行了测试。代码元数据当前代码版本v2.0用于此代码版本的代码/存储库的永久链接https://github.com/SoftwareImpacts/SIMPAC-2021-108Reproducible Capsule的永久链接https://codeocean.com/capsule/6106440/tree/v1GNU通用公共许可证（GPL）使用git的代码版本控制系统使用C++的软件代码语言、工具和服务，还包括一些python和Matlab脚本，用于可视化编译要求、操作环境依赖性请参阅README和安装指南，网址为https://github.com/DIRECTLab/SBMPO/blob/main/README.md和https://github.com/DIRECTLab/SBMPO/blob/main/QuickStart.md如果可用，链接到开发人员文档/手册例如：https://github.com/DIRECTLab/SBMPO/blob/main/README.md/问题支持电子邮件mario. usu.edu1. 介绍基于采样的模型预测优化（SBMPO）生成可行控制输入的成本最优轨迹，如果执行该轨迹，则使移动机器人、机械手、空中平台或一般动力系统能够在复杂环境中智能地操作以完成具有挑战性的任务。该轨迹直接考虑了特定于情况的输入和输出约束、系统动力学以及用于在高维中进行有效缩放的有效采样。此外，所采用的成本函数可以涉及物理激励量，例如距离、时间或能量。该算法已经成功地在自主地面车辆等多种移动机器人上展示了自动导引车（AGV）、自主飞行器（AAV）、自主水下运载工具（AUV）和在复杂环境约束下操作的腿式机器人平台[1SBMPO算法是一种基于树的采样算法，其将来自经典A* 的方面与采样方法相结合以生成可以被优化的高效树结构。SBMPO利用隐式网格来减少计算。该隐式网格通过用户定义的网格分辨率和网格轴（通常是机器人姿态x、y、y）创建。在扩展时，在同一网格内检查其他占用者的节点，如果存在另一个节点，则修剪具有较高成本的节点。这一额外的检查层减少了算法的内存占用，并提高了可扩展性，因为SBMPO不进行基于网格的搜索（它将扩展增加了10%，��� =������ 哪里本文中的代码（和数据）已由Code Ocean认证为可复制：（https://codeocean.com/）。更多关于生殖器的信息徽章倡议可在https://www.elsevier.com/physical-sciences-and-engineering/computer-science/journals上查阅。∗通讯作者。电子邮件地址：mario. usu.edu（M. Harper）。https://doi.org/10.1016/j.simpa.2021.100159接收日期：2021年8月24日;接收日期：2021年9月26日;接受日期：2021年10月7日2665-9638/©2021作者。由Elsevier B.V.出版。这是一篇开放获取的文章，使用CC BY许可证（http://creativecommons.org/licenses/by/4.0/）。可在ScienceDirect上获得目录列表软件影响杂志 首页：www.journals.elsevier.com/software-impactsM. 哈珀角Ordonez和E.柯林斯软件影响10（2021）1001592Fig. 1. SBMPO根据采样的控制输入计算系统状态的变化，并使用成本函数来优先考虑勘探区域。 控制空间中的采样确保生成的轨迹是可接受的，SBMPO直接返回控制命令的轨迹，而不是依赖于逆运动学求解器来计算控制。是每个轴的固定点，��� 由于基于树的性质，SBMPO可以很好地扩展到更高的维度，同时保持收敛速度和性能保证。该算法还受益于基于采样。基于采样的方法本质上通常是“随时”的，这意味着算法可以在任何时候终止，并且仍然返回一个可行的（如果不是最佳的）解决方案。由于减少了内存需求，这些算法也更能够考虑复杂性。虽然基于采样的方法有许多优点，但这类算法在实践中存在次优性。它们依赖于需要长迭代时间的渐近最优保证。SBMPO以牺牲“随时"性质为代价提供最佳保证大多数最先进的规划方法通常使用两个子规划器来解决复杂性：审议和反应。考虑式规划器通常假设简化的运动模型来规划整个路径生成和计算遵循路径所需的速度的转向函数，然后反应式规划器操作以跟踪路径并不断地重新规划路径以考虑考虑式规划器中未考虑的复杂性，例如动态行为。SBMPO通过在规划过程中直接使用动态模型，直接考虑整个整体优化过程中非平凡复杂性的影响，减轻了需要子规划者的繁琐必要性。SBMPO在复杂的情况下成功地执行了导航任务，而无需进行子规划。例子包括地形，大大改变UGV运动：草，植被，和陡峭的山丘[1，2]。类似地，欠驱动机械手已经使用系统动力学知识举起重物，即使重量超过电机可以提供的力[9]。虽然复杂性因应用而异，但我们的软件中包含了几个典型的情境约束，有关添加新模块的文档可在README文件中找到：https://github.com/DIRECTLab/SBMPO/。2. 架构SBMPO由模型特定组件（传播模型、边成本评估、启发式计算）和优化引擎（见图1）组成。只要模型是用这些模型特定的组件定义的，SBMPO就能够基于最小化定义的成本函数来优化计划。2.1. 添加新型号可以通过创建新的.cpp文件和关联的头文件来添加新模型。这个新的模型文件必须包含模型特定的组件作为独立的方法。这三个方法中的每一个都必须使用某些输入声明，如下所示State model：：[new_model]：：next_state（constState state，const Control control，浮点整数步长）float model：：[new_model]：：cost（constState current，const State next，const Control control）float model：：[new_model]：：heuristic（const Statecurrent，const State goal，const Control control）这 些 新 模 型 将 被 放 置 在 /lib/src/model （ 用 于 .cpp 文 件 ） 和/lib/src/include（用于.h文件）中。然后更新make文件，以包含遵循与示例用例相同模式的新模型（第3节）。2.2. 新型号示例我们举例说明了一个简单的例子（称为ACplanner），它是desired，以尽量减少美元成本运行的A/C单元。该系统有一个控制（控制[0]），用于确定空调系统是打开还是关闭。状态包含时间、系统的当前温度和外部温度。状态和控制参数将在. json文件中初始化（有关详细信息，请参阅README），.cpp文件的构建如下：状态模型：：ACplanner：：next_state（const Statestate，const Control control，public int maximum（int maximum）{State newState;State states=state;new State. size（）;float coolRate=1.5;//物体冷却的速度floatheatRate= 0.125;//物体加热的速度inttimeStep=state[0];floatoTemp =39; //假设固定温度外浮动价格=0.17;//假设每千瓦固定价格newState[0]= timeStep+ 1;//下一个状态时间newState[1]= state[1]- coolRate*control[0]+heatRate*（oTemp- state[1]）;//下一个状态temp newState[2]=price;//记录价格return new State;}float model：：ACplanner：：cost（constState current，const State next，const Control（）{浮动功耗=3.5/4;cost= Power*control[0]*next[2]; //返回成本;}float model：：ACplanner：：heuristic（const Statecurrent，const State goal，constControl control）{ double h;h =0.0; //没有启发式，这将进行穷举搜索返回h;}M. 哈珀角Ordonez和E.柯林斯软件影响10（2021）1001593图二. 1D运动规划示例。 在水平面上滚动的轮子。这创建了一个简单的美元最小化的A/C温度规划器。传播（下一状态）方法计算新的温度， 如果A/C单元打开，则基于固定的制冷和制热效率在一个时间步长中计算系统。成本函数乘以美元成本如果系统开启，则使用设定的功率消耗计算功率。没有计算启发式算法，说明即使没有启发式算法，也可以找到最佳解决方案。可以将其他约束（例如最大或最小允许系统温度）添加到README. json文件中。3. 用法和示例三个示例实现与软件集成，便于原型设计和开发。这些示例可以通过命令行参数“example=[1D kinematics]”访问关于运行示例和解释或可视化结果的具体说明可以在软件文档中找到（https：//github.com/DIRECTLab/SBMPO/blob/main/README. md）。在第一个（默认）示例中，显示了一个1D规划任务。在这种情况下，一个简单的双积分器对应的滚动轮图。二是考虑。控制输入对应于车轮的线性加速度。我们提供了在配置文件中更改参数的选项（https：//github.Com/DIRECTLab/SBMPO/blob/main/resorces/kinematic/config.json），表1列出了其中一些常见参数。还有三个优化标准：距离，速度和时间。距离最优规划将通过查看期望位置与当前位置的简单差异来优化以最小化行进的总距离。速度标志将优化以最小化总距离，同时意识到加速度约束。最后，时间最优范式将最大限度地减少到达目标位置的时间。此示例展示了如何使用优化目标的自定义定义来实现第二个例子是由左右轮速度控制的滑移UGV，同时在障碍物场中执行典型的运动规划任务。运动规划任务最小化从开始位置移动到目标位置所需的距离。用户可以容易地改变起始姿势默认情况下，障碍物在obstacles.txt文件中定义，具有三个标记：位置、位置和半径。 假设圆形障碍物是为了便于与点云传感器，如激光雷达或深度感知相机。最后一个例子运行相同的滑移操纵机器人，但使用的是能量模型。(For关于滑移转向的能量模型的细节，参见[1]）。这种能量模型变化允许优化，以基于分析函数最小化能量。 最终的轨迹是可行的和最佳的，尊重车辆电机功率和车身几何约束。表1所有运动规划任务所需的关键参数参数变量说明开始，目标定义初始和最终状态允许的最大扩展次数终止搜索扩展期间的子代数取样方法（例如，固定样本、Halton样本、随机样本等）采样时间采样期间用于传播系统模型的时间分辨率扩展（越短越准确，但代价是增加计算时间。用户应确保在采样时间减少时增加最大迭代次数约束定义控制输入的约束这可能是一个简单的约束，如最小和最大扭矩（参见1D示例中的最小和最大扭矩.最大扭矩文件），或扩展为多个输入，如转弯半径和命令速度（运动学示例中的最小和最大扭矩.最大扭矩文件������������������图三. 为Clearpath Husky滑移式UGV上的节能路径扩展节点、障碍物和优化轨迹。默认的可视化只绘制障碍物和最终路径。在这两个UGV示例中，可以记录不同的状态和其他变量。默认记录的参数是航向坐标、航向坐标、航向角、右轮角速度、左轮角速度、转弯半径。障碍物文件可以通过添加（或删除）行来更改。每行包含三列，分别对应于障碍物的位置[x，y]和半径。如果用户希望使用来自硬件的传感器反馈，则该障碍物文件可以读取点云成功执行算法后，生成的轨迹将作为JSON文件生成。这个JSON文件在每个时间间隔都有一个控制和状态向量。在Python和Matlab中都提供了一个有用的绘图和可视化脚本来解析和显示结果。3.第三章。输出及其含义的详细描述可以在本软件随附的README（解释结果小节）中找到。4. 影响该软件已经成功地优化了几个不同平台的运动计划，包括自主水下航行器，滑动转向车辆，机械臂和航天器。此外，作为M. 哈珀角Ordonez和E.柯林斯软件影响10（2021）1001594规划范例可以规划高度非完整系统，它被用作几个动态和独特的移动系统的选择算法，包括第一个智能攀爬机器人[10]。所提供的示例和框架可以定制以解决困难的规划问题，例如雪/冰表面导航、在斜坡上移动、自主船/潜艇以及其他复杂的动态车辆的规划。在许多复杂的规划应用中，由于独特的 处理我们的算法：1.灵活的模型结构：该软件可以通过允许灵活地指定模型来优化许多类型的复杂输入-输出关系。 这在模拟中得到了证明，一个动态腿机器人，全身运动很难确定分析。在这种情况下，机器学习算法被训练并实现为SBMPO内部模型。 用户可以按照自己的意愿定义模型，而无需重新配置或更改优化器。2.优化标准：用户定义的成本函数确定优化。预先配置了几个成本函数（en-能量、时间和距离），以方便在我们的软件在基于能量的规划的情况下，成本函数是根据能量消耗来编写的。最终轨迹最小化达到期望目标标准所需的总能量。3.目标条件：这个规划器的总体目标通常是获得一个空间坐标。然而，目标标准是灵活的，并且可以由用户调整为不同的（例如：在给定时间内移动并覆盖特定区域）。用户可以轻松定义特定于应用程序的成功标准。通过灵活地定义特定于应用程序的模型和优化标准，该工具可以利用用户SBMPO使用机器人系统的模型来生成一个图形，图中所示的一个。3 .第三章。该算法还可以优化唯一挑战成本函数。这些通常基于复杂的动态模型，如节能规划[1]。5. 持续发展目前，该算法使用LPA*（Lifelong Planning A*）优化器，该优化器对生成的树进行操作。计划纳入其他优化技术，如MHA*（多启发式A*），以在难以定义启发式函数的复杂领域（如复杂移动机器人）中实现更快的优化。虽然C++允许更快的收敛，但将创建Python和Matlab版本的软件。这项开发工作将嵌入机器学习库，并允许更快的采用和开发。还计划直接集成ROS。另一个额外的改进是能够超越简单的2D地图。更复杂的地图对象将被用来添加有趣的运动约束经历，而穿越植被，丘陵和软地形。CRediT作者贡献声明马里奥哈珀：软件，写作-原始草案，调查。卡米洛·奥多涅斯：调查，形式分析。Emmanuel Collins：项目管理。竞合利益作者声明，他们没有已知的竞争性财务利益或个人关系，可能会影响本文报告的工作致谢这项工作得到了美国陆军研究实验室根据合作技术联盟计划（合作协议DAAD 19-01-2-0012）赞助的机器人联盟的合作参与的支持。 美国政府有权为政府目的复制和分发重印本，而不受任何版权注释的影响。引用[1]N.古普塔角Ordonez，E.G. Collins，动力学可行的，能量有效的滑移转向车辆运动规划，Auton。机器人41（2）（2016）453//dx.doi.org/10.1007/s10514-016-9550-8网站。[2]C.奥多内兹，加-地古普塔岛Chuy，E.G. Collins，Momentum based traversal ofmobilitychallengesforautonomousground vehicles ， in ： 2013IEEEInternationalConferenceonRoboticsandAutomation，2013，http://dx.doi.org/10.1109/icra。2013.6630657。[3]C. 奥多内兹，加-地 放大图片作者：Gupta，E. 克拉克，A.M.约翰逊，PowerModeling 的Xrl六足机器人和它的应用，能源效率的运动规划，适应。暴徒机器人（2012）689http://dx.doi.org/10.1142/[4]G. Francis，E. Collins，O. Chuy，A. Sharma，基于采样的轨迹生成 用于自主航天器 交 会 和 对 接 ， 在 ： AIAA 制 导 、 导 航 和 控 制 （ GNC ） 会 议 ， arXiv ：https://arc.aiaa.org/doi/pdf/10.2514/6.2013-4549。网址https://arc.aiaa.org/doi/abs/10.2514/6.2013-4549。[5] M. Harper，J.Pace，N.古普塔角Ordonez，E.G. Collins，运动学建模，使用神 经 网 络 的 RHex 型 机 器 人 ， SPIE ， Unmanned Syst.Technol.XIX 10195（2017）http://dx.doi.org/10.1117/12.2262894。[6] M.P. Austin，M.Y. Harper，J.M. Brown，E.G. Collins，J.E.克拉克，腿部移动导航 ： 动 态 攀 登 ， IEEEtrans.robot 。 36 （ 2 ） （ 2020 ）537http://dx.doi.org/10.1109/TRO.2019.2958207[7]C.V.考德威尔，D. D。Dunlap，E.G. Collins，通过基于采样的模型预测控制进行自主水下航行器的运动规划，载于：海洋2010 MTS/IEEE SEATTLE，2010年，pp.1 http://dx.doi.org/10.1109/OCEANS。2010.5664470。[8]S.M. 李明，《规划算法》，北京大学出版社，2006年。[9] O. Chuy，J. E. G. Collins，A.夏尔马河，巴西-地张文，应用动力学方法研究机械 臂 的 力 矩 约 束 ， 机 械 工 程 学 报 ， 2001 。 动 力 学 系 统 测 量 。 Control（2016）.[10] 我的Harper，J.V. Nicholson，E.G. Collins，J. Pusey，J.E.克拉克，运行腿式机器人的节能导航，在：2019年国际机器人与自动化会 议 （ICRA），IEEE，2019年，pp。6770-6776