✅博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 如需沟通交流，扫描文章底部二维码。
（1）双自由度串级模糊自适应深度与航向控制：
对可下潜式无人船建立水平面三自由度和垂直面一自由度耦合的运动模型，推力分配矩阵由四个推进器构建。深度控制采用串级PID结构，外环为深度环输出纵倾角指令，内环为纵倾角速度环；航向控制采用同样结构。为增强参数自整定能力，在深度外环引入模糊逻辑，以深度误差和误差变化率为输入，动态调整外环比例系数Kp和微分系数Kd，内环保持固定。模糊论域设定为归一化范围[-6,6]，隶属函数采用三角形，解模糊采用重心法。仿真在水池扰动环境中进行，目标深度1.5米时，最大深度偏差稳定在±0.08米以内，较固定PID的±0.21米显著缩小。
（2）改进势场与Informed-RRT*融合全局局部路径规划：
全局路径规划器采用Informed-RRT*，在椭圆采样域内搜索出渐进最优的无碰路径，并将路径关键点作为局部吸引序列。局部规划器则在改进的人工势场框架下工作，斥力函数分段设计，当越靠近障碍物斥力呈指数增长，且引入随机逃逸力解决局部极小值困境。同时，利用势场法生成的合力方向引导目标偏差扩展，使RRT*采样偏重于势场下降方向。在包含动态障碍的仿真环境中，无人船能够以0.8m/s航速在3秒内重新规划路径成功避障，总航程较纯全局规划缩短约11%。
（3）基于Gazebo的数字孪生验证与嵌入式部署：
在ROS Melodic环境下，利用Gazebo构建包含水动力模型和传感器噪声的虚拟无人船，建立数字孪生仿真平台。将运动控制和路径规划节点移植到Jetson Nano嵌入式系统，并利用rosserial与底层BeagleBone Blue进行串口通信。水池试验中，无人船从岸基点出发，自主下潜至1.8米深度，航行至50米外的检测点，上浮回收，全程实际轨迹与规划轨迹的水平偏移平均为0.12米，深度跟随良好，验证了系统方案的工程可行性。
import rospy from geometry_msgs.msg import Twist from nav_msgs.msg import Odometry import numpy as np import skfuzzy as fuzz from skfuzzy import control as ctrl # 模糊PID深度控制器 class FuzzyDepthPID: def __init__(self): self.error = ctrl.Antecedent(np.arange(-1, 1, 0.1), 'error') self.derror = ctrl.Antecedent(np.arange(-0.5, 0.5, 0.05), 'derror') self.kp_out = ctrl.Consequent(np.arange(0, 2, 0.1), 'kp') self.kd_out = ctrl.Consequent(np.arange(0, 1, 0.05), 'kd') # 隶属函数 self.error['N'] = fuzz.trimf(self.error.universe, [-1, -0.5, 0]) self.error['Z'] = fuzz.trimf(self.error.universe, [-0.2, 0, 0.2]) self.error['P'] = fuzz.trimf(self.error.universe, [0, 0.5, 1]) # 规则 rule1 = ctrl.Rule(self.error['N'] & self.derror['N'], (self.kp_out, 1.5), (self.kd_out, 0.3)) rule2 = ctrl.Rule(self.error['Z'] & self.derror['Z'], (self.kp_out, 0.8), (self.kd_out, 0.5)) rule3 = ctrl.Rule(self.error['P'] & self.derror['P'], (self.kp_out, 1.5), (self.kd_out, 0.3)) self.ctrl_sys = ctrl.ControlSystem([rule1, rule2, rule3]) self.sim = ctrl.ControlSystemSimulation(self.ctrl_sys) def compute(self, err, derr): self.sim.input['error'] = err self.sim.input['derror'] = derr self.sim.compute() return self.sim.output['kp'], self.sim.output['kd'] # 改进人工势场局部规划 def improved_artificial_potential_field(pos, goal, obstacles, K_att=1.0, K_rep=100, d0=2.0): att_force = K_att * (goal - pos) rep_force = np.zeros(2) for obs in obstacles: dist = np.linalg.norm(pos - obs) if dist < d0: rep_force += K_rep * (1.0/dist - 1.0/d0) * (1.0/dist**2) * (pos - obs) / dist # 随机逃逸力防止局部极小 if np.linalg.norm(rep_force) < 0.1 and np.linalg.norm(att_force) > 0.5: rep_force += np.random.randn(2) * 0.5 return att_force + rep_force # ROS节点（伪代码） class USVNode: def __init__(self): rospy.init_node('usv_control') self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) self.odom_sub = rospy.Subscriber('/odom', Odometry, self.callback) def callback(self, odom): # 获取当前位置与目标，调用模糊PID和势场规划 pass如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇
