无人艇路径跟踪（vrep仿真和实艇控制，含ros通信、gps、imu，基于模糊控制、pid差速、los等方法）

占个坑，本月持续更新

2020\11\16 更新：vrep仿真，基于ros\gps\简单pid

仿真环境：vrep最新版本（CoppeliaSim）、ros-kinetic、Ubuntu16.04、Python 2.7，仿真时记得先打开ros，再打开vrep，这样会自动加载vrep_ros interface插件。

两推进器船、有波浪的水面、目标为树的位置、ros发送目标位置和船当前gps位置，接收推进器力的大小（改编自github某不知名环境）

控制量：两推进器的力的大小，正数，基于差速模型的pid控制

坐标系：

1、仿真环境中有一个世界坐标系，所有位置信息都是基于这个世界坐标系的。例如gps发送的位置不是经纬度，而是在环境中的(x,y,z)。

2、船的航向角，从x轴正方向顺时针为正逆时针为负（这里记不太清了），范围为（-1,1）。但是在某个方向好像有奇怪的跳变，这里问题还没有解决，不过没有影响到仿真。

imu订阅话题回调函数

ROS信息传输：

1、先打开ros再打开vrep，自动加载ros插件后，执行命令 rosnode list会发现有一个 sim_ros_interface的节点，这样我们在vrep环境中发送的消息都是基于这个节点。

2、在vrep界面中，双击每个名字后面的小文档（child_script），会出现该物体的lua执行代码。vrep端发送的话题主要有 gps_data、imu_data、left_motor 、right_motor、goal等，主要在 Tree 、Boat、GPS 这三个物体的文档里。

双击小文档图标

3、在vrep中使用ros，参考了vrep自带的ros例程。发布话题：初始化话题（simRos.advertise），发布话题信息(simros.publish)，shutdown。订阅话题：初始化话题（simRos.subscribe），回调函数（自定义call_back）

Tree的文档

无人艇的仿真文档。m0 m1分别是左右推进器力的大小，初始化为0；可以看到发布和订阅的话题。

usv_ros.py 代码解析：

1、初始化ros话题、目标位置、pid参数等

2、订阅话题的回调函数（按理说控制程序最好放在回调函数里，这样每次接收到数据就实时发送控制量。但是这是最初的仿真程序，后来实际控制的时候我改了。）

3、计算无人艇当前航向与目标航向的角度差。求目标航向角时用到了向量的夹角公式，向量x为x轴正方向单位向量，向量y为船到目标点。x.*y=|x||y|cos(sita)，注意arccos求出来范围是0-pi，不能判断向量是顺时针还是逆时针方向，所以需要额外进行符号判断。另外，由于角度差可能会大于pi或小于-pi，也许呀进行规范。