A rolling robot binocular vision slam data preprocessing method

Abstract

The application discloses a kind of rolling robot binocular vision SLAM data preprocessing methods, comprising: S01, measure the current three-axis Euler angle of rolling robot, obtain the current left and right eye image of rolling robot;S02, based on the inherent shaking cycle of rolling robot, yaw angle data in Euler angle is executed sliding window mean filtering, and the electronic image stabilization correction quantity is obtained from the reading of roll angle and pitch angle and the filtering result of yaw angle, according to the correction quantity and the cutting amount, perspective transformation and cutting are executed in turn, and the stabilized image is obtained;S03, the camera internal parameter corresponding to the stabilized image is obtained according to the cutting amount, and the relative external parameter between the cameras corresponding to the left and right eye stabilized image is obtained according to the correction quantity, to obtain the data that can be used for visual SLAM.The application can provide stable and reliable monitoring picture for rolling robot, and visual SLAM can be realized based on stable picture, which can be widely applied in the field of visual perception positioning of robot with strong shaking characteristics.

Description

Technical Field

[0001] This invention relates to the field of robot visual perception and localization, and in particular to a binocular visual SLAM data preprocessing method for a rolling robot. Background Technology

[0002] Rolling robots are a novel type of mobile robot. They feature a fully enclosed shell and are driven by an internal pendulum mechanism for forward and backward movement and lateral turning. They offer excellent protection and obstacle-crossing capabilities, are not prone to tipping over, and can be easily extended to include amphibious capabilities, making them ideal for security patrols, reconnaissance, and other tasks in harsh terrains and environments. However, the unavoidable significant swaying during movement makes it difficult for vision systems to obtain stable and comfortable monitoring images, and also severely impacts the normal operation of conventional visual SLAM (Simultaneous Localization and Mapping) systems. Furthermore, the compact design makes it difficult to add an additional gimbal for visual image stabilization. Summary of the Invention

[0003] In view of this, the present invention provides a simple and practical binocular vision SLAM data preprocessing method for rolling robots to ensure that the rolling robot can still obtain a stable and reliable monitoring image under shaking conditions, and to obtain the camera intrinsic parameters and relative extrinsic parameters between cameras corresponding to the image after stabilization, so that conventional vision SLAM systems can work normally using the image after stabilization.

[0004] The rolling robot on which this invention is based includes a rolling shell, a horizontally arranged main shaft between the left and right side walls of the shell, the main shaft being rigidly connected to the side walls, the main shaft being connected to the shell by bearings, an attitude sensor being rigidly connected to the main shaft, and a camera being rigidly connected to the outer side of each of the left and right side walls.

[0005] This invention provides a binocular vision SLAM data preprocessing method for a rolling robot, based on the aforementioned rolling robot, comprising the following steps:

[0006] S01. Measure the current three-axis Euler angles of the rolling robot using an attitude sensor, and acquire the current left and right eye images of the rolling robot using a camera;

[0007] S02. Based on the inherent swaying period of the rolling robot, the yaw angle data in the Euler angles is subjected to sliding window mean filtering. The electronic image stabilization correction amount is obtained from the readings of the roll angle and pitch angle in the Euler angles and the yaw angle filtering result. The perspective transformation is performed on the left and right eye images according to the correction amount. Then, the image after perspective transformation is cropped according to the set cropping amount to obtain the stabilized image.

[0008] S03. Calculate the camera intrinsic parameters corresponding to the image after stabilization based on the cropping amount, and calculate the relative extrinsic parameters between the cameras corresponding to the image after stabilization of the left and right eyes based on the amount to be corrected. The image after stabilization and its corresponding camera intrinsic parameters and relative extrinsic parameters between the cameras are the data used for visual SLAM.

[0009] Preferably, in step S02, the calculation method for obtaining the electronic image stabilization correction amount from the triaxial Euler angles is as follows:

[0010] θ adj =θ exp -θ now

[0011] In the formula, θ adj θ is the electronic stabilization correction value for Euler angles. exp Let θ be the expected value after electron stabilization at Euler angles. now Here are the current values ​​of the Euler angles; among them, for the roll and pitch angles in the Euler angles, the expected value after electronic stabilization is 0 degrees. For the yaw angle, the expected value after electronic stabilization needs to be obtained by sliding window mean filtering based on the inherent swaying period of the rolling robot. The calculation method is as follows:

[0012]

[0013] In the formula, yaw exp The expected value after electronic stabilization at the yaw angle. win Let n be the historical yaw angles within the sliding window, and n be the length of the sliding window, i.e., the inherent wobbling period of the rolling robot.

[0014] Preferably, in step S02, the electronic image stabilization correction quantity represented by Euler angles is first converted into a rotation matrix representation R0, and the calculation method is as follows:

[0015]

[0016] The calculation methods for each variable in the formula are as follows:

[0017] sin y =sin(yaw) adj )

[0018] cos y =cos(yaw) adj )

[0019] sin p =sin(pitch) adj )

[0020] cos p =cos(pitch) adj )

[0021] sin r =sin(roll) adj )

[0022] cos r =cos(roll) adj )

[0023] In the formula, yaw adj The pitch is the amount to be corrected for the current heading angle. adj The variable to be corrected for the current pitch angle, roll adj The current roll angle is the amount to be corrected; subsequently, a perspective transformation is performed based on the electronically stabilized image correction amount represented by the rotation matrix, calculated as follows:

[0024] P uv ′=KR0K -1 P uv

[0025] In the formula, P uv ′ Let P be the pixel coordinates of the scene after the image is stabilized. uv Let K be the known pixel coordinates of the scene before image stabilization, and K be the pre-calibrated camera intrinsic parameter matrix.

[0026] Preferably, in step S03, the offset c of the origin of the coordinate system in the camera intrinsic parameter matrix corresponding to the image after image stabilization is obtained based on the cropping amount. x ′ and c y ′ The calculation method is as follows:

[0027] c x ′ =c x -Δx

[0028] c y ′ =c y -Δy

[0029] In the formula, c x and c y Δx is the offset of the origin of the coordinate system in the pre-calibrated intrinsic parameter matrix K, Δx is the cropping amount in the x-direction of the image, that is, the x-coordinate of the top left pixel of the image after cropping in the image before cropping, and Δy is the cropping amount in the y-direction of the image, that is, the y-coordinate of the top left pixel of the image after cropping in the image before cropping.

[0030] Preferably, in step S03, the electronic image stabilization correction quantity represented by the rotation matrix R0 is first converted into a transformation matrix representation T0, and the calculation method is as follows:

[0031]

[0032] Subsequently, the relative extrinsic parameters T′ of the camera corresponding to the stabilized images of the left and right eyes are obtained based on the amount to be corrected. The calculation method is as follows:

[0033]

[0034] In the formula, T l For the pre-calibrated transformation matrix from the attitude sensor to the left eye camera, T r This is a pre-calibrated transformation matrix from the attitude sensor to the right eye camera. Further, multiplying and simplifying the various transformation matrices yields the following formula:

[0035]

[0036] In the formula, R0, R l R r Transformation matrices T0 and T1 are respectively. l T r The rotational component in t l t r Transformation matrix T l T r The translation component in the text.

[0037] The substantial effects of this invention are as follows: by utilizing the current posture obtained by the attitude sensor and the current image obtained by the camera, electronic image stabilization is achieved based on sliding window filtering and perspective projection, effectively overcoming the large and rapid shaking generated during the operation of the rolling robot and obtaining a stable and comfortable monitoring image; at the same time, the camera intrinsic parameters and relative extrinsic parameters between cameras corresponding to the stabilized image are calculated to complete the binocular visual SLAM data preprocessing of the rolling robot, so that conventional visual SLAM can directly use the stabilized image to work, effectively solving the problem of rapid image movement that conventional visual SLAM is afraid of. Attached Figure Description

[0038] Figure 1 This is a flowchart of one method of the present invention. Detailed Implementation

[0039] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0040] The present invention relates to a rolling robot, comprising a rolling shell, a horizontally arranged main shaft between the left and right side walls of the shell, the main shaft being rigidly connected to the side walls, the main shaft being connected to the shell by bearings, an attitude sensor being rigidly connected to the main shaft, and a camera being rigidly connected to the outer side of each of the left and right side walls.

[0041] This invention provides a binocular vision SLAM data preprocessing method for a rolling robot, based on the aforementioned rolling robot, such as... Figure 1 As shown, it includes the following steps:

[0042] S01. Measure the current three-axis Euler angles of the rolling robot using an attitude sensor, and acquire the current left and right eye images of the rolling robot using a camera;

[0043] S02. Based on the inherent swaying period of the rolling robot, the yaw angle data in the Euler angles is subjected to sliding window mean filtering. The electronic image stabilization correction amount is obtained from the readings of the roll angle and pitch angle in the Euler angles and the yaw angle filtering result. The perspective transformation is performed on the left and right eye images according to the correction amount. Then, the image after perspective transformation is cropped according to the set cropping amount to obtain the stabilized image.

[0044] S03. Calculate the camera intrinsic parameters corresponding to the image after stabilization based on the cropping amount, and calculate the relative extrinsic parameters between the cameras corresponding to the image after stabilization of the left and right eyes based on the amount to be corrected. The image after stabilization and its corresponding camera intrinsic parameters and relative extrinsic parameters between the cameras are the data used for visual SLAM.

[0045] As a further preferred embodiment, in step S02 above, the inherent swaying period of the rolling robot can be obtained by theoretical calculation, or by actually running it on various surfaces such as roads and grass, reading the roll angle change curve provided by the attitude sensor and calculating the average swaying period.

[0046] As a further preferred embodiment, in step S02 above, the calculation method for obtaining the electronic image stabilization correction amount from the three-axis Euler angles is as follows:

[0047] θ adj =θ exp -θ now

[0048] In the formula, θ adj θ is the electronic stabilization correction value for Euler angles. exp Let θ be the expected value after electron stabilization at Euler angles. now Here are the current values ​​of the Euler angles; among them, for the roll and pitch angles in the Euler angles, the expected value after electronic stabilization is 0 degrees. For the yaw angle, the expected value after electronic stabilization needs to be obtained by sliding window mean filtering based on the inherent swaying period of the rolling robot. The calculation method is as follows:

[0049]

[0050] In the formula, yaw exp The expected value after electronic stabilization at the yaw angle. win Let n be the historical yaw angles within the sliding window, and n be the length of the sliding window, i.e., the inherent wobbling period of the rolling robot.

[0051] As a further preferred embodiment, in step S02 above, the electronic image stabilization correction quantity represented by Euler angles can be converted into a rotation matrix representation R0 to facilitate perspective transformation. This conversion can be calculated based on the following method:

[0052]

[0053] The calculation methods for each variable in the formula are as follows:

[0054] sin y =sin(yaw) adj )

[0055] cos y =cos(yaw) adj )

[0056] sin p =sin(pitch) adj )

[0057] cos p =cos(pitch) adj )

[0058] sin r =sin(roll) adj )

[0059] cos r =cos(roll) adj )

[0060] In the formula, yaw adj The pitch is the amount to be corrected for the current heading angle. adj The variable to be corrected for the current pitch angle, roll adj This is the amount to be corrected for the current roll angle.

[0061] Alternatively, the conversion from Euler angles to a rotation matrix can also be accomplished using the AngleAxisd function provided by the Eigen library, which generates rotation vectors from Euler angles in the order of roll, pitch, and yaw, multiplies them on the left in sequence, and then assigns the final rotation vector to the rotation matrix within the Eigen framework.

[0062] As a further preferred embodiment, in step S02 above, before performing perspective transformation, it should be confirmed whether the image captured by the camera is distorted. If the image captured by the camera is distorted, image distortion correction processing should be performed before perspective transformation based on the distortion model of the pinhole camera and pre-calibrated distortion parameters. The distortion model satisfies the formula:

[0063] x distorted=x(1+k1r) 2 +k2r 4 +k3r 6 )+2p1xy+p2(r 2 +2x 2 )

[0064] y distorted =y(1+k1r) 2 +k2r 4 +k3r 6 )+p1(r 2 +2y 2 )+2p2xy

[0065] r 2 =x 2 +y 2

[0066] In the formula, the points considered are located on the camera normalization plane, and the origin of the coordinate system is at the center of the image. distorted The x-coordinate of the point before distortion removal is represented by y. distorted y represents the ordinate of the point before distortion removal, x represents the abscissa of the point after distortion removal, y represents the ordinate of the point after distortion removal, r represents the distance between the point and the origin, k1, k2, and k3 are three radial distortion parameters, and p1 and p2 are two tangential distortion parameters.

[0067] As a further preferred embodiment, in step S02 above, perspective transformation can be performed based on the electronic image stabilization correction amount R0 represented by the rotation matrix, according to the pinhole camera imaging model and camera intrinsic parameters, to obtain the perspective-transformed image. The calculation method is as follows:

[0068] P uv ′=KR0K -1 P uv

[0069] In the formula, P uv ′ Let P be the pixel coordinates of the scene after the image is stabilized. uv Let K be the known pixel coordinates of the scene before image stabilization, and K be a pre-calibrated camera intrinsic parameter matrix. Based on the pinhole camera projection model, the camera intrinsic parameter matrix K is defined as follows:

[0070]

[0071] In the formula, c x c is the offset of the horizontal coordinate when the image is transformed from the camera coordinate system to the pixel coordinate system. y f is the offset of the ordinate when the image is transformed from the camera coordinate system to the pixel coordinate system. x and f y The definition is as follows:

[0072] f x =αf

[0073] f y =βf

[0074] In the formula, α is the scaling factor of the horizontal axis when the image is transformed from the camera coordinate system to the pixel coordinate system, β is the scaling factor of the vertical axis when the image is transformed from the camera coordinate system to the pixel coordinate system, and f is the camera focal length.

[0075] The perspective transformation process can be performed by calling the warpPerspective function provided by OpenCV to complete the perspective transformation and obtain an uncropped, stable image.

[0076] As a further preferred embodiment, in step S02 above, if the adjustment range of the image during electronic image stabilization is large, black areas without data can easily appear in the image after electronic image stabilization, affecting the visual experience. Therefore, a fixed cropping amount can be set to crop out the region of interest in the center of the image, so as to minimize the surrounding black areas without data and obtain a cropped and stabilized image with better visual experience, that is, the final stabilized image.

[0077] As a further preferred embodiment, in step S03 above, the offset c of the origin of the coordinate system in the camera intrinsic parameter matrix corresponding to the image after stabilization can be calculated based on the cropping amount and the pinhole camera imaging model. x ′ and c y ′ The calculation method is as follows:

[0078] c x ′ =c x -Δx

[0079] c y ′ =c y -Δy

[0080] In the formula, Δx is the cropping amount in the x-direction of the image, that is, the x-coordinate of the top left corner pixel of the cropped image in the original image, and Δy is the cropping amount in the y-direction of the image, that is, the y-coordinate of the top left corner pixel of the cropped image in the original image.

[0081] As a further preferred embodiment, in step S03 above, the electronic image stabilization correction quantity represented by the rotation matrix R0 can be first converted into a transformation matrix representation T0 to facilitate the adjustment of relative extrinsic parameters between cameras. The conversion calculation method is as follows:

[0082]

[0083] As a further preferred embodiment, in step S03 above, the relative extrinsic parameter T′ between the cameras corresponding to the images after left and right eye stabilization can be obtained based on the electronic image stabilization correction amount T0 represented by the transformation matrix. The calculation method is as follows:

[0084]

[0085] In the formula, T l For the pre-calibrated transformation matrix from the attitude sensor to the left eye camera, T r This is a pre-calibrated transformation matrix from the attitude sensor to the right eye camera. The transformation matrix T is defined as follows:

[0086]

[0087] In the formula, R is the rotation component in the transformation matrix T, and t is the translation component in the transformation matrix T.

[0088] Furthermore, by multiplying and simplifying the various transformation matrices, we obtain the following formula:

[0089]

[0090] In the formula, R0, R l R r Transformation matrices T0 and T1 are respectively. l T r The rotational component in t l t r Transformation matrix T l T r The translation component in the text.

[0091] Thus, this embodiment of the invention has completed the electronic image stabilization of the binocular images and obtained the camera intrinsic parameters and the equivalent extrinsic parameters between cameras corresponding to the stabilized images.

[0092] The specific embodiments described herein are merely illustrative of the spirit of the invention. Those skilled in the art to which this invention pertains may make various modifications or additions to the described specific embodiments or use similar methods to substitute them, without departing from the spirit of the invention or exceeding the scope defined by the appended claims.

[0093] Although this paper uses terms such as attitude sensor, electronic image stabilization, and camera intrinsic and extrinsic parameters extensively, the possibility of using other terms is not excluded. These terms are used merely for the convenience of describing and explaining the essence of this invention; interpreting them as any additional limitation would contradict the spirit of this invention.

Claims

1. A method for preprocessing binocular vision SLAM data of a rolling robot, characterized in that, include: S01. Measure the current three-axis Euler angles of the rolling robot using an attitude sensor, and acquire the current left and right eye images of the rolling robot using a camera; S02. Based on the inherent swaying period of the rolling robot, the yaw angle data in the Euler angles is subjected to sliding window mean filtering. The electronic image stabilization correction amount is obtained from the readings of the roll angle and pitch angle in the Euler angles and the yaw angle filtering result. The perspective transformation is performed on the left and right eye images according to the correction amount. Then, the image after perspective transformation is cropped according to the set cropping amount to obtain the stabilized image. S03. Calculate the camera intrinsic parameters corresponding to the image after stabilization based on the cropping amount, and calculate the relative extrinsic parameters between the cameras corresponding to the image after stabilization of the left and right eyes based on the amount to be corrected. The image after stabilization and its corresponding camera intrinsic parameters and relative extrinsic parameters between the cameras are the data used for visual SLAM.

2. The binocular vision SLAM data preprocessing method for a rolling robot according to claim 1, characterized in that, In step S02, the calculation method for obtaining the electronic image stabilization correction amount from the three-axis Euler angles is as follows: i adj =θ exp -θ now In the formula, θ adj θ is the electronic stabilization correction value for Euler angles. exp Let θ be the expected value after electron stabilization at Euler angles. now Here are the current values ​​of the Euler angles; among them, for the roll and pitch angles in the Euler angles, the expected value after electronic stabilization is 0 degrees. For the yaw angle, the expected value after electronic stabilization needs to be obtained by sliding window mean filtering based on the inherent swaying period of the rolling robot. The calculation method is as follows: In the formula, yaw exp The expected value after electronic stabilization at the yaw angle. win Let n be the historical yaw angles within the sliding window, and n be the length of the sliding window, i.e., the inherent wobbling period of the rolling robot.

3. The binocular vision SLAM data preprocessing method for a rolling robot according to claim 2, characterized in that, In step S02, the electronic image stabilization correction quantity represented by Euler angles is first converted into a rotation matrix representation R0. The calculation method is as follows: The calculation methods for each variable in the formula are as follows: sin y =sin(yaw adj ) cos y =cos(yaw adj ) without p =sin(pitch adj ) cos p =cos(pitch adj ) its r =his(role adj ) cos r =cos(roll adj ) In the formula, yaw adj The pitch is the amount to be corrected for the current heading angle. adj The variable to be corrected for the current pitch angle, roll adj The current roll angle is the amount to be corrected; subsequently, a perspective transformation is performed based on the electronically stabilized image correction amount represented by the rotation matrix, calculated as follows: P uv ′=KR0K -1 P uv In the formula, P uv ′ Let P be the pixel coordinates of the scene after the image is stabilized. uv Let K be the known pixel coordinates of the scene before image stabilization, and K be the pre-calibrated camera intrinsic parameter matrix.

4. The binocular vision SLAM data preprocessing method for a rolling robot according to claim 1, characterized in that, In step S03, the offset c of the origin of the coordinate system in the camera intrinsic parameter matrix corresponding to the image after image stabilization is obtained based on the cropping amount. x ′ and c y ′ The calculation method is as follows: c x ′ =c x -Δx c y ′ = c y -Δy In the formula, c x and c y Δx is the offset of the origin of the coordinate system in the pre-calibrated intrinsic parameter matrix K, Δx is the cropping amount in the x-direction of the image, that is, the x-coordinate of the top left pixel of the image after cropping in the image before cropping, and Δy is the cropping amount in the y-direction of the image, that is, the y-coordinate of the top left pixel of the image after cropping in the image before cropping.

5. A binocular vision SLAM data preprocessing method for a rolling robot according to claim 1 or 4, characterized in that, In step S03, the electronic image stabilization correction quantity represented by the rotation matrix R0 is first converted into a transformation matrix representation T0. The calculation method is as follows: Subsequently, the relative extrinsic parameter T′ between the cameras corresponding to the images after stabilization of the left and right eyes is obtained based on the amount to be corrected. The calculation method is as follows: In the formula, T l For the pre-calibrated transformation matrix from the attitude sensor to the left eye camera, T r For the pre-calibrated transformation matrix from the attitude sensor to the right eye camera, R0, R l R r Transformation matrices T0 and T1 are respectively. l T r The rotational component in t l t r Transformation matrix T l T r The translation component in the text.

Images