hw4-1

For the OBVP problem stated in slides p.25-p.29, please get the optimal solution (control, state, and time) for partially free final state case. Suppose the position is fixed, velocity and acceleration are free here.

推导与分析过程见本人博客文章：

Optimal Boundary Value Problem (OBVP) 学习笔记_Amos98的博客-CSDN博客

hw4-2

理解问题

在STEP 1中，添加

pos(0) += vel(0) * delta_time + 0.5 * acc_input(0) * delta_time * delta_time;
pos(1) += vel(1) * delta_time + 0.5 * acc_input(1) * delta_time * delta_time;
pos(2) += vel(2) * delta_time + 0.5 * acc_input(2) * delta_time * delta_time;
vel(0) += acc_input(0) * delta_time;
vel(1) += acc_input(1) * delta_time;
vel(2) += acc_input(2) * delta_time;

STEP 2的原理：

在给定初、末状态后，代价J是一个仅依赖于时间T的变量，因此，我们希望写出J(T)的具体表达式，并对其求导，得到J最小时的T，进而得到最小的J。这一最小的J就是函数Homeworktool::OptimalBVP要返回的值。

**注意：**STEP 2的输入是初状态位置、初状态速度、末状态位置，又认为末状态速度为0，因此初、末的全状态均已知。

STEP 1 + STEP 2完成的工作是：给定初状态，采样一系列的轨迹，即生成一系列末状态，由函数Homeworktool::OptimalBVP计算出每条轨迹的最小代价J，由此选出最佳的轨迹。

“最小代价J”的理解：指的是同一组初、末状态下，以T为变量的最小可能代价，而非不同末状态下最小的代价。

解决问题

第一步，求解J(T)具体表达式。

可使用Python包sympy，详见/hw4/scripts/symbol_compute.py

在整理J(T)的表达式时，我们希望将J(T)整理为以T为变量的多项式，首先使用sympy.expand()将表达式展开为多项式，再使用sympy.collect()收集表达式中指定符号的有理指数次幂的系数。

J_expand = sympy.expand(J)
J_expr = sympy.collect(J_expand, T)
print(J_expr)
print('-------------')

J_diff = sympy.diff(J, T)
J_diff_expand = sympy.expand(J_diff)
J_diff_expr = sympy.collect(J_diff_expand, T)
print(J_diff_expr)

参考：