Robot and method of control for an autonomous vehicle to track a path consisting of directed straight lines and circles with positional feedback and continuous curvature
First Claim
Patent Images
1. An unmanned vehicle control system using a timer-interrupt-driven operation with an interval of Δ
- t, the vehicle'"'"'s position p, and heading orientation θ
are defined in the world coordinate frame, a pair (p, θ
) of the position and orientation is denoted as the vehicle transformation, the difference between a present vehicle transformation and its next transformation is denoted as the incremental transformation, the arc length Δ
s of the trajectory of the vehicle position p generated by a vehicle motion during an interval Δ
t is denoted as the vehicle incremental movement distance, and the orientation change Δ
θ
of the vehicle heading orientation θ
generated by a vehicle motion during an interval Δ
t is denoted as the vehicle incremental heading orientation change, comprising;
a computer control system for storing predetermined path data consisting of straight lines and circles, for calculating continuous-curvature motion, for making in-motion corrections using positional feedback, and for switching between the lines and circles;
wherein the computer control system comprises;
a Vehicle-Independent Subsystem; and
a Vehicle-Dependent Subsystem connected to the Vehicle-Independent Subsystem;
wherein the Vehicle-Independent Subsystem further comprises;
a Vehicle-Configuration Estimation Subsystem connected to the Vehicle-Dependent Subsystem;
a Segment-Switching Test Subsystem connected to the Vehicle-Configuration Estimation Subsystem;
a Path Buffer Subsystem connected to the Segment-Switching Test Subsystem;
a Continuous-Curvature Motion Calculation Subsystem connected to the Vehicle-Configuration Estimation Subsystem, the Path Buffer Subsystem, and the Vehicle-Dependent Subsystem;
wherein the Vehicle-Configuration Estimation Subsystem receives input of the vehicle incremental movement distance Δ
s and vehicle incremental heading orientation change Δ
θ
from the Vehicle-Dependent Subsystem, computes the present vehicle transformation q=(p,θ
) (position and orientation), trajectory curvature κ
, and vehicle speed v, and then outputs these three items to the Continuous-Curvature Motion Calculation Subsystem and also outputs the present vehicle transformation q=(p,θ
) (position and orientation), and trajectory curvature κ
to the Segment-Switching Test Subsystem;
wherein the Segment-Switching Test Subsystem receives the input of the present vehicle transformation q=(p,θ
) and trajectory curvature κ
from the Vehicle-Configuration Estimation Subsystem and the second path segment L2 (if any) from the Path Buffer Subsystem, computes a steering function J to track a path segment Li, tests whether the signs of J(L1) and J(L2) are distinct, or J(L2)=0, and, if the test is true, sends a switching-needed command to the Path Buffer Subsystem;
wherein the Path Buffer Subsystem stores a path in a form of a list (L1, L2, . . . ) of path segments, where each path segment Li is a directed straight line segment or a directed circle segment, sends the present path segment L1 to the Continuous-Curvature Motion Calculation Subsystem, sends the next path segment L2 (if any), to the Segment-Switching Test Subsystem, and shifts the contents of the list when a switching-needed command is accepted from the Segment-Switching Test Subsystem, sending the next path segment, L2, newly defined as L1, to the Continuous-Curvature Motion Calculation Subsystem;
wherein the Continuous-Curvature Motion Calculation Subsystem receives the input of the present vehicle transformation q=(p,θ
), trajectory curvature κ
, and vehicle speed υ
from the Vehicle-Configuration Estimation Subsystem, and receives the present path segment L1 input from the Path Buffer Subsystem, computing a commanded path curvature κ
c and commanded speed υ
c, and sending these two values to the Vehicle-Dependent Subsystem; and
wherein the Vehicle-Dependent Subsystem receives the commanded path curvature κ
c and commanded speed υ
c from the Continuous-Curvature Motion Calculation Subsystem, executes low-level control tasks for motors, detects the vehicle incremental movement distance Δ
s and vehicle incremental heading orientation change Δ
θ
, and sends them to the Vehicle-Configuration Estimation Subsystem.
1 Assignment
0 Petitions
Accused Products
Abstract
A vehicle control system having a computer control system, wherein the computer control system comprises a Vehicle-Independent Subsystem and a Vehicle-Dependent Subsystem, and wherein the Vehicle-Independent Subsystem inputs vehicle incremental movement distance and vehicle incremental heading-orientation change from the Vehicle-Dependent Subsystem and outputs commanded path curvature and commanded speed to the Vehicle-Dependent Subsystem, and wherein the Vehicle-Dependent Subsystem inputs the commanded path curvature and commanded speed from the Vehicle-Independent Subsystem, and outputs the incremental vehicle movement distance and vehicle heading-orientation change to the Vehicle-Independent Subsystem, wherein the computer control system has the capability of controlling the execution steps at a constant sampling time interval so that the vehicle system can track a path consisting of lines and/or circular arcs with positional feedback and continuous curvature.
67 Citations
7 Claims
-
1. An unmanned vehicle control system using a timer-interrupt-driven operation with an interval of Δ
- t, the vehicle'"'"'s position p, and heading orientation θ
are defined in the world coordinate frame, a pair (p, θ
) of the position and orientation is denoted as the vehicle transformation, the difference between a present vehicle transformation and its next transformation is denoted as the incremental transformation, the arc length Δ
s of the trajectory of the vehicle position p generated by a vehicle motion during an interval Δ
t is denoted as the vehicle incremental movement distance, and the orientation change Δ
θ
of the vehicle heading orientation θ
generated by a vehicle motion during an interval Δ
t is denoted as the vehicle incremental heading orientation change, comprising;a computer control system for storing predetermined path data consisting of straight lines and circles, for calculating continuous-curvature motion, for making in-motion corrections using positional feedback, and for switching between the lines and circles;
wherein the computer control system comprises;a Vehicle-Independent Subsystem; and a Vehicle-Dependent Subsystem connected to the Vehicle-Independent Subsystem; wherein the Vehicle-Independent Subsystem further comprises; a Vehicle-Configuration Estimation Subsystem connected to the Vehicle-Dependent Subsystem; a Segment-Switching Test Subsystem connected to the Vehicle-Configuration Estimation Subsystem; a Path Buffer Subsystem connected to the Segment-Switching Test Subsystem; a Continuous-Curvature Motion Calculation Subsystem connected to the Vehicle-Configuration Estimation Subsystem, the Path Buffer Subsystem, and the Vehicle-Dependent Subsystem; wherein the Vehicle-Configuration Estimation Subsystem receives input of the vehicle incremental movement distance Δ
s and vehicle incremental heading orientation change Δ
θ
from the Vehicle-Dependent Subsystem, computes the present vehicle transformation q=(p,θ
) (position and orientation), trajectory curvature κ
, and vehicle speed v, and then outputs these three items to the Continuous-Curvature Motion Calculation Subsystem and also outputs the present vehicle transformation q=(p,θ
) (position and orientation), and trajectory curvature κ
to the Segment-Switching Test Subsystem;wherein the Segment-Switching Test Subsystem receives the input of the present vehicle transformation q=(p,θ
) and trajectory curvature κ
from the Vehicle-Configuration Estimation Subsystem and the second path segment L2 (if any) from the Path Buffer Subsystem, computes a steering function J to track a path segment Li, tests whether the signs of J(L1) and J(L2) are distinct, or J(L2)=0, and, if the test is true, sends a switching-needed command to the Path Buffer Subsystem;wherein the Path Buffer Subsystem stores a path in a form of a list (L1, L2, . . . ) of path segments, where each path segment Li is a directed straight line segment or a directed circle segment, sends the present path segment L1 to the Continuous-Curvature Motion Calculation Subsystem, sends the next path segment L2 (if any), to the Segment-Switching Test Subsystem, and shifts the contents of the list when a switching-needed command is accepted from the Segment-Switching Test Subsystem, sending the next path segment, L2, newly defined as L1, to the Continuous-Curvature Motion Calculation Subsystem; wherein the Continuous-Curvature Motion Calculation Subsystem receives the input of the present vehicle transformation q=(p,θ
), trajectory curvature κ
, and vehicle speed υ
from the Vehicle-Configuration Estimation Subsystem, and receives the present path segment L1 input from the Path Buffer Subsystem, computing a commanded path curvature κ
c and commanded speed υ
c, and sending these two values to the Vehicle-Dependent Subsystem; andwherein the Vehicle-Dependent Subsystem receives the commanded path curvature κ
c and commanded speed υ
c from the Continuous-Curvature Motion Calculation Subsystem, executes low-level control tasks for motors, detects the vehicle incremental movement distance Δ
s and vehicle incremental heading orientation change Δ
θ
, and sends them to the Vehicle-Configuration Estimation Subsystem. - View Dependent Claims (2, 3, 4, 5)
- t, the vehicle'"'"'s position p, and heading orientation θ
-
6. An unmanned vehicle control system using a timer-interrupt-driven operation with an interval of Δ
- t, a vehicle'"'"'s position p and a heading orientation θ
are defined in a world coordinate frame, a pair (p, θ
) of a position and an orientation is denoted as a vehicle transformation, the difference between a present vehicle transformation and its next transformation is denoted as an incremental transformation, an arc length Δ
s of the trajectory of the vehicle position p generated by a vehicle motion during an interval Δ
t is denoted as a vehicle incremental movement distance, and the orientation change Δ
θ
of the vehicle heading orientation θ
generated by a vehicle motion during an interval Δ
t is denoted as a vehicle incremental heading orientation change, comprising;a computer control system for storing predetermined path data consisting of straight lines and circles, for calculating continuous-curvature motion, for making in-motion corrections using positional feedback, and for switching between the lines and circles; wherein the computer control system comprises; a Vehicle-Independent Subsystem; and a Vehicle-Dependent Subsystem connected to the Vehicle-Independent Subsystem; wherein the Vehicle-Independent Subsystem further comprises; a Vehicle-Configuration Estimation Subsystem; a Continuous-Curvature Motion Calculation Subsystem connected to the Vehicle-Configuration Estimation Subsystem; a Segment-Switching Test Subsystem connected between the Vehicle-Configuration Estimation Subsystem and the Continuous-Curvature Motion Calculation Subsystem; a Path Buffer Subsystem connected to the Segment-Switching Test Subsystem and the Continuous-Curvature Motion Calculation Subsystem; wherein the Vehicle-Configuration Estimation Subsystem receives input of the vehicle incremental movement distance Δ
s and vehicle incremental heading orientation change Δ
θ
from the Vehicle-Dependent Subsystem, computes the present vehicle transformation q=(p,θ
) (position and orientation), trajectory curvature κ
, and vehicle speed υ
, and then outputs these three items to the Continuous-Curvature Motion Calculation Subsystem and also outputs the present vehicle transformation q=(p,θ
) (position and orientation), and trajectory curvature κ
to the Segment-Switching Test Subsystem,wherein the incremental transformation, Δ
q=(Δ
x, Δ
y, Δ
θ
), from the previous positioning to the final positioning, is evaluated from Δ
s and Δ
θ
as ##EQU47## if Δ
θ
≠
0, and
space="preserve" listing-type="equation">Δ
q=(Δ
s,0,0)if Δ
θ
=0;wherein, if the vehicle'"'"'s previous transformation is q and the incremental transformation is Δ
q, the new vehicle transformation is the composition q·
Δ
q, is calculated as
space="preserve" listing-type="equation">q←
q·
Δ
q=(x, y, θ
)·
(Δ
x, Δ
y, Δ
θ
)=(x+Δ
x cos θ
-Δ
y sin θ
, y+Δ
x sin θ
+Δ
y cos θ
, θ
+Δ
θ
);wherein the present vehicle path curvature κ
is calculated by ##EQU48## wherein the present vehicle speed υ
is calculated by ##EQU49## wherein the Segment-Switching Test Subsystem receives the input of the present vehicle transformation q=(p,θ
) and trajectory curvature κ
from the Vehicle-Configuration Estimation Subsystem and receives the second path segment L2 (if any) from the Path Buffer Subsystem, computes a steering function J to track a path segment Li, tests whether the signs of J(L1) and J(L2) are distinct, or J(L2)=0, and, if the test is true, sends a switching-needed command to the Path Buffer Subsystem;wherein the Path Buffer Subsystem stores a path in a form of a list (L1, L2, . . . ) of path segments, where each path segment Li is a directed straight line segment or a directed circle segment, sends the present path segment L1 to the Continuous-Curvature Motion Calculation Subsystem, sends the next path segment L2 (if any), to the Segment-Switching Test Subsystem, and shifts the contents of the list when a switching-needed command is accepted from the Segment-Switching Test Subsystem by discarding L1 moving the old L2 into the new L1, moving the old L3 into new L2, and so forth; wherein the Continuous-Curvature Motion Calculation Subsystem receives the input of the present vehicle transformation q=(p,θ
), trajectory curvature κ
, and vehicle speed υ
from the Vehicle-Configuration Estimation Subsytem, and receives the present path segment L1 input from the Path Buffer Subsystem, computes a steering function J to track a present segment L1 by ##EQU50## where κ
is the curvature, θ
the heading of the vehicle respectively, κ
1 the curvature of segment L1, the tangential orientation of segment L1 at its closest point from the vehicle, the Δ
d1 closest distance from the vehicle to segment L1, where a, b, and c are positive constants determined by another positive paramenter k and said curvature κ
1 as follows;
space="preserve" listing-type="equation">a=3k
space="preserve" listing-type="equation">b=3k.sup.2 -κ
.sub.1.sup.2
space="preserve" listing-type="equation">c=k.sup.3 -3kκ
.sub.1.sup.2 ;and wherein the Vehicle Dependent Subsystem receives the commanded path curvature κ
c and commanded speed υ
c from the Continuous-Curvature Motion Caculation Subsystem, executes low-level control tasks for motors, detects the vehicle incremental movement distance Δ
s and vehicle incremental heading orientation change Δ
θ
, and sends them to the Vehicle-Configuration Estimation Subsystem.
- t, a vehicle'"'"'s position p and a heading orientation θ
-
7. A method of controlling an unmanned vehicle control system having a computer control system for storing predetermined path data consisting of straight lines and circles, for calculating continuous-curvature motion, for making in-motion corrections using positional feedback, and for switching between the lines and circles, wherein the computer system has a Vehicle-Independent Subsystem and a Vehicle-Dependent Subsystem, and the Vehicle-Independent Subsystem has a Vehicle-Configuration Estimation Subsystem, a Segment-Switching Test Subsystem, a Path Buffer Subsystem, and a Continuous-Curvature Motion Calculation Subsystem where the method adopts a timer-interrupt-driven operation with an interval of Δ
- t, a vehicle'"'"'s position p and a heading orientation θ
are defined in a world coordinate frame, an arc length Δ
s of a trajectory of the vehicle position p generated by a vehicle motion during an interval Δ
t is denoted as a vehicle incremental movement distance, and an orientation change Δ
θ
of the vehicle heading orientation θ
generated by a vehicle motion during an interval Δ
t is denoted as a vehicle incremental heading orientation change, the method comprising;storing a path in the form of a list (L1, L2, . . . ) of path segments in the Path Buffer Subsystem, where each path segment Li is a directed straight line segment or a directed circle segment, receiving the vehicle incremental movement distance Δ
s and vehicle incremental heading orientation change Δ
θ
, into the Vehicle-Configuration Estimation Subsystem;computing an incremental transformation Δ
q=(Δ
x, Δ
y, Δ
θ
), from the previous positioning to the final positioning, evaluated from Δ
s and Δ
θ
as ##EQU51## if Δ
θ
≠
0, and
space="preserve" listing-type="equation">Δ
q=(Δ
s, 0,
0)if Δ
θ
=0,wherein an ordinary situation, Δ
s and Δ
θ
are very small, and hence Δ
q is also tiny, and if the vehicle'"'"'s previous transformation is q and the incremental transformation is Δ
q, the new vehicle transformation is the composition q·
Δ
q;calculating this composition q·
Δ
q as
space="preserve" listing-type="equation">q←
q·
Δ
q=(x, y, θ
)·
(Δ
x, Δ
y, Δ
θ
)=(x+Δ
x cos θ
-Δ
y sin θ
, y+Δ
x sin θ
+Δ
y cos θ
, θ
+Δ
θ
);calculating the present vehicle path curvature κ
by ##EQU52## calculating the present vehicle speed υ
by ##EQU53## sending the present vehicle speed υ
, path curvature κ
, and transformation data q, to the Continuous-Curvature Motion Calculation Subsystem at the same time as the path curvature κ and
transformation data q are sent to the Segment-Switching Test Subsystem;sending the next path segment L2 (if any), from the Path Buffer Subsystem to the Segment-Switching Test Subsystem; receiving into the Segment-Switching Test Subsystem the input of the present vehicle transformation q=(p,θ
) and trajectory curvature κ
from the Vehicle-Configuration Estimation Subsystem and the second path segment L2 (if any) from the Path Buffer Subsystem;calculating the steering function J(L2), if L2 exits, ##EQU54## wherein a, b, and c are defined through a single positive constant k as follows;
space="preserve" listing-type="equation">a=3k
space="preserve" listing-type="equation">b=3k.sup.2 -κ
.sub.1.sup.2
space="preserve" listing-type="equation">c=k.sup.3 -3kκ
.sub.1.sup.2,the reciprocal of k is denoted by σ and
is called smoothness;
##EQU55## testing in the Segment-Switching Test Subsystem to see if J(L2)=0 or J(L2) crosses the zero value;
sending a switching-needed command to the Path Buffer Subsystem if the previous test is positive;shifting the contents of the list when a switching-needed command is accepted from the Segment-Switching Test Subsystem, by discarding L1, moving the old L2 into new L1, moving the old L3 into new L2, and so forth; sending the present path segment L1 from the Path Buffer Subsystem to the Continuous-Curvature Motion Calculation Subsystem; receiving into the Continuous-Curvature Motion Calculation Subsystem (i) the present path segment L1 from the Path Buffer Subsystem and (ii) the input of the present vehicle transformation q=(p,θ
), trajectory curvature κ
, and vehicle speed υ
from the Vehicle-Configuration Estimation Subsystem;computing the steering function J(L1) to track a segment L1, is ##EQU56## wherein a, b, and c are defined through a single positive constant k as follows;
space="preserve" listing-type="equation">a=3k
space="preserve" listing-type="equation">b=3k.sup.2 -κ
.sub.1.sup.2
space="preserve" listing-type="equation">c=k.sup.3 -3kκ
.sub.1.sup.2,the reciprocal of k is denoted by σ and
is called smoothness;
##EQU57## calculating the commanded vehicle speed υ
c and the commanded vehicle path curvature
space="preserve" listing-type="equation">κ
.sub.c as κ
.sub.c =κ
+J(L.sub.1)·
Δ
s;sending the commanded vehicle speed υ
c and the commanded vehicle path curvature κ
c from the Continuous-Curvature Motion Calculation Subsystem to the Vehicle-Dependent Subsystem for executing the commands; andreceiving into the Vehicle-Dependent Subsystem and responding to the commanded path curvature κ
c and commanded speed υ
c, performing low-level control tasks for motors, and responding to these data and computing the vehicle incremental movement distance Δ
s and vehicle incremental heading orientation change Δ
θ
, and sending Δ
s and Δ
θ
as feedback to the Vehicle-Configuration Estimation Subsystem.
- t, a vehicle'"'"'s position p and a heading orientation θ
Specification
-
- Resources
Litigation Campaign Assessment
Thank you for your request. You will receive a custom alert email when the Litigation Campaign Assessment is available.
×
-
Current AssigneeThe United States of America As Represented By The Secretary of Agriculture
-
Original Assigneethe united states of america as represented by the secretary of the navy
-
InventorsKanayama, Yutaka John
-
Primary Examiner(s)Zanelli, Michael J.
-
Application NumberUS09/071,451Time in Patent Office911 DaysField of Search701/23, 701/25, 701/26, 701/217, 701/300, 701/301, 901/1, 318/587US Class Current701/23CPC Class CodesG05D 1/0212 with means for defining a d...
