#!/usr/bin/env python3
# -*- coding: UTF-8 -*-
import rospy
import numpy as np
from geometry_msgs.msg import WrenchStamped
from nav_msgs.msg import Odometry
class TutorialNMPCController:
def __init__(self):
super(TutorialNMPCController, self).__init__()
# NMPC控制器参数初始化
self._prediction_horizon = 0
self._control_horizon = 0
self._dt = 0.0
# 权重矩阵
self._Q = np.zeros(shape=(6, 6)) # 状态误差权重
self._R = np.zeros(shape=(6, 6)) # 控制输入权重
self._P = np.zeros(shape=(6, 6)) # 终端状态权重
# 约束参数
self._u_min = np.zeros(shape=(6,)) # 控制输入下限
self._u_max = np.zeros(shape=(6,)) # 控制输入上限
self._x_min = np.zeros(shape=(6,)) # 状态下限
self._x_max = np.zeros(shape=(6,)) # 状态上限
# 其他参数
self._solver_options = {} # 求解器选项
self._warm_start = False # 是否使用热启动
# 控制器状态
self._prev_control = np.zeros(shape=(6,))
self._predicted_states = None
self._predicted_controls = None
self._solver_status = None
self._solve_time = 0.0
self._warm_start_solution = None
# 系统状态
self._is_init = False
self.odom_is_init = False
self.current_odom = None
# ROS相关初始化
self._control_pub = rospy.Publisher('thruster_output', WrenchStamped, queue_size=10)
self._odom_sub = rospy.Subscriber('odom', Odometry, self._odom_callback)
# 从ROS参数服务器加载参数
self._load_parameters()
# 验证关键参数
if self._control_horizon <= 0:
rospy.logwarn(f"Invalid control_horizon: {self._control_horizon}, setting to default (5)")
self._control_horizon = 5
self._is_init = True
rospy.loginfo(f"NMPC Controller initialized with control horizon: {self._control_horizon}")
rospy.loginfo("NMPC Controller initialized successfully")
def _odom_callback(self, msg):
"""里程计消息回调函数"""
self.current_odom = msg
self.odom_is_init = True
# 提取位置和姿态信息
self.current_position = np.array([
msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z
])
# 四元数转欧拉角 (简化处理，实际应用需使用完整转换)
self.current_orientation = np.array([0, 0, 0]) # 示例，需完善
rospy.logdebug("Received odometry message")
def _load_parameters(self):
"""从ROS参数服务器加载NMPC控制器参数"""
if rospy.has_param('~prediction_horizon'):
self._prediction_horizon = rospy.get_param('~prediction_horizon')
rospy.loginfo(f'Prediction Horizon = {self._prediction_horizon}')
else:
rospy.logwarn('Parameter prediction_horizon not found, using default')
self._prediction_horizon = 10
if rospy.has_param('~control_horizon'):
self._control_horizon = rospy.get_param('~control_horizon')
rospy.loginfo(f'Control Horizon = {self._control_horizon}')
else:
rospy.logwarn('Parameter control_horizon not found, using default')
self._control_horizon = 5
if rospy.has_param('~dt'):
self._dt = rospy.get_param('~dt')
rospy.loginfo(f'Sampling Time = {self._dt}')
else:
rospy.logwarn('Parameter dt not found, using default')
self._dt = 0.1
# 加载权重矩阵
# ... 其他参数加载代码保持不变 ...
def _reset_controller(self):
"""重置NMPC控制器状态"""
# 重置逻辑保持不变...
def update_controller(self):
"""更新NMPC控制器并发布控制指令"""
if not self._is_init:
rospy.logwarn("Controller not initialized, skipping update")
return False
if not self.odom_is_init:
rospy.logwarn("Odometry not received yet, skipping update")
return False
# 获取当前状态和参考轨迹
current_state = self._get_current_state()
reference_trajectory = self._get_reference_trajectory()
# 构建NMPC优化问题
problem = self._construct_optimization_problem(current_state, reference_trajectory)
# 求解优化问题
solution, status, solve_time = self._solve_optimization_problem(problem)
# 检查求解状态
if status != 'optimal':
rospy.logwarn(f"NMPC solver did not converge: {status}")
if self._prev_control is not None:
# 使用上一次控制输入作为备份
control_input = self._prev_control
else:
# 如果没有上一次控制输入，则使用零控制
control_input = np.zeros(shape=(6,))
else:
# 提取最优控制输入
control_input = self._extract_control_input(solution)
self._prev_control = control_input # 保存当前控制输入供下次使用
# 打印控制输入用于调试
rospy.loginfo(f"Control input: {control_input}")
# 发布控制指令
self.publish_control_wrench(control_input)
# 保存求解信息用于调试
self._solver_status = status
self._solve_time = solve_time
return True
def _get_current_state(self):
"""获取当前系统状态"""
if self.current_odom is None:
rospy.logerr("No odometry data available")
return np.zeros(6)
# 从里程计消息中提取状态 [x, y, z, vx, vy, vz]
state = np.array([
self.current_odom.pose.pose.position.x,
self.current_odom.pose.pose.position.y,
self.current_odom.pose.pose.position.z,
self.current_odom.twist.twist.linear.x,
self.current_odom.twist.twist.linear.y,
self.current_odom.twist.twist.linear.z
])
return state
def _get_reference_trajectory(self):
"""获取参考轨迹"""
# 示例：生成一个简单的参考轨迹 (保持当前位置)
current_state = self._get_current_state()
ref_traj = np.tile(current_state, (self._prediction_horizon + 1, 1))
# 添加一个小的目标偏移 (测试用)
ref_traj[:, 0] += 1.0 # x方向偏移1米
return ref_traj
def _construct_optimization_problem(self, current_state, reference_trajectory):
"""构建NMPC优化问题 (需根据具体求解器实现)"""
# 实际应用中需替换为真实优化问题构建逻辑
# 这里使用简化示例，返回一个模拟问题
return {"current_state": current_state, "reference": reference_trajectory}
def _solve_optimization_problem(self, problem):
"""求解NMPC优化问题"""
# 添加调试日志
rospy.loginfo(f"Solving optimization problem with control horizon: {self._control_horizon}")
# 确保 control_horizon 至少为1
control_horizon = max(1, self._control_horizon)
# 返回固定控制输入 (x方向100推力)
solution = np.tile(np.array([100, 0, 0, 0, 0, 0]), (control_horizon, 1))
rospy.loginfo(f"Solution shape: {solution.shape}")
status = 'optimal'
solve_time = 0.01
return solution, status, solve_time
def _extract_control_input(self, solution):
"""从优化解中提取控制输入"""
if solution is None or solution.size == 0:
rospy.logerr("Received empty solution, using default control")
return np.array([100, 0, 0, 0, 0, 0]) # 返回默认控制
# 提取第一个控制输入
return solution[0]
def publish_control_wrench(self, control_input):
"""发布控制指令"""
msg = WrenchStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'base_link'
msg.wrench.force.x = control_input[0]
msg.wrench.force.y = control_input[1]
msg.wrench.force.z = control_input[2]
msg.wrench.torque.x = control_input[3]
msg.wrench.torque.y = control_input[4]
msg.wrench.torque.z = control_input[5]
self._control_pub.publish(msg)
rospy.logdebug(f"Published control wrench: {control_input}")
if __name__ == '__main__':
print('Tutorial - NMPC Controller')
rospy.init_node('tutorial_nmpc_controller')
try:
node = TutorialNMPCController()
# 设置更新频率
rate = rospy.Rate(10000) # 10Hz
while not rospy.is_shutdown():
node.update_controller()
rate.sleep()
except rospy.ROSInterruptException:
print('caught exception')
print('exiting') 更改这段代码，只保留def __init__(self):、def _reset_controller(self):、def update_controller(self):
、if __name__ == '__main__':、和nmpc定义的模型和问题求解部分
ros_control软件包提供了一套控制接口、控制器管理、硬件接口和传动工具，适用于多种机器人，重写pr2机器人包。它通过控制循环回调机制，采用PID控制器进行输出控制。此外，该包支持多种可扩展的控制器，并详细定义了硬件接口和传动概念，确保功率在变换过程中的恒定。
最低0.47元/天 解锁文章
扫一扫
3451
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
