2023-12-03-Drake 运动学轨迹优化类（kinematic_trajectory_optimization）（一）

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

背景

本文主要讨论运动学轨迹优化类（KinematicTrajectoryOptimization），该类用于解决运动学轨迹优化问题。这次先讨论理论，后续再讨论具体的实现。

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

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)}$$

上述问题中，$q(\cdot )$是一个关于时间的四次阶连续可导的函数，$q(t)$表示在时间$t$时刻的机器人的关节角度（广义坐标）。$T$是一个标量，表示整个轨迹的时间长度。我们的优化目标就是找到一个$T$和一个关于时间的函数$q(\cdot )$，在满足约束的情况下，使得$J(T,q(\cdot ))$最小，其中$J(T,q(\cdot ))$是一个关于$T$和$q(\cdot )$的标量函数，表示整个轨迹的代价。

1. 问题的转化

由于在实际应用中，计算机通常不太方便对一个连续函数进行优化，因此我们通过使用一个四次B样条曲线来近似表示$q(\cdot )$，即：

$$q(t)=\sum _{i=0}^n{q}_i{B}_{i,p}(t)\text{\hspace{1em}(2)}$$

由于$T$也是一个需要优化的变量，为了简化优化问题，我们可以将优化曲线的参数$t$归一化到$[0,1]$区间，即$\tau \in [0,1],\tau =\frac{t}{T}$，曲线和总时间就可以被当作两个彼此独立的变量来处理。如下：

$$\overline{q}(\tau )=\overline{q}(\frac{t}{T})=q(t)\text{\hspace{1em}(3)}$$

求导可得：

$$\begin{array}{rl}\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{\overline{q}}{T^2}\end{array}$$