Real-time generation of safety control set and safety filtering method for mobile robot

Abstract

The present application belongs to the field of robot control technology, and relates to a kind of mobile robot safety control set real-time generation and safety filtering method, the method includes: based on the state of mobile robot system and any measurement direction satisfied, establish the obstacle measurement model under the body coordinate system of mobile robot;Obstacle measurement result under the body coordinate system of mobile robot is obtained based on obstacle measurement model;Regularization measurement model and original safety control set are constructed based on obstacle measurement result;After remodeling, the remodeling safety control set is obtained to the original safety control set;Actual control instruction is generated based on safety filter combined with nominal speed instruction and remodeling safety control set, and mobile robot is controlled to move.Its beneficial effects are to control mobile robot to complete work task under the premise of guaranteeing safety, to solve the defects such as the discontinuous of safety filter perception, the discontinuous of optimization problem solution, the dependence on global pose through the obstacle measurement model under the body coordinate system, regularization, control set remodeling and the like.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a method for real-time generation and safety filtering of a mobile robot safety control set. Background Technology

[0002] Mobile robots or vehicles operating in cluttered environments need to avoid all obstacles in the complex environment and complete their tasks. If the controlled object (e.g., the mobile robot) is controlled solely based on the instructions given in the task (e.g., speed commands) without considering the object's kinematics and safety constraints such as obstacle avoidance, collisions may occur in the closed-loop system. To address this problem, a safety filter utilizes real-time perception results from sensors such as LiDAR and multi-view cameras to generate a set of safe controls that guarantee collision avoidance. The filter then searches within this set for the optimal solution that enables the completion of the task, using it as the control input. This filters out unsafe components from the instructions given in the task, achieving safe control.

[0003] However, in cluttered environments, safety filter methods face a series of challenges. First, sensor feedback may be discontinuous in relation to the controlled object's position and measurement direction, leading to discontinuous output commands from the safety filter, inducing chattering in the closed-loop system and causing collisions. Furthermore, conventional safety filter methods are typically designed based on optimization problems; even if sensor feedback is continuous, it's difficult to ensure the linear independent constraint normality of the optimization problem, resulting in a non-Lipschitz continuous solution. More importantly, the controlled object may not accurately obtain environmental maps, position information, and its own attitude information, making it difficult to directly apply conventional safety filter-based methods. Summary of the Invention

[0004] Technical problems to be solved

[0005] In view of the above-mentioned shortcomings and deficiencies of the prior art, the present invention provides a method for real-time generation and safety filtering of a mobile robot safety control set, which solves the technical problems of perception discontinuity, optimization problem solution discontinuity, and reliance on global pose in conventional methods of controlling mobile robots in cluttered environments using safety filters.

[0006] Technical solution

[0007] To achieve the above objectives, the main technical solutions adopted by the present invention include:

[0008] This invention provides a method for real-time generation and safety filtering of a mobile robot safety control set, including:

[0009] The system state and control input of the mobile robot are determined based on the translational and rotational kinematic models of the mobile robot.

[0010] Based on system state and given safety distance Define a safe set If the initial position of the mobile robot is within the safe set Within this set, at any subsequent time, the position of the mobile robot lies within the safe set. Inside;

[0011] Based on system state and satisfaction For any measurement direction, establish an obstacle measurement model in the body coordinate system of the mobile robot, where... , express The set of real numbers, Indicates the measurement direction for measuring obstacles in the geodetic coordinate system;

[0012] Obstacle measurement results in the body coordinate system of the mobile robot are obtained based on the obstacle measurement model;

[0013] A regularized measurement model and an original safety control set are constructed based on obstacle measurement results;

[0014] The reshaped security control set is obtained by reshaping the original security control set.

[0015] The actual control commands are generated by combining the nominal speed command and the reshaped safety control set based on the safety filter to control the movement of the mobile robot.

[0016] Optionally, the system state and control inputs of the mobile robot are determined based on the translational and rotational kinematics models of the mobile robot, including:

[0017] The translational and rotational kinematic models of the mobile robot are as follows:

[0018] in, The system status of the mobile robot. For the control input of the mobile robot, It is a mobile robot. The position vector at time in the geodetic coordinate system has a range of . ; It is a mobile robot. The velocity vector at time t in the body coordinate system has a range of . ; It is a mobile robot. The rotation matrix between the volumetric coordinate system and the geodetic coordinate system at time t, its range is , , yes 3D identity matrix; This is the angular velocity matrix of the mobile robot, and its range is... satisfying in space The set consisting of all matrices.

[0019] Optionally, secure sets The definition of ,in, , It is the set of position vectors of all obstacle points in the geodetic coordinate system.

[0020] Optionally, an obstacle measurement model in the body coordinate system of the mobile robot. for:

[0021] ;

[0022] in, It is a mobile robot in the location The maximum measurement boundary of the sensor at that location, , The maximum measurement range of the sensor; variable x is the set of obstacles. or the sensor's maximum measurement boundary Up, and along the measurement direction The position vector.

[0023] Optionally, obstacle measurement results for:

[0024] ,in, This indicates the measurement direction for measuring the obstacle in the volume coordinate system. Indicates the location of the mobile robot under the geodetic coordinate system Measuring the distance to obstacles in a given direction.

[0025] Optionally, regularization of the measurement model for:

[0026] The domain of the regularization model is: , and ; and The results are from obstacle measurements. The elements in This indicates the measurement direction for measuring obstacles in the volume coordinate system. This indicates the measured distance in the corresponding measurement direction. , It is a regularization term and is a monotonically decreasing Lipschitz function; if the regularization term satisfies ,and ,in, If it is a constant, then for any , and , ,right , and ,have All are true, among which, It is a function The Lipschitz constant.

[0027] Optionally, the original security control set for:

[0028] ,in, It is a Lipschitz continuous function that passes through zero at zero and is strictly monotonically increasing. The domain of this function is... The range is .

[0029] Optionally, reshape the security control set for:

[0030] ,in, It is a constant matrix. The set of row vectors forms a positive basis, and the matrix Each row vector They are all unit vectors. ;matrix any All rows are linearly independent; any unit vector They are all sets A positive combination of elements in the middle has a cardinality of not less than 1. ; A positive constant; .

[0031] Optionally, the security filter is as follows:

[0032] ;

[0033] in, It is the nominal speed command. These are the actual control commands, that is, the commands given to the mobile robot. The velocity vector in the body coordinate system at any given time.

[0034] Beneficial effects

[0035] The beneficial effects of this invention are as follows: This invention provides a real-time generation and safety filtering method for a mobile robot safety control set, used for the safe motion control of vehicles and robots in cluttered environments, enabling them to complete tasks while ensuring safety. First, an obstacle measurement model is established in a volume coordinate system, and surrounding environmental information is acquired using sensors. Second, a regularization strategy is introduced to overcome the discontinuity of measurement results, ensuring the continuous dependence of measurement values ​​on position, attitude, and orientation. Then, based on the regularized measurement results, a safety control set with good geometric properties is constructed. Finally, a safety filter is designed, combining the nominal speed command and the reshaped safety control set to generate actual control commands, ensuring the continuity of output and the safety of the mobile robot. This method achieves real-time safe obstacle avoidance control in unknown or cluttered environments without relying on external positioning information, solving the defects of conventional safety filter methods such as discontinuous perception, discontinuous solutions to optimization problems, and reliance on global pose. Attached Figure Description

[0036] Figure 1 A schematic diagram of a typical scenario for security control in a chaotic environment provided by an embodiment of the present invention;

[0037] Figure 2 A system block diagram for security control provided in an embodiment of the present invention;

[0038] Figure 3 A schematic diagram of the scenario considered in the numerical simulation provided in the embodiments of the present invention;

[0039] Figure 4 The method proposed in this embodiment differs from conventional methods in terms of speed. Comparison chart;

[0040] Figure 5 This is a comparison diagram of the method proposed in this embodiment and conventional methods regarding the security control set;

[0041] Figure 6 A comparison diagram of the position trajectory between the method proposed in this embodiment and the conventional method in the presence of actuator dynamics;

[0042] Figure 7 The method proposed in this embodiment and conventional methods regarding position With all obstacles Comparison chart of minimum distances between them;

[0043] Figure 8 The omnidirectional mobile robot positioning provided in the embodiments of the present invention With each obstacle point A diagram illustrating the minimum distance between them;

[0044] Figure 9 The nominal speed command provided for embodiments of the present invention and safety filter output Comparison chart;

[0045] Figure 10 Yaw angle provided for embodiments of the present invention and angular velocity Comparison chart. Detailed Implementation

[0046] To better explain and facilitate understanding of the present invention, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0047] This invention proposes a real-time generation and safety filtering method for a mobile robot safety control set, used for the safe motion control of vehicles and robots in cluttered environments, enabling them to complete tasks while ensuring safety. Specifically, the key points of this invention are:

[0048] 1. Establishing an obstacle measurement model in a volume coordinate system. This invention proposes a mathematical model defined in the volume coordinate system of the controlled object to represent the sensor's measurement results of obstacles. This model directly characterizes local environmental features through the geometric relationship between the measurement direction and the distance to the obstacle in the volume coordinate system, avoiding reliance on the global coordinate system.

[0049] 2. A regularization method for measurement models in cluttered environments is proposed. This invention proposes an improved regularization method for obstacle measurement models in volume coordinates. By introducing a Lipschitz truncation regularization term to replace the quadratic regularization term in the traditional Moreau-Yosida method, the system safety and the feasibility of the safety filter are ensured.

[0050] 3. A method for reshaping the safety control set in volume coordinates is proposed. This invention proposes a method for reshaping the safety control set. For the original safety control set constructed based on the control barrier function method, a special positive basis matrix is ​​used, combined with regularized measurement results, to obtain the reshaped safety control set.

[0051] 4. Design a continuous, reactive safety filter. This invention proposes a continuous, reactive safety filter based on real-time obstacle measurement. It utilizes a quadratic programming problem to integrate and reshape the safety control set and the nominal commands used to achieve the task, ultimately enabling the safe completion of the task.

[0052] To better understand the above technical solutions, exemplary embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that the present invention can be understood more clearly and thoroughly, and that the scope of the present invention can be fully conveyed to those skilled in the art.

[0053] Firstly, referring to Figure 1 This embodiment provides a method for real-time generation and safety filtering of a mobile robot safety control set, including:

[0054] S1, based on the translational and rotational kinematics models of the mobile robot, determines the system state and control input of the mobile robot.

[0055] Optionally, the system state and control inputs of the mobile robot are determined based on the translational and rotational kinematics models of the mobile robot, including:

[0056] The translational and rotational kinematic models of the mobile robot are as follows:

[0057] in, The system status of the mobile robot. For the control input of the mobile robot, It is a mobile robot. The position vector at time in the geodetic coordinate system has a range of . ; It is a mobile robot. The velocity vector at time t in the body coordinate system has a range of . ; It is a mobile robot. The rotation matrix between the volumetric coordinate system and the geodetic coordinate system at time t, its range is , , yes 3D identity matrix; This is the angular velocity matrix of the mobile robot, and its range is... satisfying in space The set consisting of all matrices.

[0058] in, , , and for , , and The simplified version omits unnecessary time variables.

[0059] This model can serve as a kinematic model for various robots and vehicles, such as multi-rotor drones and omnidirectional mobile vehicles.

[0060] S2, defining a safety set based on the system state and a given safety distance D. If the initial position of the mobile robot is within the safe set Within this set, at any subsequent time, the position of the mobile robot lies within the safe set. Inside.

[0061] Optionally, secure sets The definition of ,in, , It is the set of position vectors of all obstacle points in the geodetic coordinate system.

[0062] Introducing a safety filter into the controlled object, i.e., the mobile robot, aims to achieve the following control objective: maintaining a sufficiently large distance between the controlled object and obstacles. In other words, for an initial state within the safety set... (that is) If the position of the controlled object For all If all objects are within the safe set, then the controlled object is safe.

[0063] S3, based on system state and satisfaction For any measurement direction, establish an obstacle measurement model in the body coordinate system of the mobile robot, where... , express The set of real numbers, This indicates the measurement direction for measuring obstacles in the geodetic coordinate system.

[0064] Optionally, an obstacle measurement model in the body coordinate system of the mobile robot. for:

[0065] ;

[0066] in, It is a mobile robot in the location The maximum measurement boundary of the sensor at that location, , The maximum measurement range of the sensor; variable It is located in the set of obstacles or the sensor's maximum measurement boundary Up, and along the measurement direction The position vector.

[0067] S4, Obtain obstacle measurement results in the body coordinate system of the mobile robot based on the obstacle measurement model.

[0068] Optionally, consider the rotation matrix of the mobile robot. For any moving body position and arbitrary rotation matrix Obstacle measurement results in the body coordinate system of the mobile robot for:

[0069] ,in, This indicates the measurement direction for measuring the obstacle in the volume coordinate system. Indicates the location of the mobile robot under the geodetic coordinate system Measuring the distance to obstacles in a given direction.

[0070] In practice, obstacle measurement results Data can be collected in real time using scanning sensors, such as LiDAR, binocular cameras, and depth cameras. A typical scenario for security control in cluttered environments considered in this invention is as follows: As shown.

[0071] S5 constructs a regularized measurement model and the original safety control set based on obstacle measurement results.

[0072] Optionally, a regularized measurement model can be constructed to overcome the discontinuity defects of obstacle measurement models. for:

[0073] The domain of the regularization model is: , and ; and The results are from obstacle measurements. The elements in This indicates the measurement direction for measuring obstacles in the volume coordinate system. This indicates the measured distance in the corresponding measurement direction. , It is a regularization term and is a monotonically decreasing Lipschitz function; if the regularization term satisfies ,and ,in, If is a constant, then the regularized measurement model possesses boundedness and Lipschitz continuity, that is:

[0074] Boundedness: for any , and The following inequality holds, meaning that the regularized measurement result is greater than or equal to the distance between the moving object and the nearest point within the measurement range, and less than or equal to the original measurement result:

[0075] ;

[0076] Lipschitz continuity: for the future Represented as The function satisfies the following inequality, which implies that the regularized result is Lipschitz continuous for the robot's position, rotation matrix, and measurement direction, that is:

[0077] For all , and All of them are valid, among which It is a function The Lipschitz constant.

[0078] Optionally, an original safety control set can be constructed using the method of controlling obstacle functions and an obstacle measurement model in a geodetic coordinate system. for:

[0079] ,in, It is a Lipschitz continuous function that passes through zero at zero and is strictly monotonically increasing (i.e.: It is a deterministic nonlinear function that takes zero at the origin, is strictly increasing with respect to its independent variable, and whose rate of change is globally bounded. The domain of this function is... The range is .

[0080] S6, after reshaping the original security control set, obtains the reshaped security control set.

[0081] Optionally, reshape the security control set for:

[0082] ,in, It is a constant matrix. The set of row vectors forms a positive basis, and the matrix Each row vector They are all unit vectors. ;matrix any All rows are linearly independent; any unit vector They are all sets A positive combination of elements in the middle has a cardinality of not less than 1. ; A positive constant; .

[0083] S7 generates actual control commands based on a safety filter, nominal speed command, and reshaped safety control set to control the movement of the mobile robot.

[0084] Optionally, the security filter is as follows:

[0085] ;

[0086] in, This is the nominal speed command, generated by the controller, whose purpose is to accomplish the task. These are the actual control commands, that is, the commands given to the mobile robot. The velocity vector in the body coordinate system at any given time.

[0087] The proposed safety filter uses only obstacle measurements. and nominal speed command It does not rely on positioning and attitude information, can guarantee Lipschitz continuity of the state of the controlled object, and has the following properties:

[0088] The quadratic programming problem (reshaping the safe control set) in the security filter is feasible and applicable to all... , and Each has a unique solution.

[0089] Security filter, denoted as The amplitude of its output result is less than the amplitude of the nominal speed command, that is:

[0090] .

[0091] The aforementioned kinematic model, safety filter, and any satisfying Lipschitz continuous The resulting closed-loop system is secure, meaning that for any and , For all Both are valid.

[0092] The system block diagram of the proposed safety control system is as follows: Figure 2 As shown.

[0093] The safety control set proposed in this embodiment overcomes the shortcomings of discontinuous measurement models, does not rely on position or attitude information, and can be automatically generated from sensing data. It is based on real-time obstacle measurement results in volume coordinates. This embodiment proposes an improved regularization method. This method overcomes the discontinuity of the measurement model by introducing a Lipschitz truncation regularization term, replacing the quadratic regularization term used in the traditional Moreau-Yosida method. The safety filter proposed in this embodiment is continuous for both the position and measurement direction of the controlled object. This embodiment proposes a continuous, reactive safety filter based on real-time obstacle measurement. The controller ensures the linear independent constraint normality of the optimization problem in the safety filter through a specific positive basis, and combined with the improved regularization method, ensures the controller possesses Lipschitz continuity, thereby improving the robustness and adaptability of the controller in uncertain environments.

[0094] The effectiveness of the proposed method in this embodiment will be verified through numerical simulation and physical experiments.

[0095] In the numerical simulation, the aforementioned kinematic model is considered, along with two triangular obstacles, the corner points of which are respectively... , , and , , The initial position of the moving machine is... The initial attitude matrix is ,like Figure 3 As shown. In Figure 3 In the text, the horizontal axis label is... Represents position vector The first component, the vertical axis label is Represents position vector The second component.

[0096] To evaluate the performance of the proposed security filter, it is compared with a conventional security filter. The conventional security filter is defined as follows:

[0097] ;

[0098] Given the parameters of the security filter proposed in this embodiment:

[0099] ;

[0100] in, It is a positive odd number. In the simulation, the nominal speed command is considered. and angular velocity matrix , and select

[0101] .

[0102] Figure 4 The velocity curves of the two methods are shown. , It is a velocity vector The first component. The blue solid line trajectory corresponds to the security filter proposed in this embodiment, which can guarantee Lipschitz continuity, while the other green dashed line trajectory is... s is not continuous with Lipschitz. The safety control sets of the two methods are shown. It can be seen that the reshaped safety control set changes more smoothly when the position of the controlled object changes.

[0103] Next, numerical simulations are used to verify the robustness of the security filter proposed in this embodiment. Considering the actuator dynamics, the actual speed... It may not be equal to the speed setpoint. The controlled object is modeled as a cascade connection of the nominal system and actuators, described by the following equations:

[0104] ;

[0105] in, That's the actual speed. It is the state of the execution system. It is the speed setpoint generated by the security filter, a constant matrix. , and Defined as:

[0106] .

[0107] Here, Indicates the status of the actuator and The coupling terms between them. It's easy to verify: actual speed... Able to asymptotically track any constant speed setpoint and tracking error The size depends on The rate of change.

[0108] Still considering Figure 3 In the scenario described, the proposed security filter is compared with a conventional security filter, and an initial state is selected as... The simulation results are as follows Figure 6 and Figure 7 As shown. In the presence of actuator dynamics, the safety filter proposed in this embodiment (represented by the blue position trajectory) helps maintain a safe distance from obstacles. Conventional methods, however, in... The nearby area is unsafe, indicating that non-Lipschite safety filters may cause unexpected transient responses in the closed-loop system and compromise the safety of the controlled object.

[0109] The effectiveness of the proposed method is illustrated through a physical experiment. In this experiment, the controlled object is a wheeled omnidirectional mobile robot that needs to avoid obstacles such as ladders and potted plants while traversing a corridor to reach the target point. The mobile robot is equipped with a four-wheeled omnidirectional chassis, a LiDAR, and a computing unit. The computing unit runs on Ubuntu 16.04, operates a safety filter under ROS Kinetic, and uses rosbag to record data and share real-time data with a host computer. The host computer runs Windows 11 and uses MATLAB to generate the nominal speed command. The nominal speed command is then described. The control objective is to guide the chassis through the cluttered corridor. Angular velocity matrix. For time Lipschitz continuity, ensure that the mobile robot continues to rotate as it moves through the corridor.

[0110] Except for the parameters listed below, the parameters used in the experiment are the same as those in the numerical simulation.

[0111] .

[0112] Experimental results are as follows Figure 8 , Figure 9 and Figure 10 As shown. Figure 8 This indicates that in a cluttered environment, the minimum distance between an omnidirectional mobile robot and obstacles is... m, slightly smaller than the expected safe distance This difference could stem from speed response errors in the chassis or inconsistencies in the lidar sensors. Figure 9 nominal speed command With safety filter output A comparison was made to verify the Lipschitz continuity of the proposed security filter, as shown in the figure. and Indicates the speed of the mobile robot The first and second components, and Indicates nominal speed command The first and second components. Figure 10 The figure shows the changes in yaw angle and yaw rate of the omnidirectional mobile robot over time, indicating that for the mobile robot equipped with the proposed safety filter, its safety is not affected by changes in the heading angle. and Representing the rotation matrix The elements in the first row and second column and the first row and first column, Represents the angular velocity matrix The element in the second row and first column.

[0113] In a second aspect, embodiments of the present invention provide a computer-readable storage medium having a computer program stored thereon, wherein the program, when executed, implements the method for real-time generation and safety filtering of a mobile robot safety control set as described in any of the first aspects above.

[0114] Thirdly, embodiments of the present invention provide a storage device, including a storage medium and a processor, wherein the storage medium stores a computer program, and when the program is executed by the processor, it implements the method for real-time generation and security filtering of a mobile robot safety control set as described in any of the first aspects above.

[0115] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0116] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, then this invention should also include these modifications and variations.

[0117] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make modifications, alterations, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A method for real-time generation of a mobile robot safety control set and safety filtering, characterized by, Comprising: determining system states and control inputs of the mobile robot based on translational kinematics and rotational kinematics models of the mobile robot; Based on system state combining given safety distance Defining safety set If the initial position of the mobile robot is within the safety set , then at any subsequent time, the position of the mobile robot is within the safety set . Based on the system state and satisfying , a mobile robot obstacle measurement model under the body coordinate system is established, wherein, , represents a set of real numbers, represents the measurement direction of measuring the obstacle in the geodetic coordinate system; obtaining obstacle measurements in the body coordinate system of the mobile robot based on an obstacle measurement model; constructing a regularized measurement model and an original safe control set based on the obstacle measurements; reshaping the original safe control set to obtain a reshaped safe control set; generating actual control commands based on a safety filter combining nominal velocity commands and the reshaped safe control set to control the mobile robot to move; where the regularized measurement model is: ； where the regularization model domain is defined as , and ; and are elements in the obstacle measurement results , denotes the measurement direction of the obstacle in the body coordinate system, denotes the measurement distance in the corresponding measurement direction, , is a regularization term and is a monotonically decreasing Lipschitz function; if the regularization term satisfies , and where is a constant, then for any , and , for , and , are all true, where is the Lipschitz constant of the function ; Original security control set Is: wherein, is a Lipschitz continuous function that is strictly monotonically increasing with a zero crossing at zero, the domain of the function is , and the range of the function is ; Remodeling safety control set For: wherein is a constant matrix, the set of row vectors forms a positive basis, and the matrix each row vector of the matrix is a unit vector, any row of the matrix is linearly independent; any unit vector is a positive combination of elements of the set ; is a positive constant; ; the safety filter is as follows: ； ； in, It is the nominal speed command. These are the actual control commands, that is, the commands given to the mobile robot. The velocity vector in the body coordinate system at any given time.

2. The method of claim 1, wherein the method further comprises: determining system states and control inputs of the mobile robot based on translational kinematics and rotational kinematics models of the mobile robot, comprising: the translational kinematics and rotational kinematics models of the mobile robot are as follows: wherein, is the system state of the mobile robot, is the control input of the mobile robot, is the position vector of the mobile robot in the body coordinate system at time , whose value domain is ; is the velocity vector of the mobile robot in the body coordinate system at time , whose value domain is ; is the rotation matrix between the body coordinate system and the earth coordinate system at time , whose value domain is , , is the dimensional identity matrix; is the angular velocity matrix of the mobile robot, whose value domain is the set of all matrices that satisfy in the space 3. The method for real-time generation and safety filtering of a mobile robot safety control set according to claim 2, characterized in that... Secure set is defined as wherein , is a set of position vectors of all obstacles in the geodetic coordinate system.

4. The method of claim 3, wherein the method further comprises: Obstacle measurement model in mobile robot body coordinate system is: wherein, is the sensor maximum measurement boundary of the mobile robot at position , , is the maximum measurement range of the sensor; the variable is the position vector of the location of the obstacle set or the sensor maximum measurement boundary and along the measurement direction .

5. The method of claim 4, wherein the method further comprises: Obstacle measurement result is: ； wherein, represents a measurement direction of the obstacle measured in the body coordinate system, represents a measurement distance of the obstacle in the direction of the earth coordinate system by the mobile robot at the position .

Images