A dynamic scene binocular vision slam method based on target tracking

Abstract

The application discloses a dynamic scene binocular vision SLAM method based on target tracking and belongs to the field of simultaneous localization and mapping. In the remote end, an embedded device is used to control a follow-up holder camera installed on a moving body and collect images. In the service end, an embedded device is used to deploy an algorithm. The algorithm adopts a SLAM algorithm combined with target tracking to exclude the interference of the shielding mechanism of the moving body itself and dynamic objects in the field of view, so as to reduce the error of pose calculation and map construction. The application can not only detect and segment dynamic objects, but also segment the objects shielding the field of view. Based on a light target detection network and a distributed structure, resource consumption is reduced, so that the algorithm can be deployed on an embedded device.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of simultaneous localization and mapping, and particularly relates to a dynamic scene binocular vision SLAM method based on target tracking. BACKGROUND

[0002] Simultaneous localization and mapping (SLAM) refers to an algorithm that a robot moves in an unknown environment, detects the surrounding environment during movement, and perceives the motion pose of itself according to the features in the environment, and simultaneously constructs a sparse point cloud map. At present, the SLAM technology is widely applied in the fields of automatic driving, unmanned aerial vehicle, virtual reality technology, augmented reality technology, etc., and provides algorithm support for perceiving the pose of itself.

[0003] Among them, the SLAM algorithm is divided into static scene SLAM and dynamic scene SLAM according to the working scene. The static scene SLAM algorithm accounts for the majority, which refers to the robot working environment as a pure static scene, such as ORBSLAM, VINS-MONO, LSDSLAM, etc. They are very accurate in pose calculation and map construction in a static environment, but in a dynamic environment, they will be disturbed by dynamic objects and have a large error. Dynamic scene SLAM improves the algorithm to exclude the disturbance of dynamic objects to achieve accurate pose estimation, such as DS-SLAM, DynaSLAM, VDO-SLAM, etc.

[0004] In the literature [C. Yu et al., "DS-SLAM: A Semantic Visual SLAM towards Dynamic Environments," 2018 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS), 2018, pp. 1168-1174, doi: 10.1109 / IROS.2018.8593691.], DS-SLAM is proposed, which is a dynamic scene SLAM based on the ORBSLAM2 framework combined with the SegNet network as a semantic segmentation network. When a new image frame is obtained, the semantic segmentation network is used to perform semantic segmentation on the image to segment out the potential moving object region. After extracting the feature points, the random sample consensus (RANSAC) is used to calculate the projection point to the epipolar distance to check the consistency of the feature points, and the outliers are screened out. If there are many outliers on the potential dynamic object, the object is considered to be dynamic, and all feature points on it are marked as outliers, and then all outliers are deleted.

[0005] In the literature [B. Bescos, J. M. Fácil, J. Civera and J. Neira, "DynaSLAM: Tracking, Mapping, and Inpainting in Dynamic Scenes," in IEEE Robotics and Automation Letters, vol. 3, no. 4, pp. 4076-4083, Oct. 2018, doi: 10.1109 / LRA.2018.2860039.], DynaSLAM is proposed, which is a dynamic scene SLAM algorithm based on the framework of ORBSLAM2, using Mask-RCNN as a semantic segmentation network. When a new image frame is obtained, the image is segmented using the semantic segmentation network, and the obvious dynamic area such as "people" is directly deleted, and the remaining area is tracked lightly; for potential moving objects (such as chairs, books), for the current frame, select the 5 frames with the largest overlap to calculate the projection and projection depth of each key point to the current frame, and generate three-dimensional points, calculate the angle formed by the key points and the three-dimensional points, if it is greater than 30°, it is considered that the point is blocked, and it is considered as a dynamic point, and it is not processed. Then use the region growing algorithm to get the mask of the dynamic object from the dynamic feature points. For the background of the dynamic object, use the information of the previous 20 frames to repair.

[0006] The principle of VDO-SLAM is similar to DynaSLAM, and Mask-RCNN is also used for semantic segmentation, but the difference is that the method of distinguishing dynamic objects is different: VDOSLAM uses dense optical flow to track objects and uses scene flow to distinguish dynamic and static objects.

[0007] The above algorithms only detect and segment external dynamic objects, and if there is a field of view occlusion in the picture, it cannot be accurately segmented, and these field of view occlusions are still dynamic objects relative to static scenes. In addition, the above algorithms use a semantic segmentation network for detecting and segmenting dynamic objects, which has high accuracy but poor real-time performance, and occupies too many resources for lightweight devices. SUMMARY

[0008] The technical problem to be solved by the present application is to provide a dynamic scene binocular vision SLAM method based on target tracking, which is suitable for the working environment of outdoor operation of engineering machinery, adopts a distributed structure, uses an embedded device at the far end to control a binocular follow-up pan-tilt camera and collect images, and uses an embedded device at the service end to deploy an algorithm, excludes the occlusion mechanism of the moving body in the field of view and the interference of dynamic objects, and reduces the error of pose calculation and map construction.

[0009] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is as follows:

[0010] A dynamic scene binocular visual SLAM method based on target tracking is proposed. At a remote end, an embedded device controls a pan-tilt camera mounted on a moving object to acquire images. At the server end, an embedded device deploys the algorithm. The algorithm employs a SLAM algorithm combined with target tracking to eliminate interference from occlusion mechanisms of the moving object itself and other dynamic objects within the field of view. The SLAM method includes the following steps:

[0011] Step 1: The SLAM algorithm's binocular initialization and object detection network reads the training model information;

[0012] Step 2: Remotely capture binocular color video images;

[0013] Step 3: The target tracking network performs target detection and tracking on the binocular color video images, and sends the results to the SLAM thread for further processing.

[0014] Step 4: The SLAM thread preprocesses the stereo color image, extracts corner points, and then uses optical flow to match the stereo key points and calculate depth information.

[0015] Step 5: Using the target tracking results from Step 3, classify the key points into static points and points to be filtered.

[0016] Step 6: Apply epipolar geometric constraints to the static points in Step 5 to remove redundant outliers, and use the PnP algorithm to solve for the pose information.

[0017] Step 7: For the points to be filtered in Step 5, use the dynamic point filtering method to filter out the real dynamic points;

[0018] Step 8: Delete the dynamic points from Step 7, delete the co-view relationship between the current frame and the point, and merge the static points;

[0019] Step 9: Update the information of the current frame, including key points, map points and their co-view relationships; perform local optimization based on pose; if the current frame is a key frame, perform loop closure detection.

[0020] A further improvement to the technical solution of this invention is as follows: In step 2, the operator on the server side uses a VR device equipped with an IMU to control the rotation of the gimbal of the servo camera, and at the remote end, a Raspberry Pi is used to connect to the servo camera to control the servo camera to acquire binocular color video images, which are then transmitted back to the server via ROS to complete the acquisition of binocular color video images; the IMU measurement model on the VR device is:

[0021]

[0022] in, is the angular velocity, and ω b is the true value of the angular velocity, and b g is the bias of the angular velocity, and n g is the noise of the angular velocity, is the acceleration, and q bw is the rotation matrix from the world to the body frame, and a w is the true value of the acceleration, and g w is the acceleration of gravity, and b a is the bias of the acceleration, and n a is the noise of the acceleration;

[0023] The conversion relationship between the angular velocity and the Euler angle is:

[0024]

[0025] where φ is the yaw angle, θ is the pitch angle, and ψ is the roll angle; ω x , ω y , and ω z are the vectors of the angular velocity decomposed in the xyz axes, respectively.

[0026] Then the input pulse numbers of the two servo motors of the follow-up gimbal camera are:

[0027]

[0028] where P ⊥ and P ∥ are the input pulses of the two servo motors of the follow-up gimbal camera, and DEG is the step angle.

[0029] The further improvement of the technical scheme of the application is that in step 3, the target tracking network is a target detection network based on yolov5s and Kalman filtering, and specifically includes the following steps:

[0030] 3.1, the target detection network detects the local motion mechanism in the image to obtain a group of rectangular frames;

[0031] 3.2, a Kalman filter is constructed for the new target output by step 3.1, and the rectangular frame coordinate information is used for initialization;

[0032] where the state quantity is set as the state transition matrix is set as Since there is no input quantity, u=0, the observation quantity is set as the observation matrix is set as

[0033] In addition, the special scene occlusion object on the moving body, i.e., the "first-person excavator arm" and the "first-person fire hose", is preset with an initial position due to the fixed starting position, i.e.:

[0034]

[0035] 3.3, track the old target output by step 3.1, and use the predicted value output by the Kalman filter to perform IOU matching on all outputs of step 3.1:

[0036]

[0037] 3.4, mark the target in step 3.3 whose matching result is greater than the threshold T as a successful tracking result, use its rectangular frame as the final result, and correct the Kalman filter;

[0038] 3.5, the tracking failure flag of the target in step 3.3 whose matching result is less than the threshold T is incremented, if the tracking failure flag is greater than the threshold F, the memory of the Kalman filter is released, otherwise the predicted value output by the Kalman filter is used as the final result;

[0039] 3.6, if step 3.1 target detection network does not output to any rectangular frame, the tracking failure flag of all existing targets is incremented, if the tracking failure flag is greater than F, the memory of the corresponding Kalman filter is released, otherwise the predicted value output by the Kalman filter is used as the final result.

[0040] Further improvement of the technical scheme of the present application is that in step 7, the dynamic point screening method comprises the following steps:

[0041] 7.1, using epipolar geometry constraint to calculate the essential matrix E of the static point,

[0042] 7.2, using SVD singular value decomposition to calculate the rotation matrix R and the translation vector t of the essential matrix E of step 7.1:

[0043]

[0044]

[0045] wherein R1 and R2 are both a possible rotation matrix, are both a possible translation matrix.

[0046] Two groups of R1 and R2 are obtained, R2, and the solution with a depth greater than zero is taken as the pose of the point to be screened.

[0047] 7.3, using the pose information in step 7.2 as a reference, classify the points to be screened in step 5 into dynamic points and static points:

[0048] According to the essential matrix E, the epipolar line l1 of the point x1 is obtained T = Ex1;

[0049] The distance formula of the point x2 to the epipolar line l1 is:

[0050]

[0051] Where x1 and x2 are a pair of matching points, F is the basis matrix, X and Y are the parameters of the epipolar line, and T represents the transpose of the matrix;

[0052] If D is greater than the threshold value D th It is considered to be a dynamic point, if D is less than the threshold value D th It is considered to be a static point.

[0053] Due to the adoption of the above technical scheme, the technical progress achieved by the present application is:

[0054] 1、The present application not only can detect and segment dynamic objects, but also can segment visual field occlusion objects, based on a lightweight target detection network and a distributed structure, resource consumption is reduced, so that the algorithm can be deployed on an embedded device.

[0055] 2、The present application proposes a lightweight visual field occlusion dynamic scene SLAM algorithm for the working environment of outdoor operation of engineering machinery, the algorithm adopts a distributed structure, a remote embedded device controls a binocular pan-tilt camera and collects images, an embedded device is used in the server to deploy the algorithm, the algorithm adopts a SLAM algorithm combined with target tracking to exclude the occlusion mechanism of the moving body itself in the visual field and the interference of dynamic objects, and reduce the error of pose calculation and map construction. BRIEF DESCRIPTION OF DRAWINGS

[0056] In order to more clearly illustrate the technical scheme in the embodiments of the present application or the prior art, the drawings needed to be used in the embodiments or the prior art description will be briefly introduced below. Obviously, the drawings in the following description are some embodiments of the present application, and those skilled in the art can also obtain other drawings according to these drawings without creative labor;

[0057] Figure 1 is a flowchart of a dynamic scene binocular vision SLAM method based on target tracking provided by the present application;

[0058] Figure 2 is a multi-target tracking algorithm flowchart based on YOLOv5s and Kalman filtering in the embodiment of the present application;

[0059] Figure 3 is the principle diagram of the dynamic feature point rejection and screening in the embodiment of the application;

[0060] Figure 4 is the hardware deployment schematic diagram in the embodiment of the application. DETAILED DESCRIPTION

[0061] It should be noted that the terms "comprising" and "having" and any variations thereof in the specification and claims of the present application and the above-mentioned drawings are intended to cover non-exclusive inclusion, for example, a process, method, system, product or device including a series of steps or units does not have to be limited to those steps or units clearly listed, but can include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

[0062] The embodiment of the present application provides a dynamic scene binocular vision SLAM method based on target tracking, solves the problems of the prior art that if a visual field obstruction relative to a static scene still belongs to a dynamic object appears in a picture, the dynamic object cannot be accurately segmented out, and when a semantic segmentation network is used for detecting and segmenting the dynamic object, real-time performance is poor and too many resources are occupied for a lightweight device, and the general idea is target detection + Kalman filtering + SLAM algorithm.

[0063] The application will be further described in detail below with reference to the drawings and embodiments:

[0064] The inventors of the present application found in the research process that most SLAM algorithms have good positioning performance in a static scene, but errors occur in a dynamic scene, because most algorithms rely on feature points on static objects for auxiliary positioning, if the feature points come from dynamic objects, the movement of the feature points between frames will be different from the movement trend of other static feature points, although the RANSAC algorithm can resist the influence of a small number of outliers, if the number of dynamic feature points is large and is in the same order of magnitude as the static feature points, the RANSAC algorithm will fail. Most current dynamic SLAM algorithms are based on a semantic segmentation network, and the region of a dynamic object is obtained through the semantic segmentation network to remove dynamic feature points. However, because the semantic segmentation network consumes a large amount of resources, it often cannot run in real time, or occupies a large amount of resources and cannot be deployed on most embedded devices. Therefore, the application focuses on removing dynamic target interference while saving computing resources, so that the algorithm can be deployed on embedded devices (small in size, relatively poor in computing power, but small computers are convenient to move).

[0065] For the working environment of outdoor operation of the engineering machinery, the whole hardware system of the application includes a server and a remote terminal;

[0066] The server is composed of an embedded device, an operator, and a VR device. The operator wears the VR device, which is provided with an IMU module. The remote follow-up gimbal camera adjusts according to the posture of the VR device. The embedded device receives the image data of the remote end and processes it by running an algorithm.

[0067] The remote end is composed of a moving body and a follow-up gimbal camera. In a construction scene, the moving body includes an excavator, a fire-fighting robot, etc. The follow-up gimbal camera is installed at a central position above the moving body and will be blocked by a part of the moving body itself (such as the first-person excavator arm or the first-person fire-fighting water gun). The follow-up gimbal camera is composed of two servos and a binocular camera. The servos move according to the IMU motion on the VR device of the server. Therefore, the follow-up gimbal camera can move according to the head motion of the operator, so that the surrounding situation can be observed without moving the moving body.

[0068] The server and the remote end communicate through a wireless local area network. The follow-up gimbal camera receives the signal of the IMU and performs corresponding actions. The follow-up gimbal camera also sends the collected binocular images to the embedded device of the server through the wireless local area network.

[0069] The embedded device of the server is provided with the required algorithm, which is composed of a SLAM algorithm part and a target tracking algorithm part. The collected binocular images are sent to the SLAM algorithm part and the target tracking algorithm part for processing. The SLAM algorithm part outputs a pose for each input binocular image. For a group of input images, a group of poses, i.e. the motion trajectory of the moving body, is output.

[0070] Specifically:

[0071] As shown in Figure 1 a dynamic scene binocular vision SLAM method based on target tracking, an embedded device is used at the remote end to control the follow-up gimbal camera installed on the moving body and collect images. An embedded device is used at the server to deploy an algorithm. The algorithm uses a SLAM algorithm combined with target tracking to exclude the interference of the moving body's own blocking mechanism and dynamic objects in the field of view. The SLAM method includes the following steps:

[0072] Step 1, SLAM algorithm binocular initialization and target detection network reading training model information;

[0073] Step 2, remote follow-up collection of binocular color video images;

[0074] The service end operator controls the rotation of the gimbal of the follow-up gimbal camera using the VR device carrying the IMU. The Raspberry Pi is used at the far end to connect the follow-up gimbal camera, control the follow-up gimbal camera to collect binocular color video images, and transmit the binocular color video images back to the service end through ROS to complete the collection of binocular color video images.

[0075] The IMU measurement model on the VR device is:

[0076]

[0077] wherein, is the angular velocity, ω b is the true value of the angular velocity, b g is the angular velocity bias, n g is the angular velocity noise, is the acceleration, q bw is the rotation matrix from world to body, a w is the true value of the acceleration, g w is the gravitational acceleration, b a is the acceleration bias, n a is the acceleration noise;

[0078] The conversion relationship between the angular velocity and the Euler angle is:

[0079]

[0080] wherein, φ is the yaw angle, θ is the pitch angle, and ψ is the roll angle; ω x , ω y , and ω z are the vectors of the angular velocity decomposed in the xyz axes, respectively.

[0081] Then the input pulse numbers of the two servo motors of the follow-up gimbal camera are:

[0082]

[0083] wherein, P ⊥ and P ∥ are the input pulses of the two servo motors of the follow-up gimbal camera, and DEG is the step angle.

[0084] Step 3, the target tracking network (based on the target detection network of yolov5s and Kalman filtering) detects and tracks the targets in the binocular color video images, and sends the results to the SLAM thread for subsequent processing.

[0085] The target detection and tracking process is as follows: after the binocular image is received by the target tracking network, the target object in the image is detected. The target object includes dynamic objects (people, vehicles, and flames) and the occlusion objects of the moving body itself (first-person excavator arm and first-person fire water gun). Because these target objects are dynamic for the SLAM algorithm, the feature points thereon will interfere with the positioning accuracy of the SLAM algorithm. After detection, the target detection network outputs the rectangular frame information (center point coordinates, width, and height) of the target object. A set of Kalman filters is used to track these rectangular frames: if the rectangular frame is not previously tracked, a new Kalman filter is initialized for it; if the rectangular frame information matches the rectangular frame tracked by the previous Kalman filter, the Kalman filter is updated. That is, each Kalman filter tracks a rectangular frame. The matching method is IOU matching. Finally, for each input binocular image, the target tracking part outputs a set of rectangular frame information of the target object for the SLAM algorithm. That is, target detection + Kalman filter tracking = target tracking.

[0086] As shown in Figure 2 , specifically comprising the following steps:

[0087] 3.1, after the YOLOv5s initialization model is loaded, the target tracking thread enters a loop when it is detected that there is an image in the queue. After the image is sent to YOLOv5s, YOLOv5s outputs a set of rectangular frames.

[0088] 3.2, memory for a new target is allocated for a Kalman filter, and the state parameters and observation parameters are initialized using the current rectangular frame information;

[0089] The specific predicted state quantities are cx, cy, w, h, dx, dy, dw, and dh, which respectively represent the horizontal and vertical coordinates of the rectangular frame center point, the width and height, and the differential quantities of these quantities.

[0090] The observation quantities are cx, cy, w, and h, which respectively represent the horizontal and vertical coordinates of the rectangular frame center point, the width, and the height. That is:

[0091]

[0092] Among them,

[0093]

[0094] In addition, for the special scene occlusion objects "first-person excavator arm" and "first-person fire water gun" on the moving body, due to the fixedness of their starting positions, the initial positions are preset, that is:

[0095]

[0096] 3.3, For the old target of the output result of step 3.1, use the predicted value output by the Kalman filter to perform IOU matching on all outputs of step 3.1, that is:

[0097]

[0098] 3.4, if the maximum IOU matching value in 3.3 is greater than 0.4, it is considered to be a successful match; the maximum matching rectangular frame is the target tracking result, which is used to correct the Kalman filter.

[0099] 3.5, if the maximum IOU matching value in 3.3 is less than 0.4, it is considered to be a failed match. Use the predicted value information provided by the Kalman filter as the final tracking rectangular frame. At the same time, the tracking loss flag is incremented, and when the tracking loss flag is greater than 12, it means that the target has been lost for 12 frames, at which point the target is determined to be lost, and the memory of the Kalman filter corresponding to the target is released.

[0100] 3.6, if in 3.1, YOLOv5s fails to detect any object due to image blur and fails to provide any rectangular frame. The tracking loss flag of all Kalman filters is incremented, and when the tracking loss flag is greater than 12, it means that the target has been lost for 12 frames, at which point the target is determined to be lost, and the memory of the Kalman filter corresponding to the target is released. The remaining Kalman filters each provide a predicted value as the final rectangular frame for target tracking.

[0101] 3.7, repeat the above steps to track all potential new dynamic objects and old objects; and transmit the final output rectangular frame and target label information to the SLAM algorithm part for subsequent processing.

[0102] Step 4, the SLAM thread performs grayscale and other preprocessing on the binocular color image, extracts the corner points, and then uses optical flow to match the binocular key points and calculate the depth information;

[0103] Specifically:

[0104] 4.1 Store the received image in the queue, and when the SLAM thread calls the image, it will first count the number of images in the queue. If the number is 0, it will wait; if the number is greater than 1, it will delete the redundant image data one by one and only use the latest image, making the system closest to real-time. The advantage of this is that it allows the system to process images that are close to real-time, but the disadvantage is that the interval between each frame of image may not be consistent.

[0105] 4.2 After grayscale, limit the contrast of the binocular RGB image and perform adaptive histogram equalization (CLAHE); the specific steps include:

[0106] 4.2.1, adjust the size of the image after grayscale;

[0107] 4.2.2, image block filling, calculate the histogram of each block Figure Two histogram array;

[0108] 4.2.3, contrast limited histogram processing; weaken the frequency of high frequency gray value, make the image gray balance, the extreme bright area and the extreme dark area are limited;

[0109] 4.2.4, generate histogram equalization mapping function using limited histogram;

[0110] 4.2.5, judge the position of the pixel point, and use bilinear interpolation to get the final image.

[0111] 4.3, extract the FAST corner points of the processed image, and perform quadtree uniformization processing on the corner points;

[0112] Specifically, the following steps are included:

[0113] 4.3.1, select a pixel p in the image, assuming its brightness is I p ;

[0114] 4.3.2, set a threshold T, select 16 pixel points on the circle with a radius of 3 centered at pixel p;

[0115] 4.3.3, if there are continuous N points on the selected circle whose brightness is greater than I p +T or less than I p -T, then the pixel p can be considered as a FAST corner point;

[0116] 4.3.4, repeat 3.1 to 3.3, perform the same operation on each pixel to get the FAST corner points of the entire image;

[0117] 4.3.5, take the entire image as a node node;

[0118] 4.3.6, traverse all nodes node;

[0119] 4.3.7, if the number of FAST corner points in node is greater than or equal to 1, split the node into 4 nodes; if the number of FAST corner points in node is 0, delete the node;

[0120] 4.3.8, repeat 3.6 and 3.7 until the number of nodes is greater than 200;

[0121] 4.3.9, keep 1 FAST corner point for each node and delete the remaining corner points.

[0122] 4.4, use LK optical flow method to track and match the corner points; this algorithm is based on the assumption of gray scale invariance, that is:

[0123] I(x+dx,y+dy,t+dt) = I(x,y,t)

[0124] where I is the gray value, x, y, t are the horizontal, vertical coordinates and time of the corner point respectively.

[0125]

[0126]

[0127]

[0128] Let I x , I y , I t ; u is the velocity of the pixel in x direction, let u be the velocity of the pixel in y direction, let v be the velocity of the pixel in y direction. Then the above formula becomes:

[0129]

[0130] If the pixels in the window have the same motion, set the window size to w x w, then there are w 2 equations:

[0131]

[0132] Solve the overdetermined equations That is

[0133]

[0134] In this way, the left eye corner point can be obtained in the approximate position of the right eye, and the left and right eye corner points can be matched.

[0135] 4.5, use the triangulation algorithm to calculate the depth of the corner point;

[0136] Let R, t be the external parameters, u be the 2D point in the pixel coordinate system, and X be the 3D point to be solved.

[0137] Then su = PX, where s is the depth, and P is the projection matrix P = KT

[0138] u ^ su = u ^ PX = 0

[0139]

[0140]

[0141] ​Only the first two equations are linearly independent, and the solution X needs an additional equation, i.e.:

[0142]

[0143] SVD can be used to solve X, i.e. to obtain 3D point coordinates.

[0144] Step 5, using the target tracking result in step 3, classifying the key points into static points and to-be-screened points;

[0145] Specifically, using the rectangular frame information provided by the target tracking network, the corner points in the image are classified into two categories, one category is the points outside the rectangular frame, which are static points; one category is the points inside the rectangular frame, which are to-be-screened points.

[0146] Step 6, performing epipolar geometric constraint on the static points in step 5 to remove redundant outliers, and using PnP algorithm to solve the pose information;

[0147] Specifically, the static points classified in step 5 are used to solve the pose by PnP. Let the homogeneous coordinates be 3D point P(X, Y, Z, 1) and 2D point p(u, v, 1). Let the transformation be p = TP, i.e.:

[0148]

[0149]

[0150] Let

[0151] The above formula is transformed into That is

[0152]

[0153] Then 6 pairs of points need to be solved.

[0154] When actually solving the pose, not only 6 pairs of points are used, which will waste a lot of corner point information. In practice, RANSAC combined with PnP is often used to transform it into a least squares problem, and the value that makes the loss function minimum is the pose of the static point.

[0155] Step 7, using a dynamic point screening method to screen out the real dynamic points from the to-be-screened points in step 5; the dynamic point screening method includes the following steps:

[0156] 7.1, using epipolar geometric constraint to obtain the essential matrix E of the static points,

[0157] Specifically, since the to-be-screened points are basically from potential dynamic objects, the motion trend thereof is inconsistent with that of static points, and thus the depth cannot be accurately calculated in step 4.5 triangulation. Therefore, epipolar geometry constraints are used, and 2D points and binocular external parameters are used to solve the pose, as shown in the following formula. Figure 3

[0158] Let s be the depth, p1 and p2 be two frames of matched 2D points, P be a matched 3D point, K be an internal parameter matrix, R be a rotation matrix, and t be a translation matrix. Then, the following formula is obtained:

[0159] s1p1=KP、s2p2=K(RP+t)

[0160] Scale equivalence: p1;KP、p2;K(RP+t)

[0161] Let the normalized camera coordinate system x1=K -1 p1、x2=K -1 p2, then:

[0162] x2;K -1 K(RP+t);RP+t;Rx1+t

[0163] x2;Rx1+t

[0164] t ^ x2;t ^ Rx1

[0165]

[0166]

[0167] wherein E=t^R is a fundamental matrix.

[0168] According to the epipolar constraint , the following formula is obtained:

[0169]

[0170]

[0171] e1u1u2+e2u1v1+e3u1+e4u2v1+e5v1v2+e6v1+e7u2+e8v2+e9=0

[0172] Let The above formula is converted into:

[0173]

[0174] Due to the scale equivalence, the degree of freedom can be reduced by 1, and only 8 pairs of points are needed to solve E.

[0175] ​7.2, using SVD singular value decomposition on the essential matrix E of step 7.1 to calculate the rotation matrix R and translation vector t:

[0176]

[0177]

[0178] wherein R1, R2 are both a possible rotation matrix, are both a possible translation matrix.

[0179] Two groups of R1, R2, are obtained, and the solution with a depth greater than zero is taken as the pose of the point to be screened.

[0180] 7.3, using the pose information in step 7.2 as a reference, classifying the points to be screened in step 5 into dynamic points and static points:

[0181] According to the essential matrix E, the epipolar line l1 = [X, Y, Z] of the point x1 is obtained. T = Ex1.

[0182] The distance formula of the point x2 to the epipolar line l1 is:

[0183]

[0184] wherein x1 and x2 are a pair of matched points, F is the fundamental matrix, X, Y are the parameters of the epipolar line, and T represents the transpose of the matrix.

[0185] If D is greater than the threshold value D th , it is considered to be a dynamic point, and if D is less than the threshold value D th , it is considered to be a static point.

[0186] Step 8, deleting the dynamic points in step 7, and deleting the co-visible relationship of the current frame and the point, and merging the static points;

[0187] Specifically, using the pose solved in step 6 as a reference, and according to the pose of the point to be screened solved in step 7, the dynamic corner points are screened. The dynamic corner points are deleted, and the remaining points inside the rectangular frame are merged with the static points to achieve the purpose of removing the interference of dynamic points.

[0188] Step 9, updating the information of the current frame, including key points, map points and their co-visible relationships; based on the pose, local optimization is performed; if the current frame is a key frame, loop detection is performed.

[0189] Embodiment

[0190] As Figure 4As shown, the remote end uses a follow-up pan-tilt camera and a Raspberry Pi to constitute a working end, the server uses an embedded device to deploy an algorithm, and the whole set of algorithms communicates through ROS. The Raspberry Pi system is Ubuntu 18.04, ROS; the embedded device is configured as Intel i5-10400, Ubuntu 18.04, ROS, and the follow-up pan-tilt camera is composed of an oak camera and two rudders. The remote end carrier experimental car is an optional item.

[0191] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present application, but not to limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacement for part or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A dynamic scene binocular vision SLAM method based on target tracking, characterized in that: The remote end uses an embedded device to control a follow-up pan-tilt camera mounted on a moving body and collect images, and the server side uses an embedded device to deploy an algorithm, which uses a SLAM algorithm combined with target tracking to exclude the shielding mechanism of the moving body itself and the interference of dynamic objects in the field of view; The SLAM method comprises the following steps: Step 1, SLAM algorithm binocular initialization and target detection network reading training model information; Step 2, remote follow-up collection of binocular color video images; Step 3, target tracking network performs target detection and tracking on the binocular color video images, and sends the results to the SLAM thread for subsequent processing; Step 4, the SLAM thread pre-processes the binocular color images, extracts the corner points, matches the binocular key points using optical flow, and calculates the depth information; Step 5, using the target tracking results in step 3, the key points are classified into static points and points to be screened; Step 6, the epipolar geometric constraint is used to remove the redundant outliers of the static points in step 5, and the PnP algorithm is used to solve the pose information; Step 7, for the points to be screened in step 5, a dynamic point screening method is used to screen out the real dynamic points; Step 8, the dynamic points in step 7 are deleted, and the co-visibility relationship between the current frame and the points is deleted, and the static points are merged; Step 9, update the information of the current frame, including key points, map points and their co-visibility relationship; based on the pose, local optimization is performed; if the current frame is a key frame, loop detection is performed.

2. The dynamic scene binocular vision SLAM method based on target tracking according to claim 1, characterized in that: In step 2, the service side operator controls the rotation of the pan-tilt of the follow-up pan-tilt camera using a VR device carrying an IMU, and the remote end uses a Raspberry Pi to connect the follow-up pan-tilt camera, control the follow-up pan-tilt camera to collect binocular color video images, and return the binocular color video images to the server side through ROS to complete the collection of binocular color video images; the IMU measurement model on the VR device is: where, is the angular velocity, ω b is the angular velocity true value, b g is the angular velocity bias, n g is the angular velocity noise, is the acceleration, q bw is the rotation matrix from world to body frame, a w is the acceleration true value, g w is the gravitational acceleration, b a is the acceleration bias, n a is the acceleration noise; The conversion relationship between angular velocity and Euler angle is: wherein φ is a yaw angle yaw, θ is a pitch angle pitch, and ψ is a roll angle roll; ω x , ω y , and ω z are vectors of angular velocity decomposed in the xyz axes, respectively; Then the input pulse numbers of the two servos of the follow-up pan-tilt camera are: Where, P ⊥ and P ∥ are the input pulses of the two steering motors of the follow-up holder camera, and DEG is the step angle.

3. The dynamic scene binocular vision SLAM method based on target tracking according to claim 1, wherein in step 3, the target tracking network is a target detection network based on yolov5s and Kalman filtering, and specifically comprises the following steps: 3.1, the target detection network detects the local motion mechanism in the image to obtain a group of rectangular frames; 3.2, a Kalman filter is constructed for the new target output by step 3.1, and the rectangular frame coordinate information is used for initialization; where the state quantity is set as The state transition matrix is set as Since there is no input quantity, u = 0, the observation quantity is set as The observation matrix is set as In addition, for the special scene shielding object "first person excavator arm" and "first person fire water gun" on the moving body, due to the fixedness of the starting position, the initial position is preset, that is: 3.3, track the old target output by step 3.1, and use the predicted value output by the Kalman filter to match the IOU of all outputs of step 3.1: 3.4, mark the target with a matching result greater than the threshold T in step 3.3 as a successful tracking result, use its rectangular frame as the final result, and correct the Kalman filter; 3.5, if the matching result of the target in step 3.3 is less than the threshold T, the tracking failure flag is incremented, if the tracking failure flag is greater than the threshold F, the memory of the Kalman filter is released, otherwise the predicted value output by the Kalman filter is used as the final result; 3.6, if the target detection network in step 3.1 does not output to any rectangular box, the tracking failure flag of all existing targets is incremented, if the tracking failure flag is greater than F, the memory of the corresponding Kalman filter is released, otherwise the predicted value output by the Kalman filter is used as the final result. 4.The dynamic scene binocular vision SLAM method based on target tracking according to claim 1, characterized in that: In step 7, the dynamic point screening method Comprises the following steps: 7.1, using epipolar geometry constraint to calculate the essential matrix E of the static point, 7.2, using SVD singular value decomposition to calculate the rotation matrix R and the translation vector t of the essential matrix E in step 7.1: wherein, R1, R2 are both a possible rotation matrix, are both a possible translation matrix; R1, R2、 The solution with a midpoint depth greater than zero is taken as the pose of the point to be screened. 7.3, using the pose information in step 7.2 as a reference, classifying the points to be screened in step 5 into dynamic points and static points: The epipolar line l1 = [X, Y, Z] for the point x1 is obtained from the essential matrix E as T = Ex1; The distance formula of point x2 to epipolar line l1 is: Where x1 and x2 are a pair of matching points, F is the fundamental matrix, X and Y are the parameters of the epipolar line, and T represents the transpose of the matrix; If D is greater than a threshold D th then the point is considered a dynamic point, and if D is less than a threshold D th then the point is considered a static point.

Images