[发明专利]基于自适应尺度约束优化的多非完整机器人车辆协同轨迹规划方法、装置及介质

说明书

本发明公开了一种基于自适应尺度约束优化的多非完整机器人车辆协同轨迹规划方法、装置及介质，该方法将相关的MVTP问题描述为耦合OCP，引入了一种称为自适应尺度约束优化(ASCO)的解耦简化策略，以得出耦合MVTP问题的最优解。ASCO在耦合的OCP中划分难以缩放的约束，并在迭代框架中克服它们。具体而言，在每次迭代中都提出了一个中间的非线性规划(NLP)问题，其中仅施加了某个“范围”内的部分碰撞约束；可以自适应地调整上述特定范围，提高收敛速度，使NLP解过程对初始解不那么敏感。

技术领域

本发明属于机器人车辆控制领域，特别涉及一种基于自适应尺度约束优化的多非完整机器人车辆协同轨迹规划方法、装置及介质。

背景技术

单车轨迹规划是指找到在运动学上可行，不会碰撞障碍物，乘客乘坐舒适，旅行时间最佳的轨迹。多车轨迹规划(MVTP)任务是单车情况的扩展，该任务还需要避免车辆之间的相互碰撞。由于这些附加约束难以与车辆数量相适应(通常称为维数灾难)，因此无法直接扩展单车轨迹规划器来处理MVTP问题。

现有的MVTP方法分为耦合和解耦类别。耦合方法可一次生成车辆的轨迹，而解耦方法可将原始任务分为多个子问题，然后将派生的解决方案组合起来，形成一个完整的解决方案。尽管多智能体规划器种类繁多，但由于以下原因，很少有人能有效地处理这种复杂的计划。首先，障碍的存在使场景变得不凸。其次，每个类似汽车的机器人车辆的运动学约束都是非线性的。第三，狭小的环境使车辆之间发生高度冲突，即，一辆车辆可能与各种车辆存在潜在的冲突。第四，对时间最优解的追求表明终端时间不是固定的，这增加了有关MVTP问题的难度。第五，每辆车都不会事先在全局上分配同质路线，即使对于一辆车，同质类的误导选择也可能使整个MVTP任务失败。这些因素耦合在一起时，超出了主流的MVTP规划人员的能力范围。

发明内容

本发明的目的是提出一种基于自适应尺度约束优化的多非完整机器人车辆协同轨迹规划方法、装置及介质，其目的在于，寻找一组车形机器人车辆在被静态障碍物占据的微小室内环境中行驶时的时间最优MVTP结果。

本发明提供的技术方案如下：

一方面，一种基于自适应尺度约束优化的多非完整机器人协同轨迹规划方法，包括：

基于出行效率与乘客舒适度，构建机器人车辆的时间与能量成本代价函数模型；

基于机器人车辆的运动原理与行驶环境，起点与终点配置信息以及自身尺寸与行驶环境障碍物尺寸，依次构建机器人车辆的运动约束条件，边界约束条件以及避碰约束条件；

利用一阶显式龙格-库塔方法将在约束条件下中的时间与能量成本代价函数模型进行离散处理，并转化为求解机器人车辆与车辆、车辆与障碍物之间最小距离的NLP问题，然后通过自适应调整碰撞风险范围，迭代求解所述NLP问题，得到各机器人车辆的运动规划轨迹。

进一步地，所述机器人车辆的时间与能量成本代价函数模型如下：

其中，v_i(τ)、a_i(τ)表示第i辆机器人车辆τ时刻轮轴中点的速度、加速度，ω_i(τ)表示第i辆机器人车辆τ时刻的角速度，t_f表示机器人车辆轨迹终端时间，N_v表示机器人车辆数量，w表示加权参数，为经验设定值，仿真时设定为0.01；

进一步地，所述机器人车辆的运动约束条件如下：