MPC_Settings.h & MPC_Settings.cpp

定义了一个Settings结构体，储存MPC的设置参数。定义了loadSettings函数加载MPC设置参数。
  1. timeHorizon_: 表示MPC时间范围的长度，单位是秒。这是预测未来状态时考虑的时间长度。
  1. solutionTimeWindow_: 用于检索优化输出的时间窗口，单位是秒。如果设置为负数，则表示使用整个时间范围。
  1. debugPrint_: 一个布尔值，决定是否显示MPC的日志输出。
  1. coldStart_: 另一个布尔值，决定是否使用前一次调用的控制器初始化（热启动）或给定的操作轨迹（冷启动）。
  1. mpcDesiredFrequency_: MPC循环的期望频率，单位是Hz。这个设置仅用于测试中的Dummy_Loop。如果设置为正数，MPC循环将模拟以给定频率运行；如果为负数，则以最大可能频率运行。
  1. mrtDesiredFrequency_: MRT（可能是指Model Reference Tracking，即模型参考跟踪）循环的期望频率，单位是Hz。这个设置也仅用于测试中的Dummy_Loop，并且应该总是设置为正数。

MPC_BASE.h & MPC_BASE.cpp

定义了一个名为 MPC_BASE 的类，它是 MPC（Model Predictive Control）方法的接口类。

公有成员函数：

  1. 构造函数和析构函数：
      • 构造函数 explicit MPC_BASE(mpc::Settings mpcSettings); 接受一个类型为 mpc::Settings 的参数，用于初始化 MPC 设置。使用移动构造函数（std::move）来接收 mpcSettings 参数，这避免了不必要的拷贝，并且确保传入的对象在构造后不会被外部使用。
      • 析构函数 virtual ~MPC_BASE() = default; 虚拟析构函数，将在派生类中进行实现。
  1. reset 函数：
      • 功能：重置类的状态到构造后的状态。注意，这个方法不能在求解器运行时调用。reset 函数将 initRun_ 重置为 true，重置计时器 mpcTimer_，并通过调用求解器的 reset 方法来重置求解器。
  1. run(scalar_t currentTime, const vector_t& currentState)：运行 MPC 的主要函数，接受当前时间和状态作为参数，执行MPC的主要例程，根据给定的当前时间和状态运行MPC算法。
    1. 检查当前时间是否超出了求解器的最终时间限制，如果是，则打印警告并返回 false。
    2. 计算最终时间 finalTime，这是当前时间加上时间范围。
    3. 如果启用了调试打印，将打印MPC被调用的信息，并开始计时器。
    4. 调用 calculateController 来计算MPC策略。
    5. 将 initRun_ 标志设置为 false，表示这不是第一次MPC运行。
    6. 如果启用了调试打印，将结束计时器并打印MPC的性能基准信息。
  1. getSolverPtr() 和 getSolverPtr() const：获取 MPC 使用的求解器的指针，分别提供了可修改和只读的版本。
  1. getTimeHorizon()：返回优化器调用的时间范围。
  1. settings()：返回 MPC 设置。

保护成员函数：

  1. calculateController 函数：
      • 参数：初始时间、初始状态和最终时间。
      • 功能：解决最优控制问题，这个函数是纯虚函数，需要在派生类中具体实现。

私有成员变量：

  1. initRun_：一个布尔值，标识是否是MPC的第一次运行。
  1. mpcSettings_：存储MPC设置的结构体。
  1. mpcTimer_：一个计时器，用于测量MPC运行的性能。

SystemObservation.h

定义了一个结构体 SystemObservation，并声明了一些与之相关的函数。
  1. SystemObservation 结构体：
      • size_t mode：表示观测时所处的模式。
      • scalar_t time：表示观测的时间。
      • vector_t state：表示系统的状态。
      • vector_t input：表示系统的输入。
  1. swap 函数：
      • 这是一个非成员函数，被声明为 friend，允许访问 SystemObservation 的私有成员。
      • 函数签名为 void swap(SystemObservation& a, SystemObservation& b) noexcept;。
      • 该函数通过 std::swap 将两个 SystemObservation 对象的成员进行交换，其中包括 mode、time、state 和 input。
  1. operator<< 重载：
      • 这是一个非成员函数，被声明为 friend，允许访问 SystemObservation 的私有成员。
      • 函数签名为 std::ostream& operator<<(std::ostream& out, const SystemObservation& observation);。
      • 该函数用于将 SystemObservation 对象的信息输出到给定的输出流 out 中，以便于显示观测信息。
      • 输出格式包括观测的时间、模式、状态和输入。

CommandData.h

定义了一个结构体 CommandData，并声明了一些与之相关的函数。
  1. SystemObservation mpcInitObservation_; ：MPC观测接口
  1. TargetTrajectories mpcTargetTrajectories_;：MPC轨迹接口

MrtObserver.h

定义了一个 MrtObserver类，用于观察和修改MPC解决方案。该类有两个虚拟方法，允许用户在MRT（Model Reference Tracking）过程的不同阶段修改MPC策略。
  • modifyActiveSolution 方法在调用 MRT_BASE::updatePolicy() 函数期间执行，允许用户修改将在调用 updatePolicy 后成为正在使用的策略。由于此函数与 updatePolicy 顺序执行，因此会阻塞主线程。因此，计算成本较高的修改应该在 modifyBufferedSolution 中进行。
  • modifyBufferedSolution 方法在新策略加载到缓冲区时调用。它允许用户在调用 updatePolicy 期间交换缓冲策略之前修改缓冲解决方案。在使用多线程MRT时，此函数不会阻塞主线程。
两个方法的调用都受相同的互斥锁保护，确保并发情况下对策略的安全访问和修改。

MRT_BASE.h & MRT_BASE.cpp

定义了一个名为MRT_BASE的基类，它用于实现模型参考跟踪（Model Reference Tracking，简称 MRT）控制器。

构造函数和析构函数

  • MRT_BASE(): 构造函数，初始化时调用 reset() 方法。
  • virtual ~MRT_BASE() = default;: 默认的虚析构函数，允许派生类正确地执行析构。

公共方法

  • void reset();: 重置类到其实例化状态。锁定互斥锁，重置策略相关的标志和指针。
  • virtual void resetMpcNode(const TargetTrajectories& initTargetTrajectories) = 0;: 要求派生类实现的纯虚函数，用于请求 MPC 节点重置。
  • bool initialPolicyReceived() const;: 检查是否已接收初始 MPC 策略。
  • virtual void setCurrentObservation(const SystemObservation& observation) = 0;: 要求派生类实现的纯虚函数，用于通知 MPC 新的状态观测。
  • const CommandData& getCommand() const;: 获取当前策略对应的命令数据的引用。获取当前活动策略的数据，如果指针为空，则抛出异常。
  • const PerformanceIndex& getPerformanceIndices() const;: 获取当前策略的性能指标数据的引用。
  • const PrimalSolution& getPolicy() const;: 获取当前优化策略的引用。
  • void initRollout(const RolloutBase* rolloutPtr);: 初始化回滚类以回滚反馈策略。
  • void evaluatePolicy(...);: 评估控制器。
  • void rolloutPolicy(...);: 根据当前时间状态使用 MPC 策略回滚控制策略。
  • bool updatePolicy();: 检查 MPC 策略是否有更新，如果有，则加载到当前使用的策略，如果成功更新，则返回 true。。
  • bool isRolloutSet() const;: 检查是否设置了内部回滚对象。
  • void addMrtObserver(std::shared_ptr<MrtObserver> mrtObserver);: 向策略更新过程添加 MRT 观察者。

受保护的方法

  • void moveToBuffer(...);: 将新策略移动到缓冲区，并允许修改缓冲策略。
  • void modifyActiveSolution(...);: 修改活动解决方案，供所有 MRT 观察者调用。
  • void modifyBufferedSolution(...);: 修改缓冲解决方案，供所有 MRT 观察者调用。

私有成员

  • std::atomic_bool policyReceivedEver_;: 标记是否曾经接收到策略。
  • bool newPolicyInBuffer_;: 标记缓冲区中是否有新策略等待交换。
  • 几个 std::unique_ptr 成员变量，分别用于存储活动的和缓冲区中的命令数据、原始解决方案和性能指标。
  • std::mutex bufferMutex_;: 互斥锁，用于保护策略缓冲区变量。
  • size_t mrtTrylockWarningThreshold_; 和 size_t mrtTrylockWarningCount_;: 用于记录尝试锁定互斥锁失败的次数。
  • std::unique_ptr<RolloutBase> rolloutPtr_;: 用于策略评估的回滚对象。
  • std::vector<std::shared_ptr<MrtObserver>> observerPtrArray_;: MRT 观察者数组。
 
 
 

MPC_MRT_Interface.h & MPC_MRT_Interface.cpp

定义了一个名为 MPC_MRT_Interface 的类，它提供了对OCS2的ROS独立接口。它包含了MPC（模型预测控制）和MRT（轨迹跟踪）模块的功能。
  • 构造函数：接受一个MPC_BASE对象的引用，并将其存储为类成员。
  • resetMpcNode ：重置MPC节点，将初始目标轨迹传递给MPC。该方法重置MPC对象，并设置新的目标轨迹，然后重置计时器。
  • setCurrentObservation ：设置当前观测值。该方法将传入的当前观测值存储在类成员中。
  • getReferenceManager ：获取ReferenceManagerInterface对象，该对象管理模式计划和目标轨迹。
  • advanceMpc ：推进MPC模块进行一次迭代。它首先测量运行MPC所需的时间，然后获取当前观测值并运行MPC以获得新的控制器。如果控制器已更新，则将其存储到缓冲区中，并检查MPC的延迟和解决方案时间窗口是否兼容。最后，它测量了发送ROS消息的延迟，并在调试模式下打印出MPC_MRT的性能指标。
  • copyToBuffer ：将MPC解决方案、命令和性能指标复制到缓冲区中。该方法首先创建MPC的解决方案、命令和性能指标对象，然后将它们移动到缓冲区中。
  • getLinearFeedbackGain ：从线性控制器中获取线性反馈增益。该方法检查策略中的控制器类型，并从线性控制器中获取反馈增益。
  • getValueFunction ：获取求解器的内部值函数。它返回请求时间和状态的值函数的二次近似。
  • getStateInputEqualityConstraintLagrangian ：计算与状态-输入约束相关的拉格朗日乘数。它返回请求时间和状态的状态-输入等式约束拉格朗日乘数。
  • getIntermediateDualSolution ：返回给定时间的中间对偶解。它返回与状态、状态-输入等式和不等式拉格朗日项相关的多重器。