A low-degree-of-freedom target search and arrival control method for a mobile robotic arm

Abstract

The present invention discloses a low-degree-of-freedom target search and arrival strategy for a mobile robot arm, which is applied to a mobile robot arm including a trolley, multiple swivel joints and swing joints. An image acquisition device is installed at the end of the mobile robot arm, and the two-joint combined motion of the mobile robot arm is selected. The target space is traversed and roughly searched with the maximum field of view angle as the step length until the target object appears in the field of view. By calculating the relative position of the target and the robot arm, the combined motion angle of the two joints is determined, so that the target object is located in the center of the field of view of the image acquisition device, and the precise positioning of the target object is achieved; the trolley is further moved closer to the target object, first the trolley turns to the direction of the target object, the swivel joint rotates in the opposite direction to compensate for the rotation of the trolley, and then the trolley moves toward the target object, during which the swing joint is adjusted to ensure that the target object does not move out of the central area of ​​the field of view, until the distance between the target object and the trolley reaches the set threshold, and the target arrival task is completed.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent control, and in particular relates to a low-degree-of-freedom target searching and reaching control method for a mobile robot arm. Background Art

[0002] A robotic arm is an automatic operating device that can imitate certain movements of human hands and arms, and then grasp, move objects or operate tools according to a set program. It can replace heavy human labor to achieve mechanization and automation of production, and can operate in harmful environments to protect personal safety. The mobile robotic arm combines the robotic arm with a trolley, which increases the degree of freedom of the entire system and greatly expands the working space of the robotic arm, allowing the robotic arm to grasp targets outside the working space.

[0003] There are generally two situations in visual search: 1. Knowing the approximate position of the target object, and conducting a targeted path search for it; 2. Not knowing the target position at all, and traversing the environment. Scholars have actively conducted research on these two situations. For example, the Chinese document patent number: CN202010444256.7, the publication / announcement date: 2020.08.25 discloses a four-eye bionic eye device, a device and a method for searching targets, which aims to solve the problems of narrow field of view, low perception efficiency and poor stability of bionic robots in the prior art. The four-eye bionic eye device provided by the invention includes a bionic vision module, a first drive module, a second drive module and a control box connected in sequence, wherein the bionic vision module includes a first binocular vision module and a second binocular vision module composed of two single-double coupled visual sensors that can move toward or away from each other. When searching for the target, the invention controls the first drive module and the second drive module to realize the pitch detection of the four-eye bionic vision module, and controls the vision module to traverse the surrounding environment 360° through traversal search, and generates a motion path after obtaining the target position. The invention combines two visual sensors to expand the scope of the visual domain, improve the search efficiency, and ensure the accuracy of target search through traversal search. The invention is limited by the camera's field of view and the car's freedom constraints, and can only obtain full-space field of view information through a multi-vision module method, which is not suitable for engineering practice.

[0004] Chinese document patent number CN202111599857.6, publication / announcement date: 2021.12.24, discloses a target search and approach method based on an underwater search robot, characterized in that, based on a random search method, visual and optical images are formed by sonar sensors and visual sensors, and convolutional neural networks are used to recognize and process acoustic and optical images to determine whether the target object appears in the acoustic and optical images. If it appears, the position of the target object is calculated by the host computer, and the robot is controlled to approach the target; if the target object does not appear, the robot is controlled to continue random search until the target is found. This invention expands the search range through dual sensors, but the search efficiency of random search is not high compared to traversal search, and the target object information is not fed back in real time during the process of controlling the robot to approach the target. When disturbed by the environment, the target image is prone to loss.

[0005] Chinese patent number CN201210447643.1, published / announced on August 12, 2015, discloses a multi-robot joint target search method that imitates animal spatial cognition. The invention steps are: (1) first determine whether the information is matched successfully; (2) establish the kinematic model and sensor model of each robot; (3) complete the calibration and matching of the internal parameters of the camera; (4) use the self-organizing attractor network algorithm model to locate the robot; (5) establish the environment map and the map fusion of multiple robots; (6) navigation and target search; (7) determine whether the task is completed; if the task is completed, the search task ends; otherwise, continue to step (2) and re-construct the map and search for the target. The invention is based on the random search method of a single robot. By expanding the search range through multiple robots, the search efficiency is greatly improved compared with a single robot. However, compared with the method of traversing the search through a single visual sensor at the end of the robot arm, the invention is more expensive and less efficient. Summary of the invention

[0006] The present invention provides a low-degree-of-freedom target search and arrival control method for a mobile manipulator. The shape and size of the combined visual field space of the working space of any two joints of the mobile manipulator and the field of view of the image acquisition device at its end are studied, and a rough search strategy is given. After obtaining the target position information, the joint movement of the mobile manipulator is adjusted to make the target object in the center of the field of view of the image acquisition device. Finally, the mobile manipulator is controlled to move toward the target object, and the target object is always in the center of the field of view of the image acquisition device during the target arrival process to obtain better arrival accuracy. Studies have shown that this method can achieve target search and arrival using a small number of degrees of freedom, that is, only controlling the movement of two joints. The controller is simple in design and has high positioning efficiency, which is suitable for engineering applications.

[0007] The present invention can be achieved through the following technical solutions:

[0008] A low-degree-of-freedom target search and arrival control method for a mobile mechanical arm is applied to a mobile mechanical arm comprising a trolley, a plurality of rotary joints and a swing joint, and an image acquisition device is arranged at the end of the mobile mechanical arm;

[0009] The two-joint visual space with the smallest invisible area in the mobile robot is selected as the search space, and the target object is roughly searched. After the target object is found, the movement of the two joints in the two-joint visual space is adjusted so that the target object is located in the center of the field of view of the image acquisition device, thereby realizing the precise positioning of the target object.

[0010] Then it is determined whether the target object exceeds the working space of the mobile robot arm. If not, the target search is completed;

[0011] If it exceeds the working space, the car is controlled to move towards the target object. During this process, the movement of the two joints in the two-joint visual space is adjusted to keep the target object in the center of the field of view of the image acquisition device at all times until the distance between the target object and the center of the robot's working space is less than the radius of the robot's working space, and the target arrival task is completed.

[0012] Further, the rough search includes the following steps:

[0013] Step I: calculate the working space of any two joints in the mobile manipulator one by one, and superimpose it with the field of view of the image acquisition device to form multiple two-joint visual spaces of the mobile manipulator, and select the two-joint visual space with the smallest invisible area as the search space, and the corresponding two joints are respectively recorded as the search swing joint and the search rotation joint;

[0014] Step II, calculating the maximum step length corresponding to the swing joint and the rotation joint when the minimum imaging distance of the image acquisition device is reached;

[0015] Step III: Search the target object by searching for the swing joint in a way of traversing one circle with the maximum step length each time the rotary joint moves, until the target object is found, thus completing the rough search.

[0016] Furthermore, the following equations are used to calculate the adjustment angles △φ and △θ of the search swing joint and the search rotation joint, so that the target object is located in the center of the field of view of the image acquisition device, and the target object is precisely positioned.

[0017]

[0018]

[0019] Among them, φ0 and θ0 represent the rotation angles corresponding to the search for the rotary joint and the search for the swing joint after the rough search, respectively, P = (X, Y, Z) = T c ·c P represents the position information of the target object in the coordinate system of the search swing joint after the rough search. c P represents the position information of the target object in the coordinate system of the image acquisition device after the rough search, T c It represents the position information of the image acquisition device in the coordinate system where the swing joint is searched, and a5 represents the Y-axis offset of the installation position of the image acquisition device relative to the end of the robotic arm.

[0020] Furthermore, when reaching the target, the trolley and the search rotary joint are controlled to move in opposite directions so that the trolley faces the target object, and then the trolley is controlled to move towards the target object at a constant speed until the distance between the two reaches a set threshold. During this period, the search swing joint is controlled to move to compensate for the offset of the target object in the field of view of the image acquisition device, ensuring that the target object is always in the center of the field of view of the image acquisition device.

[0021] Furthermore, let the heading angle of the car in the initial state be α car , the desired heading angle is α0, and the proportional controller is designed as shown in the following equation to control the movement of the search rotary joint and the car to make the car face the target object.

[0022]

[0023]

[0024] Among them, k3 is the proportionality coefficient, To search for the angular velocity of the rotary joint, is the angular velocity of the car, X w , Y w is the position information of the target object in the world coordinate system;

[0025] The speed controller is designed as shown in the following equation, which controls the search swing joint to compensate for the field of view deviation of the target object so that the target object is always in the center of the field of view of the image acquisition device.

[0026]

[0027] Among them, k4 is the proportionality coefficient; To search for the motion compensation of the swing joint, x c is the position offset information of the target object before and after the motion of the coordinate system of the image acquisition device, z c It is the z-axis component of the distance information of the target object in the current coordinate system of the image acquisition device.

[0028] The beneficial technical effects of the present invention are:

[0029] Compared with the prior art, the present invention designs a low-degree-of-freedom target search and arrival strategy for a mobile robotic arm in an unfamiliar environment. For the target search task, the high-degree-of-freedom robotic arm is simplified to a two-joint robotic arm, and other joints are not controlled to reduce the control amount. In order to improve the search efficiency of the position space, a two-step search strategy is proposed. First, the trolley is fixed, and the search step size is determined according to the working space of the two-joint robotic arm and the field of view space size of the image acquisition device, and a coarse search strategy is proposed. Then, according to the relationship between the position of the target object and the joint movement, the position relationship between the target object and the camera is accurately calculated, and the robotic arm is moved so that the target object is located at the center of the field of view space of the image acquisition device to complete the precise positioning. Finally, a arrival strategy involving the trolley is adopted to coordinate and control the trolley and the two-joint robotic arm to move to the vicinity of the target object, thereby solving the problem of reaching long-distance targets.

[0030] In addition, the controller of the present invention has a simple design, high positioning efficiency, low energy consumption, and is suitable for engineering applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 It is a schematic diagram of the overall task of the present invention;

[0032] Figure 2 A flow chart of the visual search of the mobile robot arm of the present invention;

[0033] Figure 3 It is a schematic diagram of the structure of the mobile mechanical arm of the present invention;

[0034] Figure 4 This is a schematic diagram of the field of view of the RGBD camera of the present invention;

[0035] Figure 5 This is a schematic diagram of the swing joint field of view space of the present invention;

[0036] Figure 6 It is a schematic diagram of the two-joint visual field space of the present invention;

[0037] Figure 7 It is a schematic diagram of visual space comparison of different joint combinations of the present invention;

[0038] Figure 8 A schematic diagram for determining a rough search step size of the present invention;

[0039] Fig. 9 A schematic diagram of determining the angle of the precise positioning robot arm of the present invention;

[0040] Fig.10 It is a schematic diagram of achieving the strategy of the present invention;

[0041] Fig.11 The image coordinates of the rough search target point and the change diagram of the robot arm joint angle of the present invention;

[0042] Fig.12 It is the precise positioning image trajectory of the present invention and the camera trajectory change diagram in the world coordinate system;

[0043] Fig.13 The precise positioning search of the present invention is a diagram of the coordinate change of the rotary joint, the swing joint angle and the target point in the camera coordinate system;

[0044] Fig.14 The target visual field, joint 5, and the trolley heading angle change diagram for adjusting the vehicle arm posture of the present invention;

[0045] Fig.15 The target point image coordinates and the angle change diagram of joint 6 when the target of the present invention is reached;

[0046] Fig.16 The position change of the car in the world coordinate system and the coordinate change of the target point in the camera coordinate system when the target of the present invention is reached;

[0047] Fig.17 A schematic diagram of the camera posture change in the world coordinate system during the visual search process of the present invention;

[0048] Fig.18 A schematic diagram of the angle changes of joints 5 and 6 during the visual search process of the present invention; DETAILED DESCRIPTION

[0049] The specific implementation of the present invention is described in detail below with reference to the accompanying drawings and preferred embodiments.

[0050] Existing visual search methods are based on auxiliary positioning to determine the approximate location of the target, and then conduct targeted path search. There is little research on how to complete visual search quickly, efficiently, and with low power consumption in unfamiliar environments. Figure 1 As shown, the present invention proposes a low-degree-of-freedom target search and arrival control method for a mobile robotic arm. First, the two-joint visual space with the smallest invisible area in the mobile robotic arm is selected as the search space, and a rough search is performed on the target object. After the target object is found, the movement of the two joints in the two-joint visual space is adjusted so that the target object is located in the center of the field of view of the image acquisition device, thereby achieving precise positioning of the target object. Then, it is determined whether the target object exceeds the working space of the mobile robotic arm. If not, the target search is completed. If it exceeds the working space, the trolley is controlled to move toward the target object. In this process, the movement of the two joints in the two-joint visual space is adjusted so that the target object is always in the center of the field of view of the image acquisition device until the distance between the target object and the trolley reaches the set threshold, and the target arrival is completed. The process is implemented as follows: Figure 2 As shown, the details are as follows:

[0051] The coarse search step length is determined according to the radius of the invisible area of ​​the field of view space and the size of the camera field of view. Then, based on the coarse search, the target is accurately located according to the relationship between the target position and the joint movement. Then, it is determined whether the target is in the working space of the robot arm. If so, the visual search is completed. If not, the car is controlled to drive to the vicinity of the target. Finally, the posture of the car arm is adjusted, and the car is controlled to drive towards the target at a constant speed. During the movement, the field of view of the target object is kept in the center.

[0052] 1. Coarse Search

[0053] Taking the KINOVA robot arm as an example, the mobile robot arm is a seven-degree-of-freedom robot arm, including seven rotating joints and swing joints arranged alternately, which are recorded as joints 1 to 7 from bottom to top. An image acquisition device such as an RGBD camera is set at its end, and a car is set at the bottom. The specific structure is as follows Figure 3 shown.

[0054] The parameters of the mobile robot are shown in Table 1. a is the height from the lifting platform to the ground, d1-d8 are the lengths of the links of the robotic arm, a1 is the offset between joint 1 and joint 2, a2 ​​is the offset between joint 2 and joint 3, a3 is the offset between joint 3 and joint 4, a4 ​​is the offset between joint 4 and joint 5, a5 is the offset between the end and the camera, and d is the range of motion of the lifting platform.

[0055] Table 1

[0056]

[0057] Step Ⅰ: Establish the reference coordinate system of each component as follows: Car coordinate system {car} origin O car Located at the bottom of the mobile platform, the midpoint of the bottom of the seven-degree-of-freedom robotic arm base is the origin O of the world coordinate system {w} w ; The origin O of the base coordinate system {b} of the robot b Located at the bottom center of the robot base, the origin O of the coordinate system {i} of each link i Located at the previous joint of the link, the Z of coordinate system {i} i The axis is collinear with the connecting rod axis; the origin O of the end coordinate system {e} e Located at the center of the end line, the origin O of the RGBD camera coordinate system {c} c Located at the end coordinate system Y e Direction 0.0564m.

[0058] The transformation matrix between the coordinate system {car} and the coordinate system {b} is:

[0059]

[0060] The transformation matrix between coordinate system {b} and coordinate system {1} is:

[0061]

[0062] Transformation matrix between the coordinate systems of each link of the robot It is related to the parity of the connecting rod {i}. When the connecting rod {i} is an even number, we have:

[0063]

[0064] When the connecting rod {i} is an odd number, we have:

[0065]

[0066] The transformation matrix between coordinate system {7} and coordinate system {c} is:

[0067]

[0068] By multiplying the transformation matrices obtained above, we can get the transformation matrix of the mobile robot camera:

[0069]

[0070] Step II: Determine the size of the visual space of the two-joint robotic arm according to the camera field of view and the workspace of the two-joint robotic arm. The camera at the end of the KINOVA robotic arm is an RGBD camera, where the RGB camera model is OV6450 and the depth camera model is Intelrealsense D410. The parameters are shown in Table 2.

[0071] Table 2

[0072]

[0073] The RGBD camera comprehensive field of view angle α = 53°, β = 31°, the image plane display area size is 1.792*1.008mm, and the camera field of view size is determined by the field of view angle and Z c From the camera parameters, we can know that the minimum imaging distance Z of the RGBD camera is selected. cmin = 0.2m, the camera field of view is a quadrangular pyramid, such as Figure 4 When the target is at the minimum distance Z from the camera optical center cmin =0.2m, the camera field of view is:

[0074]

[0075]

[0076] Because the camera has a higher detection accuracy at 2 meters, the present invention selects Z c=2m, then the camera field of view is:

[0077]

[0078]

[0079] Where W and H are the width and height of the camera's field of view, respectively.

[0080] The KINOVA robot arm consists of 4 revolving joints and 3 swinging joints. Joints 1, 3, 5, and 7 are revolving joints, and joints 2, 4, and 6 are swinging joints. The set of points that the end of the robot arm can reach is called the workspace, and the shape of the workspace is approximately a sphere. Since the position of the camera and the end is relatively fixed, the camera workspace and the end workspace are similar in shape but different in size. On the basis of the camera workspace, the camera field of view is superimposed to obtain the robot arm visual space, which is approximately in the shape of a solid sphere. The visual space is the visible area of ​​the camera.

[0081] By analyzing the joint motion, we can see that when only one revolving joint is rotated, the visual space shape of the mobile robot arm is a frustum; due to the existence of joint limits of the swing joint, when only one swing joint is controlled to move, the visual space shape of the robot arm is an incomplete cylinder with a concave center, such as Figure 5 By combining different joints and studying their visual space shapes and sizes, we can conclude that the visual space shape of a joint combination consisting of any swing joint and any rotation joint with a joint number smaller than it is an incomplete hollow sphere, as shown in Figure 6 As shown in the figure, the KINOVA robot arm has three swing joints, namely joint 2, joint 4, and joint 6. The corresponding visual space size comparison is Figure 7 As shown. Since a large invisible area will cause problems such as being unable to search for close objects, the present invention selects the joint combination with the smallest invisible area in the visual space for a rough search, and the remaining joints are regarded as connecting rods and remain motionless during the search. From Table 3, it can be seen that the invisible area of ​​the combination of joints 5 and 6 is the smallest. The search range of the combination of joints 5 and 6 in the world coordinate system is (±0.1738, ±2.1738m), the minimum search height is 1.0199m, and the maximum search height is 3.0199m.

[0082] Table 3

[0083]

[0084] Step III: perform a rough search for the target object.

[0085] In order to ensure that the target object is successfully searched at the minimum imaging distance of the camera, the maximum step length of joints 5 and 6 at the minimum imaging distance of the camera needs to be calculated. The relationship between the movement step length of joint 5 and the radius of the invisible area r3 is as follows: Figure 8As shown, the maximum step sizes α0 and β0 of joints 5 and 6 are as follows:

[0086]

[0087]

[0088] During the coarse search, the rotation joint moves one step at a time, and the swing joint completes one traversal within the joint limit. Set the search step sizes of joints 5 and 6 to 29° and 15° respectively. Joint 5 is a rotation joint and needs to move 13 times to complete one rotation. Joint 6 is a swing joint with a range of motion of ±120° and needs to search 16 times to complete the traversal. Rotation joint 5 moves one step and swing joint 6 searches according to the step size. If the target object is not found, joint 5 moves another step on this basis and the above process is repeated until the target object appears in the visual space. The search is stopped and the coarse search is completed. The target search process is as follows: Fig. 9 shown.

[0089] 2. Precision Positioning

[0090] exist Fig. 9 In the figure, the coordinate system of the initial position of joints 5 and 6 is recorded as {5°} and {6°}. After the rough search, joints 5 and 6 rotate φ0 and θ0 respectively. At this time, the coordinate system of joints 5 and 6 becomes {5} and {6}. The rotation angles of joints 5 and 6 after the rough search and the position coordinates of the target in the camera coordinate system at the end of the rough search are output. c P, then calculate based on the current joint angle Get the position coordinates of the target object under {6}:

[0091]

[0092] In order to ensure that the target field of view is not lost during the target arrival process, the target position needs to be further precisely located based on the rough search results so that the target is in the central area of ​​the camera's field of view. The coordinates of the target obtained after the rough search are recorded as 6 P=(X,Y,Z), control joints 5 and 6 to rotate △φ and △θ respectively on the basis of φ0 and θ0, and the coordinate system of joint 5 is also rotated from {5} to obtain the new coordinate system O5X'5Y'5Z'5. At the same time, the coordinate system of joint 6 follows {5} to rotate to O6X'6Y'6Z'6. Joint 6 rotates △θ around the Z'6 axis relative to θ0. At this time, the target object is located in the center of the camera field of view. The coordinates of the target object in the new coordinate system O6X'6Y'6Z'6 are marked as 6 P'=(X',Y',Z'), 6 The projection of P' onto the X'6O6Y'6 plane, the coordinates of the projection point are marked as P'=(X',Y',0), then the relationship between P' and φ0, θ0 can be expressed as:

[0093]

[0094] Calculations under different circumstances:

[0095]

[0096]

[0097] Among them, φ0 and θ0 represent the rotation angles corresponding to the search for the rotary joint and the search for the swing joint after the rough search, respectively, P = (X, Y, Z) = T c · c P represents the position information of the target object in the coordinate system of the search swing joint after the rough search. c P represents the position information of the target object in the coordinate system of the image acquisition device after the rough search, T c It represents the position information of the image acquisition device in the coordinate system where the swing joint is searched, and a5 represents the Y-axis offset of the installation position of the image acquisition device relative to the end of the robotic arm.

[0098] According to the solved △φ and △θ, joints 5 and 6 are controlled to move on the basis of rough search, so that the target object is located in the center of the camera field of view, and precise positioning is completed.

[0099] 3. Target Arrival

[0100] After the target is precisely positioned, the target is in the center of the camera's field of view. If the target is within the working space of the entire mobile robot arm, the robot arm can be controlled to grab the target. If the target is outside the working space of the entire mobile robot arm, the robot needs to be controlled to move near the target, and then the robot arm needs to be controlled to grab the target. When moving toward the target, it is necessary to ensure that the target is always in the center of the camera's field of view to prevent the target from being lost.

[0101] Before the visual arrival begins, adjust the posture of the car first, and record the heading angle of the car in the initial state as α car , calculate the desired heading angle α0 of the car according to the position of the target in the world coordinate system:

[0102]

[0103] According to α0 and α car , design the car angular velocity controller

[0104]

[0105] In the above formula, k3 is the proportionality coefficient, X w , Y w It is the position information of the target object in the world coordinate system.

[0106] Since the car and joint 5 rotate in opposite directions, in order to ensure that the camera's position remains unchanged in the world coordinate system, joint 5 and the car should rotate in opposite directions. Therefore, the angular velocity controller of joint 5 is designed.

[0107]

[0108] Control the car and joint 5 to rotate in opposite directions. When the car faces the target, control the car to move toward the target at a constant speed. During the movement, the target will gradually deviate from the central area of ​​the camera's field of view, such as Fig.10 As shown. Taking the center of the image plane as the reference, the area of ​​1 / 4 of the center of the image plane is the center area of ​​the field of view. When the field of view of the target object exceeds the central area, it is necessary to control the movement of joint 6 to compensate for the error. During the movement, the compensation amount θ caused by the field of view offset t for:

[0109]

[0110] In the above formula, x c is the position offset information of the target object before and after the motion of the coordinate system of the image acquisition device, z c is the z-axis component of the distance information of the target object in the current coordinate system of the image acquisition device. Design the speed controller of joint 6 during the visual reaching process:

[0111]

[0112] In the above formula, k4 is the proportionality coefficient, is the angular velocity of joint 6. During the arrival process, the image coordinates x and y are kept within the center area of ​​the image plane. When the distance z between the camera and the target c When it is less than the threshold, the goal is reached.

[0113] In order to verify the feasibility of the control method of the present invention, we conducted the following simulation experiments:

[0114] Given a spatial point in the world coordinate system w P(-1, 2, 1.3)m, at the initial moment, the coordinates of the car origin in the world coordinate system are (0, 0, -0.843)m, and the initial heading angle is 0°. Assuming that the car moves at a constant speed during the arrival process, the initial joint angle of the mobile robot is given as Q = [0; 0; 0; 0; 0; 0; 0; -120°; 0], the control cycle is 0.5s, and the 1 / 4 area in the center of the image plane is the center area of ​​the image plane. During the visual arrival process, the image coordinates of the target point satisfy When the distance between the target point and the center of the robot's workspace is less than the radius of the robot's workspace, the mobile robot stops moving. The changes in the image coordinates of the target point, the changes in the joint angles, and the changes in the position of the car and the camera in the world coordinate system during the rough search, fine positioning, and target arrival process are given. Fig.11 , 12 , 13, 14, 15, and 16. According to the proposed two-step search strategy, the target object is coarsely searched and precisely positioned successively. Fig.11 The changes of the target object in the camera's field of view and the changes of the search rotation joint and the search swing joint angle during the rough search process are given. It can be found that the target object's field of view is outside the camera's field of view at the beginning. By traversing the surroundings, the target field of view is obtained. At this time, the target object's field of view is at the edge of the camera's field of view. Accurately calculate the position relationship between the target object and the camera. Fig.12 The changes of the target object in the camera's field of view during the precise positioning process are given. It can be found that the position of the target object's field of view in the camera's field of view moves from the edge to the center, achieving precise positioning of the target object. Fig.13 The angle changes of the searched rotary joint and the searched swing joint in the process of fine positioning and the coordinate changes of the target object in the camera coordinate system are given. It can be found that the searched rotary joint and the searched swing joint can be slightly adjusted based on the joint angles obtained by the rough search to move the field of view of the target object to the center of the camera field of view. At this time, the x position of the target object in the camera coordinate system is c ,y c Approaching 0. Fig.14 The change of the target object's field of view during the arm posture adjustment process is given. During the movement, the target object's field of view is still located in the center area of ​​the camera's field of view. The search rotary joint angle change and the vehicle heading angle change are equal in size and opposite in direction. Fig.15 The target object's field of view changes and the search swing joint angle changes during the target arrival process are given. When the car moves towards the target object, the target object's field of view gradually shifts to the edge. The search swing joint is controlled to compensate for the field of view shift and the target object's field of view is always controlled to be located in the center area of ​​the image plane. Fig.16 The position change of the car in the world coordinate system and the coordinate change of the target in the camera coordinate system are given. As the car gradually approaches the target, it can be found that z c It also gradually becomes smaller than the set threshold. Fig.17 The change of the camera's position in the world coordinate system is given, from the initial moment when the camera is far away from the target to the final moment when the camera moves near the target, which proves the effectiveness of the visual search process. Fig.18 The angle changes of searching rotary joints and searching swing joints during visual search are given. The simulation results show that the strategy proposed in this invention can better complete target search and arrival under low degrees of freedom, with high positioning efficiency and suitable for engineering applications.

[0115] As described above, the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A low-degree-of-freedom target search and arrival control method for a mobile robot, characterized in that: The invention is applied to a mobile mechanical arm including a trolley, a plurality of rotary joints and a swing joint, and an image acquisition device is arranged at the end of the mobile mechanical arm. The two-joint visual space with the smallest invisible area in the mobile robot is selected as the search space, and the target object is roughly searched. After the target object is found, the movement of the two joints in the two-joint visual space is adjusted so that the target object is located in the center of the field of view of the image acquisition device, thereby realizing the precise positioning of the target object. Then it is determined whether the target object exceeds the working space of the mobile robot arm. If not, the target search is completed; If it exceeds the working space, the car is controlled to move toward the target object. During this process, the movement of the two joints in the two-joint visual space is adjusted to keep the target object in the center of the field of view of the image acquisition device until the distance between the target object and the car reaches the set threshold, and the target is reached; The rough search includes the following steps: Step I: calculate the working space of any two joints in the mobile manipulator one by one, and superimpose it with the field of view of the image acquisition device to form multiple two-joint visual spaces of the mobile manipulator, and select the two-joint visual space with the smallest invisible area as the search space, and the corresponding two joints are respectively recorded as the search swing joint and the search rotation joint; Step II, calculating the maximum step length corresponding to the swing joint and the rotation joint when the minimum imaging distance of the image acquisition device is reached; Step III: Search the target object by searching the swing joint for a circle with the maximum step length every time the search revolving joint moves, until the target object is found, thus completing the rough search; The following equations are used to calculate the adjustment angles Δφ and Δθ of the search swing joint and the search rotation joint, so that the target object is located in the center of the field of view of the image acquisition device, and the target object is precisely positioned. Among them, φ0 and θ0 represent the rotation angles corresponding to the search for the rotary joint and the search for the swing joint after the rough search, respectively, P = (X, Y, Z) = T c · c P represents the position information of the target object in the coordinate system of the search swing joint after the rough search. c P represents the position information of the target object in the coordinate system of the image acquisition device after the rough search, T c represents the coordinate transformation matrix between the image acquisition device coordinate system and the search swing joint coordinate system, and a5 represents the Y-axis offset of the installation position of the image acquisition device relative to the end of the robotic arm.

2. The low-degree-of-freedom target search and arrival control method for a mobile robot according to claim 1, characterized in that: When reaching the target, the car and the search swivel joint are controlled to move in opposite directions so that the car faces the target, and then the car is controlled to move towards the target at a constant speed until the distance between the two reaches the set threshold. During this period, the search swing joint is controlled to move to compensate for the offset of the target in the field of view of the image acquisition device, ensuring that the target is always in the center of the field of view of the image acquisition device.

3. The low-degree-of-freedom target search and arrival control method for a mobile robot according to claim 2, characterized in that: Let the heading angle of the car in the initial state be α car , the desired heading angle is α0, and the proportional controller is designed as shown in the following equation to control the movement of the search rotary joint and the car to make the car face the target object. Among them, k3 is the proportionality coefficient, To search for the angular velocity of the rotary joint, is the angular velocity of the car, X w , Y w is the position information of the target object in the world coordinate system; The speed controller is designed as shown in the following equation, which controls the search swing joint to compensate for the field of view deviation of the target object so that the target object is always in the center of the field of view of the image acquisition device. Among them, k4 is the proportionality coefficient; To search for the motion compensation of the swing joint, x c is the position offset information of the target object before and after the motion of the coordinate system of the image acquisition device, z c It is the z-axis component of the distance information of the target object in the current coordinate system of the image acquisition device.

Images