A distributed TOF ranging photosensitive system for snake-like robotic arms

Abstract

The application discloses a distributed TOF ranging photosensitive system of a snake-shaped mechanical arm, a plurality of TOF sensors are distributedly installed on the snake-shaped mechanical arm, wherein a TOF module installed at the tail end of the snake-shaped mechanical arm has photosensitive function; the TOF sensors are connected through a CAN bus and transmit measurement data to corresponding detection processing modules; the detection processing modules process received azimuth information and distance information, obtain surrounding obstacle information of the snake-shaped mechanical arm, and judge the surrounding obstacle information; the detection processing modules transmit the judgment results to a superior control unit, and the snake-shaped mechanical arm completes an obstacle avoidance task under the control of the superior control unit. The application uses a perfect technical approach, and realizes the related functions of autonomous ranging and reasonable obstacle avoidance of the snake-shaped mechanical arm in the movement process through a mature test method, has strong engineering practical significance and good developability, and can be also applicable to flexible mechanical arms, snake-shaped robots and the like in the future.

Description

Technical Field

[0001] The present invention belongs to the technical field of snake-like robot detection and control, and in particular relates to a distributed TOF ranging photosensitive system for a snake-like robot arm. Background Art

[0002] With the continuous advancement of industrialization and intelligentization, the gradual replacement of human workers by robots in industrial operations has become an inevitable trend. The widespread use of robotic arms is driving the continuous deepening of robotic arm research, with advancements towards automation, high speed, and precision. Technical research in the structural design and posture control of traditional rigid robotic arms is relatively mature. However, due to the rigid structural limitations of rigid robots, operations in unstructured, complex, and confined environments remain challenging. Snake-shaped robotic arms, with their superior flexibility and high degrees of freedom, offer unparalleled advantages over traditional rigid robotic arms in confined and complex environments, thus becoming a new direction in robotic arm research. Furthermore, the research and development of these serpentine-shaped robotic arms offers a safer, more reliable, and more intelligent option for industries such as debris rescue, industrial manufacturing, medical testing, and aerospace.

[0003] However, research on snake-like manipulators is still exploratory. Most domestic research on snake-like manipulators is still in the prototype experiment stage, still verifying the design of the snake-like manipulator structure and conducting preliminary control. Even fewer studies have been conducted on the autonomous ranging and obstacle avoidance capabilities of snake-like manipulators. Therefore, it is necessary to explore the autonomous ranging and reasonable obstacle avoidance capabilities of snake-like manipulators. Summary of the Invention

[0004] The purpose of this invention is to solve the problem that the current research on serpentine robotic arms is still under exploration, most domestic serpentine robotic arm research is in the prototype experiment stage, and the design of the serpentine robotic arm structure is still being verified, and the existing serpentine robotic arms have weak autonomous ranging and obstacle avoidance functions.

[0005] In order to achieve the purpose of the present invention, the present invention discloses a distributed TOF ranging and photosensing system of a serpentine robotic arm, comprising a serpentine robotic arm, a TOF sensor, and a detection and processing module; the TOF sensor is used to measure the orientation and distance information of obstacles, and several TOF sensors are distributedly installed on the serpentine robotic arm, wherein the TOF module installed at the end of the serpentine robotic arm has a photosensing function, can provide measurement information of ambient light illumination, and feed back relevant information to the detection and processing module; the TOF sensor is connected via a CAN bus and transmits measurement data to the corresponding detection and processing module; the detection and processing module processes the received orientation information and distance information, obtains the surrounding obstacle information of the serpentine robotic arm, and judges the surrounding obstacle information; the detection and processing module transmits the judgment result to the upper control unit, and the serpentine robotic arm completes the obstacle avoidance task under the control of the upper control unit.

[0006] Furthermore, a group of TOF sensors are distributed on the wall of each conical section of the serpentine robotic arm, with vertical downward as 0° and clockwise arranged at 0°, 90°, 180°, and 270°, with a total of four arranged on each conical section of the wall; each group of sensors is electrically connected to a corresponding detection and processing module, and each detection and processing module is connected to the upper-level control unit through a CAN bus.

[0007] Furthermore, the process of the TOF sensor collecting the position and distance information of the obstacle is as follows: the detection processing module uses the set function of the GPIO push-pull output to send a TTL level trigger signal of 10us to the four TOF sensors in turn. Then the signal transmitter inside the TOF sensor emits a modulated invisible light. If the invisible light is blocked by an obstacle during propagation, it will generate a reflected wave. The receiver inside the TOF sensor will receive the reflected wave and calculate the time difference between the emission and reception through the level holding time. According to the known speed of light, the distance between the serpentine robot arm and the obstacle is obtained; through the measurement values ​​of the distance to the same obstacle by different TOF sensors in each group of TOF sensors, the position information of the spatial obstacle can be obtained after comparison; the measurement data is uploaded through the CAN bus, coupled analysis is performed in the coordinate system, and the next motion planning instruction is made.

[0008] Furthermore, the detection and processing module processes the received orientation information and distance information by first performing error correction and compensation on the orientation information and distance information measured by the TOF sensor; then the corrected data is compared with the preset threshold of the minimum distance between the snake-like robotic arm and the obstacle. When an obstacle signal less than the minimum distance threshold appears, an instruction is output to the superior control unit to avoid and alarm.

[0009] Furthermore, due to hardware and site limitations, the TOF sensor will produce systematic errors and scene errors when measuring orientation and distance information. For systematic errors, the detection and processing module corrects them by establishing a mathematical compensation model. For scene errors, the detection and processing module compensates for data offsets by constructing a depth offset lookup table.

[0010] Furthermore, the specific process of establishing a mathematical compensation model is as follows: using the interpolation table lookup method to compensate for nonlinear errors, evenly selecting multiple positions in the wavelength range of the corresponding frequency, recording the depth measurement value corresponding to the actual distance of the position, and then performing linear interpolation to finally obtain a correspondence table between the measured phase and the phase corresponding to the actual distance; when the specific measurement results are obtained, this table is used to obtain the actual phase after calibration through the table lookup method, thereby completing the nonlinear error compensation.

[0011] Further, the specific process of constructing the depth offset lookup table is as follows: a plane is set in front of the TOF sensor, and the distance from the optical center to the plane is tested at intervals of 10 mm within a distance of 10-1000 mm from the sensor to the plane, and the average value is obtained by sampling multiple times; in order to reduce the influence of chip temperature change on the depth information, the sensor is preheated in the working state before the experiment, and the chip is kept at 60°C to obtain the corresponding offset value lookup table.

[0012] Further, the signal amplitude difference between the internal transmitter and receiver of the TOF sensor can indirectly estimate the accuracy of the measurement result. Assuming that the phase of the sine wave signal s(t) starting to be transmitted is The amplitude is A, and the reflected signal r(t) is received after a time delay Δt, at which time the phase of the signal is The amplitude of the reflected signal is B; four times are taken, and the sampling time interval is equal, which is T / 4. The phase delay Δφ of the two signals can be obtained by calculating the phase of the two signals, and the depth distance of the target can be calculated:

[0013] s(t) = A · (1 + sin(2πft))

[0014]

[0015]

[0016]

[0017]

[0018]

[0019] wherein λ is the modulation wavelength, T is the modulation light period, c is the speed of light, f is the frequency of the emitted light, t is the propagation time, Δt is the time difference, is the phase difference.

[0020] Further, the serpentine mechanical arm is composed of a cylindrical arm body taper joint and two vertebral joint end racks, each cylindrical arm body taper joint is fixed on a flexible center rod, the principle of the wire-driven serpentine mechanical arm is that: the wire-driven serpentine mechanical arm is controlled by the upper computer software or uses the remote control motor drive box to control the screw motor to move, so that the screw motor slider slides forward and backward to stretch and release the mechanical arm wire-driven rope group, the three-wire-driven rope group of the mechanical arm is used to drive six cylindrical arm body taper joints at the end to pull the six cylindrical arm body taper joints, so that the center rod is deformed to realize the flexible characteristics of the serpentine mechanical arm; a special support is arranged at the lower end of each cylindrical arm body taper joint, and the detection processing module PCB is installed on the special support of the vertebral joint lower end rack and fixed by screws; the detection processing module is electrically connected with a TOF sensor group, a CAN data line and a power line pass through the inner hole of the cylindrical arm body taper joint to connect the detection processing module, and the detection processing module is used to perceive the obstacle information around the corresponding arm segment during the movement of the serpentine mechanical arm; a photosensitive TOF sensor and a camera are installed on the end segment of the serpentine mechanical arm as a visual sensor to obtain the visual information of the end of the serpentine mechanical arm, the photosensitive TOF sensor can scan the pixel information captured by the camera in a single point or a surface array, and can also obtain the surface structure information of the whole scene in real time through the scene picture taken by the camera; the depth information of the measured object is generated through the mutual correlation between the TOF sensor group, the invisible light emitting module of the TOF sensor and the photosensitive receiving module of the TOF sensor.

[0021] Further, a horizontal plug-in row of TOF sensors is arranged on the outer wall of each cylindrical arm body taper joint at an interval of 90 degrees, so as to ensure that the measurement direction of the TOF sensor is in the plane perpendicular to the center rod of the serpentine mechanical arm; a detection processing module mounting disc is arranged at the center of the inner wall of the cylindrical arm body taper joint; a SWD plug-in row is arranged on the outer wall of the cylindrical arm body taper joint, which is convenient for burning programs and testing; a CAN1 transceiver interface and a power supply serial interface are arranged on the inner wall of the cylindrical arm body taper joint, which is convenient for the CAN data line and the power supply to pass through the internal wiring of the serpentine mechanical arm.

[0022] Compared with the prior art, the significant progress of the present application lies in: 1) a certain exploration is made on the serpentine mechanical arm body ranging and obstacle avoidance function implementation, which has a certain foresight; 2) it is almost suitable for most serpentine mechanical arms with multiple taper joints, which has good adaptability and strong universality; 3) the detection processing module is independently designed and uses CAN communication, which has strong expansibility; 4) the distance measurement of the obstacle is relatively accurate, and a better ranging and obstacle avoidance solution is provided.

[0023] In order to more clearly illustrate the functional characteristics and structural parameters of the present application, the following further illustrates the present application in combination with the drawings and specific embodiments. BRIEF DESCRIPTION OF DRAWINGS

[0024] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of this application. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0025] Figure 1 Schematic diagram of assembling an expandable detection and processing module for a snake-like robotic arm;

[0026] Figure 2 This is a diagram showing the assembly of a snake-like robotic arm based on four TOF ranging modules;

[0027] Figure 3 Schematic diagram of the sinusoidal emission and sampling waveforms of the TOF sensor;

[0028] Figure 4 Schematic diagram of light wave pulse modulation ranging of TOF sensor;

[0029] Figure 5 This is a schematic diagram of the radial perception viewing angle range of the TOF sensor module;

[0030] Figure 6 It is the line graph of the nonlinear calibration of the TOF module;

[0031] Figure 7 Lookup table for temperature change compensation of TOF module;

[0032] The accompanying drawings are marked as follows: motor drive box 1, robotic arm line drive rope group 2, cylindrical arm body cone section 3, TOF sensor group 4, obstacle 5, photosensitive TOF sensor 6, TOF sensor 12, cylindrical arm body cone section inner wall 13, cylindrical arm body cone section outer wall 14, detection and processing module mounting plate 15. DETAILED DESCRIPTION

[0033] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments; based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0034] Example

[0035] like Figure 1As shown, the snake-shaped mechanical arm is a six-segment linear drive snake-shaped mechanical arm, each segment is composed of a cylindrical arm body taper joint 3 and two vertebral joint end racks, each cylindrical arm body taper joint 3 is fixed on a flexible center rod, the principle of the linear drive snake-shaped mechanical arm is that the motor drive box 1 is controlled by the upper computer software or uses remote control to drive the screw motor to move, so that the upper slide block of the screw motor slides forward and backward to stretch and release the driving rope, three groups of driving rope cables pass through the corresponding snake-shaped mechanical arm segment from the lower end of the cylindrical arm body taper joint 3, and six groups of driving rope cables pull the ends of the six cylindrical arm body taper joints 3, so that the center rod deforms correspondingly to realize the flexible characteristics of the snake-shaped mechanical arm. Each cylindrical arm body taper joint 3 is provided with a special support at the lower end, and the detection processing module PCB board is a rectangular plate with a length of 2.5 cm and a width of 1.5 cm, and corresponding vias are designed for the mechanical arm linear drive rope group 2. The PCB board is installed on the special support of the vertebral joint lower end rack according to the structure and fixed with screws, and the detection processing module is electrically connected with the TOF sensor group 4. The CAN data line and the power line pass through the inner hole of the cylindrical arm body taper joint 3 to connect the detection processing module. The detection processing module is used to perceive the obstacle 5 information around the corresponding arm segment during the movement of the snake-shaped mechanical arm. A photosensitive TOF sensor 6 and a camera are installed on the end segment of the snake-shaped mechanical arm as a visual sensor to obtain the visual information of the end of the snake-shaped mechanical arm. The photosensitive TOF sensor 6 can scan the pixel information captured by the camera in a single point or a surface array, and can also obtain the surface structure information of the entire scene in real time through the scene picture taken by the camera; the depth information of the measured object is generated through the mutual correlation between the invisible light emitting module of the TOF sensor group 4 and the photosensitive receiving module of the TOF sensor 6.

[0036] As shown in Figure 2 Each cylindrical arm body taper joint is provided with a TOF sensor 12 horizontal plug-in row arranged every 90° on the outer wall 14 of each cylindrical arm body taper joint, forming a "four-sided exquisite" distribution form, ensuring that the measurement direction of the TOF sensor 12 is in the plane perpendicular to the center rod of the snake-shaped mechanical arm. The detection processing module mounting disc 15 is arranged at the center of the inner wall 13 of the cylindrical arm body taper joint. The SWD plug-in row is arranged on the outer wall 14 of the cylindrical arm body taper joint, which is convenient for burning programs and testing. The CAN1 transceiver interface and the power serial interface are arranged on the inner wall 13 of the cylindrical arm body taper joint, which is convenient for the CAN data line and the power to pass through the internal wiring of the snake-shaped mechanical arm.

[0037] Specifically, in this embodiment, the detection processing module uses the STM32G431C6Tx with an Arm Cortex-M4 core as the core processing chip of the detection processing module, leaving a SWD debugging interface and a CAN bus, and multiple input and output IOs are available. U0A shows the signal pins in the STM32G431C6. PA0, PA3, PA6, and PB0 are multiplexed as GPIO_MODE_IN_FLOATING floating inputs, serving as pins for receiving level signals from the TOF sensor 12. PA1, PA3, PA7, and PB8 are multiplexed as GPIO_MODE_OUT_PP push-pull outputs, used to provide 10us excitation signals to the four TOF sensors 12. PA11 and PA12 are multiplexed as CAN1's ​​RX and TX pins, leaving a CAN interface for communicating processed sensor information and recommended movement direction and distance to the upper control system. PA13 and PA14 are multiplexed as SWDIO and SWDCL pins for burning programs into the detection processing module using the SWD interface during the debugging phase and later development. PC14 and PC15 are external low-speed clocks connected to a stable 32kHz crystal oscillator. PG10 is multiplexed as a reset function for key reset and SWD burning during development. PF0 and PF1 are external high-speed clock signals connected to a stable 12MHz crystal oscillator. U0B is the digital power and analog power pins in the STM32G431C6. Connect VDD_1, VDD_2, VDD_3, and VBAT to the 3.3V power supply. At the same time, the 3.3V power supply is grounded through a capacitor. VSS_1, VSS_2, and VSS_3 are connected to the ground terminal. VDDA and VREF+ are connected to the 3.3V power supply through a 1K ohm resistor and are connected to the ground terminal through two parallel capacitors.

[0038] Specifically, in this embodiment, the external circuitry of the detection and processing module is as follows: The power supply section uses U1's AMS1117-3.3 (a 3.3V positive low-dropout voltage regulator suitable for high-efficiency linear regulators, switching power supplies, battery chargers, active small computer system interface terminals, laptop computers, and power management battery-powered instruments) to convert the 5V power supply voltage to 3.3V for use by the STM32 chip. The CAN driver section, with its built-in CAN bus protocol controller, conveniently establishes a CAN bus intelligent measurement and control node by simply connecting an external bus driver chip and appropriate anti-interference circuitry. This invention utilizes the Philips TJA1050 CAN bus driver. JP1, JP2, JP3, and JP4 are four 1×4 connector strips used to power the four TOF sensors 12, transmit excitation signals, and read level durations. JP5 is a 1×5 SWD connector strip, and JP6 is a 1×4 connector strip used for serial communication and external 5V power supply.

[0039] like Figure 4 、 Figure 5 As shown, the TOF sensor 12 uses the VL53L4CD TOF ranging module, which can provide non-contact distance sensing function, with a measurement range of 0-1300mm and a ranging accuracy of up to 1mm; the maximum sensing field of view is 18°, and the module's new generation laser emitter improves performance in light environments, with a ranging speed of up to 100Hz. The module's sensing working diagram is shown in the figure below. Figure 5 As shown, z sdu is the center point of the section of the cylindrical arm of the robot, x sdu and y sdu are the section coordinate system groups respectively. tof The center point of the TOF module installation position, x tof and y tof They are the coordinate system groups of the TOF module locations. parker is the center point of the obstacle in the environment, x parker and y parker These are the coordinate systems for the points where spatial obstacles are located. When in use, the IO port Trig triggers ranging. A high-level signal of at least 10µs is given. The module's internal signal transmitter emits a modulated invisible light beam. Any obstruction during propagation generates a reflected wave. The receiver receives this light and calculates the time difference. Based on common sense such as the speed of light, it measures the distance between the two. The module's photosensitive element automatically converts the optical signal into an electrical signal for return. The detection and processing module then automatically detects whether a signal has returned.

[0040] like Figure 3 As shown, the detection and processing module uses a TOF sensor to measure the distance to the obstacle. This embodiment uses a sine wave as a signal and uses the phase difference between the receiver and the transmitter to achieve the distance measurement function. The principle is that the phase offset is proportional to the distance between the measured point and the sensor, so the phase offset can be used to measure the distance. The signal transmitter emits a modulated invisible light (usually infrared light). When it is blocked during the propagation process, a reflected wave is generated. The receiver receives it and calculates the time difference. The distance between the two is measured based on common sense such as the speed of light. PIXEL is the sine wave amplitude, t MOD is the time for one cycle of the emission wavelength, is the time difference between the emission and reception of invisible light. The advantage of this solution is that it can be modulated using various light sources. The difference in signal amplitude between the transmitter and receiver can indirectly estimate the accuracy of the measurement result. Assume that the phase of the sinusoidal signal s(t) at the beginning of the emission is The amplitude is A. After a time delay Δt, the reflected signal r(t) is received. The phase of the signal is The amplitude attenuation of the reflected signal is B (ideal condition without ambient light interference, so there is no intensity offset). Four samplings are performed, each with an equal sampling time interval of T / 4. The phase delay Δφ of the two signals can be calculated by calculating the phase of the two signals, and the depth distance of the target can be calculated therefrom

[0041] s(t)=A·(1+sin(2πft))

[0042]

[0043]

[0044]

[0045]

[0046]

[0047] wherein λ is the modulation wavelength, T is the modulation light period, c is the light speed, f is the emission light frequency, t is the propagation time, Δt is the time difference, is the phase difference.

[0048] The detection processing module, after measuring the distance information and approximate position information of the arm body of the mechanical arm and the surrounding obstacles through the TOF sensor, first corrects and compensates the information measured by the TOF module. The corrected data obtained is compared with the set minimum close distance of the snake-shaped mechanical arm and the obstacle, and when an obstacle signal less than the minimum close distance appears, avoidance and alarm are performed. The establishment of the error compensation model is characterized in that the key lies in establishing the relationship between the error and the expected value. According to the causes of the error, it can be divided into system error and scene error. The system error is mainly caused by the limitation of the measurement principle or the structure of the sensor due to the manufacturing process or materials that cannot achieve the theoretical effect. The scene error is the error caused by the uncontrollable factors in various design scenes or application scenes of the sensor, such as measurement target reflectivity, chip temperature, etc. The system error can be corrected by establishing a mathematical compensation model. The common correction method is nonlinear calibration. The scene error usually adopts the method of constructing a depth offset query table to compensate the offset of the data, which is specifically:

[0049] As shown in Figure 6 , due to the limitation of module components, the generated square wave may not be an ideal square wave, and the time and phase difference after modulation cannot be an ideal sine wave, resulting in a nonlinear error between the measured phase difference and the ideal phase difference. The deviation presents a certain change rule as shown in Figure 6 , from Figure 6In the left figure, the inner standard circle is the ideal value expected by the experiment, while the outer ellipse is the actually measured value. The error between the two can be observed to change from small to large and then from large to small, presenting a 4-cycle in a 2π measurement period. The phase is the phase, and the confidence is the confidence. The specific change rule is shown in the right figure. Figure 6 A broken line change can be more intuitive. During calibration, an interpolation table method is generally used to compensate for non-linear errors. In the corresponding frequency wavelength range, multiple positions are uniformly selected, and the actual distance corresponding to the depth measurement value at the position is recorded, and then linear interpolation is performed to ultimately obtain a measurement phase and an actual distance corresponding phase corresponding table. When the specific measurement result is obtained, the table can be used to obtain the calibrated actual phase through the table method, thereby completing the non-linear error compensation.

[0050] As shown in Figure 7 , the scene error is mainly caused by the use of the scene. In the TOF measurement process, the depth information is mainly converted into an electrical signal by the light signal collected by the photosensitive element in the receiving sensor and stored. Different exposure times, measurement distances, and target object reflectivity can all cause measurement errors; at the same time, the change of the chip temperature also affects the photoelectron conversion process, causing depth drift. The drift error caused by the measurement distance can be compensated by estimating the residual error between the measurement value and the true depth value to establish a bias value lookup table. By establishing a corresponding relationship between the residual error and the depth value through the lookup table, it is simple and effective, and when operating, a standard plane can be set in front of the TOF sensor, and the distance from the optical center to the plane can be sampled at intervals of 10 mm within 10-1000 mm from the sensor to the plane, and the average value of 10 samples can be taken. In order to reduce the influence of the change of the chip temperature on the depth information, the sensor can be preheated for 10 minutes before the experiment, and the chip can be kept at about 60°C, and the corresponding bias value lookup table can be obtained. In order to test the influence of the chip temperature on the depth information, the TOF sensor can be tested in the measurement of a fixed standard plane scene without preheating, and the measured depth value changes as the sensor runs and the chip temperature gradually changes, and a corresponding temperature change compensation lookup table can be established after linear fitting, as shown in Figure 7 .

[0051] It is to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting; it is not intended to exclude myriad other embodiments of the present application that other inventors can develop based on the same general inventive concepts embodied by the described embodiments. That is, although the present application is described in terms of particular embodiments and illustrative figures, it should be apparent that the scope of the present application is not limited to these specific embodiments.

[0052] While the embodiments of the application have been shown and described herein, it will be understood by those skilled in the art that many changes, modifications, substitutions and alterations to these embodiments can be made without departing from the principles and spirits of the application, and it is intended that the scope of the application be limited solely by the scope of the appended claims and the equivalents thereof.

Claims

1. A distributed TOF ranging photosensitive system with a snake-like robotic arm, characterized in that: It includes a snake-like robotic arm, a TOF sensor, and a detection and processing module. The TOF sensor is used to measure the position and distance of obstacles. Several TOF sensors are distributed and installed on the snake-like robotic arm. The TOF module installed at the end of the snake-like robotic arm has a light-sensing function, which can provide measurement information of the ambient light intensity and feedback relevant information to the detection and processing module. The TOF sensor is connected via the CAN bus and transmits measurement data to the corresponding detection and processing module. The detection and processing module processes the received orientation and distance information to obtain information about the surrounding obstacles of the snake-like manipulator and make judgments about the surrounding obstacles. The detection and processing module transmits the judgment results to the upper control unit, and the snake-like manipulator completes the obstacle avoidance task under the control of the upper control unit. A group of TOF sensors are distributed on the wall of each conical segment of the serpentine robotic arm, with vertical downward being 0° and clockwise arranged at 0°, 90°, 180°, and 270°, with a total of four sensors arranged on each conical segment of the wall; each group of sensors is electrically connected to a corresponding detection and processing module, and each detection and processing module is connected to the upper control unit via a CAN bus; A photosensitive TOF sensor (6) and a camera are installed on the end section of the snake-like robotic arm as a visual sensor to obtain visual information of the end section of the snake-like robotic arm. The photosensitive TOF sensor (6) can scan pixel information captured by the camera in a single-point or array manner, and can also obtain surface structure information of the entire scene in real time through a scene picture taken by the camera. The depth information of the measured object is generated by the mutual correlation between the TOF sensor group (4) and the invisible light emitting module of the TOF sensor (6) and the photosensitive receiving module of the TOF sensor (6).

2. The distributed TOF ranging photosensitive system of a snake-like robotic arm according to claim 1, characterized in that: The process of the TOF sensor collecting the position and distance information of the obstacle is as follows: the detection processing module uses the set function of the GPIO push-pull output to send a TTL level trigger signal of 10us to the four TOF sensors in turn, and then the signal transmitter inside the TOF sensor emits a modulated invisible light. If the invisible light is blocked by an obstacle during the propagation process, it will generate a reflected wave. The receiver inside the TOF sensor receives the reflected wave and calculates the time difference between the emission and reception through the level holding time. According to the known speed of light, the distance between the snake-like robotic arm and the obstacle is obtained; through the measurement values ​​of the distance to the same obstacle by different TOF sensors in each group of TOF sensors, the position information of the spatial obstacle can be obtained after comparison; the measurement data is uploaded through the CAN bus, coupled analysis is performed in the coordinate system, and the next motion planning instruction is made.

3. The distributed TOF ranging photosensitive system of a snake-like robotic arm according to claim 1, characterized in that: The detection and processing module processes the received orientation and distance information by first performing error correction and compensation on the orientation and distance information measured by the TOF sensor; then comparing the corrected data with the preset threshold of the minimum distance between the serpentine robotic arm and the obstacle. When an obstacle signal less than the minimum distance threshold appears, an instruction is output to the upper control unit to avoid and issue an alarm.

4. The distributed TOF ranging photosensitive system of a snake-like robotic arm according to claim 3, characterized in that: Due to hardware and site limitations, the TOF sensor will produce systematic errors and scene errors when measuring orientation and distance information. For systematic errors, the detection and processing module corrects them by establishing a mathematical compensation model. For scene errors, the detection and processing module compensates for data offsets by constructing a depth offset lookup table.

5. The distributed TOF ranging photosensitive system of a snake-like robotic arm according to claim 4, characterized in that: The specific process of establishing the mathematical compensation model is as follows: using the interpolation table lookup method to compensate for nonlinear errors, evenly selecting multiple positions in the wavelength range of the corresponding frequency, recording the depth measurement value corresponding to the actual distance of the position, and then performing linear interpolation to finally obtain a correspondence table between the measured phase and the phase corresponding to the actual distance; when the specific measurement results are obtained, this table is used to obtain the actual phase after calibration through the table lookup method, thereby completing the nonlinear error compensation.

6. The distributed TOF ranging photosensitive system of a snake-like robotic arm according to claim 4, characterized in that: The specific process of constructing the depth offset lookup table is as follows: set up a plane directly in front of the TOF sensor, sample at intervals of 10mm within a distance of 10 to 1000mm between the sensor and the plane, test the distance from the optical center to the plane, and take samples multiple times to calculate the average value; to reduce the impact of chip temperature changes on depth information, let the sensor be preheated in the working state before conducting the experiment, keep the chip at 60°C, and obtain the corresponding deviation value lookup table.

7. The distributed TOF ranging photosensitive system of a snake-like robotic arm according to claim 2, characterized in that: The signal amplitude difference between the transmitter and receiver inside the TOF sensor can indirectly estimate the accuracy of the measurement result. Assume that the phase of the sinusoidal signal s(t) is The amplitude is A, and after a delay of Δt, the reflected signal r(t) is received. The phase of the signal is The amplitude of the reflected signal decays to B. Four samplings are performed, with the sampling time interval being equal, namely T / 4. The phase delay Δφ between the two signals can be calculated, from which the depth distance of the target can be calculated: s(t)=A·(1+sin(2πft)) Where λ is the modulation wavelength, T is the modulation light period, c is the speed of light, f is the emission light frequency, t is the propagation time, Δt is the time difference, is the phase difference.

8. The distributed TOF ranging photosensitive system of a snake-like robotic arm according to claim 1, characterized in that: Each section of the serpentine robot arm is composed of a cylindrical arm body cone section (3) and two vertebral end frames. Each cylindrical arm body cone section (3) is fixedly connected to a flexible central rod. The principle of the wire-driven serpentine robot arm is as follows: the upper computer software controls or uses remote control to control the movement of the lead screw motor in the motor drive box (1), so that the slider on the lead screw motor slides back and forth to stretch and release the robot arm wire drive rope group (2). The robot arm wire drive rope group (2) is composed of three groups of three, which pass through the lower end of the cylindrical arm body cone section (3) and are fixed to the corresponding serpentine robot arm section. The six groups of drive ropes are used to control the movement of the lead screw motor in the motor drive box (1). The ends of the six cylindrical arm body cones (3) are pulled to cause the central rod to deform accordingly to realize the flexible characteristics of the serpentine robotic arm; a special bracket is provided at the lower end of each cylindrical arm body cone (3); a detection and processing module PCB board is mounted on the special bracket of the vertebral lower end frame and fixed with screws; the detection and processing module is electrically connected to the TOF sensor group (4); a CAN data line and a power line pass through the inner circular hole of the cylindrical arm body cone (3) to connect to the detection and processing module; the detection and processing module is used for the serpentine robotic arm to sense the information of obstacles (5) around the corresponding arm segment during movement.

9. The distributed TOF ranging photosensitive system of a snake-like robotic arm according to claim 1, characterized in that: On the outer wall (14) of each cylindrical arm body cone, a horizontal socket strip for a TOF sensor (12) is arranged every 90 degrees to ensure that the measurement direction of the TOF sensor 12 is on a plane perpendicular to the center rod of the serpentine robotic arm; a detection and processing module mounting plate (15) is provided at the center of the inner wall (13) of the cylindrical arm body cone; the SWD socket strip is arranged on the outer wall (14) of the cylindrical arm body cone to facilitate program burning and testing; the CAN1 transceiver interface and the power serial port interface are arranged on the inner wall (13) of the cylindrical arm body cone to facilitate the routing of the CAN data line and the power supply through the inside of the serpentine robotic arm.

Images