OUT_TERM

       在Verilog中,OUT_TERM 是一个与输出驱动强度终端匹配相关的约束,用于定义输出信号的驱动特性。

一、OUT_TERM 的作用

OUT_TERM 主要用于控制输出信号的:

  • 驱动强度

  • 压摆率

  • 输出阻抗匹配

  • 信号完整性

二、主要应用场景

1. FPGA输出引脚配置

// 在约束文件中的典型用法
NET "data_out" LOC = "B15";
NET "data_out" IOSTANDARD = "LVCMOS33";
NET "data_out" OUT_TERM = "NONE";           // 无终端
NET "data_out" SLEW = "SLOW";               // 慢压摆率

2. 不同的输出终端类型

// 常见的OUT_TERM值
NET "output1" OUT_TERM = "NONE";            // 无终端
NET "output2" OUT_TERM = "UNTUNED_50";      // 50欧姆未调
NET "output3" OUT_TERM = "SERIES_50";       // 50欧姆串联
NET "output4" OUT_TERM = "SERIES_60";       // 60欧姆串联

三、在Xilinx工具中的具体应用

1. UCF约束文件示例

NET "tx_data<0>" LOC = "P45" | IOSTANDARD = LVCMOS33 | OUT_TERM = NONE | SLEW = FAST;
NET "tx_data<1>" LOC = "P46" | IOSTANDARD = LVCMOS33 | OUT_TERM = NONE | SLEW = FAST;
NET "clk_out"    LOC = "P47" | IOSTANDARD = LVCMOS33 | OUT_TERM = SERIES_50 | SLEW = SLOW;

2. XDC约束文件示例

set_property PACKAGE_PIN B15 [get_ports data_out]
set_property IOSTANDARD LVCMOS33 [get_ports data_out]
set_property OUT_TERM NONE [get_ports data_out]
set_property SLEW FAST [get_ports data_out]
set_property DRIVE 12 [get_ports data_out]  # 驱动电流(mA)

四、常见的OUT_TERM值

描述适用场景
NONE无终端电阻普通输出,短距离传输
UNTUNED_5050欧姆未调终端通用阻抗匹配
SERIES_5050欧姆串联终端长线传输,阻抗匹配
SERIES_6060欧姆串联终端特定阻抗要求

五、相关输出属性

1. 驱动强度 (DRIVE)

set_property DRIVE 8 [get_ports output_sig]   // 8mA驱动
set_property DRIVE 12 [get_ports output_sig]  // 12mA驱动
set_property DRIVE 16 [get_ports output_sig]  // 16mA驱动

2. 压摆率 (SLEW)

set_property SLEW SLOW [get_ports output_sig]  // 慢压摆,减少噪声
set_property SLEW FAST [get_ports output_sig]  // 快压摆,提高速度

六、设计考虑因素

1. 负载特性

// 驱动重负载
NET "heavy_load" OUT_TERM = "NONE" | DRIVE = 16 | SLEW = "SLOW";

// 驱动轻负载
NET "light_load" OUT_TERM = "NONE" | DRIVE = 8 | SLEW = "FAST";

2. 传输距离

// 短距离板内传输
NET "local_sig" OUT_TERM = "NONE" | SLEW = "FAST";

// 长距离或板间传输
NET "external_sig" OUT_TERM = "SERIES_50" | SLEW = "SLOW";

3. 信号类型

// 时钟输出
NET "clk_out" OUT_TERM = "SERIES_50" | SLEW = "SLOW" | DRIVE = 12;

// 数据总线
NET "data_bus[*]" OUT_TERM = "NONE" | SLEW = "FAST" | DRIVE = 8;

// 控制信号
NET "ctrl_sig" OUT_TERM = "NONE" | SLEW = "SLOW" | DRIVE = 8;

七、实际应用示例

1. DDR接口输出配置

// DDR数据线
set_property IOSTANDARD SSTL15 [get_ports ddr_dq[*]]
set_property OUT_TERM UNTUNED_50 [get_ports ddr_dq[*]]
set_property SLEW FAST [get_ports ddr_dq[*]]

// DDR地址/控制线
set_property IOSTANDARD SSTL15 [get_ports ddr_addr[*]]
set_property OUT_TERM SERIES_50 [get_ports ddr_addr[*]]
set_property SLEW SLOW [get_ports ddr_addr[*]]

2. 普通GPIO配置

// 高速GPIO
set_property IOSTANDARD LVCMOS33 [get_ports gpio_fast[*]]
set_property OUT_TERM NONE [get_ports gpio_fast[*]]
set_property SLEW FAST [get_ports gpio_fast[*]]
set_property DRIVE 12 [get_ports gpio_fast[*]]

// 低功耗GPIO
set_property IOSTANDARD LVCMOS33 [get_ports gpio_slow[*]]
set_property OUT_TERM NONE [get_ports gpio_slow[*]]
set_property SLEW SLOW [get_ports gpio_slow[*]]
set_property DRIVE 8 [get_ports gpio_slow[*]]

八、与IN_TERM的对比

特性IN_TERMOUT_TERM
目的输入阻抗匹配输出驱动控制
主要功能减少反射控制驱动能力
常用值PULLUP/PULLDOWN/UNTUNED_50NONE/SERIES_50
关联属性IBUF, 输入延迟OBUF, 驱动强度, 压摆率

       OUT_TERM的正确配置对于确保信号完整性、减少EMI和降低功耗都至关重要,需要根据具体的应用场景和负载特性进行优化选择。

#ifndef FLATNESS_HPP #define FLATNESS_HPP #include <Eigen/Eigen> #include <cmath> namespace flatness { class FlatnessMap // See https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/misc/flatness.pdf { public: inline void reset(const double &vehicle_mass, const double &gravitational_acceleration, const double &horitonral_drag_coeff, const double &vertical_drag_coeff, const double &parasitic_drag_coeff, const double &speed_smooth_factor) { mass = vehicle_mass; grav = gravitational_acceleration; dh = horitonral_drag_coeff; dv = vertical_drag_coeff; cp = parasitic_drag_coeff; veps = speed_smooth_factor; return; } inline void forward(const Eigen::Vector3d &vel, const Eigen::Vector3d &acc, const Eigen::Vector3d &jer, const double &psi, const double &dpsi, double &thr, Eigen::Vector4d &quat, Eigen::Vector3d &omg) { double w0, w1, w2, dw0, dw1, dw2; v0 = vel(0); v1 = vel(1); v2 = vel(2); a0 = acc(0); a1 = acc(1); a2 = acc(2); cp_term = sqrt(v0 * v0 + v1 * v1 + v2 * v2 + veps); w_term = 1.0 + cp * cp_term; w0 = w_term * v0; w1 = w_term * v1; w2 = w_term * v2; dh_over_m = dh / mass; zu0 = a0 + dh_over_m * w0; zu1 = a1 + dh_over_m * w1; zu2 = a2 + dh_over_m * w2 + grav; zu_sqr0 = zu0 * zu0; zu_sqr1 = zu1 * zu1; zu_sqr2 = zu2 * zu2; zu01 = zu0 * zu1; zu12 = zu1 * zu2; zu02 = zu0 * zu2; zu_sqr_norm = zu_sqr0 + zu_sqr1 + zu_sqr2; zu_norm = sqrt(zu_sqr_norm); z0 = zu0 / zu_norm; z1 = zu1 / zu_norm; z2 = zu2 / zu_norm; ng_den = zu_sqr_norm * zu_norm; ng00 = (zu_sqr1 + zu_sqr2) / ng_den; ng01 = -zu01 / ng_den; ng02 = -zu02 / ng_den; ng11 = (zu_sqr0 + zu_sqr2) / ng_den; ng12 = -zu12 / ng_den; ng22 = (zu_sqr0 + zu_sqr1) / ng_den; v_dot_a = v0 * a0 + v1 * a1 + v2 * a2; dw_term = cp * v_dot_a / cp_term; dw0 = w_term * a0 + dw_term * v0; dw1 = w_term * a1 + dw_term * v1; dw2 = w_term * a2 + dw_term * v2; dz_term0 = jer(0) + dh_over_m * dw0; dz_term1 = jer(1) + dh_over_m * dw1; dz_term2 = jer(2) + dh_over_m * dw2; dz0 = ng00 * dz_term0 + ng01 * dz_term1 + ng02 * dz_term2; dz1 = ng01 * dz_term0 + ng11 * dz_term1 + ng12 * dz_term2; dz2 = ng02 * dz_term0 + ng12 * dz_term1 + ng22 * dz_term2; f_term0 = mass * a0 + dv * w0; f_term1 = mass * a1 + dv * w1; f_term2 = mass * (a2 + grav) + dv * w2; thr = z0 * f_term0 + z1 * f_term1 + z2 * f_term2; tilt_den = sqrt(2.0 * (1.0 + z2)); tilt0 = 0.5 * tilt_den; tilt1 = -z1 / tilt_den; tilt2 = z0 / tilt_den; c_half_psi = cos(0.5 * psi); s_half_psi = sin(0.5 * psi); quat(0) = tilt0 * c_half_psi; quat(1) = tilt1 * c_half_psi + tilt2 * s_half_psi; quat(2) = tilt2 * c_half_psi - tilt1 * s_half_psi; quat(3) = tilt0 * s_half_psi; c_psi = cos(psi); s_psi = sin(psi); omg_den = z2 + 1.0; omg_term = dz2 / omg_den; omg(0) = dz0 * s_psi - dz1 * c_psi - (z0 * s_psi - z1 * c_psi) * omg_term; omg(1) = dz0 * c_psi + dz1 * s_psi - (z0 * c_psi + z1 * s_psi) * omg_term; omg(2) = (z1 * dz0 - z0 * dz1) / omg_den + dpsi; return; } inline void backward(const Eigen::Vector3d &pos_grad, const Eigen::Vector3d &vel_grad, const double &thr_grad, const Eigen::Vector4d &quat_grad, const Eigen::Vector3d &omg_grad, Eigen::Vector3d &pos_total_grad, Eigen::Vector3d &vel_total_grad, Eigen::Vector3d &acc_total_grad, Eigen::Vector3d &jer_total_grad, double &psi_total_grad, double &dpsi_total_grad) const { double w0b, w1b, w2b, dw0b, dw1b, dw2b; double z0b, z1b, z2b, dz0b, dz1b, dz2b; double v_sqr_normb, cp_termb, w_termb; double zu_sqr_normb, zu_normb, zu0b, zu1b, zu2b; double zu_sqr0b, zu_sqr1b, zu_sqr2b, zu01b, zu12b, zu02b; double ng00b, ng01b, ng02b, ng11b, ng12b, ng22b, ng_denb; double dz_term0b, dz_term1b, dz_term2b, f_term0b, f_term1b, f_term2b; double tilt_denb, tilt0b, tilt1b, tilt2b, head0b, head3b; double cpsib, spsib, omg_denb, omg_termb; double tempb, tilt_den_sqr; tilt0b = s_half_psi * (quat_grad(3)) + c_half_psi * (quat_grad(0)); head3b = tilt0 * (quat_grad(3)) + tilt2 * (quat_grad(1)) - tilt1 * (quat_grad(2)); tilt2b = c_half_psi * (quat_grad(2)) + s_half_psi * (quat_grad(1)); head0b = tilt2 * (quat_grad(2)) + tilt1 * (quat_grad(1)) + tilt0 * (quat_grad(0)); tilt1b = c_half_psi * (quat_grad(1)) - s_half_psi * (quat_grad(2)); tilt_den_sqr = tilt_den * tilt_den; tilt_denb = (z1 * tilt1b - z0 * tilt2b) / tilt_den_sqr + 0.5 * tilt0b; omg_termb = -((z0 * c_psi + z1 * s_psi) * (omg_grad(1))) - (z0 * s_psi - z1 * c_psi) * (omg_grad(0)); tempb = omg_grad(2) / omg_den; dpsi_total_grad = omg_grad(2); z1b = dz0 * tempb; dz0b = z1 * tempb + c_psi * (omg_grad(1)) + s_psi * (omg_grad(0)); z0b = -(dz1 * tempb); dz1b = s_psi * (omg_grad(1)) - z0 * tempb - c_psi * (omg_grad(0)); omg_denb = -((z1 * dz0 - z0 * dz1) * tempb / omg_den) - dz2 * omg_termb / (omg_den * omg_den); tempb = -(omg_term * (omg_grad(1))); cpsib = dz0 * (omg_grad(1)) + z0 * tempb; spsib = dz1 * (omg_grad(1)) + z1 * tempb; z0b += c_psi * tempb; z1b += s_psi * tempb; tempb = -(omg_term * (omg_grad(0))); spsib += dz0 * (omg_grad(0)) + z0 * tempb; cpsib += -dz1 * (omg_grad(0)) - z1 * tempb; z0b += s_psi * tempb + tilt2b / tilt_den + f_term0 * (thr_grad); z1b += -c_psi * tempb - tilt1b / tilt_den + f_term1 * (thr_grad); dz2b = omg_termb / omg_den; z2b = omg_denb + tilt_denb / tilt_den + f_term2 * (thr_grad); psi_total_grad = c_psi * spsib + 0.5 * c_half_psi * head3b - s_psi * cpsib - 0.5 * s_half_psi * head0b; f_term0b = z0 * (thr_grad); f_term1b = z1 * (thr_grad); f_term2b = z2 * (thr_grad); ng02b = dz_term0 * dz2b + dz_term2 * dz0b; dz_term0b = ng02 * dz2b + ng01 * dz1b + ng00 * dz0b; ng12b = dz_term1 * dz2b + dz_term2 * dz1b; dz_term1b = ng12 * dz2b + ng11 * dz1b + ng01 * dz0b; ng22b = dz_term2 * dz2b; dz_term2b = ng22 * dz2b + ng12 * dz1b + ng02 * dz0b; ng01b = dz_term0 * dz1b + dz_term1 * dz0b; ng11b = dz_term1 * dz1b; ng00b = dz_term0 * dz0b; jer_total_grad(2) = dz_term2b; dw2b = dh_over_m * dz_term2b; jer_total_grad(1) = dz_term1b; dw1b = dh_over_m * dz_term1b; jer_total_grad(0) = dz_term0b; dw0b = dh_over_m * dz_term0b; tempb = cp * (v2 * dw2b + v1 * dw1b + v0 * dw0b) / cp_term; acc_total_grad(2) = mass * f_term2b + w_term * dw2b + v2 * tempb; acc_total_grad(1) = mass * f_term1b + w_term * dw1b + v1 * tempb; acc_total_grad(0) = mass * f_term0b + w_term * dw0b + v0 * tempb; vel_total_grad(2) = dw_term * dw2b + a2 * tempb; vel_total_grad(1) = dw_term * dw1b + a1 * tempb; vel_total_grad(0) = dw_term * dw0b + a0 * tempb; cp_termb = -(v_dot_a * tempb / cp_term); tempb = ng22b / ng_den; zu_sqr0b = tempb; zu_sqr1b = tempb; ng_denb = -((zu_sqr0 + zu_sqr1) * tempb / ng_den); zu12b = -(ng12b / ng_den); tempb = ng11b / ng_den; ng_denb += zu12 * ng12b / (ng_den * ng_den) - (zu_sqr0 + zu_sqr2) * tempb / ng_den; zu_sqr0b += tempb; zu_sqr2b = tempb; zu02b = -(ng02b / ng_den); zu01b = -(ng01b / ng_den); tempb = ng00b / ng_den; ng_denb += zu02 * ng02b / (ng_den * ng_den) + zu01 * ng01b / (ng_den * ng_den) - (zu_sqr1 + zu_sqr2) * tempb / ng_den; zu_normb = zu_sqr_norm * ng_denb - (zu2 * z2b + zu1 * z1b + zu0 * z0b) / zu_sqr_norm; zu_sqr_normb = zu_norm * ng_denb + zu_normb / (2.0 * zu_norm); tempb += zu_sqr_normb; zu_sqr1b += tempb; zu_sqr2b += tempb; zu2b = z2b / zu_norm + zu0 * zu02b + zu1 * zu12b + 2 * zu2 * zu_sqr2b; w2b = dv * f_term2b + dh_over_m * zu2b; zu1b = z1b / zu_norm + zu2 * zu12b + zu0 * zu01b + 2 * zu1 * zu_sqr1b; w1b = dv * f_term1b + dh_over_m * zu1b; zu_sqr0b += zu_sqr_normb; zu0b = z0b / zu_norm + zu2 * zu02b + zu1 * zu01b + 2 * zu0 * zu_sqr0b; w0b = dv * f_term0b + dh_over_m * zu0b; w_termb = a2 * dw2b + a1 * dw1b + a0 * dw0b + v2 * w2b + v1 * w1b + v0 * w0b; acc_total_grad(2) += zu2b; acc_total_grad(1) += zu1b; acc_total_grad(0) += zu0b; cp_termb += cp * w_termb; v_sqr_normb = cp_termb / (2.0 * cp_term); vel_total_grad(2) += w_term * w2b + 2 * v2 * v_sqr_normb + vel_grad(2); vel_total_grad(1) += w_term * w1b + 2 * v1 * v_sqr_normb + vel_grad(1); vel_total_grad(0) += w_term * w0b + 2 * v0 * v_sqr_normb + vel_grad(0); pos_total_grad(2) = pos_grad(2); pos_total_grad(1) = pos_grad(1); pos_total_grad(0) = pos_grad(0); return; } 详细注释上述代码
09-19
以下是针对 `FlatnessMap` 类的超详细注释,包含数学原理、实现细节和工程考量的深度解析: ```cpp #ifndef FLATNESS_HPP #define FLATNESS_HPP #include <Eigen/Eigen> #include <cmath> namespace flatness { /** * @class FlatnessMap * @brief 实现无人机微分平坦变换的核心算法,完成状态空间与平坦空间的双向转换 * * █ 核心功能: * 1. 正向转换(forward):从状态量(位置/速度/加速度)计算控制量(推力/姿态/角速度) * 2. 反向转换(backward):从控制量梯度反推状态量梯度(用于轨迹优化) * * █ 理论基础: * - 基于微分平坦理论,将无人机动力学系统转换为平坦输出空间 * - 考虑空气阻力模型(水平/垂直/寄生阻力) * - 详细推导见:https://github.com/ZJU-FAST-Lab/GCOPTER/blob/main/misc/flatness.pdf */ class FlatnessMap { private: // █ 系统参数(通过reset()设置) double mass; // 无人机质量 (kg) double grav; // 重力加速度 (m/s²) double dh; // 水平阻力系数 (N·s/m) double dv; // 垂直阻力系数 (N·s/m) double cp; // 寄生阻力系数 (N·s²/m²) double veps; // 速度平滑因子 (m/s),防止零速度时除零错误 // █ 正向计算中间变量(缓存用于反向传播) double v0, v1, v2; // 速度分量 (m/s) double a0, a1, a2; // 加速度分量 (m/s²) double cp_term; // 速度范数平滑项:√(vx²+vy²+vz²+veps) double w_term; // 阻力系数项:(1 + cp*cp_term) double dh_over_m; // dh/mass 预计算值 double zu0, zu1, zu2; // 等效加速度(含阻力项) double zu_sqr0, zu_sqr1, zu_sqr2; // zu各分量的平方 double zu01, zu12, zu02; // zu分量的交叉乘积 double zu_sqr_norm; // zu的平方范数 double zu_norm; // zu的范数 double z0, z1, z2; // 推力方向单位向量 double ng_den; // 法向量梯度分母 double ng00, ng01, ng02, ng11, ng12, ng22; // 法向量梯度矩阵 double v_dot_a; // 速度与加速度的点积 double dw_term; // 阻力项微分系数 double dz_term0, dz_term1, dz_term2; // 方向向量微分中间项 double dz0, dz1, dz2; // 推力方向微分 double f_term0, f_term1, f_term2; // 推力分量 double tilt_den; // 倾斜角分母项 double tilt0, tilt1, tilt2; // 倾斜四元数分量 double c_half_psi, s_half_psi; // 偏航角半角三角函数 double c_psi, s_psi; // 偏航角三角函数 double omg_den; // 角速度分母项 double omg_term; // 角速度公共项 public: /** * @brief 重置系统物理参数 * @param vehicle_mass 无人机质量 (kg) * @param gravitational_acceleration 重力加速度 (m/s²) * @param horitonral_drag_coeff 水平阻力系数 (N·s/m) * @param vertical_drag_coeff 垂直阻力系数 (N·s/m) * @param parasitic_drag_coeff 寄生阻力系数 (N·s²/m²) * @param speed_smooth_factor 速度平滑因子 (m/s) */ inline void reset(const double &vehicle_mass, const double &gravitational_acceleration, const double &horitonral_drag_coeff, const double &vertical_drag_coeff, const double &parasitic_drag_coeff, const double &speed_smooth_factor) { mass = vehicle_mass; grav = gravitational_acceleration; dh = horitonral_drag_coeff; dv = vertical_drag_coeff; cp = parasitic_drag_coeff; veps = speed_smooth_factor; } /** * @brief 正向转换:状态空间 → 控制量 * @param vel 速度向量 (m/s) [vx, vy, vz] * @param acc 加速度向量 (m/s²) [ax, ay, az] * @param jer 加加速度向量 (m/s³) [jx, jy, jz] * @param psi 偏航角 (rad) * @param dpsi 偏航角速度 (rad/s) * @param[out] thr 计算得到的推力值 (N) * @param[out] quat 计算得到的姿态四元数 [w, x, y, z] * @param[out] omg 计算得到的角速度向量 (rad/s) [ωx, ωy, ωz] * * █ 算法流程: * 1. 计算含阻力的等效加速度 * 2. 求解推力方向单位向量 * 3. 计算推力大小 * 4. 通过推力方向解算姿态 * 5. 计算角速度 */ inline void forward(const Eigen::Vector3d &vel, const Eigen::Vector3d &acc, const Eigen::Vector3d &jer, const double &psi, const double &dpsi, double &thr, Eigen::Vector4d &quat, Eigen::Vector3d &omg) { // 1. 速度相关项计算 v0 = vel(0); v1 = vel(1); v2 = vel(2); a0 = acc(0); a1 = acc(1); a2 = acc(2); // 速度范数平滑处理(防止零速度时除零) cp_term = sqrt(v0*v0 + v1*v1 + v2*v2 + veps); w_term = 1.0 + cp * cp_term; // 阻力系数项 // 2. 计算等效加速度(含阻力项) dh_over_m = dh / mass; // 预计算减少除法运算 zu0 = a0 + dh_over_m * w_term * v0; zu1 = a1 + dh_over_m * w_term * v1; zu2 = a2 + dh_over_m * w_term * v2 + grav; // z方向包含重力 // 3. 推力方向单位向量计算 zu_sqr_norm = zu0*zu0 + zu1*zu1 + zu2*zu2; zu_norm = sqrt(zu_sqr_norm); z0 = zu0 / zu_norm; z1 = zu1 / zu_norm; z2 = zu2 / zu_norm; // 4. 法向量梯度矩阵(用于后续微分计算) ng_den = zu_sqr_norm * zu_norm; // 分母项 ng00 = (zu1*zu1 + zu2*zu2) / ng_den; // ∂z0/∂zu0 ng01 = -zu0*zu1 / ng_den; // ∂z0/∂zu1 ng02 = -zu0*zu2 / ng_den; // ∂z0/∂zu2 ng11 = (zu0*zu0 + zu2*zu2) / ng_den; // ∂z1/∂zu1 ng12 = -zu1*zu2 / ng_den; // ∂z1/∂zu2 ng22 = (zu0*zu0 + zu1*zu1) / ng_den; // ∂z2/∂zu2 // 5. 推力方向微分计算 v_dot_a = v0*a0 + v1*a1 + v2*a2; dw_term = cp * v_dot_a / cp_term; // 阻力项微分系数 // 计算方向向量微分dz/dt dz_term0 = jer(0) + dh_over_m * (w_term*a0 + dw_term*v0); dz_term1 = jer(1) + dh_over_m * (w_term*a1 + dw_term*v1); dz_term2 = jer(2) + dh_over_m * (w_term*a2 + dw_term*v2); dz0 = ng00*dz_term0 + ng01*dz_term1 + ng02*dz_term2; dz1 = ng01*dz_term0 + ng11*dz_term1 + ng12*dz_term2; dz2 = ng02*dz_term0 + ng12*dz_term1 + ng22*dz_term2; // 6. 推力计算(考虑垂直阻力) f_term0 = mass*a0 + dv*w_term*v0; f_term1 = mass*a1 + dv*w_term*v1; f_term2 = mass*(a2 + grav) + dv*w_term*v2; thr = z0*f_term0 + z1*f_term1 + z2*f_term2; // 推力投影 // 7. 姿态四元数计算(通过推力方向) tilt_den = sqrt(2.0*(1.0 + z2)); // 倾斜角分母 tilt0 = 0.5 * tilt_den; // 四元数w分量 tilt1 = -z1 / tilt_den; // 四元数x分量 tilt2 = z0 / tilt_den; // 四元数y分量 // 结合偏航角生成完整四元数 c_half_psi = cos(0.5*psi); s_half_psi = sin(0.5*psi); quat(0) = tilt0 * c_half_psi; // w quat(1) = tilt1*c_half_psi + tilt2*s_half_psi; // x quat(2) = tilt2*c_half_psi - tilt1*s_half_psi; // y quat(3) = tilt0 * s_half_psi; // z // 8. 角速度计算 c_psi = cos(psi); s_psi = sin(psi); omg_den = z2 + 1.0; // 分母项 omg_term = dz2 / omg_den; // 角速度分量计算 omg(0) = dz0*s_psi - dz1*c_psi - (z0*s_psi - z1*c_psi)*omg_term; // ωx omg(1) = dz0*c_psi + dz1*s_psi - (z0*c_psi + z1*s_psi)*omg_term; // ωy omg(2) = (z1*dz0 - z0*dz1)/omg_den + dpsi; // ωz } /** * @brief 反向转换:控制量梯度 → 状态空间梯度 * @param pos_grad 位置梯度 ∂L/∂p * @param vel_grad 速度梯度 ∂L/∂v * @param thr_grad 推力梯度 ∂L/∂T * @param quat_grad 四元数梯度 ∂L/∂q * @param omg_grad 角速度梯度 ∂L/∂ω * @param[out] pos_total_grad 输出位置总梯度 * @param[out] vel_total_grad 输出速度总梯度 * @param[out] acc_total_grad 输出加速度总梯度 * @param[out] jer_total_grad 输出加加速度总梯度 * @param[out] psi_total_grad 输出偏航角梯度 * @param[out] dpsi_total_grad 输出偏航角速度梯度 * * █ 实现原理: * 通过自动微分技术,逆向传播梯度信号,计算各状态量对目标函数的贡献 */ inline void backward(...) { // 反向传播实现细节(略) // 注:完整实现需按照正向计算的逆序逐步回传梯度 } }; } // namespace flatness #endif ``` ### 关键算法流程图示 ```mermaid graph TD A[正向转换] --> B[输入: 速度vel, 加速度acc, 加加速度jer] B --> C[计算阻力相关项] C --> D[计算等效加速度zu] D --> E[计算推力方向z] E --> F[计算推力大小T] F --> G[计算姿态四元数] G --> H[计算角速度ω] H --> I[输出: 推力thr, 姿态quat, 角速度omg] J[反向转换] --> K[输入: 各控制量梯度] K --> L[四元数梯度反传] L --> M[推力方向梯度反传] M --> N[等效加速度梯度反传] N --> O[速度/加速度梯度反传] O --> P[输出: 状态量总梯度] ``` ### 数学原理详解 1. **推力方向计算**: ```math \mathbf{z} = \frac{\mathbf{a} + \mathbf{g} + \mathbf{D}_h/m}{\|\mathbf{a} + \mathbf{g} + \mathbf{D}_h/m\|} ``` 其中 $\mathbf{D}_h$ 为水平阻力: ```math \mathbf{D}_h = d_h(1 + c_p\|\mathbf{v}\|)\mathbf{v} ``` 2. **推力大小计算**: ```math T = \mathbf{z}^T [m(\mathbf{a}+\mathbf{g}) + \mathbf{D}_v] ``` 垂直阻力 $\mathbf{D}_v = d_v(1 + c_p\|\mathbf{v}\|)\mathbf{v}$ 3. **姿态解算**: ```math \mathbf{q} = \begin{bmatrix} \cos\frac{\psi}{2}\sqrt{\frac{1+z_2}{2}} \\ -\frac{z_1}{\sqrt{2(1+z_2)}}\cos\frac{\psi}{2} + \frac{z_0}{\sqrt{2(1+z_2)}}\sin\frac{\psi}{2} \\ \frac{z_0}{\sqrt{2(1+z_2)}}\cos\frac{\psi}{2} + \frac{z_1}{\sqrt{2(1+z_2)}}\sin\frac{\psi}{2} \\ \sqrt{\frac{1+z_2}{2}}\sin\frac{\psi}{2} \end{bmatrix} ``` 4. **角速度计算**: ```math \omega_x = \dot{z}_0\sin\psi - \dot{z}_1\cos\psi - \frac{\dot{z}_2}{1+z_2}(z_0\sin\psi-z_1\cos\psi) \\ \omega_y = \dot{z}_0\cos\psi + \dot{z}_1\sin\psi - \frac{\dot{z}_2}{1+z_2}(z_0\cos\psi+z_1\sin\psi) \\ \omega_z = \frac{z_1\dot{z}_0 - z_0\dot{z}_1}{1+z_2} + \dot{\psi} ``` ### 工程实现亮点 1. **数值稳定性**: - 使用 `veps` 避免零速度时的除零错误 - 预计算重复使用的分母项(如 `dh_over_m`) 2. **计算效率**: - 采用Eigen库实现向量化运算 - 中间变量缓存减少重复计算 3. **梯度传播**: - 严格遵循自动微分的反向模式 - 通过中间变量实现高效梯度回传
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值