| 无人艇路径规划、避碰、路径跟踪控制(vrep仿真，含ros通信，rrt*，COLREGS，los，pid差速控制) | 您所在的位置：网站首页 › 航迹规划定义 › 无人艇路径规划、避碰、路径跟踪控制(vrep仿真，含ros通信，rrt*，COLREGS，los，pid差速控制) |
无人艇路径规划、避碰、路径跟踪控制(vrep仿真，含ros通信，rrt*，COLREGS，los，pid差速控制)
|
2021/05/26 增加rrt*-smart局部路径规划（动态避障，遵守COLREGS避碰规则）
关键词：rrt*-smart，dcpa，tcpa，COLREGS，船舶安全领域 这里先展示结果，具体实现方法等我写完论文再发。（图中绿色线条是rrt*-smart随机树，蓝色线条是障碍船的行驶轨迹，红线是无人艇规划路径。） 
rrt*-smart 规划结果1
rrt*-smart 规划结果2
路径模拟行驶
实际行驶路径(实线为已经走过的路径，虚线为规划路径)
同上
最后赠送几张海边的美景
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)、激光雷达点云；订阅话题：左右电机的推力
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 下载链接：my_boat_1221.ttt-机器学习文档类资源-CSDN下载 注意：使用激光雷达的话，将链接https://github.com/edward0im/V-Rep-Autonomous-Vehicle-Simulator里的so文件放到vrep文件夹下。 2020\10\201、找到几个不错的仿真环境，但搭建过程可能会比较费时： 基于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、卡尔曼滤波 协方差矩阵kalman滤波理解三：协方差矩阵的计算_JK-CSDN博客_误差协方差矩阵 卡尔曼滤波中的噪声协方差矩阵（R和Q）应该怎么取值,和噪声分布之间的关系是什么？ - 知乎 公式推导：卡尔曼滤波算法详细推导_victor_zy的博客-CSDN博客_卡尔曼滤波推导 |
【本文地址】