参考:
王健, 赵又群, 季学武, et al. Path tracking for vehicle parallel parking based on ADRC controller[J]. Journal of Beijing Institute of Technology, 2015, 24(2):213-221.
(1)R1的约束
(2)R2的约束
(3)L0的约束
z:误差; v0=0