In order to improve the robustness and energy saving of the controller for multiple input multiple output (MIMO) unstable systems, the mathematical model of pitching and the heave motion of the scientific ship “YUKUN” of Dalian Maritime University is taken as the research plant, and the root locus of the transfer function mathematical model with unstable zero-pole and zero-pole processes is pulled to the left half plane by using the root locus shaping technique and weighting matrix. The first-order closed-loop gain shaping algorithm is used to design the robust controller, and the nonlinear feedback driven by the bi-polar S function is introduced to replace the original error linear feedback. The simulation results show that the nonlinear feedback control improves heave and pitching when the model is perturbed, greatly reduces the control energy, and is robust to model disturbance. The solution process of the scheme is simple, and the physical significance of the controller and the controlled object of the whole system is clear. The order of the controller design is low, and the controller design method of the MIMO unstable system is supplemented and perfected.