An adaptive variable-configuration pipeline robot motion control method based on environment perception

Abstract

A motion control method for an adaptive variable-configuration pipeline robot based on environmental perception is proposed. The pipeline robot is a double-swing-arm crawler mechanism. A depth camera collects images inside the pipeline for scene recognition. According to the scene recognition results, the planes and curved surfaces in the image are segmented, and the pipeline edges are extracted. The corresponding parameters are calculated using the edges to determine the robot's passability. A path planner and a swing arm planner are designed to generate the robot's reference trajectory and swing arm angle sequence, which are smoothed and input into an MPC motion controller. The data from the depth camera, IMU and wheel tachometer are used to estimate the robot's position and state through an ESKF algorithm, and the estimation results and the collision warning signal of the TOF module are input into the MPC motion controller in real time. Finally, the control signals of the motor and the swing arm motor are output to realize the adaptive variable configuration and autonomous motion of the robot in different scenarios.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a motion control method of an adaptive variable-configuration pipeline robot based on environment perception. Background Art

[0002] Ventilation ducts are important infrastructure in factories and high-rise buildings. Pollutant accumulation and corrosion in the ducts may cause respiratory infections and other diseases. However, since ventilation ducts are usually located in the mezzanine or high in the air of the building structure, their inspection and maintenance are difficult and challenging. The development of pipeline robots to replace manual inspections in pipelines is a current development trend.

[0003] In complex pipeline environments, pipeline robots need to pass through straight pipes, curved pipes, slopes, steps and other terrain scenarios. Especially in narrow environments, there are high requirements for the operator's proficiency. There are also problems of misoperation caused by blind spots and excessive fatigue during long inspections. At present, robot autonomous inspection and autonomous motion control have been widely studied in conventional ground environments, but in narrow pipeline environments, on the one hand, the accuracy of existing perception and positioning systems will be affected by factors such as high reflectivity and low texture in the pipeline environment. On the other hand, pipeline robots need to automatically plan configurations to adapt to various terrain scenarios. Learning and model-based methods can only solve a single scenario and have poor robustness. Therefore, existing methods cannot achieve autonomous inspection and adaptive motion control of robots in pipeline environments.

[0004] The Chinese patent application number is: CN202211176185.2, and the patent document with the authorization date of July 4, 2023 discloses an autonomous intelligent pipeline inspection robot. The toe angle is adjusted by the toe control panel to realize automatic main bus tracking of the pipeline robot at various operating speeds. The Hall sensor is used to count and measure distance to avoid measurement errors caused by wear or failure of the relatively poor working conditions of the inner wall of the pipeline. However, in a smooth pipeline environment, the mileage error caused by slippage cannot be solved, and adjusting the toe angle can only solve the autonomous movement of the straight pipe scenario.

[0005] The Chinese patent application number is: CN201710719668.5, and the patent document with the authorization date of June 20, 2023 discloses an adaptive tracked walking system and an adaptive tracked robot. When encountering obstacles, the suspension mechanism is used to automatically change the shape of the track to adapt to the changing terrain. It has the characteristics of simple structure and strong adaptability. However, this passive adaptive method can only be applied to scenes with small terrain undulations during straight-line travel, and cannot solve the problems of adaptive motion control in corners and passing through step scenes.

[0006] At present, how to use the data collected by multi-sensor equipment to design an adaptive configuration method for pipeline robots to realize autonomous inspection of robots in various pipeline scenarios is an urgent problem that needs to be solved. Summary of the invention

[0007] In order to solve the above technical problems, the present invention proposes a motion control method of an adaptive variable-configuration pipeline robot based on environmental perception. The pipeline robot is a double-swing-arm crawler mechanism. The sensing equipment it carries includes a depth camera, an IMU, a wheel tachometer, and three TOF modules for ranging. The depth camera is installed in front of the robot, and the three TOF modules are respectively installed on the left and right sides and the top of the robot. A multi-sensor fusion method is used to perceive the pipeline environment and the robot status, and an adaptive variable-configuration method for the pipeline robot is proposed to realize autonomous inspection of the robot in various pipeline scenarios.

[0008] To achieve the above object, the technical solution adopted by the present invention is:

[0009] A motion control method for an adaptive variable-configuration pipeline robot based on environment perception comprises the following steps:

[0010] Step 1: Place the pipeline robot inside the pipeline, use the depth camera to collect images of the pipeline in front, and use the neural network to identify the pipeline scene after image noise reduction preprocessing. According to the recognition results, the planes and curved surfaces in the image are segmented, and the area segmentation results on the two-dimensional image are projected into the three-dimensional space through the camera projection model, and the pipeline edge is extracted;

[0011] Step 2: Use the edge line to calculate the straight pipe width, curved pipe curvature, slope angle, and step height to analyze the robot's passability;

[0012] Step 3: Design the path planner and swing arm planner to generate the robot's reference trajectory and swing arm angle sequence, perform smoothing, and input them into the MPC motion controller;

[0013] Step 4: The data from the depth camera, IMU and wheel shorthand are used to estimate the robot position and state through the ESKF algorithm. The estimation result and the collision warning signal of the TOF module are input into the MPC motion controller in real time.

[0014] Step 5: In the MPC motion controller, the kinematic model of the double-swing-arm crawler robot is used, combined with the reference trajectory and the swing-arm angle sequence, to construct a quadratic objective function, which is solved using the iterative method to output the robot's adaptive motion control signal.

[0015] Furthermore, the pipeline robot is a double-swing-arm crawler mechanism, and the sensor equipment it carries includes a depth camera, an IMU, a wheel speed recorder, and three TOF modules for ranging. The depth camera is installed in front of the robot, and the three TOF modules are installed on the left and right sides and the top of the robot respectively.

[0016] Furthermore, the specific steps of step 1 are:

[0017] The depth camera collects images in the rectangular pipe, uses bilateral filtering and backlight compensation to reduce image noise and suppress strong light, completes image preprocessing, and uses a lightweight ResNet18 neural network to perform scene recognition on the processed images. The recognition results are classified into four scenes: straight pipe, curved pipe, slope, and steps; based on the classification results, the DeepLabv3 network is used to segment the two-dimensional image into multiple plane areas P and surface area S ; Along the pixel coordinate system The axis randomly selects three points in each area and uses the camera's intrinsic parameter matrix K With the scale information Z provided by the depth camera, the point in the selected pixel coordinate system Convert to camera coordinate system :

[0018] ;

[0019] Combined with the geometric structure characteristics of the pipeline environment, the three-dimensional coordinates of the sampling points in each area are used to fit each plane area in space. and surface area The pipeline edge formed by the intersection of each area is a straight line or a curve, and the regional equation is used to calculate the spatial equation of each straight line and curve.

[0020] Furthermore, the specific steps of step 2 are:

[0021] The length, width and height of the measuring pipeline robot are , the radius of the driving wheel is R， The radius of the driven wheel on the swing arm is r , the wheelbase of the front swing arm is , the wheelbase of the rear swing arm is , the wheelbase of the moving body is ;After obtaining the equation of each edge line, using the geometric characteristics of the pipeline, in a straight pipe environment, use the two straight lines on the bottom to calculate the width of the pipeline, and use the two straight lines on the side to calculate the height of the pipeline, and compare them with the outer envelope size of the robot to determine the passability of the straight pipe environment ahead; in a slope environment, calculate the angle between the side edge lines as the slope angle , the friction coefficient between the robot and the pipe wall is ,if , the front slope is considered passable; in the step scene, the distance between the edge of the step surface and the edge of the bottom surface is calculated as the step height H ,if , then it is considered that the steps ahead can be passed, ; In the curved pipe environment, the curvature of the inner line of the curve is calculated using the two curves of the bottom surface With the outer curvature , and then judge the passing conditions of the elbow:

[0022] .

[0023] Furthermore, the specific steps of step 3 are:

[0024] In the straight pipe and curved pipe scenes, the motion control only needs to consider the moving tracks. The path planner is designed to calculate the equidistant lines of the left and right side lines as the reference trajectory of the robot, and the sampling and smoothing are performed. In the slope and step scenes, in addition to the control of the moving tracks, the angles of the front and rear swing arms must be adjusted at the same time. The kinematic model of the double-swing arm crawler pipeline robot is simplified to a contact edge model, with the front swing arm contacting the edge. , rear swing arm touches the sideline Contact edge with chassis composition,

[0025] ;

[0026] The two end points are and , The two end points are and , The two end points of and , and The angle is , and The angle is ; Use the simplified model to contact the terrain and calculate during the movement and , and mapped to the swing arm joint angle and , complete the control of the swing arm.

[0027] Furthermore, in step 3, and The constraints are as follows:

[0028] 1) At least two points on the three simplified edges are in contact with the terrain;

[0029] 2) The three simplified edges have no interference with the terrain;

[0030] 3) The vertical line of the robot's center of gravity is and between;

[0031] exist and When there are multiple solutions, the state when the robot's center of gravity is the lowest is taken as the optimal state; after obtaining the swing arm angle sequence in the discrete state during the obstacle crossing process, the swing arm angle change rate is used as the motion cost, and the dynamic programming algorithm is used to optimize the cumulative cost to obtain the optimal discrete swing arm angle sequence, and the Bezier curve is used to sample and smooth the generated optimal discrete swing arm state sequence; the smoothed reference path and swing arm angle are sent to the MPC motion controller.

[0032] Furthermore, the specific steps of step 4 are:

[0033] In order to obtain the real-time position and state information of the robot, the ESKF motion equations of the nominal state and error state in discrete time are derived based on the IMU measurement model, and then the ESKF prediction process is executed, including the calculation of the prediction of the nominal state and the prediction of the error state. In the update stage, on the one hand, the left and right track speeds obtained by the encoder are used. On the other hand, in order to alleviate the influence of cumulative errors, the distance between the pipeline connection and the vehicle body is used as the position observation to complete the update of the covariance matrix and the error state; the distance of the pipeline connection is calculated by using the rectangular constraint and the camera projection model after the edge features of the pipeline connection are extracted by the depth camera; TOF ranging modules are installed on the left and right sides and the top of the robot for collision warning; the real-time position and state of the robot and the collision warning signal are input into the MPC motion controller.

[0034] Furthermore, the specific steps of step 5 are:

[0035] Set the system status to ,in Represent the robot’s three-dimensional spatial coordinates, yaw angle, and pitch angle respectively; the system input is Respectively represent the left track input speed, right track input speed, front swing arm angle increment, and rear swing arm angle increment; the reference state is is the initial state of the system, is the desired final state of the system; using the kinematic model of the double-swing arm crawler robot , predict the system status in the next N cycles at time i , the difference between the predicted state and the reference state is constructed into a quadratic objective function, and iterative optimization is performed to minimize it, and the system input within N cycles after optimization is obtained ; In the process of updating the system status, in order to obtain more accurate three-dimensional spatial coordinates of the robot, the robot position and status information obtained in step 4 is used; the system input is input using a sliding window Real-time optimization.

[0036] Compared with the prior art, the present invention has the following beneficial effects:

[0037] 1. The present invention adopts two-dimensional image region segmentation and camera projection model to calculate the regional space equation, and combines the geometric structure of the pipeline environment to obtain the space equation of the intersecting straight lines or curves in the region, and then obtains a simplified pipeline space configuration, which is used to analyze the robot's passability and generate motion reference trajectories. Compared with the method of directly extracting the edge features of the two-dimensional image, the edge extracted by this method is more accurate, will not be affected by the strong light reflection of the inner wall, and is more robust.

[0038] 2. The present invention designs robot passability constraints in a variety of scenarios, and uses space edges and robot kinematic models to analyze the center of gravity stability of the pipeline robot, which can avoid the operator's misjudgment of the robot's passability.

[0039] 3. The present invention adopts the edge contact model to simplify the contact motion between the robot and the terrain, and designs a path planner and a swing arm planner to generate a reference trajectory and a swing arm angle sequence. The ESKF algorithm is used to fuse the depth camera, IMU and wheel speed recorder to estimate the position and state of the robot. The estimation result and the collision warning signal of the TOF module are input into the MPC motion controller in real time, a quadratic objective function is constructed, and the input control signal is iteratively optimized. The adaptive variable motion control of the pipeline robot can be realized, which helps to improve the efficiency of autonomous inspection. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 The framework diagram of the motion control method of an adaptive variable-configuration pipeline robot based on environment perception proposed by the present invention is shown;

[0041] Figure 2 The figure shows the trajectory and arm planning diagram of the dual-swing-arm crawler pipeline robot passing through four scenarios: straight pipe, curved pipe, slope, and step. (a) is the straight pipe motion planning, (b) is the curved pipe motion planning, (c) is the slope motion planning, and (d) is the step motion planning.

[0042] Figure 3 A schematic diagram showing the simplification of the double-swing-arm crawler pipeline robot model into a contact edge model;

[0043] Figure 4 A schematic diagram showing the six stages of a robot's simplified edge model passing through a staircase. DETAILED DESCRIPTION

[0044] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0045] The present invention provides a motion control method of an adaptive variable-configuration pipeline robot based on environment perception, the method comprising the following steps:

[0046] Step 1: Place the pipeline robot inside the pipeline, use the depth camera to collect images of the pipeline in front, and use the neural network to identify the pipeline scene after image noise reduction preprocessing. According to the recognition results, the planes and curved surfaces in the image are segmented, and the area segmentation results on the two-dimensional image are projected into the three-dimensional space through the camera projection model, and the pipeline edge is extracted;

[0047] Step 2: Use the edge line to calculate the straight pipe width, curved pipe curvature, slope angle, and step height to analyze the robot's passability;

[0048] Step 3: Design the path planner and swing arm planner to generate the robot's reference trajectory and swing arm angle sequence, perform smoothing, and input them into the MPC motion controller;

[0049] Step 4: The data from the depth camera, IMU and wheel shorthand are used to estimate the robot position and state through the ESKF algorithm. The estimation result and the collision warning signal of the TOF module are input into the MPC motion controller in real time.

[0050] Step 5: In the MPC motion controller, the kinematic model of the double-swing arm crawler robot is used to combine the reference trajectory and the swing arm angle sequence to construct a quadratic objective function, which is solved using an iterative method to output the robot's adaptive motion control signal. The framework of the robot motion control method is shown in Figure 1 shown.

[0051] First, the image inside the rectangular pipe captured by the depth camera is processed, and bilateral filtering and backlight compensation are used to reduce image noise and suppress strong light, completing the image preprocessing;

[0052] The lightweight ResNet18 neural network is used to perform scene recognition on the processed images, and the recognition results are classified into four scenes: straight pipe, curved pipe, slope, and steps;

[0053] According to the classification results, the DeepLabv3 network is used to segment the two-dimensional image into multiple plane regions. P and surface area S ;

[0054] Along the pixel coordinate system The axis randomly selects three points on each area and uses the camera's intrinsic parameter matrix K With the scale information Z provided by the depth camera, the point in the selected pixel coordinate system Convert to camera coordinate system :

[0055] (1);

[0056] Combined with the geometric structure characteristics of the pipeline environment, the three-dimensional coordinates of the sampling points in each area are used to fit each plane area in space. and surface area The pipeline edge formed by the intersection of each area is a straight line or a curve, and the regional equation is used to calculate the spatial equation of each straight line and curve.

[0057] The length, width and height of the measuring pipeline robot are , the radius of the driving wheel is R， The radius of the driven wheel on the swing arm is r , the wheelbase of the front swing arm is , the wheelbase of the rear swing arm is , the wheelbase of the moving body is ;After obtaining the equation of each edge line, using the geometric characteristics of the pipeline, in a straight pipe environment, use the two straight lines on the bottom to calculate the width of the pipeline, and use the two straight lines on the side to calculate the height of the pipeline, and compare them with the outer envelope size of the robot to determine the passability of the straight pipe environment ahead;

[0058] In the slope environment, the angle between the side edges is calculated as the slope angle , the friction coefficient between the robot and the pipe wall is ,if , then the slope ahead is considered passable;

[0059] In the step scene, the distance between the edge of the step surface and the edge of the bottom surface is calculated as the step height H ,if , then it is considered that the steps ahead can be passed, ;

[0060] In the pipe bending environment, the curvature of the inner line of the bend is calculated using the two curves of the bottom surface. With the outer curvature , and then judge the passing conditions of the elbow:

[0061] (2).

[0062] like Figure 2 The figure shows the trajectory and swing arm planning of the double-swing-arm crawler pipeline robot through four scenarios: straight pipe, curved pipe, slope, and step. (a) is the straight pipe motion planning, (b) is the curved pipe motion planning, (c) is the slope motion planning, and (d) is the step motion planning.

[0063] In the straight pipe and curved pipe scenarios, motion control only needs to consider the moving track. The path planner is designed to calculate the equidistant lines of the left and right side lines as the reference trajectory of the robot, and perform sampling and smoothing processing.

[0064] In slope and step scenarios, in addition to controlling the moving crawler, the angles of the front and rear swing arms must be adjusted at the same time; the kinematic model of the double-swing-arm crawler pipeline robot is simplified to a contact edge model, with the front swing arm contacting the edge. , rear swing arm touches the sideline Contact edge with chassis composition,

[0065] (3);

[0066] The two end points are and , The two end points are and , The two end points of and , and The angle is , and The angle is ; Use the simplified model to contact the terrain and calculate during the movement and , and mapped to the swing arm joint angle and , complete the control of the swing arm.

[0067] Solution and The constraints are as follows:

[0068] 1) At least two points on the three simplified edges are in contact with the terrain;

[0069] 2) The three simplified edges have no interference with the terrain;

[0070] 3) The vertical line of the robot's center of gravity is and between;

[0071] exist and When there are multiple solutions, the state of the robot with the lowest center of gravity is taken as the optimal state, such as Figure 4 As shown, it is a schematic diagram of the six stages of the robot's simplified edge model passing through the stairs.

[0072] After obtaining the swing arm angle sequence in discrete states during the obstacle crossing process, the swing arm angle change rate is used as the motion cost, and the dynamic programming algorithm is used to optimize the cumulative cost to obtain the optimal discrete swing arm angle sequence. The generated optimal discrete swing arm state sequence is sampled and smoothed using the Bezier curve; the smoothed reference path and swing arm angle are sent to the MPC motion controller.

[0073] In order to obtain the real-time position and state information of the robot, the ESKF motion equations of the nominal state and error state in discrete time are derived based on the IMU measurement model, and then the ESKF prediction process is performed, including the calculation of the prediction of the nominal state and the prediction of the error state;

[0074] In the update phase, the left and right track speeds obtained by the encoder are used. On the other hand, in order to alleviate the impact of cumulative errors, the distance between the pipeline connection and the vehicle body is used as the position observation to complete the update of the covariance matrix and error state. The distance of the pipeline connection is calculated by using the rectangular constraint and camera projection model after extracting the edge features of the pipeline connection using the depth camera.

[0075] TOF ranging modules are installed on the left and right sides and top of the robot for collision warning; the real-time position and status of the robot and the collision warning signal are input into the MPC motion controller.

[0076] Set the system status to ,in Represent the robot’s three-dimensional spatial coordinates, yaw angle, and pitch angle respectively; the system input is Respectively represent the left track input speed, right track input speed, front swing arm angle increment, and rear swing arm angle increment; the reference state is is the initial state of the system, is the desired final state of the system;

[0077] Kinematic model of double swing arm crawler robot , predict the system status in the next N cycles at time i , the difference between the predicted state and the reference state is constructed into a quadratic objective function, and iterative optimization is performed to minimize it, and the system input within N cycles after optimization is obtained ; In the process of updating the system status, in order to obtain more accurate three-dimensional spatial coordinates of the robot, the robot position and status information obtained in step 4 is used; the system input is input using a sliding window Real-time optimization.

[0078] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A motion control method for an adaptive variable-configuration pipeline robot based on environmental perception, characterized in that: The following steps are involved: Step 1: The pipeline robot is placed inside the pipeline. The depth camera captures an image of the pipeline in front. After image noise reduction preprocessing, the pipeline scene is recognized using a neural network. Based on the recognition results, the planes and curved surfaces in the image are segmented. The region segmentation results on the two-dimensional image are projected into three-dimensional space using a camera projection model, and the pipeline edge lines are extracted. The specific steps of step 1 are: The depth camera captures images inside a rectangular pipe, uses bilateral filtering and backlight compensation to reduce noise and suppress strong light, completing image preprocessing. The processed images are then subjected to scene recognition using a lightweight ResNet18 neural network, classifying the resulting scenes into four categories: straight pipe, curved pipe, slope, and steps. Based on the classification results, the DeepLabv3 network is used to segment the two-dimensional image into multiple planar regions P and curved regions S. Randomly select three points in each area along the x-axis of the pixel coordinate system, and use the camera's intrinsic parameter matrix K and the scale information Z provided by the depth camera to convert the selected point p in the pixel coordinate system into uv Convert p to the camera coordinate system C : Zp uv =Kp C Combined with the geometric structure characteristics of the pipeline environment, the three-dimensional coordinates of the sampling points in each area are used to fit each plane area P in the space i and surface area S j The pipeline edge formed by the intersection of each area is a straight line or a curve, and the regional equation is used to calculate the spatial equation of each line and curve; Step 2: Use the edge line to calculate the straight pipe width, bend curvature, slope angle, and step height to analyze the robot's passability; The specific steps of step 2 are: The length, width and height of the measuring pipeline robot are L l ,L w ,L h The radius of the driving wheel is R, the radius of the driven wheel on the swing arm is r, and the wheelbase of the front swing arm is l F , the wheelbase of the rear swing arm is l R , the wheelbase of the moving body is l B After obtaining the equation of each edge line, the geometric characteristics of the pipe are used to calculate the pipe width using two straight lines on the bottom surface and the pipe height using two straight lines on the side surface. These values ​​are compared with the robot's outer envelope size to determine the passability of the straight pipe environment ahead. In a sloped environment, the angle between the side edges is calculated as the slope angle θ. S , the friction coefficient between the robot and the pipe wall is μ, if arctan(μ)<θ S , then the front slope is considered passable; in the step scene, the distance between the edge of the step surface and the edge of the bottom surface is calculated as the step height H. It is considered that the step ahead can pass through; in the curved pipe environment, the two curves on the bottom surface are used to calculate the curvature of the inner line of the curve λ in and the outer curvature λ out , and then judge the passing conditions of the elbow: Step 3: Design a path planner and an arm planner to generate the robot's reference trajectory and arm angle sequence, perform smoothing, and input them into the MPC motion controller; The specific steps of step 3 are: In straight pipe and curved pipe scenarios, motion control only needs to consider the track. The path planner is designed to calculate the equidistant lines of the left and right side edges as the reference trajectory of the robot, and perform sampling and smoothing. In slope and step scenarios, in addition to controlling the track, the angles of the front and rear swing arms must also be adjusted. The kinematic model of the dual-swing arm tracked pipeline robot is simplified to a contact edge model, with the front swing arm contacting the edge l f , rear swing arm touches the edge line l r Contact edge with chassis l b composition, i f =arccos(l R / (Rr))+θ F i r =arccos(l R / (Rr))+θ R l r The two end points are p r With p br , l f The two end points are p f With p bf , l b The two end points are p br With p bf , l r With l b The angle is θ r , l f With l b The angle is θ f ; Use the simplified model to contact the terrain and calculate θ during the movement r and θ f , and mapped to the swing arm joint angle θ R and θ F , complete the control of the swing arm; Solve θ in step 3 r and θ f There are the following constraints: 1) At least two points on the three simplified edges are in contact with the terrain; 2) The three simplified edges do not interfere with the terrain; 3) The vertical line of the robot's center of gravity is at p br With p bf between; In θ r and θ f When multiple solutions exist, the state where the robot's center of gravity is at its lowest is taken as the optimal state. After obtaining the discrete swing arm angle sequence during the obstacle crossing process, the swing arm angle change rate is used as the motion cost. A dynamic programming algorithm is used to optimize the cumulative cost to obtain the optimal discrete swing arm angle sequence. The generated optimal discrete swing arm state sequence is sampled and smoothed using a Bezier curve. The smoothed reference path and swing arm angle are sent to the MPC motion controller. Step 4: The robot's position and state are estimated using the ESKF algorithm using the data from the depth camera, IMU, and wheel shorthand. The estimation results and the collision warning signal from the TOF module are input into the MPC motion controller in real time. The specific steps of step 4 are: In order to obtain the real-time position and state information of the robot, the ESKF motion equations of the nominal state and error state in discrete time are derived based on the IMU measurement model, and then the ESKF prediction process is executed, including the calculation of the prediction of the nominal state and the prediction of the error state. On the one hand, the left and right track speeds obtained by the encoder are used in the update stage. On the other hand, in order to alleviate the influence of cumulative errors, the distance between the pipeline connection and the vehicle body is used as the position observation to complete the update of the covariance matrix and the error state; the distance of the pipeline connection is obtained by using the depth camera to extract the edge features of the pipeline connection, and then calculated using the rectangular constraint and camera projection model; TOF ranging modules are installed on the left and right sides and the top of the robot for collision warning; the real-time position and state of the robot and the collision warning signal are input into the MPC motion controller Step 5: In the MPC motion controller, the kinematic model of the dual-swing arm crawler robot is used, combined with the reference trajectory and the swing arm angle sequence, to construct a quadratic objective function, which is solved using an iterative method to output the robot's adaptive motion control signal. The specific steps of step 5 are: Set the system state to x=[x r ,y r ,z r ,θ yaw ,θ pitch ,θ F ,θ R ] Τ , where x r ,y r ,z r ,θ yaw ,θ pitch They represent the robot’s three-dimensional spatial coordinates, yaw angle, and pitch angle respectively; the system input is u=[v L ,v R ,Δθ F ,Δθ R ] Τ , v L ,v R ,Δθ F ,Δθ R They represent the left track input speed, right track input speed, front swing arm angle increment, and rear swing arm angle increment respectively; the reference state is X ref ={x0,x1,x2,···,x end }, x0 is the initial state of the system, x end is the desired final state of the system; the kinematic model f of the double-swing arm crawler robot is used to predict the system state in the next N cycles at time i The difference between the predicted state and the reference state is constructed into a quadratic objective function, and iterative optimization is performed to minimize it, and the system input u within the optimized N cycles is obtained. i→i+N-1 During the system status update process, in order to obtain more accurate three-dimensional spatial coordinates of the robot, the robot position and status information obtained in step 4 is used; a sliding window is used to optimize the system input u in real time.

Images