CN115167429A - Method for simultaneously planning position and posture of mobile robot - Google Patents
Method for simultaneously planning position and posture of mobile robot Download PDFInfo
- Publication number
- CN115167429A CN115167429A CN202210860426.9A CN202210860426A CN115167429A CN 115167429 A CN115167429 A CN 115167429A CN 202210860426 A CN202210860426 A CN 202210860426A CN 115167429 A CN115167429 A CN 115167429A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- pose
- posture
- speed
- planning
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 26
- 230000001133 acceleration Effects 0.000 claims abstract description 16
- 238000013507 mapping Methods 0.000 claims abstract description 8
- 230000001360 synchronised effect Effects 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 9
- 239000000126 substance Substances 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 206010048669 Terminal state Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
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)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention belongs to the field of robots, and particularly relates to a method for simultaneously planning the position and the posture of a mobile robot i = (x, y, theta), i =0,1, \8230, n, n is more than or equal to 5; arranging a pose point sequence of the mobile robot to meet the motion state requirement of the mobile robot; generating continuous pose tracks represented by Bezier curves by using a De Casteljau algorithm based on lie group lie algebra representation, and obtaining a pose track speed curve of the mobile robot by using a difference method; and mapping the speed of the Cartesian space planning into the working space of the mobile robot as the speed to input and control the synchronous motion change of the robot on the position and the posture. The invention can continuously and smoothly move the robot with continuous and smooth motion speed and acceleration, and effectively improve the motion stability of the mobile robot.
Description
Technical Field
The invention belongs to the field of robots, and particularly relates to a method for simultaneously planning the position and the posture of a mobile robot.
Background
At present, a trajectory planning method of a mobile robot with three degrees of freedom mainly plans the position of the robot, and rarely considers the planning of a course angle of the mobile robot. The mobile robot with three degrees of freedom has high maneuverability and flexibility, and when the position and the attitude of the mobile robot are set, the mobile robot with three degrees of freedom can realize simultaneous pose planning without moving first and then rotating or rotating first and then moving. It is necessary to plan the position and attitude of the mobile robot simultaneously.
The existing pose planning method is to separately plan the position and the pose by using two planning methods, and then unify the operation time of the two trajectories, so that the simultaneous starting and stopping of the position and the pose are realized. However, the existing pose synchronization planning method needs to establish the relationship between the parameters in the two planning methods, and if the relationship between the two parameters is not established accurately, the synchronization of the two tracks is affected. For example, in patent nos. CN113103240A and CN113103241A, a robot pose planning method is provided, in which the provided method first plans the position and pose by two methods, and then establishes a relationship between two planning parameters to realize the synchronous planning of the position and pose trajectory; CN113942016A provides a time synchronization method for motion pose of a mechanical arm end, which obtains a position trajectory parameter and a pose trajectory parameter by respectively adopting a trapezoidal trajectory planning method and an "angle-axis" method, and further needs to adjust a pose trajectory according to the position trajectory time; in the non-patent document, "implementation of manipulator space arc pose trajectory planning algorithm", a homogeneous matrix and a quaternion are adopted to plan the position and the attitude trajectory respectively, and the attitude trajectory C2 is continuous. Although the method realizes the simultaneous planning of the position and the posture of the robot, the planning method has complex process and low calculation efficiency, and can only ensure the continuous posture C2 and cannot ensure the smooth and continuous acceleration of the posture, which affects the tracking precision of the mobile robot. Therefore, it is important to research a method for simultaneously planning the position and the posture track of the mobile robot and ensure that the motion acceleration of the mobile robot is continuous and smooth.
Disclosure of Invention
In order to make up the defects of the prior art, the invention provides a technical scheme of a method for simultaneously planning the position and the posture of a mobile robot.
In order to achieve the above purpose, the invention adopts the technical scheme that:
a method for simultaneously planning the position and the posture of a mobile robot is mainly used for synchronously planning the position and the posture track of the mobile robot with three degrees of freedom, and comprises the following steps:
step one, acquiring a pose point P of the planned mobile robot from a path planning module i =(x,y,θ),i=0,1,…,n,n≥5。
And step two, in order to meet the requirement that the initial and final speeds and the accelerated speed of the mobile robot are 0, the initial and final path points can be output to the track planning module repeatedly for four times. For example, the pose point P planned by the path planning module 0 ,P 1 ,…,P n In order to meet the motion requirement of the mobile robot, the pose point sequence input to the track planning module is P 0 ,P 0 ,P 0 ,P 0 ,P 1 ,…,P n-1 ,P n ,P n ,P n ,P n 。
And thirdly, generating a Bezier curve interpolation algorithm by adopting a De Casteljau algorithm based on lie group lie algebraic representation to ensure the synchronism of the generated mobile robot position and posture trajectory planning. The speed curve obtained by derivation of the track curve is used as the control input of the mobile robot, the mobile robot can move in a translation and rotation mode, the motion track generated by the Bezier curve can guarantee the linear speed and the smooth and continuous change of the angular speed of the mobile robot, and the stability of the motion of the mobile robot is improved.
The Bezier curve interpolation algorithm generated by the De Casteljau algorithm based on lie group lie algebraic representation is as follows:
using the rotation coordinate Q = [ v ] x, v y, v z, w x, w y, w z, ]Representing the pose of the mobile robot in a global coordinate system, where V x ,V y ,W z Respectively replaced by x, y and theta. Giving a number of mobile robots a rotation coordinate Q 1 ,Q 2 ,…,Q n E se (3), for each t e 0,1]Two adjacent lie algebraic elementsAndlie algebra betweenComprises the following steps:
after n-1 recursions, we get the value of the T-E [0,1 ∈]Orthogonal matrix formed by pose points on time Bezier pose track curve
The De Casteljau algorithm based on lie group lie algebra can fit a given series of pose points to obtain a Bezier trajectory curve passing through a starting point and a terminal point, and is characterized in that the De Casteljau algorithm can synchronously fit a given position and a given course angle of a mobile robot, does not need to divide the position and the course angle into plans and unify time, and is simple.
The Bezier curve interpolation algorithm generated by the De Casteljau algorithm based on lie group lie algebraic representation is as follows:
each t ∈ [0,1 ]]Obtaining the rotation coordinate of the pose point by the orthogonal matrix corresponding to the moment through logarithmic mappingAll the mobile robot pose points form a C3 continuous Bezier curve.
A continuous and smooth speed curve of the mobile robot is obtained by adopting a difference algorithm between adjacent pose points, and the speed of the mobile robot in the initial state and the terminal state can be zero.
Similarly, a continuous and smooth acceleration curve of the mobile robot is obtained by adopting a differential algorithm at adjacent speed points, so that the continuous smoothness is maintained, and the stability of the mobile robot is improved.
The acceleration curve of the mobile robot is obtained by adopting a differential algorithm between adjacent speed points, the acceleration curve is continuous and smooth, and the stability of the mobile robot is improved.
Step four, acquiring a course angle of the mobile robot in real time through the wheel-type odometer, mapping the planned Cartesian space velocity to a robot working space as control input, and realizing synchronous change of the position and the posture of the mobile robot, wherein the input of the mobile robot can be given by the following formula:
wherein the left side of the equation is the velocity input of the mobile robot, and the right side of the equation R (theta) is the rotation matrix of the Cartesian coordinate system to the mobile robot coordinate system, [ V ] x V y W] T Is the planned speed of the mobile robot.
Only the speed V planned at time t x ,V y ,ω]The rotation moment R (theta) mapped from the Cartesian coordinate system to the robot coordinate system at the moment is multiplied on the left side and serves as the input of the mobile robot, and therefore the mobile robot can move in a translation mode and a rotation mode.
Compared with the prior art, the invention has the beneficial effects that:
the invention is suitable for the pose planning of a three-degree-of-freedom mobile robot, a Bezier curve algorithm generated based on a Decateljau algorithm represented by a lie algebra uses the same parameter t to represent the set pose point by the lie algebra, and the position and the pose of the mobile robot corresponding to each t moment are obtained through recursion between the lie algebra and mapping between the lie algebra of the lie group for many times.
Drawings
FIG. 1 is a flow chart of a method for simultaneously planning the position and attitude of a mobile robot according to the present invention;
FIG. 2 is a diagram of a pose trajectory of a mobile robot planned by the method of the present invention;
FIG. 3 is a graph of the velocity profile of a mobile robot programmed by the method of the present invention;
FIG. 4 is a graph of the acceleration of a mobile robot planned by the method of the present invention;
FIG. 5 is a diagram of the actual motion trajectory of a mobile robot to which the method of the present invention is applied;
in fig. 2, the origin points represent the position of a given mobile robot, the asterisk points represent the actual position of the mobile robot, the thick arrows represent the course angles of the given mobile robot, and the thin arrows represent the actual course angles of the mobile robot.
Detailed Description
In the description of the present invention, it is to be understood that the terms "one end", "the other end", "outside", "upper", "inside", "horizontal", "coaxial", "central", "end", "length", "outer end", and the like, indicate orientations or positional relationships based on those shown in the drawings, and are used only for convenience in describing the present invention and for simplicity in description, and do not indicate or imply that the device or element being referred to must have a particular orientation, be constructed in a particular orientation, and be operated, and thus, should not be construed as limiting the present invention.
The invention will be further explained with reference to the drawings.
The method for simultaneously planning the position and the posture of the mobile robot, which is provided by the invention, is shown in figure 1, and the working process of the method is as follows:
step one, acquiring a planned pose point P in a path planning module i = (x, y, θ), i =0,1, \8230 ≧ 5, where the position of the mobile robot is denoted by x, y, and the orientation is denoted by θ.
And step two, in order to meet the motion state requirement of the mobile robot, the pose point sequence of the mobile robot needs to be arranged.
And thirdly, generating a Bezier curve representing lower continuous pose trajectory by adopting a De Casteljau algorithm based on lie group lie algebraic representation, and obtaining a pose trajectory speed curve of the mobile robot by using a difference method.
And step four, mapping the speed planned in the Cartesian space into a working space of the mobile robot as an input speed to control the robot to synchronously move and change on the position and the posture.
As can be seen from the pose track curves, the speed curves and the acceleration curves of the mobile robot shown in the figures 2, 3 and 4, the pose track synchronous planning method of the mobile robot is simple and efficient, and effectively improves the running stability of the mobile robot.
After the pose points of the mobile robot in the Cartesian coordinate system are obtained from the path planning module, in order to meet the requirement that the starting and ending speeds and the acceleration of the mobile robot are 0, the starting and ending path points can be repeatedly output to the track planning module for four times. For example, the pose point list planned by the path planning module is:
in order to meet the motion requirement of the mobile robot, the pose point sequence input to the track planning module is as follows:
sequence of | x | | θ |
P | |||
0 | 0 | 0 | 0 |
|
0 | 0 | 0 |
|
0 | 0 | 0 |
|
0 | 0 | 0 |
P 4 | 0.8 | 0 | 5 |
P 5 | 1.2 | 0.5 | 9 |
P 6 | 1.5 | 1 | 15 |
P 7 | 1.8 | 1.2 | 17 |
P 8 | 2.5 | 1.5 | 22 |
P 9 | 2.5 | 1.5 | 22 |
P 10 | 2.5 | 1.5 | 22 |
P 11 | 2.5 | 1.5 | 22 |
The Bezier curve interpolation algorithm generated by the De Casteljau algorithm based on lie group lie algebraic representation is as follows: using rotation coordinate Q = [ V ] x, V y, V z, W x, W y, W z, ]Representing the pose of the mobile robot in a Cartesian coordinate system, where V x ,V y ,W z And x, y and theta are used for substitution. Giving a number of mobile robots a rotation coordinate Q 1 ,Q 2 ,…,Q 12 E se (3), as shown in fig. 2, the position of the mobile robot in the cartesian coordinate system is represented by a circular dot, and the heading angle of the mobile robot in the cartesian coordinate system is represented by a thick arrow. For each t ∈ [0,1 ]]Two adjacent lie algebrasElement(s)Andlie algebra betweenComprises the following steps:
after 11 recursions, we find out that in each t ∈ [0,1 ]]Orthogonal matrix formed by pose points on time Bezier pose track curve
The Bezier curve interpolation algorithm generated by the De Casteljau algorithm based on lie group lie algebraic representation is as follows: each t ∈ [0,1 ]]Obtaining the rotation coordinate of the pose point by the orthogonal matrix corresponding to the moment through logarithmic mappingThe pose points of the mobile robot form a pose trajectory curve having C3 continuity as shown in fig. 2. The star points represent the position of the mobile robot in the cartesian coordinate system, and the thin arrows represent the heading angles of the mobile robot in the cartesian coordinate system.
The speed curve of the mobile robot is obtained by adopting a difference algorithm between adjacent pose points. At t 1 At the moment, the moving speed of the mobile robot along the X axis of the Cartesian coordinate system is according to the rotation coordinateAndis obtained by the first difference of the moving robot along the Y axis of the Cartesian coordinate system according to the rotation coordinateAndis obtained from the first difference of the second row and the speed of the mobile robot rotating around the Z axis along the Cartesian coordinate system according to the rotation coordinateAndthe first column of the differential motion is obtained, the pose track of the mobile robot is a C3 continuous Bezier curve, a speed curve obtained through a differential algorithm is continuous and smooth, the speed curve is shown in figure 3, the position and the posture of the mobile robot can be changed in a simultaneous motion mode, and the speed of the mobile robot at the starting time and the ending time is zero.
Similarly, the acceleration curve of the mobile robot is derived between adjacent velocity points using a difference algorithm, at t 1 At any moment, the mobile robot moves along the X-axis of the Cartesian coordinate system with acceleration according to the rotation coordinateAndthe moving acceleration of the mobile robot along the Y axis of the Cartesian coordinate system is obtained according to the rotation coordinateAndsecond column of (1)And obtaining the speed of the mobile robot rotating around the Z axis along the Cartesian coordinate system according to the rotation coordinate by secondary differenceAndthe first row of secondary difference obtains that the acceleration curve in each direction is smooth and continuous, the requirement that the acceleration of the mobile robot at the starting point and the terminal point is zero can be met for the mobile robot, and the acceleration curve is shown in figure 4, so that the stability of the mobile robot can be improved.
Further, the input of the mobile robot may be given by:
where the left side of the equation is the velocity input of the mobile robot and the right side of the equation R (θ) is the rotation matrix mapped from the Cartesian coordinate system to the mobile robot coordinate system, where the heading angle is obtained in real time from a wheel odometer, [ V [ V ] ] x V y W] T Is the planned speed of the mobile robot.
The position and the posture of the mobile robot are planned in sequence according to the steps, the actual motion track of the mobile robot with three degrees of freedom is shown in figure 5, the position and the posture of the mobile robot are started and stopped at the same time, the posture track is smooth and continuous, and the stability of the posture track is effectively improved. Due to the reasons of uneven ground or wheel slip and the like, the allowable position error is +/-0.10 m, the attitude error is +/-5 degrees, and the error of the actual motion track is within the allowable error range.
It will be evident to those skilled in the art that the invention is not limited to the details of the foregoing illustrative embodiments, and that the present invention may be embodied in other specific forms with other modifications and variations without departing from the spirit or scope of the invention. The present embodiments are, therefore, to be considered as illustrative and not restrictive, the scope of the invention being indicated by the appended claims rather than by the foregoing description.
Finally, it should be noted that: the above embodiments are only used to illustrate the technical solution of the present invention, and not to limit the same; while the invention has been described in detail and with reference to the foregoing embodiments, it will be understood by those skilled in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.
Claims (6)
1. A method for simultaneously planning the position and the posture of a mobile robot is characterized by comprising the following steps:
step one, acquiring a planned pose point P in a path planning module i = (= (x, y, theta), i =0,1, \ 8230;, n, n ≧ 5, where the position of the mobile robot is denoted by x, y, and the orientation is denoted by theta;
step two, arranging the pose point sequence of the mobile robot to meet the motion state requirement of the mobile robot;
thirdly, generating continuous pose tracks represented by Bezier curves by using a De Casteljau algorithm based on lie group lie algebra representation, and obtaining a pose track speed curve of the mobile robot by using a difference method;
and step four, mapping the speed planned in the Cartesian space into a working space of the mobile robot as the speed to input and control the synchronous motion change of the robot on the position and the posture.
2. The method for simultaneously planning the position and the posture of a mobile robot according to claim 1, wherein the first step specifically comprises: acquiring a position and posture sequence P of a planned starting point, a planned end point and a planned intermediate point of the mobile robot from the path planning module i =(x,y,θ),i=0,1,…,n,n≥5。
3. The method for simultaneously planning the position and the posture of the mobile robot according to claim 1, wherein the second step specifically comprises: and outputting the starting path point and the ending path point to a trajectory planning module repeatedly four times so as to meet the requirement that the starting speed and the ending speed and the acceleration of the mobile robot are zero.
4. The method for simultaneously planning the position and the posture of the mobile robot according to claim 1, wherein the step three of generating continuous pose trajectories under the representation of a Bezier curve based on the De Casteljau algorithm of the litz representation comprises:
using the rotation coordinate Q = [ v ] x ,v y ,v z ,w x ,w y ,w z ,]In which V is x ,V y ,W z Respectively replacing the rotation coordinates with x, y and theta, and giving rotation coordinates Q of a plurality of mobile robots 1 ,Q 2 ,…,Q n E se (3), for each t e 0,1]Two adjacent lie algebraic elementsAndlie algebra betweenComprises the following steps:
5. The method for simultaneously planning the position and the posture of the mobile robot according to claim 1, wherein the interpolation algorithm of the Bezier curve in the third step is as follows:
6. The method for simultaneously planning the position and the posture of the mobile robot according to claim 1, wherein the fourth step specifically comprises: the course angle of the mobile robot is obtained in real time through the wheel-type odometer, the planned Cartesian space speed is mapped to the robot work space to serve as control input, and synchronous change of the position and the posture of the mobile robot is achieved.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210860426.9A CN115167429A (en) | 2022-07-21 | 2022-07-21 | Method for simultaneously planning position and posture of mobile robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210860426.9A CN115167429A (en) | 2022-07-21 | 2022-07-21 | Method for simultaneously planning position and posture of mobile robot |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115167429A true CN115167429A (en) | 2022-10-11 |
Family
ID=83495304
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210860426.9A Pending CN115167429A (en) | 2022-07-21 | 2022-07-21 | Method for simultaneously planning position and posture of mobile robot |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115167429A (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117260746A (en) * | 2023-11-22 | 2023-12-22 | 安徽大学 | Time optimal track planning method for robot Cartesian space |
-
2022
- 2022-07-21 CN CN202210860426.9A patent/CN115167429A/en active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117260746A (en) * | 2023-11-22 | 2023-12-22 | 安徽大学 | Time optimal track planning method for robot Cartesian space |
CN117260746B (en) * | 2023-11-22 | 2024-02-09 | 安徽大学 | Time optimal track planning method for robot Cartesian space |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Moon et al. | Kinodynamic planner dual-tree RRT (DT-RRT) for two-wheeled mobile robots using the rapidly exploring random tree | |
CN112757306B (en) | Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm | |
CN105353725B (en) | Auxiliary magnet configuration space circular interpolation method is crossed for industrial robot | |
CN107608344B (en) | Vehicle motion control method and device based on trajectory planning and related equipment | |
CN113601512B (en) | General avoidance method and system for singular points of mechanical arm | |
CN109968358B (en) | Redundant robot full-joint obstacle avoidance track optimization method considering motion stability | |
CN108897321B (en) | Robot formation variable formation control method and controller based on piloting following method | |
CN109960150B (en) | Track tracking control method for inspection robot | |
JP5380672B2 (en) | Motion planner, control system, and multi-axis servo system | |
CN114670191B (en) | Seven-degree-of-freedom elbow offset mechanical arm track planning method | |
CN112223290B (en) | Joint space continuous point track transition method and device | |
Yang et al. | Smooth trajectory planning along Bezier curve for mobile robots with velocity constraints | |
CN103481288B (en) | A kind of 5 articulated robot end-of-arm tooling posture control methods | |
CN113127989B (en) | Six-degree-of-freedom mechanical arm inverse kinematics analytic solution control method | |
Brezak et al. | Path smoothing using clothoids for differential drive mobile robots | |
CN115167429A (en) | Method for simultaneously planning position and posture of mobile robot | |
CN108908347A (en) | One kind is towards redundancy mobile mechanical arm error-tolerance type repetitive motion planning method | |
CN111580453A (en) | PLC-based implementation method for improving precision of two-dimensional circular interpolation track | |
CN109623812B (en) | Mechanical arm trajectory planning method considering spacecraft body attitude motion | |
CN108189034A (en) | A kind of robot continuous path implementation method | |
CN115202349A (en) | Multi-mobile-robot cooperative formation control method, device, equipment and storage medium based on communication interference | |
CN105234930A (en) | Control method of motion of redundant mechanical arm based on configuration plane | |
CN113467475A (en) | Track tracking sliding mode control method for Mecanum wheel omnidirectional mobile robot | |
CN110109481B (en) | Multi-unmanned aerial vehicle tracking target phase angle correction method based on orbit transformation method | |
CN115808904A (en) | Industrial robot arc auxiliary point passing track planning method |
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 |