无人艇航迹跟踪（vrep，ros，gps，imu，pid，差速，los,lidar）

美丽珠海 东澳岛

 

 

2020/12/7   实艇控制总体框架（代码就不传啦）

1、gps和激光雷达的数据接收、解析、处理、发布话题

2、 坐标系转换：LLA转ECEF，ECEF转ENU

3、根据LOS及障碍物位置计算目标航向

4、PID差速转向控制  及  原地转向

 

2020\11\16 更新：vrep仿真，ros\gps\imu\lidar

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

两推进器船、有波浪的水面（改编自github某不知名环境）

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

坐标系：

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

2、船的航向角，从x轴正方向顺时针为正逆时针为负（这里记不太清了），范围为（-pi,pi）。

 ROS信息传输：

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

发布话题有角速度、线速度、gps(x,y,航向)、激光雷达点云；订阅话题：左右电机的推力

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

 

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

 

usv_ros.py 代码解析：

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

2、订阅话题的回调函数

将四元数转化为欧拉角：

3、计算无人艇当前航向与目标航向的角度差。

向量夹角的计算

4、pid差速，基础速度base_vel

5、发送话题：左右电机推力 

5、主程序中需要初始化ros节点和vrep 连接id

vrep model 下载链接：https://download.csdn.net/download/weixin_40820983/14003396

2020\10\20 

1、找到几个不错的仿真环境，但搭建过程可能会比较费时：

基于java:https://github.com/He-Ze/Autonomous-Surface-Vehicle-Simulator

基于gazebo，试了两天各种报错不能运行:https://github.com/disaster-robotics-proalertas/usv_sim_lsa

一个无人艇比赛的官方仿真环境，基于gazebo ubuntu18.04 https://github.com/osrf/vrx

这个可运行，但是模型写的不对，船跑不了！！！https://github.com/OUXT-Polaris/ros_ship_packages

 

2、卡尔曼滤波

协方差矩阵https://blog.csdn.net/u011362822/article/details/95905113   https://www.zhihu.com/question/53788909

公式推导：https://blog.csdn.net/victor_zy/article/details/82862904?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.compare&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-2.compare