调整位姿方向以获得正确路径