3D trajectory optimization for UAS insertion in civil non-segregated airspace