Rapid calibration and movement control method of robotic arm

Abstract

The present invention relates to the field of robot control technology, and in particular to a method for rapid calibration and mobile control of a mechanical arm, which is applicable to a mobile robot with a movable base, the mobile robot having a mechanical arm with multiple sets of articulated joints, the end of the mechanical arm being provided with a laser ranging sensor and a plane camera coaxially arranged with the laser ranging sensor; the method comprising: establishing a homogeneous transformation matrix from the local coordinate system of the movable base of the mobile robot to the end of the mechanical arm of the mobile robot; solving the homogeneous transformation matrix according to the current distance from the ground obtained by the laser ranging sensor and the position of the feature point of the working object measured by the plane camera in the local coordinate system x-y plane of the mobile robot; generating the motion control instruction of the mobile robot according to the homogeneous transformation matrix and the current joint angle of the mobile robot. The present invention realizes rapid calibration of an uncertain coordinate system through a two-dimensional camera and laser ranging.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and in particular to a method for rapid calibration and movement control of a robot arm. Background Art

[0002] Airport runway lights, also known as navigation lights, are one of the important equipment to ensure pilots can fly at night and in complex weather conditions. Airport runway guidance lights provide visual guidance signals for aircraft takeoff and landing at night, in low visibility or in bad weather conditions, ensuring safe takeoff and landing of aircraft. When the aircraft takes off and lands, the tires generate high temperatures due to friction, causing the tread to melt, and the melted rubber adheres to the runway lights. The high frequency of takeoffs and landings causes the clay to quickly cover the surface of the runway light translucent mirror, making the surface of the runway light dirty and reducing the light intensity of the emitted light components. In order to ensure flight safety, it is necessary to clean the lamps regularly, especially the translucent mirror components, but the high-density takeoffs and landings leave less and less time for cleaning the runway lights.

[0003] Currently, the cleaning and maintenance of airport runway lights are mainly carried out during the idle period of the runway at night. Generally, the traditional method is adopted, and regular manual cleaning and inspection are carried out by manual dragging equipment. The runway lights are cleaned mainly by applying detergent on the surface of the light-transmitting mirror, and then high-pressure washing and manual mechanical scraping. This method has a low degree of automation, low efficiency and high labor costs.

[0004] Robots with movable bases can provide good mobility and working stability, and have great potential in the automation of cleaning and maintenance of airport runway lights. However, due to the uncertainty of the relative coordinates between the movable robot arm and the work object, it is difficult to achieve high-precision operation of the mobile robot arm. The reasons may be:

[0005] ① The uncertainty of vehicle parameters (such as tire status and changes in installation parameters after long-term use) causes drift between the robot coordinate system and the vehicle polar coordinate system;

[0006] ② The objects are arranged irregularly, and due to scene requirements, it is difficult to install targets on the objects and their related structural parts, so direct calibration is not possible;

[0007] ③ Three-dimensional cameras that meet the accuracy requirements are usually large in size and difficult to install directly on the end of the robotic arm. Therefore, they are difficult to use in many scenarios that have high requirements for the robotic arm's operating space. Summary of the invention

[0008] The present invention provides a method for rapid calibration and movement control of a robot arm, which can realize high-precision operation of a robot on an operation object by rapid calibration of an uncertain coordinate system through a two-dimensional camera and laser ranging without relying on a target.

[0009] To achieve the above object, the present invention provides a method for rapid calibration and movement control of a robotic arm, which is applicable to a mobile robot with a movable base, wherein the mobile robot has a robotic arm with multiple sets of articulated joints, and a laser ranging sensor and a planar camera coaxially arranged with the laser ranging sensor are provided at the end of the robotic arm;

[0010] The method comprises:

[0011] Establishing a homogeneous transformation matrix from the local coordinate system of the movable base of the mobile robot to the end of the mechanical arm of the mobile robot;

[0012] Solving the homogeneous transformation matrix according to the current distance from the ground obtained by the laser ranging sensor and the position of the feature point of the work object measured by the plane camera on the xy plane of the local coordinate system of the mobile robot;

[0013] A motion control instruction of the mobile robot is generated according to the homogeneous transformation matrix and the current joint angle of the mobile robot.

[0014] The method for rapid calibration and mobile control of a robotic arm as described above, further, establishing a homogeneous transformation matrix from the local coordinate system of the movable base of the mobile robot to the end of the robotic arm of the mobile robot, specifically includes:

[0015] Assume that the robot has m groups of different joint angles, and record the joint angles of the i-th group of robot arms. where i=1,...m

[0016] , They are the angles from the 1st joint to the nth joint in the i-th group of robotic arms;

[0017] According to the forward kinematics of the robot arm, the homogeneous transformation matrix corresponding to the joint angle of the i-th group of robot arms is obtained:

[0018]

[0019] According to the laser ranging sensor, the current distance d from the ground is obtained i The position of the feature point of the work object in the xy plane of the local coordinate system of the mobile robot is measured by the plane camera (x i ,y i );

[0020] The homogeneous transformation matrix corresponding to the local coordinate system of the mobile robot is described as:

[0021]

[0022] in:

[0023]

[0024] According to the homogeneous transformation matrix corresponding to the local coordinate system of the mobile robot and the homogeneous transformation matrix corresponding to the joint angle of the robot's i-th group of mechanical arms, the homogeneous transformation matrix T from the local coordinate system of the movable base of the mobile robot to the end of the robot's mechanical arm is obtained:

[0025] T=T p *T ai (4).

[0026] The method for rapid calibration and movement control of a robotic arm as described above, further, solving the homogeneous transformation matrix specifically includes:

[0027] The parameters to be identified in solving the homogeneous transformation matrix are the attitude angles α, β, γ of the local coordinate system of the mobile robot relative to the large base coordinate system and the z-direction displacement variable z p ,in,

[0028] According to the obtained homogeneous transformation matrix T, the current terminal laser ranging reading d of the mobile robot is obtained. i In the geodetic coordinate system [0 0 -1] T The projection of the direction is equal to the height of the laser rangefinder at the end of the mobile robot and the ground, and we can get:

[0029]

[0030] where z i is the element of the 3rd row and 4th column of the homogeneous transformation matrix T;

[0031] According to m groups of data, the parameter vector to be identified is obtained. Identification conditions:

[0032]

[0033] Defining the Matrix Calculate the parameters of the local coordinate system of the mobile robot to be identified

[0034]

[0035] Solve the displacement z of the local coordinate system of the mobile robot relative to the large base coordinate system p and attitude angles β,γ, where

[0036]

[0037] The feature points of the work object measured by the planar camera of the mobile robot coincide with each other in the global coordinate system:

[0038]

[0039] Combining formula (2), we can deduce:

[0040]

[0041] According to equations (8) and (3), equation (10) contains only one unknown number α, which can be obtained using the nonlinear identification method lsqcurvefit toolbox. Further, according to equation (2), the homogeneous transformation matrix T of the movable base of the mobile robot is obtained: p .

[0042] The method for rapid calibration and movement control of a robotic arm as described above, further, generating a motion control instruction of the mobile robot according to the homogeneous transformation matrix and the current joint angle of the mobile robot, specifically includes:

[0043] According to formula (4) and the joint angle θ of the current robot arm, the current end position x, y, z of the robot arm is calculated, where x and y are the elements of the 3rd row, 1st column and 2nd column of T respectively;

[0044] The expected trajectory of the mobile robot operation [x d ;y d ;z d ] and its derivatives The terminal error function of the mobile robot is constructed as e = [x; y; z] - [x d ;y d ;z d ], and design the motion controller as:

[0045]

[0046] Among them J -1 is the inverse of the Jacobian matrix of the robot arm, k1 and k2 are both positive constants, and tanh is the inverse tangent function.

[0047] Compared with the prior art, the present invention has the following beneficial effects:

[0048] 1. Rapid calibration of uncertain coordinate systems is achieved through two-dimensional cameras and laser ranging, which has the advantages of simple implementation and low cost.

[0049] 2. This method can quickly acquire depth information without relying on the target.

[0050] 3. Through the combined application of planar camera measurement data and laser depth data, the robot can achieve high-precision operation of the work object. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0052] Figure 1 Schematic diagram of a homogeneous transformation matrix of a mobile robot in an embodiment of the present invention;

[0053] Figure 2 is a flow chart of a calibration and control method in an embodiment of the present invention;

[0054] Figure 3 4 is a flow chart of a parameter identification and control method in an embodiment of the present invention. DETAILED DESCRIPTION

[0055] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0056] Example:

[0057] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations of the embodiments of the present invention are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0058] In the description of the present invention, "multiple" means at least two, such as two, three, etc., unless otherwise clearly defined. In addition, unless otherwise clearly defined and defined, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two elements. For those of ordinary skill in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0059] See also Figures 1 to 3 , an embodiment of the present invention provides a method for rapid calibration and movement control of a robotic arm, which is applicable to a mobile robot with a movable base, the mobile robot having a robotic arm with multiple sets of articulated joints, a laser ranging sensor and a planar camera coaxially arranged with the laser ranging sensor are provided at the end of the robotic arm;

[0060] The method comprises:

[0061] Step 101: Establish a homogeneous transformation matrix from the local coordinate system of the movable base of the mobile robot to the end of the mechanical arm of the mobile robot.

[0062] Step 102: Solve the homogeneous transformation matrix according to the current distance from the ground obtained by the laser ranging sensor and the position of the feature point of the work object measured by the planar camera on the xy plane of the local coordinate system of the mobile robot.

[0063] Step 103: Generate a motion control instruction for the mobile robot according to the homogeneous transformation matrix and the current joint angles of the mobile robot.

[0064] In one embodiment, a method for rapid calibration and movement control of a robotic arm may further include the following process:

[0065] Step 201: Set m groups of different joint angles of the robot arm and record the i-th group of robot joint angles Where i=1,...m. They are the 1st to nth joint angles in the i-th group of joint angles respectively.

[0066] According to the forward kinematics of the robot arm, the homogeneous transformation matrix corresponding to the i-th group of robot arm joint angles is obtained:

[0067]

[0068] Step 202: Obtain the current distance d from the ground based on the laser ranging sensor installed at the end of the robotic arm. iAt the same time, the position of the feature point of the work object in the xy plane of the robot local coordinate system (x i ,y i )

[0069] Step 203: For the mobile robot coordinate system to be identified, its uncertain parameters are mainly the attitude angles α, β, γ of the mobile robot coordinate system relative to the ground base coordinate system and the z-direction displacement variable z p , the homogeneous transformation matrix corresponding to the mobile robot coordinate system can be described as:

[0070]

[0071] in:

[0072]

[0073] Step 204: Based on the homogeneous transformation matrix of the mobile robot coordinate system to be identified and the robot's i-th group of joint angles

[0074] T=T p *T ai (4)

[0075] Step 205: Based on the homogeneous transformation matrix obtained in the previous step, the current end laser range reading d of the robot is obtained. i In the geodetic coordinate system [0 0 -1] T The projection of the direction is equal to the height of the robot's end laser rangefinder and the ground:

[0076]

[0077] where z i is the element in the 3rd row and 4th column of matrix T.

[0078] Step 206: Obtain the parameter vector to be identified based on the m groups of data Identification conditions:

[0079]

[0080] Step 207: Define the matrix Calculate the coordinate system parameters of the mobile robot to be identified

[0081]

[0082] Step 208: Calculate the displacement z of the mobile robot coordinate system relative to the large base coordinate system p and attitude angles β,γ, where

[0083]

[0084] Step 209: The robot's plane camera measures that the feature points of the work object coincide in the global coordinate system:

[0085]

[0086] Combining formula (2), we can deduce:

[0087]

[0088] According to equations (8) and (3), equation (10) contains only one unknown number α, which can be obtained using the nonlinear identification method lsqcurvefit toolbox. Further, the uncertain homogeneous transformation matrix T of the robot base can be obtained according to equation (2): p

[0089] Step 210: Calculate the current end position x, y, z of the mobile robot using the current robot joint angle θ according to equation (4), where x and y are the elements of the 3rd row, 1st column and 2nd column of T respectively.

[0090] Step 211: Expected trajectory for robot operation [x d ;y d ;z d ] and its derivatives The terminal error function of the constructed robot is e = [x; y; z] - [x d ;y d ;z d ], and design the motion controller as:

[0091]

[0092] Among them J -1 is the inverse of the Jacobian matrix of the robot arm, k1 and k2 are both positive constants, and tanh is the inverse tangent function.

[0093] The robot arm rapid calibration and movement control method proposed in this embodiment is applied to a road robot, which can automatically perform cleaning and inspection tasks of target lamps according to the requirements of the work task.

[0094] It can be seen that the present invention can realize the rapid calibration of the robot arm parameters when the parameters of the movable chassis are uncertain; the calibration method required by the present invention does not need to install a 3D camera at the end of the robot arm, and the uncertain parameters of the robot arm base can be identified through the end plane camera and laser ranging; the control method proposed in the present invention can realize the high-precision operation of the robot on the working object through the combined application of the plane camera measurement data and the laser depth data.

[0095] It should be understood that the terms "center", "longitudinal", "lateral", "length", "width", "thickness", "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", "clockwise", "counterclockwise", "axial", "radial", "circumferential" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the referred device or element must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as limiting the present invention.

[0096] In the present invention, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may mean that the first and second features are in direct contact, or the first and second features are in indirect contact through an intermediate medium. Moreover, a first feature being "above", "above" or "above" a second feature may mean that the first feature is directly above or obliquely above the second feature, or simply means that the first feature is higher in level than the second feature. A first feature being "below", "below" or "below" a second feature may mean that the first feature is directly below or obliquely below the second feature, or simply means that the first feature is lower in level than the second feature.

[0097] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0098] The above embodiments are only for illustrating the technical concept and features of the present invention, and their purpose is to enable ordinary technicians in the field to understand the content of the present invention and implement it accordingly, and they cannot be used to limit the protection scope of the present invention. Any equivalent changes or modifications made based on the essence of the content of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for rapid calibration and movement control of a robotic arm, characterized in that: It is suitable for a mobile robot with a movable base, the mobile robot has a mechanical arm with multiple sets of articulated joints, and a laser distance sensor and a plane camera coaxially arranged with the laser distance sensor are arranged at the end of the mechanical arm; The method comprises: Establishing a homogeneous transformation matrix from the local coordinate system of the movable base of the mobile robot to the end of the mechanical arm of the mobile robot; Solving the homogeneous transformation matrix according to the current distance from the ground obtained by the laser ranging sensor and the position of the feature point of the work object measured by the plane camera on the xy plane of the local coordinate system of the mobile robot; A motion control instruction of the mobile robot is generated according to the homogeneous transformation matrix and the current joint angle of the mobile robot.

2. The method for rapid calibration and movement control of a robotic arm according to claim 1, characterized in that: The step of establishing a homogeneous transformation matrix from the local coordinate system of the movable base of the mobile robot to the end of the mechanical arm of the mobile robot specifically includes: Assume that the robot has m groups of different joint angles, and record the joint angles of the i-th group of robot arms. Where i = 1,...m, They are the angles from the 1st joint to the nth joint in the i-th group of robotic arms; According to the forward kinematics of the robot arm, the homogeneous transformation matrix corresponding to the joint angle of the i-th group of robot arms is obtained: According to the laser ranging sensor, the current distance d from the ground is obtained i The position of the feature point of the work object in the xy plane of the local coordinate system of the mobile robot is measured by the plane camera (x i ,y i ); The homogeneous transformation matrix corresponding to the local coordinate system of the mobile robot is described as: in: α, β, γ are the attitude angles of the local coordinate system of the mobile robot relative to the large base coordinate system, z p is the displacement variable in the z direction; According to the homogeneous transformation matrix corresponding to the local coordinate system of the mobile robot and the homogeneous transformation matrix corresponding to the joint angle of the robot's i-th group of mechanical arms, the homogeneous transformation matrix T from the local coordinate system of the movable base of the mobile robot to the end of the robot's mechanical arm is obtained: T=T p *T ai (4)。 3. The method for rapid calibration and movement control of a robotic arm according to claim 2, characterized in that: The solving the homogeneous transformation matrix specifically includes: The parameters to be identified in solving the homogeneous transformation matrix are the attitude angles α, β, γ of the local coordinate system of the mobile robot relative to the large base coordinate system and the z-direction displacement variable z p ,in, According to the obtained homogeneous transformation matrix T, the current terminal laser ranging reading d of the mobile robot is obtained. i In the geodetic coordinate system [0 0-1] T The projection of the direction is equal to the height of the laser rangefinder at the end of the mobile robot and the ground, and we can get: where z i is the element of the 3rd row and 4th column of the homogeneous transformation matrix T; According to m groups of data, the parameter vector to be identified is obtained. Identification conditions: Defining the Matrix Calculate the parameters of the local coordinate system of the mobile robot to be identified Solve the displacement z of the local coordinate system of the mobile robot relative to the large base coordinate system p and attitude angles β,γ, where The feature points of the work object measured by the planar camera of the mobile robot coincide with each other in the global coordinate system: Combining formula (2), we can deduce: According to equations (8) and (3), the unknown number α in equation (10) is obtained using the nonlinear identification method lsqcurvefit toolbox. Further, according to equation (2), the homogeneous transformation matrix T of the movable base of the mobile robot is obtained: p .

4. The method for rapid calibration and movement control of a robotic arm according to claim 2, characterized in that: Generating the motion control instruction of the mobile robot according to the homogeneous transformation matrix and the current joint angle of the mobile robot specifically includes: According to formula (4) and the joint angle θ of the current robot arm, the current end position x, y, z of the robot arm is calculated, where x and y are the elements of the 3rd row, 1st column and 2nd column of T respectively; The expected trajectory of the mobile robot operation [x d ;y d ; z d ] and its derivatives The terminal error function of the mobile robot is constructed as e = [x; y; z] - [x d ;y d ; z d ], and design the motion controller as: Among them J -1 is the inverse of the Jacobian matrix of the robot arm, k1 and k2 are both positive constants, and tanh is the inverse tangent function.

Images