[发明专利]基于分布式模型预测控制的中性浮力机器人姿态与轨迹控制方法

权利要求书

1.一种基于分布式模型预测控制的中性浮力机器人姿态与轨迹控制方法，其特征在于，包括以下步骤：

步骤一：将中性浮力系统动力学模型写成状态空间方程式并进行线性化，得到线性模型，具体操作为：

考虑中性浮力机器人在体坐标系下的动力学模型如式(1)：

其中，M为惯性质量矩阵，C(v)为科里奥利力矩阵，D(v)为机器人在水中受到的黏性阻力，g(η)为负浮力系数，τ为系统输入，为中性浮力机器人的加速度，v为中性浮力机器人的速度；

所考虑的中性浮力机器人在地理坐标系Ox_ny_nz_n和机器人体坐标系Ox_by_bz_b的关系如下式(2)：

其中，是η的导数，J(η)为运动系数矩阵；分别指机器人在Ox_n、Oy_n和Oy_z方向的位置；分别指机器人的横滚角，俯仰角以及偏航角，为机器人线速度向量，为机器人角速度向量；

联立式(1)和式(2)，得到中性浮力机器人惯性坐标系下的动力学模型：

其中，M_η(η)＝J^-T(η)MJ^-1(η)；

D_η(η,v)＝J^-T(η)D(v)J^-1(η)，g_η(η)＝J^-T(η)g(η)；

令x₁＝η，和u＝τ，则式(3)表示为：

其中，是x(t)的导数，x₁(t)和x₂(t)是系统的两个状态；

将式(4)进行线性化处理，得到系统(5)：

其中，和为系统线性化后的参数，x(t)为系统的状态，u(t)为系统的控制量；

且(x₀,u₀)为系统(5)的平衡点，同时考虑系统(5)受到有界随机扰动ω(t)的影响，且ω(t)服从一定的概率分布，则得到：

并且取：

步骤二：利用delta方法将线性模型离散化，得到系统的离散化形式，具体操作为：

使用delta算子方法对系统(6)、(7)进行离散化，并且使用预测数据来解决丢包与时延问题，经过delta算子方法离散化后的系统模型变为：

δx_p(t_k)＝A_px_p(t_k)+B_pu_p(t_k)+G_pω_p(t_k)

y_p(t_k)＝C_px_p(t_k)+D_pu_p(t_k)

其中，

T为选定的基础采样周期，即每个中性浮力机器人的控制器都以T为周期来离散化各自的系统；

再将delta算子系统写成离散化的形式得到：

x_p(t_k+1)＝(TA_p+I_p)x_p(t_k)+TB_Pu_P(t_k)+TG_pω_p(t_k) (8)

y_p(t_k)＝C_px_p(t_k)+D_pu_p(t_k) (9)；

步骤三：建立步骤二得到系统的优化问题，并将优化问题中的局部概率约束与耦合概率约束通过变化得到确定性的局部约束和耦合约束，具体操作为：

考虑每个中性浮力机器人都受到关于输出y_p(t_k)的局部概率约束：

Pr{y_p(t_k)≤h_p}≥l_p (10)

且每个中性浮力机器人耦合输出量s_cp之和满足如下形式的约束；

s_cp(t_k)＝E_cpx_p(t_k)+F_cpu_p(t_k) (11)

其中，行向量C_p，D_p，E_cp，F_cp，标量h_p，b_c和概率l_p∈[0,1]，p_c∈[0,1]都是事先根据实际情况设定的常量

步骤四：通过求解受局部约束与耦合约束的优化问题，得到最优控制量，从而对中性浮力系统进行控制。

2.根据权利要求1所述的基于分布式模型预测控制的中性浮力机器人姿态与轨迹控制方法，其特征在于，利用双模控制器的思想为每个中性浮力机器人设计如下形式的模型预测控制器：

u_p(t_k+i|t_k)＝K_px_p(t_k+i|t_k)+c_p(t_k+i|t_k),i＝0,1,……

其中，K_p为离线确定的线性状态反馈增益矩阵，使得对于子系统(A_p,B_p)，A_p+B_pK_p是严格稳定的；

对于选定的有限预测时域N，c_p(t_k+i|t_k)，i＝0,1,…,N-1是通过在线求解下述优化问题得到的；

当i≥N时，c_p(t_k+i|t_k)＝0，考虑局部成本函数：

其中，L_p可以离线求得，通过求解如下优化问题：

min J_p(C(t_k))

约束为式(10)与(12)，得到最优控制量