[发明专利]时序逻辑任务下多无人艇系统协同控制方法、终端及介质

权利要求书

1.一种时序逻辑任务下多无人艇系统协同控制方法，其特征在于，包括：

S1，建立多无人艇系统时序逻辑协同任务的STL形式化模型，包括：

基于STL形式化语句对无人艇时序逻辑任务进行规范化的描述，建立任务场景的数学模型：

φ＝G_[a，b]ψ|F_[a，b]ψ|ψ₁U_[a，b]ψ₂|φ₁∧φ₂

其中，ψ、φ为描述无人艇时序逻辑任务的STL形式化语句，ψ₁、ψ₂形如ψ，用于描述无人艇不包含时间约束的时序逻辑任务；φ₁、φ₂形如φ，用于描述无人艇带有时间约束的时序逻辑任务，其STL形式化语句中定义了任务的作用时间段[a,b]，表示约束时域；U_[a,b]是until运算符，ψ₁U_[a,b]ψ₂表示在未来的[a,b]时间段内，至少有一次满足ψ₂，且在此之前必须始终满足ψ₁；F_[a,b]是finally运算符，F_[a,b]ψ表示在未来的[a,b]时间段内，至少有一次满足ψ；G_[a,b]是global运算符，G_[a,b]ψ表示在未来的[a,b]时间段内，必须始终满足ψ的约束；STL是基于谓词μ的谓词逻辑，μ用于表示无人艇个体任务或无人艇间的协作任务，其取值由谓词函数决定，μ:＝h(x(k))≥0，其中函数h(x)是信号x的线性或非线性组合；

建立多无人艇系统耦合任务的处理机制：

对多无人艇系统采用无向图建模，其中，表示M个无人艇，表示无人艇间的耦合链路，无人艇q_i时序逻辑任务的STL形式化语句如下：

当谓词μ表示多无人艇间的协作任务时，φ_i的满足程度取决于无人艇q_i的行为和/或其他无人艇行为，如果任务φ_i的满足程度受无人艇q_j行为的影响，则称q_j参与φ_i，φ_i的满足程度取决于无人艇集合其中M表示所有影响φ_i的无人艇的数量；M个无人艇互相协作的时序逻辑任务整体建模为φ₁∧…∧φ_M；

运用所述谓词逻辑、布尔逻辑运算符时态逻辑运算符(U_[a,b]，F_[a,b]，G_[a,b])以及多无人艇耦合任务的处理机制，将目标问题的时序要求利用STL语句形式化地表示，建立多无人艇系统时序逻辑协同任务的STL形式化模型；

S2，建立双层框架下的多无人艇系统控制策略，包括：

建立无人艇运动学模型：

将无人艇前向速度u、侧向速度v和艏摇角速度r作为系统输入，将x轴、y轴上的地标位置(x,y)和航向角作为系统状态量，则无人艇运动学模型描述为：

建立无人艇水动力学模型：

将螺旋桨推力τ和舵角控制输入δ作为系统输入，将无人艇前向速度u、侧向速度v和艏摇角速度r作为系统状态量，则无人艇水动力学模型描述为：

其中，m为无人艇的质量，(·)_σ表示无人艇对速度和横摆力矩的影响系数，I_z表示质量惯性矩，(·)_h表示流体力学和力矩；

设计双层多时间尺度规划-跟踪控制框架：

基于无人艇运动学模型，在时序逻辑任务的STL形式化模型下规划无人艇的前向速度u、侧向速度v和艏摇角速度r，使多无人艇系统完成时序逻辑协同任务；基于无人艇水动力学模型，利用鲁棒模型预测控制设计无人艇推力τ和舵角控制输入δ，使无人艇的前向速度u、侧向速度v和艏摇角速度r跟踪规划层给出的参考值，形成双层多时间尺度规划-跟踪控制框架，实现时序逻辑协同任务下多无人艇系统在海上环境下的智能控制；

S3，在时序逻辑任务的STL形式化模型下实现航速规划，包括：

建立规划层目标函数：

利用STL鲁棒量化语义ρ(φ，z，k)描述无人艇系统鲁棒程度，将无人艇的位置状态对STL语句φ的满足程度量化为一个实数ρ，度量无人船的位置状态是否满足任务要求以及满足或违反任务的程度；其中，ρ(φ，z，k)＞0表示无人艇的位置状态z在时间k满足任务φ，即ρ(φ，z，k)＜0表示z在时间k违反任务φ，即ρ(φ，z，k)的值越大，鲁棒性越大，控制效果越好；时态逻辑运算符(G，F，U)在STL鲁棒量化语义中的处理规则如下：

ρ(φ₁∧φ₂，x，k)：＝min{ρ(φ₁，x，k)，ρ(φ₂，x，k)}

ρ(φ₁∨φ₂，x，k)：＝max{ρ(φ₁，x，k)，ρ(φ₂，x，k)}

ρ(G_[a，b]φ，z，k)：＝min_{k′∈[a，b]}ρ(φ，z，k+k′)

ρ(F_[a，b]φ，z，k)：＝max_{k′∈[a，b]}ρ(φ，z，k+k′)

ρ(ψU_[a，b]φ，z，k)：＝max_{k′∈[a，b]}{min{ρ(φ，z，k+k′)，min_{k″∈[k，k+k′]}ρ(ψ，z，k″)}}

基于无人艇运动学模型，采用集中控制实现M个无人艇q₁，...，q_M的时序逻辑协同任务，以φ_i为无人艇q_i的时序逻辑任务、N为采样次数、ρ为STL鲁棒性程度，得到的优化问题P_ρ表述如下：

z_i(k+1)＝f(s_i(k)，z_i(k))

其中，z_i(k)，s_i(k)分别为无人艇q_i在k时刻的系统状态和输入，即s_i(k)＝[u_i(k)；v_i(k)；r_i(k)]，z_i(k+1)＝f(s_i(k)，z_i(k))为无人艇q_i离散化的运动学模型，Q为状态量权重系数，R为输入量权重系数，Q、R一般取单位阵；α为鲁棒程度和成本函数的折中系数；为无人艇q_i时序逻辑任务的最小期望鲁棒性，如果δ＝0，则不含鲁棒性约束；

对目标函数进行平滑处理：

采用光滑核对目标函数中的不可微项进行卷积，在Meyer正交小波的基础上展开距离函数，从而提供距离函数的光滑逼近，使其不再具有奇点；

对目标函数进行近似处理：

对目标函数中的min项和/或max项进行近似处理，得到平滑可微的目标函数：

其中，表示对序列a₁,…,a_m取最大值的近似结果，参数β取值越大，近似误差越小，当β趋近于∞时，近似误差几乎为0；

利用梯度下降算法迭代寻优：

利用CVX生成M个无人艇的初始位置状态序列z_i(k)和输入序列s_i(k),i＝1,…,M,k＝0,…,N-1，形成迭代初始解；根据鲁棒量化语义ρ的处理规则得到多无人艇系统的鲁棒性程度整体表达式ρ(φ₁∧…∧φ_M,z)，并做平滑近似处理，与能量项做折中处理得到可行目标函数；利用序列二次规划SQP作为梯度优化器对优化问题进行求解，将目标函数在迭代点z^k按泰勒级数展开，得到相应的二次函数，同时将约束简化为线性函数，从而得到原问题的近似二次规划问题，在此基础上迭代搜索，输出规划层最优前向速度、侧向速度和艏摇角速度

S4，鲁棒模型预测控制实现航速跟踪，包括：

对无人艇水动力学模型进行线性离散化处理：

将所述无人艇系统水动力学模型简写为使所述无人艇系统水动力学模型不考虑海上存在的系统扰动，其中，状态量为简写模型下无人艇系统前向速度、侧向速度和艏摇角速度，输入量为简写模型下舵角和推力；规划层得到无人艇q_i在k时刻的最优航速作为跟踪层的参考速度s_r(k)＝[u_r(k),v_r(k),r_r(k)]，将无人艇系统动力学模型在s_r(k)处线性化并离散化，得到标称无人艇系统水动力学模型为：

其中，l为跟踪层采样时刻，为标称无人艇系统水动力学模型状态量，为标称无人艇系统水动力学模型输入量，

建立标称无人艇系统跟踪层目标函数：

以N_d为采样次数，则标称无人艇系统实现航速跟踪的目标函数为：

其中，Q为状态量权重系数，R为输入量权重系数，Q、R一般取单位阵；为终端代价函数，P0为代数黎卡提方程的唯一正定解；所述代数黎卡提方程为：

P＝Q+A^TPA-A^TPB(R+B^TPB)^-1B^TPA

用梯度法求解上述优化问题，可得l时刻标称无人艇系统水动力学模型的最优状态序列和最优输入序列

设计鲁棒模型预测控制律：

考虑海上存在的系统扰动，则实际无人艇系统水动力学模型为：

s(l+1)＝As(l)+Bw(l)+Cd(l)

其中，状态量s(l)＝[u(l)；v(l)；r(l)]为无人艇实际前向速度、侧向速度和艏摇角速度，输入量w(l)＝[τ(l)；δ(l)]为舵角和推力，d(l)∈D为l时刻的系统扰动，C为扰动项模型参数；

如果当前实际航速且取鲁棒控制律为：

则s(l+1)始终满足其中，分别为标称无人艇系统水动力学模型在l时刻且初始状态为s时求得的状态量和输入量，s(l)、w(l)分别为实际系统水动力学模型在l时刻的状态量和输入量，为干扰不变集，满足其中K为状态反馈控制律，满足：

K＝-(R+B^TPB)^-1B^TPA

利用跟踪层实现航速跟踪，包括：

基于标称无人艇系统水动力学模型，得到l时刻且当前系统实际状态为s时的最优控制序列此时对应的最优状态量其中如果扰动系统实际状态则取的第一个控制量将作为实际控制量施加到系统，则下一时刻状态量满足随后进入下一轮滚动，以作为初始状态求解新的标称无人艇系统控制序列取的第一个控制量按照鲁棒控制律得到l+1时刻的控制量将其施加到系统，保证实际扰动系统的状态处于以标称无人艇系统状态为中心的不变集内，实现风浪环境中多无人艇系统航速跟踪的鲁棒性和稳定性；当下一个跟踪目标来临时，重复跟踪层实现航速跟踪的步骤，最终实现整个时序逻辑任务的航速跟踪。