基于ROS2，撰写python脚本，根据给定的舵-桨动力学模型实现动力学更新

news2025/12/15 5:15:44

提问

#! /usr/bin/env python3

from control_planner import usvParam as P
from control_planner.courseController import courseLimitation

import tf_transformations  # ROS2没有自带tf.transformations, 需装第三方库
import rclpy
from rclpy.node import Node
from pid_interfaces.msg import Command

import numpy as np 
import random
import tf2_ros
import time
from geometry_msgs.msg import PointStamped, Vector3Stamped


class usvDynamics:
    def __init__(self, listener, alpha=0):
        # Initial state conditions
        self.state = np.array([
            [P.theta0],      # initial angle
            [P.thetadot0], # initial angular rate
            [P.vel_u0], #initial velocity on x direction
            [P.vel_v0]
        ])  

        # Mass of the arm, kg
        self.m = P.m * (1.+alpha*(2.*np.random.rand()-1.))
        # Damping coefficient, Ns
        self.b = P.b * (1.+alpha*(2.*np.random.rand()-1.))  
        self.b_f = P.b_f * (1.+alpha*(2.*np.random.rand()-1.))  
        self.b_r = P.b_r * (1.+alpha*(2.*np.random.rand()-1.)) 

        # the gravity constant is well known, so we don't change it.
        self.g = P.g

        # sample rate at which the dynamics are propagated
        self.Ts = P.Ts  
        self.torque_limit = P.tau_max

        #转动惯量
        self.I_z = P.I_z

        #propeller to center 
        self.r = P.r

        self.angular_v = 0.0

        self.listener = listener
    def add_wind(self, data):
        try:
            # 1. 查找最新的transform，target='base_link', source='NED'
            # 注意：ROS2的tf2中，lookup_transform(target, source, time)
            # 返回geometry_msgs.msg.TransformStamped对象
            trans = self.listener.lookup_transform(
                'base_link', 'NED', rclpy.time.Time())

            # 2. 获取平移和四元数
            trans_vec = [
                trans.transform.translation.x,
                trans.transform.translation.y,
                trans.transform.translation.z
            ]
            quat = [
                trans.transform.rotation.x,
                trans.transform.rotation.y,
                trans.transform.rotation.z,
                trans.transform.rotation.w
            ]

            # 3. wind_force_map（4维） → 旋转，只取XYZ分量（不要加第四个0）
            wind_force_map = [data[0], data[1], 0]

            # 4. 使用tf_transformations进行四元数旋转
            v_rotated = tf_transformations.quaternion_matrix(quat)[:3, :3].dot(wind_force_map)

            # 5. 累加平移，获得boat系下的力
            wind_force_boat = [
                v_rotated[0] + trans_vec[0],
                v_rotated[1] + trans_vec[1],
                v_rotated[2] + trans_vec[2]
            ]

            return wind_force_boat
        except Exception as e:
            print(f"Exception1:{e}")
            return [0, 0, 0]

    def update(self, T_u, wind_u,angular_v):
        self.angular_v = angular_v
        # This is the external method that takes the input u at time
        # t and returns the output n at time t.
        # saturate the input torque
        # T_u = self.saturate(T_u, self.torque_limit) #T_u is a list after editing
        
        self.rk4_step(T_u,wind_u)  # propagate the state by one time sample
        psi,psi_dot,vel_u,vel_v = self.h()  # return the corresponding output
        #print("thetadot is:%f"%self.state.item(1))
        #print("vel_v is:%f"%vel_v)
        #print("vel_u is:%f"%vel_u)
        # print(f"self.state:\n{self.state}")
        return psi,psi_dot,vel_u,vel_v

    def f(self, state, T_u,wind_u):
        # Return edot = f(e,u), the system state update equations
        # re-label states for readability
        wind_force_boat = self.add_wind(wind_u)
        # wind_force_boat = [0,0]
        #计算力矩
        T_l = T_u[0]
        T_r = T_u[1]

        theta = state.item(0)
        thetadot = state.item(1)
        vel_u = state.item(2)
        vel_v = state.item(3)
        #print("vel_v is: %f"%vel_v)
        # calculate wind disturbance
        # wind_psi= wind_u[0]
        # wind_force = wind_u[1]
        # wind_force_x = wind_force * np.cos(np.abs(wind_psi-theta)*np.pi/180)
        # wind_force_y = wind_force * np.sin(np.abs(wind_psi-theta)*np.pi/180)
        
        #calculate angle acceleration
        tau = (T_l-T_r)*self.r - self.b * thetadot
        thetaddot = tau/self.I_z + 0.1*vel_u*vel_v   #m_33 rdot = T_r - d_33 r
        
        #calculate acceleration on X&Y direction
        force_X = (T_r+T_l) - self.b_f * vel_u + wind_force_boat[0] #wind_force_x
        force_Y =  - self.b_f * vel_v - wind_force_boat[1] #- self.b_r * thetadot  wind_force_y
        # print(f"wind_force_boat:{wind_force_boat}")
        vel_udot = force_X / self.m + 0.01 *vel_v*thetadot
        #print("vel_udot is: %f"%vel_udot)
        vel_vdot = force_Y / self.m - 0.01 *vel_u*thetadot
        
        edot = np.array([[thetadot],
                         [thetaddot],
                         [vel_udot],
                         [vel_vdot]
                         ])

        
        return edot

    def h(self):
        # return the output equations
        # could also use input u if needed
        theta = self.state.item(0)
        #n = np.array([[theta]])
        psi = courseLimitation(theta)
        psi_dot = self.state.item(1)
        vel_u = self.state.item(2)
        vel_v = self.state.item(3)
        
        return psi,psi_dot/10,vel_u,vel_v

    def rk4_step(self, T_u, wind_u):
        # Integrate ODE using Runge-Kutta RK4 algorithm
        F1 = self.f(self.state, T_u, wind_u)
        F2 = self.f(self.state + self.Ts / 2 * F1, T_u,wind_u)
        F3 = self.f(self.state + self.Ts / 2 * F2, T_u,wind_u)
        F4 = self.f(self.state + self.Ts * F3, T_u,wind_u)
        self.state = self.state + self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)
        # self.state = np.array([[self.state.item(0)],[self.angular_v],[self.state.item(2)],[self.state.item(3)]])

    def saturate(self, u, limit):
        for i in range(len(u)):
            T = u[i]
            if abs(T) > limit:
                T = limit*np.sign(T)
                u[i] = T
        return u

上面的python代码是模拟一个双螺旋桨差速驱动的水面机器人的动力学模型，现在，我想将动力学方程改为由舵-浆分离的解耦的机器人的动力学模型（即舵控制转向力矩，浆控制推进新速度），动力学方程为：
改进的Nomoto模型（速度相关）：

T ( V ) d ψ d t + ψ = K ( V ) δ T(V) \frac{d \psi}{d t}+\psi=K(V) \delta T(V)dtdψ​+ψ=K(V)δ

  • K ( V ) ∝ V K(V) \propto V K(V)∝V 或 V 2 V^2 V2
  • T ( V ) T(V) T(V) 可能随速度增加而减小（响应更快）

要求你在现有代码的基础上，进行修改（即大框架不变，只将反映动力学方程本身的那部分机制的代码进行修改即可，参数可以随便指定一个）

注意：add_wind代表的是动力学模型中引入风这个环境干扰，暂时你不用对其进行任何修改。

回答

你提供的现有模型是基于左右推进器差速产生扭矩和推进力的动力学，而你希望将其替换为 “舵-浆解耦模型”，即使用浆推进速度、舵角控制转向的方式，并使用改进Nomoto模型建模转向动力学。

✅ 你的目标模型：

T ( V ) d ψ d t + ψ = K ( V ) δ T(V) \frac{d\psi}{dt} + \psi = K(V)\delta T(V)dtdψ​+ψ=K(V)δ

这是一个一阶系统描述船体偏航角动态：

  • $\psi$：船体航向角
  • $\delta$：舵角输入
  • $V$：前进速度（vel_u）
  • $K(V) = K_0 \cdot V$ 或 $K_0 \cdot V^2$
  • $T(V) = T_0 / (1 + cV)$（表示速度越快，转向响应更快）

🔧 需要修改的地方：

主要修改以下函数中的动力学建模逻辑：

  1. f(self, state, T_u, wind_u)
  2. h(self)
  3. 去除对左右螺旋桨的扭矩建模
  4. 添加舵角控制变量

⚙️ 修改说明和替代变量：

  • T_u 参数将简化为 [thrust_force, rudder_angle]
  • self.state 变成： ψ , ψ ˙ , v e l u , v e l v \psi, \dot{\psi}, vel_u, vel_v ψ,ψ˙​,velu​,velv​
  • 保留 add_wind 不变

我将下一步在 PCI 中修改你的 usvDynamics 类动力学部分，并保留风扰动模型。

我已经根据改进Nomoto模型完成了usvDynamics类的核心改写。以下是关键修改点总结：

🔄 修改内容：

✅ 状态变量未变（4维）：
  • $ ψ（航向角）, dψ/dt（角速度）, vel_u（前进速度）, vel_v（横向速度）$
✅ 输入变为 [thrust_force, rudder_angle]：
  • thrust_force: 推进力，控制速度（前进）
  • rudder_angle: 舵角，控制航向
✅ 航向动力学采用 Nomoto 模型：

d 2 ψ d t 2 = K ( V ) δ − ψ T ( V ) \frac{d^2\psi}{dt^2} = \frac{K(V) \delta - \psi}{T(V)} dt2d2ψ​=T(V)K(V)δ−ψ​

  • K(V) = K_0 * V
  • T(V) = T_0 / (1 + T_{decay} * V)

🧪 示例参数：

  • K_0 = 0.5
  • T_0 = 5.0
  • K_power = 1（线性随速度变化）
  • T_decay = 0.3

✅ 保留：

  • add_wind 结构不变
  • RK4 时间推进方法不变
  • 接口方法如 update()、h() 完全兼容原框架

是否需要我将此代码与你原始文件合并输出、或者将该模型类导出为独立模块供你使用？

继续提问

将此代码与我的原始文件合并输出

回答

#! /usr/bin/env python3

from control_planner import usvParam as P
from control_planner.courseController import courseLimitation

import tf_transformations  # ROS2没有自带tf.transformations, 需装第三方库
import rclpy
from rclpy.node import Node
from pid_interfaces.msg import Command

import numpy as np 
import random
import tf2_ros
import time
from geometry_msgs.msg import PointStamped, Vector3Stamped


class usvDynamics:
    def __init__(self, listener, alpha=0):
        # Initial state conditions
        self.state = np.array([
            [P.theta0],      # initial heading angle (psi)
            [P.thetadot0],   # initial heading rate (dpsi/dt)
            [P.vel_u0],      # initial surge velocity
            [P.vel_v0]       # initial sway velocity
        ])  

        self.m = P.m * (1. + alpha * (2. * np.random.rand() - 1.))
        self.b_f = P.b_f * (1. + alpha * (2. * np.random.rand() - 1.))  
        self.b_r = P.b_r * (1. + alpha * (2. * np.random.rand() - 1.)) 
        self.g = P.g
        self.Ts = P.Ts

        # Parameters for modified Nomoto model
        self.K_0 = 0.5  # gain constant
        self.T_0 = 5.0  # base time constant
        self.K_power = 1  # linear dependence
        self.T_decay = 0.3  # time constant decay with speed

        self.listener = listener

    def add_wind(self, data):
        try:
            trans = self.listener.lookup_transform(
                'base_link', 'NED', rclpy.time.Time())

            trans_vec = [
                trans.transform.translation.x,
                trans.transform.translation.y,
                trans.transform.translation.z
            ]
            quat = [
                trans.transform.rotation.x,
                trans.transform.rotation.y,
                trans.transform.rotation.z,
                trans.transform.rotation.w
            ]
            wind_force_map = [data[0], data[1], 0]
            v_rotated = tf_transformations.quaternion_matrix(quat)[:3, :3].dot(wind_force_map)

            wind_force_boat = [
                v_rotated[0] + trans_vec[0],
                v_rotated[1] + trans_vec[1],
                v_rotated[2] + trans_vec[2]
            ]

            return wind_force_boat
        except Exception as e:
            print(f"Exception1:{e}")
            return [0, 0, 0]

    def update(self, T_u, wind_u, angular_v=0.0):
        self.rk4_step(T_u, wind_u)
        return self.h()

    def f(self, state, T_u, wind_u):
        wind_force_boat = self.add_wind(wind_u)

        psi = state.item(0)
        dpsi = state.item(1)
        vel_u = state.item(2)
        vel_v = state.item(3)

        thrust_force = T_u[0]   # propulsion
        rudder_angle = T_u[1]   # steering

        # Nomoto model dynamics
        K_v = self.K_0 * vel_u**self.K_power
        T_v = self.T_0 / (1 + self.T_decay * vel_u)
        ddpsi = (K_v * rudder_angle - psi) / T_v

        force_X = thrust_force - self.b_f * vel_u + wind_force_boat[0]
        force_Y = - self.b_r * vel_v - wind_force_boat[1]

        vel_udot = force_X / self.m
        vel_vdot = force_Y / self.m

        return np.array([
            [dpsi],
            [ddpsi],
            [vel_udot],
            [vel_vdot]
        ])

    def h(self):
        theta = self.state.item(0)
        psi = courseLimitation(theta)
        psi_dot = self.state.item(1)
        vel_u = self.state.item(2)
        vel_v = self.state.item(3)
        return psi, psi_dot, vel_u, vel_v

    def rk4_step(self, T_u, wind_u):
        F1 = self.f(self.state, T_u, wind_u)
        F2 = self.f(self.state + self.Ts / 2 * F1, T_u, wind_u)
        F3 = self.f(self.state + self.Ts / 2 * F2, T_u, wind_u)
        F4 = self.f(self.state + self.Ts * F3, T_u, wind_u)
        self.state = self.state + self.Ts / 6 * (F1 + 2 * F2 + 2 * F3 + F4)

    def saturate(self, u, limit):
        for i in range(len(u)):
            T = u[i]
            if abs(T) > limit:
                T = limit * np.sign(T)
                u[i] = T
        return u

我已经将你的原始代码与新的基于“舵-浆解耦”模型的Nomoto动力学融合，保留了原有结构和风扰动模型，并替换了动力学核心。你现在可以在usvDynamics类中使用如下输入控制：

T_u = [thrust_force, rudder_angle]

是否需要我帮你：

  • 编写单元测试？
  • 接入 ROS2 节点发布订阅逻辑？
  • 可视化船体状态（如 heading/velocity 曲线）？

请告诉我下一步需求。

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/2404553.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

Scrapy爬虫教程（新手）

Scrapy爬虫教程（新手）

1. Scrapy的核心组成 引擎（engine）：scrapy的核心，所有模块的衔接，数据流程梳理。 调度器（scheduler）：本质可以看成一个集合和队列，里面存放着一堆即将要发送的请求&#…
阅读更多...
在Windows下编译出llama_cpp_python的DLL后，在虚拟环境中使用方法

在Windows下编译出llama_cpp_python的DLL后，在虚拟环境中使用方法

定位编译生成的文件 在VS2022编译完成后，在构建目录（如build/Release或build/Debug）中寻找以下关键文件： ggml.dll、ggml_base.dll、ggml_cpu.dll、ggml_cuda.dll、llama.dll（核心动态链接库） llama_cp…
阅读更多...
关于datetime获取时间的问题

关于datetime获取时间的问题

import datetime print(datetime.now())如果用上述代码，会报错： 以下才是正确代码： from datetime import datetime print(datetime.now()) 结果： 如果想格式化时间，使用代码： from datetime import da…
阅读更多...
Unity版本使用情况统计（更新至2025年5月）

Unity版本使用情况统计（更新至2025年5月）

UWA发布｜本期UWA发布的内容是Unity版本使用统计（第十六期），统计周期为2024年11月至2025年5月，数据来源于UWA网站（www.uwa4d.com）性能诊断提测的项目。希望给Unity开发者提供相关的行业趋势作为参…
阅读更多...
GPUCUDA 发展编年史：从 3D 渲染到 AI 大模型时代（上）

GPUCUDA 发展编年史：从 3D 渲染到 AI 大模型时代（上）

目录 文章目录 目录1960s~1999：GPU 的诞生：光栅化（Rasterization）3D 渲染算法的硬件化实现之路 学术界算法研究历程工业界产品研发历程光栅化技术原理光栅化技术的软件实现：OpenGL 3D 渲染管线设计 1. 顶点处理&…
阅读更多...
人机融合智能 | 可穿戴计算设备的多模态交互

人机融合智能 | 可穿戴计算设备的多模态交互

可穿戴计算设备可以对人体以及周围环境进行连续感知和计算,为用户提供随时随地的智能交互服务。本章主要介绍人机智能交互领域中可穿戴计算设备的多模态交互,阐述以人为中心的智能穿戴交互设计目标和原则,为可穿戴技术和智能穿戴交互技术的设计提供指导,进而简述支持智能穿戴交…
阅读更多...
Impromptu VLA：用于驾驶视觉-语言-动作模型的开放权重和开放数据

Impromptu VLA：用于驾驶视觉-语言-动作模型的开放权重和开放数据

25年5月来自清华和博世的论文“Impromptu VLA: Open Weights and Open Data for Driving Vision-Language-Action Models”。 用于自动驾驶的“视觉-语言-动作” (VLA) 模型前景光明，但在非结构化极端场景下却表现不佳，这主要是由于缺乏有针对性的基准测…
阅读更多...
AI智能体，为美业后端供应链注入“智慧因子”(4/6)

AI智能体，为美业后端供应链注入“智慧因子”(4/6)

摘要：本文深入剖析美业后端供应链现状，其产品具有多样性、更新换代快等特点，原料供应和生产环节也面临诸多挑战。AI 智能体的登场为美业后端供应链带来变革，包括精准需求预测、智能化库存管理、优化生产计划排程、升级供应商管理等…
阅读更多...
跨平台资源下载工具：res-downloader 的使用体验

跨平台资源下载工具：res-downloader 的使用体验

一款基于 Go Wails 的跨平台资源下载工具，简洁易用，支持多种资源嗅探与下载。res-downloader 一款开源免费的下载软件(开源无毒、放心使用)！支持Win10、Win11、Mac系统.支持视频、音频、图片、m3u8等网络资源下载.支持视频号、小程序、抖音、…
阅读更多...
数据湖是什么？数据湖和数据仓库的区别是什么？

数据湖是什么？数据湖和数据仓库的区别是什么？

目录 一、数据湖是什么 （一）数据湖的定义 （二）数据湖的特点 二、数据仓库是什么 （一）数据仓库的定义 （二）数据仓库的特点 三、数据湖和数据仓库的区别 （一&#…
阅读更多...
【深度学习新浪潮】如何入门三维重建？

【深度学习新浪潮】如何入门三维重建？

入门三维重建算法技术需要结合数学基础、计算机视觉理论、编程实践和项目经验，以下是系统的学习路径和建议： 一、基础知识储备 1. 数学基础 线性代数：矩阵运算、向量空间、特征分解（用于相机矩阵、变换矩阵推导）。几何基础：三维几何（点、线、面的表示）、射影几何（单…
阅读更多...
Codeforces Round 1025 (Div. 2) B. Slice to Survive

Codeforces Round 1025 (Div. 2) B. Slice to Survive

Codeforces Round 1025 (Div. 2) B. Slice to Survive 题目 Duelists Mouf and Fouad enter the arena, which is an n m n \times m nm grid! Fouad’s monster starts at cell ( a , b ) (a, b) (a,b), where rows are numbered 1 1 1 to n n n and columns 1 1 1 t…
阅读更多...
ubuntu中使用docker

ubuntu中使用docker

上一篇我已经下载了一个ubuntu:20.04的镜像； 1. 查看所有镜像 sudo docker images 2. 基于本地存在的ubuntu:20.04镜像创建一个容器，容器的名为cppubuntu-1。创建的时候就会启动容器。 sudo docker run -itd --name cppubuntu-1 ubuntu:20.04 结果出…
阅读更多...
[ElasticSearch] DSL查询

[ElasticSearch] DSL查询

🌸个人主页:https://blog.csdn.net/2301_80050796?spm1000.2115.3001.5343 🏵️热门专栏: 🧊 Java基本语法(97平均质量分)https://blog.csdn.net/2301_80050796/category_12615970.html?spm1001.2014.3001.5482 🍕 Collection与…
阅读更多...
iview中的table组件点击一行中的任意一点选中本行

iview中的table组件点击一行中的任意一点选中本行

<Table border ref"selection" size"small" on-row-click"onClickRow"></Table>// table组件点击一行任意位置选中onClickRow(row, index) {this.$refs.selection.toggleSelect(index)}写上toggleSelect(index)方法即可&#xff0c;…
阅读更多...
《探秘跨网段局域网IP广播：解锁网络通信的新姿势》

《探秘跨网段局域网IP广播：解锁网络通信的新姿势》

一、从基础出发：广播与跨网段 在计算机网络的世界中，广播域是一个至关重要的概念。简单来说，广播域是指网络中能接收任一台主机发出的广播帧的所有主机集合。当一台主机在广播域内发出一个广播帧时，同一广播域内的所有其他主机都可以收到该广播帧。在没有路由器或 VLAN 分割…
阅读更多...
maven微服务${revision}依赖打包无法识别

maven微服务${revision}依赖打包无法识别

1、场景描述 我现在又一个微服务项目&#xff0c;父pom的版本&#xff0c;使用<properties>定义好&#xff0c;如下所示&#xff1a; <name>ypsx-finance-center</name> <artifactId>ypsx-finance</artifactId> <packaging>pom</pack…
阅读更多...
2025年06月07日Github流行趋势

2025年06月07日Github流行趋势

项目名称&#xff1a;netbird 项目地址url&#xff1a;https://github.com/netbirdio/netbird项目语言&#xff1a;Go历史star数&#xff1a;14824今日star数&#xff1a;320项目维护者&#xff1a;mlsmaycon, braginini, pascal-fischer, lixmal, pappz项目简介&#xff1a;使…
阅读更多...
WPS中将在线链接转为图片

WPS中将在线链接转为图片

WPS中将在线链接转为图片 文章目录 WPS中将在线链接转为图片一&#xff1a;解决方案1、下载图片&#xff0c;精确匹配&#xff08;会员功能&#xff09;2、将在线链接直接转为图片 一&#xff1a;解决方案 1、下载图片&#xff0c;精确匹配&#xff08;会员功能&#xff09; …
阅读更多...
实战二：开发网页端界面完成黑白视频转为彩色视频

实战二：开发网页端界面完成黑白视频转为彩色视频

​一、需求描述 设计一个简单的视频上色应用&#xff0c;用户可以通过网页界面上传黑白视频&#xff0c;系统会自动将其转换为彩色视频。整个过程对用户来说非常简单直观&#xff0c;不需要了解技术细节。 效果图 ​二、实现思路 总体思路&#xff1a; 用户通过Gradio界面上…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023