Control method and system for an automatic charging device

Abstract

The application relates to the technical field of automatic charging, and provides a control method and system of an automatic charging device, a mechanical arm of the automatic charging device comprising three rotary joints, one translation joint and joint arms between the joints, the rotary planes of the three rotary joints being parallel to each other, the translation joint driving a charging gun to move along a translation axis, the method comprising the following steps: acquiring current pose parameters of the mechanical arm and a target pose of the charging gun; calculating a current pose of the charging gun based on the current pose parameters; and planning a joint path of the mechanical arm based on a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angles of the rotary joints according to the current pose and the target pose. The application does not need to establish a D-H kinematics model and does not need to perform complex matrix operations; moreover, the two-dimensional search method can directly obtain an optimal solution without iterative calculation, reduces the calculation time cost, and has high overall planning efficiency.

Description

Technical Field

[0001] This invention relates to the field of automatic charging technology, and more specifically to a control method and system for an automatic charging device. Background Technology

[0002] As one of the essential technologies for autonomous driving, automatic charging technology typically uses a robotic arm with multiple rotating joints to plug and unplug the charging gun. Compared with manual charging, it has significant advantages in terms of user experience, comfort, and safety.

[0003] Traditional robotic arm motion planning mostly employs the DH parameter method to construct the robotic arm's kinematic model. This method establishes a link coordinate system, defines geometric relationships such as link length, torsion angle, and offset, forming a unified kinematic description framework, and then constructs the kinematic equations of the entire robotic arm. In actual path planning, complex matrix multiplication and inversion operations are repeatedly performed to solve the mapping relationship between joint variables and end effector pose. Since each iteration involves numerous trigonometric transformations and homogeneous coordinate calculations, the entire planning process is computationally cumbersome, resulting in low overall planning efficiency. Summary of the Invention

[0004] To address the aforementioned technical problems, this invention provides a control method and system for an automatic charging device. It eliminates the need for establishing a DH kinematic model and performing complex matrix operations. Furthermore, the two-dimensional search method can directly obtain the optimal solution without iterative calculations, reducing computational time costs and resulting in high overall planning efficiency.

[0005] The technical solution adopted in this invention is as follows:

[0006] A control method for an automatic charging device, the automatic charging device comprising a robotic arm and a charging gun disposed at the end of the robotic arm, the robotic arm comprising three rotary joints, one translational joint, and joint arms between the joints, the rotation planes of the three rotary joints being parallel to each other, and the translational joint driving the charging gun to move along a translation axis, the method comprising:

[0007] S1, obtain the current pose parameters of the robotic arm and the target pose of the charging gun. The current pose parameters include the joint parameters of each joint and the length of each joint arm at the current moment.

[0008] S2, calculate the current pose of the charging gun based on the current pose parameters;

[0009] S3. Based on the current pose and the target pose, and using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint, plan the joint path of the robotic arm.

[0010] Furthermore, the control method of the automatic charging device of the present invention may also have the following additional technical features:

[0011] According to one embodiment of the present invention, the joint parameters include joint angle parameters and translation parameters, wherein the joint angle parameters include the joint angles of each rotary joint, and the translation parameters include the translation distance of the translational joint.

[0012] According to one embodiment of the present invention, the translation joint is connected to the first rotational joint, the two ends of the first joint arm are respectively connected to the first rotational joint and the second rotational joint, the two ends of the second joint arm are respectively connected to the second rotational joint and the third rotational joint, and the two ends of the third joint arm are respectively connected to the third rotational joint and the charging gun. The current pose includes the rotation angle and three-dimensional coordinates of the charging gun relative to the translation axis at the current moment. The translation axis of the translation joint is the vertical axis, and the yaw angle ψ of the charging gun is... c The calculation formula is:

[0013] ψ c =θ1+θ2+θ3+π / 2

[0014] Where θ1, θ2, and θ3 represent the joint angles of the three rotational joints, respectively;

[0015] The three-dimensional coordinates (x) of the charging gun c ,y c ,z c The calculation formulas for ) are as follows:

[0016] x c =-R3cos(θ1+θ2+θ3)+R2cos(θ1+θ2)-R1cosθ1

[0017] y c =-R3sin(θ1+θ2+θ3)+R2sin(θ1+θ2)-R1sinθ1

[0018] z c =H+△H

[0019] Wherein, R1, R2, and R3 represent the lengths of the first to third joint arms, respectively, H represents the initial height of the robotic arm, and ΔH represents the translation distance of the translation joint.

[0020] According to an embodiment of the present invention, step S3 specifically includes: S31, taking the current pose and the target pose as the starting pose and the final pose respectively, obtaining the linear motion path of the charging gun, and planning n process poses based on the linear motion path, where n is an integer greater than 1, and the first target pose is the starting pose; S32, obtaining the translation parameters of the nth process pose based on the three-dimensional coordinates of the charging gun in the nth process pose; S33, obtaining the joint angle parameters of the nth process pose based on the joint angle parameters of the charging gun in the (n-1)th process pose, using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotation joint; S34, planning the joint path of the robotic arm based on the joint parameters of each process pose.

[0021] According to an embodiment of the present invention, step S33 specifically includes: setting a joint angle search space for the second and third rotary joints corresponding to the nth process pose, and generating a two-dimensional matrix; based on the two-dimensional matrix, searching for multiple potential process poses based on the (n-1)th process pose of the charging gun; selecting the potential process pose closest to the nth process pose among the multiple potential process poses as the nth process pose, and using the joint angles of the corresponding second and third rotary joints as the final solution of the two-dimensional search method; and obtaining the joint angle of the first rotary joint corresponding to the nth process pose based on the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint.

[0022] In addition, the present invention also proposes a control system for an automatic charging device.

[0023] A control system for an automatic charging device includes a robotic arm and a charging gun disposed at the end of the robotic arm. The robotic arm includes three rotary joints, one translational joint, and joint arms between each joint. The rotation planes of the three rotary joints are parallel to each other. The translational joint drives the charging gun to move along a translation axis. The system includes: an acquisition module for acquiring the current pose parameters of the robotic arm and the target pose of the charging gun, the current pose parameters including the joint parameters of each joint at the current moment and the length of each joint arm; a calculation module for calculating the current pose of the charging gun based on the current pose parameters; and a planning module for planning the joint path of the robotic arm based on the current pose and the target pose, using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint.

[0024] According to one embodiment of the present invention, the joint parameters include joint angle parameters and translation parameters, wherein the joint angle parameters include the joint angles of each rotary joint, and the translation parameters include the translation distance of the translational joint.

[0025] According to one embodiment of the present invention, the translation joint is connected to the first rotational joint, the two ends of the first joint arm are respectively connected to the first rotational joint and the second rotational joint, the two ends of the second joint arm are respectively connected to the second rotational joint and the third rotational joint, and the two ends of the third joint arm are respectively connected to the third rotational joint and the charging gun. The current pose includes the rotation angle and three-dimensional coordinates of the charging gun relative to the translation axis at the current moment. The translation axis of the translation joint is the vertical axis, and the yaw angle ψ of the charging gun is... c The calculation formula is:

[0026] ψ c =θ1+θ2+θ3+π / 2

[0027] Where θ1, θ2, and θ3 represent the joint angles of the three rotational joints, respectively;

[0028] The three-dimensional coordinates (x) of the charging gun c ,y c ,z c The calculation formulas for ) are as follows:

[0029] x c =-R3cos(θ1+θ2+θ3)+R2cos(θ1+θ2)-R1cosθ1

[0030] y c =-R3sin(θ1+θ2+θ3)+R2sin(θ1+θ2)-R1sinθ1

[0031] z c =H+△H

[0032] Wherein, R1, R2, and R3 represent the lengths of the first to third joint arms, respectively, H represents the initial height of the robotic arm, and ΔH represents the translation distance of the translation joint.

[0033] According to an embodiment of the present invention, the planning module specifically includes: a preset unit, which is used to obtain the linear motion path of the charging gun by taking the current pose and the target pose as the starting pose and the final pose, respectively, and to plan n process poses based on the linear motion path, where n is an integer greater than 1, and the first target pose is the starting pose; a first calculation unit, which is used to obtain the translation parameters of the nth process pose based on the three-dimensional coordinates of the charging gun in the nth process pose; a second calculation unit, which is used to obtain the joint angle parameters of the nth process pose based on the joint angle parameters of the charging gun in the (n-1)th process pose, based on a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotation joint; and a planning unit, which is used to plan the joint path of the robotic arm based on the joint parameters of each process pose.

[0034] According to one embodiment of the present invention, the second calculation unit is specifically used for: setting a joint angle search space for the second and third rotary joints corresponding to the nth process pose, and generating a two-dimensional matrix; based on the two-dimensional matrix, searching for multiple potential process poses based on the (n-1)th process pose of the charging gun; selecting the potential process pose closest to the nth process pose among the multiple potential process poses as the nth process pose, and using the joint angles of the corresponding second and third rotary joints as the final solution of the two-dimensional search method; and obtaining the joint angle of the first rotary joint corresponding to the nth process pose based on the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint.

[0035] The beneficial effects of this invention are:

[0036] The control method of the automatic charging device of the present invention, by setting three parallel rotational joints and translational joints on the robotic arm, and planning the joint path of the robotic arm based on the geometric relationship between the rotation angle of the charging gun relative to the translational axis and the joint angle of the rotational joint, does not require the establishment of a DH kinematic model or complex matrix operations; moreover, the two-dimensional search method can directly obtain the optimal solution without iterative calculation, reducing the computation time cost and achieving high overall planning efficiency. Attached Figure Description

[0037] Figure 1 This is a schematic diagram illustrating an application scenario of an automatic charging device according to an embodiment of the present invention;

[0038] Figure 2 This is a schematic diagram of the structure of an automatic charging device according to an embodiment of the present invention;

[0039] Figure 3This is a schematic diagram of the structure of a robotic arm with a long and short arm design according to an embodiment of the present invention;

[0040] Figure 4 This is a schematic diagram of a robotic arm with a short-long arm design according to an embodiment of the present invention;

[0041] Figure 5 This is a flowchart of the control method of the automatic charging device according to an embodiment of the present invention;

[0042] Figure 6 This is a schematic diagram showing the positional relationship between the joints, joint arms, and charging gun in a robotic arm according to an embodiment of the present invention.

[0043] Figure 7 This is a schematic diagram illustrating an application scenario of an automatic charging device according to another embodiment of the present invention;

[0044] Figure 8 This is a schematic diagram of the structure of an automatic charging device according to another embodiment of the present invention;

[0045] Figure 9 This is a block diagram of the control system of the automatic charging device according to an embodiment of the present invention. Detailed Implementation

[0046] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0047] Most automatic charging devices use 6-axis or higher industrial robots as actuators. Common charging robotic arms are implemented using 6-axis serial robotic arms, which are equipped with 6 rotation axes. The rotation axes of a 6-DOF robotic arm have a large torque load, and the insertion process requires coupled control, which is difficult to control and requires high precision of the rotation axes; moreover, it also has the problems of high cost and large footprint.

[0048] like Figure 1 and 2As shown, the automatic charging device of this embodiment includes a robotic arm and a charging gun disposed at the end of the robotic arm. The robotic arm may include three rotary joints (first rotary joint 11, second rotary joint 12, and third rotary joint 13), one translational joint 14, and joint arms between each joint (first joint arm 15, second joint arm 16, and third joint arm 17). The rotation planes of the three rotary joints are parallel to each other. The translational joint drives the charging gun to move along the translation axis, thereby enabling the charging gun to perform four degrees of freedom of motion: two-dimensional translation, rotation about the translation axis, and translation along the translation axis in a plane parallel to the rotation plane. This achieves a simplified mechanism, reduces the load-bearing requirements of each joint, and allows for the selection of smaller joints, thus meeting the goals of smaller overall device size, lower cost, and larger operating space. The rotary joints can be driven by a rotary motor, gear transmission, synchronous belt transmission, etc.; the translational joints can be driven by a rotary motor in conjunction with a lead screw module to achieve linear motion, or they can be driven by an electric cylinder push rod or a pneumatic cylinder telescopic rod, etc. This embodiment does not limit these methods.

[0049] For ease of storage and assembly, the first joint arm 15 and the second joint arm 16 can be designed as a long-short arm or a short-long arm. If the first joint arm 15 is longer than the second joint arm 16, it can be called a long-short arm (e.g., Figure 3 (As shown); if the first joint arm 15 is shorter than the second joint arm 16, it can be called a short-long arm (e.g. Figure 4 (as shown); In this way, when all the rotary joints are retracted, their central axes can be approximately located on the same plane, minimizing the volume of the folded joint arm as much as possible, so that the robotic arm occupies less space, which is conducive to maximizing the utilization of the charging space. In some other embodiments, the size of each joint arm can also be adjusted according to specific needs, but this embodiment does not limit this.

[0050] It should be noted that, in Figures 1-4 In the illustrated embodiment, the three rotary joints of the robotic arm are connected in series. In some other embodiments, the three rotary joints may also be connected in parallel or in a series-parallel configuration. This embodiment does not limit this to any particular configuration.

[0051] like Figure 5 As shown, the control method of the automatic charging device in this embodiment of the invention includes:

[0052] S1, acquire the current pose parameters of the robotic arm and the target pose of the charging gun. The current pose parameters include the joint parameters of each joint and the length of each joint arm at the current moment. Among them, the joint parameters may include joint angle parameters and translation parameters. The joint angle parameters include the joint angles of each rotary joint, and the translation parameters include the translation distance of the translation joint.

[0053] In one specific embodiment of the present invention, the end effector of the robotic arm may also be equipped with an identification and positioning device, such as a positioning camera, which identifies the charging port of the vehicle to be charged for positioning and obtains the pose of the charging port, thereby determining the target pose of the charging gun during the automatic docking and charging process of the automatic charging device; when the automatic charging device has finished charging, the preset standby pose or the pose of the automatically docked charging gun can be used as the target pose of the charging gun, but this embodiment does not limit this.

[0054] S2, calculate the current pose of the charging gun based on the current pose parameters.

[0055] like Figure 2 As shown, in one embodiment of the present invention, the translation joint 14 can be directly or indirectly connected to the first rotary joint 11. The two ends of the first joint arm 15 are respectively connected to the first rotary joint 11 and the second rotary joint 12. The two ends of the second joint arm 16 are respectively connected to the second rotary joint 12 and the third rotary joint 13. The two ends of the third joint arm 17 are respectively connected to the third rotary joint 13 and the charging gun 2. The current pose includes the rotation angle of the charging gun 2 relative to the translation axis and its three-dimensional coordinates at the current moment. When the translation axis of the translation joint 14 is the vertical axis, each joint and joint arm in the robotic arm can be projected onto the XOY plane of the global coordinate system, as shown below. Figure 6 As shown, the yaw angle ψ of the charging gun is obtained. c The formula for calculating (the angle of rotation about the vertical axis) is:

[0056] ψ c =θ1+θ2+θ3+π / 2

[0057] Where θ1, θ2, and θ3 represent the joint angles of the three rotational joints, respectively;

[0058] The three-dimensional coordinates (x) of the charging gun c ,y c ,z c The calculation formulas for ) are as follows:

[0059] x c =-R3cos(θ1+θ2+θ3)+R2cos(θ1+θ2)-R1cosθ1

[0060] y c =-R3sin(θ1+θ2+θ3)+R2sin(θ1+θ2)-R1sinθ1

[0061] z c =H+△H

[0062] Where R1, R2, and R3 represent the lengths of the first to third joint arms, respectively, H represents the initial height of the robotic arm, and ΔH represents the translation distance of the translation joint.

[0063] like Figure 7 and Figure 8 As shown, in some other embodiments of the present invention, the translation axis of the translation joint 14 can also be a horizontal axis. In this case, the rotation angle and three-dimensional coordinates of the charging gun relative to the horizontal axis can be obtained in a similar manner as described above, which will not be repeated here.

[0064] S3, based on the current pose and the target pose, and using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint, the joint path of the robotic arm is planned.

[0065] It is understandable that, since the rotation planes of each rotary joint are parallel to each other, the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint remains unchanged. After finding the joint angles of two rotary joints using a two-dimensional search method, the joint angle of the remaining rotary joint can be calculated based on the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint. This simplifies the joint path planning calculation process, reduces computation time costs, and improves planning efficiency.

[0066] To further reduce the complexity of joint path planning, the motion path of the charging gun can be set as a linear motion path when it is preset. In this case, step S3 specifically includes the following steps S31~S34:

[0067] S31. The current pose and the target pose are taken as the starting pose and the final pose, respectively, to obtain the linear motion path of the charging gun. Based on the linear motion path, n process poses are planned, where n is an integer greater than 1, and the first target pose is the starting pose.

[0068] Specifically, the linear motion path can be divided into n points at specified intervals to obtain the three-dimensional coordinates of the charging gun in n process poses. If the rotation angle of the charging gun relative to the translation axis remains unchanged in both the target pose and the current pose, then ψ c It remains unchanged, i.e., ψ cn =ψ c If the rotation angle of the charging gun changes, ψ can also be set at one or more specified process poses (e.g., several process poses close to the initial pose, several process poses close to the final pose). c The changed values ​​are to meet the requirements.

[0069] S32, based on the three-dimensional coordinates of the charging gun in the nth process pose, obtain the translation parameters of the nth process pose. For example: when the three-dimensional coordinates of the nth process pose are (x... cn ,y cn ,z cn When z cn =H+△H nTherefore, the translation distance of the translation joint corresponding to the nth process pose can be obtained as z. cn -H.

[0070] S33, based on the joint angle parameters of the charging gun in the (n-1)th process pose, and using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotation joint, the joint angle parameters (θ) of the nth process pose are obtained. 1_n ,θ 2_n ,θ 3_n ).

[0071] In one embodiment of the present invention, step S33 may specifically include the following steps S331~S334:

[0072] S331, Set the joint angle search space for the second and third rotary joints corresponding to the nth process pose, and generate a two-dimensional matrix.

[0073] In one specific embodiment of the present invention, the joint angle search space of the second rotary joint θ2 can be set as (θ 2_n-1 -δ 1, θ 2_n-1 +δ2), the joint angle search space of the third rotational joint θ3 is (θ 3_n-1 -δ 3, θ 3_n-1 +δ4), the values ​​of δ1 and δ2 can be the same, for example, both 1°, or they can be set to asymmetrical values ​​depending on the specific situation; δ3 and δ4 can also be set in a similar way, which will not be elaborated here.

[0074] By dividing the two search spaces into m points, a two-dimensional matrix (θ) can be generated. 2_1~m ,θ 3_1~m ).

[0075] S332, based on the two-dimensional matrix, multiple potential process poses are obtained by searching the (n-1)th process pose of the charging gun.

[0076] Due to the rotation angle ψ in the pose of the charging gun during the (n-1)th process cn-1 Since the linear motion path is determined during planning, substituting the joint angle combination of the second and third rotary joints in the two-dimensional matrix into the calculation formula for the three-dimensional coordinates of the charging gun yields m potential process poses, whose corresponding two-dimensional coordinates in the XOY plane can be expressed as (x c,1~m ,y c,1~m ).

[0077] S333, select the potential process pose that is closest to the nth process pose from multiple potential process poses as the nth process pose, and use the joint angles of its corresponding second and third rotational joints as the final solution (θ) of the two-dimensional search method.2_n , θ 3_n ).

[0078] Understandably, since each process pose is generated simultaneously with determining the linear motion path of the charging gun, the error in joint angle calculation only occurs in a single process pose during the calculation process and does not accumulate between different process poses. Considering that the adjustment capability of rotational joint angles is usually limited, the step size in the search space can be adjusted according to the adjustment capability of the rotational joint. In this embodiment, the error between the potential process pose corresponding to the final solution and the initially generated nth process pose can be ignored, and it can be directly used as the nth process pose to participate in the joint angle calculation of the next process pose, so as to ensure the accuracy of joint angle planning.

[0079] S334, based on the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint, obtain the joint angle of the first rotary joint corresponding to the nth process pose.

[0080] Due to ψ c =θ1+θ2+θ3+π / 2, based on the final solution (θ) obtained in step S333. 2_n ,θ 3_n Then the joint angle θ of the first rotary joint corresponding to the nth process pose can be obtained. 1_n =ψ cn -θ 2_n -θ 3_n -π / 2.

[0081] In some other embodiments of the present invention, the motion path of the charging gun can also be planned according to the specific scenario, combining the current pose and the target pose. For example, by identifying surrounding obstacles, obstacle avoidance path planning can be performed, and then n process poses can be determined. In this case, the joint parameter calculation of each process pose can refer to the relevant calculation process above, which will not be repeated here.

[0082] S34 plans the joint path of the robotic arm based on the joint parameters of each process pose, thereby providing a reference for the joint motion control of the robotic arm.

[0083] When controlling the joints of the robotic arm, the joint parameters of each process position in the joint path can be directly used as joint control commands, or a feedforward feedback strategy can be further introduced for control, etc. This embodiment does not limit this.

[0084] According to the control method of the automatic charging device of the present invention, the joint path of the robotic arm is planned by setting three parallel rotational joints and translational joints on the robotic arm, and by using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translational axis and the joint angle of the rotational joint. This method does not require the establishment of a DH kinematic model or complex matrix operations. Moreover, the two-dimensional search method can directly obtain the optimal solution without iterative calculation, thereby reducing the computation time cost and achieving high overall planning efficiency.

[0085] Corresponding to the control method of the automatic charging device described above, this invention also proposes a control system for the automatic charging device, used to implement the control method of the automatic charging device described above. Since the system embodiments of this invention correspond to the method embodiments described above, details not disclosed in the system embodiments can be found in the method embodiments described above, and will not be repeated here.

[0086] like Figure 9 As shown, the control system of the automatic charging device in this embodiment of the invention includes an acquisition module 10, a calculation module 20, and a planning module 30. The acquisition module 10 is used to acquire the current pose parameters of the robotic arm and the target pose of the charging gun. The current pose parameters include the joint parameters of each joint at the current moment and the length of each joint arm. The joint parameters include joint angle parameters and translation parameters. The joint angle parameters include the joint angles of each rotary joint, and the translation parameters include the translation distance of the translation joint. The calculation module 20 calculates the current pose of the charging gun based on the current pose parameters. The planning module 30 is used to plan the joint path of the robotic arm based on the current pose and the target pose, using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint.

[0087] In one embodiment of the present invention, the translation joint can be directly or indirectly connected to the first rotary joint. The two ends of the first joint arm are respectively connected to the first and second rotary joints. The two ends of the second joint arm are respectively connected to the second and third rotary joints. The two ends of the third joint arm are respectively connected to the third rotary joint and the charging gun. The current pose includes the rotation angle and three-dimensional coordinates of the charging gun relative to the translation axis at the current moment. When the translation axis of the translation joint is the vertical axis, projecting each joint and joint arm in the robotic arm onto a planar coordinate system yields the yaw angle ψ of the charging gun. c The calculation formula is:

[0088] ψ c =θ1+θ2+θ3+π / 2

[0089] Where θ1, θ2, and θ3 represent the joint angles of the three rotational joints, respectively;

[0090] The three-dimensional coordinates (x) of the charging gun c ,y c,z c The calculation formulas for ) are as follows:

[0091] x c =-R3cos(θ1+θ2+θ3)+R2cos(θ1+θ2)-R1cosθ1

[0092] y c =-R3sin(θ1+θ2+θ3)+R2sin(θ1+θ2)-R1sinθ1

[0093] z c =H+△H

[0094] Where R1, R2, and R3 represent the lengths of the first to third joint arms, respectively, H represents the initial height of the robotic arm, and ΔH represents the translation distance of the translation joint.

[0095] In one embodiment of the present invention, the planning module 30 may specifically include a preset unit, a first calculation module 20, a second calculation module 20, and a planning unit. The preset unit is used to obtain the linear motion path of the charging gun by taking the current pose and the target pose as the starting pose and the final pose, respectively, and to plan n process poses based on the linear motion path, where n is an integer greater than 1, and the first target pose is the starting pose. The first calculation unit is used to obtain the translation parameters of the nth process pose based on the three-dimensional coordinates of the charging gun in the nth process pose. The second calculation unit is used to obtain the joint angle parameters of the nth process pose based on the joint angle parameters of the charging gun in the (n-1)th process pose, using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint. The planning unit is used to plan the joint path of the robotic arm based on the joint parameters of each process pose.

[0096] In one embodiment of the present invention, the second calculation unit is specifically used to: set the joint angle search space of the second and third rotary joints corresponding to the nth process pose, and generate a two-dimensional matrix; based on the two-dimensional matrix, search for multiple potential process poses based on the (n-1)th process pose of the charging gun; select the potential process pose closest to the nth process pose among the multiple potential process poses as the nth process pose, and use the joint angles of the corresponding second and third rotary joints as the final solution of the two-dimensional search method; and obtain the joint angle of the first rotary joint corresponding to the nth process pose based on the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint.

[0097] The control system of the automatic charging device according to an embodiment of the present invention, by setting three parallel rotational joints and translational joints on the robotic arm, and planning the joint path of the robotic arm based on the geometric relationship between the rotation angle of the charging gun relative to the translational axis and the joint angle of the rotational joint, does not require the establishment of a DH kinematic model or complex matrix operations; moreover, the two-dimensional search method can directly obtain the optimal solution without iterative calculation, reducing the computation time cost and achieving high overall planning efficiency.

[0098] In the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0099] In this invention, unless otherwise explicitly specified and limited, the terms "installation", "connection", "linking", "fixing", etc., should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral part; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; they can refer to the internal connection of two components or the interaction between two components.

[0100] In this invention, unless otherwise explicitly specified and limited, "above" or "below" the second feature can mean that the first feature is in direct contact with the second feature, or that the first feature is in indirect contact with the second feature through an intermediate medium. Furthermore, "above," "over," and "on top" of the second feature can mean that the first feature is directly above or diagonally above the second feature, or simply that the first feature is at a higher horizontal level than the second feature. "Below," "below," and "under" the second feature can mean that the first feature is directly below or diagonally below the second feature, or simply that the first feature is at a lower horizontal level than the second feature.

[0101] The execution order of the steps shown in the flowchart is the preferred implementation. In other embodiments of the present invention, the order can be adjusted according to the functions involved in each step, for example, they can be executed simultaneously or in the reverse order.

[0102] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequential list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in connection with, an instruction execution system, apparatus, or device. For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit a program for use by, or in connection with, an instruction execution system, apparatus, or device.

[0103] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0104] Furthermore, the functional units in the various embodiments of the present invention can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

Claims

1. A control method for an automatic charging device, characterized in that, The automatic charging device includes a robotic arm and a charging gun disposed at the end of the robotic arm. The robotic arm includes three rotary joints, one translational joint, and joint arms between the joints. The rotation planes of the three rotary joints are parallel to each other. The translational joint drives the charging gun to move along a translational axis. The method includes: S1, obtain the current pose parameters of the robotic arm and the target pose of the charging gun. The current pose parameters include the joint parameters of each joint and the length of each joint arm at the current moment. S2, calculate the current pose of the charging gun based on the current pose parameters; S3. Based on the current pose and the target pose, and using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint, plan the joint path of the robotic arm.

2. The control method for the automatic charging device according to claim 1, characterized in that, The joint parameters include joint angle parameters and translation parameters. The joint angle parameters include the joint angles of each rotational joint, and the translation parameters include the translation distance of the translational joint.

3. The control method for the automatic charging device according to claim 2, characterized in that, The translation joint is connected to the first rotational joint. The two ends of the first joint arm are connected to the first and second rotational joints, respectively. The two ends of the second joint arm are connected to the second and third rotational joints, respectively. The two ends of the third joint arm are connected to the third rotational joint and the charging gun, respectively. The current pose includes the rotation angle and three-dimensional coordinates of the charging gun relative to the translation axis at the current moment. The translation axis of the translation joint is the vertical axis, and the yaw angle ψ of the charging gun is... c The calculation formula is: ψ c =θ1+θ2+θ3+π / 2 Where θ1, θ2, and θ3 represent the joint angles of the three rotational joints, respectively; The three-dimensional coordinates (x) of the charging gun c ,y c ,z c The calculation formulas for ) are as follows: x c =-R3cos(θ1+θ2+θ3)+R2cos(θ1+θ2)-R1cosθ1 y c =-R3sin(θ1+θ2+θ3)+R2sin(θ1+θ2)-R1sinθ1 z c =H+△H Wherein, R1, R2, and R3 represent the lengths of the first to third joint arms, respectively, H represents the initial height of the robotic arm, and ΔH represents the translation distance of the translation joint.

4. The control method for the automatic charging device according to claim 2 or 3, characterized in that, Step S3 specifically includes: S31, take the current pose and the target pose as the starting pose and the final pose respectively to obtain the linear motion path of the charging gun, and plan n process poses based on the linear motion path, where n is an integer greater than 1, and the first target pose is the starting pose. S32, based on the three-dimensional coordinates of the charging gun in the nth process pose, obtain the translation parameters of the nth process pose; S33, Based on the joint angle parameters of the charging gun in the (n-1)th process pose, and based on the two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotation joint, the joint angle parameters of the nth process pose are obtained. S34, Based on the joint parameters of each process pose, plan the joint path of the robotic arm.

5. The control method for the automatic charging device according to claim 4, characterized in that, Step S33 specifically includes: Set the joint angle search space for the second and third rotary joints corresponding to the nth process pose, and generate a two-dimensional matrix; Based on the two-dimensional matrix, multiple potential process poses are obtained by searching the (n-1)th process pose of the charging gun. Select the potential process pose that is closest to the nth process pose from among the multiple potential process poses as the nth process pose, and take the joint angles of the corresponding second and third rotational joints as the final solution of the two-dimensional search method; Based on the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint, the joint angle of the first rotary joint corresponding to the nth process pose is obtained.

6. A control system for an automatic charging device, characterized in that, The automatic charging device includes a robotic arm and a charging gun disposed at the end of the robotic arm. The robotic arm includes three rotary joints, one translational joint, and joint arms between the joints. The rotation planes of the three rotary joints are parallel to each other. The translational joint drives the charging gun to move along a translational axis. The system includes: The acquisition module is used to acquire the current pose parameters of the robotic arm and the target pose of the charging gun. The current pose parameters include the joint parameters of each joint and the length of each joint arm at the current moment. The calculation module calculates the current pose of the charging gun based on the current pose parameters; The planning module is used to plan the joint path of the robotic arm based on the current pose and the target pose, using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint.

7. The control system of the automatic charging device according to claim 6, characterized in that, The joint parameters include joint angle parameters and translation parameters. The joint angle parameters include the joint angles of each rotational joint, and the translation parameters include the translation distance of the translational joint.

8. The control system of the automatic charging device according to claim 7, characterized in that, The translation joint is connected to the first rotational joint. The two ends of the first joint arm are connected to the first and second rotational joints, respectively. The two ends of the second joint arm are connected to the second and third rotational joints, respectively. The two ends of the third joint arm are connected to the third rotational joint and the charging gun, respectively. The current pose includes the rotation angle and three-dimensional coordinates of the charging gun relative to the translation axis at the current moment. The translation axis of the translation joint is the vertical axis, and the yaw angle ψ of the charging gun is... c The calculation formula is: ψ c =θ1+θ2+θ3+π / 2 Where θ1, θ2, and θ3 represent the joint angles of the three rotational joints, respectively; The three-dimensional coordinates (x) of the charging gun c ,y c ,z c The calculation formulas for ) are as follows: x c =-R3cos(θ1+θ2+θ3)+R2cos(θ1+θ2)-R1cosθ1 y c =-R3sin(θ1+θ2+θ3)+R2sin(θ1+θ2)-R1sinθ1 z c =H+△H Wherein, R1, R2, and R3 represent the lengths of the first to third joint arms, respectively, H represents the initial height of the robotic arm, and ΔH represents the translation distance of the translation joint.

9. The control system of the automatic charging device according to claim 8, characterized in that, The planning module specifically includes: The preset unit is used to take the current pose and the target pose as the starting pose and the final pose respectively to obtain the linear motion path of the charging gun, and to plan n process poses based on the linear motion path, where n is an integer greater than 1, and the first target pose is the starting pose. The first calculation unit is used to obtain the translation parameters of the nth process pose based on the three-dimensional coordinates of the charging gun in the nth process pose. The second calculation unit is used to obtain the joint angle parameters of the nth process pose based on the joint angle parameters of the charging gun in the (n-1)th process pose, using a two-dimensional search method and the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotation joint. The planning unit is used to plan the joint path of the robotic arm based on the joint parameters of each process pose.

10. The control system of the automatic charging device according to claim 9, characterized in that, The second computing unit is specifically used for: Set the joint angle search space for the second and third rotary joints corresponding to the nth process pose, and generate a two-dimensional matrix; Based on the two-dimensional matrix, multiple potential process poses are obtained by searching the (n-1)th process pose of the charging gun. Select the potential process pose that is closest to the nth process pose from among the multiple potential process poses as the nth process pose, and take the joint angles of the corresponding second and third rotational joints as the final solution of the two-dimensional search method; Based on the geometric relationship between the rotation angle of the charging gun relative to the translation axis and the joint angle of the rotary joint, the joint angle of the first rotary joint corresponding to the nth process pose is obtained.

Images