PathFollow.py API 参考

基于双积分质点模型和连续时间 LQR 的领航路径跟踪器。

  • 源码路径：multi_demo/scripts/swarm_lf/PathFollow.py
  • 本页函数/方法数：7
  • 行号对应本文档生成时采用的源码版本；源码调整后应重新生成本页。

阅读方式

本页用于快速检索签名与调用关系。核心控制和算法推导请先阅读“开发指南”中的对应章节。

PathFollow.__init__

__init__(self, start_pos, desire_path, dt=0.1)
  • 源码位置：第 11 行
  • 类别：初始化
  • 作用：初始化 PathFollow 的参数、状态缓存以及所需的通信或算法对象。
  • 主要调用：np.append, np.array, np.eye, np.linalg.inv, np.zeros, self.RiccatiEqSolve
  • 写入状态：self.A, self.B, self.C, self.K, self.P, self.Q, self.R, self.dt, self.err_record, self.last_correction_time, self.path, self.status, self.status_record

PathFollow.Acal

Acal(self)
  • 源码位置：第 42 行
  • 类别：内部计算/辅助
  • 作用：根据当前状态与期望状态计算二维加速度控制量，并执行加速度/速度边界处理。
  • 主要调用：np.append, np.array, np.copy, np.linalg.norm, np.squeeze, print, self.path.NearestP
  • 写入状态：self.err_record

PathFollow.AngleCal

AngleCal(self, a_vec)
  • 源码位置：第 74 行
  • 类别：内部计算/辅助
  • 作用：把二维加速度分解为切向与法向分量，计算轴向加速度和带符号滚转角。
  • 主要调用：np.arctan, np.array, np.linalg.norm, np.reshape, np.squeeze
  • 写入状态：不直接写入对象属性

PathFollow.StatusUpdate

StatusUpdate(self, u)
  • 源码位置：第 91 行
  • 类别：内部计算/辅助
  • 作用：更新控制器内部状态；离线基类还会按双积分模型推进状态。
  • 主要调用：np.append, np.reshape, np.squeeze
  • 写入状态：self.status, self.status_record

PathFollow.GetErr

GetErr(self, desire_status)
  • 源码位置：第 100 行
  • 类别：算法查询/计算
  • 作用：计算并返回名称所指的轨迹、误差或几何量。具体定义见本页所属模块说明。
  • 主要调用：无显式函数调用
  • 写入状态：不直接写入对象属性

PathFollow.RiccatiEqSolve

RiccatiEqSolve(A, B, Q, R)
  • 源码位置：第 105 行
  • 类别：内部计算/辅助
  • 作用：求解连续代数 Riccati 方程，返回用于构造 LQR 增益的矩阵 P。
  • 主要调用：scipy.linalg.solve_continuous_are
  • 写入状态：不直接写入对象属性

SingleTest

SingleTest()
  • 源码位置：第 110 行
  • 类别：离线测试入口
  • 作用：源码内部辅助函数；结合签名、状态写入和主要调用可定位其在调用链中的职责。
  • 主要调用：DesirePath, PathFollow, dict, go.Figure, go.Scatter, np.array, plt.add_trace, plt.show, range, uav.Acal
  • 写入状态：不直接写入对象属性