Robot control method, device and robot

Abstract

Embodiments of the present application provide a robot control method, device and robot. The method comprises: obtaining an occlusion rate of a current field of view and uncertainty information of a current chassis; wherein the uncertainty information of the current chassis is used to quantify the uncertainty of the current chassis position; if it is determined to perform active observation according to the uncertainty information of the current chassis and / or the occlusion rate of the current field of view, determining a target observation direction according to the uncertainty information of the current chassis; wherein the target observation direction is a determined observation direction of a holder to reduce the uncertainty of the chassis position; controlling the holder to rotate to the target observation direction, collecting a new perspective image through the image acquisition device, and performing factor graph optimization processing based on the new perspective image to obtain a new chassis pose. The method is used to improve the positioning accuracy of the robot.

Description

Technical Field

[0001] This application relates to the field of robotics technology, and in particular to a robot control method, device, and robot. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a core technology for robots to achieve autonomous navigation. It collects information about the surrounding environment through onboard sensors, calculates the robot's spatial pose in real time, and simultaneously builds an environmental map, providing pose and map support for subsequent tasks such as path planning, motion control, and obstacle avoidance.

[0003] Currently, mainstream SLAM solutions for robots generally adopt a rigid mounting method where perception sensors are fixed to the chassis. During operation, the robot relies on environmental features collected within a fixed field of view to perform feature matching and pose optimization to achieve localization and mapping. This type of solution has a simple hardware structure, convenient system calibration, and can meet basic navigation and positioning requirements in typical scenarios with rich environmental features and no field of view obstruction.

[0004] However, in real-world complex application scenarios, the aforementioned fixed-field-of-view SLAM scheme is prone to insufficient effective features and sparse pose constraints in low-texture, feature-sparse scenes or dynamic interference environments. This leads to a decrease in robot positioning accuracy and drift in pose estimation. In severe cases, it can even result in feature tracking loss and positioning failure, directly affecting the stability and reliability of the robot's autonomous navigation. Summary of the Invention

[0005] The robot control method, device, and robot provided in this application are used to improve the positioning accuracy of the robot.

[0006] In a first aspect, embodiments of this application provide a robot control method, the method being applied to a robot controller, the robot further including a chassis and a gimbal with rotational degrees of freedom, the gimbal being equipped with an image acquisition device; including:

[0007] Obtain the occlusion rate of the current field of view and the uncertainty information of the current chassis; wherein, the uncertainty information of the current chassis is used to quantify the uncertainty of the current chassis position;

[0008] If active observation is to be performed based on the uncertainty information of the current chassis and / or the occlusion rate of the current field of view, then the target observation direction is determined based on the uncertainty information of the current chassis; wherein, the target observation direction is the gimbal observation orientation determined with the goal of reducing the uncertainty of the chassis position;

[0009] The gimbal is controlled to rotate to the target observation direction, and a new perspective image is acquired through the image acquisition device. Based on the new perspective image, factor graph optimization processing is performed to obtain a new chassis pose.

[0010] In one possible implementation, the uncertainty information of the current chassis is the covariance matrix of the current chassis position; the step of determining to perform active observation based on the uncertainty information of the current chassis and / or the occlusion rate of the current field of view includes:

[0011] Determine the trace and the largest eigenvalue of the covariance matrix of the current chassis position; wherein the trace represents the overall uncertainty of the current chassis position; and the largest eigenvalue represents the maximum uncertainty of the current chassis position in a single direction.

[0012] If any one of the following conditions is met: the trace is greater than a first preset threshold, the maximum feature value is greater than a second preset threshold, or the occlusion rate of the current field of view is greater than a third preset threshold, then active observation is determined to be performed.

[0013] In one possible implementation, the uncertainty information of the current chassis is the covariance matrix of the current chassis position; determining the target observation direction based on the uncertainty information of the current chassis includes:

[0014] Based on the direction of the eigenvector corresponding to the largest eigenvalue of the current chassis position covariance matrix, multiple candidate observation directions are generated;

[0015] The positioning correction benefit for each candidate observation direction is evaluated to obtain the evaluation results;

[0016] Based on the evaluation results, the optimal candidate observation direction is selected as the target observation direction.

[0017] In one possible implementation, the evaluation of the positioning correction benefit for each candidate observation direction to obtain the evaluation result includes:

[0018] For each candidate observation direction, the observation information gain, scanning time cost, and navigation cost corresponding to that candidate observation direction are calculated respectively; wherein, the observation information gain represents the expected reduction in positioning uncertainty after observing in that candidate observation direction; the scanning time cost represents the total time taken for the gimbal to rotate to that candidate observation direction and complete image acquisition; and the navigation cost represents the additional cost of active observation to the navigation task;

[0019] The evaluation result is obtained by fusing the observation information gain, the scanning time cost, and the navigation cost.

[0020] In one possible implementation, calculating the observation information gain corresponding to the candidate observation direction includes:

[0021] Based on the candidate observation directions, determine the visible map points under the candidate observation directions;

[0022] For each visible map point, the visible map point is reprojected onto a pixel plane to determine the Jacobian matrix of the reprojection error; wherein the Jacobian matrix of the reprojection error characterizes the sensitivity of the visible map point to the observation of the chassis position.

[0023] Based on the Jacobian matrix corresponding to each visible map point, determine the total information matrix increment corresponding to the candidate observation direction;

[0024] The posterior position covariance matrix is ​​determined based on the current chassis position covariance matrix and the increment of the total information matrix.

[0025] The difference between the trace of the current chassis position covariance matrix and the trace of the posterior position covariance matrix is ​​determined as the observation information gain for the candidate observation direction.

[0026] In one possible implementation, calculating the scan time cost corresponding to the candidate observation direction includes:

[0027] Determine the rotation angle based on the angle corresponding to the current gimbal angle and the candidate observation direction;

[0028] If the rotation angle is greater than the preset rotation angle threshold, the acceleration segment duration, deceleration segment duration, and constant speed segment duration of the gimbal are calculated according to the trapezoidal velocity curve, and the total rotation time of the gimbal is obtained by summing the acceleration segment duration, the deceleration segment duration, and the constant speed segment duration; the preset rotation angle threshold is the ratio of the square of the maximum angular velocity of the gimbal to the maximum angular acceleration.

[0029] If the rotation angle is less than or equal to the preset rotation angle threshold, the total rotation time of the gimbal is determined according to the triangular velocity curve.

[0030] The total rotation time of the gimbal is superimposed with the preset image acquisition and processing time to obtain the scanning time cost corresponding to the candidate observation direction.

[0031] In one possible implementation, calculating the navigation cost corresponding to the candidate observation direction includes:

[0032] Starting from the current chassis position and ending at the next navigation waypoint, a baseline path planning is performed to obtain baseline indicators; the baseline indicators include baseline driving mileage, baseline travel time, and baseline cumulative steering angle.

[0033] The gimbal is simulated to rotate to the candidate observation direction for observation, and post-observation path planning is performed with the same start and end points to obtain post-observation indicators; the post-observation indicators include post-observation mileage, post-observation travel time, and post-observation cumulative steering angle.

[0034] Based on the benchmark index and the observed index, calculate the mileage difference, time difference, and steering angle difference respectively;

[0035] The absolute values ​​of the mileage difference, time difference, and steering angle difference are weighted and summed to obtain the navigation cost corresponding to the candidate observation direction.

[0036] In one possible implementation, before controlling the gimbal to rotate to the target observation direction, the method further includes:

[0037] Calculate the gimbal rotation cost when the gimbal rotates to the target observation direction, and the chassis turning cost when the chassis as a whole turns to the target observation direction;

[0038] If the cost of rotating the gimbal is less than the cost of turning the chassis, then control the gimbal to rotate to the target observation direction.

[0039] In one possible implementation, obtaining uncertainty information about the current chassis includes:

[0040] Obtain the current head fusion pose of the gimbal and the current image frame of the current field of view acquired by the image acquisition device;

[0041] Based on the current head fusion pose, determine the current first transformation matrix and the current head fusion pose covariance matrix; wherein, the first transformation matrix represents the pose transformation relationship between the gimbal coordinate system and the chassis coordinate system; the covariance matrix of the current head fusion pose is used to quantify the uncertainty of the head fusion pose.

[0042] Based on the current image frame, the current first transformation matrix, and the current head fusion pose covariance matrix, a new visual constraint factor and the weight coefficients of the new visual constraint factor are determined.

[0043] Based on the new visual constraint factor and its weight coefficient, factor graph optimization is performed to obtain the current chassis position covariance matrix; wherein, the current chassis position covariance matrix serves as the uncertainty information of the current chassis.

[0044] In one possible implementation, determining the new visual constraint factor and its weight coefficients based on the current image frame, the current first transformation matrix, and the current head fusion pose covariance matrix includes:

[0045] Based on the current first transformation matrix, determine the current second transformation matrix; wherein, the second transformation matrix represents the pose transformation relationship between the camera coordinate system and the world coordinate system;

[0046] Based on the second transformation matrix, the target map points in the world coordinate system are transformed to the camera coordinate system and then projected to obtain the predicted coordinates of the pixel plane; wherein, the target map points are the map points corresponding to the feature points in the current image frame;

[0047] The reprojection residual between the pixel coordinates of the feature points in the current image frame and the predicted coordinates is used as the new visual constraint factor;

[0048] The weight coefficients of the new visual constraint factor are determined based on the trace of the current head fusion pose covariance matrix.

[0049] In one possible implementation, the step of performing factor graph optimization processing based on the new visual constraint factor and its weight coefficients to obtain the current chassis position covariance matrix includes:

[0050] Based on the new visual constraint factor, the weight coefficient of the new visual constraint factor, and the original constraint factor and the weight coefficient of the original constraint factor, an optimization objective function is constructed; wherein, the optimization objective of the optimization objective function is to minimize the sum of the weighted Mahalanobis distances of all constraint factors;

[0051] Based on the objective function, factor graph optimization is performed to obtain the covariance matrix of the current chassis position.

[0052] Secondly, embodiments of this application provide a robot control device, the device being applied to a robot controller, the robot further including a chassis and a gimbal with rotational degrees of freedom, the gimbal being equipped with an image acquisition device; including:

[0053] The acquisition module is used to acquire the occlusion rate of the current field of view and the uncertainty information of the current chassis; wherein, the uncertainty information of the current chassis is used to characterize the uncertainty of the current chassis position;

[0054] The determination module is used to determine the target observation direction based on the uncertainty information of the current chassis when determining to perform active observation based on the uncertainty information of the current chassis and / or the occlusion rate of the current field of view; wherein, the target observation direction is the gimbal observation orientation determined with the goal of reducing the uncertainty of chassis position;

[0055] The control module is used to control the gimbal to rotate to the target observation direction, acquire new perspective images through the image acquisition device, and perform factor graph optimization processing based on the new perspective images to obtain a new chassis pose.

[0056] Thirdly, embodiments of this application provide an electronic device, including: a memory and a processor;

[0057] The memory stores computer-executed instructions;

[0058] The processor executes computer execution instructions stored in the memory, causing the processor to perform the first aspect and / or various possible implementations of the first aspect as described above.

[0059] Fourthly, embodiments of this application provide a robot, including: a chassis, a controller, and a gimbal with rotational degrees of freedom, wherein the gimbal is equipped with an image acquisition device;

[0060] The controller is used to perform the first aspect as described above and / or various possible implementations of the first aspect.

[0061] Fifthly, embodiments of this application provide a computer-readable storage medium storing computer-executable instructions, which, when executed by a processor, are used to implement the first aspect and / or various possible implementations of the first aspect.

[0062] In a sixth aspect, embodiments of this application provide a computer program product, including a computer program that, when executed by a processor, implements the first aspect and / or various possible implementations of the first aspect.

[0063] The robot control method, device, and robot provided in this application first quantify the current positioning quality by acquiring the uncertainty information of the current chassis, and judge the observation conditions by combining the occlusion situation of the current field of view. When the positioning uncertainty is too high or the field of view is limited, active observation is initiated. Then, based on the uncertainty information of the current chassis, the optimal target observation orientation is selected, and environmental images from a new perspective are acquired by an independently rotating gimbal. The newly added observation constraints are integrated into the factor graph optimization process, and finally, a chassis pose result with higher accuracy is output. This method breaks through the physical limitations of traditional fixed field of view SLAM schemes. It can actively supplement effective pose observation constraints in complex scenes such as low texture and dynamic occlusion, alleviate the positioning drift problem caused by insufficient features, and improve the stability and reliability of positioning results. At the same time, the observation action is completed by an independent gimbal, without interrupting the normal movement of the chassis, and will not cause significant interference to the original navigation task. It can improve the robot's autonomous adaptability in complex environments while ensuring navigation efficiency. Attached Figure Description

[0064] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with this application and, together with the description, serve to explain the principles of this application.

[0065] Figure 1 This application provides a schematic diagram of the structure of a robot;

[0066] Figure 2 A schematic diagram of a coordinate transformation chain provided in this application;

[0067] Figure 3 Flowchart of the robot control method provided in this application Figure 1 ;

[0068] Figure 4 Flowchart of the robot control method provided in this application Figure 2 ;

[0069] Figure 5 A schematic diagram of data flow provided for this application;

[0070] Figure 6 Flowchart of the robot control method provided in this application Figure 3 ;

[0071] Figure 7 A schematic diagram illustrating one application scenario provided in this application;

[0072] Figure 8 A schematic diagram of the control device for the robot provided in this application;

[0073] Figure 9 A schematic diagram of the structure of the electronic device provided in this application.

[0074] The accompanying drawings have illustrated specific embodiments of this application, which will be described in more detail below. These drawings and descriptions are not intended to limit the scope of the concept in any way, but rather to illustrate the concept of this application to those skilled in the art through reference to specific embodiments. Detailed Implementation

[0075] Exemplary embodiments will now be described in detail, examples of which are illustrated in the accompanying drawings. When the following description relates to the drawings, unless otherwise indicated, the same numbers in different drawings denote the same or similar elements. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with this application. Rather, they are merely examples of apparatuses and methods consistent with some aspects of this application.

[0076] Robot localization and navigation technology is widely used in scenarios such as warehousing and logistics, indoor services, and industrial inspection. A robot typically consists of a chassis, sensors mounted on the robot body, and a controller, which uses visual or laser perception results to complete environmental perception, pose estimation, and path execution.

[0077] In the above scenarios, robots often need to operate continuously in complex environments such as obstructed shelves, low-texture corridors, and frequent personnel flow, which places high demands on positioning stability, navigation continuity, and observation efficiency.

[0078] In existing solutions, a common approach is to fix the lidar or image acquisition equipment to the chassis and estimate the chassis pose using continuous observation constraints through SLAM or factor map optimization methods. When there are insufficient effective features in the current field of view, some solutions supplement the observations by rotating the chassis and then incorporate the new observations into the localization and mapping process.

[0079] However, fixed-field-of-view SLAM schemes have significant limitations when occlusion rates are high or environmental textures are insufficient. For example, in low-texture, feature-sparse scenes (such as open halls, long straight corridors, and solid-color walls), the number of effective environmental features that can be extracted within a fixed field of view is limited. In scenes with dynamic occlusion (such as pedestrians crossing or temporary obstacles obstructing the sensor's field of view), the effective observation area within a fixed field of view is further reduced. In other words, a fixed field of view struggles to acquire new observations that are beneficial for constraining pose in a timely manner, which can easily increase the uncertainty of the chassis position, leading to positioning drift, map update lag, and unstable navigation decisions.

[0080] Relying on chassis rotation to obtain new perspectives would interrupt the original travel process, increase time and energy consumption, and may cause observation and navigation actions to interfere with each other, reducing overall operational efficiency.

[0081] Faced with the contradiction between scene adaptability and operational efficiency in fixed-field-of-view SLAM localization schemes, the inventors conceived of setting up an independently rotating gimbal on the robot's head to carry image acquisition equipment, thus overcoming the physical limitations of a fixed field of view without altering the chassis's driving state. To avoid ineffective observations and additional errors caused by blindly rotating the gimbal, the inventors further proposed using the uncertainty of the chassis position as the core decision-making basis. By utilizing the covariance matrix's ability to quantify the uncertainty of the chassis position and represent the spatial distribution of errors, they can accurately identify the triggering time when the positioning quality deteriorates and supplementary observations are needed. They can also selectively choose the optimal observation direction that minimizes position uncertainty. Finally, by controlling the gimbal to rotate to the target direction to acquire images from a new perspective, additional visual observation constraints are added to the factor graph optimization. This effectively converges the positioning error without interfering with the original navigation progress, achieving a simultaneous improvement in positioning stability and navigation efficiency.

[0082] Figure 1 A schematic diagram of the structure of a robot provided in this application is shown below. Figure 1 As shown, the robot includes a chassis, a controller, and a gimbal with rotational degrees of freedom, the gimbal being equipped with image acquisition equipment.

[0083] The chassis serves as the mechanical load-bearing base and mobile execution unit of the entire machine. On the one hand, it supports all onboard components such as the gimbal, controller, sensor module, and power supply module, ensuring the structural rigidity and installation stability of the entire machine. On the other hand, it receives motion commands from the controller and executes forward, backward, turning, and speed adjustment movements to support the robot in completing autonomous navigation and operation tasks.

[0084] This application does not limit the specific structural form or drive type of the chassis, and the appropriate type can be flexibly selected according to the application scenario. An exemplary configuration is as follows: In the mobile architecture, a differential drive chassis or a Mecanum wheel omnidirectional chassis can be used, with a wheelbase typically ranging from 400 to 600 mm, a rated load covering 50 to 200 kg, a maximum travel speed of 1.5 to 2.0 m / s, and a maximum steering angular velocity of 90 to 120 degrees. It can adapt to the traffic and operation needs of different scenarios. In terms of the drive system, the chassis can be equipped with a DC brushless hub motor or a servo motor with a harmonic reducer or planetary reducer. This application does not limit the specific type of servo motor. The servo driver receives speed and torque commands issued by the controller through the CAN bus or EtherCAT bus to achieve high-precision motion closed-loop control. In terms of posture sensing configuration, an incremental photoelectric encoder is installed at the axle end of each drive wheel for data calculation of wheel speed odometer. The chassis geometric center is reserved with an IMU mounting reference surface to ensure the stability and calibration accuracy of sensor extrinsic parameters and provide reliable chassis-end motion observation data for SLAM positioning. In terms of safety and imaging stability design, the chassis is equipped with an independent electronic braking and emergency stop safety circuit to ensure safety during operation and debugging. At the same time, a shock-absorbing suspension structure is set to effectively isolate the upward transmission of road vibration, reduce motion blur of head image acquisition equipment, and improve the image quality of visual observation.

[0085] The gimbal, mounted above the chassis, serves as the mounting platform for the image acquisition equipment. Through two orthogonal rotational degrees of freedom, it drives the image acquisition equipment to adjust its spatial orientation, actively expanding the observation field of view without altering the chassis's driving state. This gimbal can be a tandem two-degree-of-freedom gimbal, with two orthogonally arranged rotational axes corresponding to two degrees of freedom (pitch and yaw). The yaw axis, located at the bottom of the gimbal and rigidly connected to the upper surface of the chassis, drives the entire gimbal to rotate horizontally around its vertical axis, with a typical rotation range of ±120° to ±180°, responsible for adjusting the horizontal observation direction. The pitch axis, located at the load mounting end on the upper part of the gimbal, drives the image acquisition equipment to tilt vertically around its horizontal axis, with a typical rotation range of -60° to +60°, responsible for adjusting the vertical observation angle. Key performance parameters of the gimbal include, for example, an encoder resolution of 0.01 degrees, a maximum angular velocity of 300 degrees per second, and a repeatability better than 0.05 degrees. It should be noted that the embodiments of this application do not limit the type, rotation angle and performance parameters of the gimbal. The gimbal can be a finished gimbal or a self-made harmonic deceleration gimbal.

[0086] The image acquisition device, mounted on top of the gimbal using a shock-absorbing bracket, serves as the robot's visual perception entry point, providing raw visual data for SLAM localization and active observation. This image acquisition device is typically an industrial-grade vision camera, which can be categorized as a monocular camera, binocular camera, or RGB-D camera, depending on the SLAM solution. The field of view of this image acquisition device is no less than 80° × 55°. It should be noted that this application does not limit the type of image acquisition device used in its embodiments.

[0087] The controller is the core of the robot's computation and decision-making, undertaking the entire process of algorithm calculation, logic scheduling, and motion control. This controller can be, for example, a microcontroller unit (MCU) or a system-on-a-chip (SoC), etc. This application does not limit the structure of the controller. The controller can serve as the execution subject in the following embodiments.

[0088] Optionally, the robot can also be equipped with various types of sensing sensors and supporting computing and storage hardware. The IMU is mounted at the geometric center of the chassis, with a synchronization delay of no more than 2ms. The chassis odometer uses an incremental photoelectric encoder with a resolution of no less than 1024 pulses per revolution, mounted on the drive wheel axle. The lidar has a horizontal field of view of 360 degrees, a vertical field of view of no less than 30 degrees, and a detection range of no less than 100 m. The processor can be an industrial computer, equipped with at least 32 GB of RAM and solid-state storage. The storage includes RAM and a solid-state drive for storing map data and pose maps. Image acquisition devices can connect to the processor via USB 3.0 or GigE interfaces, the lidar connects to the processor via Gigabit Ethernet, and the chassis odometer and IMU connect to the processor via CAN bus or RS-485.

[0089] It should be noted that the embodiments of this application do not limit the shape and type of the robot, as long as the robot can move autonomously.

[0090] First, the coordinate system transformations involved in the embodiments of this application will be explained. Figure 2 A schematic diagram of a coordinate transformation chain provided in this application is shown below. Figure 2 As shown, the world coordinate system {W} serves as the global reference, carrying the coordinate reference for the global map and loop closure detection. Based on the observation data from the chassis IMU and chassis odometer, the pose transformation matrix from the world frame to the chassis frame is estimated using the world coordinate system reference. This represents the real-time pose of the chassis in global space. Based on the angle data from the gimbal encoder and the gimbal coordinate system {P}, after yaw and pitch clearance compensation, the pose transformation matrix from the chassis coordinate system to the gimbal coordinate system is obtained. The coordinate system {C} represents the real-time rotational pose of the gimbal relative to the chassis; the camera coordinate system {C} and the gimbal coordinate system {P} are rigidly fixed, corresponding to the pre-calibrated fixed external parameters from the gimbal to the camera. The global pose of the head camera is derived through a kinematic chain transformation, i.e., the transformation from the world frame to the camera frame satisfies... This enables a step-by-step transfer of pose from the chassis to the camera. Ultimately, the head camera and LiDAR output perception results based on the corresponding spatial pose, including head images and depth observations, environmental feature points, and local obstacle information, providing raw perception input for subsequent steps such as factor map optimization and cost map updating.

[0091] The technical solution of this application and how the technical solution of this application solves the above-mentioned technical problems are described in detail below with specific embodiments. These specific embodiments can be combined with each other, and the same or similar concepts or processes may not be described again in some embodiments. The embodiments of this application will be described below with reference to the accompanying drawings.

[0092] Figure 3 Flowchart of the robot control method provided in this application Figure 1 ,like Figure 3 As shown, the method includes:

[0093] S201. Obtain the occlusion rate of the current field of view and the uncertainty information of the current chassis.

[0094] The current field of view refers to the effective field of view that the image acquisition device can cover under the current orientation. The occlusion rate of the current field of view is used to characterize the proportion of the occluded area to the effective field of view in the image obtained by the image acquisition device under the current observation orientation, and is an indicator of the quality of the current observation conditions. The uncertainty information of the current chassis is used to quantify the uncertainty of the current chassis's three-dimensional position. This uncertainty information of the current chassis can be, for example, the chassis position covariance matrix, or other information that can quantify the uncertainty of the chassis's three-dimensional position. The embodiments of this application do not limit the form of the uncertainty information of the current chassis.

[0095] In one example, the controller continuously receives the current image frame output by the image acquisition device (e.g., the current head depth map or LiDAR point cloud map). It identifies the areas occupied by shelves, walls, pedestrians, or nearby obstructions in the image through image segmentation, and then uses the ratio of the number of obstructed pixels to the number of pixels in the effective observation area as the occlusion rate. Alternatively, it can estimate the occlusion degree by jointly considering feature point sparsity and areas with abrupt depth changes. When the number of stably trackable features in the image is lower than a preset number and there are continuous occlusion boundaries in a local area, the corresponding area is included in the occlusion area. For low-texture corridor scenes, the controller can also remove ceilings, solid-color walls, or highly reflective areas before calculating the occlusion rate, so that the occlusion rate reflects the degradation degree of the observable area effective for positioning. For example, the proportion of effective depth pixels within a 0.5–3m range in front can be statistically analyzed from the current depth map, and then the occlusion rate of the current field of view can be calculated using the formula... Calculate, where, This indicates the occlusion rate of the current field of view; This indicates the number of effective pixels in the current field of view image, which refers to the total number of pixels that are not obscured by obstacles, are within the effective observation distance, and whose effective environmental features can be extracted; This represents the total number of pixels in the image region participating in the occlusion statistics. It can be the total number of pixels in the current image frame or the total number of pixels in the region of interest in front, depending on business needs. It is the denominator for calculating the effective pixel ratio.

[0096] Taking the current chassis position covariance matrix as an example, which represents the uncertainty information of the current chassis, this covariance matrix is ​​derived from the state estimation result output by factor graph optimization at the current moment. The controller integrates historical visual constraints, chassis motion constraints, and observation constraints related to currently available map points in the factor graph. After solving for the chassis pose, it extracts the sub-matrix corresponding to the chassis position dimension from the synchronously output Hessian matrix, and obtains the current chassis position covariance matrix after inversion. Specifically, the controller can use the Gauss-Newton method or the Levenberg-Marquardt method to iteratively solve the optimization objective function, and extract the covariance sub-matrix corresponding to the chassis position state through the inversion of the linearized information matrix or block inversion, which serves as the current chassis position covariance matrix.

[0097] This step synchronously acquires occlusion rate and current chassis uncertainty information, and inputs the current observation degradation level and pose uncertainty into the subsequent decision-making process, so that the controller can decide whether to adjust the observation direction based on the current positioning status, instead of relying solely on continuous observation with a fixed field of view.

[0098] S202. If active observation is to be performed based on the uncertainty information of the current chassis and / or the occlusion rate of the current field of view, then the target observation direction is determined based on the uncertainty information of the current chassis.

[0099] Active observation refers to a working mode that proactively adjusts the observation orientation to improve positioning quality. It is activated only when positioning uncertainty is too high or observation conditions are insufficient. Under normal operating conditions, it will not interfere with the robot's regular navigation process. The controller can actively drive the gimbal to change the yaw angle and / or pitch angle to obtain new perspective images from different directions. It can be understood that active observation can be triggered solely based on positioning uncertainty, solely based on occlusion, or a combination of both.

[0100] The target observation direction is the gimbal observation orientation determined with the goal of reducing the uncertainty of the chassis position. It is the spatial direction that can provide the most effective constraints for positioning.

[0101] In one example, the controller first makes an active observation trigger decision based on the current chassis uncertainty information and the current field of view occlusion rate. In specific implementation, the controller can make a judgment based on the position uncertainty represented by the current chassis uncertainty information and / or the observation degradation represented by the current field of view occlusion rate. For example, if the trace of the current chassis position covariance matrix is ​​greater than the uncertainty threshold and / or the current field of view occlusion rate is greater than the occlusion threshold, the controller determines to execute active observation.

[0102] After determining to perform active observation, the controller determines the target observation direction based on the current chassis uncertainty information. Taking the current chassis position covariance matrix as an example, this matrix characterizes the distribution of chassis position uncertainty. After eigenvalue decomposition of this matrix, the direction of the eigenvector corresponding to the largest eigenvalue is the direction with the highest chassis position uncertainty in the entire space. Therefore, the direction of the eigenvector corresponding to the largest eigenvalue of the current chassis position covariance matrix can be directly used as the target observation direction. Alternatively, the direction of the eigenvector corresponding to the largest eigenvalue of the current chassis position covariance matrix can be used as a benchmark. Within a preset spatial range of this benchmark, several discrete candidate observation directions can be generated, and each candidate observation strategy can be evaluated according to a preset evaluation strategy. The optimal candidate observation direction can then be selected as the target observation direction.

[0103] By analyzing the chassis position covariance matrix and generating the target observation direction accordingly, active observation has a clear constraint target. The gimbal rotation no longer relies on random supplementary viewpoints, thus allowing new observations to be more concentrated on the direction where the current position has greater uncertainty.

[0104] S203. Control the gimbal to rotate to the target observation direction, acquire new perspective images through image acquisition equipment, and perform factor graph optimization processing based on the new perspective images to obtain a new chassis pose.

[0105] The gimbal is a head-mounted actuator with two rotational degrees of freedom (yaw and pitch), capable of independently adjusting the observation orientation of the image acquisition device without altering the chassis's driving state. The new perspective image, acquired after the gimbal turns towards the target observation direction, differs from the original fixed field of view and provides environmental features outside the original field of view. Factor graph optimization is a computational process that integrates new visual constraints with existing constraints to update the chassis pose. The new chassis pose is a pose estimation result obtained after re-optimization with the new observation constraints, including the chassis's three-dimensional position and spatial attitude, resulting in improved positioning accuracy compared to before optimization. This application does not limit the motion planning method of the gimbal or the specific solver type for factor graph optimization.

[0106] It is understandable that the image acquisition device is fixedly mounted on the gimbal, and its mounting posture relative to the gimbal is based on pre-calibrated parameters. Therefore, changes in the gimbal angle can be directly converted into changes in the camera's observation direction.

[0107] In one example, after determining the target observation direction, the controller first converts the target observation direction into the gimbal's target yaw and pitch angles, and generates angle closed-loop control commands based on the current gimbal attitude. Upon receiving these control commands, the gimbal driver executes servo rotation. The controller can simultaneously read encoder feedback or attitude sensor feedback until the deviation between the current gimbal attitude and the target attitude is less than a preset angle threshold. Subsequently, the image acquisition device acquires a new perspective image after the target observation direction stabilizes. The controller performs distortion correction, exposure homogenization, and timestamp alignment on the new perspective image, and extracts corner points, edge points, or local descriptor features. The controller correlates and matches the features in the new perspective image with features and map points in historical keyframes to obtain the observation correspondence between the current image frame and the target map points in the world coordinate system. Based on the new perspective image, the controller constructs new visual constraints and performs factor graph optimization processing in conjunction with the original constraints to output an updated chassis pose. It also extracts a new position covariance estimate from the linearization result for use in the next control loop.

[0108] In one possible implementation, the chassis maintains its current navigation speed and continues to move forward during the gimbal rotation. The controller aligns the new perspective image with the chassis state at the corresponding moment through time synchronization before optimization. In another possible implementation, if the gimbal rotation angle exceeds a preset threshold, the controller can reduce the chassis angular velocity at the moment of image acquisition to reduce the impact of motion blur on feature matching. In yet another possible implementation, when the number of matched inliers corresponding to the new perspective image is lower than a threshold, the controller reselects the second-best candidate observation direction to perform a supplementary observation.

[0109] Through continuous processing between gimbal orientation rotation, image acquisition, and factor graph joint optimization, the new viewpoint is directly transformed into an effective constraint on the chassis pose, thereby obtaining a new chassis pose. This allows for observation supplementation without changing the original direction of chassis travel. The chassis navigation action and observation action are executed separately by the gimbal, which can maintain the continuity of positioning updates in complex dynamic environments.

[0110] The robot control method provided in this application first quantifies the current positioning quality by acquiring the uncertainty information of the current chassis, and judges the observation conditions by combining the occlusion situation of the current field of view. When the positioning uncertainty is too high or the field of view is limited, active observation is initiated. Then, based on the uncertainty information of the current chassis, the optimal target observation orientation is selected, and environmental images from a new perspective are acquired by an independently rotating gimbal. The newly added observation constraints are integrated into the factor graph optimization process, and finally, a chassis pose result with higher accuracy is output. This method breaks through the physical limitations of traditional fixed field of view SLAM schemes. It can actively supplement effective pose observation constraints in complex scenes such as low texture and dynamic occlusion, alleviate the positioning drift problem caused by insufficient features, and improve the stability and reliability of positioning results. At the same time, the observation action is completed by an independent gimbal, without interrupting the normal movement of the chassis, and will not cause significant interference to the original navigation task. It can improve the robot's autonomous adaptability in complex environments while ensuring navigation efficiency.

[0111] Figure 4 Flowchart of the robot control method provided in this application Figure 2 , Figure 5 This application provides a schematic diagram of data flow, such as... Figure 4 and Figure 5 As shown, in this embodiment... Figure 3 Based on the embodiments, a detailed explanation is provided on how to obtain the uncertainty information of the current chassis. The method includes:

[0112] S301. Obtain the current head fusion pose of the gimbal and the current image frame of the current field of view acquired by the image acquisition device.

[0113] For example, head fusion attitude refers to the real-time attitude result of the gimbal relative to the chassis, calculated by combining the gimbal encoder measurements and inertial measurement unit (IMU) observations using a data fusion algorithm. Compared to single-sensor measurements, this method offers higher accuracy and better anti-interference capabilities. The current image frame is an environmental image captured by the image acquisition device at the current moment, covering the current field of view. It is the raw data used to extract visual features and construct observation constraints. This application does not limit the specific algorithm used for attitude fusion, nor the type and parameters of the image acquisition device.

[0114] In one example, mechanical backlash compensation can be performed on the raw angle measurement data of the gimbal encoder first, and then complementary filtering and fusion can be performed with the angular velocity integral result of the IMU to output the head fusion posture at the current moment; at the same time, the image acquisition device installed on the gimbal can acquire environmental images at a preset frame rate to obtain the current image frame at the corresponding moment.

[0115] Specifically, the controller can first retrieve the static sampling covariance matrix of the pan-tilt encoder from the memory. Average gap compensation Gap residual covariance matrix The noise covariance matrix of the IMU angular velocity IMU installation extrinsic matrix Camera installation external parameters Understandably, these parameters can be obtained through experimental calibration before the robot leaves the factory or during regular maintenance.

[0116] (1) Static sampling covariance matrix

[0117] When the encoder is completely stationary, its corresponding actual mechanical angle is a constant value. At this point, fluctuations in multiple encoder readings are entirely due to inherent noise such as quantization noise, electrical interference, and code disk subdivision errors. By statistically analyzing multiple encoder angle data in a stationary state, the encoder's stationary sampling variance can be calculated using the unbiased sample variance formula. Since the noise of the yaw axis and pitch axis are independent, the cross-covariance is 0, thus a 2×2 stationary sampling covariance matrix can be obtained. .

[0118] (2) Average gap compensation and gap residual covariance matrix

[0119] The transmission hysteresis is measured using a micro-motion scanning method, following this procedure: The gimbal is reset to its mechanical zero position, the chassis remains stationary, and the yaw and pitch axes are calibrated independently. The controller controls the gimbal to perform small-angle (typically ±3°~±5°) reciprocating micro-motions along a single axis, pausing after each movement until the mechanical vibration completely dissipates, for a total of R reciprocations (e.g., R=20). After each micro-motion stabilizes, two sets of data are recorded simultaneously: the encoder angle output by the gimbal encoder. (Original reading of the encoder disk at the motor end or output end), referencing the head angle. (This can be determined by IMU attitude integration or attitude observation from a calibration target). The difference between the reference head angle and the encoder angle is used as the micro-motion scan residual, and the average value of the R micro-motion scan residuals is taken as the average gap compensation amount.

[0120] For example, according to the formula Calculate the average clearance compensation amount This refers to the correction value that needs to be added to the encoder reading; where R is the number of micro-scans. The reference head angle (in rad) is obtained by IMU integration or calibration target measurement during the r-th scan. The encoder angle (in rad) at the same moment.

[0121] Since the angle includes two independent rotational dimensions, yaw and pitch, the above averaging operation is performed on each of the two dimensions separately, resulting in the final value. It is a two-dimensional angle vector that includes yaw compensation components and pitch compensation components. This indicates the average hysteresis (average backlash compensation amount) that needs to be compensated for when the encoder readings are read.

[0122] Simultaneously, the sample variance of the micro-motion scanning residuals is calculated to form the gap residual covariance matrix. The residual of a single scan is the difference between the reference head angle and the compensated encoder angle. After calculating the sample variance of the residuals for all scans, a 2×2 covariance matrix is ​​constructed according to the two dimensions of yaw and pitch. This matrix is ​​used to quantify the random fluctuations that still exist after gap compensation, i.e., the residual uncertainty after compensation.

[0123] (3) The noise covariance matrix of the IMU angular velocity

[0124] The background noise covariance matrix of the IMU angular velocity This can be achieved through static calibration. First, keep the IMU completely stationary and collect M sets of continuous angular velocity sample data. First, calculate the mean angular velocity of all samples. In an ideal static state, this vector has a mean of zero. The unbiased sample covariance formula is then used to calculate it, i.e. The final result It reflects the noise floor level of the IMU angular velocity sensor, and the unit is (rad / s)².

[0125] (4) Install the external parameter matrix of the IMU It calibrates the rotational relationship between the IMU coordinate system and the gimbal coordinate system {P}, and is used to project the three-dimensional angular velocity measured by the IMU onto the yaw and pitch axes of the gimbal.

[0126] (5) Camera external parameters The pose transformation of the camera coordinate system {C} relative to the gimbal coordinate system {P} is obtained through hand-eye calibration and is a fixed constant. It should be noted that in this embodiment, {W} represents the world coordinate system, {B} represents the chassis coordinate system, {P} represents the gimbal coordinate system, and {C} represents the camera coordinate system. (Convention) This represents the pose transformation from the {B} coordinate system to the {A} coordinate system; therefore, the kinematic chain from the camera to the world is... .

[0127] Furthermore, the raw angle output by the gimbal encoder at time k can be... The compensated encoder angle is obtained by superimposing the gap compensation amount calibrated offline. (Stable at low frequencies but with time lag during rapid rotations). Based on the head fusion posture from the previous moment. According to the formula For the current IMU angular velocity Perform single-step integration to obtain the pose estimate of the IMU branch. and according to the formula The data from the two branches are weighted and fused using complementary filtering to obtain the final current head fusion pose. .

[0128] in, and The yaw and pitch angles output by the gimbal encoder at time k are represented in rad. This indicates the IMU output angular velocity (in rad / s). The adjacent sampling periods (in seconds) are represented by the number of sampling periods. This represents the IMU installation extrinsic parameter matrix. , representing complementary filter coefficients, The larger the value, the more reliable the IMU is in the high-frequency band, and the faster the attitude response, but the more obvious the low-frequency drift. The smaller the value, the more reliable the encoder is in the low-frequency range, resulting in more stable posture, but also a greater dynamic hysteresis. Essentially, this filter applies a low-pass filter to the encoder and a high-pass filter to the IMU, achieving a balance between long-term stability and dynamic response speed after fusion.

[0129] S302. Based on the current head fusion pose, determine the current first transformation matrix and the current head fusion pose covariance matrix.

[0130] Wherein, the first transformation matrix The first transformation matrix, representing the pose transformation relationship between the gimbal coordinate system {P} and the chassis coordinate system {B}, is a homogeneous transformation matrix containing rotation and translation components. It is used to achieve spatial coordinate transformation between the gimbal and chassis coordinate systems and serves as the intermediate transformation basis for deriving the camera's spatial pose. The covariance matrix of the current head fusion pose is also shown. This matrix is ​​used to quantify the uncertainty of the head fusion attitude. The larger the matrix value, the higher the uncertainty of the head fusion attitude and the lower the confidence of the attitude estimation. This matrix can be a 2×2 matrix, corresponding to the attitude variance and covariance of the yaw and pitch degrees of freedom, respectively, representing the confidence level of the head attitude relative to the chassis.

[0131] In one example, a rotation matrix can be constructed based on the yaw and pitch angles in the head fusion attitude. Combined with the fixed installation translation vector between the gimbal and the chassis, the homogeneous transformation matrix of the gimbal relative to the chassis is obtained as the first transformation matrix. At the same time, the current head fusion attitude covariance matrix is ​​calculated by combining the encoder's static sampling covariance matrix, gap residual covariance matrix, and IMU attitude increment covariance matrix with complementary filtering coefficients through the error propagation formula.

[0132] Specifically, based on the weighted independence assumption, the error propagation law is used according to the formula... Calculate the current head fusion pose covariance matrix. .in, The encoder static sampling covariance matrix (unit: ), used to reflect quantization noise and electrical noise; The gap residual covariance matrix (unit: ), representing the remaining error of gap compensation; Let be the variance of the angle during single-step IMU integration, and let be the angle increment during IMU attitude integration. This is obtained from the propagation of angular velocity noise through linear error: This only represents the incremental uncertainty of the IMU branch at the current moment; the attitude uncertainty of the previous moment has already been included. middle.

[0133] Current head fusion posture Substituting the two-degree-of-freedom gimbal geometric model, we construct a homogeneous transformation matrix to obtain the current first transformation matrix. The rotation components are arranged in a rotation matrix following the rotation sequence of "yaw first, then pitch". ,in Let yaw rotation matrix be the rotation matrix about the Z-axis. The pitch and rotation matrix is ​​about the Y-axis. This refers to the yaw angle in the current head fusion posture. The pitch angle is the current head fusion pose; the translation part uses a fixed translation amount calibrated offline. (Unit: m), which is the three-dimensional offset of the gimbal rotation center relative to the origin of the chassis coordinate system.

[0134] The above steps achieve kinematic decoupling and clearance compensation between the gimbal and the chassis, which can reduce attitude errors caused by gimbal mechanical clearance and sensor heterogeneity, and output more accurate and reliable head fusion attitude and head fusion attitude covariance matrix.

[0135] S303. Based on the current image frame, the current first transformation matrix, and the current head fusion pose covariance matrix, determine the new visual constraint factor and the weight coefficient of the new visual constraint factor.

[0136] Among them, the visual constraint factor is an observation-based factor used to constrain chassis pose in the factor graph optimization framework. It is constructed based on the reprojection residuals of image feature points and corresponding 3D map points, and can provide visual observation constraints for pose solving. The weight coefficient is used to adjust the contribution ratio of the corresponding visual constraint factor in the optimization objective. Its value is related to the uncertainty of the head fusion pose. The higher the pose uncertainty, the lower the weight of the corresponding visual constraint factor, thereby reducing the observation error introduced by low-precision pose.

[0137] In one example, the pose of the camera coordinate system relative to the world coordinate system can be derived by combining the prior pose of the chassis with the first transformation matrix. The predicted coordinates are obtained by projecting the 3D map points onto the pixel plane. The difference between the predicted coordinates and the actual pixel coordinates of the corresponding feature points extracted in the current image frame is used as the reprojection residual as the visual constraint factor. At the same time, the dynamic weight coefficient of the visual constraint factor is calculated based on the trace of the head fusion pose covariance matrix.

[0138] Specifically, based on the current first transformation matrix, a current second transformation matrix is ​​determined; where the second transformation matrix represents the pose transformation relationship between the camera coordinate system and the world coordinate system; based on the second transformation matrix, the target map points in the world coordinate system are transformed to the camera coordinate system and then projected to obtain the predicted coordinates of the pixel plane; where the target map points are the map points corresponding to the feature points in the current image frame; the reprojection residual between the pixel coordinates and the predicted coordinates of the feature points in the current image frame is used as a new visual constraint factor.

[0139] For example, the second transformation matrix is ​​a homogeneous transformation matrix containing rotation and translation components, representing the pose transformation relationship between the camera coordinate system and the world coordinate system. Its derivation relies on the pose relationship between the gimbal and the chassis provided by the first transformation matrix, and is the core pose reference connecting 3D spatial points and pixel imaging. The target map point is a 3D spatial point in the environment map that is matched one-to-one with the feature points of the current image frame. Projection processing is implemented based on the pinhole camera imaging model, used to map the 3D spatial points in the camera coordinate system to the 2D pixel plane to obtain the corresponding theoretical imaging position, i.e., the predicted coordinates. The reprojection residual is the difference between the actual pixel coordinates of the feature points in the current image frame and the predicted coordinates of the corresponding target map points, which intuitively reflects the degree of deviation between the visual observation and the theoretical model under the current pose estimation. The visual constraint factor is an observation-type constraint term in the factor graph optimization framework, with the reprojection residual as its core, used to impose visual observation constraints on the chassis pose in the optimization solution. This application embodiment does not limit the specific type of camera imaging model or the implementation algorithm of feature point matching.

[0140] In one example, the controller can calculate the current second transformation matrix based on the current first transformation matrix, chassis pose, and camera extrinsic parameters, and transform the target map points from the world coordinate system to the camera coordinate system. The transformed 3D points are then mapped onto the image plane via a pinhole projection model to obtain the corresponding predicted pixel coordinates. The controller further reads the feature point pixel coordinates in the current image frame and calculates the difference between the two as the reprojection residual, which is used as the core constraint term to construct a new visual constraint factor.

[0141] For example, based on a multi-coordinate system transformation chain, according to the chassis pose, the first transformation matrix, and the camera mounting extrinsic parameters, the formula is applied. Calculate to obtain the current second transformation matrix. .in, The chassis pose (the chassis pose in the world frame) is the prior pose provided by odometry calculation and IMU pre-integration. This represents the current first transformation matrix; This indicates that the camera has external parameters installed. This formula shows that the camera pose is entirely determined by the chassis pose and gimbal attitude; during optimization, only the chassis pose needs to be adjusted to simultaneously correct the camera pose.

[0142] Furthermore, the world map points pass The inverse transformation, projected onto the camera coordinate system, yields... via camera projection function (Including intrinsic parameter matrix and radial / tangential distortion correction) Predicted coordinates of 3D points projected onto a pixel plane. According to the formula Calculate pixel coordinates With predicted coordinates The reprojection residuals are added to the factor map as new visual factors. This is the reprojection residual (in pixels).

[0143] This method can accurately establish the constraint relationship between visual observation and chassis pose under the rotatable gimbal architecture, ensuring that the new perspective image obtained by adjusting the gimbal's observation orientation can be accurately converted into observation constraints that can be used for factor graph optimization. This effectively guarantees the accuracy and effectiveness of visual observation constraints and provides reliable observation support for subsequent pose optimization solutions and positioning uncertainty quantification.

[0144] Specifically, the weight coefficients of the new visual constraint factors are determined based on the trace of the current head fusion pose covariance matrix.

[0145] The trace of the head fusion attitude covariance matrix is ​​the sum of the values ​​of the diagonal elements of the matrix, corresponding to the sum of the attitude variances in the gimbal's yaw and pitch rotation dimensions. It is used to quantify the overall uncertainty of the head fusion attitude; a larger trace value indicates a larger overall error and lower confidence in the gimbal attitude estimation. Weight coefficients are used to adjust the contribution ratio of the corresponding visual constraint factor in the factor graph optimization objective. They are a quantitative representation of the confidence in visual observations; a higher weight indicates a stronger constraint effect of the visual constraint in the pose optimization solution.

[0146] In one example, the weight coefficients of the new visual constraint factor can be obtained by normalizing or mapping the trace of the current head fusion pose covariance matrix. The weight values ​​are negatively correlated with the overall uncertainty of the head fusion pose, thereby achieving adaptive matching between pose accuracy and observation weights. This application does not limit the specific mapping function form between the weight coefficients and the covariance trace; it can be flexibly set according to the system calibration results and actual working conditions.

[0147] For example, the trace of the matrix can be obtained by summing the diagonal elements of the current head fusion pose covariance matrix. Then, combined with the noise variance of pre-calibrated pixel feature detection, the equivalent total noise variance of the visual observation in this frame can be derived using the error propagation law. The inverse of the equivalent total noise variance can then be used as the weighting coefficient of the visual constraint factor. It can be represented as ,in, This represents the trace of the current head fusion pose covariance matrix, in rad². This represents the variance of pixel feature detection noise, expressed in pixels².

[0148] When head pose uncertainty is high, i.e., the trace of the head fused pose covariance matrix is ​​low... When the value is large, the weight of the visual constraint factor Consequently, the contribution of the visual constraint factor in that frame to the optimization objective decreases, preventing the backend optimization from over-relying on head observations with insufficient confidence in that frame; when the head pose estimation accuracy is high... The corresponding increase allows the visual constraints to fully exert their pose correction function. Specifically, in this embodiment, the transformation matrix of the gimbal coordinate system relative to the chassis coordinate system at time k is... Substituting these known quantities into the kinematic link, the transformation matrix of the camera coordinate system relative to the world coordinate system is derived. Then through weights The reliability of the visual constraint factor is dynamically adjusted by adding the head observation as a new visual constraint factor to the factor graph in the form of reprojection residuals, instead of directly incorporating the gimbal's head angle into the state vector for joint optimization. If the gimbal's yaw and pitch angles are added to the factor graph as independent optimization variables, the state dimension of each frame will increase by an additional 2 dimensions, and the corresponding information matrix dimension will increase from 9 dimensions to 11 dimensions per frame. This not only increases the computational complexity of data storage and optimization but also introduces additional nonlinear coupling terms, leading to an increase in the number of iterations required for optimization convergence. This embodiment, through... The method of substituting known quantities not only makes full use of the observation information of the gimbal, but also maintains the compactness of the state dimension of the factor graph, effectively controlling the computational overhead of backend optimization.

[0149] This approach introduces both the current observation geometry and head pose uncertainty when updating visual constraints, enabling the reprojection residual to accurately reflect the matching deviation between the current image and the map, while the weight coefficients reflect the reliability of the gimbal pose estimation. This allows the visual constraints to have an influence that matches the current state in factor graph optimization.

[0150] By adopting the above method, the new visual constraint factor can accurately participate in the optimization based on the projection deviation between the current frame and the map points, and the weight coefficient can be adaptively adjusted according to the uncertainty of the head fusion posture, thereby improving the stability of the coupling optimization of chassis position estimation and gimbal posture, and enhancing the consistency of positioning results in complex environments.

[0151] S304. Based on the new visual constraint factors and their weight coefficients, perform factor graph optimization to obtain the current chassis position covariance matrix.

[0152] The current chassis position covariance matrix serves as the uncertainty information for the current chassis. Factor graph optimization involves integrating multi-source observation constraints into the factor graph as factor nodes and solving for the optimal values ​​of state variables using a nonlinear optimization algorithm. This is the core method for SLAM systems to calculate pose. The state variables include chassis pose, velocity, and IMU bias for each frame, as well as the 3D coordinates of map points. The chassis position covariance matrix is ​​a statistical result output after optimization convergence, used to quantify the uncertainty of the chassis 3D position estimation. This application does not limit the specific solver used for factor graph optimization or the method for extracting the covariance matrix.

[0153] In one example, a new visual constraint factor with dynamic weight coefficients can be combined with existing constraint factors such as odometry constraint factor, IMU pre-integration constraint factor, and historical visual constraint factor to construct an optimization objective function. The Levenberg-Marquardt algorithm is used to perform nonlinear optimization. After the algorithm converges, the submatrix of the chassis position dimension is extracted from the corresponding Hessian matrix and inverted to obtain the chassis position covariance matrix at the current time.

[0154] Specifically, an optimization objective function is constructed based on the new visual constraint factors, their weight coefficients, and the original constraint factors and their weight coefficients. The optimization objective of the objective function is to minimize the sum of the weighted Mahalanobis distances of all constraint factors. Based on the objective function, factor graph optimization is performed to obtain the current chassis position covariance matrix.

[0155] The original constraint factors refer to the observation-type constraints that are pre-integrated in the factor graph optimization framework, excluding the newly added visual constraints. These may include wheel speed odometer constraints, IMU pre-integration constraints, and historical visual constraints, each with a fixed weight coefficient to characterize its inherent observation confidence. The weighted Mahalanobis distance, an error metric combining observation noise distribution, is the distance value calculated by normalizing the residual vector of the constraint factors through their corresponding weights. It reflects the influence of constraints with different confidence levels on the optimization results; the higher the weight of the constraint, the greater the proportion of its corresponding Mahalanobis distance in the optimization objective. The optimization objective function aims to minimize the sum of the weighted Mahalanobis distances of all constraint factors, serving as the core solution basis for factor graph nonlinear optimization. Factor graph optimization is a process of iteratively solving state variables such as chassis pose based on a graph optimization framework. Upon convergence, it simultaneously yields the optimal state estimate and the corresponding uncertainty statistics. The chassis position covariance matrix is ​​a matrix extracted from the optimization results specifically to quantify the uncertainty of the chassis 3D position estimate. The embodiments of this application do not limit the specific composition of the original constraint factors or the type of solution algorithm used for factor graph optimization.

[0156] In one example, a new visual constraint factor with dynamic weight coefficients can be incorporated into the optimization system along with original constraint factors such as wheel speed odometer constraint factors and IMU pre-integration constraint factors with inherent confidence weights. The weighted Mahalanobis distance term corresponding to the residual of each type of constraint factor is calculated separately. After summing all the distance terms, the overall optimization objective function is constructed, with the optimization direction being to minimize the sum of the weighted Mahalanobis distances of all constraints. Subsequently, the Levenberg-Marquardt algorithm is used to perform nonlinear iterative solution on the objective function. After the algorithm converges, the optimal chassis pose estimation result is obtained. At the same time, the inverse of the Hessian matrix corresponding to the optimization process is used to obtain the complete pose covariance matrix. The submatrix corresponding to the three-dimensional position dimension of the chassis is extracted from it, which is the chassis position covariance matrix at the current time.

[0157] For example, taking the original constraint factors including the IMU pre-integration constraint factors as an example, in two frames and Between these measurements, the angular velocity and specific force of the chassis IMU are pre-integrated to obtain 9-dimensional relative motion observations. (Including location increments) Speed ​​increment attitude increment Simultaneously, obtain the state prediction values ​​of adjacent frames. , The predicted value is calculated from the state prediction values ​​of the two frames. The difference between the two is the IMU pre-integration residual. ,Right now The covariance matrix of the residual It is derived from the original IMU noise (angular velocity white noise, acceleration white noise, bias random walk) through discrete error propagation, and characterizes the uncertainty of the pre-integration measurement.

[0158] Furthermore, the optimization objective function can be expressed as: ;in, Represents the square of the Mahalanobis distance; This represents the optimal state vector, which is the optimal solution obtained after the optimization solution converges. For example, it may include core state variables such as the chassis pose output by the optimization. This represents the state variable that minimizes the objective function on the right. The possible values ​​of ; The vector of state variables to be optimized can include, for example, chassis attitude and map point position at each time step, and is the core solution object of factor graph optimization. This represents the pixel feature detection noise covariance matrix, characterizing the measurement noise level of image feature point extraction itself. It belongs to the background noise parameter of camera visual observation and is a fixed calibration value. It can be based on the pixel feature detection noise variance. Obtain, represented as .

[0159] It is understandable that factor graph optimization is essentially a weighted least squares problem. After the algorithm converges, the uncertainty of all state variables is described by the inverse of the information matrix (Hessian matrix), i.e., the full-state covariance matrix. Furthermore, the 3×3 chassis position covariance matrix at the current moment, corresponding only to the three-dimensional position of the chassis, can be extracted from the full-state covariance matrix through marginalization elimination and sub-block extraction. (Unit is) ).

[0160] In this implementation, the new visual constraint factors and the original constraint factors jointly participate in the construction of the objective function, enabling the current chassis position covariance matrix to simultaneously incorporate information from both new observations and historical constraints. Weight coefficients are allocated according to uncertainty, allowing constraints with higher confidence to play a greater role in optimization. After the factor graph is solved, the current chassis position covariance matrix is ​​used to characterize the uncertainty of the current chassis position and serves as input for subsequent target observation direction determination and active observation control.

[0161] By adopting the above method, the new visual observation information can be integrated with the original positioning constraints into the same optimization framework, so that the update result of the current chassis position covariance matrix can more completely reflect the observation conditions and historical state constraints under the current environment, thereby improving the consistency and stability of positioning updates.

[0162] The robot control method provided in this application first obtains a high-precision gimbal head posture through multi-sensor fusion, simultaneously calculates the corresponding posture uncertainty, then constructs a visual constraint factor based on the coordinate transformation relationship between the gimbal and the chassis, and assigns dynamic weights to the visual constraint factor in combination with the posture uncertainty. Finally, the weighted visual constraint factor is incorporated into the factor graph for optimization, outputting the chassis position covariance matrix at the current moment. This method can accurately quantify the impact of gimbal posture error on visual observation. By adaptively adjusting the confidence level of the visual constraints through dynamic weights, it avoids positioning deviations caused by fluctuations in gimbal posture accuracy, improves the robustness and accuracy of factor graph pose calculation, and the output chassis position covariance matrix can truly reflect the current positioning quality, providing a reliable quantitative basis for subsequent active observation trigger judgment and observation direction planning, ensuring the stable operation of the entire active observation and positioning optimization closed loop.

[0163] Figure 6 Flowchart of the robot control method provided in this application Figure 3 ,like Figure 6 As shown, in this embodiment... Figure 3 Based on the embodiments, taking the uncertainty information of the current chassis as the current chassis position covariance matrix as an example, the control method of the robot provided in this application is described in detail. The method includes:

[0164] S401. Obtain the occlusion rate of the current field of view and the covariance matrix of the current chassis position after factor graph optimization.

[0165] It should be noted that this step is similar to the aforementioned step S201, and will not be repeated here.

[0166] S402. Determine the trace and the largest eigenvalue of the covariance matrix of the current chassis position.

[0167] The trace of the chassis position covariance matrix is ​​the sum of the values ​​of all elements on the main diagonal of the matrix, corresponding to the sum of the variances of the chassis's three-dimensional position in all directions. It is used to quantify the overall uncertainty of the chassis position; a larger trace value indicates a larger overall positioning error. (Maximum eigenvalue) It is the maximum value among all eigenvalues ​​obtained after eigenvalue decomposition of the covariance matrix, with units of 1200 ppm. This represents the maximum positional uncertainty in a single direction. The embodiments of this application do not limit the specific implementation method of matrix eigenvalue decomposition.

[0168] In one example, eigenvalue decomposition can be performed on the current chassis position covariance matrix to obtain three orthogonal eigenvalues ​​and their corresponding eigenvectors. The eigenvalue with the largest value is then selected. Simultaneously, the three variance elements along the main diagonal of the matrix are summed to obtain the trace of the current chassis position covariance matrix. The unit is ,in, , , This represents the variance of the three-dimensional position.

[0169] S403. Determine whether any one of the following conditions is met: the trace is greater than the first preset threshold, the maximum feature value is greater than the second preset threshold, or the occlusion rate of the current field of view is greater than the third preset threshold.

[0170] For example, the first preset threshold is the upper limit of tolerance for the overall positioning uncertainty of the chassis. When the trace of the covariance exceeds this threshold, it indicates that the overall positioning accuracy has fallen below an acceptable level. The first preset threshold can be, for example, 0.01, with units of... The second preset threshold is the upper limit of tolerance for unidirectional positional uncertainty. When the maximum eigenvalue exceeds this threshold, it indicates that the positioning error in a certain direction is too large, which is prone to causing drift. The second preset threshold can be, for example, 0.006, with units of... The third preset threshold is the lower tolerance limit of the current observation conditions. When the occlusion rate exceeds this threshold, it means that the existing field of view can no longer provide sufficient effective observation constraints. The third preset threshold can be, for example, 0.4. Meeting any one of the three conditions will trigger subsequent judgments, covering different types of positioning degradation scenarios. It should be noted that the specific values ​​of each preset threshold are not limited in this application embodiment and can be flexibly set according to scenario requirements and positioning accuracy requirements.

[0171] In one example, the calculated covariance trace can be compared with a preset global uncertainty threshold, the largest eigenvalue can be compared with a preset unidirectional uncertainty threshold, and the current occlusion rate can be compared with a preset occlusion threshold. If any one of these three criteria is met, the decision is made. , , If any one of the conditions is met, active observation can be performed, i.e., step S404 can be executed.

[0172] By adopting the above-mentioned determination method, the controller can promptly trigger active observation when the uncertainty of the chassis position increases or the field of view is severely obstructed, so that the acquisition of new perspectives is more in line with the current positioning requirements and the subsequent pose optimization has more effective observation constraints, thereby improving the positioning stability and the accuracy of observation triggering.

[0173] S404. Determine to perform active observation. Based on the direction of the eigenvector corresponding to the largest eigenvalue of the current chassis position covariance matrix, generate multiple candidate observation directions.

[0174] For example, the eigenvector direction corresponding to the largest eigenvalue is the spatial direction with the highest uncertainty in chassis position, and it is also the direction with the most significant benefit in positioning correction after supplementary observation. Candidate observation directions are multiple alternative observation orientations generated around the reference direction, which can avoid the problems of unusable single reference directions due to environmental occlusion or lack of effective features, thus improving the scenario adaptability of the solution. This application embodiment does not limit the number of candidate observation directions generated or the angle step size.

[0175] In one example, the eigenvector corresponding to the largest eigenvalue. Using the horizontal projection direction as the central reference, multiple discrete candidate observation directions are generated within an angle range of ±30 degrees, with a 10-degree step in the yaw dimension and a 5-degree step in the pitch dimension, so that all candidate directions cover the core area with high uncertainty.

[0176] Optionally, the candidate observation direction can also include the edge direction of the current environment map. It is understood that the outer side of the edge direction belongs to the unknown area that the robot has not yet observed. After including the edge direction as a candidate, when the gimbal turns to that direction to collect new perspective images, the environmental features of the unknown area can be extracted and added to the map at the same time, completing the expansion and update of the local mapping. This allows active observation to not only correct positioning errors but also advance the mapping progress.

[0177] S405. Evaluate the positioning correction benefits for each candidate observation direction to obtain the evaluation results.

[0178] For example, the positioning correction benefit refers to the improvement in chassis positioning accuracy that occurs when the gimbal turns to observe in the candidate direction. The evaluation result is a score obtained by quantifying the correction benefit for each candidate direction.

[0179] In one example, when evaluating the benefits of positioning correction for each candidate observation direction, the controller can construct a benefit index based on the number of visible features under the candidate direction, the uniformity of feature distribution, the complementarity with existing observation constraints, and the expected shrinkage of the largest eigenvalue of the covariance matrix, and use this benefit index as the evaluation result.

[0180] In some possible implementations, for each candidate observation direction, the observation information gain, scan time cost, and navigation cost corresponding to the candidate observation direction are calculated separately; the observation information gain, scan time cost, and navigation cost are fused to obtain the evaluation result.

[0181] Among them, the observation information gain represents the expected reduction in positioning uncertainty after observation in the candidate observation direction. It is a positive indicator for measuring the effect of observation correction. The larger the gain value, the more significant the improvement in positioning accuracy of the observation in that direction.

[0182] Scan time cost represents the total time it takes for the gimbal to rotate to the candidate observation direction and complete image acquisition. It is a negative indicator of the observation execution cost, and the longer the time, the higher the execution cost.

[0183] Navigation cost represents the additional overhead of active observation on the navigation task, including efficiency losses caused by speed adjustment, path deviation, and task delay.

[0184] In one example, the observation information gain can be based on the current chassis position covariance matrix, combined with the observation geometry of the candidate observation direction and the expected number of effective feature points in that direction, to predict the updated chassis position covariance matrix. The difference between the traces of the covariance matrix before and after the observation is used as the observation information gain for that direction. The scanning time cost can be calculated based on the angle deviation between the current gimbal angle and the candidate observation direction, combined with the maximum angular velocity and acceleration / deceleration parameters of the gimbal, and then the gimbal stabilization waiting time and image acquisition exposure time are added together, and the total time is used as the scanning time cost. The navigation cost can be calculated based on the angle between the candidate observation direction and the current navigation forward direction, combined with the robot's current travel speed, to estimate the chassis speed adjustment range and path deviation during the observation period, and converted into the navigation task time delay as the navigation cost. After the three indicators are calculated, each indicator is first normalized to unify the dimensions, and then multiplied by a preset weight coefficient. The information gain is positively weighted, and the two types of costs are negatively weighted. Finally, the weighted sum is used to obtain a comprehensive score, which is used as the evaluation result of the candidate observation direction.

[0185] This approach avoids the limitations of single-dimensional decision-making, avoids neglecting execution costs and navigation efficiency in pursuit of positioning benefits, and avoids sacrificing positioning correction effects in pursuit of excessive cost control. It achieves a reasonable balance between improving positioning accuracy, institutional operating costs, and navigation efficiency, making active observation decisions more aligned with actual operating scenarios and enhancing the engineering practicality and scenario adaptability of the entire active observation strategy.

[0186] Specifically, based on the candidate observation directions, visible map points under the candidate observation directions are determined; for each visible map point, the visible map point is reprojected onto the pixel plane to determine the Jacobian matrix of the reprojection error; where the Jacobian matrix of the reprojection error characterizes the observation sensitivity of the visible map point to the chassis position; based on the Jacobian matrix corresponding to each visible map point, the corresponding total information matrix increment for the candidate observation direction is determined; based on the current chassis position covariance matrix and the total information matrix increment, the posterior position covariance matrix is ​​determined; the difference between the trace of the current chassis position covariance matrix and the trace of the posterior position covariance matrix is ​​determined as the observation information gain for that candidate observation direction.

[0187] Visible map points refer to 3D map points that are within the field of view of the camera corresponding to the candidate observation direction and within the effective observation distance interval. The Jacobian matrix of the reprojection error characterizes the rate of change of the reprojection error with the chassis position, and is used to quantify the observation sensitivity of a single visible map point to the chassis position. The higher the magnitude of the matrix element, the stronger the constraint effect of the map point on the position error in the corresponding direction. The total information matrix increment is the cumulative result of the observation information contribution of all visible map points, representing the amount of constraint information that the observation in this direction can add to the positioning system. The larger the increment, the stronger the pose constraint brought by the observation. The posterior position covariance matrix is ​​the covariance matrix corresponding to the chassis position estimate after incorporating the observation constraint of the candidate observation direction, and is used to quantify the remaining position uncertainty after the observation is completed. The observation information gain is the difference between the trace of the current prior covariance matrix and the trace of the posterior covariance matrix, which intuitively reflects the degree of reduction in overall positioning uncertainty that a single observation can reduce.

[0188] For example, the controller can determine whether a map point falls within the camera's field of view and is not occluded based on the gimbal attitude corresponding to the current candidate observation direction, combined with camera intrinsic and extrinsic parameters and map point coordinates, thereby identifying the visible map point under the candidate observation direction. For each visible map point, it is projected onto the pixel plane via the current chassis pose and gimbal attitude to obtain predicted pixel coordinates. A reprojection error is constructed by comparing this error with the observed pixel coordinates of the corresponding image feature points. The partial derivative of this error with respect to the chassis position parameters is then used to obtain the Jacobian matrix. Each Jacobian matrix can be accumulated according to an information filtering method and combined with the observation noise covariance to construct the total information matrix increment. Subsequently, this increment is fused with the current chassis position covariance matrix to obtain the posterior position covariance matrix. The difference between the traces of the covariance matrices before and after fusion is used as the observation information gain, allowing the controller to quantify the contribution of the candidate observation direction to the reduction of positioning uncertainty.

[0189] For example, based on candidate observation directions Based on the current chassis pose and the camera's field of view angle, the visible space range of the camera in that direction is calculated. Then, the 3D map points in the global / local maps are traversed, and map points falling within the camera's field of view and within the effective observation range are selected to form a set of visible map points. For each visible map point i in the set, based on the pinhole camera observation model, the visible map point is projected onto the pixel plane, and the Jacobian matrix of the reprojection error relative to the chassis's 3D position is derived. According to the formula for information filtering Calculate the incremental information that the visible map point can provide. ,in, This is the pixel measurement noise covariance matrix, representing the noise level of a single pixel observation. It is a calibration value, determined by camera resolution and feature extraction accuracy. Since the observation noise of different map points is independent, the increment of the total information matrix corresponding to this candidate observation direction... It can be represented as The prior information matrix at the current moment. Chassis position covariance matrix The reverse, that is Therefore, by superimposing the observed information increments, we obtain the post-observational posterior information matrix. It can be represented as Furthermore, by inverting the posterior information matrix, the posterior position covariance matrix is ​​obtained. Finally, the difference between the trace of the current chassis position covariance matrix and the trace of the posterior position covariance matrix is ​​determined as the observation information gain for that candidate observation direction. ,Right now The unit is .

[0190] It should be noted that the above steps are based on the information update principle of extended Kalman filtering, which is a predictive approximation calculation. That is, it predicts the benefit before observation, rather than the actual accuracy improvement after actual observation.

[0191] The above processing enables the selection of candidate observation directions to be directly based on the number of visible features, the geometric distribution of features, and their sensitivity to pose estimation. This allows the observation information gain to reflect the actual change in the ability of the direction to constrain the chassis position, thus providing a comparable numerical basis for subsequent target observation direction selection.

[0192] Specifically, the rotation angle is determined based on the angle corresponding to the current gimbal angle and the candidate observation direction. If the rotation angle is greater than the preset rotation angle threshold, the acceleration, deceleration, and constant speed phases of the gimbal are calculated according to the trapezoidal velocity curve, and the total rotation time of the gimbal is obtained by summing the acceleration, deceleration, and constant speed phases. The preset rotation angle threshold is the ratio of the square of the maximum angular velocity of the gimbal to the maximum angular acceleration. If the rotation angle is less than or equal to the preset rotation angle threshold, the total rotation time of the gimbal is determined according to the trigonometric velocity curve. The total rotation time of the gimbal is then superimposed with the preset image acquisition and processing time to obtain the scanning time cost corresponding to the candidate observation direction.

[0193] The rotation angle amplitude is the angular deviation between the current gimbal orientation and the candidate observation direction, representing the total angle of rotation required for the gimbal to complete the turn. The preset rotation angle threshold is the critical rotation angle that distinguishes the gimbal's motion mode. Its value is the ratio of the square of the gimbal's maximum angular velocity to its maximum angular acceleration. It corresponds to the total rotation angle from a stationary position where the gimbal accelerates uniformly to its maximum angular velocity and then decelerates uniformly back to a stationary position. When the rotation angle exceeds this threshold, the gimbal exhibits a uniform motion phase; otherwise, there is no uniform motion phase.

[0194] The trapezoidal velocity curve is a classic velocity model for gimbal motion planning. The motion process is divided into three stages: uniform acceleration, uniform velocity, and uniform deceleration. It is suitable for large-angle turning scenarios. The duration of the acceleration stage is the time required for the gimbal to accelerate uniformly from a standstill to its maximum angular velocity. The duration of the deceleration stage is the time required for the gimbal to decelerate uniformly from its maximum angular velocity back to a standstill. The duration of the uniform velocity stage is the time required for the gimbal to maintain a stable rotation at its maximum angular velocity.

[0195] The triangular velocity curve is a velocity model without a uniform velocity segment. The motion process only includes two stages: uniform acceleration and uniform deceleration. The velocity curve is triangular in shape and is suitable for small turning angle scenarios.

[0196] The image acquisition and processing time is the fixed time required after the gimbal rotates to its position, waits for the mechanism to stabilize, and completes image acquisition and front-end feature processing. The scanning time cost is the total time required to complete a single active observation. This application embodiment does not limit the specific values ​​of the gimbal's maximum angular velocity and maximum angular acceleration, nor does it limit the setting of the image acquisition and processing time; these can be flexibly configured according to the gimbal's hardware parameters and actual operating conditions.

[0197] In one example, the differences between the current gimbal yaw and pitch angles and the corresponding angles of the candidate observation directions can be calculated first. The synthesized rotation angle amplitude can then be obtained by calculating the vector magnitude. Next, based on the gimbal hardware parameters, the ratio of the square of the maximum angular velocity to the maximum angular acceleration can be substituted to calculate the preset rotation angle threshold. When the rotation angle amplitude is greater than the preset rotation angle threshold, a trapezoidal velocity curve is used for calculation. First, the maximum angular velocity is divided by the maximum angular acceleration to obtain the acceleration phase duration. The deceleration phase duration is equal to the acceleration phase duration. Then, the acceleration phase duration is subtracted from the total rotation angle amplitude. The rotation angle corresponding to the deceleration phase is used to divide the difference by the maximum angular velocity to obtain the duration of the uniform speed phase. The durations of the three phases are summed to obtain the total rotation time of the gimbal. When the rotation angle amplitude is less than or equal to the preset rotation angle threshold, the trigonometric velocity curve is used for calculation. The gimbal has no uniform speed phase throughout the entire process. The total rotation time is derived from the rotation angle amplitude and the maximum angular acceleration. Finally, the calculated total rotation time of the gimbal is superimposed with the preset gimbal stabilization waiting, image acquisition and feature extraction processing time to obtain the scanning time cost corresponding to the candidate observation direction.

[0198] For example, the current gimbal angle is The target angle corresponding to the candidate observation direction is The absolute difference between the two is the total angle that needs to be rotated: Based on maximum angular velocity (Unit: rad / s) and maximum angular acceleration (Unit is) Determine the corner threshold. .

[0199] like This indicates that the turning angle is large enough that the gimbal can accelerate to its maximum angular velocity and maintain a constant speed. A trapezoidal velocity curve can be used to calculate the duration of the acceleration phase, deceleration phase, and constant speed phase of the gimbal. The acceleration phase duration... deceleration phase duration Equal to the duration of acceleration ( ); the angle that needs to be turned during the constant speed segment Therefore, the duration of the uniform speed segment The total gimbal rotation time is obtained by summing the durations of the acceleration, deceleration, and constant speed phases. ,Right now .

[0200] like This indicates that the turning angle is very small, and the gimbal must decelerate before reaching its maximum speed. The entire rotation involves only acceleration and deceleration, so a triangular velocity curve can be used. At this point, the total rotation time of the gimbal is... It is understandable that the acceleration time is assumed to be... Total turning angle Solving for The total rotation time of the gimbal is the sum of acceleration and deceleration. .

[0201] Furthermore, after the gimbal has rotated into position, it is necessary to wait for the mechanical vibration to stabilize and for image acquisition and feature processing to be completed; this part has a fixed time consumption. (For example, it could be 0.1s), final total scan time .

[0202] This approach integrates the time spent on gimbal rotation and image acquisition and processing into a single evaluation metric, ensuring that the cost of candidate observation directions reflects the total time overhead of actual active observation. Based on this, when comparing candidate observation directions, the controller can identify directions with larger rotation distances and higher acquisition and processing times as high-cost directions, and combine this with indicators such as information gain to create time constraints on the active observation scheme.

[0203] By adopting this method, the scanning time cost can be matched with the actual motion capability of the gimbal, and the cost calculation result is highly consistent with the actual execution time. This makes the selection result of candidate observation direction more in line with the real-time requirements of the robot in continuous operation scenarios, and enables the observation direction evaluation to more accurately reflect the occupation of the overall control cycle by gimbal rotation and image processing.

[0204] Specifically, starting from the current chassis pose and ending at the next navigation waypoint, a baseline path planning is performed to obtain baseline indicators. The baseline indicators include baseline mileage, baseline travel time, and baseline cumulative steering angle. The gimbal is simulated to rotate to the candidate observation direction for observation, and post-observation path planning is performed with the same start and end points to obtain post-observation indicators. The post-observation indicators include post-observation mileage, post-observation travel time, and post-observation cumulative steering angle. Based on the baseline indicators and post-observation indicators, the mileage difference, time difference, and steering angle difference are calculated respectively. The absolute values ​​of the mileage difference, time difference, and steering angle difference are weighted and summed to obtain the navigation cost corresponding to the candidate observation direction.

[0205] The baseline path planning is a conventional local path planning method used when no active observation is performed. It outputs the ideal driving baseline under observation-free conditions. The next navigation waypoint is the next target point preset in the global navigation task and is the endpoint of this local path planning. The baseline driving distance is the total driving distance corresponding to the baseline path. The baseline travel time is the total time required to traverse the baseline path using a conventional driving strategy. The baseline cumulative steering angle is the cumulative value of the steering angles required for the entire baseline path. These three together constitute the baseline indicators of the navigation task, used to measure the additional costs incurred by active observation.

[0206] Post-observation path planning simulates a scenario where the gimbal turns to a candidate observation direction to perform active observation. It involves re-planning the path with the same starting and ending points to recreate the actual impact of the observation action on the driving process. Post-observation mileage, post-observation travel time, and post-observation cumulative steering angle correspond to the total travel distance, total travel time, and cumulative steering angle under the observation scenario, respectively. Mileage difference, time difference, and steering angle difference represent the differences between the post-observation parameters and the baseline parameters, characterizing the additional overhead brought by active observation in each dimension. Weighted summation of the absolute values ​​of the three types of differences integrates the overhead of different dimensions into a unified comprehensive cost. The weights can be flexibly set according to the task's sensitivity to mileage, timeliness, and steering losses. Navigation cost quantifies the additional consumption caused by active observation actions to the original navigation task; it is a negative indicator in the comprehensive evaluation of candidate directions, with higher cost indicating greater interference with the navigation task. This application does not limit the specific algorithm used for path planning or the weight coefficients corresponding to each difference; they can be flexibly configured according to the robot's kinematic characteristics and task requirements.

[0207] In one example, the following can be used: The algorithm combines chassis kinematic constraints to plan the optimal path from the current chassis pose to the next navigation waypoint in the current local grid map without observation interference. The total path length is calculated to obtain the baseline driving mileage, and the baseline travel time is calculated by combining the normal driving speed sequence. The baseline cumulative steering angle is obtained by accumulating the steering angle changes throughout the path. Then, the algorithm simulates the gimbal turning to the candidate observation direction to perform active observation, incorporating constraints such as chassis deceleration and slight path adjustments to adapt to the observation attitude during the observation period. The driving path is replanned with the same start and end points, resulting in the post-observation driving mileage, post-observation travel time, and post-observation cumulative steering angle. The difference between the post-observation parameters and the baseline parameters is calculated, and the absolute value of each difference is multiplied by preset mileage weight, time weight, and steering weight. The weighted results are summed to obtain the navigation cost corresponding to the candidate observation direction.

[0208] For example, starting from the current chassis pose and ending at the next navigation waypoint, a normal path planning operation is performed on the local map to obtain the baseline metric without active observation, namely the baseline driving mileage. Baseline travel time Reference cumulative steering angle (The sum of the absolute values ​​of all steering angles along the path); simulating real active observation conditions, where the chassis is paused or traveling at low speed during the gimbal rotation observation period. After the observation is completed, the path is rerouted from the current state to the same endpoint to obtain the post-observation index, i.e., the mileage traveled after the observation. Travel time after observation Cumulative steering angle after observation Calculate the increment of the observed index relative to the baseline index, i.e., the mileage difference. Time difference Steering angle difference (Take the absolute value, because both left and right turns incur costs); Sum the differences across the three dimensions using weighted averages to obtain the final navigation cost. ,Right now Among them, mileage weighting =0.3, time weight =0.5, turning weight =0.2, all of which are empirical weights, reflecting the impact of mileage consumption, time delay, and steering maneuvering on navigation efficiency, respectively.

[0209] Based on the above approach, navigation cost can quantify the impact of active observation on the subsequent navigation path into a unified scalar result. By comparing the path planning results before and after observation under the same start and end conditions, navigation cost can directly reflect the additional consumption of gimbal observation on the robot's movement process, thus providing a calculable and comparable basis for selecting the target observation direction.

[0210] Specifically, the observation information gain, scan time cost, and navigation cost are fused to obtain the evaluation result. For example, the controller can set a correspondence between the evaluation result, observation information gain, scan time cost, and navigation cost, and then substitute the calculated parameters into this correspondence to obtain the evaluation result for the candidate observation direction. This correspondence could be, for example,... ;in, This indicates the evaluation results for candidate observation directions.

[0211] It should be noted that, in order to achieve dimensional uniformity for multi-dimensional indicators, the scanning time cost, navigation cost, and observation information gain are unified to... The fusion assessment is conducted under different dimensions, with two types of weighting coefficients set to complete the dimension conversion and penalty intensity adjustment respectively. The specific parameter definitions and adaptation rules are as follows: Time cost weighting The unit is The typical value is 0.5. This is used to control the intensity of the penalty for scanning time cost; The higher the value, the greater the weight of time cost in the overall evaluation. The algorithm will be more inclined to choose the direction with the shortest gimbal rotation time, rather than simply pursuing the direction with the greatest information gain. Navigation cost weight. It is a dimensionless scalar, typically with a value of 0.3, used to control the penalty strength of navigation costs; The larger the value, the stronger the penalty for path deviation, and the more the algorithm will tend to choose the observation direction that does not deviate from the original navigation path. Both types of weights can be flexibly adjusted according to the robot's task scenario. For example, in the mapping and exploration phase, the core objective is to advance the mapping progress and ensure positioning accuracy, which requires encouraging active observation actions. Therefore, the smaller values ​​for both types of weights should be selected, for example, they can be configured as follows: =0.3、 =0.2, appropriately reducing the constraint strength of time cost and navigation deviation; in the precise navigation phase, the core objective is to ensure traffic efficiency and control task delay, therefore the two weights should take the larger value, for example, it can be configured as =0.8、 =0.5, which strengthens the penalty for time and navigation costs, and reduces unnecessary path deviations and task time consumption.

[0212] S406. Based on the evaluation results, select the optimal candidate observation direction as the target observation direction.

[0213] For example, the target observation direction This is the final selected gimbal orientation for performing active observations.

[0214] In one example, all candidate observation directions can be sorted from highest to lowest according to the evaluation results, and the candidate observation direction with the highest ranking can be selected as the target observation direction.

[0215] Optionally, if the evaluation results of multiple candidate observation directions are the same, the direction with the smallest change in current gimbal attitude can be selected to reduce the gimbal rotation burden.

[0216] The purpose of this step is to autonomously decide "whether to turn the head" and "which direction to turn" when the positioning uncertainty is too high or the field of view in front is blocked, so as to obtain the maximum positioning information gain at the lowest cost.

[0217] S407. Calculate the gimbal rotation cost when the gimbal rotates to the target observation direction, and the chassis rotation cost when the chassis as a whole rotates to the target observation direction.

[0218] For example, the cost of gimbal rotation This refers to the comprehensive cost incurred by the gimbal independently rotating to the target observation direction, typically including rotation time, motor energy consumption, and impact on image stability. Chassis steering cost. This refers to the overall cost incurred in achieving the same observation orientation by rotating the entire chassis. Besides the time and energy consumed during rotation, it also includes efficiency losses caused by deviating from the original navigation path and interrupting the normal travel process. The core purpose of comparing these two types of costs is to select the method with the lower overall cost for observation correction, avoiding excessively high operating costs for supplementary observations.

[0219] In one example, the gimbal rotation cost can be characterized by the gimbal rotation angle amplitude, rotation duration, control energy consumption, and the impact on image acquisition timing during rotation. The chassis steering cost can be characterized by the chassis rotation angle amplitude, steering distance, steering time, and disturbance to the current navigation trajectory. In practical applications, both the gimbal rotation cost and the chassis steering cost can be calculated in real time by the controller based on the current gimbal attitude, chassis heading angle, and target observation direction. The model used can be a linear weighted model, a normalized scoring model, or other equivalent cost models; this application does not limit this.

[0220] In the specific implementation, after receiving the target observation direction, the controller first reads the current gimbal angle and the current chassis heading angle, and calculates the angle difference between the gimbal and the target observation direction and the angle difference between the chassis and the target observation direction, respectively. Then, combined with the maximum angular velocity and maximum angular acceleration of the gimbal, the maximum allowable steering angular velocity of the chassis, and steering control constraints, the controller calculates the gimbal rotation cost and chassis steering cost required for both to reach the target direction.

[0221] S408. Determine whether the cost of gimbal rotation is less than the cost of chassis steering.

[0222] If yes, proceed to step S409; otherwise, the gimbal rotation control can be omitted, and step S410 can be executed. By introducing a cost comparison before gimbal rotation, the two observation alignment methods—gimbal and chassis—are selected under the same evaluation criterion. This allows the controller to select the action with the lower cost to complete the target observation direction alignment and maintains better coordination between observation direction adjustment and navigation execution. Since gimbal rotation only affects the gimbal and does not disturb the chassis's attitude, this method is used when the gimbal cost is lower than the chassis cost. This keeps the control overhead of observation adjustment at a low level and allows image acquisition and subsequent factor map optimization to be performed more continuously.

[0223] S409. Control the gimbal to rotate to the target observation direction, acquire new perspective images through image acquisition equipment, and perform factor graph optimization processing based on the new perspective images to obtain a new chassis pose.

[0224] For example, if the cost of gimbal rotation is less than the cost of chassis steering and obstacle avoidance safety constraints are met, the controller outputs a gimbal rotation command, causing the gimbal to rotate around the yaw and pitch degrees of freedom to the target observation direction, while maintaining the chassis's current travel state. The obstacle avoidance safety constraints are safety thresholds for determining whether to perform gimbal rotation-based active observation. They ensure that the gimbal's steering observation action does not cause collision risks or weaken the robot's obstacle avoidance capabilities. These constraints may include self-collision safety constraints (no mechanical interference with its own structure), environmental collision safety constraints (no collision with environmental objects), and travel obstacle avoidance perception constraints (no excessive forward perception blind spots due to gimbal turning). This application does not limit the content of the obstacle avoidance safety constraints; they can be set based on actual needs.

[0225] S410, Maintain current field of view.

[0226] For example, maintaining the current field of view means not initiating active observation, keeping the original observation orientation of the image acquisition device unchanged, and continuing to perform the conventional SLAM localization and navigation tasks, avoiding the additional overhead caused by unnecessary mechanical movements. This branch is suitable for scenarios where chassis steering costs are lower, or where the overall benefits of active observation are insufficient to cover the execution costs. This application embodiment does not limit the subsequent conventional processing flow after maintaining the current field of view.

[0227] In one example, when the cost of chassis steering is determined to be less than or equal to the cost of gimbal rotation, it indicates that the method of independent observation by gimbal does not have a cost advantage. Therefore, the active observation operation of gimbal rotation is not performed, the current observation orientation remains unchanged, and the original navigation path is continued to be followed. The positioning and mapping are completed by relying on conventional multi-source observation constraints.

[0228] The robot control method provided in this application first obtains the current field-of-view occlusion rate and the chassis position covariance matrix output by the factor map. It then comprehensively determines whether to trigger active observation based on thresholds across three dimensions: overall uncertainty, single-direction uncertainty, and observation occlusion. Upon triggering, multiple candidate observation directions are generated based on the direction with the highest uncertainty. The optimal target observation direction is selected through a positioning correction benefit evaluation. Next, the execution costs of independent gimbal rotation and overall chassis steering are compared. Gimbal observation is only initiated when the cost of gimbal rotation is more favorable, acquiring new perspective images to supplement constraints and optimize chassis pose; otherwise, the original field of view is maintained for normal navigation. This method can accurately identify scenarios and timing of positioning degradation, avoiding the ineffective overhead of blindly initiating active observation. Candidate direction evaluation ensures maximum positioning correction benefits, while comparing execution costs to select the optimal implementation method. This effectively improves positioning accuracy and stability in complex scenarios without interfering with normal navigation, balancing positioning correction effects with the robot's overall operating efficiency, and enhancing the robot's adaptability in complex environments such as low-texture and high-occlusion conditions.

[0229] In some embodiments, when the gimbal rotates to the target observation direction, two-way perception processing is carried out simultaneously in the next frame. One path extracts the environmental visual features under the observation direction through the visual front end and inputs them into the factor map optimization module for pose solving and map updating. The other path acquires the depth image of the corresponding viewpoint to complete the update of the local cost map, ensuring that the new viewpoint information brought by active observation simultaneously serves the improvement of positioning accuracy and the expansion of obstacle avoidance perception (multi-source cost map fusion and navigation).

[0230] Multi-source cost map fusion and navigation: Figure 7 A schematic diagram illustrating an application scenario provided in this application, such as... Figure 7 As shown, the depth map output by the head camera, the LiDAR point cloud, and the semantic perception results are fused to generate a navigation cost map that the chassis can directly use for path planning and motion control, and finally outputs the chassis's motion speed command. The inputs to this stage include the target observation direction, used to determine the current orientation of the head camera to complete the spatial coordinate projection of the depth data, the local map updated by the localization and mapping module, used to delineate the basic passable area, and also three types of perception data: head depth map, LiDAR point cloud, and semantic obstacle detection results; the final output is a 2.5D local cost map and chassis speed control commands.

[0231] To ensure safe movement during operation, a dynamic start-stop mechanism for active observation is implemented. Specifically, for each grid cell in the local map, the controller calculates the occupancy cost corresponding to the LiDAR, the occupancy cost obtained from the head depth map projection, and the obstacle cost output by semantic detection. The maximum value among these three costs is then taken as the comprehensive cost for that grid cell. This ensures that if any sensor detects an obstacle risk, the corresponding grid cell will maintain a conservative cost value, avoiding safety hazards caused by a single sensor missing a detection. The controller monitors the distance to environmental obstacles and the chassis movement status in real time. When the minimum obstacle distance is detected to be less than 0.4 meters, or the chassis is in a sharp turn (chassis angular velocity greater than a preset threshold) or emergency braking state, the active observation process is immediately interrupted, and the triggering conditions for active observation are suspended, prioritizing the safety of navigation and obstacle avoidance. When the minimum obstacle distance is detected to be greater than 0.6 meters for 10 consecutive frames, and the chassis angular velocity is less than 15 degrees per second for 10 consecutive frames, the controller determines that the current driving environment is stable and safe, and then resumes the active observation function, with the triggering conditions for active observation reappearing. Based on the updated local cost map, the navigation controller uses the Model Predictive Control (MPC) algorithm to solve the trajectory tracking problem. The optimization objective is to minimize the weighted cumulative value of the trajectory tracking error and the change in control quantity. The solution obtains the linear velocity increment and angular velocity increment commands of the chassis, which are then output to the chassis actuator to complete the motion control.

[0232] Loop closure detection and global correction: When the robot returns to the historical observation area, loop closure constraints are used to eliminate accumulated drift during the localization process, resulting in a globally consistent pose trajectory and map. The inputs to this stage include the pose graph and keyframe library, as well as image data acquired by the gimbal's active surround view. Leveraging the gimbal's multi-degree-of-freedom rotation capabilities, it can cover side and rear scenes that cannot be observed by the chassis-fixed sensors, significantly expanding the effective observation range of loop closure detection. The final output of this stage is the globally corrected pose in the chassis's world coordinate system. The controller encodes the current frame image and historical keyframes into global descriptors. These global descriptors can be generated using a visual bag-of-words model or a global convolutional neural network (CNN) descriptor network. Then, the cosine similarity between the current frame descriptor and the historical keyframe descriptors is calculated. When the cosine similarity is greater than 0.75, the corresponding historical keyframe is determined to be a candidate loop closure frame. After confirming the loop closure, the controller will perform pose graph optimization to obtain a globally consistent pose trajectory, with the optimization objective being to minimize the sum of the odometry constraint residuals and the loop closure constraint residuals between adjacent keyframes. The chassis pose, after global correction, will be written back to the factor graph as a new prior constraint to participate in subsequent pose optimization solutions. It will also provide a more accurate pose reference for active observation, supporting subsequent observation direction planning and navigation path tracking.

[0233] In some embodiments, during robot operation, the controller continuously and synchronously acquires multi-source sensor data, specifically including an incremental photoelectric encoder chassis odometer mounted on the drive wheel axle with a resolution of no less than 1024 pulses per revolution, an IMU mounted on the chassis geometric center, a gimbal encoder with a resolution of 0.01 degrees, a head camera mounted on the top of the gimbal via a shock-absorbing bracket, and a LiDAR connected to the controller via Gigabit Ethernet. Within the back-end factor graph optimization framework, the head pose is not included in the state vector as an independent optimization state to avoid continuous expansion of the state dimension. Instead, it is used as a measurement input system after gap compensation. Simultaneously, it combines encoder noise, IMU residuals, and gap estimation errors to construct the corresponding measurement covariance, which jointly participates in the construction of the visual reprojection constraint factor.

[0234] Furthermore, based on the trace of the chassis position covariance matrix and the current field of view occlusion rate, the target rotation angle of the gimbal is calculated and generated. The motion command is sent to the gimbal driver via CAN bus or RS-485 bus to control the gimbal to prioritize active observation towards the map edge, low texture area and potential loop area, so as to maximize the positioning correction and mapping expansion benefits of a single observation.

[0235] Furthermore, the depth information output by the head camera is projected and updated onto the local cost map. The optimal chassis speed command is obtained through model predictive control algorithm and sent to chassis motor drivers such as servo drives. When navigation safety constraints are triggered, the gimbal active scanning task gives way to the chassis obstacle avoidance task, the active observation trigger condition is suspended, and the system prioritizes the safety of robot movement.

[0236] After receiving the speed command, the chassis motor driver controls the DC brushless motor to drive the differential wheel or Mecanum wheel to perform the corresponding movement. The encoder at the motor end provides real-time feedback of the speed information, forming a complete speed closed-loop control to ensure the chassis movement accuracy and operational stability.

[0237] The robot control method provided in this application relies on the active degrees of freedom of the head gimbal to replace the large-scale rotation of the chassis to acquire high-information-gain observation data. When the uncertainty of the chassis position increases or the forward field of view occlusion rate exceeds a preset threshold, the robot does not need to perform a stationary body rotation. Instead, it can supplement the observation information of map edges, low-texture areas, and potential loop areas by rapidly rotating the gimbal within a small range of yaw ±90° and pitch ±30°, without interrupting the normal movement of the chassis. This enhances the positioning effect. Related performance indicators are around 500. The test was conducted in a dynamic indoor environment, with the test conditions set at a pedestrian density of 0.1–0.3 people / km. The robot's average travel speed is 0.5–1.0 m / s. Actual test results show that the root mean square error of localization was reduced from approximately 6–9 cm in the fixed sensor SLAM scheme to approximately 3–4 cm, a reduction of approximately 45%–55%. In terms of operational efficiency and energy consumption, by relying on active observation to replace body steering, the number of non-navigational in-situ rotations performed by the chassis to supplement observation was reduced by approximately 70%, and the energy consumption per unit distance for steering was reduced by approximately 25%. Regarding perception and mapping performance, the loop closure detection success rate increased from approximately 75% to over 90%, the effective map coverage increased by approximately 50%, and the local obstacle avoidance response time was less than 80 ms. It should be noted that the above data are actual reference values ​​measured under typical indoor conditions; specific values ​​will vary depending on the environmental structure, dynamic target density, and sensor configuration.

[0238] In related technologies, mobile robots and humanoid robots typically rely on sensors fixed to their chassis for localization and mapping. Several solutions exist for achieving autonomous localization and navigation, but each has significant limitations.

[0239] (1) Chassis-fixed sensor SLAM solution: The sensor is rigidly bound to the chassis, and the pose of the camera or LiDAR is directly equivalent to the pose of the chassis. In environments with low texture, high occlusion and dynamic pedestrian flow, the fixed field of view cannot actively avoid areas with missing features, which will result in sparse pose constraints, ill-conditioned back-end optimization solutions, and consequently, the positioning error will drift with the accumulation of mileage, map update will lag, and loop closure detection will fail.

[0240] (2) Passive chassis rotation supplementary observation scheme: In order to compensate for the blind spots of the fixed field of view, new perspective observations are obtained by rotating the chassis in place. This scheme requires interrupting the normal navigation process, which will increase the total task time and turning energy consumption; it cannot provide effective position constraints during the rotation process, and is prone to introducing additional positioning drift; moreover, the turning timing and turning direction are mostly triggered manually or passively, and do not combine the current positioning uncertainty and field of view occlusion rate to achieve intelligent scheduling, resulting in insufficient observation targeting.

[0241] (3) Gimbal multi-degree-of-freedom active vision solution: The head mechanism can move independently relative to the chassis, breaking the basic assumption of the sensor and chassis being rigidly bound in traditional SLAM; the camera pose depends not only on the chassis pose, but also on the rotation parameters of the gimbal joints. The measurement error of the joint encoder will be amplified to the camera extrinsic parameters through the kinematic link, thus causing cumulative positioning error.

[0242] (4) Head position pose tight coupling optimization scheme: After incorporating the gimbal joint angle as the optimization state variable into the factor graph, the state dimension of each frame increases and the size of the Hessian matrix expands significantly, which significantly increases the solution delay and slows down the optimization convergence speed, making it difficult to meet the real-time operation requirements of the SLAM system.

[0243] (5) Conventional gimbal mechanical transmission scheme: The gear transmission structure of the two-axis servo gimbal has mechanical backlash and hysteresis, which will form a control dead zone; after frequent turning, the backlash error gradually accumulates, which will cause the camera extrinsic parameters to drift, and the projection error under long distance observation will be further amplified; the nonlinear hysteresis introduced by the hysteresis will cause the standard linear calibration method to fail, and the error will continue to change with temperature drift and mechanical wear, requiring frequent recalibration, resulting in high maintenance costs.

[0244] (6) Passive scanning scheme without active scheduling: adopting a fixed rotation speed or grid scanning strategy, regardless of the current positioning uncertainty, the same observation logic is executed, which wastes sensor resources and cannot maximize information gain; the scanning timing is not synchronized with the path planning, and the robot is prone to deviating from the optimal navigation path in order to obtain the target observation angle, which conflicts with the navigation efficiency target. At the same time, it is impossible to predict the high occlusion direction in advance, which is prone to invalid observation.

[0245] In summary, existing solutions generally fail to address six core issues: fixed field of view is prone to positioning drift, passive chassis rotation is inefficient and consumes a lot of power, independent head movement violates the rigid body binding assumption, factor graph dimensional expansion leads to a decrease in real-time performance, gimbal gap causes camera extrinsic drift, and passive scanning lacks intelligent scheduling driven by positioning uncertainty and occlusion rate.

[0246] In view of the above limitations, the embodiments of this application have the following technical effects.

[0247] (1) Addressing the positioning drift problem caused by fixed field of view: Existing fixed sensor solutions are prone to loop closure detection failure and continuous accumulation of positioning error in low texture, high occlusion and dynamic crowd flow scenarios; This application uses a head active observation mechanism to prioritize the observation direction to map edges, low texture areas and potential loop closure areas, supplementing effective visual constraints. In a dynamic environment of about 500㎡, the root mean square error of positioning can be reduced from about 6~9cm to about 3~4cm.

[0248] (2) Regarding the problem of low efficiency and high consumption of passive rotation supplementary observation: the existing scheme requires the chassis to rotate in place for a large range to supplement the observation, which will interrupt the normal navigation journey and increase the steering energy consumption; this application replaces the whole fuselage steering with a small range of active degrees of freedom of the head, which can complete the supplementary observation without interrupting the chassis movement, reducing the number of non-navigation necessary rotations of the chassis for supplementary observation by about 70%, and reducing the steering energy consumption per unit mileage by about 25%.

[0249] (3) Regarding the problem of head independent movement breaking the rigid body binding assumption: This application uses head basis kinematic decoupling and complementary filtering fusion processing to inject head posture as an external measurement quantity carrying covariance into the positioning system, which breaks through the premise limitation of rigid binding between sensors and chassis in traditional SLAM.

[0250] (4) To address the problem of solution delay caused by factor graph dimensional expansion: Existing tightly coupled schemes include head pose in the optimization state vector, which will cause the state dimension to increase and the solution time to increase. This application does not include head pose in the state vector to be optimized, but uses measurements with covariance to participate in the construction of visual reprojection factors, which effectively avoids state dimension expansion and can maintain real-time positioning output of 10~20Hz.

[0251] (5) Regarding the problem of camera extrinsic parameter drift caused by gimbal mechanical clearance: This application uses micro-motion scanning calibration combined with IMU residual optimization to estimate the clearance compensation amount of yaw and pitch dimensions online, and corrects the encoder output angle in real time, effectively suppressing camera extrinsic parameter drift caused by transmission backlash.

[0252] (6) Addressing the lack of intelligent scheduling in passive scanning: Existing passive scanning schemes do not adjust observation strategies based on positioning uncertainty and field-of-view occlusion rate, resulting in wasted sensor resources and conflicts with navigation efficiency goals. This application performs feature decomposition on the chassis position covariance, using the uncertain principal axis direction as a reference, and aligns the head rotation with the information-rich area with the smallest amplitude. Simultaneously, it compares the observation cost of head rotation and the observation cost of chassis steering online and selects the optimal one for execution, which can improve the loop closure detection success rate to over 90%, the effective map coverage rate to increase by about 50%, and the local obstacle avoidance response time to less than 80ms.

[0253] Figure 8 A schematic diagram of the control device for the robot provided in this application is shown below. Figure 8 As shown, the robot control device 500 provided in this embodiment includes:

[0254] The acquisition module 501 is used to acquire the occlusion rate of the current field of view and the uncertainty information of the current chassis; wherein, the uncertainty information of the current chassis is used to quantify the uncertainty of the current chassis position;

[0255] The determination module 502 is used to determine the target observation direction based on the uncertainty information of the current chassis and / or the occlusion rate of the current field of view when determining to perform active observation; wherein, the target observation direction is the gimbal observation orientation determined with the goal of reducing the uncertainty of the chassis position;

[0256] The control module 503 is used to control the gimbal to rotate to the target observation direction, acquire new perspective images through the image acquisition device, and perform factor map optimization processing based on the new perspective images to obtain a new chassis pose.

[0257] In one possible implementation, the uncertainty information of the current chassis is the covariance matrix of the current chassis position; the determination module 502 is used for:

[0258] Determine the trace and the largest eigenvalue of the covariance matrix of the current chassis position; where the trace represents the overall uncertainty of the current chassis position; and the largest eigenvalue represents the maximum uncertainty of the current chassis position in a single direction.

[0259] If any one of the following conditions is met: the trace is greater than the first preset threshold, the maximum eigenvalue is greater than the second preset threshold, or the occlusion rate of the current field of view is greater than the third preset threshold, then active observation is determined to be performed.

[0260] In one possible implementation, the uncertainty information of the current chassis is the covariance matrix of the current chassis position; the determination module 502 is used for:

[0261] Based on the direction of the eigenvector corresponding to the largest eigenvalue of the current chassis position covariance matrix, multiple candidate observation directions are generated;

[0262] The positioning correction benefit for each candidate observation direction is evaluated to obtain the evaluation results;

[0263] Based on the evaluation results, the optimal candidate observation direction is selected as the target observation direction.

[0264] In one possible implementation, the determining module 502 is configured to:

[0265] For each candidate observation direction, the observation information gain, scanning time cost, and navigation cost corresponding to the candidate observation direction are calculated respectively. Among them, the observation information gain represents the expected reduction in positioning uncertainty after observing in the candidate observation direction; the scanning time cost represents the total time taken for the gimbal to rotate to the candidate observation direction and complete image acquisition; and the navigation cost represents the additional cost of active observation to the navigation task.

[0266] The evaluation results are obtained by fusing the observation information gain, scanning time cost, and navigation cost.

[0267] In one possible implementation, the determining module 502 is configured to:

[0268] Based on the candidate observation directions, determine the visible map points under the candidate observation directions;

[0269] For each visible map point, the visible map point is reprojected onto the pixel plane to determine the Jacobian matrix of the reprojection error; where the Jacobian matrix of the reprojection error characterizes the sensitivity of the visible map point to the observation of the chassis position.

[0270] Based on the Jacobian matrix corresponding to each visible map point, determine the corresponding total information matrix increment for the candidate observation direction;

[0271] Based on the current chassis position covariance matrix and the increment of the total information matrix, determine the posterior position covariance matrix;

[0272] The difference between the trace of the current chassis position covariance matrix and the trace of the posterior position covariance matrix is ​​determined as the observation information gain for the candidate observation direction.

[0273] In one possible implementation, the determining module 502 is configured to:

[0274] Determine the rotation angle based on the angle corresponding to the current gimbal angle and the candidate observation direction;

[0275] If the rotation angle is greater than the preset rotation angle threshold, the acceleration, deceleration, and constant speed phases of the gimbal are calculated according to the trapezoidal velocity curve, and the total rotation time of the gimbal is obtained by summing the acceleration, deceleration, and constant speed phases. The preset rotation angle threshold is the ratio of the square of the maximum angular velocity of the gimbal to the maximum angular acceleration.

[0276] If the rotation angle is less than or equal to the preset rotation angle threshold, the total rotation time of the gimbal is determined according to the triangular velocity curve.

[0277] The total rotation time of the gimbal is added to the preset image acquisition and processing time to obtain the scanning time cost corresponding to the candidate observation direction.

[0278] In one possible implementation, the determining module 502 is configured to:

[0279] Starting from the current chassis position and ending at the next navigation waypoint, a baseline path is planned to obtain baseline indicators; the baseline indicators include baseline mileage, baseline travel time and baseline cumulative steering angle.

[0280] The simulated gimbal rotates to the candidate observation direction for observation, and performs post-observation path planning with the same start and end points to obtain post-observation indicators; the post-observation indicators include post-observation mileage, post-observation travel time, and post-observation cumulative steering angle.

[0281] Based on the baseline indicators and the observed indicators, calculate the mileage difference, time difference, and steering angle difference respectively;

[0282] The navigation cost corresponding to the candidate observation direction is obtained by weighted summing of the absolute values ​​of the mileage difference, time difference, and steering angle difference.

[0283] In one possible implementation, the control module 503 is configured to:

[0284] Calculate the gimbal rotation cost when the gimbal rotates to the target observation direction, and the chassis turning cost when the chassis as a whole turns to the target observation direction;

[0285] If the cost of rotating the gimbal is less than the cost of turning the chassis, then control the gimbal to rotate to the target observation direction.

[0286] In one possible implementation, the acquisition module 501 is used for:

[0287] Obtain the current head fusion pose of the gimbal and the current image frame of the current field of view captured by the image acquisition device;

[0288] Based on the current head fusion pose, determine the current first transformation matrix and the current head fusion pose covariance matrix; where the first transformation matrix represents the pose transformation relationship between the gimbal coordinate system and the chassis coordinate system; the covariance matrix of the current head fusion pose is used to quantify the uncertainty of the head fusion pose.

[0289] Based on the current image frame, the current first transformation matrix, and the current head fusion pose covariance matrix, determine the new visual constraint factor and the weight coefficient of the new visual constraint factor.

[0290] Based on the new visual constraint factors and their weight coefficients, factor graph optimization is performed to obtain the current chassis position covariance matrix; where the current chassis position covariance matrix serves as the uncertainty information of the current chassis.

[0291] In one possible implementation, the acquisition module 501 is used for:

[0292] Based on the current first transformation matrix, determine the current second transformation matrix; whereby the second transformation matrix represents the pose transformation relationship between the camera coordinate system and the world coordinate system;

[0293] Based on the second transformation matrix, the target map points in the world coordinate system are transformed to the camera coordinate system and then projected to obtain the predicted coordinates of the pixel plane; where the target map points are the map points corresponding to the feature points in the current image frame.

[0294] The reprojection residual between the pixel coordinates and the predicted coordinates of the feature points in the current image frame is used as a new visual constraint factor.

[0295] Based on the trace of the current head fusion pose covariance matrix, determine the weight coefficients of the new visual constraint factors.

[0296] In one possible implementation, the acquisition module 501 is used for:

[0297] Based on the new visual constraint factors, their weight coefficients, and the original constraint factors and their weight coefficients, an optimization objective function is constructed; the optimization objective of the objective function is to minimize the sum of the weighted Mahalanobis distances of all constraint factors.

[0298] Based on the objective function, factor graph optimization is performed to obtain the covariance matrix of the current chassis position.

[0299] The robot control device provided in this embodiment can execute the method provided in the above method embodiment. Its implementation principle and technical effect are similar, and will not be described in detail here.

[0300] Figure 9 A schematic diagram of the structure of the electronic device provided in this application. Figure 9 As shown, the electronic device 600 provided in this embodiment includes at least one processor 601 and a memory 602. Optionally, the electronic device 600 further includes a communication component 603. The processor 601, memory 602, and communication component 603 are connected via a bus. This electronic device may, for example, be the aforementioned controller.

[0301] In a specific implementation, at least one processor 601 executes computer execution instructions stored in memory 602, causing at least one processor 601 to perform the above-described method.

[0302] The specific implementation process of processor 601 can be found in the above method embodiments, and its implementation principle and technical effect are similar. It will not be repeated here.

[0303] In the above embodiments, it should be understood that the processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), etc. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the method disclosed in this invention can be directly implemented by a hardware processor, or implemented by a combination of hardware and software modules within the processor.

[0304] The memory may include random access memory (RAM) and may also include non-volatile memory (NVM), such as at least one disk storage device.

[0305] The bus can be an Industry Standard Architecture (ISA) bus, a Peripheral Component Interconnect (PCI) bus, or an Extended Industry Standard Architecture (EISA) bus, etc. Buses can be categorized as address buses, data buses, control buses, etc. For ease of illustration, the buses shown in the accompanying drawings are not limited to a single bus or a single type of bus.

[0306] This application also provides a computer program product, including a computer program that, when executed by a processor, implements the above-described method.

[0307] This application also provides a computer-readable storage medium storing computer-executable instructions, which, when executed by a processor, implement the above-described method.

[0308] The aforementioned readable storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk. The readable storage medium can be any available medium accessible to a general-purpose or special-purpose computer.

[0309] An exemplary readable storage medium is coupled to a processor, enabling the processor to read information from and write information to the readable storage medium. Of course, the readable storage medium can also be a component of the processor. The processor and the readable storage medium can reside in an Application Specific Integrated Circuit (ASIC). Alternatively, the processor and the readable storage medium can exist as discrete components in the device.

[0310] The division of units is merely a logical functional division; in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be indirect coupling or communication connection through some interfaces, devices, or units, and may be electrical, mechanical, or other forms.

[0311] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0312] In addition, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0313] If a function is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0314] Those skilled in the art will understand that all or part of the steps of the above-described method embodiments can be implemented by hardware related to program instructions. The aforementioned program can be stored in a computer-readable storage medium. When executed, the program performs the steps of the above-described method embodiments; and the aforementioned storage medium includes various media capable of storing program code, such as ROM, RAM, magnetic disks, or optical disks.

[0315] Finally, it should be noted that other embodiments of the invention will readily occur to those skilled in the art upon consideration of the specification and practice of the invention disclosed herein. This invention is intended to cover any variations, uses, or adaptations of the invention that follow the general principles of the invention and include common knowledge or customary techniques in the art not disclosed herein, and is not limited to the precise structures described above and shown in the accompanying drawings, and various modifications and changes can be made without departing from its scope.

Claims

1. A method for controlling a robot, characterized in that, The method is applied to a robot controller, the robot further including a chassis and a gimbal with rotational degrees of freedom, the gimbal being equipped with an image acquisition device; the method includes: Obtain the occlusion rate of the current field of view and the uncertainty information of the current chassis; wherein, the uncertainty information of the current chassis is used to quantify the uncertainty of the current chassis position; If active observation is to be performed based on the uncertainty information of the current chassis and / or the occlusion rate of the current field of view, then the target observation direction is determined based on the uncertainty information of the current chassis; wherein, the target observation direction is the gimbal observation orientation determined with the goal of reducing the uncertainty of the chassis position; The gimbal is controlled to rotate to the target observation direction, and a new perspective image is acquired through the image acquisition device. Based on the new perspective image, factor graph optimization processing is performed to obtain a new chassis pose. The uncertainty information of the current chassis is the covariance matrix of the current chassis position; determining the target observation direction based on the uncertainty information of the current chassis includes: Based on the direction of the eigenvector corresponding to the largest eigenvalue of the current chassis position covariance matrix, multiple candidate observation directions are generated; The positioning correction benefit for each candidate observation direction is evaluated to obtain the evaluation results; Based on the evaluation results, the optimal candidate observation direction is selected as the target observation direction; The evaluation of the positioning correction benefit for each candidate observation direction yields the following evaluation results: For each candidate observation direction, the observation information gain, scanning time cost, and navigation cost corresponding to that candidate observation direction are calculated respectively; wherein, the observation information gain represents the expected reduction in positioning uncertainty after observing in that candidate observation direction; the scanning time cost represents the total time taken for the gimbal to rotate to that candidate observation direction and complete image acquisition; and the navigation cost represents the additional cost of active observation to the navigation task; The evaluation result is obtained by fusing the observation information gain, the scanning time cost, and the navigation cost.

2. The method according to claim 1, characterized in that, The step of determining to perform active observation based on the uncertainty information of the current chassis and / or the occlusion rate of the current field of view includes: Determine the trace and the maximum eigenvalue of the covariance matrix of the current chassis position; wherein the trace represents the overall uncertainty of the current chassis position; and the maximum eigenvalue represents the maximum uncertainty of the current chassis position in a single direction. If any one of the following conditions is met: the trace is greater than a first preset threshold, the maximum feature value is greater than a second preset threshold, or the occlusion rate of the current field of view is greater than a third preset threshold, then active observation is determined to be performed.

3. The method according to claim 1, characterized in that, Calculating the observation information gain corresponding to the candidate observation direction includes: Based on the candidate observation directions, determine the visible map points under the candidate observation directions; For each visible map point, the visible map point is reprojected onto a pixel plane to determine the Jacobian matrix of the reprojection error; wherein the Jacobian matrix of the reprojection error characterizes the sensitivity of the visible map point to the observation of the chassis position. Based on the Jacobian matrix corresponding to each visible map point, determine the total information matrix increment corresponding to the candidate observation direction; The posterior position covariance matrix is ​​determined based on the current chassis position covariance matrix and the increment of the total information matrix. The difference between the trace of the current chassis position covariance matrix and the trace of the posterior position covariance matrix is ​​determined as the observation information gain for the candidate observation direction.

4. The method according to claim 1, characterized in that, Calculating the scan time cost corresponding to the candidate observation direction includes: Determine the rotation angle based on the angle corresponding to the current gimbal angle and the candidate observation direction; If the rotation angle is greater than the preset rotation angle threshold, the acceleration segment duration, deceleration segment duration, and constant speed segment duration of the gimbal are calculated according to the trapezoidal velocity curve, and the total rotation time of the gimbal is obtained by summing the acceleration segment duration, the deceleration segment duration, and the constant speed segment duration; the preset rotation angle threshold is the ratio of the square of the maximum angular velocity of the gimbal to the maximum angular acceleration. If the rotation angle is less than or equal to the preset rotation angle threshold, the total rotation time of the gimbal is determined according to the triangular velocity curve. The total rotation time of the gimbal is superimposed with the preset image acquisition and processing time to obtain the scanning time cost corresponding to the candidate observation direction.

5. The method according to claim 1, characterized in that, Calculating the navigation cost corresponding to the candidate observation direction includes: Starting from the current chassis position and ending at the next navigation waypoint, a baseline path planning is performed to obtain baseline indicators; the baseline indicators include baseline driving mileage, baseline travel time, and baseline cumulative steering angle. The gimbal is simulated to rotate to the candidate observation direction for observation, and post-observation path planning is performed with the same start and end points to obtain post-observation indicators; the post-observation indicators include post-observation mileage, post-observation travel time, and post-observation cumulative steering angle. Based on the benchmark index and the observed index, calculate the mileage difference, time difference, and steering angle difference respectively; The absolute values ​​of the mileage difference, time difference, and steering angle difference are weighted and summed to obtain the navigation cost corresponding to the candidate observation direction.

6. The method according to claim 1, characterized in that, Before controlling the gimbal to rotate to the target observation direction, the method further includes: Calculate the gimbal rotation cost when the gimbal rotates to the target observation direction, and the chassis turning cost when the chassis as a whole turns to the target observation direction; If the cost of rotating the gimbal is less than the cost of turning the chassis, then control the gimbal to rotate to the target observation direction.

7. The method according to any one of claims 1-6, characterized in that, Obtain uncertainties about the current chassis, including: Obtain the current head fusion pose of the gimbal and the current image frame of the current field of view acquired by the image acquisition device; Based on the current head fusion pose, determine the current first transformation matrix and the current head fusion pose covariance matrix; wherein, the first transformation matrix represents the pose transformation relationship between the gimbal coordinate system and the chassis coordinate system; the covariance matrix of the current head fusion pose is used to quantify the uncertainty of the head fusion pose. Based on the current image frame, the current first transformation matrix, and the current head fusion pose covariance matrix, a new visual constraint factor and the weight coefficients of the new visual constraint factor are determined. Based on the new visual constraint factor and its weight coefficient, factor graph optimization is performed to obtain the current chassis position covariance matrix; wherein, the current chassis position covariance matrix serves as the uncertainty information of the current chassis.

8. The method according to claim 7, characterized in that, The step of determining a new visual constraint factor and its weight coefficients based on the current image frame, the current first transformation matrix, and the current head fusion pose covariance matrix includes: Based on the current first transformation matrix, determine the current second transformation matrix; wherein, the second transformation matrix represents the pose transformation relationship between the camera coordinate system and the world coordinate system; Based on the second transformation matrix, the target map points in the world coordinate system are transformed to the camera coordinate system and then projected to obtain the predicted coordinates of the pixel plane; wherein, the target map points are the map points corresponding to the feature points in the current image frame; The reprojection residual between the pixel coordinates of the feature points in the current image frame and the predicted coordinates is used as the new visual constraint factor. The weight coefficients of the new visual constraint factor are determined based on the trace of the current head fusion pose covariance matrix.

9. The method according to claim 7, characterized in that, The step involves performing factor graph optimization based on the new visual constraint factor and its weight coefficients to obtain the current chassis position covariance matrix, including: Based on the new visual constraint factor, the weight coefficient of the new visual constraint factor, and the original constraint factor and the weight coefficient of the original constraint factor, an optimization objective function is constructed; wherein, the optimization objective of the optimization objective function is to minimize the sum of the weighted Mahalanobis distances of all constraint factors; Based on the objective function, factor graph optimization is performed to obtain the covariance matrix of the current chassis position.

10. A control device for a robot, characterized in that, The device is used in the controller of a robot, which also includes a chassis and a gimbal with rotational degrees of freedom, the gimbal being equipped with an image acquisition device; including: The acquisition module is used to acquire the occlusion rate of the current field of view and the uncertainty information of the current chassis; wherein, the uncertainty information of the current chassis is used to characterize the uncertainty of the current chassis position; The determination module is used to determine the target observation direction based on the uncertainty information of the current chassis when determining to perform active observation based on the uncertainty information of the current chassis and / or the occlusion rate of the current field of view; wherein, the target observation direction is the gimbal observation orientation determined with the goal of reducing the uncertainty of chassis position; The control module is used to control the gimbal to rotate to the target observation direction, acquire new perspective images through the image acquisition device, and perform factor graph optimization processing based on the new perspective images to obtain a new chassis pose; The uncertainty information of the current chassis is the covariance matrix of the current chassis position; determining the target observation direction based on the uncertainty information of the current chassis includes: Based on the direction of the eigenvector corresponding to the largest eigenvalue of the current chassis position covariance matrix, multiple candidate observation directions are generated; The positioning correction benefit for each candidate observation direction is evaluated to obtain the evaluation results; Based on the evaluation results, the optimal candidate observation direction is selected as the target observation direction; The evaluation of the positioning correction benefit for each candidate observation direction yields the following evaluation results: For each candidate observation direction, the observation information gain, scanning time cost, and navigation cost corresponding to that candidate observation direction are calculated respectively; wherein, the observation information gain represents the expected reduction in positioning uncertainty after observing in that candidate observation direction; the scanning time cost represents the total time taken for the gimbal to rotate to that candidate observation direction and complete image acquisition; and the navigation cost represents the additional cost of active observation to the navigation task; The evaluation result is obtained by fusing the observation information gain, the scanning time cost, and the navigation cost.

11. An electronic device, characterized in that, include: Memory, processor; The memory stores computer-executed instructions; The processor executes computer execution instructions stored in the memory, causing the processor to perform the method as described in any one of claims 1-9.

12. A robot, characterized in that, include: The chassis, controller, and gimbal with rotational freedom, wherein the gimbal is equipped with image acquisition equipment; The controller is used to perform the method as described in any one of claims 1-9.

13. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores computer-executable instructions, which, when executed by a processor, are used to implement the method as described in any one of claims 1-9.

Images