CN116300936A - Motion guiding method for docking charging and robot - Google Patents
Motion guiding method for docking charging and robot Download PDFInfo
- Publication number
- CN116300936A CN116300936A CN202310274448.1A CN202310274448A CN116300936A CN 116300936 A CN116300936 A CN 116300936A CN 202310274448 A CN202310274448 A CN 202310274448A CN 116300936 A CN116300936 A CN 116300936A
- Authority
- CN
- China
- Prior art keywords
- robot
- charging
- charging pile
- point
- position information
- 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
- 230000033001 locomotion Effects 0.000 title claims abstract description 75
- 238000000034 method Methods 0.000 title claims abstract description 52
- 238000003032 molecular docking Methods 0.000 title claims abstract description 48
- 210000001503 joint Anatomy 0.000 claims abstract description 20
- 239000011159 matrix material Substances 0.000 claims description 16
- 230000009466 transformation Effects 0.000 claims description 10
- 238000006243 chemical reaction Methods 0.000 claims description 8
- 238000010586 diagram Methods 0.000 description 9
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 4
- 125000000205 L-threonino group Chemical group [H]OC(=O)[C@@]([H])(N([H])[*])[C@](C([H])([H])[H])([H])O[H] 0.000 description 3
- 239000003550 marker Substances 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 230000001960 triggered effect Effects 0.000 description 3
- 238000004590 computer program Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000001131 transforming effect Effects 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000003287 optical effect Effects 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/0225—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving docking at a fixed facility, e.g. base station or loading bay
-
- 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/60—Other road transportation technologies with climate change mitigation effect
- Y02T10/70—Energy storage systems for electromobility, e.g. batteries
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (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 discloses a motion guiding method and a robot for butt joint charging, and relates to the technical field of robots. The motion guiding method for butt joint charging is applied to a robot, the robot is provided with a laser radar, the laser radar is used for detecting the distance of obstacles around the robot in real time, and the method comprises the following steps: fitting a plurality of line segments based on point cloud data detected by a laser radar, and determining position information of each line segment; calculating a first included angle between every two adjacent line segments, a starting point distance and an end point distance based on the position information of each line segment; judging whether a V-shaped charging pile exists around the robot or not based on the starting point distance, the end point distance, the first included angle and a preset charging pile identification condition; if the V-shaped charging pile exists, controlling the robot to move to a butt joint charging point of the V-shaped charging pile; the docking charging point is near the apex of the V-shaped charging post. So this application has accurate butt joint and charges, improves the advantage of filling electric pile discernment accuracy.
Description
Technical Field
The application relates to the technical field of robots, in particular to a motion guiding method for butt joint charging and a robot.
Background
In the prior art, the method for autonomously guiding and charging the mobile robot mainly comprises positioning, infrared guiding, visual guiding and the like. The positioning-based charging navigation method is mainly realized by means of an SLAM (Simultaneous Localization and Mapping, instant positioning and map building) system, and a robot moves to a preset charging position to charge based on positioning data, but because of lack of pose guidance of the butt joint of the robot and a charging pile, the positioning error during the butt joint can cause the fact that the robot and the charging pile cannot be aligned for charging, and finally the problem of charging failure is generated; the charging navigation method based on infrared guidance is mainly realized by an infrared sensor arranged on a robot body, and the robot guides the robot to charge and dock with a charging pile based on the infrared signal detected by the infrared sensor, but the infrared sensor has poor anti-interference performance; the charging navigation method based on visual guidance is mainly realized by acquiring images in real time by virtue of a camera arranged on a robot body, so that a robot can conveniently identify a charging guide mark on a charging pile for charging and docking, and the method is easy to be disturbed by illumination.
Disclosure of Invention
The purpose of the application is to provide a motion guiding method and robot that dock and charge, carry out the motion guiding work before robot and the charging pile dock and charge through the accurate discernment V-arrangement fills electric pile to realize the accurate butt joint of robot and charging pile, improve the success rate of charging.
Embodiments of the present application are implemented as follows:
the first aspect of the embodiments of the present application provides a motion guiding method for docking and charging, where the method is applied to a robot, the robot is equipped with a laser radar, and the laser radar is used for detecting a distance between obstacles around the robot in real time, and the method includes: fitting a plurality of line segments based on point cloud data detected by a laser radar, and determining position information of each line segment; calculating a first included angle between every two adjacent line segments, a starting point distance and an end point distance based on the position information of each line segment; judging whether a V-shaped charging pile exists around the robot or not based on the starting point distance, the end point distance, the first included angle and a preset charging pile identification condition; if the V-shaped charging pile exists, controlling the robot to move to a butt joint charging point of the V-shaped charging pile; the docking charging point is near the apex of the V-shaped charging post.
In an embodiment, calculating a first included angle between each two adjacent line segments and a start point distance and an end point distance based on position information of each line segment includes: determining a starting point position and an ending point position of each line segment according to a preset direction; and respectively calculating the starting point distance and the ending point distance between the adjacent line segments based on the starting point position of one line segment and the ending point position of the other line segment in every two adjacent line segments.
In an embodiment, determining whether a V-shaped charging pile exists around the robot based on the starting point distance, the ending point distance, the first included angle, and a preset charging pile identification condition includes: judging whether the first included angle is within a preset angle range or not; if the first included angle is within the preset angle range, judging whether the absolute value of the difference value between the starting point distance and the width of the V-shaped charging pile does not exceed a first threshold value; if the absolute value of the difference value does not exceed the first threshold value, judging whether the end point distance does not exceed the second threshold value so as to confirm whether the V-shaped charging pile exists.
In an embodiment, after determining whether the V-shaped charging pile exists around the robot, the method further includes: if the V-shaped charging pile does not exist, acquiring the rotated angle of the robot at the current position, and judging whether the rotated angle exceeds 360 degrees; if the rotation angle is not more than 360 degrees, controlling the robot to rotate according to a preset rotation direction; and re-executing the step of judging whether the V-shaped charging piles exist around the robot based on the new point cloud data detected by the laser radar.
In one embodiment, prior to controlling the robot to move to the docking charging point of the V-shaped charging stake, the method further comprises: calculating the position information of the vertex of the V-shaped charging pile according to the position information of two adjacent line segments corresponding to the V-shaped charging pile in a laser radar coordinate system; calculating a first conversion matrix converted from a laser radar coordinate system to a charging pile coordinate system based on the position information of the vertex; based on the known second and first transformation matrices, which are transformed from the robot coordinate system to the lidar coordinate system, a third transformation matrix is calculated, which is transformed from the robot coordinate system to the charging pile coordinate system, and/or a fourth transformation matrix is calculated, which is transformed from the charging pile coordinate system to the robot coordinate system.
In one embodiment, controlling the robot to move to a docking charging point of the V-shaped charging post comprises: calculating the position information of the vertex of the V-shaped charging pile according to the position information of the two adjacent line segments corresponding to the V-shaped charging pile; after the position information of the charging guide point is determined based on the position information of the vertex, controlling the robot to move from the current position to the charging guide point; and controlling the robot to move straight towards the vertex, and determining that the robot moves to a docking charging point when the robot contacts the V-shaped charging pile.
In one embodiment, before controlling the robot to move straight toward the vertex, the method further comprises: based on the charging position of the robot and the charging position of the V-shaped charging pile, the pose angle of the robot is adjusted.
In one embodiment, controlling the robot to move from the current position to the charging guide point includes: generating a reference route between the current position and the charging guide point based on the position information of the current position and the charging guide point; controlling the movement of the robot, and controlling the motion parameters of the robot based on the real-time position information of the robot and the position information of the reference route; the motion parameters comprise a motion direction and a motion speed; and when the distance between the real-time position of the robot and the charging guide point is smaller than a preset third threshold value, controlling the robot to stop moving, and confirming that the robot reaches the charging guide point.
In one embodiment, controlling the motion parameters of the robot based on the real-time position information of the robot and the position information of the reference route includes: calculating the shortest distance between the real-time position of the robot and the reference route based on the real-time position information and the position information of the reference route; judging whether the shortest distance exceeds a preset adjustment distance; if the shortest distance exceeds the preset adjustment distance, adjusting the movement direction and movement speed of the robot based on the position information of the reference route; and if the shortest distance does not exceed the preset adjustment distance, controlling the robot to continuously move according to the preset movement speed and the current movement direction.
In one embodiment, after controlling the robot to move to the docking charging point of the V-shaped charging stake, the method further comprises: judging whether the robot is accurately in butt joint with the V-shaped charging pile for charging by detecting whether a battery charging current exists in the robot; if the battery charging current does not exist, confirming that the robot is not in accurate butt joint with the V-shaped charging pile; and after the robot is controlled to move and exit the V-shaped charging pile, the robot is controlled to move to a preset charging reference point so as to re-execute the steps of fitting a plurality of line segments based on the point cloud data detected by the laser radar and determining the position information of each line segment.
A second aspect of embodiments of the present application provides a robot comprising: a lidar, a processor, and a memory for storing processor-executable instructions; the processor is connected with the laser radar; the processor is configured to perform the motion guidance method of docking charging of the first aspect of the embodiments of the present application and any of the embodiments thereof.
Compared with the prior art, the beneficial effects of this application are:
according to the method, the special-shaped V-shaped charging pile can be identified through the point cloud data obtained through laser radar detection, so that the robot and the charging pile are further executed to conduct the movement guiding work of docking and charging, the precise docking of the robot and the charging pile is realized, and the success rate of robot charging is improved. In addition, after the V-shaped charging pile is identified, the reference path is determined based on the vertex position of the V-shaped charging pile, and the motion parameters are controlled according to the real-time position information during motion, so that the working efficiency of robot motion guidance is improved.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are needed in the embodiments will be briefly described below, it being understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered limiting the scope, and that other related drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a schematic structural diagram of a robot according to an embodiment of the present disclosure;
fig. 2 is a flow chart of a motion guiding method for docking and charging according to an embodiment of the present application;
fig. 3 is a flow chart of a motion guiding method for docking and charging according to an embodiment of the present application;
fig. 4 is a schematic diagram of a robot for identifying a V-shaped charging pile based on point cloud data according to an embodiment of the present disclosure;
fig. 5 is a schematic diagram of a transformation matrix solution for transforming from a laser radar coordinate system to a charging pile coordinate system according to an embodiment of the present disclosure;
fig. 6 is a schematic diagram of an application scenario in which a robot provided in an embodiment of the present application moves from a current position to a vicinity of a V-shaped charging pile;
fig. 7 is a schematic diagram of a robot moving from a current position to a motion guidance point according to an embodiment of the present application.
Reference numerals: 1-a robot; 10-laser radar; 20-proximity switch; 30-a processor; 40-memory.
Detailed Description
The technical solutions in the embodiments of the present application will be described below with reference to the drawings in the embodiments of the present application.
Like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further definition or explanation thereof is necessary in the following figures. Meanwhile, in the description of the present application, the terms "first", "second", and the like are used only to distinguish the description, and are not to be construed as indicating or implying relative importance.
The technical solutions of the present application will be clearly and completely described below with reference to the accompanying drawings.
Referring to fig. 1, fig. 1 is a schematic structural diagram of a robot 1 according to an embodiment of the present disclosure. As shown in fig. 1, the robot 1 includes at least one lidar 10, at least one proximity switch 20, a processor 30, and a memory 40, one processor 30 being exemplified in fig. 2. The processor 30 is connected to the memory 40, and the memory 40 stores instructions executable by the at least one processor 30, the instructions being executable by the at least one processor 30 to cause the at least one processor 30 to perform a docking charging motion guidance method as in the embodiments described below.
In an application scenario, when the robot 1 needs to dock with a charging pile with a special shape, such as a V-shaped charging pile, the robot 1 generates a point cloud based on the surrounding obstacle distances detected by the laser radar 10 in real time and obtains point cloud data. Then, the robot 1 recognizes or judges whether or not a V-shaped charging pile exists around based on the point cloud data, and moves to a docking charging point of the charging pile to charge according to the charging pile shape or the vertex position after recognizing the V-shaped charging pile. The proximity switch 20 is installed on the side wall of the body of the robot 1, generates a corresponding trigger signal when contacting the charging pile, confirms that the robot 1 reaches the docking charging point of the charging pile when receiving the trigger signal, controls the robot to stop moving and starts to detect the battery charging current so as to judge whether the charging is successful or not, namely, whether the robot 1 is accurately docked with the charging pile and the charging pile is effectively charged.
Referring to fig. 2, fig. 2 is a flow chart of a motion guiding method for docking and charging according to an embodiment of the present application. This method is applied to the robot 1, and the robot 1 is mounted with the laser radar 10. As shown in fig. 2, the motion guiding method of the docking charge includes the following steps.
S110: a plurality of line segments are fitted based on the point cloud data detected by the lidar 10, and position information of each line segment is determined.
In this step, the robot 1 detects the distance of surrounding obstacles by the lidar 10 when charging is required, and obtains point cloud data. The robot 1 performs straight line fitting based on RANSAC (Random Sample Consensus, random sampling consensus) algorithm according to the point cloud data of the current frame acquired in real time to obtain a plurality of line segments composed of points with similar distances or similar features, and obtains the position information of each fitted line segment based on the distance and the orientation of each point in the point cloud data relative to the robot 1 or the position of each point relative to the robot 1.
S120: and calculating a first included angle between every two adjacent line segments, a starting point distance and an ending point distance based on the position information of each line segment.
In this step, the robot 1 marks all the fitted line segments in a preset sorting direction based on the position information of the fitted line segments, and marks the start point and the end point of each line segment in the preset sorting direction. The robot 1 uses each two adjacent line segments as a group of adjacent line segment combinations, and then calculates a start point distance and an end point distance corresponding to each group of adjacent line segment combinations based on the start point position and the end point position of the two line segments in each group of adjacent line segment combinations. The starting point distance refers to the distance between the starting point of the line segment with the front sequence number and the ending point of the line segment with the rear sequence number in each group of adjacent line segment combinations; the end point distance is the distance between the end point of the line segment with the front sequence number and the start point of the line segment with the rear sequence number in each group of adjacent line segment combinations; the first included angle refers to an included angle between two adjacent line segments in each group of adjacent line segment combinations.
S130: and judging whether a V-shaped charging pile exists around the robot 1 or not based on the starting point distance, the ending point distance, the first included angle and a preset charging pile identification condition.
In this step, the robot 1 determines whether all of the starting point distance and the end point distance calculated in the above step meet the corresponding V-shaped charging pile identification conditions according to the first included angle. For example, comparing or determining whether the starting and ending distances are both within their respective, suitable distance ranges; whether the first included angle is within a suitable included angle range. Then, the robot 1 determines whether the V-shaped charging pile exists in the surrounding environment of the current position of the robot 1 according to the comparison or judgment result.
S140: if the V-shaped charging pile exists, the robot 1 is controlled to move to a butt joint charging point of the V-shaped charging pile.
In this step, if it is determined that the V-shaped charging pile exists around the robot, the robot 1 controls the robot to move to the docking charging point of the V-shaped charging pile according to the current position and the position information of the V-shaped charging pile. The docking charging point is the position of the robot 1 when the robot 1 moves beside the V-shaped charging pile and the charging port or the charging plug of the robot 1 is just docked with the charging plug or the charging port of the V-shaped charging pile; i.e. the position where the robot 1 is in butt-joint charging with the V-shaped charging pile. The docking charging point is near the apex of the V-shaped charging post.
Referring to fig. 3, fig. 3 is a flow chart of a motion guiding method for docking and charging according to an embodiment of the present application. As shown in fig. 3, the motion guiding method of the docking charge includes the following steps.
S210: the robot 1 is controlled to move to a preset movement reference point.
After the robot 1 detects that the self electric quantity is insufficient and automatically generates a charging task instruction, or after the robot 1 receives the charging task instruction sent by the user terminal or the server, the robot 1 queries the position information of a preset charging reference point and performs path planning based on a SLAM (Simultaneous Localizationand Mapping, instant positioning and map building) system so that the robot 1 moves from the current position to the preset charging reference point according to the planned path.
S220: after moving to the charging reference point, judging whether a V-shaped charging pile exists around the robot 1.
In this step, the robot 1 fits a plurality of line segments based on the point cloud data detected by the lidar 10 after moving to the charging reference point, and determines the positional information of each line segment. Then, the robot 1 calculates a first included angle between each two adjacent line segments, a start point distance, and an end point distance based on the position information of each line segment. And finally judging whether the V-shaped charging piles exist around the robot 1 according to the first included angle, the starting point distance, the finishing point distance and preset charging pile identification conditions.
Referring to fig. 4, fig. 4 is a schematic diagram of a robot 1 according to an embodiment of the present disclosure for identifying V-shaped charging piles based on point cloud data. As shown in fig. 4, a robot 1 acquires point cloud data detected by a laser radar 10 and extracts a frame of data, then performs straight line fitting based on a RANSAC (Random SampleConsensus, random sampling agreement) algorithm, and saves position information of a plurality of line segments obtained by fitting to an array L n ＝[p sn ，p en ]。
As shown in fig. 4, the robot 1 determines a start position and an end position of each line segment according to a preset direction (the preset direction shown in fig. 4 is from left to right), and determines a sequence number relationship of all line segments according to the preset direction.L n Is a certain line segment obtained after straight line fitting, n represents the sequence of the obtained line segments in a preset direction, and p sn Is the start point of the nth line segment, and the coordinates are (x sn ，y sn )，p en Is the end point of the nth line segment, and the coordinates are (x en ，y en )。L n A linear expression can be created by the start point coordinates and the end point coordinates: y is n ＝k n x n +b n 。
Then, the robot 1 calculates a start point distance and an end point distance between adjacent line segments based on a start point position of one line segment and an end point position of the other line segment, respectively, of every two adjacent line segments. In one embodiment, the starting distance is the distance between the starting point of the line segment with the front sequence number and the ending point of the line segment with the rear sequence number in every two adjacent line segments, namely the starting distance Δd2 is the starting point (p) sn ) And the end point (p) of the (n+1) th line segment en+1 ) A distance therebetween; the end point distance Δd1 is the distance between the end point of the line segment with the front sequence number and the start point of the line segment with the rear sequence number in every two adjacent line segments, namely the end point distance is the end point (p) en ) And the start point (p) of the n+1th line segment sn+1 ) A distance therebetween; the first angle refers to the angle between every two adjacent line segments, i.e., Δθ shown in fig. 4.
The preset charging pile identification conditions are as follows: the first included angle delta theta is within a preset angle range; the absolute value of the difference between the starting point distance and the width of the V-shaped charging pile does not exceed a first threshold value; and the endpoint distance does not exceed the second threshold.
Thus, the robot 1 determines whether a V-shaped charging pile exists around the robot 1 based on the starting point distance, the ending point distance, the first included angle and the preset charging pile identification condition. Namely, the robot judges whether the first included angle delta theta is within a preset angle range or not; if the first included angle is within the preset angle range, judging whether the absolute value of the difference between the starting point distance and the actual width distance D of the V-shaped charging pile is not more than a first threshold value, namely judging whether |Deltad2-D| is smaller than or equal to line_dis_thres1; if the absolute value of the difference does not exceed the first threshold, judging whether the end point distance does not exceed the second threshold, namely judging whether Deltad 1 is smaller than or equal to line_dis_thres2 or not, and confirming whether a V-shaped charging pile exists near the charging reference point or not.
In one embodiment, the robot 1 calculates a first angle formed by each two adjacent line segments based on the position information of each line segment, and determines whether the difference between the first angle Δθ and the actual angle target_angle of the charging pile is within ±5 degrees. The robot 1 calculates the starting point distance and the end point distance between every two adjacent line segments based on the position information of each line segment, and judges whether the end point distance delta d1 does not exceed a preset second threshold line_dis_thres2, namely delta d1 is less than or equal to line_dis_thres2; judging whether the absolute value of the difference between the starting point distance delta D2 and the actual width D of the V-shaped charging pile is not more than a preset first threshold value, namely |delta D2-D| is less than or equal to line_dis_thres1.
When the above-mentioned judging results are all yes, the robot 1 determines that V-shaped charging piles exist around, and continues to execute step S240; if any of the above determination results is negative, the robot 1 confirms that no V-shaped charging piles are present around, and proceeds to step S231.
S231: if the V-shaped charging pile does not exist, acquiring the rotated angle of the robot 1 at the current position, and judging whether the rotated angle exceeds 360 degrees.
In one embodiment, a rotation angle detecting device such as a gyroscope is mounted on the body of the robot 1. After the robot 1 reaches the charging reference point, continuously recording the self rotation angle and accumulating the total rotation angle, if the robot 1 does not recognize the V-shaped charging pile in the current direction (or the current pose angle), the robot 1 acquires the accumulated rotation angle at the current position and judges whether the accumulated rotation angle exceeds 360 degrees, namely, judges whether the robot 1 rotates one circle and returns to the initial pose angle. If yes, go to step S233; if not, the process proceeds to step S232.
S232: if the rotation angle is not more than 360 degrees, the robot 1 is controlled to rotate according to the preset rotation direction.
In this step, if the robot 1 determines that the accumulated rotation angle of the robot 1 at the charging reference point does not exceed 360 degrees, the robot 1 controls the robot 1 to continue rotating at the charging reference point according to the preset rotation direction, and the robot acquires new point cloud data detected by the laser radar 10 in real time at a fixed sampling frequency, and re-executes step S220 to again determine whether a V-shaped charging pile exists around the robot 1.
S233: and generating prompt information of the failure of charging butt joint.
If the robot 1 still does not find the V-shaped charging pile after rotating 360 degrees at the preset charging reference point, the charging docking task of the robot 1 is ended, and relevant prompt information of the charging docking failure is generated.
S240: the robot 1 performs coordinate system conversion.
Referring to fig. 5, fig. 5 is a schematic diagram of a transformation matrix solution for transforming from a laser radar coordinate system to a charging pile coordinate system according to an embodiment of the present application. As shown in fig. 5, after the robot 1 recognizes the V-shaped charging pile, it needs to calculate the vertex position of the V-shaped charging pile, that is, the intersection point position O of the straight line where the two line segments are located, according to the position information of the two adjacent line segments corresponding to the V-shaped charging pile. Knowing the position information of two adjacent line segments corresponding to the V-shaped charging pile under the laser radar coordinate system, the robot 1 can obtain the coordinate (x) of the vertex O of the V-shaped charging pile by calculation intersect ，y intersect )。
The robot 1 is based on the expression y of the straight line where two adjacent line segments are located corresponding to the known V-shaped charging pile n ＝k n x n +b n Y n+1 ＝k n+1 x n+1 +b n+1 The coordinates of the vertex O of the V-shaped charging pile under a laser radar coordinate system are calculated, and the calculation formula is as follows:
y intersect ＝k n x intersect +b n 。
the robot 1 is based on the positional information (x intersect ，y intersect ) A first conversion matrix is calculated that is converted from the lidar coordinate system to the charging pile coordinate system. As shown in fig. 5, a circle is created with a vertex (or intersection) 0 as a center and r as a radius; robot 1 fills electric pile station based on V-arrangementCorresponding two adjacent line segments L 2 、L 3 Determining the position information of the circle and L 2 、L 3 Is respectively denoted as p cl And p cr . Then, the robot 1 calculates the offset angle θ from the intersection coordinates marker The calculation formula is as follows:
θ marker ＝atan2(p cl ,p cr )；
the robot 1 is based on the principle according to θ marker And intersection point O (x intersect ，y intersect ) A first transformation matrix may be constructed that transforms from a lidar coordinate system laser link to a charging pile coordinate system dock link.
Then, the robot 1 calculates a third conversion matrix converted from the robot coordinate system to the charging pile coordinate system and/or a fourth conversion matrix converted from the charging pile coordinate system to the robot coordinate system based on the known second conversion matrix and first conversion matrix converted from the robot coordinate system to the laser radar coordinate system, so as to facilitate subsequent determination of positional information of the charging guide point or real-time calculation and control of motion parameters of the robot 1 under the robot coordinate system.
S250: the robot 1 is controlled to move from the charging reference point to the charging electrical guide point.
Referring to fig. 6, fig. 6 is a schematic view of an application scenario in which a robot 1 provided in an embodiment of the present application moves from a current position to a vicinity of a V-shaped charging pile. As shown in fig. 6, the robot 1 determines the positional information of the charging guide point K on the central axis of the V-shaped charging pile, which is spaced apart from the apex of the V-shaped charging pile by a distance dis_to_dock, from the positional information of the apex O of the V-shaped charging pile. That is, the coordinates of the charging guide point are expressed as (-dis_to_dock, 0) in the charging pile coordinate system, and the origin of the charging pile coordinate system is the vertex O of the V-shaped charging pile.
The robot 1 then generates a reference route SK between the current position and the charging guide point according to the charging guide point K and the current position (typically the position of the charging reference point S), and controls the movement of the robot 1. The robot 1 controls the motion parameters of the robot according to the real-time position information in the motion process and the position information of the reference route; the motion parameters include a motion direction and a motion speed, and the motion speed includes a linear speed and an angular speed.
Referring to fig. 7, fig. 7 is a schematic diagram of a robot 1 moving from a current position to a motion guiding point according to an embodiment of the present application. Calculating the shortest distance d between the real-time position P of the robot 1 and the reference route SK based on the real-time position information P and the position information of the reference route SK in the movement process of the robot 1; then the robot 1 judges whether the shortest distance d exceeds a preset adjustment distance, and if the shortest distance exceeds the preset adjustment distance, the robot 1 adjusts the movement direction and movement speed of the robot 1 based on the position information of the reference route SK; if the shortest distance d does not exceed the preset adjustment distance, the robot 1 is controlled to continue to move according to the preset movement speed and the current movement direction.
As shown in fig. 7, the robot 1 adopts a PID algorithm for adjusting the motion parameters from the start point S (typically, the charging reference point) to the charging guide point K. When the robot 1 reaches the charging reference point, the point is taken as a starting point S, and a reference route SK is constructed according to the position information of the starting point S and the charging guide point K, so that the robot 1 moves along the reference route SK through continuous adjustment of the movement parameters. The robot 1 calculates the shortest distance d between the real-time position P and the reference route SK, and when the shortest distance d of the robot 1 deviating from the reference route while moving exceeds the preset adjustment distance dis_thres, the robot 1 returns to the reference route SK by adjusting the movement direction and the movement speed until reaching the charging guidance point K.
In one embodiment, when the shortest distance d is greater than the preset adjustment distance dis_thres, the robot 1 calculates the angular velocity V based on the PID algorithm r While reducing the linear velocity V of the robot 1 x . When the shortest distance d is less than or equal to the preset adjustment distance dis_thres, the robot 1 follows the set linear velocity V max And (5) movement.
When the distance between the real-time position P of the robot 1 and the charging guide point K is smaller than the preset third threshold, the robot 1 determines that the robot 1 has reached the charging guide point K, and the robot 1 controls itself to stop moving and continues to execute step S261.
S261: based on the charging position of the robot 1 and the charging position of the V-shaped charging pile, the pose angle of the robot 1 is adjusted to perform pile alignment.
In an embodiment, after the robot 1 reaches the charging guide point, the pose angle of the robot 1 is adjusted according to the charging position of the robot 1 and the charging position of the V-shaped charging pile, i.e. the robot 1 performs direction adjustment so that the charging positions of the robot 1 and the V-shaped charging pile can be accurately abutted.
In one embodiment, the robot 1 adjusts the pose angle to keep its orientation aligned with the direction of the center vertical line of the charging pile after reaching the charging guide point. As shown in fig. 6, the robot 1 adjusts the pose angle so as to orient itself to the slave KT 1 Direction is adjusted to KT 2 Direction.
S262: the control robot 1 moves straight to the position of the vertex of the V-shaped charging pile.
In this step, the robot 1 controls itself to a preset minimum linear velocity V after the completion of the posture angle adjustment at the charging guide point min And the robot moves along the central axis of the V-shaped charging pile and moves towards the vertex of the V-shaped charging pile so as to butt joint the robot 1 and the charging pile.
S270: and judging whether the docking charging point is reached.
In one embodiment, the front end of the robot 1 is equipped with a proximity switch 20. When the robot 1 touches the V-shaped charging pile, the proximity switch 20 is triggered and generates a corresponding trigger signal. In this step, therefore, the robot 1 continuously determines whether or not the trigger signal generated by the proximity switch 20 is received during straight travel to determine whether or not it reaches the docking charging point. If the proximity switch 20 is not triggered all the time, the robot 1 keeps a straight movement state; if the proximity switch 20 is triggered, the robot 1 stops moving and performs step S280.
S280: judging whether the robot 1 is accurately in butt joint with the V-shaped charging pile for charging.
In this step, after the robot 1 completes the docking with the V-shaped charging pile and stops traveling straight, the robot 1 determines whether or not to accurately dock and charge itself with the V-shaped charging pile, that is, whether or not the docking and charging is successful by detecting the battery charging current for the battery. If no battery charging current exists, the robot 1 confirms that the robot itself is not in accurate butt joint with the V-shaped charging pile, and step S290 is executed; if the battery charging current exists, the robot 1 confirms that the self-docking charging is successful.
S290: control robot 1 removes and withdraws from V-arrangement fills electric pile
If the battery charging current is not detected, the robot 1 determines that the docking charging fails. The robot 1 controls itself to retreat and withdraw from the V-shaped charging pile, and then the robot 1 moves again to a preset charging reference point according to the navigation to re-perform the movement guiding method of the docking charging, i.e., re-perform step S210.
According to the method, the special-shaped V-shaped charging pile is identified through the point cloud data detected by the laser radar 10, so that the motion guiding work of the robot 1 and the charging pile for butt joint and charging is further executed, the accurate butt joint of the robot 1 and the charging pile is realized, and the success rate of charging of the robot 1 is improved. In addition, after the V-shaped charging pile is identified, the reference path is determined based on the vertex position of the V-shaped charging pile, and the motion parameters are controlled according to the real-time position information during motion, so that the working efficiency of motion guidance of the robot 1 is improved.
Embodiments of the present application provide a computer-readable storage medium storing a computer program. The computer program may be executed by the processor 30 to perform the motion guidance method of docking charging.
The functions, if implemented in the form of software functional modules and sold or used as a stand-alone product, may be stored on a computer readable storage medium. Based on such understanding, the technical solution of the present application may be embodied essentially or in a part contributing to the prior art or in a part of the technical solution, in the form of a software product stored in a storage medium, including several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods of the embodiments of the present application. And the aforementioned storage medium includes: a U-disk, a removable hard disk, a Read-only memory 40 (ROM), a random access memory 40 (RAM, random Access Memory), a magnetic disk or an optical disk, or other various media capable of storing program codes.
The foregoing description is only of the preferred embodiments of the present application and is not intended to limit the same, but rather, various modifications and variations may be made by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principles of the present application should be included in the protection scope of the present application.
Claims (11)
1. A motion guiding method for docking and charging, wherein the method is applied to a robot, the robot is carried with a laser radar, the laser radar is used for detecting obstacle distances around the robot in real time, and the method comprises the following steps:
fitting a plurality of line segments based on the point cloud data detected by the laser radar, and determining the position information of each line segment;
calculating a first included angle between each two adjacent line segments, a starting point distance and an ending point distance based on the position information of each line segment;
judging whether a V-shaped charging pile exists around the robot or not based on the starting point distance, the finishing point distance, the first included angle and a preset charging pile identification condition;
if the V-shaped charging pile exists, controlling the robot to move to a butt joint charging point of the V-shaped charging pile; and the docking charging point is close to the vertex of the V-shaped charging pile.
2. The method according to claim 1, wherein calculating the first angle between each two adjacent line segments and the start point distance, the end point distance based on the position information of each line segment, comprises:
determining a starting point position and an ending point position of each line segment according to a preset direction;
and respectively calculating the starting point distance and the ending point distance between the adjacent line segments based on the starting point position of one line segment and the ending point position of the other line segment in every two adjacent line segments.
3. The method for guiding movement of docking and charging according to claim 1, wherein the determining whether a V-shaped charging pile exists around the robot based on the starting point distance, the ending point distance, the first included angle, and a preset charging pile identification condition comprises:
judging whether the first included angle is within a preset angle range or not;
if the first included angle is in the preset angle range, judging whether the absolute value of the difference between the starting point distance and the width of the V-shaped charging pile does not exceed a first threshold value;
and if the absolute value of the difference value does not exceed the first threshold value, judging whether the end point distance does not exceed a second threshold value so as to confirm whether the V-shaped charging pile exists.
4. The docking charging motion guiding method according to claim 1, wherein after the judging whether or not a V-shaped charging pile exists around the robot, the method further comprises:
if the V-shaped charging pile does not exist, acquiring the rotated angle of the robot at the current position, and judging whether the rotated angle exceeds 360 degrees;
if the rotation angle is not more than 360 degrees, controlling the robot to rotate according to a preset rotation direction;
and re-executing the step of judging whether the V-shaped charging piles exist around the robot or not based on the new point cloud data detected by the laser radar.
5. The docking charging motion guiding method of claim 1, wherein prior to said controlling the robot to move to the docking charging point of the V-shaped charging stake, the method further comprises:
calculating the position information of the vertex of the V-shaped charging pile according to the position information of two adjacent line segments corresponding to the V-shaped charging pile in a laser radar coordinate system;
calculating a first conversion matrix converted from the laser radar coordinate system to a charging pile coordinate system based on the position information of the vertex;
based on the known second transformation matrix and the first transformation matrix, which are transformed from the robot coordinate system to the lidar coordinate system, a third transformation matrix is calculated, which is transformed from the robot coordinate system to the charging pile coordinate system, and/or a fourth transformation matrix is calculated, which is transformed from the charging pile coordinate system to the robot coordinate system.
6. The motion guiding method of docking charging according to claim 1, wherein the controlling the robot to move to the docking charging point of the V-shaped charging post comprises:
calculating the position information of the vertex of the V-shaped charging pile according to the position information of the two adjacent line segments corresponding to the V-shaped charging pile;
after position information of a charging guide point is determined based on the position information of the vertex, controlling the robot to move from the current position to the charging guide point;
and controlling the robot to move straight towards the vertex, and determining that the robot moves to the docking charging point when the robot contacts the V-shaped charging pile.
7. The method of claim 6, further comprising, prior to said controlling said robot to travel straight toward said vertex:
and adjusting the pose angle of the robot based on the charging position of the robot and the charging position of the V-shaped charging pile.
8. The method of claim 6, wherein controlling the robot to move from the current position to the charging guide point comprises:
generating a reference route between the current position and the charging guide point based on the position information of the current position and the charging guide point;
controlling the robot to move, and controlling the motion parameters of the robot based on the real-time position information of the robot and the position information of the reference route; the motion parameters comprise a motion direction and a motion speed;
and controlling the robot to stop moving when the distance between the real-time position of the robot and the charging guide point is smaller than a preset third threshold value, and confirming that the robot reaches the charging guide point.
9. The docking charging motion guiding method according to claim 8, wherein the controlling the motion parameters of the robot based on the real-time position information of the robot and the position information of the reference route comprises:
calculating a shortest distance between the real-time position of the robot and the reference route based on the real-time position information and the position information of the reference route;
judging whether the shortest distance exceeds a preset adjustment distance;
if the shortest distance exceeds the preset adjustment distance, adjusting the movement direction and the movement speed of the robot based on the position information of the reference route;
and if the shortest distance does not exceed the preset adjustment distance, controlling the robot to continuously move according to the preset movement speed and the current movement direction.
10. The motion guiding method of docking charging according to claim 1, wherein after the controlling the robot to move to the docking charging point of the V-shaped charging stake, the method further comprises:
judging whether the robot is accurately in butt joint with the V-shaped charging pile for charging by detecting whether a battery charging current exists in the robot;
if the battery charging current does not exist, confirming that the robot is not accurately docked with the V-shaped charging pile;
and controlling the robot to move to a preset charging reference point after moving and exiting the V-shaped charging pile so as to re-execute the step of fitting a plurality of line segments based on the point cloud data detected by the laser radar and determining the position information of each line segment.
11. A robot, the robot comprising:
a laser radar;
the processor is connected with the laser radar;
a memory for storing processor-executable instructions;
wherein the processor is configured to perform the motion guidance method of docking charging of any one of claims 1-10.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310274448.1A CN116300936A (en) | 2023-03-14 | 2023-03-14 | Motion guiding method for docking charging and robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310274448.1A CN116300936A (en) | 2023-03-14 | 2023-03-14 | Motion guiding method for docking charging and robot |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116300936A true CN116300936A (en) | 2023-06-23 |
Family
ID=86781275
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310274448.1A Pending CN116300936A (en) | 2023-03-14 | 2023-03-14 | Motion guiding method for docking charging and robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116300936A (en) |
-
2023
- 2023-03-14 CN CN202310274448.1A patent/CN116300936A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11340628B2 (en) | Marker-combined simultaneous localization and mapping navigation method, device and system | |
| KR101926322B1 (en) | Vehicle position estimating apparatus, vehicle position estimating method | |
| CN108628324B (en) | Unmanned vehicle navigation method, device, equipment and storage medium based on vector map | |
| KR102420476B1 (en) | Apparatus and method for estimating location of vehicle and computer recordable medium storing computer program thereof | |
| CN109782770B (en) | Method for automatically charging mower | |
| KR101927038B1 (en) | Vehicle position estimating apparatus, vehicle position estimating method | |
| US11417018B2 (en) | Device and method for calibrating camera for vehicle | |
| CN109270933A (en) | Unmanned barrier-avoiding method, device, equipment and medium based on conic section | |
| EP3633480B1 (en) | Point stabilization control method and device for mobile robot | |
| JP2001515237A (en) | Docking method of autonomous motion unit using guidance beam | |
| CN112217248A (en) | Charging pile, and method and device for autonomous charging of mobile robot | |
| CN113587930B (en) | Indoor and outdoor navigation method and device of autonomous mobile robot based on multi-sensor fusion | |
| CN112928799A (en) | Automatic butt-joint charging method of mobile robot based on laser measurement | |
| JP5085251B2 (en) | Autonomous mobile device | |
| WO2019031168A1 (en) | Mobile body and method for control of mobile body | |
| CN113296119A (en) | Unmanned obstacle avoidance driving method and terminal based on laser radar and UWB array | |
| CN113064430A (en) | Quality inspection trolley obstacle avoidance and path planning algorithm based on Android mobile phone | |
| CN113467476A (en) | Non-collision detection rapid stochastic tree global path planning method considering corner constraint | |
| JP5314788B2 (en) | Autonomous mobile device | |
| US20230205234A1 (en) | Information processing device, information processing system, method, and program | |
| JP5370122B2 (en) | Moving object position estimation device and moving object position estimation method | |
| CN116300936A (en) | Motion guiding method for docking charging and robot | |
| KR20200107382A (en) | Apparatus and method for determining correction information of vehicle-sensor | |
| Garcia et al. | Autonomous indoor navigation of a stock quadcopter with off-board control | |
| CN112859013B (en) | Unmanned aerial vehicle GNSS spoofing detection method based on flight path |
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 |