Automatic charging plug gun docking method and device

Abstract

The invention relates to the technical field of automatic charging, in particular to an automatic charging insertion gun butt joint method and device, and the method comprises the steps: enabling a mechanical arm to advance from a ready insertion point posture along a Z axis of a mechanical arm tail end coordinate system, and judging the butt joint failure of a charging gun and a charging hole, the butt joint success or the contact between the charging gun and the edge of the charging hole when the mechanical arm advances; if the charging gun is in contact with the edge of the charging hole, determining a jack plane according to the posture of the ready insertion point and the posture of the contact point; at least one to-be-selected pose of the mechanical arm is determined on the insertion hole plane; enabling the mechanical arm to traverse all to-be-selected poses, and judging whether the charging gun and the charging hole are in butt joint successfully or fail in the traversal process. Therefore, contact between the charging gun and the edge of the charging hole can be judged when the mechanical arm advances, and butt joint continues after the posture of the mechanical arm is corrected in time according to the posture of the ready insertion point and the posture of the contact point during contact, so that the butt joint success rate can be improved, a high-precision camera does not need to be adopted, and the cost is reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of automatic charging, and particularly to an automatic charging gun docking method and an automatic charging gun docking device. Background Art

[0002] With the rapid development of new energy vehicles, the ownership of new energy vehicles continues to increase, and the demand for charging new energy vehicles has also gradually increased.

[0003] Currently, with the gradual maturity of vision recognition technology and collaborative robotic arm technology, visual positioning is adopted and then the charging hole is automatically docked by the collaborative robotic arm to replace manual labor to achieve automatic charging.

[0004] In related technologies, generally a high-precision camera is used for visual positioning, but the cost will be very high, and the accuracy of the vision algorithm is highly tested. Moreover, in some harsh environments such as dim light, a lot of dust, and rainy days, visual positioning is often inaccurate, and at this time the risk of rubbing against the vehicle will be very high; at the same time, only using a camera will have visual blind spots, and in these blind spots, it may collide with the vehicle. All of the above situations will bring huge economic losses to new energy vehicle owners. Summary of the Invention

[0005] In order to solve one of the above technical problems, the present invention proposes the following technical solutions.

[0006] A first aspect embodiment of the present invention proposes an automatic charging gun docking method, including the following steps: moving the robotic arm to a ready-to-insert pose, wherein a charging gun is installed at the end of the robotic arm; making the robotic arm advance along the Z-axis of the end coordinate system of the robotic arm from the ready-to-insert pose, and judging whether the docking of the charging gun and the charging hole fails, succeeds, or the charging gun contacts the edge of the charging hole during the advancement; if the charging gun contacts the edge of the charging hole, obtaining the ready-to-insert pose and the contact pose, and determining the jacking plane according to the ready-to-insert pose and the contact pose; determining at least one candidate pose of the robotic arm on the jacking plane; making the robotic arm traverse all the candidate poses, and judging whether the docking of the charging gun and the charging hole succeeds or fails during the traversal.

[0007] In addition, the automatic charging gun docking method according to the above embodiment of the present invention may further have the following additional technical features.

[0008] According to an embodiment of the present invention, the automatic charging gun docking method further includes: if the docking of the charging gun and the charging hole fails, making the robotic arm retreat and giving an alarm; if the docking of the charging gun and the charging hole succeeds, making the robotic arm insert the charging gun into the charging hole and perform charging.

[0009] According to an embodiment of the present invention, before moving the robotic arm to the pose ready for insertion, it further includes: after receiving a charging instruction, obtaining the pose P1 of the charging hole through visual recognition; retreating the pose P1 of the charging hole by a first preset distance along the Z-axis of the end coordinate system of the robotic arm to obtain the pose P2 ready for insertion.

[0010] According to an embodiment of the present invention, determining that the charging gun fails to dock with the charging hole, docks successfully, or the charging gun contacts the edge of the charging hole during forward movement includes: collecting the forward distance of the robotic arm and the data of the force sensor during forward movement, wherein the force sensor is installed at the end of the robotic arm; determining that the charging gun fails to dock with the charging hole, docks successfully, or the charging gun contacts the edge of the charging hole according to the forward distance of the robotic arm and the data of the force sensor.

[0011] According to an embodiment of the present invention, determining that the charging gun fails to dock with the charging hole, docks successfully, or the charging gun contacts the edge of the charging hole according to the forward distance of the robotic arm and the data of the force sensor includes: if the forward distance is less than a second preset distance and the change value of the data of the force sensor is greater than a change threshold, determining that the charging gun fails to dock with the charging hole; if the forward distance is greater than a third preset distance and the change value of the data of the force sensor is less than or equal to the change threshold, determining that the charging gun docks successfully with the charging hole, wherein the third preset distance is greater than the second preset distance; if the forward distance is between the second preset distance and the third preset distance and the change value of the data of the force sensor is greater than the change threshold, determining that the charging gun contacts the edge of the charging hole.

[0012] According to an embodiment of the present invention, determining the insertion hole plane according to the pose ready for insertion and the contact pose includes:

[0013] Obtaining the following plane normal vector according to the pose ready for insertion and the contact pose:

[0014]

[0015] wherein, is the plane normal vector, x2, y2, and z2 are the coordinates of the pose P2 ready for insertion, and x3, y3, and z3 are the coordinates of the contact pose P3;

[0016] According to the contact pose and the plane normal vector, determining the following point-normal equation of the insertion hole plane:

[0017] n x (x - x3) + n y (y - y3) + n z (z - z3) = 0

[0018] wherein, n x, n y and n z are the normal vector coordinates of the jack plane.

[0019] According to an embodiment of the present invention, determining at least one candidate pose of the robotic arm on the jack plane includes:

[0020] Taking two basis vectors on the jack plane and

[0021]

[0022] where n x , n y and n z are the normal vector coordinates of the jack plane;

[0023] The pose formula of any point P on the jack plane represented by the basis vectors:

[0024]

[0025] where is the vector of any point P, is the contact point vector, and s and t are arbitrary variables;

[0026] Determine the range of the arbitrary variables, and obtain at least one candidate pose of the robotic arm according to the range of the arbitrary variables and the pose formula of the any point P.

[0027] According to an embodiment of the present invention, determining whether the charging gun is successfully docked or failed to dock with the charging hole during the traversal process includes: during the traversal process, obtaining the data of the force sensor, wherein the force sensor is installed at the end of the robotic arm; if the change value of the data of the force sensor is less than the change threshold, it is determined that the docking is successful, otherwise it is determined that the docking fails.

[0028] An embodiment of the second aspect of the present invention provides an automatic charging gun docking device, including: a moving module for moving the robotic arm to the ready-to-insert pose, wherein a charging gun is installed at the end of the robotic arm; a first docking module for advancing the robotic arm along the Z-axis of the end coordinate system of the robotic arm from the ready-to-insert pose, and determining whether the charging gun fails to dock, successfully docks or the charging gun contacts the edge of the charging hole during the advancement; a first determination module for, if the charging gun contacts the edge of the charging hole, obtaining the ready-to-insert pose and the contact pose, and determining the jack plane according to the ready-to-insert pose and the contact pose; a second determination module for determining at least one candidate pose of the robotic arm on the jack plane; a second docking module for causing the robotic arm to traverse all the candidate poses, and determining whether the charging gun is successfully docked or failed to dock with the charging hole during the traversal process.

[0029] The technical solution of the embodiment of the present invention enables the robotic arm at the pose of the preparation for insertion to advance along the end Z-axis. When the robotic arm advances, it can determine the contact between the charging gun and the edge of the charging hole. And when in contact, according to the pose of the preparation for insertion and the contact pose, the pose of the robotic arm is corrected in time and then the docking continues, which can improve the docking success rate. Moreover, there is no need to use a high-precision camera, reducing the cost. Description of the Drawings

[0030] Figure 1 It is a schematic diagram of the installation at the end of the robotic arm according to the embodiment of the present invention.

[0031] Figure 2 It is a flowchart of the automatic charging gun docking method according to the embodiment of the present invention.

[0032] Figure 3 It is a flowchart of automatic docking according to an example of the present invention.

[0033] Figure 4 It is a schematic diagram of the base vectors of the jack plane according to an example of the present invention.

[0034] Figure 5 It is a structural block diagram of the automatic charging gun docking device according to the embodiment of the present invention. Detailed Embodiments

[0035] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0036] In order to improve the docking success rate of the automatic charging gun, reduce the risk of rubbing against the vehicle, and effectively avoid collision with the vehicle when in the camera blind area, the embodiment of the present invention proposes an automatic charging gun docking method, using a method that combines a low-resolution camera and a force sensor to solve the above problems.

[0037] For this reason, the embodiment of the present invention adds a force sensor, a camera (low-precision camera), and a charging gun at the end of the robotic arm, and the installation method is as Figure 1 shown, and an XYZ coordinate system of the robotic arm end is established, where the Z-axis direction is along the front of the robotic arm end, the Y-axis direction is along the right end of the robotic arm end, the X-axis direction is along the upper side of the robotic arm end, and the center point of the end circular surface is the origin.

[0038] Figure 2 It is a flowchart of the automatic charging gun docking method according to the embodiment of the present invention.

[0039] As Figure 2 shown, the automatic charging gun docking method includes the following steps S1 to S5.

[0040] S1. Move the robotic arm to the ready-to-insert position and pose, where a charging gun is installed at the end of the robotic arm.

[0041] The ready-to-insert point refers to the starting point where the robotic arm moves to the vehicle charging hole position and pose (end point) for gun insertion, which can be obtained based on the vehicle charging hole position and pose.

[0042] S2. Make the robotic arm advance along the Z-axis of the end coordinate system of the robotic arm from the ready-to-insert position and pose, and determine during the advancement whether the docking of the charging gun with the charging hole fails, succeeds, or the charging gun contacts the edge of the charging hole.

[0043] Specifically, after determining the ready-to-insert point and its position and pose P2, automatically control the robotic arm to slowly advance and probe along the end Z-axis from the ready-to-insert position and pose P2, and during the advancement, combine with the force sensor to determine in real time whether the docking of the charging gun with the vehicle charging hole fails, succeeds, or the charging gun disengages from the edge of the vehicle charging hole.

[0044] Among them, a failed docking means that the gun cannot be inserted at this time, and a successful docking means that the gun can be inserted.

[0045] S3. If the charging gun contacts the edge of the charging hole, obtain the ready-to-insert position and pose and the contact position and pose, and determine the jacking plane based on the ready-to-insert position and pose and the contact position and pose.

[0046] S4. Determine at least one candidate position and pose of the robotic arm on the jacking plane.

[0047] The candidate position and pose refers to all possible positions and poses of the robotic arm to achieve docking.

[0048] S5. Make the robotic arm traverse all candidate positions and poses, and determine during the traversal whether the docking of the charging gun with the charging hole is successful or failed.

[0049] Specifically, after obtaining at least one candidate position and pose, correct the position and pose of the robotic arm, that is, control the robotic arm to move to all candidate positions and poses in sequence, and after moving to the candidate position and pose, perform re-docking to determine whether the docking is successful or failed.

[0050] Through the above steps S1 and S2, the automatic docking of the charging gun with the vehicle charging hole is achieved, and it is possible to determine the phenomena of successful docking (positioning to the charging hole), failed docking (not positioning to the charging hole), and the charging gun contacting the edge of the charging port. Through steps S3 to S5, when the charging gun contacts the charging hole, error correction is performed to correct the position and pose of the robotic arm to improve the docking success rate, that is, the docking success rate of the corrected robotic arm is greatly improved.

[0051] Therefore, the automatic charging gun docking method according to the embodiments of the present invention can determine the contact between the charging gun and the edge of the charging hole when the robotic arm moves forward. When contacting, the pose of the robotic arm can be corrected in a timely manner according to the insertion pose and the contact pose, and then the docking can continue, which can improve the docking success rate. Moreover, there is no need to use a high-precision camera, reducing the cost.

[0052] In one embodiment, the automatic charging gun docking method may further include: if the docking between the charging gun and the charging hole fails (the charging hole is not found), the robotic arm is retracted and an alarm is given; if the docking between the charging gun and the charging hole is successful (the charging hole is found), the robotic arm inserts the charging gun into the charging hole and charges.

[0053] The failure of docking means an abnormality occurs, such as the vehicle hood being closed in advance.

[0054] Therefore, when the docking fails, retracting the robotic arm and giving an alarm can avoid a collision.

[0055] In one example, as Figure 3 shown, before step S1, it may include: after receiving a charging instruction, obtaining the pose P1 of the charging hole through visual recognition; retreating the pose P1 of the charging hole by a first preset distance D1 along the Z-axis of the end coordinate system of the robotic arm to obtain a ready-to-insert pose P2.

[0056] The first preset distance D1 can be set in advance according to the robotic arm kinematics algorithm and actual requirements.

[0057] Among them, the pose of the charging hole can be obtained through a low-precision camera at the end of the robotic arm.

[0058] After obtaining the ready-to-insert pose P2, step S2 is executed: the robotic arm moves forward along the Z-axis of the end coordinate system of the robotic arm from the ready-to-insert pose, and when moving forward, it is judged whether the docking between the charging gun and the charging hole fails, is successful, or the charging gun contacts the edge of the charging hole.

[0059] In one example, the judgment in step S2 of whether the docking between the charging gun and the charging hole fails, is successful, or the charging gun contacts the edge of the charging hole when moving forward may include: collecting the forward distance of the robotic arm and the data of the force sensor when moving forward, where the force sensor is installed at the end of the robotic arm; judging whether the docking between the charging gun and the charging hole fails, is successful, or the charging gun contacts the edge of the charging hole according to the forward distance of the robotic arm and the data of the force sensor.

[0060] Further, based on the forward distance of the robotic arm and the data of the force sensor, it is determined whether the charging gun fails to dock with the charging hole, docks successfully, or the charging gun contacts the edge of the charging hole, including: if the forward distance is less than the second preset distance and the change value of the force sensor data is greater than the change threshold, it is determined that the charging gun fails to dock with the charging hole; if the forward distance is greater than the third preset distance and the change value of the force sensor data is less than or equal to the change threshold, it is determined that the charging gun docks successfully with the charging hole, where the third preset distance is greater than the second preset distance; if the forward distance is between the second preset distance and the third preset distance and the change value of the force sensor data is greater than the change threshold, it is determined that the charging gun contacts the edge of the charging hole.

[0061] Specifically, referring to Figure 3 , after moving the robotic arm to the ready-to-insert pose P2, the force sensor is tared and zeroed, and then the robotic arm slowly advances and probes along the end Z-axis, and the forward distance and the data Fz of the force sensor are observed.

[0062] During the forward probing process, if it is found that the change value of the force sensor data is greater than the change threshold ΔF and the forward distance is less than the second preset distance D2, it is considered that an abnormality occurs (such as the car hood closing in advance, etc.). At this time, the robotic arm retreats and a warning is given, and the docking fails.

[0063] During the forward probing process, if the forward distance is greater than the third preset distance D3 and the change value of the force sensor data is less than or equal to the change threshold (no obvious change), it is considered that the docking is successful. At this time, the robotic arm safely inserts the charging gun into the hole and then starts charging.

[0064] During the forward probing process, when advancing to a distance between D2 and D3 and it is found that the change value of the force sensor data is greater than ΔF, it is considered that the charging gun contacts the edge of the charging hole (the robotic arm is in the P3 pose). At this time, the following error correction algorithm is performed to correct the error and search for the charging hole near the charging hole. If found, the charging gun is fully inserted into the charging hole and charging starts; if not found, it retreats and a warning is given, and the docking fails.

[0065] Thus, automatic docking is achieved by combining the camera and the force sensor. During the forward movement of the robotic arm, the forward distance and the force sensor data are combined to determine in real time whether the docking is successful and whether the charging gun contacts the edge of the charging hole. While improving the success rate of automatic docking and reducing the probability of collision between the charging gun and the vehicle, low-precision vision can be used to reduce costs without affecting the positioning accuracy.

[0066] When contact occurs, the ready-to-insert pose P2 = (x2 y2 z2) and the contact pose P3 = (x3 y3 z3) are obtained, and the jacking plane is determined based on them.

[0067] In one example, determining the jacking plane according to the prepared insertion point pose and the contact point pose in step S3 may include:

[0068] S31. Obtain the following plane normal vector according to the prepared insertion point pose and the contact point pose:

[0069]

[0070] where is the plane normal vector, x2, y2, and z2 are the coordinates of the prepared insertion point pose P2, and x3, y3, and z3 are the coordinates of the contact point pose P3;

[0071] S32. Determine the following point-normal equation of the jacking plane according to the contact point pose P3 = (x3 y3 z3) and the plane normal vector :

[0072] n x (x - x3) + n y (y - y3) + n z (z - z3) = 0

[0073] where n x 、n y and n z are the normal vector coordinates of the jacking plane.

[0074] Furthermore, determine at least one candidate pose of the robotic arm on the jacking plane, specifically including:

[0075] S33. Take two basis vectors and

[0076]

[0077] where n x 、n y and n z are the normal vector coordinates of the jacking plane; the basis vectors are as shown in Figure 4 .

[0078] S34. The pose formula of any point P on the jacking plane represented by the basis vectors:

[0079]

[0080] where is the vector of any point P, is the contact point vector, and s and t are arbitrary variables.

[0081] S35. Determine the range of any variable, and obtain at least one candidate pose of the robotic arm according to the range of the any variable and the pose formula of any point P. Specifically, it may include:

[0082] S351. Expand the pose formula of any point P to obtain:

[0083]

[0084] where x, y, and z are the coordinates of any point P;

[0085] S352. With the contact point pose P3 as the center and R as the radius, draw a circle on the socket plane to represent the search for the charging hole area;

[0086] S353. Combine the circle with the pose formula of any point P to obtain the range formula of any variable:

[0087]

[0088] S354. Expand the range formula of any variable to obtain:

[0089]

[0090] Let B = 2n y n z ; F = -R 2 , and simplify the above formula to obtain:

[0091] As 2 + Bst + Ct 2 + F ≤ 0

[0092] S355. Adopt the equation-solving technique: Rotate the coordinate system (rotate by θ around the z-axis) for the formula As 2 + Bst + Ct 2 + F ≤ 0 to eliminate the cross term Bst, and the following can be obtained:

[0093] A′s′ 2 + C′t′ 2 ≤ R 2

[0094] where:

[0095] S356. The relationships between the new variables s′ and t′ after rotation and the original variables s and t are as follows:

[0096] s = s′cosθ - t′sinθ

[0097] t = s′sinθ + t′cosθ

[0098] S357. According to the above formula, the following standard ellipse equation formula and the ranges of the new variables s' and t' are obtained:

[0099]

[0100] S358. Combine the above expressions to obtain the ranges of the original variables s and t, and substitute them into the formula obtained by expanding the pose formula of any point P above (i.e., the formula in S351) to obtain at least one candidate pose of the robotic arm.

[0101] Then, combine the pose of the target charging hole obtained by visual recognition, and let the robotic arm traverse all candidate poses in sequence. During the traversal process, determine whether the charging gun is successfully docked with the charging hole (find the charging hole) or the docking fails (the charging hole is not found).

[0102] Further, determining whether the charging gun is successfully docked with the charging hole or the docking fails during the traversal process in step S5 may include: during the traversal process, obtain the data of the force sensor, where the force sensor is installed at the end of the robotic arm; if the change value of the data of the force sensor is less than the change threshold, determine that the docking is successful, otherwise determine that the docking fails.

[0103] Specifically, combine the pose of the target charging hole obtained by visual recognition, and let the robotic arm traverse all candidate poses in sequence. During the traversal process, obtain the force sensor data in real time. When the change from the initial force is less than ΔF, it is considered that the charging jack has been found, and then the gun is inserted for charging. Otherwise, the charging hole is not found, the robotic arm retreats and alarms, and the docking fails.

[0104] Through the above steps, in the embodiment of the present invention, when in the visual blind area, the probability of collision between the charging gun and the vehicle can be effectively reduced. When the visual accuracy is inaccurate, combined with the force sensor, an error correction algorithm (steps S3 to S5) is used to perform error correction, further improving the docking success rate.

[0105] In summary, the automatic charging gun docking method in the embodiment of the present invention has the following advantages:

[0106] Combining a low-resolution camera and a force sensor, and using an error correction algorithm to improve the docking success rate, reduce the risk of rubbing against the vehicle, and when in the camera blind area, the present invention can also effectively avoid collision with the vehicle.

[0107] Compared with the existing solutions, the greatest advantage of the present invention is to use a lower-cost method (combining a low-precision camera and a force sensor). Without affecting the positioning accuracy, the automatic charging process can still be completed, and at the same time, when in the camera blind area, it can avoid the collision between the charging gun and the vehicle, protecting the legitimate rights and interests of the vehicle owner.

[0108] Corresponding to the automatic charging gun docking method of the above embodiments, the present invention also provides an automatic charging gun docking device.

[0109] Figure 5 It is a structural block diagram of the automatic charging gun docking device according to an embodiment of the present invention.

[0110] As Figure 5 shown, the automatic charging gun docking device includes a moving module 10, a first docking module 20, a first determination module 30, a second determination module 40, and a second docking module 50.

[0111] The moving module 10 is configured to move the robotic arm to a ready-to-insert pose, where a charging gun is installed at the end of the robotic arm; the first docking module 20 is configured to move the robotic arm forward along the Z-axis of the end coordinate system of the robotic arm from the ready-to-insert pose, and determine whether the docking of the charging gun and the charging hole fails, succeeds, or the charging gun contacts the edge of the charging hole during the forward movement; the first determination module 30 is configured to, if the charging gun contacts the edge of the charging hole, obtain the ready-to-insert pose and the contact pose, and determine the jack plane according to the ready-to-insert pose and the contact pose; the second determination module 40 is configured to determine at least one candidate pose of the robotic arm on the jack plane; the second docking module 50 is configured to make the robotic arm traverse all the candidate poses, and determine whether the docking of the charging gun and the charging hole is successful or fails during the traversal.

[0112] It should be noted that for the specific implementation manner of the automatic charging gun docking device, reference can be made to the specific implementation manner of the above automatic charging gun docking method. To avoid redundancy, it will not be elaborated here in detail.

[0113] The automatic charging gun docking device according to the embodiment of the present invention can determine that the charging gun contacts the edge of the charging hole when the robotic arm moves forward, and when contacting, correct the pose of the robotic arm in a timely manner according to the ready-to-insert pose and the contact pose and then continue docking, which can improve the docking success rate and does not require a high-precision camera, reducing the cost.

[0114] In the description of this specification, the description with reference to terms such as "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic descriptions of the above terms do not necessarily refer to the same embodiment or example. Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. An automatic charging gun docking method, characterized in that, It includes the following steps: Move the robotic arm to the ready-to-insert pose, where a charging gun is installed at the end of the robotic arm. Make the robotic arm advance along the Z-axis of the end coordinate system of the robotic arm from the ready-to-insert pose, and during the advancement, determine whether the docking between the charging gun and the charging hole fails, succeeds, or the charging gun contacts the edge of the charging hole. If the charging gun contacts the edge of the charging hole, obtain the ready-to-insert pose and the contact pose, and determine the jacking plane according to the ready-to-insert pose and the contact pose. Determine at least one candidate pose of the robotic arm on the jacking plane. Make the robotic arm traverse all the candidate poses, and during the traversal, determine whether the docking between the charging gun and the charging hole succeeds or fails.

2. The automatic charging gun docking method according to claim 1, wherein It also includes: If the docking between the charging gun and the charging hole fails, make the robotic arm retreat and give an alarm. If the docking between the charging gun and the charging hole succeeds, make the robotic arm insert the charging gun into the charging hole and charge.

3. The automatic charging gun docking method according to claim 1, wherein, Before moving the robotic arm to the ready-to-insert pose, it also includes: After receiving the charging instruction, obtain the pose P1 of the charging hole through visual recognition. Retreat the pose P1 of the charging hole along the Z-axis of the end coordinate system of the robotic arm by a first preset distance to obtain the ready-to-insert pose P2.

4. The automatic charging gun docking method according to claim 1, wherein Determine whether the docking between the charging gun and the charging hole fails, succeeds, or the charging gun contacts the edge of the charging hole during the advancement, including: Collect the advancement distance of the robotic arm and the data of the force sensor during the advancement, where the force sensor is installed at the end of the robotic arm. Judge whether the docking between the charging gun and the charging hole fails, succeeds, or the charging gun contacts the edge of the charging hole according to the advancement distance of the robotic arm and the data of the force sensor.

5. The automatic charging gun docking method according to claim 4, wherein, Judge whether the docking between the charging gun and the charging hole fails, succeeds, or the charging gun contacts the edge of the charging hole according to the advancement distance of the robotic arm and the data of the force sensor, including: If the advancement distance is less than the second preset distance and the change value of the data of the force sensor is greater than the change threshold, determine that the charging gun fails to dock with the charging hole. If the advancement distance is greater than the third preset distance and the change value of the data of the force sensor is less than or equal to the change threshold, determine that the charging gun successfully docks with the charging hole, where the third preset distance is greater than the second preset distance. If the advancement distance is between the second preset distance and the third preset distance and the change value of the data of the force sensor is greater than the change threshold, determine that the charging gun contacts the edge of the charging hole.

6. The automatic charging gun docking method according to claim 1, characterized in that Determine the jacking plane according to the ready-to-insert pose and the contact pose, including: Obtain the following plane normal vector according to the ready-to-insert pose and the contact pose: wherein, is the plane normal vector, x2, y2, and z2 are the coordinates of the pose P2 of the point to be inserted, and x3, y3, and z3 are the coordinates of the contact pose P3; Determine the following point-normal equation of the jacking plane according to the contact pose and the plane normal vector: n x (x - x3)+n y (y - y3)+n z (z - z3) = 0 Among them, n x , n y and n z are the normal vector coordinates of the jack plane.

7. The automatic charging gun docking method according to claim 1, characterized in that Determine at least one candidate pose of the robotic arm on the jacking plane, including: Take two basis vectors on the jack plane and where n x , n y and n z are the normal vector coordinates of the jack plane; The pose formula of any point P on the jacking plane represented by the base vector: Among them, is the vector of any point P, is the contact point vector, and s and t are arbitrary variables; Determine the range of any variable, and obtain at least one candidate pose of the robotic arm according to the range of the any variable and the pose formula of the any point P.

8. The automatic charging gun docking method according to any one of claims 1-7, characterized in that Determine whether the docking between the charging gun and the charging hole succeeds or fails during the traversal, including: During the traversal process, obtain the data of the force sensor, where the force sensor is installed at the end of the robotic arm; If the change value of the data of the force sensor is less than the change threshold, it is determined that the docking is successful, otherwise it is determined that the docking fails.

9. An automatic charging gun docking device, characterized in that It includes: A moving module for moving the robotic arm to the pose of the ready-to-insert position, where a charging gun is installed at the end of the robotic arm; A first docking module for advancing the robotic arm along the Z-axis of the coordinate system at the end of the robotic arm from the ready-to-insert position, and determining whether the docking of the charging gun and the charging hole fails, is successful, or the charging gun contacts the edge of the charging hole during the advancement; A first determination module for, if the charging gun contacts the edge of the charging hole, obtaining the ready-to-insert position and the contact position, and determining the jack plane according to the ready-to-insert position and the contact position; A second determination module for determining at least one candidate pose of the robotic arm on the jack plane; A second docking module for causing the robotic arm to traverse all the candidate poses, and determining whether the docking of the charging gun and the charging hole is successful or fails during the traversal.

Images