An Unmanned Ship Trajectory Tracking Method Based on Adaptive Preview Points and Projection Positioning

Abstract

The present invention discloses an unmanned ship trajectory tracking method based on an adaptive preview point and projection positioning. The method steps include: presetting an initial path for the unmanned ship to navigate; controlling the unmanned ship to navigate according to the initial path and obtaining the real-time motion state of the unmanned ship; judging whether the real navigation path of the unmanned ship deviates from the initial path according to the real-time motion state of the unmanned ship. If the navigation path deviates from the initial path, a target point on the initial path is obtained; obtaining the yaw distance and yaw angle of the unmanned ship according to the target point; correcting the real-time motion state of the unmanned ship according to the real-time motion state, yaw distance and yaw angle of the unmanned ship. The method of the present invention can take into account both the heading error and the trajectory error. Compared with the double closed-loop control of the rudder angle error and the trajectory error, it has better trajectory smoothness and rudder angle stability; the position of the preview point can be adaptively adjusted under different speeds and different trajectory curvatures to take into account both the trajectory tracking accuracy and the navigation stability.

Description

Technical Field

[0001] The present invention relates to a trajectory tracking method, in particular to a trajectory tracking method for an unmanned ship based on an adaptive preview point and projection positioning. Background Art

[0002] The trajectory tracking of a ship refers to a carrier such as an autonomous vehicle or a ship starting from a certain place and reaching a preset position through the control of a trajectory tracking system within a specified time. A high-performance trajectory tracking algorithm is urgently needed for surface unmanned ships.

[0003] Currently, the trajectory tracking control method based on a preview point is widely used, and a tracking scheme is calculated according to the preview point. However, the distance of the preview point is mostly preset manually, which will bring a series of possible problems, mainly manifested as the phenomena of leading steering and lagging steering before and after a large-curvature bend, low tracking accuracy in complex roads, poor control effect when the initial deviation is large, and too frequent adjustment when the deviation from the preset path is not large, etc. Summary of the Invention

[0004] In order to solve the problems existing in the background art, the present invention provides a trajectory tracking method for an unmanned ship based on an adaptive preview point and projection positioning, which can adaptively decide a control scheme according to the situation of the unmanned ship and the path, change the distance of the preview point, so as to make full use of path information to improve the tracking accuracy.

[0005] The technical solution adopted by the present invention is as follows:

[0006] The method of the present invention includes the following steps:

[0007] Step 100, preset an initial path for the unmanned ship to navigate;

[0008] Step 200, control the unmanned ship to navigate according to the initial path, and obtain the real-time motion state of the unmanned ship;

[0009] Step 300, under ideal conditions, the unmanned ship should follow the initial path as much as possible. Under the interference of the sea surface environment, the unmanned ship will deviate from the initial path to navigate; at this time, according to the real-time motion state of the unmanned ship, judge whether the real navigation path of the unmanned ship deviates from the initial path. If the navigation path deviates from the initial path, obtain the target point on the initial path;

[0010] Step 400, obtain the yaw distance and yaw angle of the unmanned ship according to the target point;

[0011] Step 500, correct the real-time motion state of the unmanned ship according to the real-time motion state, yaw distance and yaw angle of the unmanned ship.

[0012] In step 100 described above, the preset initial path of the unmanned ship's navigation, which is the path to be tracked by the unmanned ship, is usually a curved path composed of a series of path points or functional expressions. Taking the first path point in the initial path as the coordinate origin, the latitude line as the x-axis, and the longitude line as the y-axis, the path points of the initial path are expressed as:

[0013] P(i) = [px(i), py(i)]

[0014] where P(i) is the i-th path point in the initial path, px(i) is the x-axis coordinate of the i-th path point, and py(i) is the y-axis coordinate of the i-th path point.

[0015] In step 200 described above, the real-time motion state of the unmanned ship includes the heading angle, the navigation speed v(t), and the current position [x(t), y(t)] of the unmanned ship at the current moment t. Among them, x(t) is the x-axis coordinate of the unmanned ship at the current moment t, and y(t) is the y-axis coordinate of the unmanned ship at the current moment t.

[0016] In step 300 described above, it is judged whether the real navigation path of the unmanned ship deviates from the initial path:

[0017] If the real navigation path of the unmanned ship does not deviate from the initial path, continue to control the unmanned ship to navigate according to the initial path;

[0018] If the real navigation path of the unmanned ship deviates from the initial path, that is, there is a lateral deviation of the navigation path relative to the initial path, the following judgment is made:

[0019] If the real navigation path of the unmanned ship deviates from the initial path for the first time, traverse all the path points in the initial path, and take the path point closest to the current position of the unmanned ship in front of the current unmanned ship as the target point;

[0020] If the real navigation path of the unmanned ship does not deviate from the initial path for the first time, start from the target point P(m) obtained when the unmanned ship last deviated from the initial path, and traverse the initial path between all the path points in front of the unmanned ship, and obtain the path point closest to the current position of the unmanned ship as the current target point. Among them, P(m) is the m-th path point in the initial path, v(t) is the navigation speed of the current unmanned ship, Δt is the unit time of the control frequency of the control signal issued when controlling the unmanned ship to navigate, and s is the average distance between the path points of the initial path.

[0021] In step 400 described above, take the tangent line of the target point on the initial path as the projection line, obtain the projection length of the straight line between the current position of the unmanned ship and the target point on the projection line as the yaw distance D, and obtain the angle between the tangent line of the current position of the unmanned ship on the real navigation path and the projection line as the yaw angle α.

[0022] In the said step 500, a preset projection threshold λ and a course angle threshold are used to determine the difference between the actual navigation path of the unmanned ship and the preset path;

[0023] Adjustment of the course angle:

[0024] According to the yaw distance D and the yaw angle α of the current unmanned ship, the following judgments are made to adjust the course angle of the current unmanned ship:

[0025] If D ≤ λ or then the course angle of the current unmanned ship is not adjusted;

[0026] If D > λ and then the course angle of the current unmanned ship is adjusted so that the current unmanned ship heads towards the target point;

[0027] Adjustment of the navigation speed:

[0028] According to the yaw distance D and the yaw angle α of the current unmanned ship, the following judgments are made simultaneously to adjust the navigation speed of the current unmanned ship:

[0029] If D ≤ λ and or D ≤ λ and then in the initial path in front of the current unmanned ship, starting from the target point, two points on the initial path are taken as preview points at the same distance d 1 d 1 = v(t) * Δt, where v(t) is the navigation speed of the current unmanned ship, and Δt is the unit time of the control frequency of the control signal issued when controlling the navigation of the unmanned ship; and the following judgments are made:

[0030] If the course angles when the unmanned ship sails in a straight line from the current position to the two preview points are both less than the course angle threshold then control the current unmanned ship to accelerate;

[0031] If the course angles when the unmanned ship sails in a straight line from the current position to the two preview points are both not less than the course angle threshold or only one of the course angles is not less than the course angle threshold then keep the navigation speed v(t) of the current unmanned ship;

[0032] If D > λ, then in the initial path in front of the current unmanned ship, starting from the target point, the jth preview point is taken at a distance d according to the following formula j Take the jth preview point:

[0033]

[0034] where κ jis the curvature of the j-th preview point in the initial path, where j = 1, 2, 3;

[0035] A total of three preview points P1, P2, and P3 are taken, and the preview point distance d is calculated, d = d 1 + d 2 + d 3 ; Denote the current target point as P0, calculate the included angles α1, α2, and α3 between the heading angle of the unmanned ship and the vectors P0P1, P0P2, and P0P3 respectively, and then calculate the current heading angle of the unmanned ship according to the following formula:

[0036]

[0037] where T 0 is a preset following time constant, which is adjusted according to the actual situation of the control device that issues the control signal;

[0038] Make the following judgments according to the heading angle:

[0039] If holds, then control the current unmanned ship to decelerate, and the greater the heading angle, the greater the deceleration amplitude;

[0040] If holds, then maintain the navigation speed v(t) of the current unmanned ship.

[0041] The beneficial effects of the present invention are:

[0042] 1. The trajectory control method of following the preview point can take into account both the heading error and the trajectory error. Compared with the double-closed-loop control of the rudder angle error and the trajectory error, it has better trajectory smoothness and rudder angle stability.

[0043] 2. The positions of the preview points under different speeds and different trajectory curvatures can be adaptively adjusted to take into account both the trajectory tracking accuracy and the navigation stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 is a flowchart of the trajectory tracking method for the adaptive preview point and projection positioning;

[0045] Figure 2 is a schematic diagram of the projection length of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0046] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0047] As Figure 1 shown, the method of the present invention includes the following steps:

[0048] Step 100, preset the initial path of the unmanned ship to sail.

[0049] In step 100, the preset initial path of the unmanned ship's navigation, which is the path to be tracked by the unmanned ship, is usually a curved path composed of a series of path points or functional formulas. For example, Figure 2 As shown, taking the first path point in the initial path as the coordinate origin, the latitude line as the x-axis, and the longitude line as the y-axis, the path points of the initial path are represented as:

[0050] P(i) = [px(i), py(i)]

[0051] where P(i) is the i-th path point in the initial path, px(i) is the x-axis coordinate of the i-th path point, and py(i) is the y-axis coordinate of the i-th path point.

[0052] In step 200, control the unmanned ship to navigate according to the initial path and obtain the real-time motion state of the unmanned ship.

[0053] In step 200, the real-time motion state of the unmanned ship includes the heading angle of the unmanned ship at the current moment t, the navigation speed v(t), and the current position [x(t), y(t)]. Among them, x(t) is the x-axis coordinate of the unmanned ship at the current moment t, and y(t) is the y-axis coordinate of the unmanned ship at the current moment t.

[0054] In step 300, under ideal conditions, the unmanned ship should follow the initial path as much as possible. Under the interference of the sea surface environment, the unmanned ship will deviate from the initial path. At this time, according to the real-time motion state of the unmanned ship, judge whether the real navigation path of the unmanned ship deviates from the initial path. In step 300, judge whether the real navigation path of the unmanned ship deviates from the initial path:

[0055] If the real navigation path of the unmanned ship does not deviate from the initial path, continue to control the unmanned ship to navigate according to the initial path;

[0056] If the real navigation path of the unmanned ship deviates from the initial path, that is, there is a lateral deviation of the navigation path relative to the initial path, then make the following judgment:

[0057] If the real navigation path of the unmanned ship deviates from the initial path for the first time, traverse all the path points in the initial path and take the path point closest to the current position of the unmanned ship in front of the current unmanned ship as the target point;

[0058] If the real navigation path of the unmanned ship does not deviate from the initial path for the first time, start from the target point P(m) obtained when the unmanned ship last deviated from the initial path and traverse the initial path in front of the unmanned ship For all the path points between them, obtain the path point closest to the current position of the unmanned ship as the current target point, where P(m) is the m-th path point in the initial path, v(t) is the navigation speed of the current unmanned ship, Δt is the unit time of the control frequency of the control signal issued when controlling the navigation of the unmanned ship, and s is the average distance between the path points of the initial path.

[0059] If the navigation path deviates from the initial path, obtain the target point on the initial path.

[0060] Step 400: Obtain the yaw distance and yaw angle of the unmanned ship according to the target point.

[0061] As Figure 2 shown, in step 400, take the tangent line of the target point on the initial path as the projection line, obtain the projection length of the straight line between the current position of the unmanned ship and the target point on the projection line as the yaw distance D, and obtain the included angle between the tangent line of the current position of the unmanned ship on the actual navigation path and the projection line as the yaw angle α.

[0062] Step 500: Correct the real-time motion state of the unmanned ship according to the real-time motion state, yaw distance and yaw angle of the unmanned ship.

[0063] In step 500, preset a projection threshold λ and a heading angle threshold for judging the difference between the actual navigation path of the unmanned ship and the preset path;

[0064] Adjustment of the heading angle:

[0065] According to the yaw distance D and yaw angle α of the current unmanned ship, make the following judgments to adjust the heading angle of the current unmanned ship:

[0066] If D ≤ λ or then do not adjust the heading angle of the current unmanned ship;

[0067] If D > λ and then adjust the heading angle of the current unmanned ship so that the heading of the current unmanned ship faces the target point;

[0068] Adjustment of the navigation speed:

[0069] According to the yaw distance D and yaw angle α of the current unmanned ship, make the following judgments at the same time to adjust the navigation speed of the current unmanned ship:

[0070] If D ≤ λ and or D ≤ λ and then in the initial path in front of the current unmanned ship, starting from the target point, take two points on the initial path at the same distance d 1 as preview points, d 1= v(t) * Δt, where v(t) is the current navigation speed of the unmanned ship, and Δt is the unit time of the control frequency of the control signal issued when controlling the navigation of the unmanned ship; and the following judgments are made:

[0071] If the course angles of the unmanned ship when sailing in a straight line from the current position to these two preview points are both less than the course angle threshold Then control the current unmanned ship to accelerate;

[0072] If the course angles of the unmanned ship when sailing in a straight line from the current position to these two preview points are both not less than the course angle threshold Or only one of the course angles is not less than the course angle threshold Then maintain the navigation speed v(t) of the current unmanned ship;

[0073] If D > λ, then in the initial path in front of the current unmanned ship, starting from the target point, according to the following formula at a distance d j Take the jth preview point:

[0074]

[0075] where k j is the curvature of the jth preview point in the initial path, j = 1, 2, 3;

[0076] A total of three preview points P1, P2, and P3 are taken, and the preview point distance d is calculated, d = d 1 + d 2 + d 3 ; Denote the current target point as P0, calculate the included angles α1, α2, and α3 between the course angle of the unmanned ship and the vectors P0P1, P0P2, and P0P3 respectively, and then calculate the current course turning angle of the unmanned ship according to the following formula:

[0077]

[0078] where T 0 is a preset following time constant, which is adjusted according to the actual situation of the control device that issues the control signal;

[0079] Make the following judgments according to the course turning angle:

[0080] If At this time, control the current unmanned ship to decelerate, and the greater the course turning angle, the greater the deceleration amplitude;

[0081] If At this time, maintain the navigation speed v(t) of the current unmanned ship.

Claims

1. An unmanned ship trajectory tracking method based on adaptive preview points and projection positioning, characterized in that: Step 100, preset the initial path of the unmanned ship's navigation; In the said step 100, the preset initial path of the unmanned ship's navigation is a curved path composed of a series of path points. Taking the first path point in the initial path as the coordinate origin, the latitude line as the x-axis, and the longitude line as the y-axis, the path points of the initial path are expressed as: P(i) = [px(i), py(i)] where P(i) is the i-th path point in the initial path, px(i) is the x-axis coordinate of the i-th path point, and py(i) is the y-axis coordinate of the i-th path point; Step 200, control the unmanned ship to navigate according to the initial path, and obtain the real-time motion state of the unmanned ship; In the said step 200, the real-time motion state of the unmanned ship includes the heading angle, the navigation speed v(t) and the current position [x(t), y(t)] of the unmanned ship at the current moment t, where x(t) is the x-axis coordinate of the unmanned ship at the current moment t, and y(t) is the y-axis coordinate of the unmanned ship at the current moment t; Step 300, according to the real-time motion state of the unmanned ship, judge whether the real navigation path of the unmanned ship deviates from the initial path. If the navigation path deviates from the initial path, obtain the target point on the initial path; Step 400, obtain the yaw distance and yaw angle of the unmanned ship according to the target point; In the said step 400, take the tangent line of the target point on the initial path as the projection line, obtain the projection length of the straight line between the current position of the unmanned ship and the target point on the projection line as the yaw distance D, and obtain the included angle between the tangent line of the current position of the unmanned ship on the real navigation path and the projection line as the yaw angle α; Step 500, correct the real-time motion state of the unmanned ship according to the real-time motion state, yaw distance and yaw angle of the unmanned ship; In the described step 500, a preset projection threshold λ and a heading angle threshold are set Adjustment of the heading angle: According to the current yaw distance F and yaw angle α of the unmanned ship, make the following judgments to adjust the current heading angle of the unmanned ship: If D ≤ λ or then the course angle of the current unmanned boat is not adjusted; If D > λ and then adjust the heading angle of the current unmanned ship so that the heading of the current unmanned ship is towards the target point; Adjustment of the navigation speed: According to the current yaw distance D and yaw angle α of the unmanned ship, make the following judgments at the same time to adjust the current navigation speed of the unmanned ship: If F ≤ λ and when, or F ≤ λ and when, then in the initial path in front of the current unmanned ship, starting from the target point, take two points on the initial path at the same distance d 1 as preview points, d 1 = v(t) * Δt, where v(t) is the sailing speed of the current unmanned ship, and Δt is the unit time of the control frequency of the control signal issued when controlling the sailing of the unmanned ship; and make the following judgments: If the heading angles of the unmanned ship when sailing in a straight line from the current position to the two preview points are both less than the heading angle threshold then control the current unmanned ship to accelerate; If the heading angles of the unmanned ship when it sails in a straight line from the current position to the two preview points are both not less than the heading angle threshold or only one of the heading angles is not less than the heading angle threshold then keep the sailing speed v(t) of the current unmanned ship; If D > λ, then in the initial path in front of the current unmanned ship, starting from the target point, according to the following formula at a distance d j Take the j-th preview point: where κ j is the curvature of the j-th preview point in the initial path, j = 1, 2, 3; A total of three preview points P1, P2, and P3 are taken, and the preview point distance d is calculated, d = d 1 + d 2 + d 3 ; Denote the current target point as P0, calculate the included angles α1, α2, and α3 between the heading angle of the unmanned ship and the vectors P0P1, P0P2, and P0P3 respectively, and then calculate the current heading angle of the unmanned ship according to the following formula: where T 0 is a preset following time constant; Make the following judgments according to the heading rotation angle: If then, control the current unmanned ship to decelerate; If is satisfied, then maintain the current sailing speed v(t) of the unmanned ship.

2. The unmanned ship trajectory tracking method based on adaptive preview points and projection positioning according to claim 1, characterized in that: In the said step 300, judge whether the real navigation path of the unmanned ship deviates from the initial path: If the real navigation path of the unmanned ship does not deviate from the initial path, continue to control the unmanned ship to navigate according to the initial path; If the real navigation path of the unmanned ship deviates from the initial path, make the following judgments: If the real navigation path of the unmanned ship deviates from the initial path for the first time, traverse all the path points in the initial path, and take the path point closest to the current position of the unmanned ship in front of the current unmanned ship as the target point; If the actual navigation path of the unmanned ship is not the first deviation from the initial path, starting from the target point P(m) obtained from the previous deviation of the unmanned ship from the initial path, traverse all the path points in the initial path in front of the unmanned ship to obtain the path point closest to the current position of the unmanned ship as the current target point. Here, P(m) is the m-th path point in the initial path, v(t) is the current navigation speed of the unmanned ship, Δt is the unit time of the control frequency of the control signal issued when controlling the navigation of the unmanned ship, and s is the average distance between the path points of the initial path.

Images