6DOF 模块文档¶
所属工具箱：RflySim Model
简介¶
一句话描述：该模块集成了基于四元数的刚体六自由度运动模型与简易环境模型，用于计算无人载具飞行过程中的位置、姿态、速度等运动状态，提供标准化的总线输出。
该模块是RflySim Model工具链中实现无人系统动力学仿真的核心基础模块，主要面向各类固定翼、多旋翼等无人载具的动力学仿真开发场景。模块接收由力和力矩计算模块、碰撞摩擦模块、风干扰模块输出的合力力矩、碰撞干扰力力矩、环境风速输入，结合用户配置的载具质量、转动惯量、初始运动状态参数，基于四元数法求解刚体六自由度运动方程，同时通过简易环境模型完成站心NED坐标到大地LLA GPS坐标的转换，并基于WGS 84基准计算对应位置的地球重力加速度，最终将所有运动状态与环境参数封装为标准Bus输出。
在RflySim仿真工具链中，该模块计算得到的载姿位置运动状态会进一步传输给CopterSim，实现PX4飞控的硬件在环仿真，同时也会将状态信息发送给RflySim3D，完成飞行过程的三维可视化展示，支撑从动力学建模到闭环仿真、可视化验证的全流程开发。
端口说明¶
输入端口（Inputs）¶
| 端口名 | 数据类型 | 维度 | 说明 |
|---|---|---|---|
FM_body |
double |
1×6 |
机体坐标系下载具所受合力与合力矩拼接而成的向量，由力和力矩模块输出 |
fColi |
double |
1×6 |
机体坐标系下碰撞和摩擦产生的力和力矩拼接而成的向量，由碰撞模块和地面模块输出，无对应模块时输入全零向量即可 |
Wind |
double |
1×3 |
站心坐标系（常为NED）下的风速向量，由风干扰模块输出，无对应模块时输入全零向量即可 |
输出端口（Outputs）¶
| 端口名 | 数据类型 | 维度 | 说明 |
|---|---|---|---|
6DOF Bus |
Bus |
- | 封装了简易环境模型和刚体六自由度模型所有输出的Simulink Bus对象，包含位置、速度、姿态、角速度、GPS坐标等所有运动状态信息 |
参数配置（Parameters）¶
双击模块打开的 Mask 对话框中可配置以下参数：
| 参数名 | 类型 | 默认值 | 可选值/范围 | 说明 |
|---|---|---|---|---|
Init_PosE |
double |
[0 0 0] |
任意3维向量 | 站心坐标系（常为NED）下的载具初始位置，单位：米 |
Init_VelB |
double |
[0 0 0] |
任意3维向量 | 机体坐标系下的载具xyz速度初始向量，单位：m/s |
Init_AngEuler |
double |
[0 0 0] |
任意3维向量 | 机体坐标系下载具初始欧拉姿态角，单位：弧度（rad） |
Init_RateB |
double |
[0 0 0] |
任意3维向量 | 机体坐标系下的载具绕xyz轴的角速度初始向量，单位：rad/s |
Param_uavMass |
double |
1 |
>0 |
无人载具的总质量，单位：kg |
Param_uavJ |
double |
diag([0.01, 0.01, 0.02]) |
3×3对角矩阵，主对角线元素>0 | 载具的转动惯量矩阵，主对角线元素为机体系xyz三个主轴的转动惯量，单位：kg·m² |
Param_GPSLatLong |
double |
[30.533800, 114.358200] |
2维向量，纬度范围(-90, 90)，经度范围(-180, 180) |
地图GPS原点的纬度和经度，单位：度 |
Param_envAltitude |
double |
-16 |
<0 |
地图GPS原点的海拔高度，竖直向下为正，高于海平面填负值，单位：米 |
参数设置说明¶
Init_PosE¶
站心NED坐标系的x轴指向北，y轴指向东，z轴指向地心方向（向下），初始位置为相对于GPS原点的偏移量。
Init_AngEuler¶
默认采用Z-Y-X顺规的欧拉角（偏航-俯仰-横滚），对应载常用的yaw-pitch-roll姿态定义。
Param_uavJ¶
模块仅支持对角转动惯量矩阵，非对角惯性积通常在小型载具中可忽略，直接填写3×3对角矩阵即可。
Param_GPSLatLong¶
该参数定义了站心NED坐标系的原点经纬度，用于将NED下相对位置转换为LLA大地坐标。
Param_envAltitude¶
由于Simulink内置大气模型不支持海平面以下的输入，该参数要求必须为负值（海平面为0，高于海平面高度为负，符合向下为正的定义规则）。
模块特性（Block Characteristics）¶
| 特性项 | 值 |
|---|---|
| 支持的数据类型 | double、single |
| 直接馈通（Direct Feedthrough） | 否 |
| 采样时间 | 离散 |
| 代码生成支持 | 是 |
数据通信协议¶
本模块不涉及网络通信。
相关模块¶
| 模块名 | 说明 |
|---|---|
ForceAndMoment |
计算多旋翼无人机总拉力和力矩，输出机体坐标系下的合力与合力矩 |
WindDisturb |
生成指定特性的风干扰，输出NED坐标系下的风速向量 |
GroundInteract |
模拟地面碰撞与摩擦效应，输出机体坐标系下碰撞摩擦产生的力和力矩 |
Visualization |
根据6DOF输出的状态信息实现无人机仿真的可视化展示 |
注意事项与常见问题¶
- 初始化顺序：该模块为RflySim仿真的核心动力学模块，在整个仿真模型中必须位于所有依赖其输出的模块（如控制器、传感器模块）上游，保证仿真迭代时先更新载具动力学状态，再计算其他模块输出，避免状态延迟导致仿真发散。
- 参数合法性校验：
Param_envAltitude参数必须设置为负值，该参数定义高于海平面为负，若设置为非负值会导致Simulink内置大气模型报错，引发仿真终止。转动惯量矩阵Param_uavJ必须为对角矩阵，非对角元素需保持为0，不符合格式的输入会导致六自由度动力学计算错误。 - 坐标系一致性：本模块采用NED站心坐标系和机体坐标系定义，所有输入输出遵循该约定，接入自定义模块时需要保证坐标系转换正确，避免因坐标系不匹配出现姿态计算发散、位置偏移错误。
- 输入补零要求：若模型中未添加碰撞摩擦模块和风干扰模块，对应
fColi和Wind输入端口必须接全零向量，不可悬空，否则会导致动力学积分计算出现未定义错误。 - 采样时间匹配：该模块默认使用固定步长离散积分，建议仿真步长设置不大于1ms，同时保证模块采样时间与仿真固定步长一致，过大的采样时间会导致六自由度积分误差累积，引发仿真不稳定。
- GPS坐标换算说明：输出的LLA坐标是基于输入的GPS原点参数经逆投影变换得到，若不需要GPS定位输出，也不可随意修改
Param_GPSLatLong参数，否则会导致重力加速度计算错误，影响动力学仿真结果。
更新日志¶
v4.1.0(2024-07-11): 初始版本发布，提供基于四元数的刚体六自由度运动模型与简易环境模型，支持初始状态配置、惯量参数设置、GPS坐标换算和重力加速度计算功能。