CN107804446B - 用于水下航行器的三自由度仿水翼推进机构及其运动学控制方法 - Google Patents

Abstract

本发明公开了一种用于水下航行器的三自由度仿水翼推进机构及其运动学控制方法，用于解决现有水下仿水翼推进装置实用性差的技术问题。技术方案是所述推进机构由锥齿轮机构、翻转运动机构、仿生水翼和固定底座四部分组成。锥齿轮机构有两个自由度，可以带动整个翻转运动机构和仿生水翼实现运动幅值可调的竖直拍动、水平划动，从而使仿生水翼翼尖实现翼尖轨迹的调节；翻转机构独立驱动仿生水翼的翻转运动。为实现仿生水翼翼尖轨迹的调节，本发明公开了一种推进机构的运动学控制方法。从而既可以实现仿生水翼翼尖轨迹的调节，又可独立调节仿生水翼在任意位置迎水面积，更好地模拟生物的扑翼运动，实用性好。

Description

用于水下航行器的三自由度仿水翼推进机构及其运动学控制 方法

技术领域

本发明涉及一种水下仿水翼推进装置，具体涉及一种用于水下航行器的三自由度仿水翼推进机构。还涉及这种三自由度仿水翼推进机构的运动学控制方法。

背景技术

水下航行器作为一种水下无人移动平台，在海洋资源探索、海洋科学考察和军事等领域有着广阔的应用前景。目前的水下航行器多采用螺旋桨推进，单个螺旋桨推进器只能产生固定方向的推进力，然而，在自然界的水生动物中，海龟、海豚、企鹅等动物都采用鳍状肢体运动，有学者研究发现，仿生推进较同功率螺旋桨推进器有更高的推进效率。海龟等生物在水中游动时，身体躯干不作大幅度摆动，依靠鳍状前肢的扑动产生动力，称之为扑翼推进方式。扑翼推进方式融合了鸟类飞行和鱼类游动两种运动特性，又能独立控制单翼运动姿态并协调双翼一起运动达到矢量推进的效果。因此，尽管此类动物具有较大的体形，但是它们却具有爆发力强、机动性高、稳定性好、噪声低等特点。将这种推进方式应用于水下无人航行器，将极大地提高水下无人航行器推进效率。

文献1“申请公布号是CN101003301A的中国发明专利”中公开了一种水下仿水翼推进装置，该装置包括仿生水翼、水翼拍动单元、水翼旋转单元和底板固定单元。该推进装置的仿生水翼安装在水翼拍动单元上，水翼拍动单元又和水翼旋转单元连接在一起。但由于仅有两个自由度，这种装置在控制仿生水翼翼尖按照8字形运动轨迹运动时，不能随意调节仿生水翼在某一位置下的迎水面积。

文献2“申请公布号是CN102079382A的中国发明专利”中公开了一种水下机械式仿生扑翼推进器，该推进器由水平运动机构、竖直运动机构、翻转运动机构、仿生扑翼、固定底板五部分组成。整个机构运动过程为：电机驱动偏置曲柄滑块机构运动，带动水平滑块在水平滑块运动导轨中左右运动；同时，电机驱动凸轮推杆机构，带动竖直滑块相对水平滑块作竖直方向上的运动，两个运动叠加实现扑翼末梢运动轨迹的拟合，同时带动翻转运动机构运动，从而实现扑翼的翻转运动。该推进器可以在控制仿生水翼翼尖按照8字形运动轨迹运动时，通过翻转运动机构调节仿生水翼的迎水面积，使得扑翼向前运动时迎水面积小、向后划水时迎水面积大。但是受到连杆、凸轮等机械结构的限制，实际应用中一旦推进器机构中连杆、凸轮等零件尺寸确定，水平运动机构的水平运动幅值、竖直运动机构的竖直运动幅值以及翻转运动机构的运动幅值将相应确定，同一个推进器可实现的仿生运动轨迹的可行域受到限制，无法调节仿生水翼翼尖轨迹，而实际水生生物在通过水翼扑翼运动时，不同前进速度下水翼翼尖的轨迹是不同的。

发明内容

为了克服现有水下仿水翼推进装置实用性差的不足，本发明提供一种用于水下航行器的三自由度仿水翼推进机构及其运动学控制方法。所述推进机构由锥齿轮机构、翻转运动机构、仿生水翼和固定底座四部分组成。锥齿轮机构有两个自由度，可以带动整个翻转运动机构和仿生水翼实现运动幅值可调的竖直拍动、水平划动，从而使仿生水翼翼尖实现翼尖轨迹的调节；翻转机构独立驱动仿生水翼的翻转运动。为实现仿生水翼翼尖轨迹的调节，本发明公开了一种推进机构的运动学控制方法。从而既可以实现仿生水翼翼尖轨迹的调节，又可独立调节仿生水翼在任意位置迎水面积，更好地模拟生物的扑翼运动，实用性好。

本发明解决其技术问题所采用的技术方案是：一种用于水下航行器的三自由度仿水翼推进机构，其特点是：由锥齿轮机构、翻转运动机构、仿生水翼15和固定底座四部分组成。所述固定底座包括第一伺服舵机固定件4和第二伺服舵机固定件5。所述锥齿轮机构包括第一伺服舵机1、第一舵盘16、第一锥齿轮8、第一锥齿轮轴7、第一轴向限位螺钉19、第二伺服舵机2、第二舵盘17、第二锥齿轮10、第二锥齿轮轴9、第二轴向限位螺钉20、第三锥齿轮12、第三锥齿轮轴11、第三轴向限位螺钉21和中心支撑架13。第一伺服舵机1通过第一伺服舵机固定件4安装在水下航行器上，第一伺服舵机1输出轴与第一舵盘16连接，第一舵盘16与第一锥齿轮轴7连接，第一锥齿轮8固定在第一锥齿轮轴7上，第一锥齿轮轴7右端穿过中心支撑架13，使用第一轴向限位螺钉19限制中心支撑架13相对于第一锥齿轮轴7的轴向运动；第二伺服舵机2通过第二伺服舵机固定件5安装在水下航行器上，第二伺服舵机2输出轴与第二舵盘17连接，第二舵盘17与第二锥齿轮轴9连接，第二锥齿轮10固定在第二锥齿轮轴9上，第二锥齿轮轴9左端穿过中心支撑架13，使用第二轴向限位螺钉20限制中心支撑架13相对于第二锥齿轮轴9的轴向运动；第三锥齿轮12固定在第三锥齿轮轴11上，第三锥齿轮轴11下端穿过中心支撑架13，使用第三轴向限位螺钉21限制中心支撑架13相对于第三锥齿轮轴11的轴向运动。所述第一锥齿轮8、第二锥齿轮10、第三锥齿轮12的模数、齿数均相等，第一锥齿轮8与第三锥齿轮12垂直啮合，第二锥齿轮10与第三锥齿轮12垂直啮合。第三锥齿轮12初始安装位置为其中心轴线处于竖直平面。所述翻转运动机构包括第三伺服舵机3、第三舵盘18和水翼连接轴14。第三伺服舵机3输出轴接第三舵盘18，第三舵盘18与水翼连接轴14连接，水翼连接轴14与仿生水翼15连接；第三伺服舵机3通过伺服舵机连接件6与第二锥齿轮轴9上端连接。

所述仿生水翼15边缘具有柔性。

一种上述用于水下航行器的三自由度仿水翼推进机构的运动学控制方法，其特点是包括以下步骤：

步骤一、对三自由度仿水翼推进机构进行运动学建模，抽象出数学模型，以分析指导机构的运动控制。

(a)锥齿轮机构建模。

在第一伺服舵机1、第二伺服舵机2的驱动下，第三锥齿轮12做空间定点转动。该定点运动分解为绕自身对称轴旋转的自转运动和绕第一伺服舵机1中心轴线旋转的周转运动，用自转角度θ_y和周转角度θ_p两个参数确定第三锥齿轮12的空间姿态，根据齿轮啮合关系以及刚体运动学知识，得到第一伺服舵机1、第二伺服舵机2的转动角度与第三锥齿轮12的姿态角的关系：

根据公式(1)：

θ₁＝θ_p+θ_y,θ₂＝θ_y-θ_p (2)

式中，θ₁为第一伺服舵机1的转动角度，θ₂为第二伺服舵机2的转动角度，θ_y为第三锥齿轮12的自转角度，θ_p为第三锥齿轮12的周转角度。

(b)水翼运动学建模。

第三伺服舵机3与第三锥齿轮12连接在一起，具有相同的运动规律，第三伺服舵机3转角θ₃直接控制仿生水翼15绕轴的横滚角θ_r，且有

θ₃＝θ_r (3)

式中，θ₃为第三伺服舵机3转角，θ_r为仿生水翼15绕轴的横滚角。

综上，用θ_y、θ_p和θ_r三个角度确定仿生水翼15的空间姿态，描述仿生水翼15相对于惯性参考系的空间姿态的矩阵为：

已知水翼上一点在水翼坐标系中的位置坐标^Bp，则该点在惯性参考系中的空间坐标为：

步骤二、建立机构的逆运动学，找到三维空间下的仿生水翼15翼尖运动轨迹坐标与机构关节空间下的第一伺服舵机1、第二伺服舵机2转角θ₁、θ₂之间的关系。

已知水翼上一点^Bp＝[R 0 0]^T在惯性坐标系中的坐标^Ep＝[p_x p_y p_z]^T，则根据公式(4)：

根据公式(5)求解描述仿生水翼15空间姿态角θ_y、θ_p：

根据公式(2)、公式(6)，得到θ₁、θ₂与^Ep＝[p_x p_y p_z]^T关系：

步骤三、输入所需的仿生水翼15翼尖一点^Bp＝[R 0 0]^T在惯性坐标系中的运动轨迹，在目标轨迹上取n个目标轨迹点，这些目标轨迹点在惯性坐标系中的坐标为^Ep_i＝[p_ixip_iy p_iz]^T,(i＝1,2,3,...,n)，通过公式(7)求解每一个目标轨迹点^Ep_i所对应的第一伺服舵机1、第二伺服舵机2转角θ_i1、θ_i2。

步骤四、根据仿生水翼15翼尖处于不同位置时的水翼翼板迎水面积，确定仿生水翼15翼尖处于不同位置时的水翼横滚角θ_r，根据公式(3)求解第三伺服舵机3转角θ₃。

步骤五、用步骤三、步骤四求解的机构关节空间下的关节角θ₁、θ₂和θ₃数据直接控制角度输出的第一伺服舵机1、第二伺服舵机2和第三伺服舵机3，使仿生水翼15完成目标运动状态。

本发明的有益效果是：所述推进机构由锥齿轮机构、翻转运动机构、仿生水翼和固定底座四部分组成。锥齿轮机构有两个自由度，可以带动整个翻转运动机构和仿生水翼实现运动幅值可调的竖直拍动、水平划动，从而使仿生水翼翼尖实现翼尖轨迹的调节；翻转机构独立驱动仿生水翼的翻转运动。为实现仿生水翼翼尖轨迹的调节，本发明公开了一种推进机构的运动学控制方法。从而既可以实现仿生水翼翼尖轨迹的调节，又可独立调节仿生水翼在任意位置迎水面积，更好地模拟生物的扑翼运动，实用性好。

以下结合附图和实施例详细说明本发明。

附图说明

图1是本发明用于水下航行器的三自由度仿水翼推进机构的运动简图。

图2是本发明用于水下航行器的三自由度仿水翼推进机构的的立体示意图。

图3是本发明用于水下航行器的三自由度仿水翼推进机构的的立体分解图。

图4是本发明用于水下航行器的三自由度仿水翼推进机构的的锥齿轮机构的运动学模型。

图5是本发明用于水下航行器的三自由度仿水翼推进机构的翼尖做8字形运动的轨迹示意图。

图6是本发明用于水下航行器的三自由度仿水翼推进机构的翼尖做划水运动的轨迹示意图。

图中，1-第一伺服舵机，2-第二伺服舵机，3-第三伺服舵机，4-第一伺服舵机固定件，5-第二伺服舵机固定件，6-伺服舵机连接件，7-第一锥齿轮轴，9-第二锥齿轮轴，11-第三锥齿轮轴，8-第一锥齿轮，10-第二锥齿轮，12-第三锥齿轮，13-中心支撑架，14-水翼连接轴，15-仿生水翼，16-第一舵盘，17-第二舵盘，18-第三舵盘，19-第一轴向限位螺钉，20-第二轴向限位螺钉，21-第三轴向限位螺钉。

具体实施方式

以下实施例参照图1－6。

装置实施例。本发明一种用于水下航行器的三自由度仿水翼推进机构，由锥齿轮机构、翻转运动机构、仿生水翼、固定底座四部分组成。仿生水翼15安装在翻转运动机构上，翻转运动机构又和锥齿轮机构连接在一起，锥齿轮机构安装在固定底座上。

所述固定底座包括第一伺服舵机固定件4、第二伺服舵机固定件5。

所述锥齿轮机构包括第一伺服舵机1、第一舵盘16、第一锥齿轮8、第一锥齿轮轴7、第一轴向限位螺钉19、第二伺服舵机2、第二舵盘17、第二锥齿轮10、第二锥齿轮轴9、第二轴向限位螺钉20、第三锥齿轮12、第三锥齿轮轴11、第三轴向限位螺钉21、中心支撑架13。第一伺服舵机1通过第一伺服舵机固定件4安装在水下航行器上，第一伺服舵机1输出轴与第一舵盘16连接，第一舵盘16与第一锥齿轮轴7连接，第一锥齿轮8固定在第一锥齿轮轴7上，第一锥齿轮轴7右端穿过中心支撑架13，使用第一轴向限位螺钉19限制中心支撑架13相对于第一锥齿轮轴7的轴向运动；第二伺服舵机2通过第二伺服舵机固定件5安装在水下航行器上，第二伺服舵机2输出轴与第二舵盘17连接，第二舵盘17与第二锥齿轮轴9连接，第二锥齿轮10固定在第二锥齿轮轴9上，第二锥齿轮轴9左端穿过中心支撑架13，使用第二轴向限位螺钉20限制中心支撑架13相对于第二锥齿轮轴9的轴向运动；第三锥齿轮12固定在第三锥齿轮轴11上，第三锥齿轮轴11下端穿过中心支撑架13，使用第三轴向限位螺钉21限制中心支撑架13相对于第三锥齿轮轴11的轴向运动。所述第一锥齿轮8、第二锥齿轮10、第三锥齿轮12模数、齿数均相等，第一锥齿轮8与第三锥齿轮12垂直啮合，第二锥齿轮10与第三锥齿轮12垂直啮合。第三锥齿轮12初始安装位置为其中心轴线处于竖直平面。

所述仿生水翼15有一定翼形，水翼边缘具有柔性。

所述翻转运动机构包括第三伺服舵机3、第三舵盘18、水翼连接轴14。第三伺服舵机3输出轴接第三舵盘18，第三舵盘18与水翼连接轴14连接，水翼连接轴14与仿生水翼15连接；第三伺服舵机3再通过伺服舵机连接件6与第二锥齿轮轴9上端连接。

本实施方式中，将两套三自由度仿水翼推进机构对称安装到水下航行器的两侧。第一锥齿轮轴7、第二锥齿轮轴9中心轴线指向水下航行器的前后方向，第一锥齿轮轴7靠近水下航行器的头部，第三锥齿轮轴11的中心轴线初始位置指向航行器顶部。

三自由度仿水翼推进机构工作时，翻转运动机构直接独立驱动仿生水翼15做翻转运动，以改变其迎水面积。锥齿轮机构有两个自由度，可以带动整个翻转运动机构和仿生水翼15实现一定运动幅值的竖直拍动、水平划动，从而使仿生水翼15翼尖按照预定运动轨迹运动。因为水下航行器左右对称安装了两套推进机构，两套推进机构的运动是各自独立的。

方法实施例1。

步骤一、对三自由度仿水翼推进机构进行运动学建模，抽象出数学模型，以分析指导机构的运动控制。

为了描述水翼以及机构的运动，建立惯性参考坐标系OXYZ，记作坐标系{E}，取第一锥齿轮轴7、第二锥齿轮轴9和第三锥齿轮轴11的交点为惯性坐标系原点，以第一锥齿轮8和第二锥齿轮10的对称中心轴线为OY轴，正方向指向第二锥齿轮10，取竖直向上为OZ轴正方向，根据右手法则，OX轴与机构初始状态下水翼连接轴14的中心轴线重合。

此外，建立与仿生水翼15固连的水翼坐标系oxyz，记作坐标系{B}，坐标原点与惯性坐标系坐标原点重合，ox轴与水翼连接轴14的中心轴线始终重合，oz轴始终垂直于仿生水翼15纵对称面，oy轴根据右手法则确定。

(a)锥齿轮机构建模。

在第一伺服舵机1、第二伺服舵机2的驱动下，第三锥齿轮12做空间定点转动。该定点运动分解为绕自身对称轴旋转的自转运动和绕第一伺服舵机1中心轴线旋转的周转运动，用自转角度θ_y和周转角度θ_p两个参数确定第三锥齿轮12的空间姿态，根据齿轮啮合关系以及刚体运动学知识，得到第一伺服舵机1、第二伺服舵机2的转动角度与第三锥齿轮12的姿态角的关系：

根据公式(1)：

θ₁＝θ_p+θ_y,θ₂＝θ_y-θ_p (2)

式中，θ₁为第一伺服舵机1的转动角度，θ₂为第二伺服舵机2的转动角度，θ_y为第三锥齿轮12的自转角度，θ_p为第三锥齿轮12的周转角度。

(b)水翼运动学建模。

第三伺服舵机3与第三锥齿轮12连接在一起，具有相同的运动规律，第三伺服舵机3转角θ₃直接控制仿生水翼15绕轴的横滚角θ_r，且有

θ₃＝θ_r (3)

式中，θ₃为第三伺服舵机3转角，θ_r为仿生水翼15绕轴的横滚角。

综上，用θ_y、θ_p和θ_r三个角度确定仿生水翼15的空间姿态，描述仿生水翼15相对于惯性参考系的空间姿态的矩阵为：

已知水翼上一点在水翼坐标系中的位置坐标^Bp，则该点在惯性参考系中的空间坐标为：

步骤二、建立机构的逆运动学，找到三维空间下的仿生水翼15翼尖运动轨迹坐标与机构关节空间下的第一伺服舵机1、第二伺服舵机2转角θ₁、θ₂之间的关系。

已知水翼上一点^Bp＝[R 0 0]^T在惯性坐标系中的坐标^Ep＝[p_x p_y p_z]^T，则根据公式(4)：

根据公式(5)求解描述仿生水翼15空间姿态角θ_y、θ_p：

根据公式(2)、公式(6)，得到θ₁、θ₂与^Ep＝[p_x p_y p_z]^T关系：

步骤三、取仿生水翼15旋转轴上一固定点^Bp＝[R 0 0]^T，R＝201.9mm，所输入的仿生水翼15翼尖目标轨迹为一处于R＝201.9mm的球面上的8字形轨迹，在目标轨迹上取n个目标轨迹点，这些目标轨迹点在惯性坐标系中的坐标为^Ep_i＝[p_ixi p_iy p_iz]^T,(i＝1,2,3,...,n)，此处取n＝32，前十个目标轨迹点的坐标为：

将目标轨迹点代入(7)式，即可求解出能够使机构带动水翼上固定点^Bp到达这些目标轨迹点所需的机构关节空间下的关节角θ₁、θ₂。

序号	θ<sub>1</sub>/°	θ<sub>2</sub>/°	序号	θ<sub>1</sub>/°	θ<sub>2</sub>/°
						1	0	65.32814	6	0	6.786512
2	16.40296	70.28802	7	7.000337	-2.79038
						3	31.76804	70.9981	8	12.41212	-13.9719
4	45.36	67.73806	9	14.76253	-24.901
						5	56.73157	61.57681	10	12.96186	-34.0547

步骤四、根据仿生水翼15翼尖处于8字形轨迹不同位置时的水翼翼板迎水面积，确定仿生水翼15翼尖处于不同位置时的水翼横滚角θ_r，根据公式(3)求解第三伺服舵机3转角θ₃。

步骤五、用步骤三、步骤四求解的机构关节空间下的关节角θ₁、θ₂和θ₃数据直接控制角度输出的第一伺服舵机1、第二伺服舵机2和第三伺服舵机3，使仿生水翼15完成目标运动状态。

方法实施例2.

当三自由度仿水翼机构做划水运动时，取仿生水翼15旋转轴上一固定点^Bp＝[R 00]^T为参考点，则该点的运动轨迹近似为一段弧线，仿生水翼15运动过程中仿生水翼15横滚角θ_r存在变化。为了实现这种运动，机构运动学控制方法步骤与具体实施方式一相同，区别在于步骤三第一伺服舵机1、第二伺服舵机2转角θ₁、θ₂求解过程中，所输入的仿生水翼15翼尖目标轨迹为一条位于水平面XOY上的弧线，通过公式(7)求解得到第一伺服舵机1、第二伺服舵机2转角θ₁、θ₂。

Claims (3)

1.一种用于水下航行器的三自由度仿水翼推进机构，其特征在于：由锥齿轮机构、翻转运动机构、仿生水翼(15)和固定底座四部分组成；所述固定底座包括第一伺服舵机固定件(4)和第二伺服舵机固定件(5)；所述锥齿轮机构包括第一伺服舵机(1)、第一舵盘(16)、第一锥齿轮(8)、第一锥齿轮轴(7)、第一轴向限位螺钉(19)、第二伺服舵机(2)、第二舵盘(17)、第二锥齿轮(10)、第二锥齿轮轴(9)、第二轴向限位螺钉(20)、第三锥齿轮(12)、第三锥齿轮轴(11)、第三轴向限位螺钉(21)和中心支撑架(13)；第一伺服舵机(1)通过第一伺服舵机固定件(4)安装在水下航行器上，第一伺服舵机(1)输出轴与第一舵盘(16)连接，第一舵盘(16)与第一锥齿轮轴(7)连接，第一锥齿轮(8)固定在第一锥齿轮轴(7)上，第一锥齿轮轴(7)右端穿过中心支撑架(13)，使用第一轴向限位螺钉(19)限制中心支撑架(13)相对于第一锥齿轮轴(7)的轴向运动；第二伺服舵机(2)通过第二伺服舵机固定件(5)安装在水下航行器上，第二伺服舵机(2)输出轴与第二舵盘(17)连接，第二舵盘(17)与第二锥齿轮轴(9)连接，第二锥齿轮(10)固定在第二锥齿轮轴(9)上，第二锥齿轮轴(9)左端穿过中心支撑架(13)，使用第二轴向限位螺钉(20)限制中心支撑架(13)相对于第二锥齿轮轴(9)的轴向运动；第三锥齿轮(12)固定在第三锥齿轮轴(11)上，第三锥齿轮轴(11)下端穿过中心支撑架(13)，使用第三轴向限位螺钉(21)限制中心支撑架(13)相对于第三锥齿轮轴(11)的轴向运动；所述第一锥齿轮(8)、第二锥齿轮(10)、第三锥齿轮(12)的模数、齿数均相等，第一锥齿轮(8)与第三锥齿轮(12)垂直啮合，第二锥齿轮(10)与第三锥齿轮(12)垂直啮合；第三锥齿轮(12)初始安装位置为其中心轴线处于竖直平面；所述翻转运动机构包括第三伺服舵机(3)、第三舵盘(18)和水翼连接轴(14)；第三伺服舵机(3)输出轴接第三舵盘(18)，第三舵盘(18)与水翼连接轴(14)连接，水翼连接轴(14)与仿生水翼(15)连接；第三伺服舵机(3)通过伺服舵机连接件(6)与第二锥齿轮轴(9)上端连接。

2.根据权利要求1所述的用于水下航行器的三自由度仿水翼推进机构，其特征在于：所述仿生水翼(15)边缘具有柔性。

3.一种权利要求1所述用于水下航行器的三自由度仿水翼推进机构的运动学控制方法，其特征在于包括以下步骤：

步骤一、对三自由度仿水翼推进机构进行运动学建模，抽象出数学模型，以分析指导机构的运动控制；

(a)锥齿轮机构建模；

在第一伺服舵机(1)、第二伺服舵机(2)的驱动下，第三锥齿轮(12)做空间定点转动；该定点转动分解为绕自身对称轴旋转的自转运动和绕第一伺服舵机(1)中心轴线旋转的周转运动，用自转角度θ_y和周转角度θ_p两个参数确定第三锥齿轮(12)的空间姿态，根据齿轮啮合关系以及刚体运动学知识，得到第一伺服舵机(1)、第二伺服舵机(2)的转动角度与第三锥齿轮(12)的姿态角的关系：

根据公式(1)：

θ₁＝θ_p+θ_y,θ₂＝θ_y-θ_p (2)

式中，θ₁为第一伺服舵机(1)的转动角度，θ₂为第二伺服舵机(2)的转动角度，θ_y为第三锥齿轮(12)的自转角度，θ_p为第三锥齿轮(12)的周转角度；

(b)水翼运动学建模；

第三伺服舵机(3)与第三锥齿轮(12)连接在一起，具有相同的运动规律，第三伺服舵机(3)转角θ₃直接控制仿生水翼(15)绕轴的横滚角θ_r，且有

θ₃＝θ_r (3)

式中，θ₃为第三伺服舵机(3)转角，θ_r为仿生水翼(15)绕轴的横滚角；

综上，用θ_y、θ_p和θ_r三个角度确定仿生水翼(15)的空间姿态，描述仿生水翼(15)相对于惯性参考系的空间姿态的矩阵为：

已知水翼上一点在水翼坐标系中的位置坐标^Bp，则该点在惯性参考系中的空间坐标为：

步骤二、建立机构的逆运动学，找到三维空间下的仿生水翼(15)翼尖运动轨迹坐标与机构关节空间下的第一伺服舵机(1)、第二伺服舵机(2)转角θ₁、θ₂之间的关系；

已知水翼上一点^Bp＝[R 0 0]^T在惯性坐标系中的坐标^Ep＝[p_x p_y p_z]^T，则根据公式(4)：

根据公式(5)求解描述仿生水翼(15)空间姿态角θ_y、θ_p：

根据公式(2)、公式(6)，得到θ₁、θ₂与^Ep＝[p_x p_y p_z]^T关系：

步骤三、输入所需的仿生水翼(15)翼尖一点^Bp＝[R 0 0]^T在惯性坐标系中的运动轨迹，在目标轨迹上取n个目标轨迹点，这些目标轨迹点在惯性坐标系中的坐标为^Ep_i＝[p_ixi p_iyp_iz]^T,(i＝1,2,3,...,n)，通过公式(7)求解每一个目标轨迹点^Ep_i所对应的第一伺服舵机(1)、第二伺服舵机(2)转角θ_i1、θ_i2；

步骤四、根据仿生水翼(15)翼尖处于不同位置时的水翼翼板迎水面积，确定仿生水翼(15)翼尖处于不同位置时的水翼横滚角θ_r，根据公式(3)求解第三伺服舵机(3)转角θ₃；

步骤五、用步骤三、步骤四求解的机构关节空间下的关节角θ₁、θ₂和θ₃数据直接控制角度输出的第一伺服舵机(1)、第二伺服舵机(2)和第三伺服舵机(3)，使仿生水翼(15)完成目标运动状态。