[发明专利]仿生滑翔机器海豚的滑翔深度控制方法、系统、装置

权利要求书

1.一种仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，该方法包括：

步骤S100，获取预设滑翔深度、预设偏航角；

步骤S200，基于当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取估计速度，并结合所述预设偏航角，通过偏航控制器获取两侧胸鳍的控制量；

步骤S300，基于所述预设滑翔深度、当前时刻所述仿生滑翔机器海豚的深度信息，构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹，并结合当前时刻的估计速度，通过模型预测控制方法获取所述仿生滑翔机器海豚的下潜控制量；

步骤S400，根据所述下潜控制量，通过浮力原理得到所述仿生滑翔机器海豚的浮力调节机构中活塞的目标位置，并依据当前活塞位置获取活塞的控制量；

步骤S500，基于所述活塞的控制量和所述两侧胸鳍的控制量，控制所述仿生滑翔机器海豚进行滑翔。

2.根据权利要求1所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，步骤S200中“通过滑模观测器获取估计速度”，其方法为：

基于所述仿生滑翔机器海豚的坐标系，构建其对应的全状态三维动力学模型；

通过忽略侧向运动对所述全状态三维动力学模型进行简化，得到简化动力学模型；

根据当前时刻所述仿生滑翔机器海豚的深度信息、惯导信息，通过滑模观测器获取所述简化动力学模型的速度向量，将其作为所述仿生滑翔机器海豚的估计速度。

3.根据权利要求2所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，所述简化动力学模型，其表示为：

其中，v＝[u,w,q]^T分别表示海豚坐标系下的前向速度、纵向速度以及俯仰角速度，M＝diag{m₁,m₂,m₃}表示包含附加质量的惯性质量矩阵,m₁,m₂,m₃为质量参数，D＝diag{d₁,d₂,d₃}表示被简化为常数项的阻尼矩阵，d₁,d₂,d₃为阻尼参数，表示输入矩阵，u_c为实时控制量，为机器人重心与浮心的位置向量，G_m为重力加速度，θ为俯仰角，C(v)为科氏力和向心力矩阵，为速度导数。

4.根据权利要求1所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，步骤S300中“通过偏航控制器获取两侧胸鳍的控制量”其方法为：

其中，u_f为两侧胸鳍的控制量，k_f为偏航控制器的权重系数，k_p为比例因子，k_i为积分因子，k_d为微分因子，e_ψ为偏航角误差，为偏航角误差导数。

5.根据权利要求1所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，步骤S400中“构建贝塞尔曲线并进行分段，获取分段式的下潜速度参考轨迹”，其方法为：

基于所述预设滑翔深度和当前时刻所述仿生滑翔机器海豚的深度信息，构建二阶贝塞尔曲线轨迹，将其作为所述仿生滑翔机器海豚的深度参考轨迹；

对所述深度参考轨迹求导，得到下潜速度参考轨迹；

根据所述预设滑翔深度和当前时刻所述仿生滑翔机器海豚的深度信息的差的绝对值与预设的深度阈值的大小，对所述下潜速度参考轨迹进行分段，得到分段式的下潜速度参考轨迹。

6.根据权利要求5所述的仿生滑翔机器海豚的滑翔深度控制方法，其特征在于，所述分段式的下潜速度参考轨迹，其计算方法为：

其中，Y_ref为分段式的下潜速度参考轨迹，c₁、c₂表示权重系数，d_r为预设滑翔深度，d为深度信息，d_theshold为预设的深度阈值，V_ref为下潜速度参考轨迹。