A method of path planning for an autonomous vehicle, the method comprising: receiving a request for a two-segment turn of a vehicle from a current swath to a next swath in a work area following a guidance line in a headland at a periphery of the work area; in response to receiving the request, receiving information of the current swath including an end position of the current swath, information of the next swath, information of the guidance line, and a current position of the vehicle; determining a trajectory of the turn based on the information of the current swath, the information of the next swath, the information of the guidance line and the current position of the vehicle, the trajectory including a first segment, a second segment, and at least two initial segments (1010, 1020) wherein: a first initial segment of the at least two initial segments starts at the current position and ends at the end position of the current swath and a second initial segment of the at least two initial segments starts at the end position of the current swath and ends at the beginning position of the turn; the first segment starts from a beginning position of the turn at the current swath and ends at an intermediate position; the second segment starts from the intermediate position and ends at an ending position of the turn at the next swath, wherein at least a portion of the second segment of the trajectory follows the guidance line; and wherein the vehicle changes from a forward gear to a reverse gear, or vice versa, as the vehicle transitions from each segment to the next segment; and outputting the trajectory to a control system of the vehicle for executing the turn by following the first initial segment, the second initial segment, the first segment and the second segment of the trajectory successively. receiving a request for a two-segment turn of a vehicle from a current swath to a next swath in a work area following a guidance line in a headland at a periphery of the work area; in response to receiving the request, receiving information of the current swath including an end position of the current swath, information of the next swath, information of the guidance line, and a current position of the vehicle; determining a trajectory of the turn based on the information of the current swath, the information of the next swath, the information of the guidance line and the current position of the vehicle, the trajectory including a first segment, a second segment, and at least two initial segments (1010, 1020) wherein: a first initial segment of the at least two initial segments starts at the current position and ends at the end position of the current swath and a second initial segment of the at least two initial segments starts at the end position of the current swath and ends at the beginning position of the turn; the first segment starts from a beginning position of the turn at the current swath and ends at an intermediate position; the second segment starts from the intermediate position and ends at an ending position of the turn at the next swath, wherein at least a portion of the second segment of the trajectory follows the guidance line; and wherein the vehicle changes from a forward gear to a reverse gear, or vice versa, as the vehicle transitions from each segment to the next segment; and a first initial segment of the at least two initial segments starts at the current position and ends at the end position of the current swath and a second initial segment of the at least two initial segments starts at the end position of the current swath and ends at the beginning position of the turn; the first segment starts from a beginning position of the turn at the current swath and ends at an intermediate position; the second segment starts from the intermediate position and ends at an ending position of the turn at the next swath, wherein at least a portion of the second segment of the trajectory follows the guidance line; and wherein the vehicle changes from a forward gear to a reverse gear, or vice versa, as the vehicle transitions from each segment to the next segment; and outputting the trajectory to a control system of the vehicle for executing the turn by following the first initial segment, the second initial segment, the first segment and the second segment of the trajectory successively.