一种水下机器人路径跟踪方法、设备及存储设备

摘要

本发明提供了一种水下机器人路径跟踪方法、设备及存储设备，所述方法包括步骤：设定水下机器人期望跟踪的路径；建立水下机器人的坐标系；以所述坐标系建立水下机器人电机布局方式；建立水下机器人的控制系统模型；设定所述控制系统模型中的传递函数及电机模型；进行坐标变换，建立世界坐标系和水下机器人坐标系的联系；完成水下机器人的路径跟踪。一种水下机器人路径跟踪设备及存储设备，用来实现所述方法。本发明在水下机器人受到水流扰动的情况下可以高效实时地对期望路径进行跟踪前进。

权利要求

1.一种水下机器人路径跟踪方法，所述方法由硬件设备实现，其特征在于：包括以下步骤：设定水下机器人期望跟踪的路径；建立水下机器人的坐标系；以所述坐标系建立水下机器人电机布局方式；建立水下机器人的控制系统模型；设定所述控制系统模型中的传递函数及电机模型；进行坐标变换，建立世界坐标系和水下机器人坐标系的联系；完成水下机器人的路径跟踪；

所述水下机器人坐标系包括：移动坐标系，具体为：为x轴、y轴和z轴；旋转坐标系，具体为：l轴、m轴和n轴；

以所述坐标系建立水下机器人电机布局方式具体为：所述电机具体数量为8台电机，记为：M1、M2、M3、M4、M5、M6、M7及M8；所述8台电机的功能具体为：M1及M4用于控制l轴的稳定，M2及M3用于控制m轴的稳定，M5及M6用于控制n轴的稳定，M7及M8用于控制x轴的稳定，M5及M6用于控制y轴的稳定，M1、M2、M3及M4用于控制z轴的稳定；上述电机布局不存在耦合关系，故所述x轴、y轴、z轴、l轴、m轴及n轴的稳定性为分开控制；

所述水下机器人的控制系统模型包括：控制器、电机、水下机器人本体、运动干扰及测量误差；所述设定水下机器人期望跟踪的路径还包括：将路径转化成离散点；

所述完成水下机器人的路径跟踪控制具体步骤包括：向水下机器人发送一个离散点；

水下机器人接受到所述一个离散点后，向该离散点移动；设定水下机器人与离散点的距离阈值；判断水下机器人与离散点的距离是否小于所述距离阈值；若不小于，则水下机器人继续接近离散点，并返回判断距离阈值的步骤继续执行；若小于，则判断离散点是否为最后一离散点；若是，则完成路径跟踪；若不是，则向水下机器人再发送一个新离散点；水下机器人向新离散点接近并回到对距离阈值进行判断的步骤继续执行；

所述传递函数包括：水下机器人路径跟踪传递函数及控制器传递函数；所述水下机器人路径跟踪传递函数为：其中，M为水下机器人的质量，k′为水下机器人在水中执行平动的阻尼系数，T′(t)为水下机器人在水中受到的外力；所述控制器传递函数为：时域输出表达式为：

其中，Kp为比例系数，KI为积分系数，KD为微分系数，所述电机模型具体为：直流电机占空比-输出拉力模型及直流电机占空比-输出推力模型；所述电机模型采用回归拟合方式得到，基本公式为：其中，A,B及C为拟合参数；

所述进行坐标变换，建立世界坐标系和水下机器人坐标系的联系具体为：

其中，xp及yp为机器人坐标系，x及y为世界坐标系，(a，b)为机器人坐标系原点在世界坐标系中的坐标，θ为旋转角。

2.如权利要求1所述的一种水下机器人路径跟踪方法，其特征在于：所述直流电机占空比-输出拉力模型经过拟合具体为：所述直流电机占空比-输出推力模型具体为：

3.存储设备，其特征包括：所述存储设备存储指令及数据用于实现权利要求1～2所述的任意一种方法。

4.一种水下机器人路径跟踪设备，其特征在于：包括：处理器及权利要求3中所述存储设备；所述处理器加载并执行权利要求3中所述存储设备中的指令及数据用于实现权利要求1～2所述的任意一种方法。

说明书

一种水下机器人路径跟踪方法、设备及存储设备

技术领域

[0001] 本发明涉及自动控制技术领域，具体涉及一种水下机器人路径跟踪方法、设备及存储设备。

背景技术

[0002] 水下机器人需要实现自动搜寻的工作，其工作方式为：当客户端给水下机器人规划好一条搜寻路径后，水下机器人沿着已规划好的搜寻路径前进。因此，水下机器人如何能够依据一条已知路径进行追踪前进，就成为业界关注的问题。

发明内容

[0003] 本发明提供了一种水下机器人路径跟踪方法、设备及存储设备，在水下机器人受到扰动的情况下可以高效实时地对期望路径进行跟踪，从而有效解决上述技术问题。

[0004] 本发明提供的技术方案是：一种水下机器人路径跟踪方法，所述方法包括步骤：设定水下机器人期望跟踪的路径；建立水下机器人的坐标系；以所述坐标系建立水下机器人电机布局方式；建立水下机器人的控制系统模型；设定所述控制系统模型中的传递函数及电机模型；进行坐标变换，建立世界坐标系和水下机器人坐标系的联系；完成水下机器人的路径跟踪。存储设备，所述存储设备存储指令及数据用于实现所述一种水下机器人路径跟踪方法。一种水下机器人路径跟踪设备，所述设备包括处理器及所述存储设备；所述处理器加载并执行所述存储设备中的指令及数据用于实现所述的一种水下机器人路径跟踪方法。

[0005] 本发明的有益效果是：本发明提供了一种水下机器人路径跟踪方法、设备及存储设备，通过解耦式布局水下机器人电机获得水下机器人各个轴的解耦控制方式，然后通过回归拟合方式得到电机模型，再通过设定好的各种传递函数确定各部位输入输出关系，最后通过坐标变换将世界坐标系与机器人坐标系建立关系，从而在水下机器人受到水流扰动的情况下可以高效实时地沿期望路径前进。

附图说明

[0006] 图1是本发明实施例中水下机器人路径跟踪方法的整体流程图；

[0007] 图2是本发明实施例中水下机器人坐标系及电机布局示意图；

[0008] 图3是本发明实施例中水下机器人路径跟踪系统模型示意图；

[0009] 图4是本发明实施例中电机占空比与力拟合示意图；

[0010] 图5是本发明实施例中电机占空比与电机输出力拟合曲线示意图；

[0011] 图6是本发明实施例中坐标变换示意图；

[0012] 图7是本发明实施例中水下机器人路径跟踪控制具体步骤流程图；

[0013] 图8是本发明实施例中微小水流干扰下水下机器人的路径跟踪效果示意图；

[0014] 图9是本发明实施例中中等水流干扰下水下机器人的路径跟踪效果示意图；

[0015] 图10是本发明实施例中强水流干扰下水下机器人的路径跟踪效果示意图；

[0016] 图11是本发明实施例的硬件设备工作示意图。

具体实施方式

[0017] 为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地描述，下文中提到的具体技术细节，如：方法，设备等，仅为使读者更好的理解技术方案，并不代表本发明仅局限于以下技术细节。

[0018] 本发明的实施例提供了一种水下机器人路径跟踪方法、设备及存储设备。请参阅图1，图1是本发明实施例中水下机器人路径跟踪方法的整体流程图，所述方法由硬件设备实现，具体步骤包括：

[0019] S101：设定水下机器人期望跟踪的路径，将路径转化成离散点。

[0020] S102：建立水下机器人的坐标系，包括：移动坐标系，具体为：为x轴、y轴和z轴；旋转坐标系，具体为：l轴、m轴和n轴。

[0021] S103：以所述坐标系建立水下机器人电机布局方式，具体为：所述电机具体数量为8台电机，记为：M1、M2、M3、M4、M5、M6、M7及M8；所述8台电机的功能具体为：M1及M4用于控制l轴的稳定，M2及M3用于控制m轴的稳定，M5及M6用于控制n轴的稳定，M7及M8用于控制x轴的稳定，M5及M6用于控制y轴的稳定，M1、M2、M3及M4用于控制z轴的稳定；上述电机布局不存在耦合关系，故所述x轴、y轴、z轴、l轴、m轴及n轴的稳定性为分开控制。

[0022] S104：建立水下机器人的控制系统模型，所述水下机器人的控制系统模型包括：控制器、电机、水下机器人本体、运动干扰及测量误差。

[0023] S105：设定所述控制系统模型中的传递函数及电机模型。所述传递函数包括：水下机器人路径跟踪传递函数及控制器传递函数；所述水下机器人路径跟踪传递函数为：

[0024]

[0025] 其中，M为水下机器人的质量，k′为水下机器人在水中执行平动的阻尼系数，T′(t)为水下机器人在水中受到的外力；所述控制器传递函数为：

[0026]

[0027] 时域输出表达式为：

[0028]

[0029] 其中，Kp为比例系数，KI为积分系数，KD为微分系数。所述电机模型具体为：直流电机占空比-输出拉力模型及直流电机占空比-输出推力模型；所述电机模型采用回归拟合方式得到，基本公式为：

[0030]

[0031] 其中，A,B及C为拟合参数。所述直流电机占空比-输出拉力模型经过拟合具体为：

[0032]

[0033] 所述直流电机占空比-输出推力模型具体为：

[0034]

[0035] S106：进行坐标变换，建立世界坐标系和水下机器人坐标系的联系。具体为：

[0036]

[0037] 其中，xp及yp为水下机器人坐标系，x及y为世界坐标系，(a，b)为水下机器人坐标系原点在世界坐标系中的坐标，θ为旋转角。

[0038] S107：完成水下机器人的路径跟踪。

[0039] 参见图2，图2是本发明实施例中水下机器人坐标系及电机布局示意图，包括：x轴、y轴、z轴、l轴、m轴、n轴、电机M1、电机M2、电机M3、电机M4、电机M5、电机M6、电机M7及电机M8。其中，x轴、y轴及z轴为运动方向轴，l轴、m轴及n轴为旋转方向轴，z轴及n轴的方向相同。电机M1及电机M4用于控制l轴的稳定，电机M2及电机M3用于控制m轴的稳定，电机M5及电机M6用于控制n轴的稳定，电机M7及电机M8用于控制x轴的稳定，电机M5及电机M6用于控制y轴的稳定，电机M1、电机M2、电机M3及电机M4用于控制z轴的稳定；上述电机布局不存在耦合关系，故所述x轴、y轴、z轴、l轴、m轴及n轴的稳定性为分开控制。

[0040] 参见图3，图3是本发明实施例中水下机器人路径跟踪系统模型示意图，包括：预期的角度或位置R(s)、PWM波的占空比P(s)、控制器Gc(s)、电机Gd(s)、电机输出的力或力矩F(s)、运动干扰Td(s)、机器人G1(s)、实际位置或角度θ(s)、测量误差N(s)及测量到的位置或角度。由图中可见，所述控制系统模型采用的是PID控制模型。

[0041] 参见图4，图4是本发明实施例中电机占空比与力拟合示意图，包括：电机产生的拉力401、水槽402、水下电机403、线404及拉力计405。图中显示的是电机占空比与电机输出拉力进行拟合的实验系统，对于电机占空比与电机输出推力进行拟合的实验系统，其他装置不变，只需要将电机旋转180°安装即可，在此不再赘述。

[0042] 参见图5，图5是本发明实施例中电机占空比与电机输出力拟合曲线示意图，包括：电机占空比与电机输出拉力关系点501、电机占空比与电机输出拉力拟合曲线502、电机占空比与电机输出推力关系点503及电机占空比与电机输出拉力拟合曲线504。图中各点的具体数据见表1及表2，表1为电机占空比与电机输出拉力关系表，表2为电机占空比与电机输出推力关系表。

[0043] 表1

[0044]

[0045] 表2

[0046]

[0047] 表3给出了电机占空比与输出拉力的拟合性能，表4给出了电机占空比与输出推力的拟合性能。

[0048] 表3

[0049]

[0050] 表4

[0051]

[0052] 参见图6，图6是本发明实施例中坐标变换示意图，包括：世界坐标系601及水下机器人坐标系602，两者间的转化公式为：

[0053]

[0054] 其中，x1及y1为水下机器人坐标系602，x及y为世界坐标系601，x’及y’为旋转后的水下机器人坐标系，P(x，y)为点P在世界坐标系601中的坐标，xp及yp为点P在旋转后的水下机器人坐标系中的坐标，T(a，b)为水下机器人坐标系602的原点在世界坐标系601中的坐标，θ为旋转角。

[0055] 参见图7，图7是本发明实施例中水下机器人路径跟踪控制具体步骤流程图，包括：

[0056] S701：向水下机器人发送一个离散点。

[0057] S702：水下机器人接受到所述一个离散点后，向该离散点移动。当水下机器人接收到一个离散点的坐标后，水下机器人立刻计算离散点位置与水下机器人当前位置的距离以及水下机器人n轴方向与离散点的夹角。利用n轴位置控制器保证水下机器人的前进方向一直对准离散点，并将离散点位置与水下机器人间的距离作为输入传递给x轴位置控制器，其用于控制电机输出推力推动水下机器人前进。

[0058] S703：设定水下机器人与离散点的距离阈值。

[0059] S704：判断水下机器人与离散点的距离是否小于所述距离阈值。

[0060] S705：若不小于，则水下机器人继续接近离散点，并返回判断距离阈值的步骤继续执行。

[0061] S706：若小于，则判断离散点是否为最后一离散点。

[0062] S707：若是，则完成路径跟踪。

[0063] S708：若不是，则向水下机器人再发送一个新离散点。

[0064] S709：水下机器人向新离散点接近并回到对距离阈值进行判断的步骤继续执行。

[0065] 参见图8，图8是本发明实施例中微小水流干扰下水下机器人的路径跟踪效果示意图。由图中可以看出，在微小水流下，水下机器人能够保证跟踪路径的精度，具有较好的效果。

[0066] 参见图9，图9是本发明实施例中中等水流干扰下水下机器人的路径跟踪效果示意图。由图中可以看出，水下机器人在中等水流的干扰下，跟踪路径出现了一定的偏移，尤其在每个离散点附近，但机器人的实际跟踪路径没有偏离目标路径太远。当离散点变得更加密集时，这种偏离效果会大大减弱。可以得出结论，在中等水流下，机器人路径跟踪的效果依然不错。

[0067] 参见图10，图10是本发明实施例中强水流干扰下水下机器人的路径跟踪效果示意图。由图中可以看出，水下机器人在路径跟踪时实际路径与目标路径偏移较大，这种情况已经不适合用路径跟踪来搜寻物体。当水流过强时，只能通过增加离散点的密度来减小路径偏移量。

[0068] 参见图11，图11是本发明实施例的硬件设备工作示意图，所述硬件设备具体包括：一种水下机器人路径跟踪设备1101、处理器1102及存储设备1103。

[0069] 水下机器人路径跟踪设备1101：所述一种水下机器人路径跟踪设备1101实现所述一种水下机器人路径跟踪方法。

[0070] 处理器1102：所述处理器1102加载并执行所述存储设备1103中的指令及数据用于实现所述的一种水下机器人路径跟踪方法。

[0071] 存储设备1103：所述存储设备1103存储指令及数据；所述存储设备1103用于实现所述的一种水下机器人路径跟踪方法。

[0072] 通过执行本发明的实施例，本发明权利要求里的所有技术特征都得到了详尽阐述。

[0073] 区别于现有技术，本发明的实施例提供了一种水下机器人路径跟踪方法、设备及存储设备，通过解耦式布局水下机器人电机获得水下机器人各个轴的解耦控制方式，然后通过回归拟合方式得到电机模型，再通过设定好的各种传递函数确定各部位输入输出关系，最后通过坐标变换将世界坐标系与机器人坐标系建立关系，从而在水下机器人受到水流扰动的情况下可以高效实时地沿期望路径前进。

[0074] 以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。