CN-122015862-A - Industrial scene-oriented intelligent robot long-term positioning method and system

Abstract

The invention discloses an intelligent robot long-term positioning method and system for an industrial scene, which belong to the technical field of robot positioning, wherein multiple optimization factors are fused by a laser radar, a visual camera and an IMU (inertial measurement unit) multisensor of a target robot, multiple source information of the target robot is fused by a target optimization factor graph to form complementation, positioning robustness of robot positioning in a complex industrial environment is ensured, meanwhile, pose estimation of the robot is dynamically restrained by absolute pose measurement values in a target optimization factor graph, the state of the robot is judged, accumulation of integrating errors of an odometer is eliminated, accumulation drift is avoided, and in addition, a sliding window method is utilized for carrying out edge processing on a residual error optimization objective function of the robot, so that positioning resource exhaustion caused by map growth is avoided, and high precision, high reliability and long-term continuous operation of robot positioning are realized.

Inventors

• CHEN HAO
• LI JIANGTAO
• ZHANG TAO
• SHI JINZHU

Assignees

• 光象(北京)科技有限公司

Dates

Publication Date

20260512

Application Date

20260211

Claims (10)

1. 1. An intelligent robot long-term positioning method oriented to industrial scenes is characterized by comprising the following steps: acquiring a pre-constructed point cloud map and a visual marker map; Obtaining key frame visual image data of a target robot, performing mark detection on the key frame visual image data to obtain a visual mark target in the key frame visual image data based on the visual mark map, calculating a robot visual positioning result through a PnP algorithm based on the visual mark target, and performing confidence judgment on the robot visual positioning result to take the robot visual positioning result judged through the confidence judgment as an effective visual absolute pose measurement value; Acquiring key frame laser radar point cloud data of a target robot and a robot pose estimation result at the previous moment, wherein the robot pose estimation result at the previous moment is used as an initial laser radar pose estimation value, performing iterative nearest point registration on the key frame laser radar point cloud data and the point cloud map to obtain a robot point cloud registration result, and performing convergence judgment on the robot point cloud registration result to obtain a robot point cloud registration result which is judged by the convergence as an effective point cloud absolute pose measurement value; Acquiring historical robot IMU data of a target robot, generating a robot motion state based on the historical robot IMU data, generating a robot positioning state based on the effective vision absolute pose measured value and the effective point cloud absolute pose measured value, and judging a stationary state of the target robot by utilizing the robot motion state and the robot positioning state to obtain the robot state of the target robot; Establishing a target optimization factor graph according to the robot state, the effective vision absolute pose measured value and the effective point cloud absolute pose measured value of the target robot, establishing a robot residual error optimization target function of the target robot according to the target optimization factor graph, and carrying out marginalization on the robot residual error optimization target function through a sliding window method so as to solve the robot pose optimal state estimation according to the robot residual error optimization target function subjected to marginalization; And taking the optimal state estimation of the robot pose as a reference pose state of the target robot, and performing motion recursion and pose state update on the reference pose state to realize high-frequency pose state output of the target robot.
2. 2. The industrial scene-oriented intelligent robot long-term positioning method according to claim 1, wherein the point cloud map comprises three-dimensional structure coordinates of a target industrial scene, the visual marker map comprises marker information of a plurality of fixed objects in the target industrial scene, wherein the marker information of each fixed object comprises a center point three-dimensional coordinate, a corner point three-dimensional coordinate, object information and an object marker ID corresponding to each fixed object, and the center point three-dimensional coordinate and the corner point three-dimensional coordinate are coordinate values in a world system.
3. 3. The industrial scene oriented intelligent robot long-term positioning method according to claim 2, wherein obtaining key frame visual image data of a target robot, performing marker detection on the key frame visual image data to obtain a visual marker target in the key frame visual image data based on the visual marker map, and calculating a robot visual positioning result through a PnP algorithm based on the visual marker target, comprising: Acquiring a key frame identification standard, acquiring original key frame visual image data through a visual camera of the target robot according to the key frame identification standard, and performing image preprocessing on the original key frame visual image data to obtain key frame visual image data; Performing mark detection on the key frame visual image data based on the visual mark map to detect all fixed objects including visual marks from the key frame visual image data as visual mark targets, and integrating each visual mark target into a visual mark target sequence according to the object mark ID; Acquiring the center point three-dimensional coordinates and the corner point three-dimensional coordinates of each visual marker object in the visual marker object sequence from the visual marker map, and extracting corner point two-dimensional coordinates of each visual marker object in the visual marker object sequence from the key frame visual image data, wherein the corner point two-dimensional coordinates are coordinate values under a visual camera coordinate system of the target robot; performing one-to-one correspondence on corner three-dimensional coordinates and corner two-dimensional coordinates corresponding to each visual marking target in the visual marking target sequence to form corner coordinate pairs, and integrating each corner coordinate pair corresponding to each visual marking target in the visual marking target sequence to form a corner coordinate point set; And calculating a rigid body transformation matrix of the visual camera coordinate system of the target robot relative to the world coordinate system for the angular point coordinate point set by utilizing a PnP algorithm, so as to calculate a robot visual positioning result of the target robot by utilizing the rigid body transformation matrix of the visual camera coordinate system of the target robot relative to the world coordinate system.
4. 4. The industrial scene oriented intelligent robot long-term localization method of claim 3, wherein performing a confidence determination on the robot vision localization result to take the robot vision localization result determined by the confidence determination as an effective vision absolute pose measurement value comprises: Based on the robot vision positioning result, carrying out two-dimensional re-projection on each vision marking target in the vision marking target sequence to obtain angular point re-projection two-dimensional coordinates of each vision marking target under a vision camera coordinate system of the target robot; Performing error calculation on angular point re-projection two-dimensional coordinates of each angular point corresponding to each visual mark target and corresponding angular point two-dimensional coordinates to obtain angular point re-projection errors of each angular point corresponding to each visual mark target, and respectively averaging the angular point re-projection errors of each angular point to each visual mark target to obtain average re-projection errors of each visual mark target; Acquiring a preset maximum re-projection error threshold, and calculating a mark confidence coefficient corresponding to each visual mark target in the visual mark target sequence based on the average re-projection error and the maximum re-projection error threshold of each visual mark target according to the following formula (1): (1) Wherein, the Representing the average re-projection error corresponding to the visual marker object, Representing the maximum re-projection error threshold, The function is solved for the minimum value, Representing the mark confidence corresponding to the visual mark target; Calculating the perimeter of a marking pixel corresponding to each visual marking target according to the two-dimensional coordinates of each corner point corresponding to each visual marking target under the visual camera coordinate system of the target robot, and calculating the marking visual angle weight corresponding to each visual marking target based on the visual positioning result of the robot; for each visual marker in the visual marker sequence, calculating a marker weight value corresponding to each visual marker in the visual marker sequence according to the following formula (2) based on the marker pixel perimeter and the marker viewing angle weight corresponding to each visual marker: (2) Wherein, the Representing the perimeter of the marking pixels corresponding to the visual marking target, Representing the marking visual angle weight corresponding to the visual marking target, Representing a marking weight value corresponding to the visual marking target; performing weighted average calculation on the marking confidence coefficient and the marking weight value corresponding to each visual marking target in each visual marking target sequence to obtain single-frame robot visual positioning result confidence coefficient; Calculating the corresponding single-frame robot visual positioning result confidence coefficient for each key frame respectively, and carrying out exponential moving average filtering on the single-frame robot visual positioning result confidence coefficient based on each key frame so as to obtain the final robot visual positioning result confidence coefficient of the target robot; Acquiring a preset confidence coefficient threshold value, and judging the confidence coefficient of the final robot visual positioning result of the target robot by utilizing the confidence coefficient threshold value to obtain a judging result; if the judging result is that the confidence coefficient of the final robot vision positioning result is lower than the confidence coefficient threshold value, the robot vision positioning result of the target robot is considered to be not judged by the confidence coefficient, the robot vision positioning result is taken as an invalid positioning result, and output is not carried out; And if the judging result is that the confidence coefficient of the final robot vision positioning result is not lower than the confidence coefficient threshold value, judging that the robot vision positioning result of the target robot passes through the confidence coefficient judgment, and outputting the robot vision positioning result as an effective vision absolute pose measured value.
5. 5. The industrial scene-oriented intelligent robot long-term positioning method according to claim 1, wherein obtaining key frame laser radar point cloud data of a target robot and a robot pose estimation result at a previous moment, wherein the robot pose estimation result at the previous moment is used as an initial laser radar pose estimation value, and performing iterative closest point registration on the key frame laser radar point cloud data and the point cloud map to obtain a robot point cloud registration result, comprises: acquiring a key frame identification standard, acquiring original key frame laser radar point cloud data through a laser radar of the target robot according to the key frame identification standard, and downsampling the original key frame laser radar point cloud data by utilizing a voxel grid filtering method to acquire key frame laser radar point cloud data; Placing each point in the key frame laser radar point cloud data in the point cloud map, searching out a map point with the nearest Euclidean distance to each point in the key frame laser radar point cloud data in the point cloud map as a nearest map point, and matching each point in the key frame laser radar point cloud data with the corresponding nearest map point as a point pair; Acquiring a preset point pair matching rule, screening each point pair based on the point pair matching rule, and selecting each point pair conforming to the point pair matching rule as an effective point pair; Constructing a least square function based on each effective point pair, and solving a rigid body transformation matrix of a laser radar coordinate system of the target robot relative to a world coordinate system through the least square function; acquiring a robot pose estimation result at the previous moment of the current key frame, taking the robot pose estimation result at the previous moment as an initial laser radar pose estimation value, and updating the initial laser radar pose estimation value based on a rigid body transformation matrix of a laser radar coordinate system of a target robot relative to a world coordinate system and the initial laser radar pose estimation value to obtain a current laser radar pose estimation value; And carrying out iterative updating on the current laser radar pose estimation value by using an iterative closest point registration algorithm, so as to take the last updated current laser radar pose estimation value as a robot point cloud registration result.
6. 6. The industrial scene oriented intelligent robot long-term localization method of claim 5, wherein performing convergence determination on the robot point cloud registration result to take the robot point cloud registration result determined by convergence as an effective point cloud absolute pose measurement value comprises: Extracting Euclidean distance of each effective point pair for each effective point pair, screening out the maximum value of the Euclidean distance of each effective point pair as the maximum effective matching distance, and calculating the average square effective matching distance of each effective point pair based on the Euclidean distance of each effective point pair; obtaining a preset maximum matching distance threshold and an average square matching distance threshold, comparing the maximum matching distance threshold with the maximum effective matching distance to obtain a first comparison result, comparing the average square effective matching distance with the average square matching distance threshold to obtain a second comparison result, and performing distance convergence judgment on the robot point cloud registration result according to the first comparison result and the second comparison result to obtain a distance convergence judgment result; acquiring a preset target robot standard height interval, extracting a corresponding target robot height value from the robot point cloud registration result, and carrying out high convergence judgment on the target robot height value according to the target robot standard height interval to obtain a high convergence judgment result; acquiring a preset target robot standard angle interval, extracting a corresponding target robot pitch angle and a corresponding target robot roll angle based on the robot point cloud registration result, and performing angle convergence judgment on the target robot pitch angle and the target robot roll angle according to the target robot standard angle interval to obtain an angle convergence judgment result; And taking the distance convergence determination result, the height convergence determination result and the angle convergence determination result as convergence determination results, and taking the robot point cloud registration result which is determined by convergence determination result as an effective point cloud absolute pose measurement value.
7. 7. The industrial scene oriented intelligent robot long-term positioning method of claim 1, wherein obtaining historical robot IMU data of a target robot, generating a robot motion state based on the historical robot IMU data, generating a robot positioning state based on the effective vision absolute pose measurement value and the effective point cloud absolute pose measurement value, and performing stationary state determination on the target robot by using the robot motion state and the robot positioning state to obtain a robot state of the target robot, comprising: For the current moment, acquiring historical robot IMU data of a target robot, wherein the historical robot IMU data are robot IMU data of each historical moment, and the historical robot IMU data comprise robot acceleration data and robot angular velocity data; Acquiring a preset static acceleration threshold value and a static angular velocity threshold value, calculating average values of the robot acceleration data and the robot angular velocity data at each moment in the historical robot IMU data to obtain a historical robot acceleration average value and a historical robot angular velocity average value, judging the acceleration state of the historical robot acceleration average value by utilizing the static acceleration threshold value to obtain a robot acceleration state, and judging the angular velocity state of the historical robot angular velocity average value by utilizing the static angular velocity threshold value to obtain a robot angular velocity state; Integrating the acceleration state and the angular velocity state of the robot to form a robot motion state of a target robot; Acquiring the effective vision absolute pose measurement value and/or the effective point cloud absolute pose measurement value corresponding to the two key frames closest to the current moment, and differencing the effective vision absolute pose measurement value and/or the effective point cloud absolute pose measurement value corresponding to the two key frames to obtain a positioning result position change amount and a positioning result pose change amount; Acquiring a preset robot positioning state static standard, and judging the positioning state of the position variable quantity of the positioning result and the posture variable quantity of the positioning result so as to obtain the robot positioning state of the target robot; and judging the stationary state of the target robot based on the robot motion state and the robot positioning state to obtain the robot state of the target robot.
8. 8. The industrial scene oriented intelligent robot long-term positioning method according to claim 1, wherein establishing a target optimization factor graph according to the robot state, the effective vision absolute pose measurement value and the effective point cloud absolute pose measurement value of a target robot to establish a robot residual error optimization objective function of the target robot according to the target optimization factor graph, and performing marginalization processing on the robot residual error optimization objective function by a sliding window method to solve a robot pose optimal state estimation according to the marginalized robot residual error optimization objective function, comprising: defining state variables for a target robot, wherein the state variables comprise a robot position vector, a robot gesture quaternion, a robot speed vector and a robot IMU bias; Acquiring preset robot pose priori data and robot IMU bias parameters, pre-integrating the historical robot IMU data of a target robot to obtain IMU pre-integration factors, generating corresponding absolute pose constraint factors based on the effective vision absolute pose measurement values and/or the effective point cloud absolute pose measurement values, generating corresponding vertical pose constraint and vertical velocity constraint for the target robot based on the robot pose priori data, taking the vertical pose constraint as a pose priori factor, taking the vertical velocity constraint as a velocity priori factor, and generating IMU bias smoothing factors based on the robot IMU bias parameters; Taking the state variable of the target robot as a state node, and taking the IMU pre-integration factor, the absolute pose constraint factor, the pose priori factor, the speed priori factor and the IMU bias smoothing factor as optimization factors to establish a pre-target optimization factor graph according to the state node and the optimization factors; performing state constraint on the pre-target optimization factor graph according to the robot state of the target robot to generate a target optimization factor graph; generating a covariance matrix for each optimization factor based on the target optimization factor graph, weighting the residual error of each optimization factor based on the covariance matrix of each optimization factor to construct a nonlinear least square function of the residual error of each optimization factor, and taking the nonlinear least square function as a robot residual error optimization target function; obtaining a preset maximum capacity of a sliding window, carrying out marginalization processing on the target optimization factor graph based on the maximum capacity of the sliding window, and screening and eliminating all state nodes in the target optimization factor graph by utilizing a fixed hysteresis marginalization strategy to obtain a robot residual error optimization objective function for finishing marginalization processing; And solving an optimal robot state value for a robot residual error optimization objective function subjected to marginalization processing by using a nonlinear optimization method, and taking the optimal robot state value as the optimal state estimation of the robot pose.
9. 9. The industrial scene oriented intelligent robot long-term positioning method according to claim 1, wherein taking the robot pose optimal state estimate as a reference pose state of a target robot, and performing motion recursion and pose state update on the reference pose state to realize high-frequency pose state output of the target robot, comprises: acquiring real-time robot IMU data of a target robot, and extracting the current robot position, the current robot gesture and the current robot speed of the target robot at the current moment from the real-time robot IMU data; Taking the optimal state estimation of the robot pose as a reference pose state of a target robot, and performing position recursion, pose recursion and speed recursion on the reference pose state based on the current robot position, the current robot pose and the current robot speed to obtain a position recursion result, a pose recursion result and a speed recursion result; And updating the reference pose state according to the position recursion result, the pose recursion result and the speed recursion result to obtain a pose state estimation value of the robot at the current moment, and outputting the pose state estimation value of the robot at the current moment in a high frequency mode according to the output frequency of the target unmanned aerial vehicle to the IMU data.
10. 10. An industrial scene-oriented intelligent robot long-term positioning system, which is characterized in that the industrial scene-oriented intelligent robot long-term positioning system is applied to the industrial scene-oriented intelligent robot long-term positioning method as claimed in any one of claims 1 to 9, and comprises the following steps: the point cloud and visual map acquisition unit is used for acquiring a pre-constructed point cloud map and a pre-constructed visual marker map; The visual absolute pose measuring unit is used for acquiring key frame visual image data of a target robot, carrying out mark detection on the key frame visual image data to obtain a visual mark target in the key frame visual image data based on the visual mark map, calculating a robot visual positioning result through a PnP algorithm based on the visual mark target, and carrying out confidence degree judgment on the robot visual positioning result to take the robot visual positioning result judged through the confidence degree as an effective visual absolute pose measuring value; The system comprises a point cloud absolute pose measurement unit, a target robot position and pose measurement unit and a target robot position and pose measurement unit, wherein the point cloud absolute pose measurement unit is used for acquiring key frame laser radar point cloud data of the target robot and a robot position and pose estimation result at the last moment, the robot position and pose estimation result at the last moment is used as an initial laser radar position and pose estimation value, iterative nearest point registration is carried out on the key frame laser radar point cloud data and the point cloud map to obtain a robot point cloud registration result, convergence judgment is carried out on the robot point cloud registration result, and the robot point cloud registration result judged through the convergence is used as an effective point cloud absolute pose measurement value; The robot state judging unit is used for acquiring historical robot IMU data of the target robot, generating a robot motion state based on the historical robot IMU data, generating a robot positioning state based on the effective vision absolute pose measured value and the effective point cloud absolute pose measured value, and judging the static state of the target robot by utilizing the robot motion state and the robot positioning state to obtain the robot state of the target robot; The pose optimal state estimation unit is used for establishing a target optimization factor graph according to the robot state of the target robot, the effective vision absolute pose measured value and the effective point cloud absolute pose measured value, establishing a robot residual error optimization target function of the target robot according to the target optimization factor graph, carrying out marginalization on the robot residual error optimization target function through a sliding window method, and solving the robot pose optimal state estimation according to the robot residual error optimization target function subjected to marginalization; and the pose state generating and outputting unit is used for taking the optimal state estimation of the robot pose as a reference pose state of the target robot, and performing motion recursion and pose state updating on the reference pose state so as to realize high-frequency pose state output of the target robot.

Description

Industrial scene-oriented intelligent robot long-term positioning method and system Technical Field The invention belongs to the technical field of robot positioning, and particularly relates to an intelligent robot long-term positioning method and system for industrial scenes. Background In robot navigation and autonomous operation, accurate and robust long-term positioning is a cornerstone for realizing intelligent application thereof. Especially in structured industrial scenarios, such as automated storage, flexible production lines and large logistics centers, robots need to perform uninterrupted operations in a stationary environment for up to several hours or even days, which presents serious challenges for stability, accuracy and sustainability of the positioning system. In the prior art, the main flow of robot positioning technology is mainly divided into three types, wherein the first technical scheme is a positioning method based on a single sensor, for example, only a laser radar is used for point cloud registration, or only a vision sensor is used for identifying a preset mark, the robustness of the method is seriously insufficient, the method is extremely easy to fail due to environmental interference (such as that a dynamic object shields a laser beam, and the illumination is severely changed to influence vision characteristics), and the method is difficult to adapt to a complex industrial environment. The second technical scheme is various odometer technologies, including a visual inertial odometer, a laser inertial odometer and the like, which realize continuous relative pose estimation by fusing multi-sensor data. Although the accuracy is higher in a short period, the dead reckoning is carried out by the integral increment, the accumulated error is inevitably generated, and along with the extension of the running time, the error is accumulated continuously, so that the positioning result deviates from the real position seriously, and the absolute accuracy requirement of long-term operation cannot be met. The third technical scheme is a synchronous positioning and map construction technology, which can construct an environment map and realize autonomous positioning, and although the problem of accumulated errors of the odometer is relieved to a certain extent, in long-term operation, the scale of the map can be continuously expanded, the demands for calculation memory and processing capacity are infinitely increased, and finally, the system is overwhelmed, and long-term stable operation in a real sense cannot be realized. From the foregoing, how to provide a method and a system for positioning an intelligent robot for long-term use in an industrial scenario, which can improve positioning robustness, avoid accumulated drift, and realize high precision, high reliability and long-term continuous operation, has become a subject to be studied in the field. Disclosure of Invention The invention aims to provide a long-term positioning method and a long-term positioning system for an intelligent robot facing industrial scenes, which are used for solving the problems in the prior art. In order to achieve the above purpose, the present invention adopts the following technical scheme: In a first aspect, the present invention provides an industrial scenario-oriented intelligent robot long-term positioning method, including: acquiring a pre-constructed point cloud map and a visual marker map; Obtaining key frame visual image data of a target robot, performing mark detection on the key frame visual image data to obtain a visual mark target in the key frame visual image data based on the visual mark map, calculating a robot visual positioning result through a PnP algorithm based on the visual mark target, and performing confidence judgment on the robot visual positioning result to take the robot visual positioning result judged through the confidence judgment as an effective visual absolute pose measurement value; Acquiring key frame laser radar point cloud data of a target robot and a robot pose estimation result at the previous moment, wherein the robot pose estimation result at the previous moment is used as an initial laser radar pose estimation value, performing iterative nearest point registration on the key frame laser radar point cloud data and the point cloud map to obtain a robot point cloud registration result, and performing convergence judgment on the robot point cloud registration result to obtain a robot point cloud registration result which is judged by the convergence as an effective point cloud absolute pose measurement value; Acquiring historical robot IMU data of a target robot, generating a robot motion state based on the historical robot IMU data, generating a robot positioning state based on the effective vision absolute pose measured value and the effective point cloud absolute pose measured value, and judging a stationary state of the target robot by utilizing the robot motion stat