Manipulator motion control method for actively searching shielding target

Abstract

The invention discloses a manipulator motion control method for actively searching a shielding target, and relates to the technical field of manipulator motion planning and control, and the manipulator motion control method comprises the following steps: planning an angle range, an accumulated rotation angle and a default rotation direction of arc rotation motion of a manipulator end tool in a space horizontal plane; solving the radius of the plane arc track based on the position coordinates of the visual camera and the guide point, obtaining and verifying the pose information of the target point based on the visual camera, verifying and updating the accumulated rotation angle and the rotation direction, solving the joint configuration of the manipulator after the single-step rotation motion, and controlling the manipulator to move to a new joint configuration and then continuously searching for the target. According to the method, the shielding target is actively searched by planning and controlling the movement of the manipulator, so that the flexibility and the initiative of the movement of the manipulator can be brought into play, and the shielding object can be accurately identified, positioned and grabbed without increasing the software and hardware cost.

Description

Technical Field

[0001] The present application relates to the technical field of manipulator motion planning and control, and in particular to a manipulator motion control method for actively searching for occluded targets. Background Art

[0002] At present, with the rapid development of automation and intelligent manufacturing, robots are increasingly used in industrial production, social services, agriculture and forestry cooperation and other fields. However, in actual application scenarios, due to the widespread object occlusion phenomenon, vision-guided robots are unable to accurately identify and locate target objects and are difficult to perform grasping tasks. For example, when carrying out fruit harvesting operations in a natural environment, dense branches and leaves and overlapping fruits often interfere with the judgment of the detection and recognition algorithm, resulting in the inability of the picking robot to accurately grasp the target fruit, which seriously affects the success rate and operation efficiency of automatic picking. In classification and sorting tasks, the target object may be partially blocked by the higher goods in front, causing the recognition system to fail, which in turn causes the execution unit to be unable to grasp specific items. This problem limits the application potential of robots in complex environments.

[0003] In related technologies, in order to solve the above occlusion problem: 1) One method is to use more sensors to build a complex perception system, and use multi-source information fusion technology to segment and locate the target; however, this method significantly increases the cost of equipment. 2) Another method is to improve the visual recognition algorithm and increase the diversity of data sets, and obtain better detection and recognition strategies by increasing the model parameters of the learning algorithm; however, this method usually relies on greater computing power, more storage space and longer training time, which greatly increases the cost of software and hardware.

[0004] Therefore, in fact, by imitating the process of human grasping objects, more environmental information can be obtained by switching different perspectives to search for occluded targets. Therefore, given the flexibility of the manipulator's movement, a manipulator motion control method for actively searching for occluded targets is urgently needed to control its active movement to change the perspective to search for the target object, so as to achieve accurate positioning and grasping of the occluded target without increasing the cost. Summary of the invention

[0005] The purpose of the present application is to provide a manipulator motion control method for actively searching for occluded targets, so as to solve the above-mentioned occlusion problem in the related art: 1) One method is to use more sensors to build a complex perception system, and to segment and locate the target through multi-source information fusion technology; however, this method significantly increases the equipment cost; 2) Another method is to improve the visual recognition algorithm and increase the diversified data set, and to obtain a better detection and recognition strategy by increasing the model parameters of the learning algorithm; however, this method usually relies on greater computing power, more storage space and longer training time, which greatly increases the problem of software and hardware costs.

[0006] The present application provides a manipulator motion control method for actively searching for an obstructed target, which adopts the following technical solution:

[0007] A manipulator motion control method for actively searching for an obstructed target comprises the following steps:

[0008] S1. Plan the angle range, cumulative rotation angle, and default rotation direction of the robot end tool in the horizontal plane of space.

[0009] S2, solving the radius of the plane arc trajectory based on the position coordinates of the visual camera and the guide point;

[0010] S3, acquiring and verifying the position and posture information of the target point based on the visual camera;

[0011] S4, verify and update the accumulated rotation angle and rotation direction;

[0012] S5, solving the joint configuration of the manipulator after a single-step rotation movement;

[0013] S6, controlling the manipulator to move to a new joint configuration and continue searching for the target;

[0014] The manipulator comprises n joint degrees of freedom, an end tool gripper and a visual camera, and the visual camera is installed on the manipulator.

[0015] Preferably, the circular rotation motion refers to that in three-dimensional space, no matter how the robot arm is installed, the current position of the end tool of the robot is taken as the starting point of the arc, and through reasonable planning and control, the end tool makes circular rotation motion around a certain center of a circle in the horizontal plane, and during the movement, the angle between the tool pointing axis and the horizontal plane is always kept unchanged.

[0016] Preferably, the radius of the plane arc trajectory is solved based on the position coordinates of the visual camera and the guide point, and the specific implementation process is as follows:

[0017] S201, calculating the coordinates of a certain guide point P on the target object in the manipulator tool coordinate system based on the hand-eye calibration method;

[0018] S202, calculating the angle α1 between the guide point P and the opening and closing motion plane of the manipulator end tool based on the projection method;

[0019] S203, calculating the angle α2 between the opening and closing motion plane of the end tool and the horizontal plane based on the dihedral angle principle;

[0020] S204, calculate the radius of the plane arc trajectory:

[0021] R=L p ·cosα,

[0022] Among them, L p is the distance from the guide point P to the origin of the tool system, α≈α1+α2.

[0023] Preferably, the acquisition and verification of the position and posture information of the target point based on the visual camera means that when the manipulator makes a circular rotation motion in the horizontal plane of space, the visual camera synchronously executes the target detection and recognition algorithm; if the visual camera acquires a valid target point, the manipulator stops the circular search motion, and then performs tasks such as target grasping; otherwise, it continues to perform active search for occluded target motion.

[0024] Preferably, the verifying and updating the accumulated rotation angle and rotation direction comprises the following steps:

[0025] First, verify the cumulative rotation angle θ of the robot's circular rotation motion sum Whether it exceeds the planned angle range; if it exceeds the angle range limit, the rotation direction of the end tool is updated to make it move in the opposite direction along the arc trajectory; otherwise, the default rotation direction remains unchanged;

[0026] Next, update the accumulated rotation angle of the manipulator's circular rotation motion to

[0027] θ sum =θ sum +ψ·Δθ,

[0028] Among them, ψ=-1 corresponds to the end tool moving clockwise in the horizontal plane, and ψ=1 corresponds to the end tool moving counterclockwise in the horizontal plane; Δθ(>0) is the single step length of the manipulator's rotational motion.

[0029] As a preferred method, the joint configuration of the manipulator after a single-step rotation movement can be solved by mapping q * =f(R,θ sum ) to indicate.

[0030] Preferably, the mapping q * =f(R,θ sum ) is as follows:

[0031] S501, calculate the homogeneous transformation matrix of the robot end tool coordinate system relative to the world coordinate system in the initial configuration, denoted as

[0032]

[0033] Among them, r 11 ~r 33 is the element of the rotation transformation matrix of the tool coordinate system of the manipulator in the initial configuration relative to the world coordinate system, x t0 ,y t0 、z t0 is the corresponding position coordinate;

[0034] S502, calculate the homogeneous transformation matrix of the manipulator tool system performing planar circular motion in the world system, and the calculation steps are as follows:

[0035] 1) Calculate the coordinates of the center C of the plane arc trajectory:

[0036]

[0037] in, ω0=arccosr 21 ;

[0038] 2) Calculate the new position coordinates of the tool system relative to the world system after the manipulator moves:

[0039]

[0040] 3) Calculate the homogeneous transformation matrix of the tool system relative to the world system after the manipulator moves:

[0041]

[0042] Among them, Trans(·) and Rot(·) are fourth-order translation and rotation operators respectively. is the matrix The homogeneous rotation matrix composed of the first three order sub-matrices of ;

[0043] S503. After calculating the motion of the manipulator, the homogeneous transformation matrix of the wrist coordinate system relative to the base coordinate system is:

[0044]

[0045] Among them, T -1 represents the inverse of the matrix T; is the homogeneous transformation matrix of the manipulator base coordinate system relative to the world coordinate system, is the homogeneous transformation matrix of the robot tool coordinate system relative to the wrist coordinate system, which is usually a constant;

[0046] S504, calculate the joint configuration after the manipulator moves: Where IK(·) represents the inverse kinematics function of the robot.

[0047] Preferably, the controlling the manipulator to move to a new joint configuration and continue searching for the target comprises the following steps:

[0048] First, verify the solved new joint configuration q * Whether it is a valid value, including verifying whether its dimension is consistent with the joint degrees of freedom of the manipulator, and verifying whether the value of each element is within the rotation angle range of the corresponding joint;

[0049] Next, based on the validity judgment result of the new joint configuration, the operation direction of the control method is determined; that is, if q * If q is not a valid value, then return to step S4 to verify and update the accumulated rotation angle and rotation direction, and continue to solve the new joint configuration after the manipulator makes the next rotation movement; if q * If it is a valid value, the manipulator is controlled to move to the new joint configuration, and then returns to step S3 to continue searching for the target point and verifying its validity; this process is repeated until the target search algorithm is closed.

[0050] Compared with the prior art, the beneficial effects of this application are:

[0051] The present application proposes a method for actively changing the viewing angle by controlling the movement of a manipulator in order to search for occluded targets. This method gives full play to the flexibility and initiative of the multi-degree-of-freedom manipulator movement, thereby enabling the manipulator to actively search for occluded targets during grasping tasks, thereby achieving the effect of accurately identifying and positioning the manipulator.

[0052] At the same time, compared with the process of solving the target occlusion problem by improving the visual recognition algorithm, which requires a lot of computing power and storage resources to train a large data set, the present application uses a control method that can actively search for occluded targets through a manipulator. It only includes commonly used matrix operations and visual recognition technology, and only requires conventional computing and storage units, which effectively reduces the cost of software and hardware. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 A flowchart of a manipulator motion control method for actively searching for an obstructed target according to an embodiment of the present application;

[0054] Figure 2 A schematic diagram of a manipulator according to an embodiment of the present application performing a planar circular motion to search for a target;

[0055] Figure 3 This is a schematic diagram of a manipulator based on a visual camera grabbing an obstructed target fruit in an embodiment of the present application;

[0056] Figure 4 A simplified schematic diagram of solving the radius of a plane arc trajectory according to an embodiment of the present application. DETAILED DESCRIPTION

[0057] The following is combined with Figure 1-4 This application is described in further detail.

[0058] The present application embodiment discloses a manipulator motion control method for actively searching for an obstructed target, referring to Figure 1 In this embodiment, the manipulator motion control method includes the following steps:

[0059] S1. Plan the angle range, cumulative rotation angle, and default rotation direction of the robot end tool in the horizontal plane of space.

[0060] Specifically, the robot movement needs to establish the world coordinate system and the tool coordinate system first. w O w Y w plane, with the vertical direction as the Z of the space coordinate system w Axis, based on the right-hand coordinate system rule, establish the reference world coordinate system O w -X w Y w Z w Then take the direction of the end tool of the robot as Z t Axis, perpendicular to the tool plane upward as Y t Axis, with the tool opening and closing movement direction as X t Axis, based on the right-hand coordinate system rule, establish the tool coordinate system O t -X t Y t Z t .

[0061] More specifically, refer to Figure 2 In this embodiment, the circular arc rotation motion refers to the three-dimensional space, regardless of the way the robot arm is installed, the installation methods of the robot arm include upright installation, oblique installation, inverted installation, etc. Then, the current position of the end tool of the robot arm is taken as the starting point of the arc, and the end tool is moved around the horizontal plane X through reasonable planning and control. w O w Y w The center of a circle C in the circle rotates in an arc, and during the movement, the tool always points to the axis Z. t With horizontal plane X w O w Y w The angle of triangle O remains unchanged. t Stability of PC, where point P is a guide point on the target.

[0062] At the same time, in step S1, the angle range of the planned arc rotation motion refers to the initialization expected arc angle, which is recorded as [-θ max ,θ max ]. Among them, θ max (>0) is the maximum rotation angle of a single direction with the current position of the tool as the starting point, with the counterclockwise rotation direction as positive and the clockwise rotation direction as negative.

[0063] The cumulative rotation angle of the planned arc rotation motion refers to the cumulative rotation angle of the initialization end tool for arc rotation motion, denoted as θ sum =0, and always satisfies θ sum ∈[-θ max ,θ max ].

[0064] The default rotation direction of the planned circular rotation motion means that the end tool is initialized to make a planar circular motion in a clockwise direction to search for the target, and the clockwise rotation direction is recorded as ψ=-1, and the corresponding counterclockwise rotation direction is recorded as ψ=1.

[0065] S2. Calculate the radius of the plane arc trajectory based on the position coordinates of the visual camera and the guide point.

[0066] Specifically, refer to Figure 3 In this embodiment, the manipulator includes n joint degrees of freedom, an end tool gripper and a visual camera, and the visual camera is installed on the manipulator in the manner of "eyes on the hand". By using the manipulator equipped with a visual camera and an end tool gripper, the target fruit can be automatically identified and grasped, for example, the target fruit is a pineapple; thus, the automatic and intelligent harvesting of fruits can be realized.

[0067] At the same time, according to the biological and mechanical characteristics of pineapple plants, when the manipulator accurately grasps the target point Q on the fruit, when the target point Q is located at the connection between the pineapple crown bud and the fruit body, the fruit can be easily picked by breaking it sideways or cutting it with a knife. However, due to the obstruction of dense branches and leaves, the visual camera usually cannot directly detect and identify the target point Q, especially the inability to accurately obtain the depth distance information of the target point, which seriously affects the success rate of the manipulator's grasping. Then, considering that the top of the fruit crown bud is usually the highest point of the pineapple plant and is not easily obstructed by branches and leaves, the top of the crown bud is used as the guide point P. The spatial coordinates of the guide point P can be obtained by the visual camera, and then the manipulator end tool is guided to make a circular motion in the horizontal plane of space to search for the target point Q; once the obstructing branches and leaves are bypassed and the position information of the target point Q is obtained, the manipulator stops the circular search motion and then performs the grasping action.

[0068] More specifically, refer to Figure 4In this embodiment, in order to realize the above-mentioned rotational motion, the radius R (>0) of the plane arc trajectory must be determined. Therefore, the radius of the plane arc trajectory is solved based on the position coordinates of the visual camera and the guide point. The specific solution process is as follows:

[0069] S201, based on the hand-eye calibration method, calculate the coordinates of a certain guide point P on the target object in the manipulator tool coordinate system, and the coordinates are

[0070]

[0071] in, It is the homogeneous transformation matrix of the robot tool coordinate system relative to the visual camera coordinate system, which is generally obtained by the hand-eye calibration algorithm; It represents the homogeneous position coordinates of the guide point P in the visual camera coordinate system, which is generally obtained by visual recognition algorithms such as image feature extraction or deep learning.

[0072] S202, calculate the angle α1 between the guide point P and the opening and closing motion plane of the manipulator end tool based on the projection method; specifically, the angle between the guide point P and the manipulator tool system coordinate plane XtOtZt calculated based on the projection method is

[0073]

[0074] S203, based on the dihedral angle principle, calculate the angle α2 between the opening and closing motion plane of the end tool and the horizontal plane; specifically, calculate the angle of the dihedral angle formed by the tool system coordinate plane XtOtZt and the world system coordinate plane XwOwYw as

[0075]

[0076] in, Represents the normal vector of the plane XtOtZt Normal vector to plane XwOwYw The inner product of and are the moduli of the corresponding vectors, and

[0077] S204, calculate the radius of the plane arc trajectory:

[0078] R=L p ·cosα,

[0079] Among them, L p is the distance from the guide point P to the origin of the tool system, α≈α1+α2. Simplified illustration of each parameter and reference to its geometric meaning Figure 4 shown.

[0080] S3. Acquire and verify the position and posture information of the target point based on the visual camera.

[0081] Specifically, in this embodiment, the position and posture information of the target point is acquired and verified based on the visual camera, which means that when the manipulator makes a circular rotation motion in the horizontal plane of space, the visual camera synchronously executes the target detection and recognition algorithm; if the visual camera acquires a valid target point, the manipulator stops the circular search motion, and then performs tasks such as target grasping; otherwise, it continues to perform active search for occluded target motion.

[0082] More specifically, when the manipulator rotates in an arc with point P as the guide point and R as the radius in the horizontal plane of space, the visual camera synchronously executes the target detection and recognition algorithm, and verifies and determines in real time whether a valid target point is obtained. The valid target point refers to the data information of the target point Q obtained based on the visual recognition algorithm, which at least contains the three-dimensional position coordinates, and the coordinate value is not much different from the coordinate value of the feature point nearby, so as to avoid visual noise and non-target point interference. If the visual camera obtains a valid target point, the manipulator stops the arc search movement and then performs tasks such as target grasping. Otherwise, continue to execute the following steps.

[0083] S4. Verify and update the accumulated rotation angle and rotation direction.

[0084] Specifically, in this embodiment, verifying and updating the accumulated rotation angle and rotation direction includes the following steps:

[0085] First, verify the cumulative rotation angle θ of the robot's circular rotation motion sum Whether it exceeds the planned angle range; if it exceeds the angle range limit, the rotation direction of the end tool is updated to make it move in the opposite direction along the arc trajectory; otherwise, the default rotation direction remains unchanged.

[0086] More specifically, the angle range is [-θ max ,θ max ], that is, if θ sum <-θ max , then the updated rotation direction of the robot is counterclockwise, that is, ψ=1. sum >θ max , then the updated rotation direction of the robot is clockwise, that is, ψ = -1.

[0087] Next, update the accumulated rotation angle of the manipulator's circular rotation motion, which is

[0088] θ sum =θ sum +ψ·Δθ,

[0089] Among them, ψ=-1 corresponds to the end tool moving clockwise in the horizontal plane, and ψ=1 corresponds to the end tool moving counterclockwise in the horizontal plane; Δθ (>0) is the single-step step length of the manipulator's rotational motion, which is usually a given prior value.

[0090] S5. Solve the joint configuration of the manipulator after a single-step rotation motion.

[0091] Specifically, in this embodiment, after the manipulator performs a single-step rotational motion along an arc trajectory in the horizontal plane of space, the joint configuration after the manipulator performs a single-step rotational motion can be solved by mapping q * =f(R,θ sum ) is used to represent it. Among them, q * represents the n-dimensional joint variables of the manipulator with n joint degrees of freedom; f(·) represents the plane arc radius R and the cumulative rotation angle θ sum A function that maps to the manipulator joint variables.

[0092] More specifically, its mapping q * =f(R,θ sum ) is as follows:

[0093] S501, calculate the homogeneous transformation matrix of the robot end tool coordinate system relative to the world coordinate system in the initial configuration, denoted as

[0094]

[0095] Among them, r 11 ~r 33 is the element of the rotation transformation matrix of the tool coordinate system of the manipulator in the initial configuration relative to the world coordinate system; t0 ,y t0 、z t0 are the corresponding position coordinates.

[0096] in addition,

[0097] in, is the homogeneous transformation matrix of the manipulator base coordinate system relative to the world coordinate system; is the homogeneous transformation matrix of the robot tool coordinate system relative to the wrist coordinate system, which is usually a constant; It is the homogeneous transformation matrix of the wrist coordinate system of the manipulator in the initial position relative to the base coordinate system.

[0098] S502, calculate the homogeneous transformation matrix of the manipulator tool system performing planar circular motion in the world system, and the calculation steps are as follows:

[0099] 1) Calculate the coordinates of the center C of the plane arc trajectory:

[0100]

[0101] in,

[0102] 2) Calculate the new position coordinates of the tool system relative to the world system after the manipulator moves:

[0103]

[0104] 3) Calculate the homogeneous transformation matrix of the tool system relative to the world system after the manipulator moves:

[0105]

[0106] Among them, Trans(·) and Rot(·) are fourth-order translation and rotation operators respectively; is the matrix The homogeneous rotation matrix composed of the first three sub-matrices of

[0107]

[0108] S503. After calculating the motion of the manipulator, the homogeneous transformation matrix of the wrist coordinate system relative to the base coordinate system is:

[0109]

[0110] Among them, T -1 represents the inverse of the matrix T; is the homogeneous transformation matrix of the manipulator base coordinate system relative to the world coordinate system, are the homogeneous transformation matrices of the robot tool coordinate system relative to the wrist coordinate system, and they are usually constants.

[0111] S504, calculate the joint configuration after the manipulator moves: Where IK(·) represents the inverse kinematics function of the robot.

[0112] S6. Control the manipulator to move to a new joint position and continue searching for the target.

[0113] Specifically, in this embodiment, the control of the manipulator to continue searching for the target after moving to the new joint configuration includes the following steps:

[0114] First, verify the solved new joint configuration q * Whether it is a valid value, including verifying whether its dimension is consistent with the joint degrees of freedom of the manipulator, and verifying whether the value of each element is within the rotation angle range of the corresponding joint;

[0115] Next, based on the validity judgment result of the new joint configuration, the operation direction of the control method is determined; that is, if q* If q is not a valid value, then return to step S4 to verify and update the accumulated rotation angle and rotation direction, and continue to solve the new joint configuration after the manipulator makes the next rotation movement; if q * If it is a valid value, the manipulator is controlled to move to the new joint configuration, and then returns to step S3 to continue searching for the target point and verifying its validity; this process is repeated until the target search algorithm is closed.

[0116] The above descriptions are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A manipulator motion control method for actively searching for occluded targets, characterized in that: The following steps are involved: S1. Plan the angle range, cumulative rotation angle, and default rotation direction of the robot end tool in the horizontal plane of space. S2, solving the radius of the plane arc trajectory based on the position coordinates of the visual camera and the guide point; S3, acquiring and verifying the position and posture information of the target point based on the visual camera; S4, verify and update the accumulated rotation angle and rotation direction; S5, solving the joint configuration of the manipulator after a single-step rotation movement; S6, controlling the manipulator to move to a new joint configuration and continue searching for the target; The manipulator comprises n joint degrees of freedom, an end tool gripper and a visual camera, and the visual camera is installed on the manipulator.

2. A manipulator motion control method for actively searching for an obstructed target according to claim 1, characterized in that: The circular rotation motion refers to that in three-dimensional space, no matter how the robot arm is installed, the current position of the end tool of the robot arm is taken as the starting point of the arc, and through reasonable planning and control, the end tool makes an arc rotation motion around a certain center of a circle in the horizontal plane, and during the movement, the angle between the tool pointing axis and the horizontal plane is always kept unchanged.

3. The method for controlling the motion of a manipulator for actively searching for an obstructed target according to claim 1, characterized in that: The specific implementation process of solving the radius of the plane arc trajectory based on the position coordinates of the visual camera and the guide point is as follows: S201, calculating the coordinates of a certain guide point P on the target object in the manipulator tool coordinate system based on the hand-eye calibration method; S202, calculating the angle α1 between the guide point P and the opening and closing motion plane of the manipulator end tool based on the projection method; S203, calculating the angle α2 between the opening and closing motion plane of the end tool and the horizontal plane based on the dihedral angle principle; S204, calculate the radius of the plane arc trajectory as R = L p ·cosα, Among them, L p is the distance from the guide point P to the origin of the tool system, α≈α1+α2.

4. The method for controlling the motion of a manipulator for actively searching for an obstructed target according to claim 1, characterized in that: The acquisition and verification of the position and posture information of the target point based on the visual camera means that when the manipulator performs circular rotation in the horizontal plane of space, the visual camera synchronously executes the target detection and recognition algorithm; if the visual camera acquires a valid target point, the manipulator stops the circular search movement and then performs tasks such as target grasping; Otherwise, continue to actively search for occluded target motion.

5. The method for controlling the motion of a manipulator for actively searching for an obstructed target according to claim 1, characterized in that: The verification and updating of the accumulated rotation angle and rotation direction comprises the following steps: First, verify the cumulative rotation angle θ of the robot's circular rotation motion sum Whether it exceeds the planned angle range; if it exceeds the angle range limit, the rotation direction of the end tool is updated to make it move in the opposite direction along the arc trajectory; otherwise, the default rotation direction remains unchanged; Next, update the accumulated rotation angle of the manipulator's circular rotation to θ sum =θ sum +ψ·Δθ, Among them, ψ=-1 corresponds to the end tool moving clockwise in the horizontal plane, and ψ=1 corresponds to the end tool moving counterclockwise in the horizontal plane; Δθ(>0) is the single step length of the manipulator's rotational motion.

6. The method for controlling the motion of a manipulator for actively searching for an obstructed target according to claim 1, characterized in that: The joint configuration of the robot after a single-step rotation motion can be solved by mapping q * =f(R,θ sum ) to indicate.

7. A manipulator motion control method for actively searching for an obstructed target according to claim 6, characterized in that: The mapping q * =f(R,θ sum ) is as follows: S501, calculate the homogeneous transformation matrix of the robot end tool coordinate system relative to the world coordinate system in the initial configuration, denoted as Among them, r 11 ~r 33 is the element of the rotation transformation matrix of the tool coordinate system of the manipulator in the initial configuration relative to the world coordinate system, x t0 ,y t0 、z t0 is the corresponding position coordinate; S502, calculate the homogeneous transformation matrix of the manipulator tool system performing planar circular motion in the world system, and the calculation steps are as follows: 1) Calculate the coordinates of the center C of the plane arc trajectory: in, ω0=arccosr 21 ; 2) Calculate the new position coordinates of the tool system relative to the world system after the manipulator moves: 3) Calculate the homogeneous transformation matrix of the tool system relative to the world system after the manipulator moves: Among them, Trans(·) and Rot(·) are fourth-order translation and rotation operators respectively. is the matrix The homogeneous rotation matrix composed of the first three order sub-matrices of ; S503. After calculating the motion of the manipulator, the homogeneous transformation matrix of the wrist coordinate system relative to the base coordinate system is: Among them, T -1 represents the inverse of the matrix T; is the homogeneous transformation matrix of the manipulator base coordinate system relative to the world coordinate system, is the homogeneous transformation matrix of the robot tool coordinate system relative to the wrist coordinate system, which is usually a constant; S504, calculate the joint configuration after the manipulator moves: Where IK(·) represents the inverse kinematics function of the robot.

8. The method for controlling the motion of a manipulator for actively searching for an obstructed target according to claim 7, characterized in that: The controlling the manipulator to move to the new joint configuration and then continue searching for the target comprises the following steps: First, verify the solved new joint configuration q * Whether it is a valid value, including verifying whether its dimension is consistent with the joint degrees of freedom of the manipulator, and verifying whether the value of each element is within the rotation angle range of the corresponding joint; Next, based on the validity judgment result of the new joint configuration, the operation direction of the control method is determined; If q * If q is not a valid value, then return to step S4 to verify and update the accumulated rotation angle and rotation direction, and continue to solve the new joint configuration after the manipulator makes the next rotation movement; if q * If it is a valid value, the manipulator is controlled to move to the new joint configuration, and then returns to step S3 to continue searching for the target point and verifying its validity; this process is repeated until the target search algorithm is closed.

Images