基于时空同步规划的无人船决策规划方法

技术领域

本发明涉及无人艇技术领域，更具体地说，涉及一种基于时空同步规划的无人船决策规划方法。

背景技术

目前无人艇随着智能化程度的不断提高，必将在江湖测绘和水质监测、海洋工程与海洋军事领域有广泛的应用。当前决策规划问题一直是无人艇产品研发的重难点，特别是高度动态和静态拥堵环境下的智能航行问题。寻找一种能够适应未知环境并规划出最优轨迹的方法能够显著提高无人船的安全、效率和经济效益。

公开日2021年5月14日，公开号CN 112799405A，发明名称为“基于动态障碍物环境下的无人船路径规划方法”的专利申请，方法通过D*Lite算法进行改进解决了环境动态下的路径规划问题。但是该方法无法有效处理高速移动对象的规划问题，特别是在保证最小偏移测线的前提下的动态速度规划问题。也无法同时考虑运动学约束获得舒适高效的轨迹输出。

公开日2022年10月14日，公开号CN 115185262A,发明名称为“一种基于最小安全会遇的动态避障路径快速规划方法”的专利申请。方法通过计算多个船只的最短会遇距离和最小会遇时间并安装某种优先序列计算下一刻规划速度和方向。但是该方法仅仅限制在贪婪规划，忽略了高度动态对象的预测和船体运动学约束，导致规划输出不平滑，也无法满足测绘和水质作业尽量贴线行驶的要求。另外这种方法无法处理拥挤环境下的轨迹规划问题。

总而言之，无人艇决策规划的难点在于：(1)需要考虑高度动态环境下对象的运动预测问题，从而使得规划不会朝令夕改；(2)需要考虑无人艇本身运动学甚至动力学约束问题，从而使得规划结果能够被控制真正执行；(3)需要考虑效率和舒适的问题，更能体现无人艇的智能导航能力。

发明内容

本发明要解决的技术问题在于，提供一种基于时空同步规划的无人船决策规划方法，其通过通用决策规划方法，使得无人艇同时考虑平台运动约束、障碍物约束、舒适度要求、效率要求、安全要求，并给出满足要求的时空轨迹。

本发明解决其技术问题所采用的技术方案是：构造一种基于时空同步规划的无人船决策规划方法，包括以下步骤：

S1、由集控后台根据用户应用要求下发任务测线或者航线，以任务测线或者航线作为决策规划的指引线；

S2、从感知单元实时订阅静态障碍物列表和动态障碍物一段时间的轨迹列表；

S3、构建最优控制问题：定义代价函数，运动学约束，边界条件以及碰撞约束，并求解最优控制问题；

S4、控制单元订阅规划结果，将轨迹分成路径-时间曲线和速度-时间曲线分别进行闭环调节跟踪。

按上述方案，所述步骤S3中，包括以下步骤：

S301、构建最优控制问题OCP；

S302、动态规划决策，同时开辟凸空间获得粗解：

S303、可迭代规划优化算法。

按上述方案，所述步骤S301中，最优控制问题OCP的形式如下：

MinimizeJ(z(t),u(t))

z(t)∈[z

z(0)＝z

h

其中，z(t)表示系统状态向量，z(t)代表[x(t),y(t),θ(t),v(t),a(t)]，[x(t),y(t)]表达无人船位置，θ(t)代表无人船当前航向，v(t)代表无人船当前纵向速度，a(t)代表加速度；u(t)＝[jerk(t),ω(t)]，其中

按上述方案，所述步骤S301中，运动学约束f

碰撞约束h

按上述方案，所述步骤S301中，将非线性碰撞约束简化为盒子约束，具体形式如下：

J(z(t),u(t))为规划要求的代价函数总和，具体形式如下：

按上述方案，所述步骤S302中，T代表规划时间，通过将时间[0,T]划分成N

按上述方案，所述步骤S303中，可迭代规划优化算法的临时OCP问题如下：

z(t)∈[z

z(0)＝z

按上述方案，所述可迭代规划优化算法的步骤如下：

S303-a：初始化χ←traj

S303-b：while循环，如果iter＜iter

S303-c：构建并求解临时OCP问题，得到解χ，计算运动学约束违反度量

S303-d：将当前解χ作为下一次迭代初始解，并重新计算航行隧道，更新变量：iter←iter+1,w

S303-e：从当前解中抽出轨迹部分作为决策规划规划的最终解答。

实施本发明的基于时空同步规划的无人船决策规划方法，具有以下有益效果：

1、本发明能够满足高度动态环境下江河测绘的智能航行要求，最大化的减少补测的次数，同时安全、高效和舒适；

2、本发明能够满足拥堵环境下的湖泊水质监测的航行要求，能很好的在大量静态或者动态环境下高效作业；

3、本发明由于同时考虑了环境碰撞约束和平台自身运动学约束，使得智能航行更加高效，节能，安全和舒适。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明基于时空同步规划的无人船决策规划方法的可迭代最优化算法的流程图；

图2是本发明基于时空同步规划的无人船决策规划方法的S-L-T状态空间。

具体实施方式

为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。

如图1所示，本发明的基于时空同步规划的无人船决策规划方法，包括以下步骤：

S1：由集控后台根据用户应用要求下发任务测线或者航线，以此作为决策规划的指引线。

S2：从感知单元实时订阅静态障碍物列表和动态障碍物一段时间的轨迹列表。

S3：构建最优控制问题，定义代价函数，运动学约束，边界条件以及碰撞约束，并求解此问题，输出安全、舒适、高效的规划结果。

具体包含以下步骤：

S301、构建最优控制问题(OCP)，形式如下：

MinimizeJ(z(t),u(t))

z(t)∈[z

z(0)＝z

h

其中，z(t)表示系统状态向量，具体代表[x(t),y(t),θ(t),v(t),a(t)]，其中[x(t),y(t)]表达无人船位置，θ(t)代表无人船当前航向，v(t)代表无人船当前纵向速度，a(t)代表加速度。u(t)＝[jerk(t),ω(t)]，其中

运动学约束f

碰撞约束h

J(z(t),u(t))为规划要求的代价函数总和。一般而言，代价函数要包括高效，也就是尽量贴近参考线行驶；舒适，要求纵向加速度和横向加速度尽量小；安全，要求规划路径不能于静态和动态对象有碰撞，J(z(t),u(t))的具体形式如下：

S302、动态规划决策，同时开辟凸空间获得粗解。

如图2所示，S-L-T状态空间，T代表规划时间，通过将时间[0,T]划分成N

S303、可迭代规划优化算法

根据粗解{(x

z(t)∈[z

z(0)＝z

解决临时OCP问题算法流程如下：

S303-a：初始化χ←traj

S303-b：while循环，如果iter＜iter

S303-c：构建并求解临时OCP问题，得到解χ，计算运动学约束违反度量

S303-d：将当前解χ作为下一次迭代初始解，并重新计算航行隧道，更新变量：iter←iter+1,w

S303-e：从当前解中抽出轨迹部分作为决策规划规划的最终解答。

S4：控制单元订阅规划结果，将轨迹分成路径-时间曲线和速度-时间曲线分别进行闭环调节跟踪。

本发明的基于时空同步规划的无人船决策规划方法，适用于高度动态环境下的江河测绘任务，也适合于障碍密集的湖泊水质采样任务。首先根据应用要求获得任务测线或者航线，以此作为决策规划的指引线；然后从感知单元实时订阅静态障碍物列表和动态障碍物一段时间的轨迹列表；进一步构建决策规划的最优控制问题(OCP)，定义代价函数，运动学约束，边界条件以及碰撞约束，通过直接在空间-时间三维空间上进行动态规划(DP)获得实时决策结果，为OCP开辟凸空间并提供了规划的初始解，在此基础上构造航行隧道，将非线性环境碰撞约束转换为凸盒子约束。随后利用可迭代式的最优求解算法(ITOM)不断改进环境约束于运动学约束冲突，输出一条安全、效率和舒适的轨迹；最后控制单元将规划轨迹分成路径-时间曲线和速度-时间曲线分别进行闭环调节跟踪。本发明能有效的同时处理无人船本身运动学约束、静态障碍物约束以及高度动态对象约束，并输出安全、效率和舒适的规划结果。这对于在长江大量动态货船环境下的无人测绘作业以及脏乱湖泊的水质检测作业都有很好的应用。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。