主权项 |
基于多项式的无人水下航行器空间轨迹规划方法,其特征在于,包括以下几个步骤:步骤一:无人水下航行器利用传感器获得当前自身位姿、速度信息作为轨迹规划的起点信息,起点信息包括初始时间t<sub>0</sub>、初始速度v<sub>0</sub>、初始位姿q<sub>0</sub>=(x<sub>0</sub>,y<sub>0</sub>,z<sub>0</sub>,φ<sub>0</sub>,θ<sub>0</sub>,ψ<sub>0</sub>),无人水下航行器接收轨迹规划的终点信息,终点信息包括终点时间t<sub>f</sub>、终点速度v<sub>f</sub>、终点位姿q<sub>f</sub>=(x<sub>f</sub>,y<sub>f</sub>,z<sub>f</sub>,φ<sub>f</sub>,θ<sub>f</sub>,ψ<sub>f</sub>),无人水下航行器接收障碍物信息;步骤二:利用多项式参数化空间轨迹方程,得到空间轨迹方程的固定系数与可调系数;步骤三:利用轨迹规划的起点信息和终点信息求解空间轨迹方程的固定系数;步骤四:建立轨迹优化的性能指标,求解空间轨迹方程的可调系数的最优值;步骤五:判断空间轨迹是否满足无碰条件,如果满足无碰条件,将可调系数的最优值代入空间轨迹方程,并输出空间轨迹,如果不满足无碰条件,求解可调系数的次优值,并将可调系数的次优值代入空间轨迹方程,并输出空间轨迹。 |