Autonomous obstacle avoidance method and system for a probe-in manipulator based on three-dimensional force information

Abstract

The application discloses a kind of based on three-dimensional force information's probe in operation mechanical arm autonomous obstacle avoidance method and system, at least including data acquisition unit, motion execution unit and control unit, data acquisition unit is used to collect the three-dimensional force data of mechanism end when soil exploration sampling and send to control unit;Motion execution unit is used to receive instruction from control unit and the position information of each joint of mechanical arm is sent to control unit;Control unit is first used to receive data from data acquisition unit and is transformed into three-dimensional force information and receives pose data information from motion execution unit, and then according to force situation judgment control mode, finally motion control instruction is sent to motion execution unit and control is completed.The application effectively reduces the stress of end mechanism operation by combining end stress information into the motion control of probe operation and carrying out real-time strategy adjustment, protects mechanism from being damaged and improves operation success rate.

Description

Technical Field

[0001] This invention belongs to the fields of information technology and geological and geotechnical engineering exploration technology, and in particular relates to an autonomous obstacle avoidance method and system for a robotic arm for exploration operations based on three-dimensional force information. Background Technology

[0002] Extraterrestrial regolith sampling missions are of great significance for studying the development of the universe and the origin of life. By carrying a robotic arm with a sampling device, planetary rovers can collect samples and perform in-situ analysis of the regolith on celestial surfaces. The Curiosity rover from the Mars Science Laboratory, during its mission to explore Peak Sharp, conducted continuous in-situ sampling and analysis of the Martian regolith. Shallow drilling, as one of the mainstream extraterrestrial sampling methods, can effectively preserve soil stratification information and has wide applications. Typical drilling and sampling missions include the US Apollo series missions, the Soviet Lunar missions, and China's Chang'e 5 lunar exploration mission. Furthermore, communication delays make ground-based remote control difficult to implement on more distant celestial bodies such as Mars, making automated sampling processes particularly important.

[0003] A major challenge in extraterrestrial automated sampling missions lies in the unknown nature of the environment. Research has revealed that shallow layers of rock and rock fragments may exist on the Moon and Mars. Therefore, sampling devices are likely to encounter hard objects during extraterrestrial operations, generating significant contact forces that could damage the sampler or lead to mission failure. Furthermore, research on strategy adjustments based on the interaction between the end-effector and the environmental medium is limited in extraterrestrial sampling missions. Existing methods include rock contact recognition based on vibration signals and real-time adjustment of drilling strategies by identifying medium properties, but none of these methods can avoid obstacles through attitude adjustments. Summary of the Invention

[0004] To overcome the shortcomings of the existing technology, the present invention aims to provide an autonomous obstacle avoidance method and system for a probing robotic arm based on three-dimensional force information. This solves the problems of existing technologies, such as the inability to adjust the working posture in real time according to the environment and susceptibility to obstacle interference. It further improves the environmental adaptability of the sampling system, improves the stress condition of the end sampler during operation, reduces mechanical wear, and increases the success rate of sampling operations.

[0005] To achieve the above and other objectives, this invention proposes an autonomous obstacle avoidance system for a probing robotic arm based on three-dimensional force information, comprising at least a data acquisition unit, a motion execution unit, and a control unit.

[0006] The data acquisition unit is used to collect three-dimensional force data of the sampler at the end of the mechanism during soil exploration and sampling, and to amplify and convert the electrical signals generated by the force into analog and digital signals before sending them to the control unit.

[0007] The motion execution unit: The motion execution unit is used to receive instructions from the control unit and send the position information of each joint of the robotic arm to the control unit to realize two-degree-of-freedom rotation and single-degree-of-freedom translational motion;

[0008] The control unit first receives data from the data acquisition unit and converts it into three-dimensional force information, and then receives pose data from the motion execution unit. Based on the force conditions, it determines the control method and adjusts the strategy in real time. It sets safe force thresholds and dangerous force thresholds. If the force is below the safe force threshold, operation continues; if the force is above the dangerous force threshold, operation stops; if the force is between the two, obstacle avoidance is achieved through admittance control and rotational avoidance. Finally, the motion control command is sent to the motion execution unit to complete the control.

[0009] Preferably, the data acquisition unit includes at least a multi-dimensional force sensor and a data acquisition card; in the force information acquisition function of the control unit, a sliding filter method is used to reduce data noise.

[0010] Preferably, the motion execution unit includes at least a three-degree-of-freedom robotic arm and a lower-level control system. The three-degree-of-freedom robotic arm structure includes rotary joints in the X and Y axes of the mechanism base coordinate system and translational joints in the Z axis. The functions of the lower-level control system include: communicating with each joint of the robotic arm via a CAN bus, reading the position of each joint in real time and sending it to the control unit, and receiving control commands from the control unit and parsing and sending them to each joint controller.

[0011] To achieve the above and other objectives, an autonomous obstacle avoidance method for a probing robotic arm based on three-dimensional force information is proposed, which specifically includes the following steps:

[0012] S1: Read data from the multi-dimensional force sensor and process the data to obtain the effective external force;

[0013] S2: Compare the effective external force with the threshold. If it exceeds the danger threshold, the operation is in a dangerous state and must be stopped immediately. If it exceeds the safety threshold but does not exceed the danger threshold, the operation is in an obstacle encounter state. After making an avoidance adjustment, the effective external force is compared with the threshold again. If it is below the safety threshold, the operation is in a safe state and can continue.

[0014] S3: Read the angle values ​​of each joint from the lower-level machine, obtain the current depth through forward kinematics calculation, and compare it with the target depth. If the target depth is reached, the probe operation is completed. If the target depth is not reached, translate the joint to continue probe, and repeat step S1 to read the multi-dimensional force sensor data.

[0015] Preferably, step S1 in the control process of the control unit further includes the following steps:

[0016] S11: Perform sliding filtering on the data from the multi-dimensional force sensor in the data acquisition unit to reduce signal noise and obtain the applied external force F. Including the force values ​​F in the X, Y, and Z directions of the end sampler x F y F z S12: Set the lateral force safety threshold F. The resultant force in the X and Y directions is defined as the lateral force, and the force in the Z direction is defined as the thrust resistance. side and the safety threshold of resistance in the exploration direction F main ;

[0017] S13: Compare the measured lateral force and directional resistance with their respective safety thresholds. If either exceeds the safety threshold, retain it; otherwise, reset it to zero. This yields the effective external force Fe.

[0018]

[0019] Preferably, in the control process of the control unit, the avoidance adjustment step in step S2 is achieved through admittance control and rotational avoidance, and further includes the following steps:

[0020] S21: Obstacle avoidance is achieved by utilizing the rotational response of the rotary joint along the X and Y axes of the tool coordinate system, with the rotational speed and effective external force F as parameters. e Proportional to the initial angle θ0, the rotational speed is 0 when the force is safe. The desired joint angle θ is obtained by integrating the rotational speed and adding it to the initial angle θ0. d ,in θ x0 θ y0 θ represents the initial angles of the X and Y axis rotary motors, respectively. xd θ yd Let X and Y represent the target angles of the rotary motor along the X and Y axes, respectively. The conversion relationship is as follows:

[0021]

[0022]

[0023] In the formula, Let be the first derivative of the joint rotation angle, and Represents the joint rotation speed, where Let X and Y be the rotational speeds of the rotary motors on the X and Y axes, respectively, which are proportional to the magnitude of the external force. Let A be the gain coefficient matrix of the rotational speed and the external force. a x a is the x-axis rotational gain coefficient. y The y-axis rotational gain coefficient;

[0024] S22: Desired joint angle θ dThe desired position information X is obtained through forward kinematics calculation. d Then the desired location information X d The data is fed into the admittance controller based on the effective external force F. e Adjustments were made to obtain the actual target position X. r ,in x d y d z d These represent the desired X, Y, and Z coordinates of the end sampler in the Cartesian coordinate system, respectively. r y r z r Let X, Y, and Z represent the actual target coordinates of the end sampler in the Cartesian coordinate system. The admittance controller calculation process is as follows:

[0025]

[0026] In the formula, M represents the mass coefficient matrix and m x m y m z The mass coefficients of the end sampler along the X, Y, and Z axes in Cartesian coordinates are given respectively, and B represents the damping coefficient matrix. b x b y b z The damping coefficients of the end sampler along the X, Y, and Z axes in Cartesian coordinates are given respectively, and K represents the stiffness coefficient matrix. k x k y k z The stiffness coefficients of the end sampler along the X, Y, and Z axes in Cartesian coordinates are respectively. Let be the second derivative of the actual target position, representing the actual target acceleration. Let be the second derivative at the desired position, representing the desired acceleration. The first derivative of the actual target position represents the actual target velocity. Let represent the first derivative of the desired position, and let represent the desired velocity.

[0027] Preferably, the multi-dimensional force sensor used in step S1 is a six-dimensional force / torque sensor with a force range of ±100N in each direction, a torque range of ±100NM, an error of no more than 3%FS, and a sampling frequency of 200Hz. In practice, only three-dimensional force signals are used.

[0028] Preferably, the control unit is implemented using a microcomputer NUC running the Ubuntu system, and the program runs in the ROS robot operation control system.

[0029] Preferably, the lower-level control system in the control unit is implemented using an STM32 microcontroller and interacts with the microcomputer NUC of the control unit via serial communication with a baud rate of 115200.

[0030] Compared with existing technologies, the autonomous obstacle avoidance method and system for exploration operations based on three-dimensional force information of this invention have the following advantages: By sensing the force on the end sampler through the force sensor mounted on the sampler and integrating the force information into the control strategy, the working posture is adjusted in real time according to the environmental conditions during operation. Combined with rotational avoidance and admittance control, effective obstacle avoidance is achieved during sampling drilling, and the operation is stopped in time in dangerous situations, which greatly improves the environmental adaptability of exploration operations and effectively improves the stress conditions of the mechanism. Attached Figure Description

[0031] Figure 1 This is a structural diagram of an autonomous obstacle avoidance method and system for a probing robotic arm based on three-dimensional force information, according to the present invention.

[0032] Figure 2 This is a structural diagram of the three-degree-of-freedom probing robotic arm used in this invention;

[0033] Figure 3 This is a flowchart of the operation of a probing robotic arm system based on three-dimensional force information according to the present invention;

[0034] Figure 4 This is a control system diagram of an autonomous obstacle avoidance method for a probing robotic arm based on three-dimensional force information according to the present invention; Detailed Implementation

[0035] The following describes the embodiments of the present invention through specific examples and in conjunction with the accompanying drawings. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific examples, and various details in this specification can also be modified and changed based on different viewpoints and applications without departing from the spirit of the present invention.

[0036] Figure 1 This is a structural diagram of an autonomous obstacle avoidance method and system for a probing robotic arm based on three-dimensional force information, as described in this invention, including:

[0037] The microcomputer NUC, as the execution hardware of the system control unit, is characterized by its small size, low power consumption, and strong computing power. It is widely used in industrial control, development and debugging tasks. Here, it communicates with the data acquisition card via serial port to receive multi-dimensional force signals from the data acquisition unit, and at the same time communicates with the STM32 lower-level machine via serial port to receive joint position signals from the motion execution unit and send control commands.

[0038] Specifically, the control unit uses the Ubuntu 20.04 operating system and executes programs in parallel within the noetic version of the ROS robot control operating system. It runs NUC-STM32 lower-level machine interaction nodes, motion control nodes, and NUC-data acquisition card sensing and monitoring nodes, which respectively perform basic motion control, motion operation control, and multi-dimensional force monitoring tasks. All nodes are written to the same launch file and are started uniformly via roslaunch.

[0039] The multidimensional force sensor, as the front-end component of the system's data acquisition unit, uses the principle of resistance strain to convert multidimensional force / torque signals into voltage signals for measurement. Here, it is used to monitor the X, Y, and Z three-dimensional force conditions of the end sampler in real time and transmit the voltage signals to the data acquisition card.

[0040] The data acquisition card, as the signal processing element of the system's data acquisition unit, uses a 16-bit high-precision ADC to acquire multiple voltage signals from the multi-dimensional force sensor at a frequency of 10MHz. It converts the analog voltage signals into digital signals and transmits them to the NUC of the control unit via serial communication.

[0041] The STM32 lower-level machine serves as the basic motion control element of the motion execution unit. It uses an STM32 microcontroller as the program processing chip and interacts with each robotic arm joint via CAN bus communication. It sends position control commands and reads the encoder values ​​from each joint in real time. The actual angle is obtained by dividing the encoder value by the joint reduction ratio. Simultaneously, the lower-level machine interacts with the NUC of the control unit via serial communication, sending the actual angle and receiving control commands from the NUC. It parses and forwards the control commands from the NUC. Specifically, the control commands sent by the NUC are fixed-length communication frame arrays, stored in hexadecimal target position data in the order from rotary joints to translational joints. Upon receiving the commands, the STM32 lower-level machine parses them into the corresponding three joints and transmits them via CAN bus communication. Each joint has a different ID number for identification.

[0042] The three-degree-of-freedom probing robotic arm is used to perform two-degree-of-freedom rotational motion and one-degree-of-freedom translational motion. It achieves posture adjustment and obstacle avoidance through rotational motion and probing operations through translational motion. Each joint has integrated drive and control, and receives control directly through CAN bus communication, interacting with the STM32 lower-level computer.

[0043] Figure 2 This is a structural diagram of the three-degree-of-freedom probing robotic arm used in this invention, including:

[0044] Rotary joint 1, using CAN bus communication control, provides rotational motion in the X direction, and uses a harmonic geared motor in conjunction with an absolute encoder to execute position control commands, with a torque of 20NM.

[0045] Rotary joint 2, using CAN bus communication control, provides rotational motion in the Y direction, and uses a harmonic geared motor in conjunction with an absolute encoder to execute position control commands, with a torque of 20NM.

[0046] Translation joint 3 is controlled via CAN bus communication, providing translational motion in the Z direction. It is achieved using a servo motor and a lead screw slider, and is coupled with an absolute encoder to execute position control commands. The maximum load is 50N.

[0047] Figure 3 This is a flowchart of the operation of a probing robotic arm system based on three-dimensional force information according to the present invention, including:

[0048] Step S1: First, start all task nodes in the NUC of the control unit using the ROS system roslaunch command to begin the sampling and exploration operation.

[0049] In step S2, during operation, the force information of the end sampler is captured by the multi-dimensional force sensor and generates a strain electrical signal. The multi-dimensional force signal is further sampled by the data acquisition card and transmitted to the NUC. The three-dimensional force information sampled by the data acquisition card is read by the NUC-data acquisition card sensing and monitoring node, and the effective external force value is obtained after data processing.

[0050] In step S3, the motion control node uses branch logic. The effective external force is further compared with the set threshold. If it exceeds the danger threshold, it is in a dangerous state and enters the stop operation branch, and the mechanism retracts. If it exceeds the safety threshold but does not exceed the danger threshold, it is in an obstacle encounter state, and the mechanism adjusts through admittance control and rotation avoidance. If it is below the safety threshold, it is in a safe state, the translation joint probes, and the mechanism continues to operate.

[0051] Specifically, the operation strategy in step S3 above is controlled by the motion control node publishing control topics in real time. The NUC-STM32 lower-level machine interaction node subscribes to the control topics and sends instructions to the STM32 lower-level machine in real time to complete the control.

[0052] In step S4, the NUC-STM32 lower-level computer interaction node reads the joint angle values ​​from the lower-level computer and publishes the robotic arm posture topic. The motion control node subscribes to the topic and obtains the current depth through forward kinematics calculation, and compares it with the target depth. If the target depth is reached, the probing operation is completed. If the target depth is not reached, the joint is translated to continue probing, and step S1 is repeated to read the multi-dimensional force sensor data.

[0053] As a further explanation of step S2, in the NUC-data acquisition card sensing and monitoring node, the steps for obtaining effective external force through data processing include:

[0054] S21: Perform sliding filtering on the data from the multi-dimensional force sensor in the data acquisition unit to reduce signal noise and obtain the applied external force F. Including the force values ​​F in the X, Y, and Z directions of the end sampler x F y F z And let the resultant force in the X and Y directions be the lateral force, and the force in the Z direction be the resistance in the probing direction;

[0055] S22: Set the lateral force safety threshold F side and the safety threshold F of the pressure in the exploration direction z ;

[0056] S23: If the lateral net force exceeds the threshold, it is retained; if it is below the threshold, it is reset to zero, thus obtaining the effective external force F. e Let the forces measured in the lateral X and Y directions be F. x F y :

[0057]

[0058] Figure 4 This is a control system diagram of an autonomous obstacle avoidance method for a probing robotic arm based on three-dimensional force information according to the present invention. It further explains the obstacle avoidance control in step S3, which is implemented in the motion control node and includes the following steps:

[0059] Step S31:

[0060] Obstacle avoidance is achieved by utilizing the rotational response of the rotary joint along the X and Y axes of the coordinate system, where the rotational speed is related to the effective external force F. e Proportional to the initial angle θ0, the rotational speed is 0 when the force is safe. The desired joint angle θ is obtained by integrating the rotational speed and adding it to the initial angle θ0. d ,in θ x0 θ y0 θ represents the initial angles of the X and Y axis rotary motors, respectively. xd θ yd Let X and Y represent the target angles of the rotary motor along the X and Y axes, respectively. The conversion relationship is as follows:

[0061]

[0062]

[0063] In the formula, Let be the first derivative of the joint rotation angle, and Represents the joint rotation speed, where Let X and Y be the rotational speeds of the rotary motors on the X and Y axes, respectively, which are proportional to the magnitude of the external force. Let A be the gain coefficient matrix of the rotational speed and the external force. a x a is the x-axis rotational gain coefficient. y The y-axis rotational gain coefficient;

[0064] Step S32: Desired joint angle θ d The desired position information X is obtained through forward kinematics calculation. d Then the desired location information X d The data is fed into the admittance controller based on the effective external force F. e Adjustments were made to obtain the actual target position X. r ,in x d y d z d These represent the desired X, Y, and Z coordinates of the end sampler in the Cartesian coordinate system, respectively. r y r z r Let X, Y, and Z represent the actual target coordinates of the end sampler in the Cartesian coordinate system. The admittance controller calculation process is as follows:

[0065]

[0066] In the formula, M represents the mass coefficient matrix and m x m y m z The mass coefficients of the end sampler along the X, Y, and Z axes in Cartesian coordinates are given respectively, and B represents the damping coefficient matrix. b x b y b z The damping coefficients of the end sampler along the X, Y, and Z axes in Cartesian coordinates are given respectively, and K represents the stiffness coefficient matrix. k x k y k z The stiffness coefficients of the end sampler along the X, Y, and Z axes in Cartesian coordinates are respectively. Let be the second derivative of the actual target position, representing the actual target acceleration. Let be the second derivative at the desired position, representing the desired acceleration. The first derivative of the actual target position represents the actual target velocity. Let represent the first derivative of the desired position, and let represent the desired velocity.

[0067] Specifically, admittance control indirectly achieves force compliance by establishing the relationship between the mechanism's position and motion commands and the external forces applied. In actual implementation, the process is discretized and executed in a distributed manner. Taking a single direction as an example, the discrete compliance execution process is as follows:

[0068]

[0069]

[0070]

[0071] In the formula, T is the operating cycle, x((n+1)T) represents the current actual position, and x(nT) represents the actual position in the previous cycle. These are the actual speeds for the current and previous cycles, respectively. These are the actual accelerations for the current and previous cycles, respectively, x. d ((n+1)T) represents the current actual position, x d (nT) represents the actual position in the previous cycle. These are the actual speeds for the current and previous cycles, respectively. Let be the actual accelerations in the current and previous cycles, respectively, and let m, b, and k be the mass, damping, and stiffness coefficients, respectively.

[0072] This example conducted multiple comparative experiments under different obstacle environments, mainly testing the difference in force conditions under autonomous obstacle avoidance control and original control. The probe sample was simulated lunar soil. The statistical results showed that during the sampling operation with autonomous obstacle avoidance control applied, the resistance in all directions of the end mechanism was significantly reduced, with the resistance in the X, Y, and Z directions decreasing by an average of 46.7%, 57.0%, and 64.9%, respectively.

[0073] As can be seen, the autonomous obstacle avoidance method and system for a probing robotic arm based on three-dimensional force information of the present invention, by sensing the three-dimensional force conditions in X, Y, and Z by force sensors mounted on the end sampler, and combining the rotational avoidance and admittance control of the three-degree-of-freedom probing robotic arm, effectively avoids obstacles during the sampling drilling process, effectively improves the force conditions at the end, and provides an important reference for solving the problem of interference from potential obstacles in extraterrestrial sampling operations, ensuring the safety of the mechanism and realizing the automated probing operation process.

[0074] The above embodiments are merely illustrative of the principles and effects of the present invention and are not intended to limit the invention. Any person skilled in the art can make modifications and changes to the above embodiments without departing from the spirit and scope of the invention. Therefore, the scope of protection of the present invention should be as set forth in the claims.

Claims

1. An autonomous obstacle avoidance system for a robotic arm of an exploration machine based on three-dimensional force information, characterized in that: The system comprises at least a data acquisition unit, a motion execution unit and a control unit, The data acquisition unit is used to collect the three-dimensional force data of the mechanism end sampler during soil exploration sampling, and after the electrical signals generated by the force are amplified and converted into analog signals and digital signals, the signals are sent to the control unit; The motion execution unit is used to receive instructions from the control unit and send the position information of each joint of the mechanical arm to the control unit, so as to realize two-degree-of-freedom rotation and single-degree-of-freedom translation motion. The control unit is used to receive data from the data acquisition unit and convert the data into three-dimensional force information, receive pose data information from the motion execution unit, judge the control mode according to the force condition, adjust the strategy in real time, set a safe force threshold and a dangerous force threshold, continue the work if the force is lower than the safe force threshold, stop the work if the force is higher than the dangerous force threshold, and realize obstacle avoidance motion through admittance control and rotational avoidance if the force is between the two thresholds, and then send the motion control instructions to the motion execution unit to complete the control. The method comprises the following steps: S1: reading the data from the multi-dimensional force sensor in the data acquisition unit and processing the data to obtain effective external force; S11: sliding filter processing is performed on the data from the multi-dimensional force sensor in the data acquisition unit to reduce signal noise to obtain the external force F, and the external force , including the force values F of the end sampler X, Y, Z in three directions x、 F y , F z， and let the combined force of X and Y directions be the lateral force, and the force in Z direction be the probe direction resistance; S12: Set lateral force safety threshold F side and probe-in direction resistance safety threshold F main ; S13: compare the measured lateral force and the probe-in direction resistance with respective safety thresholds, and if either is above the safety threshold, retain, otherwise zero, thereby obtaining the effective external force F e : ； S2: comparing the effective external force obtained in step S1 with the set threshold value, if the threshold value is exceeded, the work is immediately stopped, if the safe threshold value is exceeded but the dangerous threshold value is not exceeded, the work is adjusted to avoid obstacles, and then the effective external force is compared with the threshold value, if the safe threshold value is lower than the effective external force, the work is continued; The avoidance adjustment is realized through admittance control and rotational avoidance, and at least comprises the following steps: S21: obstacle avoidance is performed by rotating the joint in response to the force on the X, Y axes of the tool coordinate system, and the rotation speed is proportional to the effective force F e is proportional to the effective force F, and the rotation speed is 0 when the force is safe, and the rotation speed is integrated and superimposed on the initial angle θ0 to obtain the desired joint angle θ d , , , , respectively represent the initial angles of the X and Y axis rotation motors, , respectively represent the target angles of the X and Y axis rotation motors, and the conversion relationship is as follows: ； In the formula, is the first order differential of joint rotation angle, and represents the joint rotation speed, wherein , are the rotation speeds of the X and Y axis rotation motors, respectively, and are directly proportional to the external force, A is a gain coefficient matrix of the rotation speed and the external force, and , a x is the x-axis rotation gain coefficient, a y is the y-axis rotation gain coefficient; S22: Desired joint angle θ d The desired position information X is obtained through forward kinematics calculation. d Then the desired location information X d The data is fed into the admittance controller based on the effective external force F. e Adjustments were made to obtain the actual target position X. r ,in , , , , These represent the expected X, Y, and Z coordinates of the end sampler in the Cartesian coordinate system. , , Let X, Y, and Z represent the actual target coordinates of the end sampler in the Cartesian coordinate system. The admittance controller calculation process is as follows: ； In the formula, denotes the mass coefficient matrix and , , , respectively X, Y, Z axis mass coefficient of the end sampler in the Cartesian coordinate system, denotes the damping coefficient matrix and , , , respectively X, Y, Z axis damping coefficient of the end sampler in the Cartesian coordinate system, denotes the stiffness coefficient matrix and , , , respectively X, Y, Z axis stiffness coefficient of the end sampler in the Cartesian coordinate system, is the second derivative of the actual target position, indicating the actual target acceleration, is the second derivative of the desired position, indicating the desired acceleration, is the first derivative of the actual target position, indicating the actual target speed, denotes the first derivative of the desired position, indicating the desired speed; S3: reading the joint angle values from the lower computer, obtaining the current depth through forward kinematics calculation, comparing the current depth with the target depth, if the target depth is reached, the exploration work is completed, if the target depth is not reached, the joint is translated to continue the exploration, and the step S1 is repeated to read the multi-dimensional force sensor data. 2.The three-dimensional force information based autonomous obstacle avoidance system for a probe operation manipulator arm, as claimed in claim 1, wherein: The data acquisition unit comprises at least a multi-dimensional force sensor and a data acquisition card. 3.The three-dimensional force information based autonomous obstacle avoidance system for a manipulator of a penetration work machine according to claim 1 or 2, characterized in that: The motion execution unit comprises at least a three-degree-of-freedom mechanical arm and a lower computer control system; the three-degree-of-freedom mechanical arm comprises a mechanism base coordinate system X, a rotation joint in the Y-axis direction and a translation joint in the Z-axis direction; the lower computer control system is used to communicate with each joint of the mechanical arm through CAN bus, read the position of each joint in real time and send the position to the control unit, receive the control instructions from the control unit and analyze and send the instructions to each joint controller. 4.The three-dimensional force information based autonomous obstacle avoidance system for a probe operation manipulator according to claim 1, wherein: The control unit is realized by a microcomputer NUC loaded with Ubuntu system, and the program runs in the ROS robot operation control system. 5.The three-dimensional force information based autonomous obstacle avoidance system for a probe operation manipulator according to claim 3, wherein: The lower computer control system is realized by an STM32 single-chip microcomputer, which interacts with the microcomputer NUC of the control unit through serial communication, and the baud rate is 115200. 6.The three-dimensional force information based autonomous obstacle avoidance system for a probe operation manipulator according to claim 1, wherein: The multi-dimensional force sensor used in step S1 is a six-dimensional force / torque sensor, each force range is ±100N, torque range is ±100NM, error is not higher than 3%F.S., and sampling frequency is 200Hz.

Images