无人艇路径规划、避碰、路径跟踪控制(vrep仿真，含ros通信，rrt*，COLREGS，los，pid差速控制)

风铃777

已于 2022-02-22 15:30:17 修改

阅读量2.2w

点赞数 60

分类专栏： ROS Ubuntu 文章标签： r语言 vr 自动驾驶

于 2021-01-01 20:26:15 首次发布

本文链接：https://blog.csdn.net/weixin_40820983/article/details/109176031

版权

ROS 同时被 2 个专栏收录

5 篇文章

订阅专栏

Ubuntu

3 篇文章

订阅专栏

2021/05/26 增加rrt*-smart局部路径规划（动态避障，遵守COLREGS避碰规则）

关键词：rrt*-smart，dcpa，tcpa，COLREGS，船舶安全领域

这里先展示结果，具体实现方法等我写完论文再发。（图中绿色线条是rrt*-smart随机树，蓝色线条是障碍船的行驶轨迹，红线是无人艇规划路径。）

最后赠送几张海边的美景

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某不知名环境）

跟踪结果：

（黑色为航向，init_vel 、max 、min 为pid中的基准推力、限幅推力，不是真实的速度，真实的船速从vrep传感器获得）

参数：P=10,I=1.2,init_vel=5.0,len_usv=1.25,line_length=50.0,N=4,min_speed=-10.0,max_speed=10.0 稳定状态速度约1m/s

高速状态较好参数
参数：init_vel=40.0_max=80.0_min=-80.0_P=15.0_I=3.0

快捷获取物体信息的方式（vrep中的lua脚本）：

--初始化一个multiarray的话题  
pub_lv=simROS.advertise('/info','std_msgs/Float32MultiArray')
simROS.publisherTreatUInt8ArrayAsString(pub_lv)

--获取信息
 pos1=sim.getObjectPosition(Boat,-1)  
--xyz坐标
 l_v1,a_v=sim.getObjectVelocity(Boat)
--线速度，角速度
angle1=sim.getObjectOrientation(Boat,-1)[3]
--方位角 （-pi到pi）

--将信息打包成一维数组，lua下标从1开始
data_all={}
data_all[1]=angle1
data_all[2]=pos1[1]
data_all[3]=pos1[2]
...

--发布话题
simROS.publish(pub_lv, {data=data_all})

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

坐标系：

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

2、船的航向角，从x轴正方向逆时针为正顺时针为负，范围为（-pi,pi）。

ROS信息传输：

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

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