本发明涉及一种基于优化算法的全驱动水下航行器航迹跟踪控制方法，属于水下航行器航迹跟踪控制技术领域。包括：获得航行器的运动参数，设计其运动姿态约束向量和速度约束向量；然后计算跟踪误差，并根据航行器的六自由度模型得到运动学控制器和动力学控制器；最后由运动学控制器得到期望速度，由动力学控制器推导出个自由度上的力和力矩；直至航迹跟踪完成。解决全驱动航行器在跟踪三维曲线中的冗余问题，同时考虑了航行器在运动过程中的姿态和速度约束问题。由姿态与速度约束构造优化问题为凸优化问题，有多种可靠方法求解，且求解速度可满足实时性的要求；充分考虑了全驱动航行器的运行中的速度饱和问题，实用性强。