[发明专利]一种基于控制障碍函数模型预测控制算法的无人船避障控制方法

说明书

本发明涉及一种基于控制障碍函数模型预测控制算法的无人船避障控制方法，属于无人船技术领域。具体包括以下步骤：S1：建立无人船运动学模型和避碰障碍目标模型；S2：依据障碍与无人船之间的位姿信息，设计无人船控制障碍函数约束下的避障优化目标代价函数；S3：实时优化求解代价函数，规划无人船未来时刻的参考轨迹；S4：将规划算法求解得到的未来时刻轨迹作为无人船的参考轨迹，设计分层闭环控制算法实现实时跟踪。本发明能够针对无人船运动中控制中的非线性多约束的特点，实现无人船水面实时避障轨迹规划。

技术领域

本发明属于无人船技术领域，具体涉及一种基于控制障碍函数模型预测控制算法的无人船避障控制方法。

技术背景

无人船作为一种仅依赖自身运动规划与控制系统即可在特定水域执行任务的运动平台，在面对复杂的水上环境，受到自身航程以及通讯距离的限制，很难在运行过程中时刻受到人为控制。为此需要开发出优越的运动规划算法提高无人船自主航行能力。

除水面波动干扰外，水面障碍是无人船安全运行的最大威胁，目前全局避障轨迹规划算法较为成熟，但是水面环境随时间时刻变化，需要开发出实时避障轨迹规划算法提升无人船的航行安全性。传统规划方法针对固定障碍有较快的规划速度，但很难适应环境的变化，并且未考虑无人船运行存在多种约束等问题。

发明内容

针对上述存在的技术问题，本发明设计了一种基于控制障碍函数模型预测控制算法的无人船避障控制，能够针对无人船运动中控制中的非线性多约束的特点，实现无人船水面实时避障轨迹规划。

为实现上述功能，本发明采用以下方案：

一种基于控制障碍函数模型预测控制算法的无人船避障控制方法，其特征在于：具体包括以下步骤：S1：建立无人船运动学模型和避碰障碍目标模型；S2：依据障碍与无人船之间的位姿信息，设计无人船控制障碍函数约束下的避障优化目标代价函数；S3：实时优化求解代价函数，规划无人船未来时刻的参考轨迹；S4：将规划算法求解得到的未来时刻轨迹作为无人船的参考轨迹，设计分层闭环控制算法实现实时跟踪。

进一步，所述S1包含以下步骤：

S11：建立无人船的运动学模型，模型描述为：

其中，P_x(k)和P_y(k)表示质心在水平面投影仪的位置，θ(k)表示无人船航向角，V_x(k)和V_y(k)表示两个方向的行进速度，W(k)表示转向角速度。

S12：建立环境中障碍目标模型，模型描述为：将无人船的外接圆看做一个质心，同时将障碍物的外接圆膨胀，膨胀后的半径为两者最小安全距离。

进一步，所述S2包含以下步骤：

S21：设计控制障碍函数，使用欧几里得范数得的距离约束给予无人船一定得避障能力，设计欧式几何距离约束的模型预测控制优化算，估计规划所需优化的目标函数以及系统约束条件可表示为：

其中，x_t+k∣t代表无人船在输入序列u_t:t+N-1∣t的作用下第k个时间周期时的状态向量；q(x_t+k∣t,u_t+k∣t)与p(x_t+N∣t)分别代表过程代价以及终端代价；g代表符合安全运行的距离约束。

S22：将离散控制障碍函数与模型预测控制一同应用到规划问题中，设计连续可微函数作为无人船的安全保障条件：

其中，h是控制障碍函数，如果对任意都存在/并存在一个扩展/类函数γ，对于任意的输入u都能使控制障碍函数满足：