PolynomialTraj.py API 参考

多维三次多项式轨迹、误差导数、曲率/挠率与路径参数管理。

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

阅读方式

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

generate_ordered_path_points

generate_ordered_path_points(num_points, grid_size=5000)
  • 源码位置：第 7 行
  • 类别：内部计算/辅助
  • 作用：源码内部辅助函数；结合签名、状态写入和主要调用可定位其在调用链中的职责。
  • 主要调用：int, np.ceil, np.column_stack, np.linspace, np.meshgrid, np.random.shuffle, np.sqrt, np.zeros, xx.ravel, yy.ravel
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.__init__

__init__(self, dimension, points, vel=[], auto_vel_flag=1, loop_flag=0)
  • 源码位置：第 31 行
  • 类别：初始化
  • 作用：初始化 PolynomialTraj3Order 的参数、状态缓存以及所需的通信或算法对象。
  • 主要调用：Polynomial3Order, np.append, np.array, np.ones, np.zeros, range, self.AdjustT, self.AutoTlist, self.AutoVel, self.GetDesireP
  • 写入状态：self.AdjustT_t, self.dimension, self.e_hessian, self.e_jacobian, self.loop_flag, self.phi, self.phi_jacobian, self.points, self.polys, self.pos_record, self.t_list, self.v, self.vel

PolynomialTraj3Order.AutoVel

AutoVel(self)
  • 源码位置：第 62 行
  • 类别：内部计算/辅助
  • 作用：根据相邻路径段方向自动生成各关键点的三次多项式边界速度。
  • 主要调用：GetMidVec, len, np.append, np.linalg.norm, np.zeros, range
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.AutoVel.GetMidVec

GetMidVec(vec1, vec2)
  • 源码位置：第 63 行
  • 类别：算法查询/计算
  • 作用：计算并返回名称所指的轨迹、误差或几何量。具体定义见本页所属模块说明。
  • 主要调用：np.linalg.norm
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.AutoTlist

AutoTlist(self)
  • 源码位置：第 95 行
  • 类别：内部计算/辅助
  • 作用：按路径段长度和内部参考速度分配每段的路径参数区间。
  • 主要调用：len, np.append, np.array, np.linalg.norm, range
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.Update

Update(self, pos, t)
  • 源码位置：第 110 行
  • 类别：内部计算/辅助
  • 作用：更新输入对应的内部缓存，避免后续计算重复执行。
  • 主要调用：np.all, np.array, self.AdjustT, self.EHessianCal, self.EJacobianCal, self.PhiCal, self.PhiJacobianCal
  • 写入状态：self.AdjustT_t, self.e_hessian, self.e_jacobian, self.phi, self.phi_jacobian, self.pos_record

PolynomialTraj3Order.AdjustT

AdjustT(self, t)
  • 源码位置：第 124 行
  • 类别：内部计算/辅助
  • 作用：对闭合路径循环路径参数，对开放路径执行端点钳位。
  • 主要调用：无显式函数调用
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.FindIndex

FindIndex(self, t)
  • 源码位置：第 145 行
  • 类别：内部计算/辅助
  • 作用：查找路径参数所在的分段多项式区间。
  • 主要调用：FindIndex, self.AdjustT
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.PhiCal

PhiCal(self, pos, t)
  • 源码位置：第 150 行
  • 类别：内部计算/辅助
  • 作用：计算实际位置与参数轨迹期望位置之间的逐维误差。
  • 主要调用：GetPos, np.array, range
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.PhiJacobianCal

PhiJacobianCal(self, t)
  • 源码位置：第 155 行
  • 类别：内部计算/辅助
  • 作用：构造位置—路径参数扩展空间中的轨迹误差雅可比。
  • 主要调用：GetD, np.append, np.array, np.eye, range
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.EJacobianCal

EJacobianCal(self, t)
  • 源码位置：第 162 行
  • 类别：内部计算/辅助
  • 作用：计算二次误差能量对位置和路径参数的梯度。
  • 主要调用：GetD, np.append, np.array, range
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.EHessianCal

EHessianCal(self, t)
  • 源码位置：第 168 行
  • 类别：内部计算/辅助
  • 作用：计算误差能量 Hessian 的源码近似形式。
  • 主要调用：GetD, GetDD, np.array, np.eye, np.sum, range
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.TangentCal

TangentCal(self, t)
  • 源码位置：第 183 行
  • 类别：内部计算/辅助
  • 作用：计算并归一化参数轨迹的一阶导数，得到单位切向量。
  • 主要调用：GetD, np.array, np.linalg.norm, range, self.AdjustT
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.TangentVCal

TangentVCal(self, t)
  • 源码位置：第 190 行
  • 类别：内部计算/辅助
  • 作用：返回参数轨迹未归一化的一阶导数。
  • 主要调用：GetD, np.array, range, self.AdjustT
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.GetDesireP

GetDesireP(self, t)
  • 源码位置：第 196 行
  • 类别：算法查询/计算
  • 作用：按路径参数分别求各维分段多项式位置。
  • 主要调用：GetPos, np.array, range
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.GetPhi

GetPhi(self, pos, t)
  • 源码位置：第 201 行
  • 类别：算法查询/计算
  • 作用：计算当前位置相对隐式曲面或参数轨迹的约束误差。
  • 主要调用：self.Update
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.GetPhiJacobian

GetPhiJacobian(self, pos, t)
  • 源码位置：第 205 行
  • 类别：算法查询/计算
  • 作用：更新缓存并返回轨迹约束函数的雅可比矩阵。
  • 主要调用：self.Update
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.GetEJacobian

GetEJacobian(self, pos, t)
  • 源码位置：第 209 行
  • 类别：算法查询/计算
  • 作用：更新缓存并返回标量误差能量的梯度。
  • 主要调用：self.Update
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.GetEHessian

GetEHessian(self, pos, t)
  • 源码位置：第 213 行
  • 类别：算法查询/计算
  • 作用：更新缓存并返回标量误差能量的 Hessian 近似。
  • 主要调用：self.Update
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.GetSampleTraj

GetSampleTraj(self, point_amount=200)
  • 源码位置：第 217 行
  • 类别：算法查询/计算
  • 作用：等间隔采样路径参数，返回参考轨迹点与对应参数。
  • 主要调用：np.append, np.array, np.linspace, np.zeros, self.GetDesireP
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.CurveCal

CurveCal(self, t)
  • 源码位置：第 226 行
  • 类别：内部计算/辅助
  • 作用：基于分段多项式系数计算三维曲率、挠率及副法向量。
  • 主要调用：GetMandPara, dTCal, ddTCal, np.array, np.concatenate, np.linalg.norm, np.reshape, np.squeeze, print, self.AdjustT
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.CurveCal2

CurveCal2(self, t)
  • 源码位置：第 264 行
  • 类别：内部计算/辅助
  • 作用：用一至三阶导数计算挠率的另一种实现。
  • 主要调用：Get3D, GetD, GetDD, np.array, np.cross, np.linalg.norm, np.reshape, np.squeeze, range, self.AdjustT
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.ThetaCal

ThetaCal(self, theta_0, t_now, dt)
  • 源码位置：第 279 行
  • 类别：内部计算/辅助
  • 作用：对挠率沿路径参数积分，更新移动标架旋转角。
  • 主要调用：integrate.quad, self.CurveCal2
  • 写入状态：不直接写入对象属性

PolynomialTraj3Order.ThetaCal.get_d_theta

get_d_theta(t_now)
  • 源码位置：第 281 行
  • 类别：算法查询/计算
  • 作用：源码内部辅助函数；结合签名、状态写入和主要调用可定位其在调用链中的职责。
  • 主要调用：self.CurveCal2
  • 写入状态：不直接写入对象属性