CN116400697A - Mobile control method, device, equipment and storage medium of self-mobile equipment - Google Patents
Mobile control method, device, equipment and storage medium of self-mobile equipment Download PDFInfo
- Publication number
- CN116400697A CN116400697A CN202310379297.6A CN202310379297A CN116400697A CN 116400697 A CN116400697 A CN 116400697A CN 202310379297 A CN202310379297 A CN 202310379297A CN 116400697 A CN116400697 A CN 116400697A
- Authority
- CN
- China
- Prior art keywords
- path
- sub
- self
- mobile device
- point
- 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 51
- 238000004590 computer program Methods 0.000 claims description 7
- 238000010276 construction Methods 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 10
- 230000008569 process Effects 0.000 description 10
- 230000006870 function Effects 0.000 description 4
- 241001417527 Pempheridae Species 0.000 description 2
- 230000004069 differentiation Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012546 transfer 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0214—Control 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
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0221—Control 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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The application relates to the technical field of automatic control, and provides a mobile control method, a device, equipment and a storage medium of self-mobile equipment, wherein the method comprises the following steps: when the self-mobile device is located at the end point of the first sub-path, acquiring the current pose of the self-mobile device, the starting point of the second sub-path and an expected pose angle corresponding to the starting point; the first sub-path and the second sub-path are adjacent sub-paths which are parallel to each other in the pre-planning path; generating a curve path between the first sub-path and the second sub-path according to the current pose, the starting point of the second sub-path and the expected pose angle; the control slave mobile device moves from the end point of the first sub-path to the start point of the second sub-path based on the curved path. According to the scheme, the self-moving equipment can smoothly move among different sub-paths, so that the turning-around slipping of the self-moving equipment is reduced.
Description
Technical Field
The present disclosure relates to the field of intelligent robots, and in particular, to a method, an apparatus, a device, and a storage medium for controlling movement of a self-mobile device.
Background
With the development of technology, self-moving devices such as a sweeper and a mower with a self-moving function are widely used in various fields. In practical application, when the self-moving device performs the arcuate line operation, the self-moving device performs turning around and turning around at the joint of the two mutually parallel path end points. In the turning process, the friction force between the wheels on the self-moving equipment and the ground is small, so that slipping often occurs, and the self-moving equipment fails to work.
Therefore, how to reduce the possibility of the self-moving device turning around and slipping during the arcuate line operation of the self-moving device is a problem to be solved.
Disclosure of Invention
The application provides a mobile control method, a mobile control device, mobile control equipment and a storage medium of a self-mobile device, and aims to realize smooth movement of the self-mobile device among different sub-paths, so that the occurrence of the problem of turning around and slipping of the self-mobile device is reduced.
In order to achieve the above object, the present application provides a movement control method of a self-mobile device, including:
when the self-mobile device is located at the end point of the first sub-path, acquiring the current pose of the self-mobile device, the starting point of the second sub-path and an expected pose angle corresponding to the starting point; the first sub-path and the second sub-path are adjacent sub-paths which are parallel to each other in the pre-planning path;
generating a curve path between the first sub-path and the second sub-path according to the current pose, the starting point of the second sub-path and the expected pose angle;
the control slave mobile device moves from the end point of the first sub-path to the start point of the second sub-path based on the curved path.
In addition, in order to achieve the above object, the present application further provides a movement control device of a self-mobile device, including:
the acquisition module is used for acquiring the current pose of the self-mobile device, the starting point of the second sub-path and the expected pose angle corresponding to the starting point when the self-mobile device is positioned at the end point of the first sub-path; the first sub-path and the second sub-path are adjacent sub-paths which are parallel to each other in the pre-planning path;
the generating module is used for generating a curve path between the first sub-path and the second sub-path according to the current pose, the starting point of the second sub-path and the expected pose angle;
and the control module is used for controlling the self-mobile device to move from the end point of the first sub-path to the start point of the second sub-path based on the curve path.
In addition, to achieve the above object, the present application further provides a self-mobile device, including:
a memory and a processor;
the memory is connected with the processor and used for storing programs;
the processor is configured to implement the steps of the movement control method of the self-mobile device as described above by running a program stored in the memory.
In addition, in order to achieve the above object, the present application further provides a computer readable storage medium, where the computer readable storage medium stores a computer program, and the computer program when executed by a processor causes the processor to implement the steps of the movement control method of the self-mobile device as described above.
The method comprises the steps of obtaining the current pose of the self-moving device, the starting point of a second sub-path and an expected attitude angle corresponding to the starting point when the self-moving device is located at the end point of the first sub-path, wherein the first sub-path and the second sub-path are adjacent sub-paths which are parallel to each other in a pre-planning path, and generating a curve path between the first sub-path and the second sub-path according to the obtained current pose, the starting point of the second sub-path and the expected attitude angle, namely planning the trend of the curve path by considering the moving direction of the self-moving device and the direction of predicting the next sub-path, so that the self-moving device can be controlled to move to the starting point of the second sub-path from the end point of the first sub-path based on the curve path, the movement of the self-moving device can be more attached to the curve path, smooth movement of the self-moving device between different sub-paths can be realized, and the problem of operation failure caused by turning around and slipping of the self-moving device is reduced.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed in the description of the embodiments will be briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present application, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a schematic flow chart of steps of a mobile control method of a self-mobile device according to an embodiment of the present application;
FIG. 2 is a schematic diagram of a pre-planned path provided by an embodiment of the present application;
fig. 3 is a schematic step flow diagram of another mobile control method of a self-mobile device according to an embodiment of the present application;
FIG. 4 is a schematic diagram of a curved path provided by an embodiment of the present application;
FIG. 5 is a flowchart illustrating a step of controlling a mobile device to move from an end point of a first sub-path to a start point of a second sub-path based on a curved path according to an embodiment of the present application;
FIG. 6 is a schematic diagram of tracking a trajectory based on the curved path to determine tracking curvature provided by an embodiment of the present application;
fig. 7 is a schematic block diagram of a mobile control device of a self-mobile device according to an embodiment of the present application;
fig. 8 is a schematic block diagram of a self-mobile device according to an embodiment of the present application.
Detailed Description
The following description of the embodiments of the present application will be made clearly and fully with reference to the accompanying drawings, in which it is evident that the embodiments described are some, but not all, of the embodiments of the present application. All other embodiments, which can be made by one of ordinary skill in the art without undue burden from the present disclosure, are within the scope of the present disclosure.
The flow diagrams depicted in the figures are merely illustrative and not necessarily all of the elements and operations/steps are included or performed in the order described. For example, some operations/steps may be further divided, combined, or partially combined, so that the order of actual execution may be changed according to actual situations.
It is to be understood that the terminology used in the description of the present application is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. As used in this specification and the appended claims, the singular forms "a," "an," and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise.
It should also be understood that the term "and/or" as used in this specification and the appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes such combinations.
The embodiment of the application provides a mobile control method, a device, equipment and a storage medium of self-mobile equipment, which are used for realizing smooth movement of the self-mobile equipment among different sub-paths instead of direct turning around and turning movement, so that turning around and slipping of the self-mobile equipment are reduced.
Referring to fig. 1, fig. 1 is a flowchart illustrating a mobile control method of a self-mobile device according to an embodiment of the present application.
As shown in fig. 1, the method for controlling movement of a self-mobile device according to the embodiment of the present application includes steps S101 to S103.
S101, when the self-mobile device is located at the end point of a first sub-path, acquiring the current pose of the self-mobile device, the starting point of a second sub-path and an expected pose angle corresponding to the starting point; the first sub-path and the second sub-path are adjacent sub-paths which are parallel to each other in the pre-planning path.
The self-mobile device may be a device including a self-mobile auxiliary function, and the self-mobile auxiliary function may be implemented by the vehicle-mounted terminal. Accordingly, the self-moving device may also be a vehicle with an on-board terminal, or may be a semi-self-moving device or a completely autonomous moving device, for example, a self-moving device including, but not limited to, a sweeper, a mower, a transfer robot, etc. may control the self-moving device.
Typically, prior to performing a job from a mobile device, a corresponding pre-planned path is planned according to a work area, wherein the pre-planned path comprises a plurality of mutually parallel sub-paths. For example, as shown in fig. 2, the pre-planned path is an arcuate line work path, and includes sub-path 1, sub-path 2, sub-path 3, and the like, which are parallel to each other. And in the process of operating the self-mobile device, the self-mobile device moves according to the pre-planned path.
It should be noted that, the pre-planned path may be a path generated by a user according to a setting of a working area, or the pre-planned path may be a path automatically generated by a self-mobile device for the working area by adopting a path planning algorithm, where the path planning algorithm may use a longest boundary of the working area as a planning main direction, and perform arcuate path planning on the working area in the main direction, or may also perform zigzag path planning according to the working area, and the method is not limited herein.
It should be noted that the first sub-path may be any one sub-path in the pre-planned path, and the second sub-path is an adjacent sub-path parallel to the first sub-path, and is a next sub-path of the first sub-path in the pre-planned path, that is, the traveling sequence of the first sub-path and the second sub-path is moving from the first sub-path to the second sub-path. And when the self-mobile device is positioned at the end point of the first sub-path in the pre-planning path in the process of operating, acquiring the current pose of the self-mobile device, the starting point of the second sub-path and the expected pose angle corresponding to the starting point.
Illustratively, the current pose of the self-mobile device includes a current location of the self-mobile device, a current pose angle, and the like. The desired attitude angle corresponding to the start point of the second sub-path may be understood as a desired attitude angle corresponding to the start point of the second sub-path when the mobile device moves to the start point of the second sub-path, for example, if the desired attitude angle corresponding to the start point of the second sub-path when the mobile device moves to the start point of the second sub-path is parallel to the direction of the second sub-path, the desired attitude angle corresponding to the start point of the second sub-path is the direction angle of the second sub-path.
The current pose of the mobile device, the starting point of the second sub-path and the expected pose angle corresponding to the starting point are obtained, and the purpose is to generate a curve path between the first sub-path and the second sub-path based on the parameters. In practical applications, it is not necessary to generate a curved path between the first sub-path and the second sub-path in all situations. For example, in the case where the first sub-path is already the last sub-path in the pre-planned path, it is not necessary to generate a curved path, and accordingly, it is not necessary to perform the operation in step S101.
Thus, to avoid invalid operations, in some embodiments, step S101 is preceded by step S104, step S101 comprising sub-step S1011, as shown in fig. 3.
S104, constructing a circular area by taking the end point of the first sub-path as the circle center according to a preset reference radius value; the reference radius value is larger than the distance between adjacent sub-paths in the pre-planned path;
s1011, if there are other path points of the non-traveling path in the circular area, determining the sub-path corresponding to the path point as a second sub-path, and executing step S104 to obtain the current pose of the self-mobile device, the starting point of the second sub-path and the expected pose angle corresponding to the starting point.
The preplanned path comprises a plurality of mutually parallel sub-paths, the distance d between adjacent sub-paths in the preplanned path is obtained, and a corresponding reference radius value r is set according to the distance d between the adjacent sub-paths in the preplanned path, wherein the reference radius value r is larger than the distance d between the adjacent sub-paths. Then, a circular area is constructed by taking the end point of the first sub-path as the circle center and taking the reference radius value r as the circular radius. Illustratively, the reference radius value r is greater than the spacing d between adjacent sub-paths, and the difference between the reference radius value r and the spacing d between adjacent sub-paths is not greater than a predetermined difference a, i.e., 0 < r-d.ltoreq.a. It should be noted that, the specific value of the preset difference a may be flexibly set according to the actual situation, which is not particularly limited herein.
The reference radius value r is slightly larger than the distance d between adjacent sub-paths, instead of being infinite, so that the size of a circular area constructed with the end point of the first sub-path as the center of the circle and the reference radius value r as the radius of the circle is limited within a corresponding range.
For the constructed circular area, if there are no other non-traveling path points except the first sub-path, for example, if the first sub-path is the last sub-path in the pre-planned path, then there are no other non-traveling path points in the circular area, at this time, no invalid operation is performed to obtain the current pose of the mobile device, the starting point of the second sub-path, and the expected pose angle corresponding to the starting point, thereby further improving the intelligence of the self-mobile device.
Otherwise, if there are other route points of the non-driving route except the first sub-route in the circular area, determining the sub-route corresponding to the route point as the second sub-route, and obtaining the current pose of the mobile device, the starting point of the second sub-route and the expected pose angle corresponding to the starting point, wherein the specific operation is as described in the corresponding description above and is not repeated here.
S102, generating a curve path between the first sub-path and the second sub-path according to the current pose, the starting point of the second sub-path and the expected pose angle.
In order to enable the self-mobile device to smoothly move between the first sub-path and the second sub-path, a curve path between the first sub-path and the second sub-path is generated based on the current position of the self-mobile device, namely the end point of the first sub-path and the start point of the second sub-path, and the current attitude angle of the self-mobile device and the expected attitude angle corresponding to the start point of the second sub-path are taken into consideration.
Illustratively, the tangential direction of the curved path at the end of the first sub-path coincides with the direction corresponding to the current attitude angle of the self-mobile device, and the tangential direction of the curved path at the start of the second sub-path coincides with the direction corresponding to the desired attitude angle of the start of the second sub-path. It will be appreciated that the curved path thus generated is a smooth transition curve between the first sub-path and the second sub-path, such that the self-moving device moves smoothly along the curved path from the first sub-path to the second sub-path.
In some embodiments, the curved path between the first sub-path and the second sub-path may be based on a curve determined by a track planning algorithm in the related art, such as a dubin curve, as this application is not limited in this regard.
The curve path is exemplified by a dubin curve. For example, as shown in fig. 4, point a is the current position of the mobile device (i.e., the end point of the first sub-path), point B is the start point of the second sub-path, the current attitude angle of the mobile device is parallel to the direction of the first sub-path, i.e., the direction indicated by the arrow at point a, and the desired attitude angle of the start point of the second sub-path is parallel to the direction of the second sub-path, i.e., the direction indicated by the arrow at point B, and the direction of the first sub-path is 180 degrees different from the direction of the second sub-path, i.e., the current attitude angle of the mobile device is 180 degrees different from the desired attitude angle of the start point of the second sub-path. From the point a, the point B, and the 180 degrees difference from the current attitude angle of the mobile device and the desired attitude angle of the start point of the second sub-path, an LRL type dubin curve, i.e., a curve path, is generated, where L refers to left turn and R refers to right turn.
In some embodiments, prior to generating the curved path between the first sub-path and the second sub-path, the method further comprises:
acquiring a current pose, a starting point of a second sub-path and an expected pose angle when the self-mobile device is positioned on the first sub-path and does not reach the end point of the first sub-path; wherein the current pose comprises a current position and a current pose angle of the self-mobile device;
determining a distance of the current position from the end point of the first sub-path;
acquiring an orientation angle of a first sub-path;
determining an angle difference value between a current attitude angle and an orientation angle;
and if the distance is smaller than or equal to the preset distance threshold value and the angle difference value is smaller than or equal to the preset angle threshold value, determining to execute the step of generating the curve path between the first sub-path and the second sub-path.
In the foregoing embodiment, the curved path between the first sub-path and the second sub-path is generated by performing the operations obtained from the current pose of the mobile device, the start point of the second sub-path, and the desired pose angle corresponding to the start point, when the mobile device is located at the end point of the first sub-path, that is, when the current position of the mobile device is the end point of the first sub-path. That is, a curved path between the first sub-path and the second sub-path is regenerated from the mobile device reaching the end of the first sub-path. In this embodiment, in order to improve efficiency, when the self-mobile device is located on the first sub-path and has not reached the end point of the first sub-path, the position of the end point of the first sub-path and the orientation angle of the first sub-path are obtained in addition to the current position and the current attitude angle of the self-mobile device, the start point of the second sub-path and the desired attitude angle corresponding to the start point, the distance from the current position of the self-mobile device to the end point of the first sub-path is determined according to the current position of the self-mobile device and the position of the end point of the first sub-path, and the angle difference between the current attitude angle of the self-mobile device and the orientation angle of the first sub-path is calculated and determined according to the current attitude angle of the self-mobile device and the orientation angle of the first sub-path.
And presetting a corresponding preset distance threshold for the distance from the current position of the mobile device to the end point of the first sub-path, and presetting a corresponding preset angle threshold for the angle difference between the current attitude angle of the mobile device and the orientation angle of the first sub-path. The specific values of the preset distance threshold and the preset angle threshold can be flexibly set according to actual conditions, and are not particularly limited in the application.
After determining the distance from the current position of the mobile device to the end point of the first sub-path and the angle difference from the current attitude angle of the mobile device and the orientation angle of the first sub-path, comparing the distance from the current position of the mobile device to the end point of the first sub-path with a preset distance threshold and comparing the angle difference from the current attitude angle of the mobile device and the orientation angle of the first sub-path with a preset angle threshold. If the distance from the current position of the mobile device to the end point of the first sub-path is smaller than or equal to a preset distance threshold, that is, the current gesture angle of the mobile device and the angle difference between the orientation angles of the first sub-path are smaller than or equal to a preset angle threshold, that is, the deviation between the current gesture angle of the mobile device and the orientation angles of the first sub-path is small, a curve path between the first sub-path and the second sub-path, for example, a dubin curve between the first sub-path and the second sub-path is generated.
By the method, when the self-mobile device does not reach the end point of the first sub-path, the curve path between the first sub-path and the second sub-path is generated in advance, so that the self-mobile device can be controlled to smoothly move to the second sub-path, and the working efficiency of the self-mobile device is improved. And the method is that under the condition that the distance from the current position of the mobile device to the end point of the first sub-path is smaller than or equal to a preset distance threshold value and the angle difference value between the current gesture angle of the mobile device and the orientation angle of the first sub-path is smaller than or equal to a preset angle threshold value, a curve path between the first sub-path and the second sub-path is generated, and the reliability of the curve path is ensured.
In some embodiments, generating a curved path between the first sub-path and the second sub-path based on the current pose, the start point of the second sub-path, and the desired pose angle, includes:
determining a current location of the self-mobile device as a first end point of the curved path and determining a start point of the second sub-path as a second end point of the curved path;
and generating a curve path according to the first endpoint, the second endpoint, the current attitude angle and the expected attitude angle.
Unlike the previous embodiments, instead of determining the end point of the first sub-path as the end point of the curved path, the current location of the self-mobile device is determined as the end point of the curved path. Illustratively, the current location of the self-mobile device is determined as a first end point of the curved path, and the start point of the second sub-path is determined as a second end point of the curved path. And determining the direction corresponding to the current attitude angle of the self-mobile device as the tangential direction of the curve path corresponding to the first end point, determining the direction corresponding to the expected attitude angle of the starting point of the second sub-path as the tangential direction of the curve path corresponding to the second end point, and generating the curve path between the first sub-path and the second sub-path.
Because the distance from the current position of the mobile device to the end point of the first sub-path is less than or equal to the preset distance threshold, and the angle difference between the current attitude angle of the mobile device and the orientation angle of the first sub-path is less than or equal to the preset angle threshold, the generated curve path is still a smooth transition curve between the first sub-path and the second sub-path.
S103, the self-mobile device is controlled to move from the end point of the first sub-path to the start point of the second sub-path based on the curve path.
After the curved path is generated, the mobile device may be controlled to move along the curved path until the start point of the second sub-path is reached. For example, the control device moves along a curved path with corresponding movement parameters including, but not limited to, linear velocity, angular velocity, etc.
In some embodiments, as shown in fig. 5, step S103 includes sub-steps S1031 through S1034.
S1031, controlling the forward direction corresponding to the next path point of the self-mobile device alignment curve path relative to the starting point of the curve path;
s1032, calculating a tracking curvature corresponding to track tracking by the self-mobile equipment based on the curve path;
s1033, calculating a movement parameter of the self-moving device according to the tracking curvature, wherein the movement parameter comprises an expected linear speed of the self-moving device and an expected angular speed of the self-moving device;
s1034, controlling the self-mobile device to smoothly turn from the end point of the first sub-path to the start point of the second sub-path based on the movement parameter.
According to the generated curve path, the forward direction corresponding to the next path point of the curve path relative to the starting point of the curve path can be determined, for example, if the starting point of the curve path is the point A, the next path point of the curve path is the point C, the forward direction of the point C relative to the point A is determined, and the self-moving equipment is controlled to align with the forward direction.
And according to the curve path, calculating a tracking curvature corresponding to the track tracking by the self-mobile device based on the curve path.
For example, as shown in fig. 6, (Cx, cy) represents the current position coordinates of the self-mobile device, (Gx, gy) represents the route point position coordinates on the curved path, ld is the straight line distance between (Cx, cy) and (Gx, gy), R represents the radius of curvature of the curved path, and 2α represents the circle center angle corresponding to the curved track between (Cx, cy) and (Gx, gy). According to the sine theorem, tracking curvature K is calculated according to the following formula (1):
K＝2*sinα/Ld (1)
after the tracking curvature K corresponding to the track tracking is obtained, according to the tracking curvature K and the set expected linear velocity V, the expected angular velocity W is calculated according to the following formula (2):
W＝V*K (2)
then, based on the desired linear velocity V and the desired angular velocity W, the control is performed to smoothly steer the mobile device from the end point of the first sub-path to the start point of the second sub-path. Illustratively, the self-moving device is controlled to smoothly steer from the end point of the first sub-path to the start point of the second sub-path based on the desired linear velocity V and the desired angular velocity W using a PID (Proportion Integration Differentiation, proportional-integral-derivative control) control technique.
In some embodiments, controlling the smooth steering movement of the self-mobile device from the end point of the first sub-path to the start point of the second sub-path based on the movement parameter comprises:
calculating the difference between the current actual linear velocity and the expected linear velocity of the self-mobile device;
determining a motor control signal according to the difference value and a preset control model;
and controlling a driving motor of the self-moving equipment according to the motor control signal.
The difference between the current actual linear velocity V of the mobile device and the desired linear velocity V is calculated by detecting the actual linear velocity V corresponding to the current movement of the mobile device based on the desired linear velocity V by the velocity sensor. And forming a control deviation by the difference between the actual linear velocity V' and the expected linear velocity V, taking the control deviation as the input of a preset control model, and determining a corresponding motor control signal through the control model. The motor control signal may be a signal such as a speed, a position, and a torque of the motor.
For example, taking a PID controller as an example, a difference between the actual linear velocity V' and the desired linear velocity V is formed into a control deviation, and the PID controller is used to process the control deviation by linear combination according to a proportion, integration and differentiation, so as to determine a corresponding motor control signal.
And determining a motor control signal, namely controlling a driving motor of the self-moving device according to the motor control signal, so as to control the self-moving device to smoothly turn from the end point of the first sub-path to the start point of the second sub-path.
In the above embodiment, when the self-mobile device is located at the end point of the first sub-path, the current pose of the self-mobile device, the start point of the second sub-path, and the expected pose angle corresponding to the start point are obtained, wherein the first sub-path and the second sub-path are adjacent sub-paths parallel to each other in the pre-planned path, and according to the obtained current pose, the start point of the second sub-path, and the expected pose angle, a curved path between the first sub-path and the second sub-path is generated, that is, the movement direction of the self-mobile device and the direction of the predicted next sub-path are considered to plan the trend of the curved path, so that the self-mobile device is controlled to move from the end point of the first sub-path to the start point of the second sub-path based on the curved path, the movement of the self-mobile device can be more attached to the curved path, smooth movement between different sub-paths of the self-mobile device is realized, instead of direct turning and the problem of job failure caused by turning around of the self-mobile device is reduced.
Referring to fig. 7, fig. 7 is a schematic block diagram of a mobile control device of a self-mobile device according to an embodiment of the present application. The movement control device of the self-mobile device may be configured in the self-mobile device, for executing the aforementioned movement control method of the self-mobile device.
As shown in fig. 7, the movement control device 1000 of the self-moving apparatus includes: an acquisition module 1001, a generation module 1002 and a control module 1003.
An obtaining module 1001, configured to obtain, when the self-mobile device is located at an end point of the first sub-path, a current pose of the self-mobile device, a start point of the second sub-path, and a desired pose angle corresponding to the start point; the first sub-path and the second sub-path are adjacent sub-paths which are parallel to each other in the pre-planning path;
a generating module 1002, configured to generate a curved path between the first sub-path and the second sub-path according to the current pose, the start point of the second sub-path, and the desired pose angle;
a control module 1003 is configured to control the mobile device to move from the end point of the first sub-path to the start point of the second sub-path based on the curved path.
In an embodiment, the movement control device 1000 of the self-mobile device further includes a region construction module, configured to construct a circular region with the end point of the first sub-path as a center of a circle according to a preset reference radius value; the reference radius value is larger than the distance between adjacent sub-paths in the pre-planned path;
the obtaining module 1001 is further configured to determine, if there are other path points of the non-traveling path in the circular area, a sub-path corresponding to the path point as a second sub-path, and obtain a current pose of the mobile device, a start point of the second sub-path, and a desired pose angle corresponding to the start point.
In an embodiment, the obtaining module 1001 is further configured to obtain the current pose, the start point of the second sub-path, and the desired pose angle when the self-mobile device is located on the first sub-path and has not reached the end point of the first sub-path; wherein the current pose comprises a current position and a current pose angle of the self-mobile device; determining a distance of the current position from the end point of the first sub-path; acquiring an orientation angle of a first sub-path; determining an angle difference value between a current attitude angle and an orientation angle;
the generating module 1002 is further configured to generate a curved path between the first sub-path and the second sub-path if the distance is less than or equal to a preset distance threshold and the angle difference is less than or equal to a preset angle threshold.
In an embodiment, the generating module 1002 is further configured to:
determining a current location of the self-mobile device as a first end point of the curved path and determining a start point of the second sub-path as a second end point of the curved path;
and generating a curve path according to the first endpoint, the second endpoint, the current attitude angle and the expected attitude angle.
In an embodiment, the control module 1003 is further configured to:
controlling the forward direction corresponding to the next path point of the self-moving equipment alignment curve path relative to the starting point of the curve path;
calculating a tracking curvature corresponding to track tracking of the self-mobile equipment based on the curve path;
according to the tracking curvature, calculating a movement parameter of the self-moving equipment, wherein the movement parameter comprises an expected linear speed of the self-moving equipment and an expected angular speed of the self-moving equipment;
the control self-mobile device smoothly steers from the end point of the first sub-path to the start point of the second sub-path based on the movement parameters.
In an embodiment, the control module 1003 is further configured to:
calculating the difference between the current actual linear velocity and the expected linear velocity of the self-mobile device;
determining a motor control signal according to the difference value and a preset control model;
and controlling a driving motor of the self-moving equipment according to the motor control signal.
The modules in the mobile control device 1000 of the self-mobile device correspond to the steps in the mobile control method embodiment of the self-mobile device, and the functions and implementation processes thereof are not described herein in detail.
Referring to fig. 8, fig. 8 is a schematic block diagram of a self-mobile device according to an embodiment of the present application.
As shown in fig. 8, the self-mobile device 800 may include a processor 810, a memory 820. The processor 810 and the memory 820 are connected via a system bus, such as an I2C (Inter-integrated Circuit) bus.
Specifically, the processor 810 may be a Micro-controller Unit (MCU), a central processing Unit (Central Processing Unit, CPU), or a digital signal processor (Digital Signal Processor, DSP), or the like.
Specifically, the Memory 820 may be a Flash chip, a Read-Only Memory (ROM) disk, an optical disk, a U-disk, a removable hard disk, or the like.
Those skilled in the art will appreciate that the structure shown in fig. 8 is merely a block diagram of a portion of the structure associated with the present application and is not limiting of the self-moving device 800 to which the present application is applied, and that a particular self-moving device 800 may include more or fewer components than shown, or may combine certain components, or have a different arrangement of components.
The processor 810 is configured to execute the program stored in the memory 820, thereby implementing the following steps:
when the self-mobile device is located at the end point of the first sub-path, acquiring the current pose of the self-mobile device, the starting point of the second sub-path and an expected pose angle corresponding to the starting point; the first sub-path and the second sub-path are adjacent sub-paths which are parallel to each other in the pre-planning path;
generating a curve path between the first sub-path and the second sub-path according to the current pose, the starting point of the second sub-path and the expected pose angle;
the control slave mobile device moves from the end point of the first sub-path to the start point of the second sub-path based on the curved path.
In some embodiments, the processor 810, prior to implementing the current pose obtained from the mobile device, the start point of the second sub-path, and the desired pose angle corresponding to the start point, is further configured to implement:
according to a preset reference radius value, a circular area is constructed by taking the end point of the first sub-path as the circle center; the reference radius value is larger than the distance between adjacent sub-paths in the pre-planned path;
if there are other path points of the non-traveling path in the circular area, determining the sub-path corresponding to the path point as a second sub-path, and acquiring the current pose of the mobile device, the starting point of the second sub-path and the expected pose angle corresponding to the starting point.
In some embodiments, the processor 810, prior to implementing generating the curved path between the first sub-path and the second sub-path, is further configured to implement:
acquiring a current pose, a starting point of a second sub-path and an expected pose angle when the self-mobile device is positioned on the first sub-path and does not reach the end point of the first sub-path; wherein the current pose comprises a current position and a current pose angle of the self-mobile device;
determining a distance of the current position from the end point of the first sub-path;
acquiring an orientation angle of a first sub-path;
determining an angle difference value between a current attitude angle and an orientation angle;
and if the distance is smaller than or equal to the preset distance threshold value and the angle difference value is smaller than or equal to the preset angle threshold value, generating a curve path between the first sub-path and the second sub-path.
In some embodiments, processor 810, when implementing generating a curved path between the first sub-path and the second sub-path based on the current pose, the start point of the second sub-path, and the desired pose angle, is to implement:
determining a current location of the self-mobile device as a first end point of the curved path and determining a start point of the second sub-path as a second end point of the curved path;
and generating a curve path according to the first endpoint, the second endpoint, the current attitude angle and the expected attitude angle.
In some embodiments, the processor 810, when implementing control of the mobile device to move from the end of the first sub-path to the start of the second sub-path based on the curved path, is configured to implement:
controlling the forward direction corresponding to the next path point of the self-moving equipment alignment curve path relative to the starting point of the curve path;
calculating a tracking curvature corresponding to track tracking of the self-mobile equipment based on the curve path;
according to the tracking curvature, calculating a movement parameter of the self-moving equipment, wherein the movement parameter comprises an expected linear speed of the self-moving equipment and an expected angular speed of the self-moving equipment;
the control self-mobile device smoothly steers from the end point of the first sub-path to the start point of the second sub-path based on the movement parameters.
In some embodiments, the processor 810, when implementing control of smooth steering movement from the mobile device from the end point of the first sub-path to the start point of the second sub-path based on the movement parameters, is configured to implement:
calculating the difference between the current actual linear velocity and the expected linear velocity of the self-mobile device;
determining a motor control signal according to the difference value and a preset control model;
and controlling a driving motor of the self-moving equipment according to the motor control signal.
It should be noted that, for convenience and brevity of description, the specific working process of the self-mobile device 800 described above may refer to the corresponding process in the foregoing embodiment of the mobile control method of the self-mobile device, which is not described herein again.
Embodiments of the present application also provide a computer readable storage medium storing a computer program, where the computer program when executed by a processor causes the processor to implement the steps of the movement control method of the self-mobile device provided in the foregoing embodiments. For example, the computer program is loaded by a processor, the following steps may be performed:
when the self-mobile device is located at the end point of the first sub-path, acquiring the current pose of the self-mobile device, the starting point of the second sub-path and an expected pose angle corresponding to the starting point; the first sub-path and the second sub-path are adjacent sub-paths which are parallel to each other in the pre-planning path;
generating a curve path between the first sub-path and the second sub-path according to the current pose, the starting point of the second sub-path and the expected pose angle;
the control slave mobile device moves from the end point of the first sub-path to the start point of the second sub-path based on the curved path.
The specific implementation of each operation may be referred to the foregoing embodiments of the mobile control method of the self-mobile device, and will not be described herein.
The computer readable storage medium may be an internal storage unit of the mobile device 800 of the foregoing embodiment, for example, a hard disk or a memory of the mobile device 800. The computer readable storage medium may also be an external storage device of the mobile device 800, such as a plug-in hard disk, smart Media Card (SMC), secure Digital (SD) Card, flash memory Card (Flash Card) or the like, which are provided on the mobile device 800.
Because the computer program stored in the computer readable storage medium can execute any of the mobile control methods of the self-mobile device provided in the embodiments of the present application, the beneficial effects that any of the mobile control methods of the self-mobile device provided in the embodiments of the present application can be achieved, which are detailed in the previous embodiments and are not described herein.
It should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or system. Without further limitation, an element defined by the phrase "comprising one … …" does not exclude the presence of other like elements in a process, method, article, or system that comprises the element.
The foregoing is merely a specific embodiment of the present application, but the protection scope of the present application is not limited thereto, and any equivalent modifications or substitutions will be apparent to those skilled in the art within the scope of the present application, and these modifications or substitutions should be covered in the protection scope of the present application.
Claims (10)
1. A method of movement control of a self-mobile device, the method comprising:
when the self-mobile device is located at the end point of the first sub-path, acquiring the current pose of the self-mobile device, the starting point of the second sub-path and an expected pose angle corresponding to the starting point; wherein the first sub-path and the second sub-path are adjacent sub-paths which are parallel to each other in a pre-planned path;
generating a curve path between the first sub-path and the second sub-path according to the current pose, the starting point of the second sub-path and the expected pose angle;
controlling the self-moving device to move from the end point of the first sub-path to the start point of the second sub-path based on the curve path.
2. The movement control method of a self-moving device according to claim 1, characterized in that before the obtaining of the current pose of the self-moving device, the start point of the second sub-path, and the desired pose angle corresponding to the start point, the method further comprises:
according to a preset reference radius value, a circular area is constructed by taking the end point of the first sub-path as the circle center; the reference radius value is larger than the distance between adjacent sub-paths in the pre-planned path;
and if the path points of other non-traveling paths exist in the circular area, determining the sub-path corresponding to the path points as the second sub-path, and executing the step of acquiring the current pose of the self-mobile device, the starting point of the second sub-path and the expected pose angle corresponding to the starting point.
3. The method of motion control of a self-mobile device according to claim 1, wherein prior to said generating a curved path between said first sub-path and said second sub-path, said method further comprises:
acquiring the current pose, the starting point of the second sub-path and the expected pose angle when the self-mobile device is located on the first sub-path and does not reach the end point of the first sub-path yet; wherein the current pose comprises a current position and a current pose angle of the self-mobile device;
determining a distance of the current location from an end point of the first sub-path;
acquiring an orientation angle of the first sub-path;
determining an angle difference between the current attitude angle and the orientation angle;
and if the distance is smaller than or equal to a preset distance threshold value and the angle difference value is smaller than or equal to a preset angle threshold value, determining to execute the step of generating the curve path between the first sub-path and the second sub-path.
4. The movement control method of the self-mobile device according to claim 3, wherein the generating a curved path between the first sub-path and the second sub-path according to the current pose, the start point of the second sub-path, and the desired pose angle includes:
determining a current location of the self-mobile device as a first end point of the curved path and determining a start point of the second sub-path as a second end point of the curved path;
and generating the curve path according to the first endpoint, the second endpoint, the current attitude angle and the expected attitude angle.
5. The movement control method of the self-moving device according to claim 1, wherein the controlling the self-moving device to move from the end point of the first sub-path to the start point of the second sub-path based on the curved path includes:
controlling the self-moving equipment to align the forward direction corresponding to the next path point of the curve path relative to the starting point of the curve path;
calculating a tracking curvature corresponding to the track tracking of the self-mobile device based on the curve path;
according to the tracking curvature, calculating a movement parameter of the self-moving equipment, wherein the movement parameter comprises an expected linear speed of the self-moving equipment and an expected angular speed of the self-moving equipment;
controlling the self-moving device to smoothly turn from the end point of the first sub-path to the start point of the second sub-path based on the movement parameter.
6. The movement control method of the self-moving device according to claim 5, wherein the controlling the self-moving device to smoothly steer movement from an end point of the first sub-path to a start point of the second sub-path based on the movement parameter includes:
calculating the difference between the current actual linear velocity of the self-mobile device and the expected linear velocity;
determining a motor control signal according to the difference value and a preset control model;
and controlling the driving motor of the self-moving equipment according to the motor control signal.
7. A movement control apparatus of a self-moving device, comprising:
the acquisition module is used for acquiring the current pose of the self-mobile device, the starting point of the second sub-path and the expected pose angle corresponding to the starting point when the self-mobile device is positioned at the end point of the first sub-path; wherein the first sub-path and the second sub-path are adjacent sub-paths parallel to each other in the pre-planned path;
the generation module is used for generating a curve path between the first sub-path and the second sub-path according to the current pose, the starting point of the second sub-path and the expected pose angle;
and the control module is used for controlling the self-moving device to move from the end point of the first sub-path to the start point of the second sub-path based on the curve path.
8. The motion control apparatus of claim 7, further comprising an area construction module configured to construct a circular area based on a preset reference radius value and centered at an end point of the first sub-path; the reference radius value is larger than the distance between adjacent sub-paths in the pre-planned path;
the obtaining module is further configured to determine a sub-path corresponding to the path point as the second sub-path if there are other path points of the non-traveling path in the circular area, and obtain the current pose of the self-mobile device, the starting point of the second sub-path, and the expected pose angle corresponding to the starting point.
9. A self-moving device, comprising:
a memory and a processor;
the memory is connected with the processor and used for storing programs;
the processor is configured to implement the steps of the movement control method of the self-moving device according to any one of claims 1 to 6 by running a program stored in the memory.
10. A computer-readable storage medium, characterized in that the computer-readable storage medium stores a computer program which, when executed by a processor, causes the processor to implement the steps of the movement control method of a self-moving device as claimed in any one of claims 1 to 6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310379297.6A CN116400697A (en) | 2023-03-31 | 2023-03-31 | Mobile control method, device, equipment and storage medium of self-mobile equipment |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310379297.6A CN116400697A (en) | 2023-03-31 | 2023-03-31 | Mobile control method, device, equipment and storage medium of self-mobile equipment |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116400697A true CN116400697A (en) | 2023-07-07 |
Family
ID=87007107
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310379297.6A Pending CN116400697A (en) | 2023-03-31 | 2023-03-31 | Mobile control method, device, equipment and storage medium of self-mobile equipment |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116400697A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117629221A (en) * | 2023-12-01 | 2024-03-01 | 上海联适导航技术股份有限公司 | Diagonal harrowing path planning method |
-
2023
- 2023-03-31 CN CN202310379297.6A patent/CN116400697A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117629221A (en) * | 2023-12-01 | 2024-03-01 | 上海联适导航技术股份有限公司 | Diagonal harrowing path planning method |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108473132B (en) | Parking assistance system, parking assistance method, and program | |
| JP6599543B2 (en) | Automated guided vehicle | |
| EP0595416A1 (en) | Method and apparatus for smooth control of a vehicle with automatic recovery from interference | |
| JP7369626B2 (en) | Vehicle control system, vehicle control method and program | |
| CN116400697A (en) | Mobile control method, device, equipment and storage medium of self-mobile equipment | |
| JP4467533B2 (en) | Folding line following mobile robot and control method of broken line following mobile robot | |
| Dai et al. | Formation control of mobile robots with obstacle avoidance based on GOACM using onboard sensors | |
| CN113885514B (en) | AGV path tracking method and system based on fuzzy control and geometric tracking | |
| JP2008129695A (en) | Path generation system and method for traveling object | |
| JP2007257200A (en) | Mobile body and control method for it | |
| Lin et al. | Path planning for autonomous car parking | |
| US12099371B2 (en) | Fast continuous regulation of nonholonomic mobile robots | |
| EP3680742B1 (en) | Self-propelled device, and travelling control method and travelling control program of self-propelled device | |
| JP4577247B2 (en) | Mobile body and control method thereof | |
| CN116269040A (en) | Robot, method, device, equipment and storage medium for planning cleaning path | |
| Karatas et al. | Randomized searches and nonlinear programming in trajectory planning | |
| CN112947487B (en) | Automatic guided vehicle and curve path tracking method and control device thereof | |
| CN114815846A (en) | Movement control method and device for self-moving equipment and storage medium | |
| Salaris et al. | Epsilon-optimal synthesis for vehicles with vertically bounded field-of-view | |
| Vilca et al. | An overall control strategy based on target reaching for the navigation of an urban electric vehicle | |
| Gawron et al. | Algorithmization of constrained monotonic maneuvers for an advanced driver assistant system in the intelligent urban buses | |
| Chitsaz | On time-optimal trajectories for a car-like robot with one trailer | |
| Soueres et al. | A path following controller for wheeled robots which allows to avoid obstacles during transition phase | |
| CN113176784A (en) | Space-time joint trajectory optimization method for automatic driving | |
| CN116382282A (en) | Control method, device, equipment and storage medium of self-mobile equipment |
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 |