CN113859482A - 基于gps巡航与姿态解算的智能救生系统及控制方法 - Google Patents

Abstract

本发明公开了一种基于GPS巡航与姿态解算的智能救生系统，包括救生圈、设置于救生圈上的多个机械臂以及主控模块，每个机械臂内设有陀螺仪模块，用于采集水波纹数据；陀螺仪模块电连接一姿态解算模块，与主控模块电连接，用于获取陀螺仪模块的数据，并通过波动检测定位算法获取振动源位置；救生圈上设有一GPS模块，主控模块通过GPS模块和巡航算法控制救生圈沿预设巡航路线或搜救路线运动；救生圈上设有多个电机马达，用于驱动救生圈运动。本发明通过在传统救生圈上加设机械臂、主控模块、陀螺仪模块、姿态解算模块、GPS模块以及电机马达，以较低成本将传统救生圈升级为智能救生圈，能够对指定水域进行巡航和搜救，救援效率高。

Description

基于GPS巡航与姿态解算的智能救生系统及控制方法

技术领域

本发明涉及救生装置领域，特别涉及一种基于GPS巡航与姿态解算的智能救生系统及控制方法。

背景技术

传统的智能救生装置多数设计为在下水时佩戴在游泳者身上的装置。遇险时，智能防溺水装置能检测并启动报警系统，弹出气囊等浮漂装置，发出设计的声光报警等。这类产品很好地提升了一类溺水事件的生存率，但对于意外落水类事件，仍然无法解决。

随后发展的遥控式智能救生圈，配备高速水下螺旋桨，操作人员在岸边使用遥控便能使智能救生圈迅速接近落水者。这一设计不仅解决了救援时救生人员下水的安全隐患问题，还因其强大的水上行驶能力(救生员12倍以上)，有效减地少了救援时间，大大增加落水者生还几率。与传统智能救生装置比，遥控式智能救生圈能应变更多的突发状况。但其遥控的设计，依赖于配备专业的救生人员，对于无人管辖的水域如水库，人工湖等，不能很好地完成救援。并且，还受到遥控距离的限制，对于室外面积大的水域，无法有效地完成救援。再者，现有的遥控式智能救生圈的设计大多为“U型”异形救生圈，需要定制，造价昂贵，在基金上难以充足配备至所需水域。

后续发展的智能救援系统使用多个传感器如人体红外感应，超声波、激光测距及人脸识别来实现落水者位置检测与营救。其采集的参数多，对比任务繁重，且红外检测，人脸识别和测距在现实环境下受不确定因素如天气，光线，障碍物等影响大，难以降低或消除误差，导致识别准确率低，救援效率难以保证。

目前国内的水面救援主要依赖专业救生人员，而水库等由于地理位置偏僻，面积较大等原因，较少配置专业人员，导致该类场景溺水事件高发。市场现有产品以遥控系统为主，无法摆脱对他人救援的依赖，在少人出没的地点或时间段不具备适用性，同时价格相对较高。

发明内容

本发明所要解决的技术问题是：提供一种基于GPS巡航与姿态解算的智能救生系统，能适用于无人、缺少管制的大型开放水域，在操作人员与智能救生设施配备难以充足实现的情况下实现高效率、低成本防溺水智能救生。

本发明解决上述问题所采用的技术方案为：基于GPS巡航与姿态解算的智能救生系统，包括救生圈、设置于救生圈上的多个机械臂以及主控模块，每个所述机械臂内设有一与主控模块电连接的陀螺仪模块，用于采集水波纹数据，实时分析水波纹波动幅度及频率；所述陀螺仪模块电连接一姿态解算模块，所述姿态解算模块与主控模块电连接，用于获取陀螺仪模块的俯仰角和翻滚角，并通过波动检测定位算法获取振动源位置；所述救生圈上设有一与主控模块电连接的GPS模块，所述主控模块通过GPS模块和巡航算法控制救生圈沿预设巡航路线或搜救路线运动；所述救生圈上设有多个电机马达，用于驱动救生圈进行运动。

与现有技术相比，本发明的优点在于：通过在传统救生圈上加设机械臂、主控模块、陀螺仪模块、姿态解算模块、GPS模块以及电机马达，以较低成本将传统救生圈升级为智能救生圈，能够对指定水域进行巡航和搜救，救援效率高。

优选的，所述救生圈上设有一用于检测落水者是否抓住救生圈的水压传感器。这样，通过水压传感器来检测水压值，来判断落水者是否抓住救生圈，再通过主控模块驱动电机马达，带落水者返回最近的岸边，智能化程度高。

优选的，所述GPS模块获取的救生圈坐标和指定点坐标的差值通过PID算法转换为电机马达的转速。差值越大，说明救生圈距离越远，通过PID算法转换的电机马达转速需更快，这样通过PID算法来不断调整，以确保驶向救援位置的准确性和高效性。

优选的，所述机械臂包括密封箱和连接杆，所述陀螺仪模块设置于密封箱内，所述连接杆一端与密封箱连接，另一端通过一抱箍与救生圈固定。这样，机械臂能够紧密的固定在救生圈上，保证整个智能救生系统的稳定性，同时也确保各个模块与救生圈位置的相对固定。

优选的，所述机械臂为四个，四个所述机械臂周向均匀分布设置于救生圈外圈上。这样，通过四个机械臂成轴对称形式均匀分布在救生圈上，保证整个救生圈能够平稳的落在水面上，同时也方便通过波动检测定位算法来确定落水者的位置。

优选的，所述救生圈上设有多个太阳能发电系统，所述太阳能发电系统与陀螺仪模块、主控模块、GPS模块和电机马达电连接。这样，通过太阳能充电，保证整个系统能量充足。

本发明所要解决的第二技术问题是：提供一种基于GPS巡航与姿态解算的智能救生系统的控制方法，通过对陀螺仪模块采集的水波纹数据来判断是进入巡航模式还是搜救模式，并通过波动检测定位算法来确定落水者的位置，来提高搜救的效率。

本发明解决上述问题所采用的技术方案为：一种基于GPS巡航与姿态解算的智能救生系统的控制方法，包括如下步骤：

A1、校准所有陀螺仪模块，并将整个智能救生系统置于指定水域；

A2、主控模块分析陀螺仪模块采集的水波纹数据是否超过阈值，若不是，进入步骤A3，否则，进入A4；

A3、进入巡航模式，主控模块控制电机马达按照给定转速通过巡航算法沿预设巡航路线在指定水域范围内巡航，返回步骤A2；

A4、进入搜救模式，姿态解算模块获取陀螺仪模块的俯仰角和翻滚角，并通过波动检测定位算法获取振动源位置；

A5、GPS模块获取救生圈的坐标和指定点坐标的差值通过PID算法转换为电机马达的转速，主控模块控制电机马达按照转换后的转速和预设搜救路线，到达落水者位置；

A6、若水压传感器检测到水压数据超出预设水压阈值，则主控模块进入返航模式，主控模块驱动电机马达带动救生圈返回最近岸边，否则返回步骤A4。

这样，整个智能救生系统平时均处于巡航模式，能够全天候检测水域状态，若出现落水者，能够第一时间转换为搜救模式，把落水者带到岸边，提高救援效率。

优选的，在步骤A5中，救生圈坐标和指定点坐标的差值越大，电机马达的转速越快。这样，保证救生圈能以最快速度到达落水者位置。

优选的，在步骤A4中，所述波动检测定位算法包括如下步骤：

S11、四个陀螺仪模块分别获取各自的俯仰角和翻滚角数据，并将数据传递至姿态解算模块；

S12、姿态解算模块获取数据，经卡尔曼滤波及傅里叶变换处理得到水波纹的幅度大小和频率f；

S13、以救生圈中心为原点，四个陀螺仪模块为坐标轴，建立直角坐标系，根据频率与波长的关系公式，得到水波纹波长在X轴和Y轴向量，

其中，l_x为水波纹波长在X轴的向量，f_x为水波纹频率在X轴的分量，l_y为水波纹波长在Y轴的向量，f_y，为水波纹频率在Y轴的分量；

S14、确定振动源象限，主控模块判断四个水波纹幅度绝对值的大小，水波纹幅度绝对值最大的两个相邻陀螺仪所夹象限为振动源位置；

S15、确定振动源距离和角度，水波纹幅度绝对值最大的两个相邻陀螺仪和振动源建立三角形，其三个内角分别为angNew1，angNew2，angNew3，对应的三条边分别为side1，side2，side3，

angNew1＝θ₁+45°

angNew2＝θ₂+45°

angNew3＝180°-angNew1-angNew2＝90°-θ₁-θ₂

side1＝sin(angNew1)×side3÷sin(angNew3)

side2＝sin(angNew2)×side3÷sin(angNew3)

其中，θ₁为第一陀螺仪与X轴夹角，θ₂为第二陀螺仪与Y轴夹角，l_x1为第一陀螺仪检测的水波纹波长在X轴的向量，l_y1为第一陀螺仪检测的水波纹波长在Y轴的向量，l_x2为第二陀螺仪检测的水波纹波长在X轴的向量，l_y2为第二陀螺仪检测的水波纹波长在Y轴的向量，d为第二陀螺仪到救生圈中心点距离，

振动源距离原点的距离distance：

振动源的角度angle：

这样，通过波动检测定位算法，计算出落水者距离救生圈的距离，以及角度，明确落水者的位置，提高救援效率。

优选的，在步骤A3中，所述巡航算法包括如下步骤：

S21、测量边界坐标，取点n个，记录每个点的坐标(x_n，y_n)，通过n个边界坐标点的坐标取均值，算出中心点坐标(x，y)，记迭代次数为1，其中

S22、将n个边界坐标点分别与中心点连线，以n个边界坐标点分别到中心点的距离为移动参数乘以设定的参数，算出新点的坐标(x′_n，y′_n)，迭代次数加一，其中，

k为向内移动的距离参数；

S23、将上一次产生新点的坐标分别与中心点连线，以上一次产生新点分别到中心点的距离为移动参数乘以设定的参数，算出下一个新点的坐标，并使迭代次数加一；

S24、重复步骤S23，直至达到预设的迭代次数；

S25、将所有点按照顺序首尾相连，形成螺线巡航路线。

这样，螺线循线能提高巡逻效率，这样避免了单侧巡逻的局限性，对水域的实时情况能有更及时的反馈。

附图说明

图1是本发明基于6PS巡航与姿态解算的智能救生系统结构示意图；

图2是本发明机械臂爆炸示意图；

图3是本发明基于GPS巡航与姿态解算的智能救生系统的控制方法流程图；

图4是本发明波动检测与定位模型；

图5是本发明智能救生系统坐标轴示意图；

图6是本发明二象限振动数学模型；

图7是本发明自动巡航算法模型；

图8是本发明螺线自动巡航模型；

图9是本发明不规则水域巡航模型。

图中标号说明：1、救生圈，2、机械臂，21、密封箱，211、箱体，2111、凹槽，2112、通孔，212、密封圈，213、箱盖，22、连接杆，221、连接柱，222、外支撑杆，223、外支撑套，224、第一轴承，225、第二轴承，226、锁销，23、固定块，231、第一固定块，232、第二固定块，233、第三轴承，234、连接器，24、固定块凹槽，3、抱箍。

具体实施方式

下面结合附图对本发明的实施例作进一步描述。

实施例一

如图1-2所示，本实施例涉及基于GPS巡航与姿态解算的智能救生系统，包括救生圈1、设置于救生圈1上的多个机械臂2以及主控模块。

其中，每个机械臂2内设有一与主控模块电连接的陀螺仪模块，用于采集水波纹数据，实时分析水波纹波动幅度及频率。

陀螺仪模块电连接一姿态解算模块，姿态解算模块与主控模块电连接，用于获取陀螺仪模块的俯仰角和翻滚角，并通过波动检测定位算法获取振动源位置。

救生圈1上设有一与主控模块电连接的GPS模块，主控模块通过GPS模块和巡航算法控制救生圈沿预设巡航路线或搜救路线运动。

救生圈1上设有多个电机马达，用于驱动救生圈进行运动。

其中，每个机械臂2内均设有一个陀螺仪模块，与陀螺仪模块连接的姿态解算模块可以是一个；也可以是多个，每个姿态解算模块与陀螺仪模块一一对应。在本实施例中，为了方便数据处理和节约成本，使用一个姿态解算模块，共同处理多个陀螺仪模块采集的数据。

同时，陀螺仪模块与姿态解算模块采用的电连接，可以是的有线电连接，也可以是无线通讯电连接。姿态解算模块、GPS模块和主控模块均设置在一个控制箱中，并固定在救生圈上，同时密封箱上设有对应的防水结构。电机马达与主控模块电连接，并固定在机械臂2远离救生圈一侧的端部，通过加大距离，方便调整救生圈运动。

采用自组装式设计结构设计，可在原有国标式普通救生圈进行本智能救生系统的搭建，以较低的成本将传统救生圈升级为智能救生圈。将通过内置自编自动巡航算法，在实时自动巡航搜救模式下，对规定水域进行巡逻，包括“S”型路线或者螺线路线。同时，通过组装于救生圈上的陀螺仪模块采得水波纹数据，并对其进行波动检测，实时分析水波纹的波动幅度及频率。当分析水波纹的数据超过警戒阈值，系统自动从巡航模式切换至搜救模式，定位溺水者，控制电机马达自动驶向被救援方向。当系统检测到溺水者已搭载于救生圈上，将定位至距离最近的岸边点并自动返回。

在本实施例中，自动巡航的速度可以根据水域大小调整，一个救生系统能够覆盖全水域巡逻，但建议采用多系统划区域的巡逻方式，控制单个系统的管辖水域，以提高救援效率，防止因为距离所带来的时间差耽误最佳救援时机。

其中，救生圈1上设有一用于检测落水者是否抓住救生圈1的水压传感器。通过水压传感器来检测水压值，进而判断落水者是否抓住救生圈1，再通过主控模块驱动电机马达，带落水者返回最近的岸边，智能化程度高。

GPS模块获取救生圈的坐标和指定点坐标(可以为水域中心点，可以为原始原点，也可以是落水者的振动点)的差值通过PID算法转换为电机马达的转速。在确定落水者位置后，PID算法不断调整四个电机马达的转速，使整个智能救生系统能够沿着落水者位置前进，确保救援位置的准确性和高效性。救生圈坐标和指定点坐标的差值越大，通过PID算法转换为远离振动源位置的电机马达的转速越快。

参见图2所示，在本实施例中，机械臂2包括密封箱21、连接杆22和固定块23，陀螺仪模块和姿态解算模块均设置于密封箱21内，连接杆22一端与密封箱21连接，另一端与固定块23连接，固定块23抵接于救生圈1，固定块23通过一抱箍3与救生圈1固定。

具体的，密封箱21包括箱体211、密封圈212和箱盖213，箱体211内设有一容纳陀螺仪模块和姿态解算模块的凹槽2111，箱盖213通过密封圈212与箱体211紧密连接，箱体211侧边设有通孔2112。

连接杆22包括连接柱221、外支撑杆222、外支撑套223、第一轴承224、第二轴承225和锁销226，外支撑杆222通过第一轴承224套设于连接柱221上，外支撑套223套设于外支撑杆222上，并且外支撑套223通过第二轴承225与连接柱221活动连接。锁销226贯穿箱体211上的通孔2112以及连接柱221与箱体211固定。

固定块23包括第一固定块231、第二固定块232、第三轴承233和连接器234，连接器234一端内设有一容纳第三轴承233的连接器通孔，第一固定块231通过第三轴承233与第二固定块232固定，连接器234另一端与外支撑套223固定连接。第一固定块231和第二固定块232上均设有一个用于抱箍固定的固定块凹槽24。

机械臂2通过连接杆22，长度可调，确保陀螺仪模块的灵活性，精确检测位置波动，并保持与救生圈1相连，为波动检测提供多功能和准确的数据，能更好地适应波浪运动，灵敏地反馈波浪变化，为波动检测的准确率提供了保障。其长度可调节性能够很好地适应不同应用水域。

在本实施例中，机械臂2为四个，四个机械臂2周向均匀分布设置于救生圈1外圈上。通过四个机械臂2成轴对称形式均匀分布在救生圈1上，保证整个救生圈1能够平稳的落在水面上，同时也方便通过波动检测定位算法来确定落水者的位置。

为了实现24小时续航，救生圈1上设有多个太阳能发电系统，太阳能发电系统与陀螺仪模块、主控模块、GPS模块和电机马达电连接。通过太阳能充电，保证整个系统能量充足。利用太阳能这种绿色新能源，实现低能耗系统的构建，延长了续航时间，保证了自动巡航功能的可实施性。太阳能发电系统单独设置在另一个密封箱，也对其进行防水设置，保证整个系统在水域上不会造成进水短路，对系统造成破坏。

本实施例，项目代码可移植性高，适应多种使用环境，选用小体积电子硬件减轻系统重量，提高集成度，结构图纸完备，成本较低，适于量产。

实施例二

参见图1-3所示，一种基于GPS巡航与姿态解算的智能救生系统的控制方法，包括如下步骤：

A1、校准所有陀螺仪模块，并将整个智能救生系统置于指定水域；

A2、主控模块分析陀螺仪模块采集的水波纹数据是否超过阈值，若不是，进入步骤A3，否则，进入A4；

A3、进入巡航模式，主控模块控制电机马达按照给定转速通过巡航算法沿预设巡航路线在指定水域范围内巡航，返回步骤A2；

A4、进入搜救模式，姿态解算模块获取陀螺仪模块的俯仰角和翻滚角，并通过波动检测定位算法获取振动源位置；

A5、GPS模块获取救生圈的坐标和指定点坐标的差值通过PID算法转换为电机马达的转速，主控模块控制电机马达按照转换后的转速和预设搜救路线，到达落水者位置；

A6、若水压传感器检测到水压数据超出预设水压阈值，则主控模块进入返航模式，主控模块驱动电机马达带动救生圈返回最近岸边，否则返回步骤A4。

落水者位置的搜寻主要通过波动检测定位功能来实现，通过自编的波动检测定位算法和姿态解算模块实现该功能。在本实施例中，机械臂2为四个，且均匀分布设置在救生圈1上，那么在相邻机械臂上的姿态解算模块相隔90°，通过实时获取各个机械臂上的水波纹俯仰角和翻滚角，一共八个数据。使用主控模块的stm32开发板对姿态解算模块得到的数据进行卡尔曼滤波，傅立叶变换等处理。随后建立数学几何模型，通过自编算法判断落水者位置。最后根据所得数据，驱动智能救生圈快速驶向落水者位置，其间通过PID算法不断调整以确保驶向救援位置的准确率。当落水者抓住救生圈使水压传感器的数据超过设定阈值时，救生圈将触发进入返航任务，自动返回最近的岸边。

在步骤A5中，救生圈坐标和指定点坐标的差值越大，通过PID算法转换为远离振动源位置的电机马达的转速越快。保证救生圈能以最快速度到达落水者位置。

参见图4-6所示，在本实施例中，在步骤A4中，波动检测定位算法包括如下步骤：

S11、四个陀螺仪模块分别获取各自的俯仰角和翻滚角数据，并将数据传递至姿态解算模块。

S12、姿态解算模块获取数据，经卡尔曼滤波及傅里叶变换处理得到水波纹的幅度大小和频率f。卡尔曼滤波及傅里叶变换处理，在主控模块中进行处理，本实施例采用的主控模块为stm32开发版。

S13、以救生圈中心为原点，四个陀螺仪模块为坐标轴，建立直角坐标系，将平面振动源位置分为一、二、三、四象限四个区域，参见图5所示。根据频率与波长的关系公式，得到水波纹波长在X轴和Y轴向量，

其中，l_x为水波纹波长在X轴的向量，f_x为水波纹频率在X轴的分量，l_y为水波纹波长在Y轴的向量，f_y为水波纹频率在Y轴的分量。

S14、确定振动源象限，原始数据经过一系列处理后，先进行逻辑判断，主控模块判断四个水波纹幅度绝对值的大小，水波纹幅度绝对值最大的两个相邻陀螺仪所夹象限为振动源位置。

当确定振动源所在象限时，对不同象限建立不同数学模型，开始波动检测定位，计算波动中心位置。

参见图6所示，假设振动源在第二象限，与振动源较近的两个陀螺仪模块A、D的数据只用于确定振动源所在象限，另外两个稍远的陀螺仪B、C的数据被用来精准定位。本算法基于正弦定理。一个三角形中，各边和所对角的正弦之比相等，且该比值等于该三角形外接圆的直径(半径的2倍)长度。

S15、确定振动源距离和角度，水波纹幅度绝对值最大的两个相邻陀螺仪和振动源建立三角形，其三个内角分别为angNew1，angNew2，angNew3，对应的三条边分别为side1，side2，side3，

angNew1＝θ₁+45°

angNew2＝θ₂+45°

angNew3＝180°-angNew1-angNew2＝90°-θ₁-θ₂

side1＝sin(angNew1)×side3÷sin(angNew3)

side2＝sin(angNew2)×side3÷sin(angNew3)

其中，θ₁为第一陀螺仪与X轴夹角，θ₂为第二陀螺仪与Y轴夹角，l_x1为第一陀螺仪检测的水波纹波长在X轴的向量，l_y1为第一陀螺仪检测的水波纹波长在Y轴的向量，l_x2为第二陀螺仪检测的水波纹波长在X轴的向量，l_y2为第二陀螺仪检测的水波纹波长在Y轴的向量，d为第二陀螺仪到救生圈中心点距离。

最后，利用勾股定理与正弦定理，根据公式进一步计算，振动源距离原点的距离distance：

振动源的角度angle：

其他象限可用相似方法完成振动源定位。

为了实现自动巡航功能，本救生圈配备GPS模块，内置团队设计的自动巡航算法，持续进行自动巡航，巡逻水域每个角落。自动巡航算法有两套循线策略，分别通过“S”形循线及螺线循线，适应不同水域的需求。其中螺线循线能提高巡逻效率，这样避免了单侧巡逻的局限性，对水域的实时情况能有更及时的反馈。

自动巡航功能减少了专业救生人员的人力成本，使救生系统在偏僻水域以及夜间等少人经过的环境下能独立工作。为确保自动巡航的效率，本实施例意在保证巡逻覆盖率的同时，缩短完成单次区域巡逻的时间。

参见图7所示，在步骤A3中，巡航算法包括如下步骤：

S21、测量边界坐标，取点n个，记录每个点的坐标(x_n，y_n)，通过n个边界坐标点的坐标取均值，算出中心点坐标(x，y)，记迭代次数为1，其中

参见图7左侧附图所示，在本实施例中，测量边界坐标一般选取20个。

S22、将n个边界坐标点分别与中心点连线，以n个边界坐标点分别到中心点的距离为移动参数乘以设定的参数，算出新点的坐标(x′_n，y′_n)，迭代次数加一，参见图7右侧附图所示，其中，

k为向内移动的距离参数。

S23、将上一次产生新点的坐标分别与中心点连线，以上一次产生新点分别到中心点的距离为移动参数乘以设定的参数，算出下一个新点的坐标，并使迭代次数加一。

S24、重复步骤S23，直至达到预设的迭代次数。迭代次数就是一共向内移动的次数，一共产生了几圈数据坐标。通常来说，距离水域中心最近的一圈的点离水域中心在20～50厘米。

S25、将所有点按照顺序首尾相连，形成螺线巡航路线。

在本实施例中，巡航算法中的向内移动的距离参数k和迭代次数，都可以根据真实环境形状设定，以确保自动巡航的效果。

通过救生圈搭载GPS模块定位，用GPS获取的救生圈坐标与指定点(可以为原始原点，也可以是落水者的振动点)的差值通过PID算法控制，转化为固定在救生圈四周的电机马达的转速，实现转向和前进后退运动，使救生圈按照首尾相接的顺序依次经过所有点，实现多点多角度全面覆盖，减少因人眼视野盲区所造成的不及时救援，完成指定水域范围内的自动巡航。

在GPS自动巡航的过程中，进行实时陀螺仪模块数据分析，结合姿态解算模块，监测是否有需要救援生命体征。使用陀螺仪模块反馈的水波纹的大小及频率来定位落水者的位置，并通过反馈的参数实时调整马达的转速来快速营救落水者。其检测相较于已有技术更为精确，并且在不同环境下表现更稳定。该自动巡航救生系统减少从发现到救援的时间，减少了场景中对于特殊专业救生人员的需求，同时也解决了现有大多数产品遥控距离限制的问题。

本实施例采用的自动巡航算法效率高，稳定性强，适用于边缘规则水域(参见图8所示)，以及边缘不规则水域(参见图9所示)的自动巡视。

本发明的有益效果为：无需人工操控，避免了在人员溺水场景中对于特殊专业救生人员的需求，实现快速智能“无人救援”；智能型、自适应决策支持智能救生系统，适用于户外大型的，缺少专业人员监管的水域。以一种安全可靠，便利易操作，低成本的方式，实现静置救生圈的自主巡航及检测落水人员，减少溺水人员死亡率。

上述说明示出并描述了本发明的若干优选实施例，但如前所述，应当理解本发明并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述发明构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。

Claims (10)

1.基于GPS巡航与姿态解算的智能救生系统，其特征在于：包括救生圈(1)、设置于救生圈(1)上的多个机械臂(2)以及主控模块，

每个所述机械臂(2)内设有一与主控模块电连接的陀螺仪模块，用于采集水波纹数据，实时分析水波纹波动幅度及频率；

所述陀螺仪模块电连接一姿态解算模块，所述姿态解算模块与主控模块电连接，用于获取陀螺仪模块的俯仰角和翻滚角，并通过波动检测定位算法获取振动源位置；

所述救生圈(1)上设有一与主控模块电连接的GPS模块，所述主控模块通过GPS模块和巡航算法控制救生圈沿预设巡航路线或搜救路线运动；

所述救生圈(1)上设有多个电机马达，用于驱动救生圈进行运动。

2.根据权利要求1所述的基于GPS巡航与姿态解算的智能救生系统，其特征在于：所述救生圈(1)上设有一用于检测落水者是否抓住救生圈(1)的水压传感器。

3.根据权利要求2所述的基于GPS巡航与姿态解算的智能救生系统，其特征在于：所述GPS模块获取的救生圈坐标和指定点坐标的差值通过PID算法转换为电机马达的转速。

4.根据权利要求1所述的基于GPS巡航与姿态解算的智能救生系统，其特征在于：所述机械臂(2)包括密封箱(21)和连接杆(22)，所述陀螺仪模块设置于密封箱(21)内，所述连接杆(22)一端与密封箱(21)连接，另一端通过一抱箍(3)与救生圈(1)固定。

5.根据权利要求4所述的基于GPS巡航与姿态解算的智能救生系统，其特征在于：所述机械臂(2)为四个，四个所述机械臂(2)周向均匀分布设置于救生圈(1)外圈上。

6.根据权利要求1-5所述的基于GPS巡航与姿态解算的智能救生系统，其特征在于：所述救生圈(1)上设有多个太阳能发电系统，所述太阳能发电系统与陀螺仪模块、主控模块、GPS模块和电机马达电连接。

7.一种根据权利要求1-6任意一项所述的基于GPS巡航与姿态解算的智能救生系统的控制方法，其特征在于：包括如下步骤：

A1、校准所有陀螺仪模块，并将整个智能救生系统置于指定水域；

A2、主控模块分析陀螺仪模块采集的水波纹数据是否超过阈值，若不是，进入步骤A3，否则，进入A4；

A3、进入巡航模式，主控模块控制电机马达按照给定转速通过巡航算法沿预设巡航路线在指定水域范围内巡航，返回步骤A2；

A4、进入搜救模式，姿态解算模块获取陀螺仪模块的俯仰角和翻滚角，并通过波动检测定位算法获取振动源位置；

A5、GPS模块获取救生圈坐标和指定点坐标的差值通过PID算法转换为电机马达的转速，主控模块控制电机马达按照转换后的转速和预设搜救路线，到达落水者位置；

A6、若水压传感器检测到水压数据超出预设水压阈值，则主控模块进入返航模式，主控模块驱动电机马达带动救生圈返回最近岸边，否则返回步骤A4。

8.根据权利要求7所述的基于GPS巡航与姿态解算的智能救生系统的控制方法，其特征在于：在步骤A5中，救生圈坐标和指定点坐标的差值越大，电机马达的转速越快。

9.根据权利要求7或8所述的基于GPS巡航与姿态解算的智能救生系统的控制方法，其特征在于：在步骤A4中，所述波动检测定位算法包括如下步骤：

S11、四个陀螺仪模块分别获取各自的俯仰角和翻滚角数据，并将数据传递至姿态解算模块；

S12、姿态解算模块获取数据，经卡尔曼滤波及傅里叶变换处理得到水波纹的幅度大小和频率f；

S13、以救生圈中心为原点，四个陀螺仪模块为坐标轴，建立直角坐标系，根据频率与波长的关系公式，得到水波纹波长在X轴和Y轴向量，

其中，l_x为水波纹波长在X轴的向量，f_x为水波纹频率在X轴的分量，l_y为水波纹波长在Y轴的向量，f_y为水波纹频率在Y轴的分量；

S14、确定振动源象限，主控模块判断四个水波纹幅度绝对值的大小，水波纹幅度绝对值最大的两个相邻陀螺仪所夹象限为振动源位置；

S15、确定振动源距离和角度，水波纹幅度绝对值最大的两个相邻陀螺仪和振动源建立三角形，其三个内角分别为angNew1，angNew2，angNew3，对应的三条边分别为side1，side2，side3，

angNew1＝θ₁+45°

angNew2＝θ₂+45°

angNew3＝180°-angNew1-angNew2＝90°-θ₁-θ₂

side1＝sin(angNew1)×side3÷sin(angNew3)

side2＝sin(angNew2)×side3÷sin(angNew3)

其中，θ₁为第一陀螺仪与X轴夹角，θ₂为第二陀螺仪与Y轴夹角，l_x1为第一陀螺仪检测的水波纹波长在X轴的向量，l_y1为第一陀螺仪检测的水波纹波长在Y轴的向量，l_x2为第二陀螺仪检测的水波纹波长在X轴的向量，l_y2为第二陀螺仪检测的水波纹波长在Y轴的向量，d为第二陀螺仪到救生圈中心点距离，

振动源距离原点的距离distance：

振动源距离原点的角度angle：

10.根据权利要求7所述的基于GPS巡航与姿态解算的智能救生系统的控制方法，其特征在于：在步骤A3中，所述巡航算法包括如下步骤：

S21、测量边界坐标，取点n个，记录每个点的坐标(x_n，y_n)，通过n个边界坐标点的坐标取均值，算出中心点坐标(x，y)，记迭代次数为1，其中

S22、将n个边界坐标点分别与中心点连线，以n个边界坐标点分别到中心点的距离为移动参数乘以设定的参数，算出新点的坐标(x′_n，y′_n)，迭代次数加一，其中，

k为向内移动的距离参数；

S23、将上一次产生新点的坐标分别与中心点连线，以上一次产生新点分别到中心点的距离为移动参数乘以设定的参数，算出下一个新点的坐标，并使迭代次数加一；

S24、重复步骤S23，直至达到预设的迭代次数；

S25、将所有点按照顺序首尾相连，形成螺线巡航路线。

Images