Anti-self-trapping semantic navigation method and system for wheeled and footed rover based on vision-touch fusion

Abstract

The wheel-footed planetary rover anti-self-trapping semantic navigation method and system based on visual-touch fusion relates to the technical field of intelligent driving of deep space exploration vehicles. It provides a solution to the problem that the planetary rover is prone to self-trapping during driving on soft terrain, and successfully deploys the solution on the independently developed planetary rover, which includes a 12-degree-of-freedom wheel-footed mobile platform, a rotating six-dimensional force / torque sensor, a capacitive proximity switch, an absolute angle sensor, a ZED2 camera, a 3D laser radar, an external reference fixing bracket, and an NVIDIA Orin host computer and a semantic navigation control algorithm system; the camera and the laser radar are fixed on the external reference bracket; the rotating six-dimensional force / torque sensor is installed in the front two wheels of the vehicle, and 32-way capacitive proximity switches are evenly installed around the wheels; the wheel-footed mobile platform is provided with a lower computer main controller and a motor driver, the motor driver is connected to the mobile platform main controller, and the lower computer main controller is connected to the host computer. It can be widely used in the field of autonomous navigation technology of robots and unmanned vehicles on planetary terrain and outdoor off-road terrain.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent driving of deep space exploration vehicles, and in particular to a wheeled and footed mobile robot and a vision-tactile fusion anti-self-trapping semantic navigation method and system. Background Art

[0002] In 2009, the US Mars rover Spirit was unable to get out of the sand and caused economic losses of up to $400 million. The main reasons for the Spirit's self-entrapment include the following two points: First, the rover lacked a real-time wheel-soil contact mechanical model during its movement, resulting in a lack of perception of the mechanical properties of soft soil / wheel-soil interaction state, resulting in the inability to perceive the changes in soil mechanical properties in real time when the rover walked on terrain that was prone to self-entrapment. Second, the current lunar or Mars rovers use ground remote control + autonomous navigation when patrolling in orbit. When the rover senses the danger of self-entrapment, it cannot make autonomous decisions in time and re-plan the route to ensure safe driving.

[0003] In the existing patent literature, some technical solutions are given for the autonomous navigation method of the planetary rover, but no solution is given for the situation where the planetary rover cannot make autonomous decisions in time to re-plan the route to ensure safe driving when it senses that it is in danger.

[0004] For example, the existing patent document with the document number CN116088525A discloses a method for autonomous navigation of a planetary rover that takes into account the physical characteristics of the ground, which includes: constructing an autonomous navigation model of the planetary rover based on deep reinforcement learning; obtaining the operating status data of the planetary rover; inputting the operating status data into the autonomous navigation model, and outputting the navigation motion decision of the planetary rover; and controlling the motion of the planetary rover according to the navigation motion decision. This patent document realizes and improves the autonomous navigation capability of the planetary rover by constructing an autonomous navigation model based on deep reinforcement learning theory, which facilitates the planetary rover to autonomously perform planetary exploration without manual decision-making, improves the adaptability and movement ability of the planetary rover, improves the autonomy, intelligence and detection efficiency of the planetary rover, and realizes end-to-end mapping from environmental perception directly to motion decision-making, thereby improving the execution efficiency of the planetary rover in performing planetary exploration tasks. The prior art does not mention how to ensure the safety of the planetary rover when it is operating on soft terrain.

[0005] Therefore, how to ensure the safety of the rover when running on soft terrain has become a technical problem that needs to be solved urgently by those skilled in the art. Summary of the invention

[0006] The technical problems to be solved by the present invention are:

[0007] The present invention is to solve the technical problems that the existing planetary rover is difficult to effectively identify and classify in the planetary environment due to complex terrain, and is prone to self-trapping due to poor perception of wheel-soil action, and cannot make timely and autonomous decisions to escape from the predicament. The present invention gives the planetary rover the ability of visual and tactile fusion perception, as well as the ability of the planetary rover to make autonomous decisions to escape from the predicament when encountering danger, thereby ensuring the safe and intelligent operation of the planetary rover in the complex planetary terrain environment.

[0008] The technical solution adopted by the present invention to solve the above technical problems is:

[0009] A vision-tactile fusion anti-self-trapping wheeled-legged planetary rover, the planetary rover comprising a wheeled-legged planetary rover body, a rotating six-dimensional force / torque sensor (2), two groups of 32-channel capacitive proximity switches (4), an absolute angle sensor (3), a ZED2 binocular camera (5), a 3D laser radar (6), an external reference fixing bracket, 12 sets of servo motors and harmonic reducers thereof, a robot lower computer controller (1-1), and a robot upper computer (1-2) equipped with a vision-tactile fusion anti-self-trapping wheeled-legged planetary rover semantic navigation system (1-2). Rin computing platform (on-board computer), the rotation six-dimensional force / torque sensor (2) and the absolute angle sensor (3) are installed on the central axis corresponding to each of the two front wheels of the rover body, two 32-way capacitive proximity switches are evenly arranged around the corresponding front wheels, the ZED2 binocular camera (5) and the 3D laser radar (6) are both installed on an external reference fixing bracket, and the external reference fixing bracket is used for external reference calibration of the camera optical center coordinate system of the ZED2 binocular camera (5) and the laser radar optical center coordinate system of the 3D laser radar (6);

[0010] The upper-layer algorithm processing corresponding to the wheeled-legged rover anti-self-trapping semantic navigation system based on vision-touch fusion is performed on the robot host computer Orin computing platform (NVIDIA Orin host computer), and its speed information is sent to the robot lower computer controller (1-1) (STM32 lower computer) through ROS communication. The robot lower computer controller (1-1) controls the servo motor and its harmonic reducer by controlling the driver (single-chip microcomputer), so that the rover completes trajectory planning and target movement.

[0011] The body of the planetary rover is within the scope of existing technology. 12 sets of servo motors and their harmonic reducers are connected by flanges to form the rotation, swing, and leg-lifting joints of the robot's legs. Different joints are connected by flanges and non-standard parts to form the legs. The connecting parts of the leg-lifting joints are fixed on the body, thereby completing the connection between the robot's legs and the body. The body of the wheel-foot planetary rover is the mechanical structure, which includes two front wheels and two rear wheels. Each wheel is equipped with a rotating motor, a swing motor, and a leg-lifting motor, a total of 12 servo motors, each servo motor is integrated with a corresponding harmonic reducer, forming 12 sets of servo motors and their harmonic reducers.

[0012] The rotating joints are connected to the wheels. The transmission structure formed by four sets of rotating joints, swing joints and leg lifting joints and related parts constitutes four vehicle legs.

[0013] A wheeled-legged rover anti-self-trapping semantic navigation system based on visual-touch fusion, the semantic navigation system is used to be deployed on the wheeled-legged rover anti-self-trapping, the semantic navigation system includes a tactile perception system and a camera-lidar visual perception system, the corresponding signals calculated by the two systems are combined with a Transformer-based semantic segmentation algorithm and a laser F-LOAM positioning algorithm, and a semantic map is constructed using Octomap as a semantic mapping module (for obtaining tactile perception signals and visual perception signals);

[0014] Trajectory planning module: Use the improved DWA algorithm combined with the cost map obtained by the semantic mapping module (trajectory planning algorithm) to calculate three costs: target distance cost, semantic cost, and heading deviation cost. Use the three costs to evaluate multiple plans generated by the advanced DWA algorithm, select the optimal trajectory, and send the linear velocity and angular velocity information of the optimal trajectory to the robot lower computer controller (1-1);

[0015] The robot lower computer controller (1-1) controls the start and operation of 12 sets of servo motors and their harmonic reducers through various motor drivers, thereby realizing autonomous semantic navigation of the rover when it runs on soft terrain.

[0016] Furthermore, for the tactile perception system, the 32-way proximity switch installed on the wheel and the STM32 microcontroller in the wheel are used to collect proximity switch signals, and the TCP / IP communication method is combined to obtain the entry angle θ of the current wheel action. 1 The wheel driving torque is obtained by using the six-dimensional force / torque and absolute angle sensor at the wheel axle, combined with the Ethernet port and TCP / IP communication method. D The ground's support force on the wheel is F N , the direction is vertically upward and the data is collected at a frequency of 10 Hz;

[0017] The S1 switch function is introduced, combined with the static fluctuation error of the absolute angle sensor at rest. If the absolute value of the difference between the wheel angle at the current moment and the wheel angle at the previous moment is less than 0.01 rad, the vehicle is considered to be in a stationary braking state. It is used to distinguish the motion state (including forward and backward) from the braking state. The expression is as shown in formula (1). Then, according to the expanded unified ground mechanics model, formula (2) calculates the soil equivalent bearing coefficient K σ and soil equivalent shear strength coefficient K τ , give the two corresponding weights to obtain the comprehensive parameter K Σ, used to measure the comprehensive ability of the current soil to resist compression and shear, and obtain the parameter threshold as a danger warning;

[0018]

[0019]

[0020] K Σ =αK σ +(1-α)K τ ,(0≤α≤1) (3)

[0021] The parameter b represents the width of the wheel, the parameter r represents the radius of the wheel, Δθ is the difference between the current wheel angle and the previous wheel angle, in radians, the parameter α is a weight value, and the parameter k 1 ,k 2 ,k 3 is about the entry angle θ 1 The specific expression of the function is as follows:

[0022]

[0023]

[0024]

[0025] The comprehensive parameter K calculated in real time Σ Through ROS, it is published to the semantic mapping module at a frequency of 10 Hz. Σ Exceeding the safety parameter threshold means that the mechanical properties of the soil under the current rover are destroyed, and these positions will no longer be suitable for moving forward. The coordinates of the current destroyed mechanical properties are recorded in a two-way queue container. When constructing the 2D semantic grid map, the coordinates of the points where the mechanical properties are destroyed are traversed, and these coordinate points are expanded by a certain area and stored in the 2D semantic grid map as an inaccessible cost, so as to integrate tactile information into the semantic map and ensure the safety of the rover's navigation.

[0026] Furthermore, for the lidar-camera visual perception system, the RGB original image captured by the left eye of the ZED2 camera is used to artificially divide the planet's terrain into five types: soft sand, gravel-sand, large rocks, bedrock, and outcropping rock. The crossing priority is bedrock>gravel-sand>soft sand, and large rocks and outcropping rock are prohibited. The corresponding semantic segmentation dataset is constructed, and the semantic categories of different terrains in the image and their corresponding confidence information are obtained through the Transformer-based semantic segmentation algorithm GA-Nav; the depth point cloud is obtained from the 3D lidar and ZED2 depth camera, and then the lidar point cloud is analyzed. And perform voxel filtering with the same resolution as part of the camera point cloud; obtain the coordinate relationship between the camera left eye centroid coordinate system and the lidar optical center coordinate system through external parameter calibration; use a customized camera-lidar fusion method to preferentially fuse the lidar point cloud with semantic information on the camera pixel plane, and then add part of the camera point cloud to enable the robot to see the feature-rich ground. Using this customized fusion algorithm, the accuracy of the lidar is retained and the sparseness defect of the laser is compensated, ensuring that the robot can see rich ground textures. Finally, the deep semantic point cloud is obtained through the customized camera-lidar fusion method for subsequent semantic mapping modules.

[0027] Furthermore, for the semantic mapping module, it is characterized in that the depth information and semantic information in the deep semantic point cloud are used to construct a 3D voxel semantic map based on the octree structure; the map construction process is: first, voxel filtering is performed on the lidar point cloud and part of the camera point cloud to ensure that the resolution of the voxel filtering is equal to the resolution of the octree semantic map, so that the nodes in the voxel can fuse and update the semantics of the previous and next frames; secondly, the point cloud semantic fusion described in claim 4 is performed; then the projection range of the fused deep semantic point cloud is restricted, and only the deep semantic point cloud within a certain radius is projected, and then the Octomap library is called to construct a Max semantic octree based on Max fusion, or a Bayesian semantic octree is constructed. an fused Bayesian semantic octree, and then use the Max algorithm and the Bayesian algorithm to fuse the semantic information between two adjacent frames at each semantic node to complete the map semantic node update task; then, deploy the laser SLAM algorithm F-LOAM to provide the robot's posture positioning information in the world coordinate system, and after speed conversion, obtain the speed information in the vehicle coordinate system to complete the incremental construction of the 3D semantic map; finally, the 3D semantic map is projected to obtain a 2D semantic map, and combined with the tactile perception system described in claim 3, the tactile parameter information is fused to obtain a 2D semantic map that contains both visual perception information and tactile perception information, thereby realizing the function of the trajectory planning module.

[0028] Further, for the trajectory planning module, the motor and reducer and the robot body kinematics are considered, and the speed window sampling is performed according to the current linear velocity and angular velocity of the planetary rover, combined with the instantaneous linear acceleration and angular acceleration of the robot; after a certain period of simulation, a speed sampling trajectory that satisfies both the robot kinematics and the dynamics is obtained; but since this trajectory is at a low speed, within a certain sampling time, the sampling trajectory length is short and the length is not uniform, lacking the predictability within a certain path range in the future, therefore, an equal curvature prediction trajectory is introduced, and the speed of the prediction trajectory is determined by the 2D semantic map resolution and sampling time of claim 5, and the predicted angular velocity is solved according to the condition of equal curvature with the sampling trajectory, so as to predict the speed and angular velocity, simulate the movement of a certain distance, and ensure that all the prediction trajectories are of equal length. Then, each prediction trajectory is expanded considering the width of the vehicle body to calculate the semantic cost of crossing, and the distance cost and heading deviation cost between the end point of the sampling trajectory and the target point are considered at the same time, and the cost evaluation of the prediction trajectory is completed, and the sampling trajectory corresponding to the prediction trajectory with the minimum cost is finally output as the optimal trajectory and passed to the STM32 lower computer described in claim 1 to control the movement of the robot.

[0029] It is worth noting that the traditional DWA speed sampling causes the car to only move forward and cannot directly reverse to avoid obstacles. In order to adapt to tactile perception, the present invention adds a reverse speed sampling space to support timely reversing to escape from trouble when the wheels of the front star car sense that it is about to sink; and because the predicted trajectory in the prior art is long, and when encountering the target point, the predicted trajectory will touch the obstacle area, resulting in the robot being unable to reach the target point near the obstacle. To address this problem, the present invention compares the predicted trajectory with the distance to the target point, and the predicted trajectory converges within a certain circumference of the target point, and changes the length of part of the predicted trajectory to ensure that the robot can normally reach the target point near the obstacle.

[0030] A semantic navigation method for wheeled and footed rover to prevent self-trapping based on vision-touch fusion, the process of the method is as follows:

[0031] The anti-self-trapping semantic navigation method of the wheeled and footed planetary rover with visual and tactile fusion is characterized by comprising the following steps:

[0032] Step S101, start the ROS driver of the ZED2 camera to ensure that the left eye of the camera can normally collect original image data and camera point cloud; the original image information and point cloud information are transmitted to the laser radar-camera visual perception system through ROS communication at a frequency of 20 Hz;

[0033] Step S102, start the semantic segmentation model GA-Nav based on the Transformer deep learning framework, receive the camera left eye image as the input of the segmentation model, and after prediction by the GA-Nav model, output the pixel-level semantic category map and the corresponding confidence map at a frequency of 10 Hz, and transmit them to the lidar-camera visual perception system through ROS communication;

[0034] Step S103, starting the ROS driver of the laser radar to obtain the laser radar point cloud, and changing the point cloud format to serve as the input of the positioning algorithm and the laser radar-camera visual perception system;

[0035] Step S104, start the F-LOAM laser SLAM algorithm to obtain the real-time position and posture of the robot in the world coordinate system, and transmit it to the mapping module at a frequency of 10 Hz for incremental semantic map construction; at the same time, the posture information is also transmitted to the written odometer calculation program to calculate the current linear velocity and angular velocity of the robot, and finally input the odometer information into the planning module at a frequency of 10 Hz;

[0036] Step S105, starting the custom camera-lidar fusion algorithm described in claim 4, obtaining the ZED camera left eye original image, camera point cloud and semantic segmentation information described in step S102 described in step S101, and combining with the lidar point cloud described in step S103, completing the fusion of semantic information and point cloud information through the fusion method described in claim 4, and transmitting the fused deep semantic point cloud to the semantic mapping module at a frequency of about 8 Hz;

[0037] Step S106, starting the semantic mapping module described in claim 5, performing the semantic map construction method described in claim 5 according to the deep semantic point cloud generated in step S105, generating a 2D semantic grid map that has not yet integrated tactile information, and inputting it into a subsequent planning module at a frequency of about 8 Hz;

[0038] Step S107, starting the tactile perception system described in claim 3, including using a six-dimensional force sensor, a proximity switch and its acquisition microcontroller to obtain input parameters of tactile information, combining the modified tactile parameter extraction algorithm to perform calculations, and using the calculated parameters as monitoring indicators to monitor the tactile characteristics of the front wheels in real time, and finally outputting them to the 2D semantic grid map described in S106 at a frequency of about 10 Hz, so that the semantic map contains tactile information;

[0039] Step S108, starting the trajectory planning module described in claim 6, the module receives the 2D semantic grid map in S107, obtains the optimal trajectory of the robot in the current state after calculation by the improved DWA algorithm in claim 6, and finally outputs the speed information corresponding to the optimal trajectory at a frequency of 5 Hz;

[0040] Step S109, starting the STM32 slave computer described in claim 2, preparing to receive the motion control instruction from step S108;

[0041] Step S110, start the ROS_Bridge program, receive the speed information output by S108, and then pass the speed information to the STM32 lower computer through serial communication. After receiving the motion command, the lower computer controls the robot to move according to the planned trajectory.

[0042] Furthermore, in step S107, the six-dimensional force, proximity switch and absolute angle sensor acquire the normal force and the entry angle, and obtain the target tactile parameters after calculation.

[0043] Furthermore, in steps S102, S103, and S105, in the host computer Orin, the road conditions and terrain crossing priorities in the current field of view are obtained after processing by the deep learning model, and the deep semantic point cloud is obtained after customized camera-lidar fusion.

[0044] Furthermore, in the steps S104 and S106, visual information and tactile information are simultaneously considered to construct a semantic map containing visual information and tactile information; in the step S108, a 2D semantic grid map containing both visual and tactile information is received, and the improved DWA trajectory planning algorithm is used to obtain the optimal trajectory in the current state.

[0045] The beneficial effects of the present invention are:

[0046] The present invention effectively solves the technical problems of the existing planetary rover in the planetary environment, which is difficult to effectively identify and classify the complex terrain, and the lack of perception of the wheel-soil effect, which makes it easy to get stuck and unable to make autonomous decisions in time to get out of the predicament. The technical means defined in the present invention can give the planetary rover the ability of visual and tactile fusion perception, as well as the ability of the planetary rover to make autonomous decisions to get out of the predicament when encountering danger, ensuring the safe and intelligent operation of the planetary rover in the complex planetary terrain environment. The specific advantages are reflected in the following aspects:

[0047] (1) Based on the unified ground mechanics model, a modified algorithm for extracting comprehensive tactile parameters is provided, which makes up for the lack of tactile information perception of the planetary rover in the planetary environment. It allows the planetary rover to add tactile perception on the basis of visual perception, making the planetary rover's autonomous navigation more intelligent and safe.

[0048] (2) We constructed a semantic segmentation dataset of five types of planetary terrains and implemented the Transformer-based visual semantic segmentation technology to perform online real-time semantic segmentation on different terrains in the planetary environment and obtain the semantic categories and confidence information of different terrains as the visual perception basis of the planetary rover.

[0049] (3) Customized camera-lidar fusion, adding part of the camera point cloud to make up for the fact that the number of ground point clouds is insufficient after fusion when using only lidar, resulting in the loss of ground features. The coverage height of the camera point cloud on the pixel plane is adjustable, and the lidar point cloud is fused first, which not only retains the accuracy of the laser point cloud, but also makes up for the defect of its point cloud sparsity.

[0050] (4) The semantic map construction module constructs a global 3D semantic map based on the F-LOAM lidar SLAM positioning and combines it with the deep semantic point cloud to selectively project it onto the local 2D semantic map, and finally integrates the tactile information into the 2D semantic map. The final semantic map contains visual and tactile information and is used in the trajectory planning module.

[0051] (5) The improved DWA algorithm is expanded to increase the negative speed sampling space to meet the function of tactile obstacle avoidance. When the tactile parameters under the robot's feet exceed the safety threshold, the area will be set as an inaccessible area. At this time, the robot must retreat to safely escape from the predicament.

[0052] (6) The communication between the upper computer and the lower computer is ROS framework, TCP / IP, and network port communication.

[0053] Further features and aspects of the present invention will be clearly described in the following description of specific embodiments with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 It is the overall intention of the visual-tactile fusion anti-self-trapping semantic navigation process;

[0055] Figure 2 It is a flow chart of visual semantic segmentation and custom camera-lidar fusion;

[0056] Figure 3 It is the process of building semantic maps by combining deep semantic point cloud with laser positioning;

[0057] Figure 4 It is the process of extracting tactile information and fusing tactile information into 2D semantic map;

[0058] Figure 5 It is the process of implementing trajectory planning using the improved DWA algorithm in a 2D semantic map;

[0059] Figure 6This is a 3D model diagram of the complete hardware system of the planet rover, in which the rotating six-dimensional force / torque sensor is installed in the front two wheels of the vehicle, 32-way capacitive proximity switches are evenly installed around the wheels, the absolute angle sensor is installed at the center axis of the wheel in combination with the six-dimensional force, the ZED2 binocular camera and 3D laser radar are installed on the external parameter fixing bracket with the specified external parameters, the motor and the reducer are connected by flanges to form the rotation, swing, and leg-lifting joints of the robot's legs, and the leg-lifting connectors are fixed to the vehicle body to complete the connection between the robot's legs and the vehicle body;

[0060] Figure 7 This is a real picture of the planet car after processing;

[0061] Figure 8 This is the semantic segmentation information diagram of the GA-Nav model. The left side is the input camera original image, the middle is the color semantic category prediction, and the right side is the confidence of the pixel-level semantic category.

[0062] Fig. 9 It is the classification of planetary terrain, which can be roughly divided into five types: soft sand, gravel-sand, large rocks, bedrock, and outcropping rock layers;

[0063] Fig.10 yes Fig. 9 The semantic segmentation of each type of terrain is as follows according to the semantic color and traversable priority: orange soft sand in the third, green bedrock in the first, gravel-sand in the second, dark blue large rocks that are prohibited from crossing, and red outcropping rock layers that are prohibited from crossing;

[0064] Fig.11 This is a pixel plane diagram of the customized camera-lidar fusion, which prioritizes the fusion of the lidar point cloud (the dotted line in the figure) and then the camera point cloud with restricted height (the brown shaded part below in the figure);

[0065] Fig.12 It is a customized camera-lidar fusion point cloud map, in which the horizontal point cloud represents the fusion result of the lidar point cloud, and the surface point cloud represents the fusion result of part of the camera point cloud;

[0066] Fig.13 This is the mapping effect of the laser SLAM algorithm F-LOAM in a rectangular indoor environment.

[0067] Fig.14This is a tactile information extraction diagram of the rover walking in soft sandy terrain for about 250 seconds. The units of the three parameters in the figure are all Pa / m, among which, the range of soil equivalent bearing coefficient Ktheta (gray square dotted line) is [515717.59, 6118915.53], and the range of soil equivalent shear strength coefficient Ktao (red circle dotted line) is [33577.7, 2066792.44]. When the weight coefficient is 0.6, the calculated comprehensive parameter Ksum (blue triangle dotted line) is in the range of [349331.87, 4498066.29]. Therefore, the safety threshold reference comprehensive parameter is temporarily set to 300000 Pa / m. When the measured comprehensive parameter is less than the safety threshold, it indicates that the mechanical properties of the foot are about to be destroyed.

[0068] Fig.15 It is the process of constructing a 3D incremental semantic map. The three-dimensional world is presented in the form of octree voxels, and different colors in the voxels represent different semantic information.

[0069] Fig.16 The left side shows the projection relationship between the point cloud and the 2D cost. For example, desks and chairs are projected as obstacle costs, and the floor is a passable low-cost area. The right side shows the 2D grid semantic map after the projection radius limit of the 3D semantic map.

[0070] Fig.17 There are three trajectories in the trajectory planning module. The leftmost one represents the ordinary velocity sampling trajectory, which is used to generate a trajectory that satisfies both kinematics and dynamics. The middle one is the equal curvature prediction trajectory, which is mainly used to evaluate the semantic cost of the sampling trajectory. The right side is the superposition of these two trajectories.

[0071] Fig.18 It is the process of semantic navigation. The middle window on the left shows the semantic segmentation result. The original camera image is shown below. The light green represents the initialized 2D grid map. The "L"-shaped orange projection represents the obstacle cost. The red sphere in the figure represents the target point. The arc-shaped yellow trajectory represents the equal curvature prediction trajectory. The starting point of the trajectory represents the robot coordinate system. The 7 shorter blue parallel lines are the semantic evaluation trajectories of equal body extension, which are used to evaluate the semantic cost of the current trajectory considering the body width.

[0072] Fig.19 It simulates the movement situation where the mechanical characteristics of the left front wheel's tactile perception are destroyed. The sampling trajectory no longer moves forward, but reverses to avoid the area where the mechanical characteristics are destroyed, which proves the effectiveness of tactile perception and ensures the safe driving of the rover.

[0073] Description of symbols in the accompanying drawings:

[0074] 1. Wheel-legged mobile robot, 1-1. Robot lower computer controller, 1-2. Robot upper computer Orin computing platform, 1-3. Motor driver, 4. Capacitive proximity switch sequence, 2. Rotational six-dimensional force / torque sensor, 3. Absolute angle sensor, 5. ZED2 binocular camera, 6. 3D laser radar, 7. Display screen, 8. Artificial planetary environment experimental site, 9. Network port switch, 10. Six-dimensional force data acquisition card, 11-1. Camera left pixel plane, 11-2. Laser radar point cloud position, 11-3. Camera point cloud position, 12-1. Camera surface semantic point cloud, 12-2. Laser radar line semantic point cloud, 13-1. RGB original image input, 13-2. Semantic segmentation map corresponding to the original image, 13-3. Incremental 3D semantic map, 14-1. Output optimal trajectory, 14 -2 represents the driving target point, 14-3 represents the 2D initialization semantic map, 14-4 represents the projection of the impassable obstacle, 14-5 represents the passable equal curvature sampling trajectory after being intercepted with a radius of 0.5m from the end point, 14-6 represents the passable low-cost terrain projection, 14-7 represents the equal body width expansion trajectory (to evaluate the semantic cost of the equal curvature sampling trajectory), 15-1 represents the square cost area representing the mechanical property destruction area at the left wheel, 15-2 represents the selected optimal trajectory used to avoid the mechanical property destruction area under the feet, 15-3 represents the black speed sampling trajectory indicating that the front is impassable; 16-1 rotation joint (including rotation motor and reducer), 16-2 swing joint (including swing motor and reducer), 16-3 leg lifting joint (including leg lifting motor and reducer), 17 external reference fixing bracket. DETAILED DESCRIPTION

[0075] The present embodiment provides a visual-tactile-integrated semantic navigation algorithm system under a camera-lidar visual perception system, a tactile perception system, a semantic map construction module, and a trajectory planning module, and a self-developed wheeled-footed rover, including a 12-degree-of-freedom wheeled-footed mobile platform, a rotating six-dimensional force / torque sensor, a capacitive proximity switch, an absolute angle sensor, a ZED2 camera, a 3D lidar, an external reference fixing bracket, 12 sets of servo motors and a harmonic reducer, and an NVIDIA Orin host computer. The rotating six-dimensional force / torque sensor is installed in the front two wheels of the vehicle, and a 32-way capacitive proximity switch is installed in the front two wheels of the vehicle. The robot is evenly mounted around the wheel, and the proximity signal is collected through the single-chip computer module in the wheel. The absolute angle sensor is combined with the six-dimensional force and installed on the center axis of the wheel. The ZED2 camera and 3D laser radar are installed on the external parameter fixing bracket with specified external parameters. The motor and the reducer are connected by flanges to form the rotation, swing and leg-lifting joints of the robot's legs, and are fixed to the vehicle body through the leg-lifting connector to complete the connection between the robot's legs and the vehicle body; the wheel-foot mobile platform is equipped with a lower computer main controller and a motor driver. The motor driver is connected to the mobile platform main controller, and the lower computer main controller is connected to the upper computer.

[0076] The present invention provides a visual-tactile fusion anti-self-trapping semantic navigation method for wheeled-footed planetary rover, comprising the following steps:

[0077] Step S101, start the ROS driver of the ZED2 camera to ensure that the left eye of the camera can normally collect original image data and camera point cloud; the original image information and point cloud information are transmitted to the laser radar-camera visual perception system through ROS communication at a frequency of 20 Hz;

[0078] Step S102, start the semantic segmentation model GA-Nav based on the Transformer deep learning framework, receive the camera left eye image as the input of the segmentation model, and after prediction by the GA-Nav model, output the pixel-level semantic category map and the corresponding confidence map at a frequency of 10 Hz, and transmit them to the lidar-camera visual perception system through ROS communication;

[0079] Step S103, start the ROS driver of the laser radar to obtain the laser radar point cloud, change the point cloud format and use it as input for the positioning algorithm and the laser radar-camera visual perception system.

[0080] Step S104, start the F-LOAM laser SLAM algorithm to obtain the real-time position and posture of the robot in the world coordinate system, and transmit it to the mapping module at a frequency of 10 Hz for incremental semantic map construction; at the same time, the posture information is also transmitted to the written odometer calculation program to calculate the current linear velocity and angular velocity of the robot, and finally input the odometer information into the planning module at a frequency of 10 Hz;

[0081] Step S105, starting the custom camera-lidar fusion algorithm, obtaining the ZED camera left eye original image, camera point cloud and semantic segmentation information described in step S102 described in step S101, and combining with the lidar point cloud described in step S103, completing the fusion of semantic information and point cloud information through the custom camera-lidar fusion method, and transmitting the fused deep semantic point cloud to the semantic mapping module at a frequency of about 8 Hz;

[0082] Step S106, starting the semantic mapping module described in claim 5, performing the semantic map construction method described in claim 5 according to the deep semantic point cloud generated in step S105, generating a 2D semantic grid map that has not yet integrated tactile information, and inputting it into a subsequent planning module at a frequency of about 8 Hz;

[0083] Step S107, starting the tactile perception system described in claim 3, including using a six-dimensional force sensor, a proximity switch and its acquisition microcontroller to obtain input parameters of tactile information, combining the modified tactile parameter extraction algorithm to perform calculations, and using the calculated parameters as monitoring indicators to monitor the tactile characteristics of the front wheels in real time, and finally outputting them to the 2D semantic grid map described in S106 at a frequency of about 10 Hz, so that the semantic map contains tactile information;

[0084] Step S108, start the trajectory planning module described in claim 6, which receives the 2D semantic grid map in S107, obtains the optimal trajectory of the robot in the current state after calculation by the improved DWA algorithm in claim 6, and finally outputs the speed information corresponding to the optimal trajectory at a frequency of 5 Hz.

[0085] Step S109, starting the STM32 slave computer described in claim 2, and preparing to receive the motion control instruction from step S108.

[0086] Step S110, start the ROS_Bridge program, receive the speed information output by S108, and then pass the speed information to the STM32 lower computer through serial communication. After receiving the motion command, the lower computer controls the robot to move according to the planned trajectory.

[0087] In step S102, the RGB original image is used to artificially divide the planet's terrain into five types: soft sand, gravel-sand, large rocks, bedrock, and outcropping rock. The crossing priority is bedrock>gravel-sand>soft sand, and large rocks and outcropping rock are prohibited from crossing. The corresponding semantic segmentation dataset is constructed, and the semantic segmentation model GA-Nav based on the Transformer deep learning framework takes the terrain captured by the camera as input. After prediction by the GA-Nav model, the category of each pixel and its confidence in belonging to the category are obtained to complete the semantic segmentation of the terrain.

[0088] In step S104, the F-LOAM laser SLAM algorithm is started, which can only obtain the real-time position and posture of the robot in the world coordinate system, but cannot directly obtain the real-time linear velocity and angular velocity in the robot body coordinate system. Therefore, it is necessary to convert the real-time posture in the world coordinate system into the linear velocity and angular velocity in the robot body coordinate system, so as to obtain the laser odometer for the planning module. The following is the conversion process.

[0089] The posture, z, of the vehicle coordinate system in the world coordinate system is obtained from F-LOAM at a frequency of 10Hz, so the speed calculation interface is also written with a speed frequency of 10Hz. Therefore, according to the two adjacent postures at time t1 and t0, the expression is as follows, which represents the XYZ coordinates of the current vehicle coordinate system in the world coordinate system and the angle of rotation around the XYZ axis of the world coordinate system from left to right.

[0090]

[0091]

[0092]

[0093] The speed is calculated by formula (3) to obtain the current t 1 At this moment, the velocity of the vehicle coordinate system in the world coordinate system The obtained speed needs to be converted to the vehicle coordinate system to obtain the current t 1 The linear velocity v of the robot at the moment t1 , its direction needs to be combined with the linear velocity of the robot in the X direction in the world coordinate system at this time and rotated around the Z axis by angle yaw t1 To judge, the linear velocity expression of the robot in normal motion is as shown in formula (4), and the robot moves along the Y-axis of the world coordinate system, and the linear velocity expression is as shown in formula (5).

[0094]

[0095]

[0096] The angular velocity ω t1 And the angular velocity around the Z axis in the world coordinate system The same, the robot linear velocity v in the vehicle coordinates is obtained t1 and angular velocity ω t1 , as odometer information, and then sent to the trajectory planning module using ROS communication.

[0097] In step S105, the RGB original image captured by the left eye of the ZED2 camera is used to obtain the semantic categories of different terrains in the image and their corresponding confidence information through the Transformer-based semantic segmentation algorithm GA-Nav; the depth point cloud is obtained from the 3D lidar and the ZED2 depth camera, and then voxel filtering with the same resolution is performed on the lidar point cloud and part of the camera point cloud; the coordinate relationship between the camera left eye centroid coordinate system and the lidar optical center coordinate system is obtained through external parameter calibration; a customized camera lidar fusion method is used to limit the height of the camera point cloud in the pixel plane to ensure that the camera point cloud Covering the ground, the rover can obtain richer ground textures, so as to ensure that the robot walks through areas with lower costs first at the visual level. The left-eye pixel plane size of the ZED2 camera used is 360x640. When fusing the point cloud with the semantic image, confidence image, and RGB original image, a hash table of the same size as the pixel plane size is first constructed, and the lidar point cloud is first fused with the semantic information, and then the camera point cloud is considered. The use of this custom fusion algorithm retains the accuracy of the lidar and makes up for the laser sparsity defect, ensuring that the robot can see rich ground textures. Finally, the deep semantic point cloud is obtained through the custom camera-lidar fusion method for the subsequent semantic mapping module.

[0098] In step S106, first, octomap is used as the representation of the 3D map. The octomap represents a large bounded space as an octree of occupied grids (voxels). Each node in the octree represents a voxel of a specific size, depending on its level in the tree. Each parent node of the octree is subdivided into 8 child nodes until the optimal resolution is reached. In this way, large-scale three-dimensional maps can be effectively stored in memory. The root node is continuously divided into eight parts, and finally divided 16 times to obtain a leaf node. The resolution of its leaf node is set to 0.08m. At this time, the volume of the root node is sufficient to cover the area to be mapped. Octomap uses collision rate and loss rate to probabilistically update the occupancy of an octree according to different measurements of the sensor as shown in the following formula.

[0099]

[0100] For occupancy updates, z 1:t-1 is the observation data of the sensor from time 1 to t. The occupancy probability P(ν|z 1:t ) depends on the observation value P(ν|z t ), the predicted value P(ν|z 1:t-1 ) and an initial prior probability P(ν) which is usually set to 0.5.

[0101] Secondly, the point cloud semantic fusion described in step S107 is performed; then the projection range of the fused deep semantic point cloud is restricted, and only the deep semantic point cloud within a certain radius is projected, and then the Octomap library is called to construct a Max semantic octree based on Max fusion.

[0102] When performing Max fusion, we only include the semantic color with the highest confidence generated by semantic segmentation and its confidence in the generated point cloud. The same information is stored in each voxel of the octree map. When inserting the point cloud into the octree, if a voxel has a new measurement, we fuse the two semantic information: if the two semantic colors are the same, the semantic color is kept and the confidence is the average of the two semantic confidences. In the other case, if the two semantic colors are different, the semantic with higher confidence is kept and the confidence is reduced to 0.9 as a penalty for disagreement. The advantage of maximum probability fusion is that only one semantic information is stored, so it has more memory storage efficiency.

[0103] Optionally, a Bayesian semantic octree based on Bayesian fusion is constructed, storing only the 3 highest semantic colors and their confidences, called a semantic set. The other classes are grouped into the same class, denoted as c_others, and the confidence is denoted as conf_others, which is equal to 1 minus the sum of the 3 highest semantic confidences. The process of fusing two semantic sets is as follows: If the two semantic sets have the same elements, that is, the first 3 semantic colors are the same. Then we perform classic Bayesian fusion on these 3 classes and the other c classes. In the other case, if the semantic colors of the two sets are not the same, first add a new semantic color to each set so that they have the same semantic color. To do this, the necessary semantic colors are obtained by splitting the confidence of the class c_others. Each new semantic color will be assigned a confidence of 0.8*conf_others, while conf_others will drop to 0.2*conf_others. The insight of this is that if a semantic color is not in one set but in another set, it may have a large proportion of probability in the other set. Finally, we only keep the top 3 colors with the highest confidence in the fused semantic set

[0104] Finally, the Max algorithm and Bayesian algorithm are selectively used to fuse the semantic information between two adjacent frames at each semantic node to complete the task of updating the map semantic nodes. The laser SLAM algorithm F-LOAM is then deployed to provide the robot's position positioning information in the world coordinate system, and the speed information in the vehicle coordinate system is obtained after speed conversion to complete the incremental construction of the 3D semantic map.

[0105] In step S107, the 32-way proximity switches installed on the wheels and the STM32 microcontroller in the wheels are used for proximity switch signal collection. In combination with the TCP / IP communication method, the 32 proximity switches are used to obtain whether they are in contact with the ground. Each proximity switch has only two values, "0" or "1". The number of "1"s in each frame is counted and then multiplied by 11.25° to obtain the entry angle θ of the current wheel-soil action. 1 The wheel driving torque is obtained by using the six-dimensional force / torque and absolute angle sensor at the wheel axle, combined with the Ethernet port and TCP / IP communication method. D The ground's support force on the wheel is F N , the direction is vertically upward and the data is collected at a frequency of 10 Hz.

[0106] The S1 switch function is introduced, combined with the static fluctuation error of the absolute angle sensor at rest. If the absolute value of the difference between the wheel angle at the current moment and the wheel angle at the previous moment is less than 0.01 rad, the vehicle is considered to be in a stationary braking state. This function is used to distinguish the motion state (including forward and backward) from the braking state. The expression is shown in formula (7). Then, according to the expanded unified ground mechanics model, formula (8) calculates the soil equivalent bearing coefficient K σ and soil equivalent shear strength coefficient K τ , give the two corresponding weights to obtain the comprehensive parameter K Σ , which is used to measure the comprehensive ability of the current soil to resist compression and shear, and obtain the parameter threshold as a danger warning.

[0107]

[0108]

[0109] K Σ =αK σ +(1-α)K τ ,(0≤α≤1) (9)

[0110] The parameter b represents the width of the wheel, the parameter r represents the radius of the wheel, Δθ is the difference between the current wheel angle and the previous wheel angle, in radians, the parameter α is a weight value, and the parameter k 1 ,k 2 ,k 3 is about the entry angle θ 1 The specific expression of the function is as follows.

[0111]

[0112]

[0113]

[0114] The comprehensive parameter K to be calculated Σ Through ROS, it is published to the semantic mapping module described in claim 2 at a frequency of 10 Hz. When the comprehensive parameter K Σ Exceeding the safety parameter threshold means that the mechanical properties of the soil under the current rover are destroyed, and these positions will no longer be suitable for moving forward. The coordinates of the current destroyed mechanical properties are recorded in a two-way queue container. When constructing the 2D semantic grid map, the coordinates of the points where the mechanical properties are destroyed are traversed, and these coordinate points are expanded by a certain area and stored in the 2D semantic grid map as an inaccessible cost. In this way, tactile information is integrated into the semantic map to ensure the safety of the rover's navigation.

[0115] In step S108, the trajectory planning module, according to the current kinematic model of the planetary rover, is a four-wheel steering negative phase steering. At this time, the kinematic model can be simplified into a two-wheel differential motion model. The car can move forward, backward and rotate. The car can move forward Δs = vΔt at adjacent moments. The position expression of the next state of the car along the XY coordinate axis is obtained by converting it to the world coordinate system through formula (13).

[0116]

[0117] Then, speed sampling is performed, and the reasonable acceleration of the current planetary car is obtained according to the hardware conditions of the planetary car motor. Assuming that the current speed of the car is (V0, W0), when the linear angular velocity within _dt (0.1s) is sampled according to the maximum angular acceleration and linear acceleration, a series of linear angular velocities (Vn, Wn) are obtained, and one of the linear angular velocities (v, w) is taken, and the uniform motion is t seconds (5s). The route taken by the car at the current linear angular velocity v, w is a pendulum arc, and then the current route cost is calculated to obtain the current route cost.

[0118] After speed sampling, the trajectory is obtained The velocity sampling trajectory satisfies the kinematics and dynamics of the vehicle, but the semantic cost of the sampling trajectory is not considered. Sampling the same linear velocity, different angular velocity and the same length, the same curvature trajectory satisfies kinematics but not dynamics So will Add to In the above equation, we can obtain the solution that satisfies the kinematic and dynamic constraints. In order to achieve the above trajectory superposition, the equal curvature prediction trajectory Linear velocity v s and angular velocity ω s The calculation method is as follows:

[0119]

[0120] Where grid_res represents the grid resolution of the traversability cost map, and Δt represents the sampling period. d ,ω d )express The linear velocity and angular velocity of the final planner are Trajectory to predict In order to achieve this goal, it is necessary to ensure that the two trajectories are on the same arc during prediction. Since they have the same starting point, that is, the same curvature as the velocity sampling trajectory, the above formula can be used to calculate The angular velocity ω s , which is convenient for the subsequent calculation of the semantic cost of the predicted trajectory.

[0121] The velocity sampling trajectory and the constant curvature prediction trajectory are expressed in the form of They respectively represent the position, angle, linear velocity and angular velocity information of a sampling point in the current trajectory. The trajectory cost is mainly divided into three types: target distance cost Represents the sampling trajectory The distance between the end of the trajectory and the target point is to make the selected trajectory move towards the target point as much as possible;

[0122]

[0123] Semantic cost First, all the equal curvature prediction trajectories are expanded according to the vehicle body width to obtain a series of equidistant lines. The red color represents the target point. The cost of all vehicle body width trajectories (here n is 7) is used as the cost of the current equal curvature prediction trajectory, so as to select the crossing path with the lowest semantic cost.

[0124]

[0125] Course deviation cost The angle is determined by the velocity sampling trajectory The angle formed by the end point, the origin of the vehicle coordinate system and the target point. This angle indicates the direction deviation of the end point of the simulated trajectory relative to the target point. This ensures that the vehicle's heading is relatively stable.

[0126]

[0127] After the three costs are normalized, corresponding weights are given to the three costs, and the final trajectory evaluation function expression is as follows.

[0128]

[0129] At the same time, the part where the equal curvature predicted trajectory overlaps with the target point is intercepted, and only the equal curvature predicted trajectory within a radius of 0.5m from the current robot to the target point is retained. This ensures that the robot can converge to the target point faster and avoids the situation where the target point cannot be reached due to being close to an obstacle.

[0130] After step S108, the upper computer sends the speed information of the optimal trajectory to the lower computer through the ROS_Bridge interface in the form of serial communication, and the lower computer controls the robot to make the expected movement.

[0131] Example:

[0132] The present invention provides a solution to the problem that the planetary rover is prone to self-entrapment during driving on soft terrain, and successfully deploys the solution on the independently developed planetary rover, which includes a 12-degree-of-freedom wheel-foot mobile platform, a rotating six-dimensional force / torque sensor, a capacitive proximity switch, an absolute angle sensor, a ZED2 camera, a 3D laser radar, an external reference fixed bracket, and an NVIDIA Orin host computer and a semantic navigation control algorithm system; the camera and the laser radar are fixed on the external reference bracket; the rotating six-dimensional force / torque sensor is installed in the front two wheels of the vehicle, and the 32-way capacitive proximity switch is evenly installed around the wheel; the wheel-foot mobile platform is provided with a lower computer main controller and a motor driver, the motor driver is connected to the mobile platform main controller, and the lower computer main controller is connected to the host computer. It can be widely used in the field of autonomous navigation technology of robots and unmanned vehicles on planetary terrain and outdoor off-road terrain.

[0133] The following is combined with Figure 1-19 , the present invention is further described in detail with specific embodiments.

[0134] like Figure 6As shown, the present invention discloses a visual-tactile fusion semantic navigation algorithm system under a camera-lidar visual perception system, a tactile perception system, a semantic map construction module, a trajectory planning module, and a self-developed wheeled-footed planetary rover, including a 12-degree-of-freedom wheeled-footed mobile platform 1, a rotating six-dimensional force / torque sensor 2, a capacitive proximity switch sequence 1-4, an absolute angle sensor 3, a ZED2 camera 4, a 3D laser radar 5, an external reference fixing bracket between 4 and 5, 12 sets of servo motors and harmonic reducers, and an NVIDIA Orin host computer 1-2. The rotating six-dimensional force / torque sensor is installed in the front two wheels of the vehicle, 32 road Capacitive proximity switches are evenly installed on the wheels, and proximity signals are collected through the single-chip microcomputer module in the wheels. The absolute angle sensor is combined with the six-dimensional force and installed on the center axis of the wheel. The ZED2 camera and 3D laser radar are installed on the external parameter fixing bracket with specified external parameters. The motor and the reducer are connected by flanges to form the rotation, swing and leg lifting joints of the robot's legs, and are fixed to the vehicle body through the leg lifting connector to complete the connection between the robot's legs and the vehicle body; the wheel-foot mobile platform is equipped with a lower computer main controller 1-1 and a motor driver 1-3. The motor driver is connected to the mobile platform main controller, and the lower computer main controller is connected to the upper computer 1-2.

[0135] Step S101, start the ROS driver of the ZED2 camera 4 to ensure that the left eye of the camera can normally collect original image data and camera point cloud; the original image information and point cloud information are transmitted to the laser radar-camera visual perception system through ROS communication at a frequency of 20Hz;

[0136] Step S102: Start the semantic segmentation model GA-Nav based on the Transformer deep learning framework, and use the RGB original image captured by the left eye of the ZED2 camera. Figure 8 a) Obtain the semantic categories of different terrains in the image through the semantic segmentation algorithm GA-Nav Figure 8 b) and its corresponding confidence information 8c); obtain the depth point cloud from the 3D lidar and ZED2 depth camera, and then perform voxel filtering with the same resolution on the lidar point cloud and part of the camera point cloud; obtain the coordinate relationship between the camera left eye centroid coordinate system and the lidar optical center coordinate system through external parameter calibration; use the following method Fig.11 The customized camera-lidar fusion method limits the height of the camera point cloud in the pixel plane Cam_Limit to ensure that the camera point cloud covers the ground, so that the rover can obtain richer ground textures, thereby ensuring that it will walk through areas with lower cost at the visual level. Fig.18 14-6 in the figure, the left eye size of the ZED2 camera used is 360x640 pixel plane 11-1. When the point cloud is fused with the semantic image, confidence image, and RGB original image, a hash table with the same size as the pixel plane size is first constructed, and the lidar point cloud 11-2 is preferentially combined with Figure 8 The semantic information in the image is fused, and then the camera point cloud 11-3 is considered for fusion. This custom fusion algorithm not only retains the accuracy of the lidar, but also makes up for the laser sparsity defect, ensuring that the robot can see rich ground textures. Finally, the deep semantic point cloud is obtained through the custom camera-lidar fusion method for the subsequent semantic mapping module.

[0137] Step S103, start the ROS driver of the laser radar to obtain the laser radar point cloud, change the point cloud format to the point cloud in XYZIRT dimension, and then use it as the input of the positioning algorithm and the laser radar-camera visual perception system.

[0138] Step S104, start the F-LOAM laser SLAM algorithm to obtain the real-time position and posture of the robot in the world coordinate system, and transmit it to the mapping module at a frequency of 10 Hz for incremental semantic map construction; however, the algorithm can only obtain the real-time position and posture of the robot in the world coordinate system, and cannot directly obtain the real-time linear velocity and angular velocity in the robot body coordinate system. Therefore, it is necessary to convert the real-time position and posture in the world coordinate system into the linear velocity and angular velocity in the robot body coordinate system, so as to obtain the laser odometer for the planning module.

[0139] The conversion process is to obtain the posture of the vehicle coordinate system in the world coordinate system with a frequency output of 10Hz from F-LOAM, so the speed calculation interface published speed frequency is also 10Hz. Therefore, according to the two adjacent postures of the robot at time t1 and t0, the current t is obtained by differential calculation of formulas (1~3). 1 At this moment, the velocity of the vehicle coordinate system in the world coordinate system The obtained speed needs to be converted to the vehicle coordinate system to obtain the current t 1 The linear velocity v of the robot at the moment t1 , its direction needs to be combined with the linear velocity of the robot in the X direction in the world coordinate system at this time and rotated around the Z axis by angle yaw t1 To judge, the linear velocity expression of the robot in normal motion is as shown in formula (4), and the robot moves along the Y axis of the world coordinate system. At this time, the linear velocity expression is shown in formula (5); and the angular velocity ω t1 And the angular velocity around the Z axis in the world coordinate system The same, the robot linear velocity v in the vehicle coordinates is obtained t1 and angular velocity ω t1 , as odometer information, and finally input the odometer information into the planning module at a frequency of 10Hz;

[0140] Step S105, start the custom camera-lidar fusion algorithm, obtain the ZED camera left eye original image, camera point cloud and semantic segmentation information described in step S102 described in step S101, and combine with the lidar point cloud described in step S103, complete the fusion of semantic information and point cloud information through the custom camera-lidar fusion method, and use the fused deep semantic point cloud Fig.12 , transmitted to the semantic mapping module at a frequency of about 8 Hz;

[0141] Step S106: start the semantic mapping module, based on the deep semantic point cloud generated in step S105 Fig.12 The 3D semantic map is constructed based on the octree structure. First, octomap is used as the representation of the 3D map. The octomap represents a large bounded space as an octree of occupied grids (voxels). Each node in the octree represents a voxel of a certain size, depending on its level in the tree. Each parent node of the octree is subdivided into 8 child nodes until the optimal resolution is reached. In this way, large-scale 3D maps can be effectively stored in memory. The root node is continuously divided into eight parts, and finally the leaf node is obtained by dividing it 16 times. The resolution of its leaf node is set to 0.08m. At this time, the volume of the root node is sufficient to cover the area to be built. Octomap uses the collision rate and loss rate to probabilistically update the occupancy of an octree according to different measurements of the sensor as shown in formula (6), which is used to determine whether the current octree node is occupied. If the occupancy probability of the current node is greater than 0.5, it proves that it is occupied, but the occupied semantic category also needs to consider the semantic information to update to different category colors.

[0142] Secondly, the point cloud semantic fusion described in step S107 is performed; then the projection range of the fused deep semantic point cloud is restricted, and only the deep semantic point cloud within a certain radius is projected, and then the Octomap library is called to construct a Max semantic octree based on Max fusion.

[0143] When performing Max fusion, we only include the semantic color with the highest confidence generated by semantic segmentation and its confidence in the generated point cloud. The same information is stored in each voxel of the octree map. When inserting the point cloud into the octree, if a voxel has a new measurement, we fuse the two semantic information: if the two semantic colors are the same, the semantic color is kept and the confidence is the average of the two semantic confidences. In the other case, if the two semantic colors are different, the semantic with higher confidence is kept and the confidence is reduced to 0.9 as a penalty for disagreement. The advantage of maximum probability fusion is that only one semantic information is stored, so it has more memory storage efficiency.

[0144] Optionally, a Bayesian semantic octree based on Bayesian fusion is constructed, storing only the 3 highest semantic colors and their confidences, called a semantic set. The other classes are grouped into the same class, denoted as c_others, and the confidence is denoted as conf_others, which is equal to 1 minus the sum of the 3 highest semantic confidences. The process of fusing two semantic sets is as follows: If the two semantic sets have the same elements, that is, the first 3 semantic colors are the same. Then we perform classic Bayesian fusion on these 3 classes and the other c classes. In the other case, if the semantic colors of the two sets are not the same, first add a new semantic color to each set so that they have the same semantic color. To do this, the necessary semantic colors are obtained by splitting the confidence of the class c_others. Each new semantic color will be assigned a confidence of 0.8*conf_others, while conf_others will drop to 0.2*conf_others. The reason for this is that if a semantic color is not in one set but in another set, it may have a large proportion of probability in the other set. Finally, we only keep the top 3 colors with the highest confidence in the fused semantic set.

[0145] The Max algorithm and Bayesian algorithm are selectively used to fuse the semantic information between two adjacent frames at each semantic node to complete the map semantic node update task; the laser SLAM algorithm F-LOAM is then deployed to provide the robot's position positioning information in the world coordinate system, and the speed information in the vehicle coordinate system is obtained after speed conversion to complete the incremental construction of the 3D semantic map as shown in 13-3. After projection, a 2D semantic grid map that has not yet integrated tactile information is generated as shown in Fig.16 As shown, it is input into the subsequent planning module at a frequency of about 8 Hz;

[0146] Step S107, start the tactile perception system, and use the 32-way proximity switches installed on the wheel and the STM32 microcontroller in the wheel to collect proximity switch signals. Combined with the TCP / IP communication method, the 32 proximity switches are used to obtain whether they are in contact with the ground. Each proximity switch has only two values, "0" or "1". The number of "1"s in each frame is counted, and then multiplied by 11.25° to obtain the entry angle θ of the current wheel-soil action. 1 The wheel driving torque is obtained by using the six-dimensional force / torque and absolute angle sensor at the wheel axle, combined with the Ethernet port and TCP / IP communication method. D The ground's support force on the wheel is F N , the direction is vertically upward, and the data is collected at a frequency of 10 Hz and calculated in combination with the modified tactile parameter extraction algorithm.

[0147] The S1 switch function is introduced, combined with the static fluctuation error of the absolute angle sensor at rest. If the absolute value of the difference between the wheel angle at the current moment and the wheel angle at the previous moment is less than 0.01 rad, the vehicle is considered to be in a stationary braking state, otherwise it is considered to be in a normal moving state. It is used to distinguish the moving state (including forward and backward) from the braking state. The expression is shown in the above formula (7). Then, the soil equivalent bearing coefficient K is calculated according to the expanded unified ground mechanics model formula (8): σ and soil equivalent shear strength coefficient K τ , give the two corresponding weights to obtain the comprehensive parameter K Σ The specific expression is shown in the above formula (9), which is used to measure the comprehensive ability of the current soil to resist compression and shear, and obtain the parameter threshold as a danger warning.

[0148] The comprehensive parameter K to be calculated Σ Through ROS, it is published to the semantic mapping module described in claim 2 at a frequency of 10 Hz. When the comprehensive parameter K Σ Exceeding the safety parameter threshold means that the mechanical properties of the soil under the current rover are destroyed. Fig.19 As shown in the 15-1 area in the figure, these positions are no longer suitable for moving forward, and the forward sampling trajectory 15-3 fails. The optimal trajectory is 15-2 for retreating. Then the coordinates of the points where the current mechanical properties are destroyed are recorded in a two-way queue container. When constructing the 2D semantic grid map, the coordinates of the points where the mechanical properties are destroyed are traversed, and these coordinate points are expanded by a certain area and stored in the 2D semantic grid map as an inaccessible cost. Fig.16 b), thus achieving the fusion of tactile information into the semantic map and ensuring the safety of the rover's navigation.

[0149] Step S108, start the trajectory planning module, which receives the 2D semantic grid map in S107. According to the current kinematic model of the rover, which is a four-wheel steering negative phase steering, the kinematic model at this time can be simplified into a two-wheel differential motion model. The car can move forward, backward and rotate. The car can move forward Δs=vΔt at adjacent moments. The world coordinate system is converted to the position of the next state of the car along the XY coordinate axis by equation (13); then, velocity sampling is performed, and the reasonable acceleration of the current rover is obtained according to the hardware conditions of the rover motor. Assuming that the current speed of the car is (V0, W0), when the linear angular velocity within _dt (0.1s) is sampled according to the maximum angular acceleration and the linear acceleration, a series of linear angular velocities (Vn, Wn) are obtained, and one of the (v, w) linear angular velocities is taken, and the uniform motion is t seconds (5s). It is obtained that the route taken by the car at the current linear angular velocity v, w is a pendulum arc, and then the current route cost is calculated to obtain the current route cost.

[0150] After speed sampling, the trajectory is obtained Fig.17 As shown in a), the velocity sampling trajectory satisfies the kinematics and dynamics of the vehicle, but the semantic cost of the sampling trajectory is not considered. Therefore, in order to consider the semantic cost, the trajectory after velocity sampling is Sampling is performed to ensure that the linear velocity is the same as v s , a series of isocurvature trajectories with the same radius of curvature, different angular velocities, and the same length that satisfy kinematics but not dynamics Fig.17 b), so Overlay to In the above equation, we can obtain the solution that satisfies the kinematic and dynamic constraints. Fig.17 c) shows that the prediction trajectory meets the equal curvature Linear velocity v s and angular velocity ω s The calculation method of is shown in formula (14) above. The final planner uses Trajectory to predict semantic cost.

[0151] Speed ​​sampling trajectory and constant curvature predicted trajectory There are m trajectories each, and the two trajectories are represented by They respectively represent the position, angle, linear velocity and angular velocity information of a sampling point in the current trajectory. The trajectory cost is mainly divided into three types: target distance cost Represents the sampling trajectory The distance between the end of the target point and the target point is to make the selected trajectory move towards the target point as much as possible. Its expression is shown in the above formula (15); semantic cost What needs to be considered is the sampling trajectory Corresponding to the predicted trajectory of equal curvature The semantic cost of the vehicle width cannot be ignored, so it is expanded according to the vehicle width. Get a series of equidistant lines τ k ,k∈[1,n](here n is 7), such as Fig.18 As shown in 14-7, the red color in 14-2 represents the target point. The cost of all vehicle width trajectories (here there are 7, that is, n is 7) is used as the cost of the current equal curvature prediction trajectory. The specific expression is shown in the above formula (16); the heading deviation cost The angle is determined by the velocity sampling trajectory The angle formed by the end point, the origin of the vehicle coordinate system and the target point is as shown in formula (17). This angle represents the direction deviation of the end point of the simulated trajectory relative to the target point, thereby ensuring that the vehicle's heading is relatively stable.

[0152] After the three costs are normalized and given corresponding weights, the final trajectory evaluation function expression is obtained: At the same time, the part where the equal curvature predicted trajectory coincides with the target point is intercepted, and only the equal curvature predicted trajectory within a radius of 0.5m from the current robot to the target point is retained. Fig.18 As shown in Figure 14-5, this ensures that the robot can converge to the target point faster and avoids the situation where the target point cannot be reached due to the close distance to the obstacle.

[0153] After calculation by the improved DWA algorithm, the optimal trajectory of the robot in the current state is obtained, and finally the speed information corresponding to the optimal trajectory is output at a frequency of 5 Hz.

[0154] Step S109, start the STM32 slave computer to prepare to receive the motion control instruction from step S108.

[0155] Step S110, start the ROS_Bridge program, receive the speed information output by S108, and then pass the speed information to the STM32 lower computer through serial communication. After receiving the motion command, the lower computer controls the robot to move according to the planned trajectory.

[0156] The above description is only for the preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and changes.

Claims

1. A visual-tactile fusion anti-self-trapping wheeled-foot planetary vehicle, the planetary vehicle comprising a wheeled-foot planetary vehicle body, characterized in that: It also includes a rotating six-dimensional force / torque sensor (2), two groups of 32-channel capacitive proximity switches (4), an absolute angle sensor (3), a ZED2 binocular camera (5), a 3D laser radar (6), an external reference fixing bracket, 12 sets of servo motors and harmonic reducers thereof, a robot lower computer controller (1-1), and a robot upper computer Orin computing platform equipped with a wheeled rover anti-self-trapping semantic navigation system (1-2) based on visual-touch fusion. The rotating six-dimensional force / torque sensor (2) and the absolute angle sensor (3) are installed on the central axis corresponding to each of the two front wheels of the rover body. The two groups of 32-channel capacitive proximity switches are evenly arranged around the corresponding front wheels. The ZED2 binocular camera (5) and the 3D laser radar (6) are both installed on the external reference fixing bracket. The external reference fixing bracket is used for external reference calibration of the camera optical center coordinate system of the ZED2 binocular camera (5) and the laser radar optical center coordinate system of the 3D laser radar (6). The upper-layer algorithm processing corresponding to the wheeled-legged rover anti-self-trapping semantic navigation system based on vision-touch fusion is all performed on the robot host computer Orin computing platform, and its speed information is sent to the robot lower computer controller (1-1) through ROS communication. The robot lower computer controller (1-1) controls the servo motor and its harmonic reducer by controlling the driver, so that the rover completes trajectory planning and target movement.

2. A wheeled-footed rover anti-self-trapping semantic navigation system based on visual-touch fusion, characterized in that: The semantic navigation system is used to be deployed on the anti-self-trapping wheeled rover. The semantic navigation system includes a tactile perception system and a camera-lidar visual perception system. The corresponding signals calculated by the two systems are combined with a Transformer-based semantic segmentation algorithm and a laser F-LOAM positioning algorithm, and a semantic map is constructed using Octomap as a semantic mapping module; Trajectory planning module: Use the improved DWA algorithm combined with the cost map obtained by the semantic mapping module to calculate three costs: target distance cost, semantic cost, and heading deviation cost. Use the three costs to evaluate multiple plans generated by the advanced DWA algorithm, select the optimal trajectory, and send the linear velocity and angular velocity information of the optimal trajectory to the robot lower computer controller (1-1); The robot lower computer controller (1-1) controls the start and operation of 12 sets of servo motors and their harmonic reducers through various motor drivers, thereby realizing autonomous semantic navigation of the rover when it runs on soft terrain.

3. The wheeled-legged rover anti-self-trapping semantic navigation system based on visual-touch fusion according to claim 2 is characterized in that: For the tactile sensing system, the 32-way proximity switch installed on the wheel and the STM32 microcontroller in the wheel are used to collect proximity switch signals, and the current wheel-soil action entry angle θ1 is obtained by combining the TCP / IP communication method. The wheel drive torque T is obtained by combining the six-dimensional force / torque and absolute angle sensor at the wheel axle with the Ethernet port and TCP / IP communication method. D The ground's support force on the wheel is F N , the direction is vertically upward and the data is collected at a frequency of 10 Hz; The S1 switch function is introduced, combined with the static fluctuation error of the absolute angle sensor at rest. If the absolute value of the difference between the wheel angle at the current moment and the wheel angle at the previous moment is less than 0.01 rad, the vehicle is considered to be in a stationary braking state. It is used to distinguish the motion state (including forward and backward) from the braking state. The expression is as shown in formula (1). Then, according to the expanded unified ground mechanics model, formula (2) calculates the soil equivalent bearing coefficient K σ and soil equivalent shear strength coefficient K τ , give the two corresponding weights to obtain the comprehensive parameter K Σ , used to measure the comprehensive ability of the current soil to resist compression and shear, and obtain the parameter threshold as a danger warning; K Σ =αK σ +(1-α)K τ ,(0≤α≤1) (3) The parameter b represents the width of the wheel, the parameter r represents the radius of the wheel, Δθ is the difference between the current wheel angle and the previous wheel angle, in radians, the parameter α is a weight value, and the parameters k1, k2, and k3 are functions of the entry angle θ1. The specific expressions are as follows: The comprehensive parameter K calculated in real time Σ Through ROS, it is published to the semantic mapping module at a frequency of 10 Hz. Σ Exceeding the safety parameter threshold means that the mechanical properties of the soil under the current rover are destroyed, and these positions will no longer be suitable for moving forward. The coordinates of the current destroyed mechanical properties are recorded in a two-way queue container. When constructing the 2D semantic grid map, the coordinates of the points where the mechanical properties are destroyed are traversed, and these coordinate points are expanded by a certain area and stored in the 2D semantic grid map as an inaccessible cost, so as to integrate tactile information into the semantic map and ensure the safety of the rover's navigation.

4. The wheeled-legged rover anti-self-trapping semantic navigation system based on visual-touch fusion according to claim 2 or 3, characterized in that: For the lidar-camera visual perception system, the RGB original image captured by the left eye of the ZED2 camera is used to artificially divide the planet's terrain into five types: soft sand, gravel-sand, large rocks, bedrock, and outcropping rock. The crossing priority is bedrock>gravel-sand>soft sand, and large rocks and outcropping rock are prohibited. The corresponding semantic segmentation dataset is constructed, and the semantic categories of different terrains in the image and their corresponding confidence information are obtained through the Transformer-based semantic segmentation algorithm GA-Nav; the depth point cloud is obtained from the 3D lidar and ZED2 depth camera, and then the lidar point cloud and some The camera point cloud performs voxel filtering of the same resolution; the coordinate relationship between the camera left eye centroid coordinate system and the lidar optical center coordinate system is obtained through external parameter calibration; a customized camera-lidar fusion method is used to preferentially fuse the lidar point cloud with semantic information on the camera pixel plane, and then some camera point clouds are added to enable the robot to see the feature-rich ground. This customized fusion algorithm retains the accuracy of the lidar and compensates for the laser sparsity defect, ensuring that the robot can see rich ground textures. Finally, a deep semantic point cloud is obtained through a customized camera-lidar fusion method for subsequent semantic mapping modules.

5. The wheeled-legged rover anti-self-trapping semantic navigation system based on vision-touch fusion according to claim 4 is characterized in that: For the semantic mapping module, it is characterized in that the depth information and semantic information in the deep semantic point cloud are used to construct a 3D voxel semantic map based on the octree structure; the map construction process is: first, voxel filtering is performed on the lidar point cloud and part of the camera point cloud to ensure that the resolution of the voxel filtering is equal to the resolution of the octree semantic map, so that the nodes in the voxel can fuse and update the semantics of the previous and next frames; secondly, the point cloud semantic fusion described in claim 4 is performed; then the projection range of the fused deep semantic point cloud is restricted, and only the deep semantic point cloud within a certain radius is projected, and then the Octomap library is called to construct a Max semantic octree based on Max fusion, or a Bayesian based octree is constructed. The fused Bayesian semantic octree is then used to fuse the semantic information between two adjacent frames at each semantic node using the Max algorithm and the Bayesian algorithm respectively to complete the map semantic node update task; then, the laser SLAM algorithm F-LOAM is deployed to provide the robot's posture positioning information in the world coordinate system, and the speed information in the vehicle coordinate system is obtained after speed conversion to complete the incremental construction of the 3D semantic map; finally, the 3D semantic map is projected to obtain a 2D semantic map, and combined with the tactile perception system described in claim 3, the tactile parameter information is fused to obtain a 2D semantic map that contains both visual perception information and tactile perception information, thereby realizing the function of the trajectory planning module.

6. The wheeled-legged rover anti-self-trapping semantic navigation system based on vision-touch fusion according to claim 5 is characterized in that: For the trajectory planning module, the motor, reducer and robot kinematics are considered. According to the current linear velocity and angular velocity of the rover, combined with the instantaneous linear acceleration and angular acceleration of the robot, the velocity window is sampled. In order to achieve predictability within a certain path range in the future, an equal curvature prediction trajectory is introduced. The speed of the predicted trajectory is determined by the 2D semantic map resolution and sampling time. According to the condition of equal curvature with the sampled trajectory, the predicted angular velocity is solved to predict the speed and angular velocity, simulate the movement of a certain distance, and ensure that all predicted trajectories are of equal length. Then, each predicted trajectory is expanded considering the width of the vehicle body. It is used to calculate the semantic cost of crossing, and considers the distance cost and heading deviation cost between the end point of the sampling trajectory and the target point at the same time, completes the cost evaluation of the predicted trajectory, and finally outputs the sampling trajectory corresponding to the predicted trajectory with the minimum cost as the optimal trajectory and transmits it to the STM32 lower computer to control the movement of the robot; increases the reverse speed sampling space to support timely reversing to get out of trouble when the wheels of the front rover sense that they are about to sink; compares the predicted trajectory with the distance of the target point, and the predicted trajectory converges within a certain circumference of the target point, and changes the length of part of the predicted trajectory to ensure that the robot can normally reach the target point near the obstacle.

7. A method for preventing self-trapping of wheeled and footed rover based on vision-touch fusion using the system of claim 1, characterized in that: The process of the method is: The anti-self-trapping semantic navigation method of the wheeled and footed planetary rover with visual and tactile fusion is characterized by comprising the following steps: Step S101, start the ROS driver of the ZED2 camera to ensure that the left eye of the camera can normally collect original image data and camera point cloud; the original image information and point cloud information are transmitted to the laser radar-camera visual perception system through ROS communication at a frequency of 20 Hz; Step S102, start the semantic segmentation model GA-Nav based on the Transformer deep learning framework, receive the camera left eye image as the input of the segmentation model, and after prediction by the GA-Nav model, output the pixel-level semantic category map and the corresponding confidence map at a frequency of 10 Hz, and transmit them to the lidar-camera visual perception system through ROS communication; Step S103, starting the ROS driver of the laser radar to obtain the laser radar point cloud, and changing the point cloud format to serve as the input of the positioning algorithm and the laser radar-camera visual perception system; Step S104, start the F-LOAM laser SLAM algorithm to obtain the real-time position and posture of the robot in the world coordinate system, and transmit it to the mapping module at a frequency of 10 Hz for incremental semantic map construction; at the same time, the posture information is also transmitted to the written odometer calculation program to calculate the current linear velocity and angular velocity of the robot, and finally input the odometer information into the planning module at a frequency of 10 Hz; Step S105, starting the custom camera-lidar fusion algorithm described in claim 4, obtaining the ZED camera left eye original image, camera point cloud and semantic segmentation information described in step S102 described in step S101, and combining with the lidar point cloud described in step S103, completing the fusion of semantic information and point cloud information through the fusion method described in claim 4, and transmitting the fused deep semantic point cloud to the semantic mapping module at a frequency of about 8 Hz; Step S106, starting the semantic mapping module described in claim 5, performing the semantic map construction method described in claim 5 according to the deep semantic point cloud generated in step S105, generating a 2D semantic grid map that has not yet integrated tactile information, and inputting it into a subsequent planning module at a frequency of about 8 Hz; Step S107, starting the tactile perception system described in claim 3, including using a six-dimensional force sensor, a proximity switch and its acquisition microcontroller to obtain input parameters of tactile information, combining the modified tactile parameter extraction algorithm to perform calculations, and using the calculated parameters as monitoring indicators to monitor the tactile characteristics of the front wheels in real time, and finally outputting them to the 2D semantic grid map described in S106 at a frequency of about 10 Hz, so that the semantic map contains tactile information; Step S108, starting the trajectory planning module described in claim 6, the module receives the 2D semantic grid map in S107, obtains the optimal trajectory of the robot in the current state after calculation by the improved DWA algorithm in claim 6, and finally outputs the speed information corresponding to the optimal trajectory at a frequency of 5 Hz; Step S109, starting the STM32 slave computer described in claim 2, preparing to receive the motion control instruction from step S108; Step S110, start the ROS_Bridge program, receive the speed information output by S108, and then pass the speed information to the STM32 lower computer through serial communication. After receiving the motion command, the lower computer controls the robot to move according to the planned trajectory.

8. The anti-self-trapping semantic navigation method for wheeled-legged rover based on vision-touch fusion according to claim 7 is characterized in that: In step S107, the six-dimensional force, proximity switch and absolute angle sensor acquire the normal force and the entry angle, and obtain the target tactile parameters after calculation.

9. The anti-self-trapping semantic navigation method for wheeled-legged rover based on vision-touch fusion according to claim 8, characterized in that: In the steps S102, S103, and S105, in the host computer Orin, the road conditions and terrain crossing priorities in the current field of view are obtained after processing by the deep learning model, and the deep semantic point cloud is obtained after the customized camera-lidar fusion.

10. The anti-self-trapping semantic navigation method for wheeled-legged rover based on vision-touch fusion according to claim 9 or claim 8, characterized in that: In the steps S104 and S106, visual information and tactile information are simultaneously considered to construct a semantic map containing visual information and tactile information; in the step S108, a 2D semantic grid map containing both visual and tactile information is received, and the improved DWA trajectory planning algorithm is used to obtain the optimal trajectory in the current state.

Images