Cooperative control method for hexapod robot and bionic mechanical arm

Abstract

The invention provides a cooperative control method for a hexapod robot and a bionic mechanical arm, and the method comprises the steps: firstly transmitting a laser pulse through a laser radar at high frequency, and generating three-dimensional point cloud data based on a plurality of distance measurement points in a space; performing denoising, registration, filtering, simplification and data fusion on the data to construct a complete three-dimensional scene model; generating a digital topographic map containing terrain surface and obstacle information through feature extraction; the main control component controls the movement of the multi-joint legs based on a terrain model: in the walking stage, in combination with global path planning and a local obstacle avoidance algorithm, static obstacle avoidance and dynamic obstacle real-time avoidance are realized; and after the robot reaches the vicinity of the target, the main control component controls the mechanical arm of the robot to grab, and the angle of each joint of the mechanical arm is adjusted by utilizing path planning and an obstacle avoidance algorithm, so that the mechanical arm avoids the obstacle, and the mechanical arm can accurately grab the target object.

Description

Technical Field

[0001] The present invention belongs to the technical field of bionic robot control, and particularly relates to a cooperative control method for a hexapod robot and a bionic manipulator. Background Art

[0002] In the existing manipulator and hexapod robot technologies, their respective design concepts and functional orientations have a direct impact on their application scenarios. A manipulator usually consists of rotating joints, linear actuators, and sensors. Its motion principle is based on the rotation and linear motion of the joints, and it can achieve complex action sequences through programming. However, the adaptability of the manipulator in a dynamic environment is relatively weak. For example, when it is necessary to operate on uneven or variable terrains, the manipulator often has difficulty maintaining a stable working state, resulting in low operation efficiency. In addition, traditional manipulators are usually in a fixed state and cannot operate in narrow spaces (such as mine shafts and pipelines), which limits their application in specific environments. More importantly, the manipulator cannot effectively avoid obstacles during the grasping process, resulting in poor mobility and environmental adaptability.

[0003] Similarly, although hexapod robots perform well in terms of mobility and stability, they are inadequate when performing fine tasks. Current hexapod robots can only rely on the front arms (left front arm and right front arm) or rear arms (left rear arm and right rear arm) to pick up items. However, due to the short leg arms and the ability to only pick up objects directly in front of the robot, it cannot reach items located on its left or right. When walking on only four legs, the stability of the robot will drop significantly. Therefore, the operation accuracy and flexibility of hexapod robots are far inferior to those of manipulators. At the same time, due to the special structure of hexapod robots, the terrains they adapt to are relatively complex, such as irregular terrains like mine shafts, mountains, and ruins, which pose high requirements for their walking stability.

[0004] To address the above problems, how to achieve stable movement of hexapod robots in complex terrains, how to achieve effective and stable grasping of targets in special environments, and how to realize the cooperative control of manipulators and hexapod robots will be the key technical problems that we urgently need to solve. Summary of the Invention

[0005] The objective of the present invention is to propose a cooperative control method for a hexapod robot and a bionic manipulator. Through this method, the hexapod robot can achieve stable walking on rough terrains like a spider. At the same time, the equipped bionic manipulator can perform multi-angle movements, be able to avoid various obstacles on the grasping path in a complex environment, and achieve automatic grasping. In addition, this method also supports the cooperative control of the robot and the manipulator. When the target is not within the grasping range of the manipulator, the hexapod robot can make fine adjustments to meet the operation conditions; after grasping, the bionic manipulator can autonomously avoid obstacles on the path to ensure that it does not interfere with the movement of the robot.

[0006] The technical solution adopted by the present invention is: a cooperative control method for a hexapod robot and a bionic manipulator. The hexapod robot includes a leg mechanism, a camera, a lidar, a manipulator, a robot body, and a mechanical claw;

[0007] The six leg mechanisms are arranged around the robot body and are used to drive the movement of the robot body;

[0008] The camera is arranged above the robot body and is used to collect the images around the robot;

[0009] The lidar is arranged above the robot body and is used to sense the terrain around the robot;

[0010] The manipulator is arranged on the robot body, and a mechanical claw is arranged on the manipulator;

[0011] A main control component is arranged inside the robot body. The main control component can transmit the images collected by the camera back to the terminal and is used to control the movement of the leg mechanism and the manipulator;

[0012] The control method of the hexapod robot includes the following steps:

[0013] S1. The lidar calculates the distance to the target object by emitting laser pulses and measuring the time it takes for the pulses to reflect back from the target object to the sensor. The frequent emission of laser pulses can obtain multiple ranging points in space, thereby generating a set of data points with three-dimensional coordinates and generating a three-dimensional point cloud dataset;

[0014] S2. Remove noise from the three-dimensional point cloud dataset, perform registration, filtering, and simplification;

[0015] S3. Merge the point cloud data after multiple laser scans into a unified coordinate system. The merging process usually includes a registration step to align multiple datasets to obtain a complete three-dimensional scene;

[0016] S4. Extract the features of the terrain surface, obstacles, or buildings from the point cloud data and generate a topographic map;

[0017] S5. The main control component controls the movement of the leg mechanism of the robot so that it walks according to the modeled terrain, and uses multi-joint control to ensure the coordinated movement of the legs of the robot, thereby effectively adapting to the terrain;

[0018] S6. When encountering static obstacles, the robot uses global path planning to calculate an optimal path from the starting point to the ending point based on the map and the target position, and adopts local obstacle avoidance during the movement to ensure avoiding moving obstacles. This combination of global and local path planning ensures the efficient movement of the robot in a complex environment;

[0019] After reaching near the target in S7, the manipulator of the robot is controlled by the main control component to perform grasping, and the path planning and obstacle avoidance algorithms are used to adjust the angles of the joints of the manipulator, so that the manipulator can avoid obstacles and ensure that the manipulator can accurately grasp the target object.

[0020] Furthermore, the mechanical claw includes a servo motor, a large arm link, a small arm link, a clamping link, a mechanical claw support, a fixed platform and a ball screw. The servo motor is installed on the fixed platform, and the fixed platform is connected to the mechanical claw support by screws; a grasping mechanism formed by the large arm link, the small arm link and the clamping link is arranged on the upper and lower sides of the mechanical claw support; the ball screw converts the rotational motion of the servo motor into a linear motion to drive the grasping mechanism to move.

[0021] Furthermore, the manipulator includes a shoulder joint part, a large arm rod and a small arm rod. The shoulder joint and the robot body are rotated by a fourth servo motor; one end of the large arm rod is fixedly connected to the shoulder joint; the other end of the large arm rod is connected to one end of the small arm rod by a third servo motor; the other end of the small arm rod controls the swing and rotation of the mechanical claw through a second servo motor and a first servo motor which are arranged perpendicular to each other.

[0022] Furthermore, the leg mechanism includes a front leg rod, a rear leg rod and a connecting joint; the front leg rod and the rear leg rod are swung in a two-stage series by a second leg servo motor and a third leg servo motor; the rotation between the connecting joint and the robot body is driven by a first leg servo motor.

[0023] Furthermore, in S2, the statistical outlier removal method is adopted to remove noise, specifically:

[0024] For each point p i and the point set N(p i ) in its neighborhood, calculate the average distance μ i and the standard deviation σ i :

[0025]

[0026] where, ||p i -p j || is the Euclidean distance between point p i and point p j ;

[0027] If the average distance μ i of point p i exceeds a certain threshold μ thresh plus multiple kσ of the standard deviation i , then this point is considered an outlier:

[0028] μi = μ thresh + kσ i

[0029] Otherwise, the point is retained.

[0030] Furthermore, the specific steps of S3 are as follows:

[0031] Minimize the distance between the source point cloud P s and the target point cloud P t through an iterative method to align the two. In each iteration, ICP calculates the nearest neighbor of each point and optimizes the transformation matrix by the least squares method to minimize the distance between the point clouds.

[0032] Calculate the distance between each pair of points p i and q i :

[0033] d i = ‖p i - q i ‖

[0034] Minimize the distance between the source point cloud and the target point cloud:

[0035]

[0036] where E(R, t) represents the distance error after the source point cloud is rotated and translated; R is the rotation matrix; t is the translation vector;

[0037] Optimize R and t by the least squares method to obtain the optimal transformation matrix to minimize the distance between the point clouds.

[0038] Furthermore, the specific steps of S4 are as follows:

[0039] Randomly select 3 points {p1, p2, p3} and calculate the plane parameters determined by these three points; assume the equation of the plane is:

[0040] ax + by + cz + d = 0

[0041] where (a, b, c) is the normal vector of the plane and d is the plane offset;

[0042] Calculate the distance ρ i between this plane and other points p i :

[0043]

[0044] If it is less than ρ i a certain preset threshold ε, then the point p i is considered an inlier of the plane;

[0045] For each round of random sampling, calculate the number N of inliers, and select the model with the most inliers as the final plane;

[0046] Assume that the elevation values of the given points p1, p2, … p n are z(p1), z(p2), … z(p n ), then the elevation value of the new point p0 is z(p0);

[0047] Generate a topographic map using Kriging interpolation method, and its formula is:

[0048]

[0049] where δ i is the weight to be solved, and the constraint condition is:

[0050]

[0051] The weight δ of Kriging i is solved by the following system of equations:

[0052]

[0053] where γ(p i , p j ) is the autocovariance function between points p i and p j .

[0054] Furthermore, the global path planning adopts the A* algorithm, and the cost function f(n) of the A* algorithm consists of two parts:

[0055] f(n) = g(n) + h(n)

[0056] where f(n) is the total cost function of node n; g(n) is the actual cost from the starting point to the current node n; h(n) is the heuristic function, representing the estimated cost from the current node n to the target node;

[0057] Use the Euclidean distance:

[0058]

[0059] where (x n , y n ) are the coordinates of node n, and (x t , y t ) are the coordinates of the target node t;

[0060] The local obstacle avoidance adopts the artificial potential field method:

[0061] Assume that the coordinates of the target point are (x t , yt ) If the current position of the robot is (x, y), then the attractive force can be expressed as F a as:

[0062] F a = -k a ·(x - x t , y - y t )

[0063] where k a is the coefficient of the attractive force; (x - x t , y - y t ) is the vector between the current position of the robot and the target position, pointing to the target point;

[0064] Assume the coordinates of the i-th obstacle are (x 0i , y 0i ), and the current position of the robot is (x, y). Then the repulsive force generated by the obstacle can be expressed as:

[0065]

[0066] where d i is the distance from the robot to the i-th obstacle; k r is the coefficient of the repulsive force; r0 is a threshold, indicating that the repulsive force will only be generated when the distance from the obstacle is less than r0;

[0067] Furthermore, in S7, the method for the robotic arm to avoid obstacles is specifically as follows:

[0068] Define the minimum safe distance d safe between the gripper and the obstacle. If the distance d obs between the obstacle and the gripper is less than d safe , then the path needs to be adjusted;

[0069] When avoiding obstacles, the method of virtual force field is used to adjust the movement of the robotic arm. The formula is:

[0070] F total = F goal + F obs

[0071] where F goal is the force generated by the target position; F obs is the reverse force generated by the obstacle;

[0072] The target force F goal aims to guide the gripper to the target position P goal , and its expression is:

[0073] F goal = kgoal ·(P goal -P current )

[0074] where k goal is the constant of the target attraction force, and P current is the current position of the robotic gripper.

[0075] The obstacle force F obs is calculated based on the distance between the obstacle and the robotic arm. Assuming the obstacle position is P obs , the reaction force generated by the obstacle can be expressed as:

[0076]

[0077] where k obs is the constant of the obstacle repulsion force, and ‖P current -P obs ‖ 3 is the distance from the robotic gripper to the obstacle;

[0078] The force is transformed from the task space to the joint space using the Jacobian matrix. The formula is:

[0079] τ = J T ·F total

[0080] where τ = [τ1, τ2, … τ6] is the torque of each joint of the robotic arm; J is the Jacobian matrix of the robotic arm, which describes the relationship between the speed of the robotic gripper and the joint angular velocity; F total is the total force in the task space.

[0081] The beneficial effects of the present invention are as follows: By cooperating the hexapod robot with the bionic robotic arm, the lidar first scans the terrain, and then generates a topographic map after being processed by the main control component. The hexapod robot autonomously plans a route to the vicinity of the target, and at the same time autonomously avoids obstacles during the movement. When it reaches the vicinity of the target, the robotic arm performs a grasping operation, and at the same time moves flexibly during the grasping process to avoid obstacles, and finally successfully grasps the target. BRIEF DESCRIPTION OF THE DRAWINGS

[0082] Figure 1 is the structure diagram of the hexapod robot;

[0083] Figure 2 is the top view of the robotic gripper;

[0084] Figure 3 is the side view of the robotic gripper;

[0085] Figure 4 is the side view of the robotic arm;

[0086] Figure 5It is the left view of the robotic arm;

[0087] Figure 6 It is the structural schematic diagram of the leg mechanism;

[0088] Figure 7 It is the schematic diagram of the collaborative control method for the hexapod robot and the bionic robotic arm;

[0089] Figure 8 It is the A* algorithm path planning model;

[0090] In the figure: 1 - leg mechanism, 101 - connecting joint, 102 - hind leg rod, 103 - second leg servo, 104 - first leg servo, 105 - third leg servo, 106 - front leg rod, 2 - camera, 3 - lidar, 4 - robotic arm, 401 - forearm rod, 402 - upper arm rod, 403 - shoulder joint part, 404 - third servo, 405 - first servo, 406 - second servo, 407 - fourth servo, 5 - robot body, 6 - robotic claw, 601 - servo, 602 - upper arm link, 603 - forearm link, 604 - clamping link, 606 - robotic claw support, 607 - fixed platform, 608 - ball screw. Specific implementation mode

[0091] The present invention will be further described below with reference to the accompanying drawings.

[0092] As Figures 1 to 6 shown, the present invention is a collaborative control method for a hexapod robot and a bionic robotic arm. The hexapod robot in the present invention includes a leg mechanism 1, a camera 2, a lidar 3, a robotic arm 4, a robot body 5 and a robotic claw 6; six leg mechanisms 1 are arranged around the robot body 5 and are used to drive the robot body to move, and can adapt to the walking ability on rough terrain. The leg mechanism 1 includes a front leg rod 106, a hind leg rod 102 and a connecting joint 101; the front leg rod 106 and the hind leg rod 102 are realized by a second leg servo 103 and a third leg servo 105 for two-stage series swing; the rotation between the connecting joint 101 and the robot body 5 is driven by a first leg servo 104. The bottom of the front leg rod 106 is provided with a semi-sphere made of plastic material, which can prevent slipping and reduce the stress when landing.

[0093] The camera 2 is arranged above the robot body 5 and is used to collect the images around the robot.

[0094] The lidar 3 is arranged above the robot body 5 and is used to sense the terrain around the robot.

[0095] The robotic arm 4 is arranged on the robot body 5. The robotic arm 4 includes a shoulder joint part 403, a large arm rod 402 and a small arm rod 401. The shoulder joint 403 and the robot body 5 are rotated through a fourth servo 407; one end of the large arm rod 402 is fixedly connected to the shoulder joint 403; the other end of the large arm rod 402 is connected to one end of the small arm rod 401 through a third servo 404; the other end of the small arm rod 401 controls the swing and rotation of the mechanical claw 1 through a second servo 406 and a first servo 405 which are arranged perpendicular to each other.

[0096] A mechanical claw 6 is arranged on the robotic arm 4. The grasping ability of the hexapod robot is mainly realized by the mechanical claw 1. The mechanical claw 6 includes a servo 601, a large arm connecting rod 602, a small arm connecting rod 603, a clamping connecting rod 604, a mechanical claw support 606, a fixed platform 607 and a ball screw 608. The servo 601 is installed on the fixed platform 607, and the fixed platform 607 is connected to the mechanical claw support 606 through screws; a grasping mechanism formed by the large arm connecting rod 602, the small arm connecting rod 603 and the clamping connecting rod 604 is arranged on the upper and lower sides of the mechanical claw support 606; the clamping connecting rod 604 is designed to be longer to facilitate grasping larger items. The ball screw 608 converts the rotational motion of the servo 601 into a linear motion to drive the grasping mechanism to move.

[0097] A main control component is arranged inside the robot body 5. The main control component uses a Raspberry Pi and can transmit the pictures collected by the camera back to the terminal and is used to control the movement of the leg mechanism 1 and the robotic arm 4.

[0098] As Figure 7 shown, based on the above device, the control method of the hexapod robot in the present invention includes the following steps:

[0099] S1, the lidar calculates the distance of the target object by emitting laser pulses and measuring the time for the pulses to be reflected back to the sensor from the target object; after these reflected signals are processed, a high-precision three-dimensional point cloud data set is generated. The point cloud data collected by the lidar contains a large amount of three-dimensional coordinate information (X, Y, Z). These data are usually raw, unordered and contain noise, and a series of point cloud processing steps are required to remove noise, perform registration, filtering and simplification, etc.

[0100] S2, removing noise, performing registration, filtering and simplification on the three-dimensional point cloud data set.

[0101] The noise can be removed by the statistical outlier removal method. The statistical outlier removal is based on the density of points in the local neighborhood of the point cloud. Specifically: for each point, calculate its distance from the surrounding points; if the average distance of a certain point from the neighboring points is much greater than the global distance, then this point is considered an outlier. The following is the specific formula:

[0102] For each point pi and the set of points N(p i ) in its neighborhood, calculate the average distance μ i and the standard deviation σ i :

[0103]

[0104] where, ||p i - p j || is the Euclidean distance between point p i and point p j .

[0105] If the average distance μ j of point p i exceeds a certain threshold μ thresh plus multiple kσ of the standard deviation i , then this point is considered an outlier:

[0106] μ i = μ thresh + kσ i

[0107] Otherwise, this point is retained.

[0108] S3. Merge the point cloud data after multiple laser scans into a unified coordinate system.

[0109] In multiple laser scans, the point cloud may come from different perspectives or different times. These point cloud data need to be merged into a unified coordinate system through registration technology. Use the Iterative Closest Point (ICP). The ICP algorithm is the most commonly used point cloud registration algorithm. It minimizes the distance between the source point cloud P s and the target point cloud P t to make them aligned. In each iteration, ICP calculates the nearest neighbor of each point and optimizes the transformation matrix through the least squares method to minimize the distance between the point clouds.

[0110] Calculate the distance between each pair of points p i and q i :

[0111] d i = ‖p i - q i ‖

[0112] Minimize the objective function (minimize the distance between the source point cloud and the target point cloud):

[0113]

[0114] Among them, E(R, t) is the sum of squared residuals, representing the distance error after the source point cloud is rotated and translated; R is the rotation matrix; t is the translation vector.

[0115] Optimize R and t through the least squares method to obtain the optimal transformation matrix.

[0116] S4. Extract the features of the terrain surface, obstacles or buildings from the point cloud data and generate a topographic map.

[0117] After the point cloud data processing is completed, the extraction and construction of the terrain model follow. The goal of this step is to extract features such as the terrain surface, obstacles or buildings from the point cloud data. Generally, ground points refer to those laser reflection points on the ground, rather than the reflection points of vegetation, buildings or other objects. The Random Sample Consensus (RANSAC) algorithm can be used for ground extraction. By randomly selecting sample points, a plane model is estimated, and inliers and outliers are determined through consistency judgment.

[0118] Randomly select three points {p1, p2, p3} and calculate the plane parameters determined by these three points; assume the equation of the plane is:

[0119] ax + by + cz + d = 0

[0120] Among them, (a, b, c) is the normal vector of the plane; d is the plane offset.

[0121] Calculate the distance ρ i between this plane and other points p i :

[0122]

[0123] If it is less than ρ i a certain preset threshold ε, then it is considered that the point p i is an inlier of the plane. For each round of random sampling, calculate the number N of inliers and select the model with the most inliers as the final plane.

[0124] After that is the generation of the topographic map. The generation of the topographic map usually depends on the subsequent processing of the point cloud data, converting these point cloud data into a format that can be displayed in map software. The Kriging interpolation method can be used to generate the topographic map. It is an interpolation method based on spatial statistics, using the autocovariance model to infer the values between discrete points. For the terrain modeling of point cloud data, Kriging interpolation can generate a smooth digital terrain model.

[0125] Assume that the elevation values of the given points p1, p2, … p n are z(p1), z(p2), … z(p n ), and estimate its elevation value z(p0) at the new point p0.

[0126] Kriging interpolation formula:

[0127]

[0128] where δ i is the weight to be determined, and the constraint condition is:

[0129]

[0130] The weight δ of Kriging i is solved by the following system of equations:

[0131]

[0132] where γ(p i , p j ) is the autocovariance function between point p i and p j .

[0133] Finally, a topographic map that can be recognized and used for movement by the robot is generated.

[0134] S5. The main control component controls the movement of the robot's leg mechanism so that it walks according to the modeled terrain.

[0135] The hexapod robot can move by imitating the gait of animals. In order to enable the hexapod robot to walk according to the modeled terrain, the kinematic models of the robot (including body movement and leg movement), the ground model, and gait control are the key.

[0136] The present invention sets up a coordinate system (x b , y b , z b ) fixed on the robot body, with its origin at the center of the robot and the z b axis pointing upward. Each leg has its own local coordinate system (x i , y i , z i ) for describing the positions of the various joints and the end of the leg. The goal is to enable the hexapod robot to adjust the position of each leg according to the changes in the terrain and walk stably.

[0137] The terrain is described by the modeled height function z(x, y), which represents the height of the ground at any position (x, y). Therefore, the height function z(x, y) of the terrain provides the target height for each leg. Specifically, when the robot is moving, the robot needs to adjust the end height according to the position of its legs to adapt to the terrain.

[0138] The position of the hexapod robot's body is usually represented by translation and rotation. The translation of the body can be represented by (x b , yb , z b ) is represented. The posture of the robot (i.e., the rotation of the main body) can be represented by the pitch angle θ x , roll angle θ y and yaw angle θ z . Posture control adjusts the stability and walking mode of the robot by changing the orientation of the robot's main body.

[0139] Generally, periodic gait control is used to achieve stable walking. Periodic gait control enables the robot to maintain balance and adapt to terrain changes in different gaits according to the movement sequence and periodic movement pattern of each leg of the robot. To achieve this goal, the present invention periodically distributes the leg movements of the robot to each time step and ensures the stability and smoothness of the robot through appropriate control strategies, determining the support and swing sequences of each leg, as well as the target position, speed, and acceleration of the legs within one cycle.

[0140] A gait cycle T is a complete cycle of all leg movements when the robot is walking. Generally, the gait cycle is divided into multiple time steps Δt. Within each time step, the robot controls the target position of each leg and performs support and swing operations. Assume that the cycle T is divided into N small time steps Δt, i.e.:

[0141] T = N·Δt

[0142] Within each time step, the robot will control the movement of the legs according to the gait pattern.

[0143] Hexapod robots usually use periodic gait control. The triangular gait is one of the most commonly used gait patterns. In the triangular gait pattern, the six legs are divided into two groups, with three legs supporting the ground simultaneously and the other three legs swinging in the air. Generally, the selection of these three supporting legs and swinging legs is based on a periodic rule and can be divided by the time steps in the gait cycle. Assume that the six legs are numbered L1, L2, …, L6. According to the rule of the triangular gait, these legs are divided into two groups of three legs, namely the supporting legs and the swinging legs. Then, within each gait cycle, the roles of the supporting legs and the swinging legs will alternate. To control the movement of each leg within the gait cycle, the present invention describes the movement trajectory of each leg through the following control strategy.

[0144] First, for the calculation of the target position, assume that the position of the robot's main body in three-dimensional space is (x b , y b , z b ). The end position (x i (t), y i (t), z i (t)) of each leg is affected by the position of the main body and the terrain model z(x, y).

[0145] Keep the height at the end of each support leg approximately constant, ensure its stable support on the ground, and design the motion trajectory of the swing leg according to the gait cycle T. The motion equations of the support legs are relatively simple because they are relatively stationary within one cycle and their main task is to support the robot. The speed of the support legs should be close to zero as they are only used for support.

[0146] The target position of the swing leg changes with time. Within each cycle, the motion of the swing leg is periodic, and the present invention describes its motion trajectory through sine or cosine functions. Assuming that the motion trajectory of the swing leg changes periodically with time t, then its position can be expressed as:

[0147]

[0148] z i = z(x i (t), y i (t)) + δz i

[0149] The speed and acceleration of the swing leg can be obtained by differentiating the position equation. To ensure a smooth gait transition, the roles of the support leg and the swing leg can be exchanged at the midpoint T / 2 of each cycle. The gait cycle T is usually fixed, and the motion of the legs at each time step within the cycle can be controlled by discretizing time.

[0150] S6. When facing obstacles on the topographic map, the hexapod robot needs to avoid obstacles and select an optimal travel route. First, it obtains information about the terrain and obstacles through lidar, then generates an environmental map using the perception data, and identifies the position and shape of the obstacles. Based on the map and obstacle information, it plans an obstacle avoidance path. During the travel process, the robot needs to continuously monitor the obstacles ahead and adjust the route according to the actual situation to avoid collisions. When the robot already has a global map, it will use global path planning to calculate an optimal path from the starting point to the ending point based on the map and the target position. In a static obstacle environment, the global path planning algorithm can effectively avoid obstacles. At the same time, during the motion process, local obstacle avoidance is adopted to ensure avoiding moving obstacles, and the route of the robot can be adjusted in real time in these situations to avoid collisions.

[0151] In the present invention, the global planning path uses the A* algorithm to find the optimal path from the starting point to the target point in a grid map with obstacles, taking into account both the actual cost from the starting point to the current node and the estimated cost from the current node to the target node, so as to efficiently find the optimal path.

[0152] The cost function f(n) of the A* algorithm consists of two parts:

[0153] f(n) = g(n) + h(n)

[0154] Among them, f(n) is the total cost function of node n (i.e., the current cost function); g(n) is the actual cost from the starting point to the current node n (i.e., the actual cost such as the number of steps or time required to reach the current node); h(n) is a heuristic function, representing the estimated cost from the current node n to the target node (i.e., the estimated cost to the target, usually a heuristic distance metric).

[0155] Commonly used heuristic functions include Manhattan distance, Euclidean distance, and Chebyshev distance. The present invention uses Euclidean distance because it is applicable to movements in any direction:

[0156]

[0157] where (x n , y n ) are the coordinates of node n; (x t , y t ) are the coordinates of the target node t.

[0158] Local obstacle avoidance adopts the artificial potential field method, which guides the robot towards the target by creating a "potential field" while avoiding collisions with obstacles. The principle is based on the gravitational and repulsive force models in physics. The target node generates an attractive force on the robot, while the obstacle generates a repulsive force on the robot. Eventually, the robot will move along the direction of the resultant force of these forces.

[0159] The target point is usually regarded as an "attraction source", whose function is to attract the robot to the target position. The magnitude of the attractive force is related to the distance between the robot and the target, and is usually represented by a function of the distance.

[0160] Assume that the coordinates of the target point are (x t , y t ), and the current position of the robot is (x, y), then the attractive force can be expressed as F a as follows:

[0161] F a = -k a ·(x - x t , y - y t )

[0162] where k a is the coefficient of the attractive force, controlling the intensity of the attractive force. Usually, k a is a positive constant; (x - x t , y - y t ) is the vector between the current position of the robot and the target position, pointing to the target point.

[0163] The repulsive force generated by the obstacle is used to prevent the robot from colliding with the obstacle. The magnitude of the repulsive force is related to the distance between the robot and the obstacle. Assume the coordinates of the i-th obstacle are (x 0i , y 0i ), and the current position of the robot is (x, y). Then the repulsive force generated by the obstacle can be expressed as:

[0164]

[0165] where d i is the distance from the robot to the i-th obstacle; k r is the coefficient of the repulsive force, which controls the intensity of the repulsive force; r0 is a threshold, indicating that the repulsive force will only be generated when the distance from the obstacle is less than r0.

[0166] As Figure 8 shown, the red dots represent obstacles, the green dots represent the starting point, the blue dots represent the target point, and the blue lines represent the path found by the A* algorithm.

[0167] The total moving force of the robot is the synthesis of the attractive force and the repulsive forces of all obstacles. In this way, the robot can autonomously plan the route and avoid the obstacles on the route, and finally reach the target location smoothly.

[0168] S7. The main control component controls the robotic arm with six degrees of freedom to perform grasping, and uses path planning and obstacle avoidance algorithms to adjust the angles of the joints of the robotic arm to make the robotic arm avoid obstacles.

[0169] The motion range of the six-degree-of-freedom robotic arm mimics the kinematic structure of the human body and imitates the functions of the human arm. The shoulder joint, elbow joint, and wrist joint of the human body each have different degrees of freedom and motion ranges, and the six-degree-of-freedom robotic arm achieves similar flexibility and diversity through the combination of multiple joints. The degrees of freedom and motion ranges of each joint can be adjusted through the kinematic control of the robotic arm, thereby facilitating the adjustment of the shape of the robotic arm in different situations, and can flexibly adjust autonomously through machine learning to avoid obstacles during the movement of the robot and ensure safety.

[0170] The control equations of the six-degree-of-freedom robotic arm can be analyzed from inverse kinematics and forward kinematics. Forward kinematics is to calculate the position and orientation of the end effector by knowing the angles (or positions) of each joint. Inverse kinematics is to calculate the angles (or positions) of each joint according to the desired position and orientation of the end effector.

[0171] The calculation of forward kinematics usually adopts the DH parameter method, that is, the geometric relationship between each joint and link is described by a set of fixed standardized parameters. The DH parameters include:

[0172] θ i : The rotation angle of the i-th joint;

[0173] d i : offset of the i-th joint;

[0174] a i : the length of the connecting rod of the i-th joint;

[0175] α i : The torsion angle of the i-th joint.

[0176] The DH parameter table is in the following form (transformation matrix for each joint):

[0177]

[0178] Using the DH parameter method, the position and posture of the entire robotic gripper can be obtained by multiplying the transformation matrix of each joint:

[0179]

[0180] in, is the pose matrix of the robotic gripper, which includes position and orientation.

[0181] The position P and direction R of the robot claw are given by gives:

[0182]

[0183] Where R is a 3×3 rotation matrix and P is a position vector.

[0184] In most cases, the inverse kinematics solution of a 6-DOF manipulator relies on numerical optimization methods. The goal of inverse kinematics is to solve the following equations:

[0185]

[0186] Among them, T d is the target pose of the given gripper; The position of the robotic gripper is determined by the joint angles θ1 to θ6.

[0187] In addition to the angle control equations, the kinematic equations of the manipulator must also be considered to ensure that the manipulator can reach the target position smoothly and efficiently. The kinematic relationship can be solved using the Jacobian matrix, which describes the relationship between the position change of the manipulator gripper and the change in joint angle. The Jacobian matrix can be used to easily convert the velocity in the joint space to the task space. The Jacobian matrix J can be expressed as:

[0188] v=J·θ

[0189] Among them, v is the linear and angular velocity of the robotic gripper (task space velocity); θ is the angular velocity of the joint; J is the Jacobian matrix.

[0190] Through the Jacobian matrix, the transformation from the task space to the joint space can be achieved, and the inverse kinematics can also be solved through the pseudo-inverse:

[0191]

[0192] Among them, is the pseudo-inverse of the Jacobian matrix.

[0193] The above is the basic control method of the robotic arm. By controlling the movement angles of each servo on the robotic arm, the robotic arm can move accurately and efficiently to the position of the target to be grasped for the grasping operation. However, due to the limitations of the environment where the robot is located, there may be obstacles on the grasping path during grasping. Then, the robotic arm needs to avoid the obstacles autonomously, and then readjust the servo angles on the original grasping route to avoid the obstacles for grasping.

[0194] S5: To make the six-degree-of-freedom robotic arm avoid obstacles, it is usually necessary to consider the positional relationship between the current position of the robotic gripper and the position of the obstacle, and use path planning and obstacle avoidance algorithms to adjust the angles of each joint of the robotic arm. First, the position and pose of the robotic gripper need to be determined, and then the position of the obstacle, which is obtained through a lidar and spatially modeled. Finally, the target position of the robotic gripper (assuming there are no obstacles).

[0195] The present invention adjusts the joint angles of the robotic arm according to the position of the obstacle, so that the robotic gripper avoids the obstacle and reaches the target position, including the following steps:

[0196] Target pose planning: The target position and pose that the robotic arm needs to reach;

[0197] Obstacle detection: Detect the obstacle through a sensor and calculate the distance between the obstacle and the robotic gripper;

[0198] Path adjustment: If the path of the obstacle intersects with that of the robotic gripper, the path needs to be dynamically adjusted.

[0199] First, a geometric model for obstacle avoidance is established. For the angle adjustment of each joint, a safety distance d safe is first defined, that is, the minimum safety distance between the robotic gripper and the obstacle. If the distance d obs between the obstacle and the robotic gripper is less than d safe , then the path needs to be adjusted. The movement of the obstacle and the robotic arm is a non-linear relationship. Therefore, it is necessary to adjust the movement of the robotic arm through the displacement vector and the force field, and change the position of the robotic gripper by controlling the angular change θ i (t) of each joint.

[0200] When avoiding obstacles, the method of virtual force field can be used to adjust the movement of the robotic arm. The obstacle will generate a reverse force, and the robotic gripper will be affected by this reverse force to avoid the obstacle. The control formula of this method is usually based on the force field model, and the formula is:

[0201] F total = F goal + F obs

[0202] Where: F goal is the force generated by the target position; F obs is the reverse force generated by the obstacle.

[0203] The target force F goal is intended to guide the robotic gripper to the target position P gosl , and its expression is:

[0204] F goal = k goal ·(P goal - P current )

[0205] Where k goal is the constant of the target attraction; P current is the current position of the robotic gripper.

[0206] The obstacle force F obs is calculated according to the distance between the obstacle and the robotic arm, and usually decreases with the increase of the distance. Assuming the obstacle position is P obs , the reverse force generated by the obstacle can be expressed as:

[0207]

[0208] Where k obs is the constant of the obstacle repulsion; ‖P current - P obs ‖ 3 is the distance from the robotic gripper to the obstacle.

[0209] Once the total force F total of the robotic gripper is calculated, the force can be transformed from the task space to the joint space by using the Jacobian matrix, which is realized by the following formula:

[0210] τ = J T · F total

[0211] Where τ = [τ1, τ2, … τ6] is the torque of each joint of the robotic arm; J is the Jacobian matrix of the robotic arm, which describes the relationship between the speed of the robotic gripper and the angular velocity of the joint; F totalis the total force in the task space.

[0212] Through the torque τ, a control algorithm (such as a PID controller) can be used to adjust the angles of each joint so that the robotic gripper avoids obstacles and finally reaches the target position.

Claims

1. A cooperative control method for a hexapod robot and a bionic robotic arm, characterized in that: The hexapod robot includes a leg mechanism (1), a camera (2), a lidar (3), a robotic arm (4), a robot body (5), and a robotic claw (6); The six leg mechanisms (1) are arranged around the robot body (5) and are used to drive the movement of the robot body; The camera (2) is arranged above the robot body (5) and is used to collect the images around the robot; The lidar (3) is arranged above the robot body (5) and is used to sense the terrain around the robot; The robotic arm (4) is arranged on the robot body (5), and a robotic claw (6) is arranged on the robotic arm (4); A main control component is arranged inside the robot body (5). The main control component can transmit the images collected by the camera back to the terminal and is used to control the movement of the leg mechanism (1) and the robotic arm (4); The control method of the hexapod robot includes the following steps: S1. The lidar calculates the distance of the target object by emitting laser pulses and measuring the time for the pulses to reflect back from the target object to the sensor. The frequent emission of laser pulses can obtain multiple ranging points in space, thereby generating a set of data points with three-dimensional coordinates and generating a three-dimensional point cloud dataset; S2. Remove noise, perform registration, filtering, and simplification on the three-dimensional point cloud dataset; S3. Merge the point cloud data after multiple laser scans into a unified coordinate system. The merging process usually includes a registration step to align multiple datasets to obtain a complete three-dimensional scene; S4. Extract the features of the terrain surface, obstacles, or buildings from the point cloud data and generate a topographic map; S5. The main control component controls the movement of the leg mechanism of the robot to make it walk according to the modeled terrain, and uses multi-joint control to ensure the coordinated movement of the robot's legs, thereby effectively adapting to the terrain; S6. When encountering static obstacles, the robot uses global path planning to calculate an optimal path from the starting point to the ending point based on the map and the target position, and adopts local obstacle avoidance during the movement to ensure avoiding moving obstacles; S7. After reaching near the target, the main control component controls the robotic arm of the robot to perform grasping, and uses path planning and obstacle avoidance algorithms to adjust the angles of the joints of the robotic arm to make the robotic arm avoid obstacles.

2. The collaborative control method of a hexapod robot and a bionic robotic arm according to claim 1, characterized in that: The robotic claw (6) includes a servo motor (601), a large arm connecting rod (602), a small arm connecting rod (603), a clamping connecting rod (604), a robotic claw support (606), a fixed platform (607), and a ball screw (608). The servo motor (601) is installed on the fixed platform (607), and the fixed platform (607) is connected to the robotic claw support (606) by screws; a grasping mechanism formed by the large arm connecting rod (602), the small arm connecting rod (603), and the clamping connecting rod (604) is arranged on the upper and lower sides of the robotic claw support (606); the ball screw (608) converts the rotational motion of the servo motor (601) into a linear motion to drive the grasping mechanism to move.

3. The collaborative control method of a hexapod robot and a bionic robotic arm according to claim 1, characterized in that: The robotic arm (4) includes a shoulder joint member (403), a large arm member (402), and a small arm member (401). The shoulder joint (403) and the robot body (5) are rotated by a fourth servo (407). One end of the large arm member (402) is fixedly connected to the shoulder joint (403). The other end of the large arm member (402) is connected to one end of the small arm member (401) by a third servo (404). The other end of the small arm member (401) controls the swing and rotation of the robotic claw (1) through a second servo (406) and a first servo (405) that are perpendicularly arranged.

4. A collaborative control method for a hexapod robot and a bionic robotic arm according to claim 1, characterized in that: The leg mechanism (1) includes a front leg member (106), a rear leg member (102), and a connecting joint (101). The front leg member (106) and the rear leg member (102) are swinged in a two-stage series by a second leg servo (103) and a third leg servo (105). The rotation between the connecting joint (101) and the robot body (5) is driven by a first leg servo (104).

5. The collaborative control method of a hexapod robot and a bionic robotic arm according to claim 1, characterized in that: In S2, the noise is removed by using the statistical outlier removal method. Specifically: For each point p i and the set of points N(p i ) in its neighborhood, calculate the average distance μ i and the standard deviation σ i : Among them, ||p i -p j || is the Euclidean distance between point p i and point p j ; If the point p i has an average distance μ i that exceeds a certain threshold μ thresh plus a multiple kσ of the standard deviation i , then this point is considered an outlier: μ i = μ thresh + kσ i Otherwise, the point is retained.

6. The collaborative control method of a hexapod robot and a bionic robotic arm according to claim 1, characterized in that, The specific steps of S3 are as follows: Minimize the distance between the source point cloud P s and the target point cloud P t by an iterative method to align the two; in each iteration, ICP calculates the nearest neighbor of each point and optimizes the transformation matrix by the least squares method to minimize the distance between the point clouds; Calculate the distance between each pair of points p i and q i : d i = || p i - q i || Minimize the distance between the source point cloud and the target point cloud: where E(R,t) represents the distance error after the source point cloud is rotated and translated; R is the rotation matrix; t is the translation vector; Optimize R and t by the least squares method to obtain the optimal transformation matrix, so that the distance between the point clouds is minimized.

7. A collaborative control method for a hexapod robot and a bionic robotic arm according to claim 1, characterized in that, The specific steps of S4 are as follows: Randomly select 3 points {p1, p2, p3}, and calculate the plane parameters determined by these three points. Assume the equation of the plane is: ax + by + cz + d = 0 where (a, b, c) is the normal vector of the plane, and d is the plane offset; Calculate the distance ρ between this plane and other point p i i :​ If it is less than ρ i a certain preset threshold ε, then it is considered that the point p i is an interior point of the plane; For each round of random sampling, calculate the number N of inliers, and select the model with the most inliers as the final plane. Suppose the elevation values of the given points p1, p2, … p n are z(p1), z(p2), … z(p n ), then the elevation value of the new point p0 is z(p0); Generate a topographic map using the Kriging interpolation method. The formula is: Among them, δ i is the weight to be solved, and the constraint condition is: Kriging weights δ i Solving the following system of equations: where, γ(p i , p j ) is the autocovariance function between the points p i and p j .

8. The collaborative control method of a hexapod robot and a bionic robotic arm according to claim 1, characterized in that: The global path planning uses the A* algorithm. The cost function f(n) of the A* algorithm consists of two parts: f(n) = g(n) + h(n) where f(n) is the total cost function of node n; g(n) is the actual cost from the starting point to the current node n; h(n) is the heuristic function, representing the estimated cost from the current node n to the target node. Use the Euclidean distance: where (x n , y n ) are the coordinates of node n, and (x t , y t ) are the coordinates of target node t; The local obstacle avoidance uses the artificial potential field method: Assume that the coordinates of the target point are (x t , y t ), and the current position of the robot is (x, y). Then the attractive force can be expressed as F a as follows: F a = -k a ·(x - x t , y - y t ) where k a is the coefficient of attraction; (x - x t , y - y t ) is the vector between the current position of the robot and the target position, pointing to the target point; Suppose the coordinates of the $i$-th obstacle are $(x 0i , y 0i )$, and the current position of the robot is $(x, y)$. Then the repulsive force generated by the obstacle can be expressed as: where d i is the distance from the robot to the i-th obstacle; k r is the coefficient of the repulsive force; r0 is a threshold value, indicating that the repulsive force will only be generated when the distance to the obstacle is less than r0.

9. The collaborative control method of a hexapod robot and a bionic robotic arm according to claim 1, characterized in that: In S7, the method for the robotic arm to avoid obstacles is specifically: Define the minimum safe distance d between the robotic gripper and the obstacle safe If the distance d between the obstacle and the robotic gripper obs is less than d safe then the path needs to be adjusted; When avoiding obstacles, the method of using a virtual force field is adopted to adjust the movement of the robotic arm. The formula is: F total = F goal + F obs Among them, F goal is the force generated at the target position; F obs is the reverse force generated by the obstacle; Target force F goal which is intended to guide the robotic gripper to the target position P goal , and its expression is: F goal = k goal ·(P goal - P current ) where k goal is a constant of the target attraction force, and P current is the current position of the robotic gripper; Obstacle force F obs Calculated based on the distance between the obstacle and the robotic arm. Assuming the position of the obstacle is P obs , the reaction force generated by the obstacle can be expressed as: where k obs is the constant of the obstacle repulsive force, ‖P current - P obs ‖ 3 is the distance from the robotic gripper to the obstacle; Use the Jacobian matrix to transform the force from the task space to the joint space. The formula is: τ = J T ·F total where τ = [τ1, τ2, … τ6] is the torque of each joint of the robotic arm; J is the Jacobian matrix of the robotic arm, which describes the relationship between the velocity of the robotic gripper and the joint angular velocity; F total is the total force in the task space.

Images