本代码是当时水下机器人做的一个项目，实测代码可以用。如有不足之处，请与作者联系。