CN108107890B - A kind of safe trajectory planing method of Nonlinear Uncertain Systems - Google Patents

A kind of safe trajectory planing method of Nonlinear Uncertain Systems Download PDF

Info

Publication number
CN108107890B
CN108107890B CN201711308943.0A CN201711308943A CN108107890B CN 108107890 B CN108107890 B CN 108107890B CN 201711308943 A CN201711308943 A CN 201711308943A CN 108107890 B CN108107890 B CN 108107890B
Authority
CN
China
Prior art keywords
error
nonlinear
algorithm
path
value
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201711308943.0A
Other languages
Chinese (zh)
Other versions
CN108107890A (en
Inventor
罗建军
高登巍
袁建平
马卫华
王明明
朱战霞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwest University of Technology
Original Assignee
Northwest University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Northwest University of Technology filed Critical Northwest University of Technology
Priority to CN201711308943.0A priority Critical patent/CN108107890B/en
Publication of CN108107890A publication Critical patent/CN108107890A/en
Application granted granted Critical
Publication of CN108107890B publication Critical patent/CN108107890B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)
  • Feedback Control In General (AREA)

Abstract

The invention discloses a kind of safe trajectory planing methods of Nonlinear Uncertain Systems, the present invention is a kind of new nonlinear kinetics paths planning method, consider measurement and systematic error, it proposes and local feedback control invariant set size is corrected using the method for huge Baudrillard gold collection difference again, and carry out the algorithm of path planning online.Specific steps include: Step 1: open loop track library calculates;Step 2: calculating local invariant collection;Step 3: calculating error propagation;Step 4: path planning.From simulation result it can be seen that the validity of method proposed by the invention and algorithm, the present invention can form the secure path of Global robust, to meet the requirement of Nonlinear Uncertain Systems safe trajectory planning.

Description

A kind of safe trajectory planing method of Nonlinear Uncertain Systems
Technical field
The present invention relates to nonlinear system path planning and dynamics Controlling field, in particular to a kind of nonlinear uncertain The safe trajectory planing method of system.
Background technique
If robot is cannot carry out path planning in the case where oneself fully known state, using algorithm for estimating come pre- Possible path is surveyed to find safety and the lesser path of error, such as BRM (belief roadmap) algorithm, RRBT (Rapidly-exploring Random Belief Trees) algorithm.Compared to traditional routing algorithm, consider it is uncertain because Element provides a good guarantee for safe path, avoids because probabilistic have what influence physical planning path was run Safety.But dynamic system does not ensure that the operation of safe trajectory, such as executive capability are saturated sometimes, system deviation etc., Therefore it has been born and has met the path planning algorithms of Dynamic Constraints such as.And for nonlinear system, meet linear control method Dynamic Prediction can be only present in the neighborhood of very little, Russ professor Tedrake in the laboratory EECS of MIT proposes to use LQR-trees method is converted the local linear feedback control overall situation for nonlinear Control problem and is connected using random search tree algorithm The form connect forms the suboptimum track for meeting the requirement of dynamics feedback control, this is a kind of nonlinear control algorithm.Domain of attraction The feasibility problems of local linearization are solved perfectly with the introducing of invariant set, Existence of Global Stable is completed in hybrid system switching.It is non-thread Property system attractive domain and invariant set calculating it is very time-consuming, therefore A.Majumda introduces path library and carries out off-line calculation, and adopts Combined sequence method case in carries out merging the connection for completing invariant set online.
Summary of the invention
In order to overcome disadvantage mentioned above, the present invention provides a kind of new nonlinear kinetics paths planning method, this method Consider feedback path planning in existing measuring uncertainty, and in the case that error there are feedback control still can be stable arrive Up to dbjective state.
The present invention is to be achieved through the following technical solutions:
A kind of safe trajectory planing method of Nonlinear Uncertain Systems, comprising the following steps:
Step 1: open loop track library calculates
Path point is generated using PRM method at random, designs attachable kinetic locus for adjacent path point line, it is raw It is nonlinear programming problem at local path library, and carries out that open loop track and opened loop control, open loop track and open loop is calculated Control constitutes track library;
Step 2: calculating local invariant collection
Off-line calculation is carried out using the algorithm of nonlinear system domain of attraction and invariant set, is obtained for the peace for determining reachable set The range of full stability-of-path;
Step 3: calculating error propagation
First by Nonlinear Systems ' Discrete, partial feedback coefficient K is obtained by LQR algorithmkControl rate is obtained, is missed Poor elliptic equation;
Step 4: path planning
Pipeline is in tkValue E (the t at momentk) and error ellipse ε (0, Pk) collection difference operation is carried out, it obtains credible under error condition Pipeline, and the logical estimated value provided detects whether the inlet that true value section can be allowed be in next pipeline composition sequence Connection, and complete path planning splicing.
In step 2, off-line calculation is carried out using SOS algorithm, for all tracks in step 1 in the library of track, is solved Time-varying Riccati equation obtains the lyapunov function in time interval, the initial value as SOS algorithm;And then it obtains for true Determine the stable range of the safe trajectory of reachable set.
In step 2, time-varying Riccati equation are as follows:
Wherein S (t) is matrix to be solved, and A (t) is the later coefficient matrix of system linearization, and Q, R are design parameter square Battle array.
Error ellipse equation specific steps are obtained in step 3 are as follows:
Definition: oval ε (q, Q) indicates that q is oval center of circle phasor coordinate, and Q is elliptic parameter, is poised for battle matrix for positive definite. And(<>indicates inner product);
First by Nonlinear Systems ' Discrete:
xk+1=g (xk,uk,wk)
yk+1=h (xk,vk)
Wherein, wkAnd vk+1Respectively system noise and observation noise.X and y is respectively state variable and observed quantity variable, u For system input.Partial feedback coefficient K is obtained by LQR liner quadratic regulator algorithmkObtain control rate:
Assuming thatFor the error of state actual value and estimated value, error ellipse:
ee∈ε(0,Pk)
PkFor posteriori error estimate, it is assumed thatFor the error of state estimation and nominal state,
Wherein AkFor system linearization coefficient, HkFor the Jacobian matrix of observational equation.Lk+1For filtering gain.It enablesVirtual condition indicates are as follows:
So that
Wherein: AG=Ak+BkKk, Therefore the predicted value nominally estimated:Wherein elliptic parameter are as follows:
In step 4, specific algorithm is as follows:
V, n indicate that random walk node, Q indicate that data structure storehouse, v.path indicate to reach the path of present node, road Diameter by dynamics estimation be attached, be stored in v [MT], MT be a tuple { τ, F, c, u } respectively indicate track, Funnel, consuming value, control force }, FORECAST (n, v) is the calculating of two node error propagations, obtains new contain not really Qualitative funnel F, updates the value in v [MT], SEQCOMPOSITION (F1,F2) indicate two funnel sequence connection meters It calculates, can connect return 1 otherwise is 0.
Compared with prior art, the invention has the following beneficial technical effects:
The present invention considers influence of the measuring uncertainty for feedback control stability region, is contracted using huge Baudrillard gold collection difference The stabilization of system, reappraises path using estimation of deviation in the case of the size in small practical stability region exists with error in judgement Safe avoidance problem in planning process, and optimal trajectory is searched for using path search algorithm.Specifically, using off-line calculation Mode of the motor pool in splicing and detection, road of the design nonlinear motion robot under constraint and measurement uncertain condition Diameter forms a kind of safe trajectory planing method of Nonlinear Uncertain Systems.This method is existing in considering feedback path planning Measuring uncertainty, and in the case that error there are feedback control still can be stable arrival dbjective state.It can from simulation result To find out that the validity of method proposed by the invention and algorithm, the present invention can form the secure path of Global robust, thus full The requirement of sufficient Nonlinear Uncertain Systems safe trajectory planning.
Detailed description of the invention
Fig. 1 is path planning graph and kinetic locus;
Fig. 2 is PRM node and most short kinetic pathways;
Fig. 3 is most short safe trajectory;
Fig. 4 is safe trajectory when error increases;
Fig. 5 is the safe trajectory when local measurement missing.
Specific embodiment
With reference to the accompanying drawing and specific embodiment the present invention will be further described.
The present invention by path planning measuring uncertainty and newest dynamics program results combine, propose one kind Feedback path based on probabilistic roadmap method plans (FBRM, Feedback Belief Roadmap), using off-line calculation motor pool In the mode of splicing and detection, designs nonlinear motion robot and constraining and measuring the path under uncertain condition, shape At a kind of safe trajectory planing method of Nonlinear Uncertain Systems.Consider measurement and systematic error, proposes using Pang Deli The method of sub- gold collection difference corrects local feedback control invariant set size again, and carries out the algorithm of path planning online.This method Including following four step:
Step 1: open loop track library calculates
Path point is generated using PRM method at random, designs attachable dynamics for adjacent path point line (edge) Track.Generation local path library is nonlinear programming problem (NLP, nonlinear programming), soft using GPOPS-II Part carries out that open loop track and opened loop control is calculated, these tracks and control constitute track library.As shown in Figure 1.
Step 2: calculating local invariant collection
There are many algorithm of nonlinear system domain of attraction and invariant set, and the present invention uses SOS (sums of squares) algorithm Carry out off-line calculation.Firstly, solving time-varying Riccati equation for all tracks in track library in step 1:Obtain the lyapunov function S in time interval t ∈ [0 T] (t), the initial value as SOS algorithm.SOS algorithm is available for the stable range of the safe trajectory of reachable set is determined, this can Be expressed as a pipeline (funnel) with image, into funnel stateful be proved to that specific objective collection can be reached State.The corresponding corresponding funnel in track in track library each in this way.
Step 3: calculating error propagation
Definition: oval ε (q, Q) indicates that q is the oval center of circle, and(<>table Show inner product).
First by Nonlinear Systems ' Discrete:
Wherein wkAnd vk+1Respectively system noise and observation noise.Partial feedback coefficient K is obtained by LQR algorithmkIt obtains Control rate:
Assuming thatFor the error of actual value and estimated value, error ellipse:
ee∈ε(0,Pk) (3)
PkFor posteriori error estimate, ε () indicates oval.Posterior estimator error can be using ESMF (extension set-membership filtering) Or EKF (Extended Kalman filter) and other filtering algorithms obtain.Assuming thatFor optimal estimation and nominal state Error.
It enablesSo virtual condition can indicate are as follows:
So that
Wherein: AG=Ak+BkKk, Therefore the predicted value nominally estimated:Wherein elliptic parameter:
Step 4: path planning
Funnel is in tkValue E (the t at momentk) and error ellipse ε (0, Pk) carry out the poor (Pontryagin of collection Difference) operation obtains believable funnel under error condition, and being detected whether by the estimated value of (7) offer can be with The inlet for allowing true value section to be in next funnel constitutes sequence connection (Sequential composition), and complete Splice at path planning.Specific algorithm is as follows:
V, n indicate that random walk node, Q indicate that data structure storehouse, v.path indicate to reach the path of present node, this A little paths are attached by dynamics estimation, are stored in v [MT], and MT is that a tuple { τ, F, c, u } respectively indicates { rail Mark, funnel, consuming value, control force }, FORECAST (n, v) is the calculating (step 3) of two node error propagations, is obtained new Contain probabilistic funnel F, update v [MT] in value.SEQCOMPOSITION(F1,F2) indicate two funnel sequences Column connection calculates, and can connect return 1 otherwise is 0.
By taking ground mobile robot as an example.Assuming that the forward movement speed of ground robot specifications 10m/s, is turned by adjusting To allowing robot to carry out safe avoidance, as shown in Figure 2-5:
As shown in Figure 2-5, black region is constraint, and gray area funnel, dotted line is the edge of connecting node, Pore is node, and black fine line is kinetic locus, and yellow line indicates previous funnel exit region, under green ellipse representation A funnel entrance.Fig. 2 is shown in the map there are barrier in the case where not considering error along kinetic locus Shortest path, and motion profile will appear deviation in the case where there are noise, therefore in the case where no preferably measurement This track may stabilization may also rapid divergence to it cannot be guaranteed that safety.It is available very small in error by this method In the case where run along most short safe trajectory, and be constantly in funnel because of the presence of feedback control, such as Fig. 3 institute Show.
In the biggish situation of initial error, for shortest path because not measuring, error ellipse has exceeded funnel Constraint, obtain uncertain state by collecting difference and cannot keep in funnel.Therefore path planning side proposed by the present invention Method gives the shortest path of new guarantee safety, as shown in Figure 4.Because this paths is there are measuring node dark color dot, from And guarantees that error is limited in funnel and ensure that safety.But if remove one of measurement point as shown in figure 5, because surveying Amount can not reduce uncertainty in original route, therefore intermediate path can not ensure that control is stablized, therefore also not can guarantee rail Mark safety, as shown in figure 5, the track that method of the invention will select another measurement more accurate but distant.From contracting It can be seen that the deviation ellipse of estimation has been over the inlet radius containing probabilistic new funnel, therefore nothing in sketch map Method reaches target trajectory.
From simulation result it can be seen that the validity of method proposed by the invention and algorithm.The present invention can form global Shandong The secure path of stick, to meet the requirement of Nonlinear Uncertain Systems safe trajectory planning.
More than, only presently preferred embodiments of the present invention is not limited only to practical range of the invention, all according to the invention patent The equivalence changes and modification that the content of range is done all should be technology scope of the invention.

Claims (2)

1. a kind of safe trajectory planing method of Nonlinear Uncertain Systems, which comprises the following steps:
Step 1: open loop track library calculates
Path point is generated using PRM method at random, designs attachable kinetic locus, generation office for adjacent path point line Portion, library, track is nonlinear programming problem, and carries out that open loop track and opened loop control, open loop track and opened loop control is calculated Constitute track library;
Step 2: calculating local invariant collection
Off-line calculation is carried out using the algorithm of nonlinear system domain of attraction and invariant set, is obtained for the safe rail for determining reachable set The stable range of mark;
Step 3: calculating error propagation
First by Nonlinear Systems ' Discrete, partial feedback coefficient K is obtained by LQR algorithmkControl rate is obtained, it is ellipse to obtain error Equation of a circle;
Step 4: path planning
Pipeline is in tkValue E (the t at momentk) and error ellipse ε (0, Pk) collection difference operation is carried out, obtain believable pipe under error condition Road, and the logical estimated value provided detects whether that the inlet that true value section can be allowed to be in next pipeline constitutes sequence and connects It connects, and completes path planning splicing;
In step 2, off-line calculation is carried out using SOS algorithm, for all tracks in step 1 in the library of track, solves time-varying Riccati equation obtains the lyapunov function in time interval, the initial value as SOS algorithm;Obtaining in turn can for determination The range stable up to the safe trajectory of collection;
In step 2, time-varying Riccati equation are as follows:
Wherein S (t) is matrix to be solved, and A (t) is the later coefficient matrix of system linearization, and Q, R are design parameter matrix;
Error ellipse equation specific steps are obtained in step 3 are as follows:
Definition: oval ε (q, Q) indicates that q is oval center of circle phasor coordinate, and Q is elliptic parameter, is poised for battle matrix for positive definite, and<>indicates inner product;
First by Nonlinear Systems ' Discrete:
xk+1=g (xk,uk,wk)
yk+1=h (xk,vk)
Wherein, wkAnd vk+1Respectively system noise and observation noise, x and y are respectively state variable and observed quantity variable, and u is to be System input, obtains partial feedback coefficient K by LQR liner quadratic regulator algorithmkObtain control rate:
Assuming thatFor the error of state actual value and estimated value, error ellipse:
ee∈ε(0,Pk)
PkFor posteriori error estimate, it is assumed thatFor the error of state estimation and nominal state,
Wherein AkFor system linearization coefficient, HkFor the Jacobian matrix of observational equation, Lk+1For filtering gain;It enablesVirtual condition indicates are as follows:
So that
Wherein: AG=Ak+BkKk, Therefore the predicted value nominally estimated:Wherein elliptic parameter are as follows:
2. a kind of safe trajectory planing method of Nonlinear Uncertain Systems according to claim 1, which is characterized in that step In rapid four, specific algorithm is as follows:
V, n indicate that random walk node, Q indicate that data structure storehouse, v.path indicate to reach the path of present node, and path is logical Cross dynamics estimation be attached, be stored in v [MT], MT be a tuple { τ, F, c, u } respectively indicate track, funnel, Consuming value, control force }, FORECAST (n, v) is the calculating of two node error propagations, is obtained new containing probabilistic Funnel F updates the value in v [MT], SEQCOMPOSITION (F1,F2) indicate that two funnel sequence connections calculate, it can connect Connecing return 1 otherwise is 0.
CN201711308943.0A 2017-12-11 2017-12-11 A kind of safe trajectory planing method of Nonlinear Uncertain Systems Active CN108107890B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711308943.0A CN108107890B (en) 2017-12-11 2017-12-11 A kind of safe trajectory planing method of Nonlinear Uncertain Systems

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711308943.0A CN108107890B (en) 2017-12-11 2017-12-11 A kind of safe trajectory planing method of Nonlinear Uncertain Systems

Publications (2)

Publication Number Publication Date
CN108107890A CN108107890A (en) 2018-06-01
CN108107890B true CN108107890B (en) 2019-11-15

Family

ID=62209640

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711308943.0A Active CN108107890B (en) 2017-12-11 2017-12-11 A kind of safe trajectory planing method of Nonlinear Uncertain Systems

Country Status (1)

Country Link
CN (1) CN108107890B (en)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109102194B (en) * 2018-08-20 2021-06-18 安徽佳略信息科技有限公司 Driving behavior scoring method based on global positioning system and inertial sensor
CN109669459B (en) * 2018-12-28 2022-05-10 西北工业大学 Dynamic feedback path planning method adopting invariant set
CN109708643B (en) * 2019-01-14 2020-07-07 北京理工大学 Evaluation and selection method for asteroid surface optical navigation road sign
CN111123701B (en) * 2019-11-27 2021-07-06 武汉理工大学 Automatic driving path tracking anti-interference control method based on pipeline prediction model

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100869922B1 (en) * 2006-05-12 2008-11-21 삼성전자주식회사 Apparatus and method for controlling uplink power in broadband wireless communication terminal
US9499197B2 (en) * 2014-10-15 2016-11-22 Hua-Chuang Automobile Information Technical Center Co., Ltd. System and method for vehicle steering control
CN104567907A (en) * 2015-01-22 2015-04-29 四川汇源吉迅数码科技有限公司 Method for real-time path planning based on dynamic feedback
CN104808671B (en) * 2015-05-19 2017-03-15 东南大学 A kind of robot path planning method under domestic environment
CN204615704U (en) * 2015-05-28 2015-09-02 常州先进制造技术研究所 A kind of modularization controllor for step-by-step motor based on ARM platform
CN106896812B (en) * 2017-01-11 2019-11-15 西北工业大学 A kind of feedback path planing method there are under measuring uncertainty

Also Published As

Publication number Publication date
CN108107890A (en) 2018-06-01

Similar Documents

Publication Publication Date Title
CN108107890B (en) A kind of safe trajectory planing method of Nonlinear Uncertain Systems
Estrada et al. Hierarchical SLAM: Real-time accurate mapping of large environments
CN106197428B (en) A kind of SLAM method using metrical information Optimum distribution formula EKF estimation procedure
KR20180052636A (en) Automated map generation for mobile device navigation, tracking and positioning in GPS denied or inaccurate regions
CN106772524B (en) A kind of agricultural robot integrated navigation information fusion method based on order filtering
CN106444835A (en) Underwater vehicle three-dimensional path planning method based on Lazy Theta satellite and particle swarm hybrid algorithm
KR20120069335A (en) Robot and method for planning path of the same
CN109211246B (en) Planet landing trajectory planning method under uncertain environment
CN106679672A (en) AGV (Automatic Guided Vehicle) location algorithm based on DBN (Dynamic Bayesian Network) and Kalman filtering algorithm
CN108873915A (en) Dynamic obstacle avoidance method and its omnidirectional&#39;s security robot
CN108871341A (en) A kind of concurrently positioning of global optimization and build drawing method
Tang et al. OdoNet: Untethered speed aiding for vehicle navigation without hardware wheeled odometer
CN106896812B (en) A kind of feedback path planing method there are under measuring uncertainty
CN109855623A (en) Geomagnetic model online approximating method based on Legendre multinomial and BP neural network
Noormohammadi-Asl et al. Multi-goal motion planning using traveling salesman problem in belief space
Belhajem et al. A robust low cost approach for real time car positioning in a smart city using Extended Kalman Filter and evolutionary machine learning
CN109188352B (en) Combined navigation relative positioning method
Obregón et al. Precise positioning and heading for autonomous scouting robots in a harsh environment
Elisha et al. Active online visual-inertial navigation and sensor calibration via belief space planning and factor graph based incremental smoothing
Lu et al. An information potential approach for tracking and surveilling multiple moving targets using mobile sensor agents
Zhang et al. Map matching in road crossings of urban canyons based on road traverses and linear heading-change model
AU2021273605B2 (en) Multi-agent map generation
Crocoll et al. Laser-aided navigation with loop closure capabilities for Micro Aerial Vehicles in indoor and urban environments
Li et al. An enhanced direction calibration based on reinforcement learning for indoor localization system
Nguyen et al. CKF‐Based Visual Inertial Odometry for Long‐Term Trajectory Operations

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant