一、基本问题
通过多阶多项式可以去拟合一条无人机轨迹,通常可以使用五阶多项式,并给定初始和终末状态量通过求解方程组来计算多项式参数,从而拟合出一条轨迹。
然而,如果多项式的阶数很高,对应的自由度就会很高,需要很多条件来计算轨迹,当方程组数<自由度时,就会计算出很多条轨迹,那么如何去判定或者说计算出一条最优的轨迹,就需要通过求解OBVP问题(optimal boundary value problem)来获得一条最优轨迹。
二、基本思路
1. 设置无人机系统的状态,输入,系统模型,目标函数。
2. 设置hamilton 函数
其中,的维度等于系统状态量的个数
3. 计算hamilton function对状态量的导数
4. 得到的可以用来计算出
5. 计算最优的u,可以通过计算,直接将代入,对u(t)求导即可。
6. 计算最优的s,对前面计算的积分即可,得到的s就是轨迹的最优表达式,但是包含参数变量。
7. 通过起始和终末条件来计算出,将0和T代入s的表达式中,计算出。
8. 同时cost function J也变成了只与时间T相关的表达式。
9. 最后,对于给定初始和终末状态,同时给定人为希望的时间T,我们就可以计算出一条最优轨迹。
三、具体实施
1. 设置系统,(注意,这里是将系统划分为三个维度,单独解决问题,目标函数是三维的)
状态State:
输入Input:
系统模型model:
目标函数,最小化Jerk的平方:
2. 针对每一维,计算Hamilton 函数:
最后,
3. 根据最小值原理,计算
4. 解微分方程,得到
5. 代入求解最优控制量u*
求导
6. 对u*积分,并且有初始状态,可以得到最优状态轨迹表达式
7. 最后指定终末状态
得到
8.将参数代入到目标函数中可以得到J
这里的是可以被代掉的,但是终末状态的具体数值未知,因此用简化表示。
9. 对进行求导,令其等于0,计算最优的T。将T代入就可以得到最优轨迹。
四、极小值原理
五、求解方程
在求解的时候,会遇到解一元四次方程组的问题,具体解法看这篇博客。