CN105068536A - Moving substrate track planner achieved based on nonlinear optimization method - Google Patents

Moving substrate track planner achieved based on nonlinear optimization method Download PDF

Info

Publication number
CN105068536A
CN105068536A CN201510495076.0A CN201510495076A CN105068536A CN 105068536 A CN105068536 A CN 105068536A CN 201510495076 A CN201510495076 A CN 201510495076A CN 105068536 A CN105068536 A CN 105068536A
Authority
CN
China
Prior art keywords
speed
track
path point
nonlinear optimization
constraint
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.)
Granted
Application number
CN201510495076.0A
Other languages
Chinese (zh)
Other versions
CN105068536B (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.)
Jilin University
Original Assignee
Jilin University
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 Jilin University filed Critical Jilin University
Priority to CN201510495076.0A priority Critical patent/CN105068536B/en
Publication of CN105068536A publication Critical patent/CN105068536A/en
Application granted granted Critical
Publication of CN105068536B publication Critical patent/CN105068536B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Abstract

The invention provides a moving substrate track planner achieved based on the nonlinear optimization method and belongs to the field of communication technology. The objective of theinvention is todesign a moving substrate track planner which satisfies requirements on speed in a task planning process of a movable substrate on the basis of the nonlinear optimization method. According to the invention, three times of b spline curves are used for parameterizing their tracks; linear scaling is performed on the whole moving time so that constrains on speed and accelerated speed can be satisfied; and theconstrained tracks can be calculated with the nonlinear optimization method. The whole track planning algorithm is encapsulated into the standard input/output function and downloaded and operated in an FPGA, so the track planner is formedwhich can be easily used by a user.

Description

Based on the mobile foundation trajectory planning device that nonlinear optimization method realizes
Technical field
The invention belongs to communication technical field.
Background technology
Usually, the application of mobile foundation in industry, harbour, planet surface exploration, the cleaning of nuke rubbish district, mining etc., its mission planning is the path point expected .Along with the development of science and technology, the application of mobile foundation is more and more extensive, requires also more and more higher to the complexity that mobile foundation is finished the work, and the application such as in spacecraft orbit simulation is the path point of speed dependent to its mission planning .Due in traditional application, do not have demand to speed, speed is determined indirectly by planning algorithm.But for new application, because speed is given by mission planning, must directly plan speed.Because traditional trajectory planning algorithm is not directly planned speed, cannot satisfy the demands.Mainly there is following problem:
1, straight-line segment adds parabolic path planing method: be connected by straight line between path point, and pass through curve transition at tie point place.The track generated cannot meet continual curvature, but cannot change curvature instantaneously due to steering mechanism, can bring very large error like this to track following.
2, adopt polynomial expression to carry out the method for trajectory planning, because its polynomial expression order can increase with the increase of path point number, cannot satisfy the demands.
3, adopt batten to carry out the method for trajectory planning, owing to cannot directly to plan speed or in planning process, optimized variable too much, cannot meet the demand of complex task to real-time.
At present in order to solve the difficulty that mobile foundation trajectory planning runs into, many experts have proposed many shaping methods:
China Patent Publication No. CNIO1508113A, publication date is on March 19th, 2014, and number of patent application is 200910071524.9.Name of patent application be " a kind of method for planning track of robot based on cosine second-order ".The mathematical model of the derivative of the position of the first given second order geometric locus based on cosine of this patent, speed, acceleration and acceleration, the position of given two desired point and speed.Then, the value of boundary condition is substituted in described mathematical model, list system of equations and solve the parameter of model.Finally, the threshold values of the derivative of acceleration is limited according to the amplitude of the position between two desired point and the relation of speed and the derivative of acceleration and acceleration.Determine final planned trajectory.But the method only carries out trajectory planning for starting point and terminal, cannot meet the demand of multipath point.
China Patent Publication No. CNIO3645725A, publication date is on August 19th, 2009, and number of patent application is 201310745783.1, and name of patent application is " a kind of robot teaching method for planning track and system ".That patent describes a kind of robot teaching method for planning track and system, relate to the robot teaching field in industrial process, it comprises: carrying out in teaching process to robot, gathers the spatial key point of teaching track; According to the spatial key point of teaching track, with many-knot spline interpolation function and least-square fitting approach, obtain teaching geometric locus.But the method does not consider the constraint that topworks exists, to complex task, cannot satisfy the demands.
China Patent Publication No. application publication number CNIO2794767A, publication date is on November 28th, 2012, and number of patent application is 201210319744.0, and name of patent application is " the robotic joint space B-spline method for planning track of vision guide ".This patent relates to a kind of robotic joint space B-spline method for planning track of vision guide; it comprises the steps: the first step, in three-dimensional bracket, installs two-degree-of-freedorobot robot; at front end mounting industrial camera, the direction of motion of travelling belt is perpendicular to the plane of movement of two-degree-of-freedorobot robot; Second step, to get on travelling belt after first tracing point at industrial camera, and within the time that two-degree-of-freedorobot robot moves to first tracing point, according to obtaining some joints timing node sequence, to construct B-spline curves; The mode of the 3rd step, employing increase knot vector and control vertex extends the B-spline curves of above-mentioned structure, to make the joint position point of B-spline curves through increasing; 4th step, the location point adopted on De Buer recursive algorithm calculating B-spline curves, move to drive two-degree-of-freedorobot robot.This patent cannot be planned speed, cannot meet the demand of particular task to speed.
Summary of the invention
The object of the invention is for mobile foundation demand to speed proposition in task planning process, adopt the method design of the nonlinear optimization mobile foundation trajectory planning device realized based on nonlinear optimization method satisfied the demands.
The present invention adopts three b SPL to carry out parametrization to track, and carries out linear scale to meet the constraint to speed, acceleration to whole run duration, then, is solved the track meeting constraint by nonlinear optimization;
If task exports as path point :
1. speed is determined direction: coupled together by all path point line segments, each spot speed is decomposed in x-axis and y-axis direction projection respectively , so just by path point be transformed to ;
2. B-spline is utilized to carry out parametrization to track: by path point by represent, corresponding speed by represent; Wherein . establish reference mark to be asked by represent
A, determine the corresponding time parameter values of each path point : first by total movement time normalization to [0,1]. namely
(1)
(2)
B, determine knot vector
C, right carry out parametrization: utilize knot vector , and reference mark generate parametric expressions
(3)
U is the parameter value after run duration t normalization;
be secondary B-spline basis function, its expression is provided by following formula:
(4)
(5)
(6)
Wherein ;
D, according to given path point following system of equations can be arranged:
(7)
Wherein , that is:
(8)
Order:
(9)
Then: (10)
Owing to having carried out normalization to parameter value, its parameter point actual speed corresponding has been
(11)
Then: (12)
That is: (13)
Reference mark can be obtained thus can by parameter represent, that is:
(14)
By this reference mark substitute into formula (3), then the track in x direction and y direction all can by parameter represent with the time parameter values u after normalization, that is:
(15);
3. retrain
There is following physical constraint in moving base:
Constraint of velocity: (16)
Wherein: (17)
Acceleration retrains: (18)
Wherein (19);
4. nonlinear optimization solves the track meeting constraint:
Trajectory planning problem is converted into following nonlinear optimal problem, selects least energy optimization problem:, (20)
subjectto
(21)
Will this nonlinear optimal problem is solved as optimized variable.
The application of the present invention in FPGA, becomes the input-output function of standard, downloads in FPGA and run by whole algorithm packaging, form trajectory planning device.
Beneficial effect of the present invention is:
The present invention only adopts three piecewise polynomials to carry out parametrization to track, directly can plan speed, and meet continual curvature, greatly reduce the computation burden of processor.And adopt nonlinear optimization method to be optimized track, can processing speed and acceleration constraint.
The present invention, in the process of carrying out nonlinear optimization, only adopts an optimized variable, greatly reduces the time of optimization, meet real-time demand.
The present invention says that whole trajectory planning algorithm is packaged into the input-output function of standard, downloads in FPGA and runs, and forms trajectory planning device, convenient for users to use.
Accompanying drawing explanation
Fig. 1. for velocity reversal solves figure;
Fig. 2. be x and the y graph of a relation of planning;
Fig. 3. be the rate curve of planning;
Fig. 4. be the x change curve in time of planning;
Fig. 5. the y change curve in time of planning;
Fig. 6. the accelerating curve of planning.
Embodiment
Adopt three b SPL to carry out parametrization to track, and linear scale is carried out to meet the constraint to speed, acceleration to whole run duration, then, solved the track meeting constraint by nonlinear optimization;
If task exports as path point :
1. first speed is determined direction: coupled together by all path point line segments, the velocity reversal of first and last path point is determined by the direction of Article 1 line segment and the last item line segment respectively.The velocity reversal of other each points is determined by the vertical line direction of the angular bisector of its correspondence.Finally each spot speed is decomposed in x-axis and y-axis direction projection respectively , so just by path point be transformed to ;
2. B-spline is utilized to carry out parametrization to track: by path point by represent, corresponding speed by represent; Wherein . establish reference mark to be asked by represent
A, determine the corresponding time parameter values of each path point : first by total movement time normalization to [0,1]. namely
(1)
(2)
B, determine knot vector
:
C, right carry out parametrization: utilize knot vector , and reference mark generate parametric expressions
(3)
U is the parameter value after run duration t normalization;
be secondary B-spline basis function, its expression is provided by following formula:
(4)
(5)
(6)
Wherein ;
D, according to given path point following system of equations can be arranged:
(7)
Wherein , that is:
(8)
Order:
(9)
Then: (10)
Owing to having carried out normalization to parameter value, its parameter point actual speed corresponding has been
(11)
Then: (12)
That is: (13)
For the matrix in formula (13) obtain according to formula (9), matrix for known quantity, reference mark can be obtained thus can by parameter represent, that is:
(14)
Due to all comprise position and the speed in x direction and y direction, reference mark comprise the reference mark in x direction and y direction.By this reference mark substitute into formula (3), then the track in x direction and y direction all can by parameter represent with the time parameter values u after normalization, that is:
(15);
3. retrain
There is following physical constraint in moving base:
Constraint of velocity: (16)
Wherein: (17)
Acceleration retrains: (18)
Wherein (19);
4. nonlinear optimization solves the track meeting constraint:
Trajectory planning problem is converted into following nonlinear optimal problem, selects least energy optimization problem:, (20)
subjectto
(21)
Will this nonlinear optimal problem is solved as optimized variable.
According to what solve namely can basis solve the timing node vector of this motion correspondence.According to with the cubic polynomial coefficient that every section of timing node is corresponding can be solved.
the present inventionapplication in FPGA, from the angle of Project Realization, becomes the input-output function of standard, downloads in FPGA and run by whole algorithm packaging, form trajectory planning device.The path point being input as speed dependent of planner , concrete input data layout is as Fig. 1. and export the timing node for completing needed for whole motion, and the cubic polynomial coefficient in every period, concrete output data layout is as Fig. 2. and all data are transmitted by serial ports.
The data layout of serial communication is:
Table 1 inputs data layout
Table 2 exports data layout
Table 1 represents input data layout, and frame head is 1,9, and 35,66,96. the first five data of Frame that ought receive are 1,9,35,66,96, then think that these frame data are effective, otherwise, think that these frame data are invalid.Path point number represents given path point number.Data represent concrete path point.Check bit adopts odd, ensures the validity of data.Table 2 represents and exports data layout, and frame head is 1,9,35,96,66, for these data of receiving equipment identification.Timing node value has represented timing node corresponding to whole motion.The multinomial coefficient of track in every section of timing node of every section of timing node infrapolynomial coefficient representative planning.Check bit adopts odd, ensures the validity of data.
Below this aspect, 5 is example, carries out simulating, verifying to the method.
Path point: (0,0,1), (5,3,1), (8 ,-1,1.35), (11.5 ,-3,0.5), (15,1.5,0)
Constraint
First two of each path point represents x, y coordinate above, and last representation speed, is random selecting.The actual maximal rate of mobile foundation that corresponding constraint is processed by laboratory respectively and acceleration obtain, and can modify according to the constraint of concrete pedestal.
Carry out trajectory planning according to above algorithm, program results is as Fig. 2-Fig. 6.
Fig. 2 and Fig. 3 represents the x obtained according to above algorithmic rule respectively, the time dependent curve of y coordinate.* point in Fig. 4 represents the position that given path point comprises, and the curve in this figure is according to the x of planning, and y curve draws, and can find out that the track of planning can accurately by given position by this figure.* point in Fig. 5 represents the speed that given path point comprises, and the curve in this figure is the rate curve change curve in time of planning, can find out that the track of planning can meet given speed requirement by this figure.Fig. 5 and Fig. 6 be representation speed and acceleration change curve in time respectively, meets speed and acceleration constraint.As can be seen from above program results, the curve of planning can be accurate through path point, and this trajectory planning algorithm can directly be planned speed, and the trajectory tortuosity of generation is continuous, meets speed and acceleration constraint.

Claims (2)

1. the mobile foundation trajectory planning device realized based on nonlinear optimization method, it is characterized in that: adopt three b SPL to carry out parametrization to track, and linear scale is carried out to meet the constraint to speed, acceleration to whole run duration, then, the track meeting constraint is solved by nonlinear optimization;
If task exports as path point :
1. speed is determined direction: coupled together by all path point line segments, each spot speed is decomposed in x-axis and y-axis direction projection respectively , so just by path point be transformed to ;
2. B-spline is utilized to carry out parametrization to track: by path point by represent, corresponding speed by represent; Wherein . establish reference mark to be asked by represent
A, determine the corresponding time parameter values of each path point : first by total movement time normalization to [0,1]. namely
(1)
(2)
B, determine knot vector
:
C, right carry out parametrization: utilize knot vector , and reference mark generate parametric expressions
(3)
U is the parameter value after run duration t normalization;
be secondary B-spline basis function, its expression is provided by following formula:
(4)
(5)
(6)
Wherein ;
D, according to given path point following system of equations can be arranged:
(7)
Wherein , that is:
(8)
Order:
(9)
Then: (10)
Owing to having carried out normalization to parameter value, its parameter point actual speed corresponding has been
(11)
Then: (12)
That is: (13)
Reference mark can be obtained thus can by parameter represent, that is:
(14)
By this reference mark substitute into formula (3), then the track in x direction and y direction all can by parameter represent with the time parameter values u after normalization, that is:
(15);
3. retrain
There is following physical constraint in moving base:
Constraint of velocity: (16)
Wherein: (17)
Acceleration retrains: (18)
Wherein (19);
4. nonlinear optimization solves the track meeting constraint:
Trajectory planning problem is converted into following nonlinear optimal problem, selects least energy optimization problem:, (20)
subjectto
(21)
Will this nonlinear optimal problem is solved as optimized variable.
2. the application of mobile foundation trajectory planning device in FPGA realized based on nonlinear optimization method according to claim 1, is characterized in that: the input-output function whole algorithm packaging being become standard, downloads in FPGA and run, and forms trajectory planning device.
CN201510495076.0A 2015-08-13 2015-08-13 The mobile foundation trajectory planning device realized based on nonlinear optimization method Active CN105068536B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510495076.0A CN105068536B (en) 2015-08-13 2015-08-13 The mobile foundation trajectory planning device realized based on nonlinear optimization method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510495076.0A CN105068536B (en) 2015-08-13 2015-08-13 The mobile foundation trajectory planning device realized based on nonlinear optimization method

Publications (2)

Publication Number Publication Date
CN105068536A true CN105068536A (en) 2015-11-18
CN105068536B CN105068536B (en) 2017-12-15

Family

ID=54497926

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510495076.0A Active CN105068536B (en) 2015-08-13 2015-08-13 The mobile foundation trajectory planning device realized based on nonlinear optimization method

Country Status (1)

Country Link
CN (1) CN105068536B (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106802625A (en) * 2017-03-14 2017-06-06 成都工业学院 A kind of derivative hyperspace machine tool track motion reappearance method
CN108717265A (en) * 2018-05-30 2018-10-30 重庆邮电大学 A kind of unmanned vehicle cruise tracking control system and control method based on control variable parameter
CN109795043A (en) * 2019-03-11 2019-05-24 江阴久盛科技有限公司 Multiple-grooved one-pass molding diamond wire method for slotting guide roller

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101508113A (en) * 2009-03-11 2009-08-19 哈尔滨工业大学 Robot track programming method based cosine second-order
CN102794767A (en) * 2012-08-31 2012-11-28 江南大学 B spline track planning method of robot joint space guided by vision
CN104020665A (en) * 2014-06-25 2014-09-03 北京邮电大学 Minimum saltus trajectory optimization method of mechanical arm based on multi-objective particle swarm optimization algorithm
CN104062902A (en) * 2014-05-15 2014-09-24 江南大学 Delta robot time optimal trajectory planning method

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101508113A (en) * 2009-03-11 2009-08-19 哈尔滨工业大学 Robot track programming method based cosine second-order
CN102794767A (en) * 2012-08-31 2012-11-28 江南大学 B spline track planning method of robot joint space guided by vision
CN104062902A (en) * 2014-05-15 2014-09-24 江南大学 Delta robot time optimal trajectory planning method
CN104020665A (en) * 2014-06-25 2014-09-03 北京邮电大学 Minimum saltus trajectory optimization method of mechanical arm based on multi-objective particle swarm optimization algorithm

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
FIELD G.,等: "Iterative dynamic programming: an approach to minimum energy trajectory planning for robotic manipulators", 《PROCEEDINGS OF THE IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION》 *
HUANG M S,等: "Minimum-energy point-to-point trajectory planning for a motor-toggle servomechanism", 《MECHATRONICS, IEEE/ASME TRANSACTIONS ON》 *
张红强,等: "机器人关节空间B样条轨迹设计的混沌优化", 《电机与控制学报》 *
钱东海,等: "基于B样条路径的机器人时间最优轨迹规划", 《上海交通大学学报》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106802625A (en) * 2017-03-14 2017-06-06 成都工业学院 A kind of derivative hyperspace machine tool track motion reappearance method
CN108717265A (en) * 2018-05-30 2018-10-30 重庆邮电大学 A kind of unmanned vehicle cruise tracking control system and control method based on control variable parameter
CN108717265B (en) * 2018-05-30 2021-05-18 重庆邮电大学 Unmanned aerial vehicle cruise tracking control system and control method based on control variable parameterization
CN109795043A (en) * 2019-03-11 2019-05-24 江阴久盛科技有限公司 Multiple-grooved one-pass molding diamond wire method for slotting guide roller

Also Published As

Publication number Publication date
CN105068536B (en) 2017-12-15

Similar Documents

Publication Publication Date Title
CN104062902B (en) Delta robot time optimal trajectory planning method
CN106826829A (en) A kind of industrial robot fairing trace generator method of Controllable Error
CN103085072B (en) Method for achieving industrial robot off-line programming based on three-dimensional modeling software
CN105772917B (en) A kind of three joint spot welding robot's Trajectory Tracking Control methods
CN105068536A (en) Moving substrate track planner achieved based on nonlinear optimization method
CN104942808A (en) Robot motion path off-line programming method and system
CN103386683A (en) Kinect-based motion sensing-control method for manipulator
CN106041928B (en) A kind of robot manipulating task task generation method based on part model
CN106346480B (en) A kind of multiple degrees of freedom injection machine arm modeling method based on UG and MATLAB
CN105773620A (en) Track planning and control method of free curve of industrial robot based on double quaternions
CN101482979A (en) Fairing-optimized CAD method for NURBS space curve curvature continuous split joint
CN102375416A (en) Human type robot kicking action information processing method based on rapid search tree
CN103941647A (en) Flexible accelerating and decelerating control interpolation method for embedded NC equipment
Ruchanurucks et al. Humanoid robot motion generation with sequential physical constraints
Ramer et al. A robot motion planner for 6-DOF industrial robots based on the cell decomposition of the workspace
CN106777475A (en) A kind of injection machine arm dynamics synergy emulation method of confined space constraint
CN106338966B (en) A kind of industrial robot trajectory planning programmed method
CN206825428U (en) A kind of real-time acquisition device of three-dimensional point cloud
CN104020718A (en) Self-adaption real-time interpolation method for NURBS curve reference model
Gao et al. Time-optimal trajectory planning of industrial robots based on particle swarm optimization
Zou et al. A novel robot trajectory planning algorithm based on NURBS velocity adaptive interpolation
CN104536381A (en) System and method for measuring contour error of multi-axis movement control system
CN109794939A (en) A kind of parallel Shu Fangfa of welding robot motion planning
CN102393680B (en) Parameter curve interpolation method based on vector interpolation of conical surface cutter
Gao et al. A fixed-distance planning algorithm for 6-DOF manipulators

Legal Events

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