2024-01-22-Drake 运动学轨迹优化类（kinematic_trajectory_optimization）（二）

Drake 运动学轨迹优化类（kinematic_trajectory_optimization）（二）

1. 背景

本文继续上文的话题，继续分析kinematic_trajectory_optimization类的具体代码实现。

这部分的代码位于下面两个文件中：

planning\trajectory_optimization\kinematic_trajectory_optimization.h
planning\trajectory_optimization\kinematic_trajectory_optimization.cc

回顾上一篇文章的讨论，我们知道，该类的主要目的是解决如下问题：

$$\begin{array}{rlrl}& \underset{T,q(\cdot )}{\min }& & J(T,q(\cdot ))\\ & \text{subject to}& & q(t_0)=q_0,q(t_f)=q_f\\ & & & \dot{q}(t_0)={\dot{q}}_0,\dot{q}(t_f)={\dot{q}}_f\\ & & & \ddot{q}(t_0)={\ddot{q}}_0,\ddot{q}(t_f)={\ddot{q}}_f\\ & & & {\dot{q}}_{min}\le \dot{q}(t)\le {\dot{q}}_{max}\\ & & & {\ddot{q}}_{min}\le \ddot{q}(t)\le {\ddot{q}}_{max}\\ & & & \text{obstacle avoidance}\\ & & & \text{kinematics, dynamics constrains}\end{array}\text{\hspace{1em}(1)}$$

并且有以下关系：

$$\begin{array}{rl}\overline{q}(\tau )& =\overline{q}(\frac{t}{T})=q(t)\\ \dot{q}(t)& =\frac{dq}{dt}=\frac{dq}{d\tau }\frac{d\tau }{dt}=\frac{\dot{\overline{q}}(\tau )}{T}\\ \ddot{q}(t)& =\frac{d^{2}q}{d{t}^2}=\frac{d}{dt}(\frac{dq}{dt})=\frac{d}{d\tau }(\frac{dq}{d\tau })\frac{d\tau }{dt}=\frac{\ddot{\overline{q}}(\tau )}{T^2}\end{array}\text{\hspace{1em}(2)}$$

经过转化，我们可以得到如下形式的优化问题：

min T , q ‾ 0 : n