Flexible distributed positioning and tracking control method and device for mobile robot

Abstract

This invention provides a method and apparatus for flexible distributed localization and tracking control of a mobile robot, comprising: constructing a motion model of the mobile robot; constructing a mobile sensor network, the mobile sensor network including dynamic models of each mobile sensor and a communication topology established based on the communication relationships between different mobile sensors; constructing a detection model for each mobile sensor according to the pose relationship between each mobile sensor and the mobile robot; constructing an attack model for each mobile sensor subjected to external attack, the attack model being constructed based on the detection model of its respective mobile sensor; using the attack models of the mobile sensors subjected to external attack, the detection models of the mobile sensors not subjected to external attack, and the communication topology to perform distributed localization of the mobile robot; and based on the distributed localization results, performing trajectory tracking of the mobile robot by setting a tracking controller for each mobile sensor in the mobile sensor network.

Description

Technical Field

[0001] This invention relates to the field of software technology, and more specifically, to a method and apparatus for flexible distributed positioning and tracking control of mobile robots. Background Technology

[0002] In recent years, mobile robot technology has developed rapidly, leading to its widespread application in industry, agriculture, military, and civilian fields. As mobile robots increasingly appear in various practical application scenarios, their tracking has become fundamental to completing a series of tasks. The core and key to achieving mobile robot tracking is its localization, that is, obtaining the robot's specific location information through certain methods. Clearly, only by knowing the robot's trajectory can we further track it.

[0003] In mobile robot tracking tasks, the task is accomplished by a group of mobile intelligent agents (robots or intelligent vehicles) equipped with sensors. Therefore, the localization of the mobile robot is achieved by a mobile sensor network composed of these mobile sensors. Compared to static sensor networks, mobile sensor networks offer greater flexibility and higher localization performance. Distributed localization based on them is crucial for completing tracking tasks, significantly improving the tracking performance of mobile robots while exhibiting high robustness and low computational overhead. However, as a typical cyber-physical system, mobile sensor networks face certain security risks at both the information and physical levels. External malicious attackers focus on the information level, often using various attack methods to disrupt the sensor network's estimation and localization tasks. Spoofing attacks are a typical example. Spoofing attacks involve external attackers focusing on the detection information obtained by the sensors. Based on known sensor detection information, they replace the true detection values ​​with arbitrary erroneous values, thus affecting the local estimation of the attacked sensor. Then, through inter-sensor communication, they further influence the localization results of all sensors in the entire sensor network. This type of attack affects sensor networks from a point to a surface, significantly disrupting target localization and thus preventing successful tracking of mobile robots.

[0004] Currently, some distributed resilient localization methods exist to address the disruptive effects of deception attacks on localization. However, these methods mostly target static targets or moving targets with linear motion models. Localizing mobile robots with nonlinear motion models presents a greater challenge, but also holds significant practical value. Summary of the Invention

[0005] In view of this, to solve the above problems, the present invention provides a method and apparatus for flexible distributed positioning and tracking control of mobile robots, the technical solution of which is as follows:

[0006] A method for resilient distributed localization and tracking control of a mobile robot, the method comprising:

[0007] Construct a motion model of a mobile robot as a tracking target, the motion model being used to characterize the pose of the mobile robot;

[0008] A mobile sensor network is constructed for locating and tracking the mobile robot. The mobile sensor network includes a dynamic model of each mobile sensor and a communication topology established based on the communication relationship between different mobile sensors. The dynamic model is used to characterize the pose of the mobile sensor.

[0009] Based on the pose relationship between each mobile sensor and the mobile robot, a detection model for each mobile sensor is constructed.

[0010] An attack model is constructed for each mobile sensor subjected to external attack, and the attack model is constructed based on the detection model of the respective mobile sensor;

[0011] The mobile robot is distributedly located using attack models of each mobile sensor subjected to external attacks, detection models of each mobile sensor not subjected to external attacks, and the communication topology.

[0012] Based on the distributed positioning results, the mobile robot is tracked by setting a tracking controller for each mobile sensor in the mobile sensor network.

[0013] Preferably, the construction of the motion model of the mobile robot as the tracking target includes:

[0014] Construct a motion model of the mobile robot based on its pose:

[0015]

[0016] Where, r k+1 This represents the pose of the mobile robot at time k+1; This represents the pose of the mobile robot at time k, including the orientation angle. and location This represents the change in the orientation angle of the mobile robot as measured by the odometer during the time interval from time k to time k+1. k+1 and position offset v k+1 ;

[0017] The process noise resulting from the measurement by the odometer is represented by a Gaussian distribution N(0,Q); R(φ)k Let ) denote a rotation matrix that satisfies

[0018] Preferably, the construction of the mobile sensor network for locating and tracking the mobile robot includes:

[0019] Identify multiple mobile sensors used to constitute the mobile sensor network;

[0020] A dynamic model of each motion sensor is constructed based on the pose and control input of each motion sensor:

[0021]

[0022] in, Let i represent the pose of the i-th moving sensor at time k+1, where i = 1, ..., N; This represents the pose of the i-th motion sensor at time k, including the orientation angle. and location Δt represents the time interval from time k to time k+1; This represents the control input of the i-th motion sensor at time k, including angular velocity. and linear velocity

[0023] The communication topology is modeled based on the distance between different mobile sensors:

[0024] G k =(V, E) k A k )

[0025] Among them, G k The topology graph representing the communication topology at time k is a time-varying undirected graph; V represents a point set containing N points, where N is the number of the plurality of mobile sensors; E k A represents the set of edges at time k, where there is an edge between two mobile sensors whose distance at time k is less than a preset maximum communication distance; k The adjacency matrix of the topology graph representing the communication topology at time k.

[0026] Preferably, the step of constructing a detection model for each motion sensor based on the pose relationship between each motion sensor and the mobile robot includes:

[0027] For each motion sensor, based on the position of the motion sensor and the position of the mobile robot, it is determined whether the mobile robot is within the detection range of the motion sensor;

[0028] If the mobile robot is not within the detection range of the motion sensor, then the detection value of the motion sensor is determined to be 0;

[0029] If the mobile robot is within the detection range of the motion sensor, then the detection model of the motion sensor is constructed:

[0030]

[0031] in, This represents the detection value obtained by the i-th motion sensor at time k; Describes Gaussian detection noise, with a distribution that satisfies It is the covariance matrix; the detection matrix.

[0032] Preferably, the construction of the attack model for each mobile sensor subjected to external attack includes:

[0033] For each mobile sensor subjected to external attack, construct an attack model for that mobile sensor:

[0034]

[0035] in, This represents the detection value obtained by the i-th mobile sensor when it is attacked at time k; This indicates the degree to which external attacks affect the detection value.

[0036] Preferably, the distributed localization of the mobile robot using the attack model of each mobile sensor subjected to external attack, the detection model of each mobile sensor not subjected to external attack, and the communication topology includes:

[0037] For each mobile sensor, calculate the prior pose estimate of the mobile robot and its corresponding estimation error covariance:

[0038]

[0039]

[0040]

[0041] in, This represents the prior estimate of the mobile robot's pose at time k calculated by the i-th motion sensor, including the prior estimate of the orientation angle. and prior estimates of location This represents the covariance of the estimation error corresponding to the prior estimation of pose. This represents the noise gain matrix obtained by linearizing the motion model of the mobile robot at time k, satisfying... matrix Q represents the covariance of process noise in robot dynamics;

[0042] The pose prior estimate and its corresponding estimation error covariance calculated by this mobile sensor are distributedly fused with the pose prior estimates and their corresponding estimation error covariance calculated by other mobile sensors:

[0043]

[0044]

[0045] in, This represents the pose prior estimate obtained by the i-th mobile sensor after fusing the pose prior estimates of other communicating mobile sensors, including the fused prior estimates of the orientation angle and the fused prior estimates of the position. The covariance is the covariance obtained by fusing the pose prior estimates of other mobile sensors with which the i-th mobile sensor communicates with the corresponding estimation error covariance. At time k, the adjacency matrix A k The (i, j)th element; α k For time-varying parameters, satisfying α0>0、0<τ α <1;

[0046] Determine whether the mobile sensor has acquired a detection value at the current moment;

[0047] If the moving sensor does not obtain a detection value at the current moment, the posterior pose estimate and its corresponding estimation error covariance are determined based on the estimated value and covariance obtained by the fusion of the moving sensor values:

[0048]

[0049] in, This represents the posterior estimate of the mobile robot's pose at time k+1 calculated by the i-th motion sensor, including the posterior estimate of the orientation angle. and posterior estimation of location This represents the covariance of the estimation error corresponding to the prior estimation of pose.

[0050] If the motion sensor obtains a detection value at the current moment, calculate the corresponding Kalman gain matrix of the motion sensor:

[0051]

[0052] in, This represents the Kalman gain matrix calculated by the i-th mobile sensor at time k.

[0053] Based on the corresponding Kalman gain matrix of the motion sensor, update the corresponding estimation error covariance of the pose posterior estimate:

[0054]

[0055] Calculate the corresponding detection residual of the motion sensor:

[0056]

[0057] in, This represents the detection residual calculated by the i-th moving sensor at time k;

[0058] Based on the corresponding detection residual of the motion sensor, determine whether the motion sensor has been attacked, and update the posterior pose estimate:

[0059]

[0060] in, Indicates whether the i-th mobile sensor is attacked at time k. The time indicates that the attack is being carried out, and the corresponding when This indicates that no attack has occurred, and the corresponding... It is a defined time-varying neighborhood value.

[0061]

[0062] in, when hour when hour satisfy

[0063] Preferably, the mobile robot is tracked by setting a tracking controller for each mobile sensor in the mobile sensor network, including:

[0064] Construct proportional controllers for each mobile sensor in the mobile sensor network:

[0065]

[0066] in, c represents the control input at time k of the i-th motion sensor; ω and c v All are constant coefficients with a value greater than 0;

[0067] The motion model of the mobile robot is tracked by a proportional controller for each motion sensor.

[0068] A mobile robot flexible distributed positioning and tracking control device, the device comprising:

[0069] A motion model construction module is used to construct a motion model of a mobile robot as a tracking target, wherein the motion model is used to characterize the pose of the mobile robot;

[0070] A mobile sensor network construction module is used to construct a mobile sensor network for locating and tracking the mobile robot. The mobile sensor network includes a dynamic model of each mobile sensor and a communication topology established based on the communication relationship between different mobile sensors. The dynamic model is used to characterize the pose of the mobile sensor.

[0071] The detection model construction module is used to construct the detection model of each mobile sensor based on the pose relationship between each mobile sensor and the mobile robot.

[0072] An attack model construction module is used to construct attack models for each mobile sensor subjected to external attacks. The attack models are constructed based on the detection models of the respective mobile sensors.

[0073] A distributed positioning module is used to perform distributed positioning of the mobile robot by utilizing the attack models of each mobile sensor subjected to external attacks, the detection models of each mobile sensor not subjected to external attacks, and the communication topology.

[0074] The trajectory tracking module is used to track the trajectory of the mobile robot based on the distributed positioning results by setting a tracking controller for each mobile sensor in the mobile sensor network.

[0075] Preferably, the motion model construction module is specifically used for:

[0076] Construct a motion model of the mobile robot based on its pose:

[0077]

[0078] Where, r k+1 This represents the pose of the mobile robot at time k+1; This represents the pose of the mobile robot at time k, including the orientation angle. and location This represents the change in the orientation angle of the mobile robot as measured by the odometer during the time interval from time k to time k+1. k+1 and position offset vk+1 ;

[0079] The process noise resulting from the measurement by the odometer is represented by a Gaussian distribution N(0,Q); R(φ) k Let ) denote a rotation matrix that satisfies

[0080] Preferably, the mobile sensor network construction module is specifically used for:

[0081] Identify multiple mobile sensors used to constitute the mobile sensor network;

[0082] A dynamic model of each motion sensor is constructed based on the pose and control input of each motion sensor:

[0083]

[0084] in, Let i represent the pose of the i-th moving sensor at time k+1, where i = 1, ..., N;

[0085] This represents the pose of the i-th motion sensor at time k, including the orientation angle. and location Δt represents the time interval from time k to time k+1; This represents the control input of the i-th motion sensor at time k, including angular velocity. and linear velocity

[0086] The communication topology is modeled based on the distance between different mobile sensors:

[0087] G k =(V, E) k A k )

[0088] Among them, G k The topology graph representing the communication topology at time k is a time-varying undirected graph; V represents a point set containing N points, where N is the number of the plurality of mobile sensors; E k A represents the set of edges at time k, where there is an edge between two mobile sensors whose distance at time k is less than a preset maximum communication distance; k The adjacency matrix of the topology graph representing the communication topology at time k.

[0089] Compared with the prior art, the beneficial effects achieved by the present invention are as follows:

[0090] This invention provides a method and apparatus for flexible distributed localization and tracking control of a mobile robot, comprising: constructing a motion model representing the pose of the mobile robot; constructing a mobile sensor network, the mobile sensor network including dynamic models representing the poses of each mobile sensor and a communication topology established based on the communication relationships between different mobile sensors; constructing a detection model for each mobile sensor based on the pose relationship between each mobile sensor and the mobile robot; constructing an attack model for each mobile sensor subjected to external attack, the attack model being constructed based on the detection model of its respective mobile sensor; using the attack models of the mobile sensors subjected to external attack, the detection models of the mobile sensors not subjected to external attack, and the communication topology, performing distributed localization of the mobile robot; and based on the distributed localization results, performing trajectory tracking of the mobile robot by setting a tracking controller for each mobile sensor in the mobile sensor network. This invention can perform distributed flexible localization and trajectory tracking for mobile robots with nonlinear operating models, and has high practical application value. Specifically, this invention has the following advantages:

[0091] 1) The mobile sensor network used in this invention enables the localization of mobile robots, which has better flexibility and localization performance compared to traditional static sensor networks. The distributed localization method designed in this invention has better robustness compared to traditional centralized methods, while reducing the computational requirements of each sensor and reducing the energy consumption caused by computation.

[0092] 2) The target being tracked in this invention is a mobile robot with a nonlinear motion model, which is a commonly used model in practical applications. This allows the positioning and tracking control method of this invention to be directly applied to practical scenarios.

[0093] 3) This invention successfully realizes distributed positioning and tracking control under external attacks, effectively handling the malicious impact of external attacks on the positioning and tracking process, and has high practical application value in various fields such as civilian and military applications. Attached Figure Description

[0094] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0095] Figure 1 This is a flowchart of a mobile robot elastic distributed positioning and tracking control method provided in an embodiment of the present invention;

[0096] Figure 2This is a schematic diagram of the motion trajectory of a mobile robot provided in an embodiment of the present invention;

[0097] Figure 3 This is a schematic diagram of the motion trajectory of a mobile sensor provided in an embodiment of the present invention;

[0098] Figure 4 The evolution curves of the distance between the mobile robot and each mobile sensor provided in the embodiments of the present invention;

[0099] Figure 5 This is a schematic diagram of the structure of the mobile robot elastic distributed positioning and tracking control device provided in an embodiment of the present invention. Detailed Implementation

[0100] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0101] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0102] To address the existing technical problems, this invention proposes a flexible distributed positioning and tracking control method for mobile robots, which at least partially solves one of the aforementioned technical problems.

[0103] See Figure 1 , Figure 1 This is a flowchart illustrating the method for a mobile robot's elastic distributed localization and tracking control provided in an embodiment of the present invention. Figure 1 As shown, the mobile robot's flexible distributed localization and tracking control method includes the following steps:

[0104] S10, Construct a motion model of the mobile robot as the tracking target. The motion model is used to characterize the pose of the mobile robot.

[0105] In this embodiment of the invention, the mobile robot is a robot with a nonlinear motion model that is commonly used in real-world scenarios. For the mobile robot serving as the tracking target, its motion model can be constructed based on its pose (including orientation angle and position).

[0106] Specifically, step S10, "Constructing a motion model of the mobile robot as the tracking target," can be performed using the following steps:

[0107] The motion model of the mobile robot is constructed based on its pose, as shown in the following formula (1):

[0108]

[0109] Where, r k+1 This represents the pose of the mobile robot at time k+1; This represents the pose of the mobile robot at time k, including the orientation angle. and location

[0110] This represents the change in the orientation angle of the mobile robot measured by the odometer during the time interval from time k to time k+1. k+1 and position offset v k+1 ;

[0111] The process noise introduced by the odometer measurement is represented by the Gaussian distribution N(0,Q); R(φ) k Let ) denote a rotation matrix that satisfies

[0112] It should be noted that the rotation matrix R(φ) k The function of ) is to transform the velocity vector and corresponding noise in the sensor coordinate system to the global coordinate system.

[0113] It should also be noted that the parameters in the embodiments of the present invention Used to represent the space of real numbers. To represent an n-dimensional vector space, for example, Represents a three-dimensional vector space.

[0114] S20, Construct a mobile sensor network for localization and tracking of the mobile robot. The mobile sensor network includes the dynamic model of each mobile sensor and the communication topology established based on the communication relationship between different mobile sensors. The dynamic model is used to characterize the pose of the mobile sensor.

[0115] In this embodiment of the invention, to achieve the localization and tracking of the mobile robot, N mobile intelligent agents (robots or intelligent vehicles) equipped with sensors are used, referred to as mobile sensors. Multiple mobile sensors constitute a mobile sensor network, and the dynamic model of each mobile sensor in the mobile sensor network can be constructed based on the pose (including orientation angle and position) of each mobile sensor.

[0116] Furthermore, the communication between these mobile sensors is distance-limited; communication is only possible when the distance between two mobile sensors is less than the maximum communication distance. Therefore, as the mobile sensors move continuously, the communication relationships between the N mobile sensors constantly change with their positions. Based on this, this invention models the communication topology of the mobile sensor network as a time-varying undirected graph.

[0117] In the specific implementation process, step S20, "constructing a mobile sensor network for locating and tracking the mobile robot," can be achieved through the following steps:

[0118] Identify multiple mobile sensors used to form a mobile sensor network;

[0119] The dynamic model of each motion sensor is constructed based on the pose and control input of each motion sensor, as shown in the following formula (2):

[0120]

[0121] in, Let i represent the pose of the i-th moving sensor at time k+1, where i = 1, ..., N;

[0122] This represents the pose of the i-th motion sensor at time k, including the orientation angle. and location Δt represents the time interval from time k to time k+1 (i.e., between two consecutive sampling times); This represents the control input of the i-th motion sensor at time k, including angular velocity. and linear velocity

[0123] Model the communication topology based on the distance between different mobile sensors:

[0124] G k =(V, E) k A k )

[0125] Among them, G k The topology graph representing the communication topology at time k is a time-varying undirected graph; V represents a point set containing N points, where N is the number of multiple mobile sensors; E k A represents the set of edges at time k, where there is an edge between two mobile sensors whose distance at time k is less than a preset maximum communication distance; k The adjacency matrix of the topology graph representing the communication topology at time k.

[0126] It should be noted that V = {v1, v2, ..., v} N} represents a point set containing N points; E k = {(i,j)} represents the edge set at time k, and (i,j) represents v i With v j The existence of this edge means that motion sensor i and motion sensor j can communicate with each other; Let (i, j) represent the corresponding adjacency matrix, and let (i, j) exist at time k. otherwise, The row sum of an adjacency matrix is ​​always equal to 1.

[0127] S30: Based on the pose relationship between each mobile sensor and the mobile robot, construct the detection model for each mobile sensor.

[0128] In this embodiment of the invention, each mobile sensor in the mobile sensor network has its own detection range. The mobile sensor can only detect the robot and obtain a corresponding detection value when the robot is within its detection range. Therefore, for each mobile sensor, its detection model can be determined based on the pose relationship between its dynamic model and the mobile robot. Specifically, the construction of the mobile sensor's detection model is divided into two cases: when the mobile robot is not within its detection range, the detection value of the mobile sensor is 0; when the mobile robot is within its detection range, a corresponding detection model needs to be constructed.

[0129] In the specific implementation process, step S30, "constructing the detection model of each motion sensor based on the positional relationship between the dynamic model of each motion sensor and the motion model of the mobile robot," can be achieved through the following steps:

[0130] For each motion sensor, based on the position of the motion sensor and the position of the mobile robot, it is determined whether the mobile robot is within the detection range of the motion sensor;

[0131] If the mobile robot is not within the detection range of the motion sensor, then the detection value of the motion sensor is determined to be 0;

[0132] If the mobile robot is within the detection range of the motion sensor, then the detection model of the motion sensor is constructed as shown in the following formula (3):

[0133]

[0134] in, This represents the detection value obtained by the i-th motion sensor at time k; Describes Gaussian detection noise, with a distribution that satisfies It is the covariance matrix; the detection matrix.

[0135] In other words, for a mobile sensor i, i = 1, ..., N, the detection model is constructed as shown in the following formula (4):

[0136]

[0137] The nonlinear function is shown in the following formula (5):

[0138]

[0139] It should be noted that, It is Gaussian detection noise, and its distribution satisfies It is the covariance matrix; R s It is the radius of the detection range of the motion sensor. The detection range of each motion sensor is a circle of the same size centered on that motion sensor.

[0140] S40, construct attack models for each mobile sensor subjected to external attacks. The attack models are constructed based on the detection models of the respective mobile sensors.

[0141] When mobile robots are localized using mobile sensor networks, malicious external attackers attempt to disrupt the localization process through deception attacks. They target the mobile sensors capable of detecting the target, replacing the detected values ​​with arbitrary values.

[0142] Therefore, in this embodiment of the invention, for the mobile sensor i, i = 1, ..., N, its detection value can be replaced with any value when attacked. Specifically, for each mobile sensor subjected to external attack, the attack model of the mobile sensor is constructed as shown in the following formula (6):

[0143]

[0144] in, This represents the detection value obtained by the i-th mobile sensor when it is attacked at time k; This indicates the degree to which external attacks affect the detection value.

[0145] It should be noted that, This characterizes the impact of the attack on the detection information. The above formula can be improved by selecting appropriate... This represents any arbitrary value, thus effectively demonstrating the arbitrariness of the attack.

[0146] S50 uses attack models of each mobile sensor subjected to external attacks, detection models of each mobile sensor not subjected to external attacks, and communication topology to perform distributed localization of the mobile robot.

[0147] In this embodiment of the invention, the elastic distributed localization of the mobile robot can be divided into three steps: prediction, consistency, and update.

[0148] In the specific implementation process, step S50, "using the attack models of each mobile sensor subjected to external attack, the detection models of each mobile sensor not subjected to external attack, and the communication topology, to perform distributed localization of the motion model of the mobile robot," can be performed as follows:

[0149] For each mobile sensor, the prior pose estimate of the mobile robot and its corresponding estimation error covariance are calculated as shown in the following formula (7):

[0150]

[0151] in, This represents the prior estimate of the mobile robot's pose at time k calculated by the i-th motion sensor, including the prior estimate of the orientation angle. and prior estimates of location This represents the covariance of the estimation error corresponding to the prior estimation of pose. This represents the noise gain matrix obtained by linearizing the motion model of the mobile robot at time k, satisfying... matrix Q represents the covariance of process noise in robot dynamics;

[0152] The pose prior estimate and its corresponding estimation error covariance calculated by the mobile sensor are distributedly fused with the pose prior estimates and their corresponding estimation error covariance calculated by other mobile sensors, as shown in the following formulas (8) and (9):

[0153]

[0154]

[0155] in, This represents the pose prior estimate obtained by the i-th mobile sensor after fusing the pose prior estimates of other communicating mobile sensors, including the fused prior estimates of the orientation angle and the fused prior estimates of the position. The covariance is the covariance obtained by fusing the pose prior estimates of other mobile sensors with which the i-th mobile sensor communicates with the corresponding estimation error covariance. At time k, the adjacency matrix A k The (i, j)th element; α k For time-varying parameters, satisfying α0>0、0<τ α <1;

[0156] Determine whether the mobile sensor has acquired a detection value at the current moment;

[0157] If the motion sensor does not obtain a detection value at the current moment, the posterior pose estimate and its corresponding estimation error covariance are determined based on the estimated value and covariance obtained by the motion sensor fusion, as shown in the following formula (10):

[0158]

[0159] in, This represents the posterior estimate of the mobile robot's pose at time k+1 calculated by the i-th motion sensor, including the posterior estimate of the orientation angle. and posterior estimation of location This represents the covariance of the estimation error corresponding to the prior estimation of pose.

[0160] If the motion sensor obtains a detection value at the current moment, calculate the corresponding Kalman gain matrix of the motion sensor, as shown in the following formula (11):

[0161]

[0162] in, This represents the Kalman gain matrix calculated by the k-th motion sensor at time k.

[0163] Based on the corresponding Kalman gain matrix of the mobile sensor, the estimation error covariance of the pose posterior estimate is updated as shown in the following formula (12):

[0164]

[0165] The corresponding detection residual of the motion sensor is calculated as shown in the following formula (13):

[0166]

[0167] in, This represents the detection residual calculated by the i-th moving sensor at time k;

[0168] Based on the corresponding detection residual of the motion sensor, determine whether the motion sensor has been attacked, and update the pose posterior estimate as shown in the following formulas (14) and (15):

[0169]

[0170] in, Indicates whether the i-th mobile sensor is attacked at time k. The time indicates that the attack is being carried out, and the corresponding when This indicates that no attack has occurred, and the corresponding... It is a defined time-varying neighborhood value.

[0171]

[0172] in, when hour when hour satisfy

[0173] In this embodiment of the invention, the prediction is to calculate the pose estimate of the mobile robot based on the motion model of the mobile robot and all the detection values ​​obtained before the current time. This estimate is called the prior estimate (i.e., the pose prior estimate mentioned above). Correspondingly, the estimation error covariance matrix (i.e., the estimation error covariance mentioned above) corresponding to the estimate is also calculated. The calculation method is as shown in the above formula (7).

[0174] After each mobile sensor calculates its local prior estimate (i.e., pose prior estimate and its corresponding estimation error covariance), in order to make better use of the historical detection information obtained to obtain a more accurate estimate, they will communicate with each other, transmit their local estimates to other mobile sensors, and fuse the information transmitted by other mobile sensors with their own information. The specific fusion method is shown in the above formulas (8) and (9).

[0175] The update is to use the detection value obtained at the current time to calculate the posterior estimate of the mobile robot's pose (i.e., the pose posterior estimate above) and the corresponding error covariance matrix (i.e., the estimated error covariance above). Therefore, if no detection value is obtained at the current time (time k), that is, the mobile robot is not within the detection range of the motion sensor i, i = 1, ..., N at the current time, the posterior estimate is equal to the fused prior estimate, as shown in the above formula (10).

[0176] If the moving sensor i obtains a detection value at the current time, the Kalman gain matrix can be calculated according to the detection model, as shown in the above formula (11). Then, the corresponding estimation error covariance of the pose posterior estimate can be directly updated, as shown in the above formula (12).

[0177] To update the pose posterior estimate using the newly acquired detection values, it is first necessary to calculate the detection residual, as shown in formula (13) above. When the moving sensor i is not attacked, If the mobile sensor i is attacked. The detection residual can be used to determine whether the motion sensor i has been attacked. This invention uses variables. To reflect whether the motion sensor i is attacked at time k, as shown in formula (14) above, when This means that the mobile sensor i has been attacked. It is a time-varying neighborhood value defined in this invention, which decreases continuously over time.

[0178] To mitigate the impact of attacks on positioning performance when mobile sensors are compromised, this invention defines an adaptive gain. Make This limits the impact of the attack to a time-varying, gradually decreasing neighborhood. Using adaptive gain, the update calculation of pose posterior estimation satisfies the above formula (15).

[0179] S60, based on distributed positioning results, tracks the trajectory of a mobile robot by setting a tracking controller for each mobile sensor in the mobile sensor network.

[0180] In this embodiment of the invention, a proportional controller is selected as the tracking controller for the motion sensor to perform trajectory tracking on the mobile robot. Specifically, a proportional controller is constructed for each motion sensor in the motion sensor network; the motion model of the mobile robot is tracked through the proportional controller of each motion sensor. The proportional controller is shown in the following formula (16):

[0181]

[0182] in, c represents the control input at time k of the i-th motion sensor; ω and c v All are constant coefficients with a value greater than 0.

[0183] To facilitate understanding of the present invention, the following specific example will be used to further illustrate the implementation of the present invention:

[0184] In this example, the dynamic equations (i.e., motion model) of the tracked mobile robot are shown in the following formula (17):

[0185]

[0186] Here, the process Gaussian noise The covariance matrix Q = 0.001I, where I is the identity matrix. The initial position of the mobile robot is set to r0 = [1 2.5 1]. T .

[0187] In this embodiment, six motion sensors are used for the localization and tracking of the mobile robot, forming a motion sensor network. The motion equations (i.e., dynamic models) of the motion sensors are shown in the following formula (18):

[0188]

[0189] The initial pose of each motion sensor is set to The detection range of each motion sensor is a circle with a radius of 3m centered on that motion sensor. When the mobile robot is within the detection range of the motion sensor, the sensor detection model is as shown in the following formula (19):

[0190]

[0191] Here, the covariance matrix of the Gaussian detection noise is R = 0.001I. When sensor i, i = 1, ..., 6, is attacked, its detected value becomes 3 times the true detected value.

[0192] The mobile robot described above is localized using the elastic distributed positioning method designed in this invention, with the initial estimates of each motion sensor set as follows: The initial covariance matrix is ​​set to Each parameter is set to α0 = 0.2, τ α =0.75,

[0193] See Figure 2 , Figure 2 This is a schematic diagram of the motion trajectory of a mobile robot provided in an embodiment of the present invention; see also Figure 3 , Figure 3 This is a schematic diagram of the motion trajectory of the mobile sensor provided in an embodiment of the present invention; see also Figure 4 , Figure 4 The curves showing the evolution of the distance between the mobile robot and each mobile sensor are provided in an embodiment of the present invention. Figure 2 , Figure 3 , Figure 4 As shown, Figure 2 and Figure 3 The motion trajectories of the mobile robot and six motion sensors are schematically shown respectively. Figure 4 The diagram illustrates the evolution of the distance between the mobile robot and its various mobile sensors.

[0194] from Figure 2 and Figure 3 It can be roughly seen that the trajectory of the moving sensor is constantly approaching the trajectory of the moving robot. In order to display the tracking results more accurately, Figure 4 It clearly demonstrates that the distance between each mobile sensor and the mobile robot is close to 0, which means that target tracking has been achieved.

[0195] Therefore, the mobile robot elastic distributed localization and tracking control method provided by this invention has better robustness, lower requirements for the computing power of each mobile sensor, and reduced energy consumption caused by computation. Since the tracked target is a mobile robot with a nonlinear motion model, this method can be directly applied to real-world scenarios. Furthermore, this method effectively handles the malicious impact of external attacks on the localization and tracking process, and has high practical application value in various fields such as civilian and military applications.

[0196] Based on the mobile robot elastic distributed localization and tracking control method provided in the above embodiments, the present invention correspondingly provides an apparatus for executing the above mobile robot elastic distributed localization and tracking control method, the structural schematic diagram of which is shown below. Figure 5 Shown, including:

[0197] The motion model construction module 10 is used to construct a motion model of the mobile robot as the tracking target. The motion model is used to represent the pose of the mobile robot.

[0198] The mobile sensor network construction module 20 is used to construct a mobile sensor network for localization and tracking of the mobile robot. The mobile sensor network includes the dynamic model of each mobile sensor and the communication topology established based on the communication relationship between different mobile sensors. The dynamic model is used to characterize the pose of the mobile sensor.

[0199] The detection model building module 30 is used to build a detection model for each mobile sensor based on the pose relationship between each mobile sensor and the mobile robot.

[0200] Attack model construction module 40 is used to construct attack models for each mobile sensor subjected to external attacks. The attack model is constructed based on the detection model of the respective mobile sensor.

[0201] The distributed positioning module 50 is used to perform distributed positioning of the mobile robot by utilizing the attack model of each mobile sensor that has been attacked externally, the detection model of each mobile sensor that has not been attacked externally, and the communication topology.

[0202] The trajectory tracking module 60 is used to track the trajectory of a mobile robot based on distributed positioning results by setting a tracking controller for each mobile sensor in the mobile sensor network.

[0203] Optional, motion model building module 10, specifically used for:

[0204] Constructing a motion model for a mobile robot based on its pose:

[0205]

[0206] Where, r k+1 This represents the pose of the mobile robot at time k+1; This represents the pose of the mobile robot at time k, including the orientation angle. and location

[0207] This represents the change in the orientation angle of the mobile robot measured by the odometer during the time interval from time k to time k+1. k+1 and position offset v k+1 ;

[0208] The process noise introduced by the odometer measurement is represented by the Gaussian distribution N(0,Q); R(φ) k Let ) denote a rotation matrix that satisfies

[0209] Optionally, the mobile sensor network building module 20 is specifically used for:

[0210] Identify multiple mobile sensors used to form a mobile sensor network;

[0211] A dynamic model of each motion sensor is constructed based on its pose and control input:

[0212]

[0213] in, Let i represent the pose of the i-th moving sensor at time k+1, where i = 1, ..., N;

[0214] This represents the pose of the i-th motion sensor at time k, including the orientation angle. and location Δt represents the time interval from time k to time k+1; This represents the control input of the i-th motion sensor at time k, including angular velocity. and linear velocity

[0215] Model the communication topology based on the distance between different mobile sensors:

[0216] G k =(V, E) k， A k )

[0217] Among them, G k The topology graph representing the communication topology at time k is a time-varying undirected graph; V represents a point set containing N points, where N is the number of multiple mobile sensors; E kA represents the set of edges at time k, where there is an edge between two mobile sensors whose distance at time k is less than a preset maximum communication distance; k The adjacency matrix of the topology graph representing the communication topology at time k.

[0218] Optional, the detection model building module, specifically used for:

[0219] For each motion sensor, based on the position of the motion sensor and the position of the mobile robot, it is determined whether the mobile robot is within the detection range of the motion sensor;

[0220] If the mobile robot is not within the detection range of the motion sensor, then the detection value of the motion sensor is determined to be 0;

[0221] If the mobile robot is within the detection range of the motion sensor, then construct the detection model for the motion sensor:

[0222]

[0223] in, This represents the detection value obtained by the i-th motion sensor at time k; Describes Gaussian detection noise, with a distribution that satisfies It is the covariance matrix; the detection matrix.

[0224] Optional, attack model building module, specifically used for:

[0225] For each mobile sensor subjected to external attack, construct an attack model for that mobile sensor:

[0226]

[0227] in, This represents the detection value obtained by the i-th mobile sensor when it is attacked at time k; This indicates the degree to which external attacks affect the detection value.

[0228] Optional, distributed positioning module, specifically used for:

[0229] For each mobile sensor, calculate the prior pose estimate of the mobile robot and its corresponding estimation error covariance:

[0230]

[0231]

[0232]

[0233] in, This represents the prior estimate of the mobile robot's pose at time k calculated by the i-th motion sensor, including the prior estimate of the orientation angle. and prior estimates of location This represents the covariance of the estimation error corresponding to the prior estimation of pose. This represents the noise gain matrix obtained by linearizing the motion model of the mobile robot at time k, satisfying... matrix Q represents the covariance of process noise in robot dynamics;

[0234] The pose prior estimate and its corresponding estimation error covariance calculated by this mobile sensor are distributedly fused with the pose prior estimates and their corresponding estimation error covariance calculated by other mobile sensors:

[0235]

[0236]

[0237] in, This represents the pose prior estimate obtained by the i-th mobile sensor after fusing the pose prior estimates of other communicating mobile sensors, including the fused prior estimates of the orientation angle and the fused prior estimates of the position. The covariance is the covariance obtained by fusing the pose prior estimates of other mobile sensors with which the i-th mobile sensor communicates with the corresponding estimation error covariance. At time k, the adjacency matrix A k The (i, j)th element; α k For time-varying parameters, satisfying α0>0、0<τ α <1;

[0238] Determine whether the mobile sensor has acquired a detection value at the current moment;

[0239] If the moving sensor does not obtain a detection value at the current moment, the posterior pose estimate and its corresponding estimation error covariance are determined based on the estimated value and covariance obtained by the fusion of the moving sensor values:

[0240]

[0241] in, This represents the posterior estimate of the mobile robot's pose at time k+1 calculated by the i-th motion sensor, including the posterior estimate of the orientation angle. and posterior estimation of location This represents the covariance of the estimation error corresponding to the prior estimation of pose.

[0242] If the motion sensor obtains a detection value at the current moment, calculate the corresponding Kalman gain matrix of the motion sensor:

[0243]

[0244] in, This represents the Kalman gain matrix calculated by the i-th mobile sensor at time k.

[0245] Based on the corresponding Kalman gain matrix of the motion sensor, update the corresponding estimation error covariance of the pose posterior estimate:

[0246]

[0247] Calculate the corresponding detection residual of the motion sensor:

[0248]

[0249] in, This represents the detection residual calculated by the i-th moving sensor at time k;

[0250] Based on the corresponding detection residual of the motion sensor, determine whether the motion sensor has been attacked, and update the posterior pose estimate:

[0251]

[0252] in, Indicates whether the i-th mobile sensor is attacked at time k. The time indicates that the attack is being carried out, and the corresponding when This indicates that no attack has occurred, and the corresponding... It is a defined time-varying neighborhood value.

[0253]

[0254] in, when hour when hour satisfy

[0255] Optional, a trajectory tracking module, specifically used for:

[0256] Constructing proportional controllers for each mobile sensor in a mobile sensor network:

[0257]

[0258] in, c represents the control input at time k of the i-th motion sensor; ω and c v All are constant coefficients with a value greater than 0;

[0259] The motion model of the mobile robot is tracked by a proportional controller for each motion sensor.

[0260] It should be noted that the detailed functions of each module in the embodiments of the present invention can be found in the corresponding disclosures of the embodiments of the mobile robot elastic distributed positioning and tracking control method described above, and will not be repeated here.

[0261] The above provides a detailed description of a mobile robot elastic distributed positioning and tracking control method and device provided by the present invention. Specific examples have been used to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only for the purpose of helping to understand the method and core ideas of the present invention. At the same time, for those skilled in the art, there will be changes in the specific implementation methods and application scope based on the ideas of the present invention. Therefore, the content of this specification should not be construed as a limitation of the present invention.

[0262] It should be noted that the various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the apparatus disclosed in the embodiments, since it corresponds to the method disclosed in the embodiments, the description is relatively simple; relevant parts can be referred to the method section.

[0263] It should also be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that elements inherent to a process, method, article, or apparatus that comprises a list of elements, or elements inherent to such processes, methods, articles, or apparatus, are also included. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0264] The above description of the disclosed embodiments enables those skilled in the art to make or use the invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for flexible distributed localization and tracking control of a mobile robot, characterized in that, The method includes: Construct a motion model of a mobile robot as a tracking target, the motion model being used to characterize the pose of the mobile robot; A mobile sensor network is constructed for locating and tracking the mobile robot. The mobile sensor network includes a dynamic model of each mobile sensor and a communication topology established based on the communication relationship between different mobile sensors. The dynamic model is used to characterize the pose of the mobile sensor. Based on the pose relationship between each mobile sensor and the mobile robot, a detection model for each mobile sensor is constructed. An attack model is constructed for each mobile sensor subjected to external attack, and the attack model is constructed based on the detection model of the respective mobile sensor; The mobile robot is distributedly located using attack models of each mobile sensor subjected to external attacks, detection models of each mobile sensor not subjected to external attacks, and the communication topology. Based on the distributed positioning results, the mobile robot is tracked by setting a tracking controller for each mobile sensor in the mobile sensor network. The construction of the mobile sensor network for locating and tracking the mobile robot includes: Identify multiple mobile sensors used to constitute the mobile sensor network; A dynamic model of each motion sensor is constructed based on the pose and control input of each motion sensor: in, Let represent the pose of the i-th moving sensor at time k+1, where i = 1, ..., N; This represents the pose of the i-th motion sensor at time k, including the orientation angle. and location Δt represents the time interval from time k to time k+1; This represents the control input of the i-th motion sensor at time k, including angular velocity. and linear velocity The communication topology is modeled based on the distance between different mobile sensors: G k =(V,E k ,A k ) Among them, G k The topology graph representing the communication topology at time k is a time-varying undirected graph; V represents a point set containing N points, where N is the number of the plurality of mobile sensors; E k A represents the set of edges at time k, where there is an edge between two mobile sensors whose distance at time k is less than a preset maximum communication distance; k The adjacency matrix of the topology graph representing the communication topology at time k; The step of constructing a detection model for each motion sensor based on the pose relationship between each motion sensor and the mobile robot includes: For each motion sensor, based on the position of the motion sensor and the position of the mobile robot, it is determined whether the mobile robot is within the detection range of the motion sensor; If the mobile robot is not within the detection range of the motion sensor, then the detection value of the motion sensor is determined to be 0; If the mobile robot is within the detection range of the motion sensor, then the detection model of the motion sensor is constructed: in, This represents the detection value obtained by the i-th motion sensor at time k; Describes Gaussian detection noise, with a distribution that satisfies It is the covariance matrix; the detection matrix. The construction of attack models for each mobile sensor subjected to external attacks includes: For each mobile sensor subjected to external attack, construct an attack model for that mobile sensor: in, This represents the detection value obtained by the i-th mobile sensor when it is attacked at time k; This indicates the degree to which external attacks affect the detection value.

2. The method according to claim 1, characterized in that, The construction of the motion model for the mobile robot as the tracking target includes: Construct a motion model of the mobile robot based on its pose: Where, r k+1 This represents the pose of the mobile robot at time k+1; This represents the pose of the mobile robot at time k, including the orientation angle. and location This represents the change in the orientation angle of the mobile robot as measured by the odometer during the time interval from time k to time k+1. k+1 and position offset v k+1 ; The process noise resulting from the measurement by the odometer is represented by a Gaussian distribution N(0,Q); R(φ) k Let ) denote a rotation matrix that satisfies 3. The method according to claim 1, characterized in that, The distributed localization of the mobile robot using the attack model of each mobile sensor subjected to external attack, the detection model of each mobile sensor not subjected to external attack, and the communication topology includes: For each mobile sensor, calculate the prior pose estimate of the mobile robot and its corresponding estimation error covariance: in, This represents the prior estimate of the mobile robot's pose at time k calculated by the i-th motion sensor, including the prior estimate of the orientation angle. and prior estimates of location This represents the covariance of the estimation error corresponding to the prior estimation of pose. This represents the noise gain matrix obtained by linearizing the motion model of the mobile robot at time k, satisfying... matrix Q represents the covariance of process noise in robot dynamics; The pose prior estimate and its corresponding estimation error covariance calculated by this mobile sensor are distributedly fused with the pose prior estimates and their corresponding estimation error covariance calculated by other mobile sensors: in, This represents the pose prior estimate obtained by the i-th mobile sensor after fusing the pose prior estimates of other communicating mobile sensors, including the fused prior estimates of the orientation angle and the fused prior estimates of the position. The covariance is the covariance obtained by fusing the pose prior estimates of other mobile sensors with which the i-th mobile sensor communicates with the corresponding estimation error covariance. At time k, the adjacency matrix A k The (i, j)th element; α k For time-varying parameters, satisfying Determine whether the mobile sensor has acquired a detection value at the current moment; If the moving sensor does not obtain a detection value at the current moment, the posterior pose estimate and its corresponding estimation error covariance are determined based on the estimated value and covariance obtained by the fusion of the moving sensor values: in, This represents the posterior estimate of the mobile robot's pose at time k+1 calculated by the i-th motion sensor, including the posterior estimate of the orientation angle. and posterior estimation of location This represents the covariance of the estimation error corresponding to the prior estimation of pose. If the motion sensor obtains a detection value at the current moment, calculate the corresponding Kalman gain matrix of the motion sensor: in, This represents the Kalman gain matrix calculated by the i-th mobile sensor at time k. Based on the corresponding Kalman gain matrix of the motion sensor, update the corresponding estimation error covariance of the pose posterior estimate: Calculate the corresponding detection residual of the motion sensor: in, This represents the detection residual calculated by the i-th moving sensor at time k; Based on the corresponding detection residual of the motion sensor, determine whether the motion sensor has been attacked, and update the posterior pose estimate: in, Indicates whether the i-th mobile sensor is attacked at time k. This indicates that the attack has occurred, and the corresponding response is as follows: when This indicates that no attack has occurred, and the corresponding... It is a defined time-varying neighborhood value. in, when hour when hour satisfy 4. The method according to claim 3, characterized in that, Tracking the mobile robot's trajectory is achieved by setting a tracking controller for each mobile sensor in the mobile sensor network, including: Construct proportional controllers for each mobile sensor in the mobile sensor network: in, c represents the control input at time k of the i-th motion sensor; ω and c v All are constant coefficients with a value greater than 0; The motion model of the mobile robot is tracked by a proportional controller for each motion sensor.

5. A mobile robot flexible distributed positioning and tracking control device, characterized in that, The device includes: A motion model construction module is used to construct a motion model of a mobile robot as a tracking target, wherein the motion model is used to characterize the pose of the mobile robot; A mobile sensor network construction module is used to construct a mobile sensor network for locating and tracking the mobile robot. The mobile sensor network includes a dynamic model of each mobile sensor and a communication topology established based on the communication relationship between different mobile sensors. The dynamic model is used to characterize the pose of the mobile sensor. The detection model construction module is used to construct the detection model of each mobile sensor based on the pose relationship between each mobile sensor and the mobile robot. An attack model construction module is used to construct attack models for each mobile sensor subjected to external attacks. The attack models are constructed based on the detection models of the respective mobile sensors. A distributed positioning module is used to perform distributed positioning of the mobile robot by utilizing the attack models of each mobile sensor subjected to external attacks, the detection models of each mobile sensor not subjected to external attacks, and the communication topology. The trajectory tracking module is used to track the trajectory of the mobile robot based on the distributed positioning results by setting a tracking controller for each mobile sensor in the mobile sensor network. Specifically, the mobile sensor network construction module is used for: Identify multiple mobile sensors used to constitute the mobile sensor network; A dynamic model of each motion sensor is constructed based on the pose and control input of each motion sensor: in, Let i represent the pose of the i-th moving sensor at time k+1, where i = 1, ..., N; This represents the pose of the i-th motion sensor at time k, including the orientation angle. and location Δt represents the time interval from time k to time k+1; This represents the control input of the i-th motion sensor at time k, including angular velocity. and linear velocity The communication topology is modeled based on the distance between different mobile sensors: G k =(V,E k ,A k ) Among them, G k The topology graph representing the communication topology at time k is a time-varying undirected graph; V represents a point set containing N points, where N is the number of the plurality of mobile sensors; E k A represents the set of edges at time k, where there is an edge between two mobile sensors whose distance at time k is less than a preset maximum communication distance; k The adjacency matrix of the topology graph representing the communication topology at time k; The detection model construction module is specifically used for: For each motion sensor, based on the position of the motion sensor and the position of the mobile robot, it is determined whether the mobile robot is within the detection range of the motion sensor; If the mobile robot is not within the detection range of the motion sensor, then the detection value of the motion sensor is determined to be 0; If the mobile robot is within the detection range of the motion sensor, then the detection model of the motion sensor is constructed: in, This represents the detection value obtained by the i-th motion sensor at time k; Describes Gaussian detection noise, with a distribution that satisfies It is the covariance matrix; the detection matrix. The attack model construction module is specifically used for: For each mobile sensor subjected to external attack, construct an attack model for that mobile sensor: in, This represents the detection value obtained by the i-th mobile sensor when it is attacked at time k; This indicates the degree to which external attacks affect the detection value.

6. The apparatus according to claim 5, characterized in that, The motion model construction module is specifically used for: Construct a motion model of the mobile robot based on its pose: Where, r k+1 This represents the pose of the mobile robot at time k+1; This represents the pose of the mobile robot at time k, including the orientation angle. and location This represents the change in the orientation angle of the mobile robot as measured by the odometer during the time interval from time k to time k+1. k+1 and position offset v k+1 ; The process noise resulting from the measurement by the odometer is represented by a Gaussian distribution N(0,Q); R(φ) k Let ) denote a rotation matrix that satisfies

Images