✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、EI、SCI写作与指导毕业论文、期刊论文经验交流。✅ 专业定制毕设、代码✅如需沟通交流可以私信或者点击《获取方式》1基于二阶贝塞尔曲线的平滑靠泊路径规划与卡尔曼滤波状态观测器根据码头泊位环境和船舶运动约束提出先跟踪靠泊路径至泊位外、后平行靠泊的两段式方案。靠泊路径采用二阶贝塞尔曲线生成控制点选择泊位外侧正横位置、转向点和泊位内部中点保证曲率连续平滑。路径曲率半径最小值控制在2倍船长以上以避免急转弯。同时利用卡尔曼滤波器设计船舶状态观测器从GPS和惯导数据中滤除一阶波浪力引起的高频振荡。状态向量包含低频位置、速度及高频分量观测器通过截止频率0.05Hz分离低频运动。海试数据表明滤波后位置噪声标准差由0.8m降至0.12m提高了控制精度。2模型预测控制与推力分配二次规划的实现船舶三自由度运动模型考虑风浪流环境力线性化后作为MPC预测模型。MPC采用预测时域40s控制时域15s权重矩阵Q和R通过逆向优化设计调定。在每个控制周期求解带约束二次规划约束包括舵角限幅±35°、推进器推力限幅±80kN以及安全边界约束。推力分配问题转化为标准二次规划目标函数兼顾推力误差最小、能耗最小和避免奇异结构。采用有效集法实时求解经优化后总推力误差1%。MATLAB仿真验证在6级风干扰下MPC控制器船舶轨迹偏离计划航线误差小于0.3m而串级PID误差为1.2m。推力分配满足全回转推进器角度变化率约束无推力饱和。3实船试验与毫米波雷达泊位探测系统中的网格密度降噪算法依托海豚1试验船在青岛码头进行自主靠离泊试验。开发泊位探测软件接入毫米波雷达提出网格密度与RCS双重降噪法将雷达点云投影到水平面建立0.5m×0.5m网格统计每个网格内点数作为密度滤除密度低于5的网格同时根据RCS阈值滤除弱小目标保留码头轮廓。再利用直线特征提取算法识别泊位边缘直线。试验过程中泊位探测系统成功识别5m宽的泊位探测距离80m。MPC控制器接收到泊位信息后船舶平稳靠泊横向接触速度仅为0.08m/s小于规范要求的0.15m/s。全程无人操控验证了算法的工程实用性。import numpy as np from scipy.linalg import block_diag import cvxpy as cp import control # 贝塞尔曲线路径生成 def bezier_curve(p0, p1, p2, num_points100): t np.linspace(0,1,num_points) path np.outer((1-t)**2, p0) np.outer(2*(1-t)*t, p1) np.outer(t**2, p2) return path # 卡尔曼滤波器状态观测器 class KalmanObserver: def __init__(self, A, B, C, Q, R): self.A A; self.B B; self.C C; self.Q Q; self.R R self.x_est np.zeros(A.shape[0]); self.P np.eye(A.shape[0]) def update(self, u, y, dt): A_d np.eye(len(self.x_est)) self.A*dt # 预测 self.x_est A_dself.x_est self.B*dt*u self.P A_dself.PA_d.T self.Q*dt # 更新 K self.Pself.C.T np.linalg.inv(self.Cself.Pself.C.T self.R) self.x_est K (y - self.Cself.x_est) self.P (np.eye(len(self.x_est))-Kself.C) self.P return self.x_est # MPC控制器 def mpc_ship_control(x0, ref_traj, A, B, Np40, Nc15): # 构建QP x cp.Variable((3, Np1)) u cp.Variable((2, Nc)) cost 0; constraints [x[:,0] x0] for k in range(Np): cost cp.sum_squares(x[:,k]-ref_traj[:,k]) if k Nc: cost 0.01*cp.sum_squares(u[:,k]) constraints [x[:,k1] Ax[:,k] Bu[:,k]] else: constraints [x[:,k1] Ax[:,k]] constraints [cp.abs(u) [80e3, np.deg2rad(35)]] prob cp.Problem(cp.Minimize(cost), constraints) prob.solve(solverOSQP) return u[:,0].value # 网格密度降噪 def grid_density_filter(points, grid_size0.5, min_points5): min_x, min_y points.min(axis0); max_x, max_y points.max(axis0) grid {} for p in points: i int((p[0]-min_x)//grid_size); j int((p[1]-min_y)//grid_size) grid[(i,j)] grid.get((i,j),0)1 filtered [p for p in points if grid.get((int((p[0]-min_x)//grid_size), int((p[1]-min_y)//grid_size)),0) min_points] return np.array(filtered)
海豚1船舶自主靠离泊MPC控制算法与试验【附程序】
✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、EI、SCI写作与指导毕业论文、期刊论文经验交流。✅ 专业定制毕设、代码✅如需沟通交流可以私信或者点击《获取方式》1基于二阶贝塞尔曲线的平滑靠泊路径规划与卡尔曼滤波状态观测器根据码头泊位环境和船舶运动约束提出先跟踪靠泊路径至泊位外、后平行靠泊的两段式方案。靠泊路径采用二阶贝塞尔曲线生成控制点选择泊位外侧正横位置、转向点和泊位内部中点保证曲率连续平滑。路径曲率半径最小值控制在2倍船长以上以避免急转弯。同时利用卡尔曼滤波器设计船舶状态观测器从GPS和惯导数据中滤除一阶波浪力引起的高频振荡。状态向量包含低频位置、速度及高频分量观测器通过截止频率0.05Hz分离低频运动。海试数据表明滤波后位置噪声标准差由0.8m降至0.12m提高了控制精度。2模型预测控制与推力分配二次规划的实现船舶三自由度运动模型考虑风浪流环境力线性化后作为MPC预测模型。MPC采用预测时域40s控制时域15s权重矩阵Q和R通过逆向优化设计调定。在每个控制周期求解带约束二次规划约束包括舵角限幅±35°、推进器推力限幅±80kN以及安全边界约束。推力分配问题转化为标准二次规划目标函数兼顾推力误差最小、能耗最小和避免奇异结构。采用有效集法实时求解经优化后总推力误差1%。MATLAB仿真验证在6级风干扰下MPC控制器船舶轨迹偏离计划航线误差小于0.3m而串级PID误差为1.2m。推力分配满足全回转推进器角度变化率约束无推力饱和。3实船试验与毫米波雷达泊位探测系统中的网格密度降噪算法依托海豚1试验船在青岛码头进行自主靠离泊试验。开发泊位探测软件接入毫米波雷达提出网格密度与RCS双重降噪法将雷达点云投影到水平面建立0.5m×0.5m网格统计每个网格内点数作为密度滤除密度低于5的网格同时根据RCS阈值滤除弱小目标保留码头轮廓。再利用直线特征提取算法识别泊位边缘直线。试验过程中泊位探测系统成功识别5m宽的泊位探测距离80m。MPC控制器接收到泊位信息后船舶平稳靠泊横向接触速度仅为0.08m/s小于规范要求的0.15m/s。全程无人操控验证了算法的工程实用性。import numpy as np from scipy.linalg import block_diag import cvxpy as cp import control # 贝塞尔曲线路径生成 def bezier_curve(p0, p1, p2, num_points100): t np.linspace(0,1,num_points) path np.outer((1-t)**2, p0) np.outer(2*(1-t)*t, p1) np.outer(t**2, p2) return path # 卡尔曼滤波器状态观测器 class KalmanObserver: def __init__(self, A, B, C, Q, R): self.A A; self.B B; self.C C; self.Q Q; self.R R self.x_est np.zeros(A.shape[0]); self.P np.eye(A.shape[0]) def update(self, u, y, dt): A_d np.eye(len(self.x_est)) self.A*dt # 预测 self.x_est A_dself.x_est self.B*dt*u self.P A_dself.PA_d.T self.Q*dt # 更新 K self.Pself.C.T np.linalg.inv(self.Cself.Pself.C.T self.R) self.x_est K (y - self.Cself.x_est) self.P (np.eye(len(self.x_est))-Kself.C) self.P return self.x_est # MPC控制器 def mpc_ship_control(x0, ref_traj, A, B, Np40, Nc15): # 构建QP x cp.Variable((3, Np1)) u cp.Variable((2, Nc)) cost 0; constraints [x[:,0] x0] for k in range(Np): cost cp.sum_squares(x[:,k]-ref_traj[:,k]) if k Nc: cost 0.01*cp.sum_squares(u[:,k]) constraints [x[:,k1] Ax[:,k] Bu[:,k]] else: constraints [x[:,k1] Ax[:,k]] constraints [cp.abs(u) [80e3, np.deg2rad(35)]] prob cp.Problem(cp.Minimize(cost), constraints) prob.solve(solverOSQP) return u[:,0].value # 网格密度降噪 def grid_density_filter(points, grid_size0.5, min_points5): min_x, min_y points.min(axis0); max_x, max_y points.max(axis0) grid {} for p in points: i int((p[0]-min_x)//grid_size); j int((p[1]-min_y)//grid_size) grid[(i,j)] grid.get((i,j),0)1 filtered [p for p in points if grid.get((int((p[0]-min_x)//grid_size), int((p[1]-min_y)//grid_size)),0) min_points] return np.array(filtered)
