# Navigation Control Method of Indoor Mobile Robot Based on Visual Servo

Authors: Chongfu Huang; Wei Zhang
Journal: International Journal of Antennas and Propagation
 (2022)

---

## Abstract

In order to improve the navigation control effect of indoor mobile robot, this paper studies the navigation control method of indoor mobile robot combined with visual servo technology. For the problem that the target may be out of the camera’s field of view during the servo process, a control algorithm with field-of-view constraints is proposed, which assigns a certain weight to the image feature error at each moment. Moreover, this paper realizes the servo process that the image features are always located in the field of view by setting the variable values involved in the weight value calculation method. Through the simulation test, it is verified that the indoor mobile robot navigation control method based on visual servo has certain effects.


---

## Body

## 1. Introduction

Visual navigation and positioning technology have emerged in recent years, and are also the further development of computer image processing technology in the field of intelligent control. This technology utilizes the industrial camera installed on the moving carrier to continuously collect the pose information in the navigation pattern in the driving path, and input it to the controller of the driving system after processing and calculating using the internal processor to realize the navigation [1]. Generally speaking, if a QR code image is laid on the preset driving path, the processing calculation is to parse out the relevant position information through special analysis software; and calculate the pose of the motion carrier according to the geometric relationship of these positions, if the preset driving path is paved with images containing several target objects. The internal processor first uses the target detection algorithm to identify and locate the area of each small target and then uses the corner detection algorithm to extract several corners of the target area. The geometric relationship of the corner points is calculated to obtain the pose of the motion vector [2]. Comparing the two navigation patterns, the QR code-type navigation pattern is complex in design and difficult to lay on the industrial road, while the navigation image with the target object type has a simple design and is more convenient to lay on the industrial road. In addition, the target detection algorithm containing the target object-type navigation pattern has been continuously researched and improved on the basis of the fusion of deep learning algorithms in recent years, which is a key step for the motion carrier to use this kind of navigation pattern for visual navigation and positioning [3].RFID is a wireless communication technology that can identify the target only through radio signals without establishing direct contact with the target, and can exchange data with the target. Generally speaking, RFID devices can be divided into four frequency bands: low frequency, high frequency, ultrahigh frequency, and microwave according to the different frequency bands of the radiofrequency carrier [4]. Among them, ultrahigh frequency (UHF) RFID has the advantages of long reading distance and high reading speed compared with other frequency-band RFID and has been widely used in the field of warehousing environment and logistics and transportation [5].A typical RFID system is mainly composed of: reader, antenna, tag, and a background server. Among them, the main function of the reader is to generate radiofrequency signals and transmit them through the antenna so as to communicate with the tags and to filter and demodulate the received radiofrequency signals to obtain the information [6]. The antenna is the bridge between the RFID reader and the tag in the communication process. It can be used to transmit the radiofrequency signal generated by the reader and receive the radiofrequency signal reflected by the tag and transmit it to the reader for processing. The main function of the background server is to control the reader and manage the signals read by the reader. Tags can be divided into active tags (Activetag), semiactive tags (Semiactivetag), and passive tags (Passivetag) according to whether they need additional power supply [7]. Since active tags need to be powered by batteries, although the reading distance is relatively long, the battery life is limited, and the batteries need to be replaced regularly and the cost is high. Passive tags do not need battery power, small size, low cost, and long life, so no source tags are widely used [8].Detection means obtaining the effective information of the target from the video frame, which is the basis for the effective operation of the subsequent tracking module, which can be said to be the basic link of visual tracking. The detection methods studied so far mainly include the background difference method, frame difference method, and motion field estimation method [9]. The frame difference method is a commonly used method in the detection of moving objects. This method is to process the difference between the video frames accordingly to obtain the position information of the moving object. The effect is not good when the moving target is small; the background difference rule is to mathematically model the fixed background. Once a moving target appears in the background model, it is immediately recognized as a foreground target. Of course, accurate modeling is not easy, and if the focal length needs to be adjusted and the background depth of the field changes, then the modeling work will be extremely difficult [10]; the most common motion field estimation method is the optical flow method, which obtains the optical flow direction of the pixels of the moving target in a two-dimensional image. This method has a large amount of calculation and is sensitive to noise, and is also used as a basic link in the tracking process [11].Matching tracking can be divided into four categories: model-based tracking, deformed template-based tracking, feature-based tracking, and region-based tracking. The idea based on the region matching method is to detect or extract the target template according to a certain method in the video sequence, find the best matching point based on the template matching algorithm in the following tracking, determine the target region, and update it regularly or irregularly in a template; feature-based tracking first extracts obvious features (centroids, corners, and boundaries) from the image and then matches the best similar target in the next video frame; this method can adapt to the tracking situation of partial occlusion, deformation-based. The template method hopes to perform matching by extracting the target contour or edge. Since the edge model is a vector model, it has good elastic adaptability and can be well adapted to complex backgrounds and occlusion situations, but it is troublesome to initialize contour extraction; model-based tracking is mainly by modeling the target (2D or 3D) to achieve the target body multipose tracking, and the accuracy is high but the real-time performance is poor [12].Visual tracking based on filtering technology is also a very important technology in the field of visual tracking. This method is different from other methods that directly process images to obtain target information. Instead, it models the target and estimates its state to obtain target information. Obtain target tracking information [13]. Commonly used filtering methods are Kalman filter (KF), extended Kalman filter (EKF), unscented Kalman filter (UKF), particle filter (PF), and so on. Generally, the basic Kalman filter is only used for linear problems, and it is basically nonlinear in tracking problems, so it is rarely used [14]; EKF and UKF can transform nonlinear problems through linearization methods. Of course, too serious nonlinear problems, linearization, and non-Gaussian row problems cannot be completely solved; particle filtering does not require linear and Gaussian row assumptions for the problem, based on Bayesian theory and Monte Carlo methods, random discrete sampling of the target state of the system (extract particles), represent the system state probability density [15].Fusion-based tracking is actually a combination of multiple tracking algorithms, but in fact, many tracking problems require fusion methods because each algorithm has its own strengths and weaknesses, so fusion use can learn from each other, and the tracking effect achieves our expectations. Requirements to improve tracking accuracy. Of course, the multimethods included in the fusion method can be a variety of different tracking algorithms or can be different aspects of the same algorithm, such as multifeature fusion and multimodel fusion, which is to select a variety of features and models under the same tracking algorithm. Improving the tracking algorithm can effectively improve the tracking accuracy and solve problems such as occlusion [16]. Of course, the more fusion methods, the more accurate the tracking, but this also increases the constraints on the target object, complicates the environment, and greatly affects the real-time performance. Therefore, how to fuse the tracking methods reasonably is also one of the research directions of the tracking algorithm.This paper combines visual servo technology to study the navigation control method of indoor mobile robot to improve the navigation control effect of indoor mobile robot.


## 2. Visual Servo System Design

### 2.1. Design of Constrained and Delayed Controller

In the visual servo system, the image feature information is out of the field of view of the camera, which will lead to a failed visual servo. By adding certain constraints to the feature quantities in the image space, the control input that satisfies the visual field constraints can be obtained, and the failure phenomenon caused by the target not existing in the camera's visual field can be avoided. Since the sampling period of the vision sensor is inconsistent with the control period of the manipulator, the time delay at each moment of the system is different. Moreover, performing certain feature estimates for each moment is expected to fundamentally compensate for the image delay problem.The weight error is defined as assigning a certain weight value to each error amount on the basis of the task error specified by the system, which is expressed as(1)ew=Wet.Among them,W is the weight matrix of the characteristic error amount of the system image and is in the form of a diagonal matrix, which, respectively, corresponds to each error amount in the system.For systems with multiple feature information, the specific form of the weight matrix is as follows:(2)W=W1⋯0⋮⋱⋮0⋮Wk.Among them,Wi is the weight value corresponding to the i-th feature error in the system, which depends on the image feature amount at the current moment.Therefore, the above formula (1) can be specifically described as(3)ew1ew2⋮ewl=W10⋯00W2⋯0⋮⋮⋱⋮00⋯Wl=et1et2⋮etl.In the above formula,l represents the number of feature quantities, which is twice the number of corresponding feature points.When the weight value of each error amount in the system is set to 1, that is,W = Itxl, at this point, ew=et is satisfied, and the system is in the traditional form of the control law. In order to constrain the field of view of the camera, the weight value is set according to the feature amount at this time for different feature error amounts to achieve the effect of constraint. The determination of the weight value will now be described.


#### 2.1.1. Determine the Visual Field Constraint Range of the System

The visual servo control system takes the acquired image features as the amount of information, and the image features reflect the pixel coordinates of the target object in the camera image plane, so the resolution of the vision sensor limits the range of available feature amounts in the system. If the maximum resolution of the camera used isu∗ vr, the visual field constraint range of the system is the size value of the maximum resolution, corresponding to the range of feature quantities in the horizontal and vertical directions, respectively.su and sv represent the image feature values in the horizontal and vertical directions, respectively, su− and sv− represent the lower limit values of the image feature quantities in the horizontal and vertical directions, and su+sv+ represent the upper limit values of the image feature quantities in the horizontal and vertical directions. Then, the mathematical description of the constraint range in the horizontal direction and the constraint range in the vertical direction obtained by the vision sensor is as follows:(4)su∈su−,su+=0,ul,sv∈sv−,sv+=0,vl.The constraint range determined by the resolution of the vision sensor itself in the above is called the physical constraint of the system field of view.


#### 2.1.2. Define the Security Range of System Features

When the image feature value is closer to the edge, it is easier to leave the field of view. On the basis of the physical constraints of the field of view, by setting an adjustment parameterρ, it is used to adjust the safety range space of the set feature quantity, which is defined as the safety limit of the field of view. The connection between the two constraint ranges is as follows:(5)su−ρ=su−+ρsu+−su−,su+ρ=su++ρsu+−su−,(6)sv−ρ=sv−+ρsv+−sv−,sv+ρ=sv++ρsv+−sv−.In formulas (5) and (6), ρ satisfies 0≤ρ≤05. When the parameter ρ is set to 0, the security constraints in the system are equivalent to physical constraints, and the coefficient ρ can be adjusted appropriately according to the size of the visual field constraints in the system.The image plane for the restricted field of view is shown in Figure1.Figure 1
Image field-of-view constraint range diagram.According to the prescribed visual field constraints and safety constraints, the calculation method of the weight value corresponding to the system image feature value is given, and the formula is as follows:(7)Wu=e−sui−sum2nsui−su−ρmsuρ−suim,su−ρ<su−i<su+ρ,0,su−≤sui<su−ρ||su+ρ≤sui<su+,Wv=e−svi−svm2nsvi−sv−ρmsv+ρ−svim,sv−ρ<sv−i<sv+ρ,0,sv−≤svi<sv−ρ||sv+ρ≤svi<sv+..In the formula,sum and svm are the median value of the safety constraint range in the horizontal and vertical directions of the image, and n and m are the adjustment coefficients for weight calculation.The median calculation formula is as follows:(8)sum=su−ρ+su+ρ2,svm=sv−ρ+sv+ρ2.The schematic diagram of the weight curve is given. In the case that the physical constraint range of the visual field is 610∗ 180, the adjustment coefficient of the safety range setting in the setting system is p = 0.25, and the values of n and m are shown in Figures 2(a) and 2(b)n = m = 1 is set, and the value of p is as marked in Figures 2(c) and 2(d).Figure 2
Curve diagrams of feature weights. (a) Horizontal direction weight curve diagram, n/m. (b) Vertical directional weight curve diagram, n/m. (c) Horizontal direction weight curve diagram,ρ. (d) Vertical directional weight curve diagram, ρ.
(a)(b)(c)(d)According to the distance between the target feature set by the system and the current actual feature, the values of the parametersp and n, m can be adjusted appropriately so that the system can achieve the desired effect.The system completes visual servoing, that is, when the task error in the system meets the set error value, it is considered that the system reaches the set desired image feature position. The system counts the number of features whose current error value is less than the set error value in real time. For the situation where four target position points are set in the system, there are eight image features. When the number of statistics ≥6, the weight of the feature in the system is reset to 1, which can improve the situation of inactivity within the safe range.For the error for which the weight value is set, the task error vectoret becomes the weight error vector ew in the system. Then, the interaction matrix used in the system is also given the same weight value, that is, the basic interaction matrix J is changed to a weighted interaction matrix Jw=WJI so as to form a new variable weight control law. The form is as follows:(9)vc=−λWJIk+Wetk,q˙k=−λJqk+ςTeck+WJIk+Wetk.The speed control quantity obtained by the weight control law is substituted into the discrete visual servo system model, and the joint incremental model of the system is obtained as(10)Δqk+1=αΔqk+βTrq˙k=αΔqk−λβTrJqk+ςTeck+WJIk+Wetk.LetA∈ℂn×m, if matrix X∈ℂn×m satisfies some or all of the following relations:(11)AXA=A,XAX=X,AXH=AX,XAH=XA.Then,X is called the generalized inverse of A.Specifically, ifX satisfies equation (i), then denote X = A(i), then the set of A(i) is A{i}, A(1) is denoted as A-, and A(1, 2, 3, 4) are recorded as A+. The relevant theorems about the operational properties of generalized inverse matrices are as follows:If we setS∈Cmm×m, T∈Cnn×n and B= SAT, then:(12)SAT+=T−1A+S−1.According to the above theorem, the expanded form of the interaction matrix with weights is considered. The weight matrix is a diagonal matrix of dimension 8 × 8, and the interaction matrix is a matrix of dimension 8 × 6. Moreover, it is considered that the matrix is always in the state of full rank, so the equation is simplified by adding the form of unit matrix to the model, and the calculation is further deduced. It is believed that the above equation (13) can be simplified into the following equation:(13)Δqk+1=αΔqk−λβTrJqk+ςTeck+JIk+etk=αΔqk−λβTrCk+etk.Considering the delay problem caused by sampling, the above equation is further transformed into the following equation:(14)Δqk+1=αΔqk−λβTrCk−d+etk−d.Therefore, the application of the stable gain threshold analysis under the traditional exponential convergence algorithm to the visual field constraint of the weight design is still feasible.


### 2.2. Image Feature Estimation with Delay Compensation

In the robotic arm vision servo control system, the sampling rate of the used vision sensor is generally slower than the communication rate of the robotic arm. Moreover, the sampling period is longer than the control period of the robotic arm, which makes the image features used in the system have a time delay problem, which in turn affects the performance of the control system, and it is mainly reflected in the longer servo time to a certain extent. The relationship between the camera sampling rate, the control frequency of the robotic arm, and the related image feature sequence in the system is shown in Figure3. Starting from the image feature itself, the influence of time delay is reduced by real-time estimation and compensation of the image feature quantity at the current moment of the system.Figure 3
Feature time-series diagram of the visual servo control system. (a) Frequency difference chart and (b) feature timing diagram.
(a)(b)The control period of the robotic arm isT, and the sampling period of the camera is T. The system can obtain hT in real time. However, the image feature information within the sampling interval cannot be obtained, so the actual value obtained is replaced by the estimated value for the image feature within the sampling interval of the vision sensor.The image feature quantity obtained by the system at the current moment is represented ass(k), and the image feature prediction value at the current moment is represented as s(k). Finally, the estimated value of the current moment is applied to the system to realize servo control.This paper mathematically expresses the motion of system image features:(15)uk+1=uk+VukTr,vk+1=vk+VvkTr..In the formula,s2k and Vvk are the image feature velocities at the current moment, uk and vk are the image features at the current moment k.The state variableX(k) of the system is defined as the combination of image feature and image feature velocity, and the output vector Y(k) is the image feature, as follows:(16)Xk=s1ks2ks3ks4kV1kV2kV3kV4kT,Yk=s1ks2ks3ks4k.In the formula,V=VukVvk is the image feature velocity.si=uikvik is the image feature.Combined with the mathematical formula (15) of the characteristic motion, the state description equation is obtained:(17)Xk+1=AXk+wk,Yk=CXk+vk..Among them,(18)A=I8×8TrI8×808×8I8×8,C=I8×808×8.In the formula,A is the system state transition matrix, Cis the observation matrix of the system, w is the process noise matrix with mean 0 and variance Q, and v is the observation noise sequence for the system with mean 0 and variance R.The Kalman filter algorithm is used to complete the calculation of the image feature estimator at the current moment through the state quantity of the system at the previous moment and the predicted quantity at the current moment. The model is as follows, divided into prediction and update parts.


#### 2.2.1. Prediction Equation

(19)X1k=AXk,P1k=APkAT+Qk..In the formula,X1(k) is the state predictor at the current moment, P1(k) is the covariance matrix of X1(k), and Q(k) is the state transition covariance matrix.


#### 2.2.2. Update Equation

(20)Kk=P1kCTRk+CP1kCT−1,X^k=X1k+KkYk−CX1k,Pk=I−KkCP1k.In the formula,K(k) is the Kalman gain value, X^k is the best estimated value at the current moment, P(k) is the modified covariance matrix, R(k) is the observation noise covariance matrix, and Y(k) is the current image feature quantity predicted by historical data at the current moment.The image feature quantities of the firstm moments of the system are, respectively, recorded as s(1), s(2), …, s(m). The known m sets of data fit a polynomial description equation to expect the estimation of the current image feature value through the polynomial.The equation representation of the polynomial fit is as follows:(21)Y˜k+1=k1X1k+k2X2k.In the formula,X1(k), X2(k) is the image feature quantity in the state variable and the image feature velocity quantity, Y˜k+1 is the estimated value of the feature quantity at the current moment to the feature quantity at the next moment, and k1, k2 is the coefficient value of the image feature amount and the image feature velocity amount. Fitting the coefficients of the polynomial through the acquired historical data makes the objective function f reach the minimum value, and the function is as follows:(22)f=∑k=1m−1∑i=14bksiuk+1−Y˜2i−1k+12+sivk+1−Y˜2ik+12.In the formula,siu, Siv is the horizontal and vertical components of the image feature point at the current moment, Y˜i is the i-th component of the predicted image feature quantity, bk is the weight value of the k-th group of data in the objective function, and the sum is 1.For the assumed content specified by this system, when the system is at timeTr1, its image feature value is the acquired image feature value, that is, the value at time Tc1. The image feature quantity at time Tr1 is used as the image acquisition quantity at each time in the sampling interval, and when the system is at time Tr2, the image feature quantity is the calculated estimated value s^r2. The image feature quantity at time Tr1 is taken as the feature quantity at the previous moment, and the current prediction quantity s¯r2 is calculated by the polynomial obtained from the historical data. After that, by combining the two data, the image features at the current time Tr2 are estimated in one step, and the result value s^r2 is used as the feature quantity at the time Tr2. By analogy, the feature quantity at time Trm of the system is the quantity obtained at time Tr1, and the actual usage quantity at time Trm at the current time is obtained after Trm−1-step estimation through the prediction quantity of historical data. Alternatively, by estimating the number of delay periods for the acquisition amount at the current moment, the system will exhibit faster convergence.


## 2.1. Design of Constrained and Delayed Controller

In the visual servo system, the image feature information is out of the field of view of the camera, which will lead to a failed visual servo. By adding certain constraints to the feature quantities in the image space, the control input that satisfies the visual field constraints can be obtained, and the failure phenomenon caused by the target not existing in the camera's visual field can be avoided. Since the sampling period of the vision sensor is inconsistent with the control period of the manipulator, the time delay at each moment of the system is different. Moreover, performing certain feature estimates for each moment is expected to fundamentally compensate for the image delay problem.The weight error is defined as assigning a certain weight value to each error amount on the basis of the task error specified by the system, which is expressed as(1)ew=Wet.Among them,W is the weight matrix of the characteristic error amount of the system image and is in the form of a diagonal matrix, which, respectively, corresponds to each error amount in the system.For systems with multiple feature information, the specific form of the weight matrix is as follows:(2)W=W1⋯0⋮⋱⋮0⋮Wk.Among them,Wi is the weight value corresponding to the i-th feature error in the system, which depends on the image feature amount at the current moment.Therefore, the above formula (1) can be specifically described as(3)ew1ew2⋮ewl=W10⋯00W2⋯0⋮⋮⋱⋮00⋯Wl=et1et2⋮etl.In the above formula,l represents the number of feature quantities, which is twice the number of corresponding feature points.When the weight value of each error amount in the system is set to 1, that is,W = Itxl, at this point, ew=et is satisfied, and the system is in the traditional form of the control law. In order to constrain the field of view of the camera, the weight value is set according to the feature amount at this time for different feature error amounts to achieve the effect of constraint. The determination of the weight value will now be described.


### 2.1.1. Determine the Visual Field Constraint Range of the System

The visual servo control system takes the acquired image features as the amount of information, and the image features reflect the pixel coordinates of the target object in the camera image plane, so the resolution of the vision sensor limits the range of available feature amounts in the system. If the maximum resolution of the camera used isu∗ vr, the visual field constraint range of the system is the size value of the maximum resolution, corresponding to the range of feature quantities in the horizontal and vertical directions, respectively.su and sv represent the image feature values in the horizontal and vertical directions, respectively, su− and sv− represent the lower limit values of the image feature quantities in the horizontal and vertical directions, and su+sv+ represent the upper limit values of the image feature quantities in the horizontal and vertical directions. Then, the mathematical description of the constraint range in the horizontal direction and the constraint range in the vertical direction obtained by the vision sensor is as follows:(4)su∈su−,su+=0,ul,sv∈sv−,sv+=0,vl.The constraint range determined by the resolution of the vision sensor itself in the above is called the physical constraint of the system field of view.


### 2.1.2. Define the Security Range of System Features

When the image feature value is closer to the edge, it is easier to leave the field of view. On the basis of the physical constraints of the field of view, by setting an adjustment parameterρ, it is used to adjust the safety range space of the set feature quantity, which is defined as the safety limit of the field of view. The connection between the two constraint ranges is as follows:(5)su−ρ=su−+ρsu+−su−,su+ρ=su++ρsu+−su−,(6)sv−ρ=sv−+ρsv+−sv−,sv+ρ=sv++ρsv+−sv−.In formulas (5) and (6), ρ satisfies 0≤ρ≤05. When the parameter ρ is set to 0, the security constraints in the system are equivalent to physical constraints, and the coefficient ρ can be adjusted appropriately according to the size of the visual field constraints in the system.The image plane for the restricted field of view is shown in Figure1.Figure 1
Image field-of-view constraint range diagram.According to the prescribed visual field constraints and safety constraints, the calculation method of the weight value corresponding to the system image feature value is given, and the formula is as follows:(7)Wu=e−sui−sum2nsui−su−ρmsuρ−suim,su−ρ<su−i<su+ρ,0,su−≤sui<su−ρ||su+ρ≤sui<su+,Wv=e−svi−svm2nsvi−sv−ρmsv+ρ−svim,sv−ρ<sv−i<sv+ρ,0,sv−≤svi<sv−ρ||sv+ρ≤svi<sv+..In the formula,sum and svm are the median value of the safety constraint range in the horizontal and vertical directions of the image, and n and m are the adjustment coefficients for weight calculation.The median calculation formula is as follows:(8)sum=su−ρ+su+ρ2,svm=sv−ρ+sv+ρ2.The schematic diagram of the weight curve is given. In the case that the physical constraint range of the visual field is 610∗ 180, the adjustment coefficient of the safety range setting in the setting system is p = 0.25, and the values of n and m are shown in Figures 2(a) and 2(b)n = m = 1 is set, and the value of p is as marked in Figures 2(c) and 2(d).Figure 2
Curve diagrams of feature weights. (a) Horizontal direction weight curve diagram, n/m. (b) Vertical directional weight curve diagram, n/m. (c) Horizontal direction weight curve diagram,ρ. (d) Vertical directional weight curve diagram, ρ.
(a)(b)(c)(d)According to the distance between the target feature set by the system and the current actual feature, the values of the parametersp and n, m can be adjusted appropriately so that the system can achieve the desired effect.The system completes visual servoing, that is, when the task error in the system meets the set error value, it is considered that the system reaches the set desired image feature position. The system counts the number of features whose current error value is less than the set error value in real time. For the situation where four target position points are set in the system, there are eight image features. When the number of statistics ≥6, the weight of the feature in the system is reset to 1, which can improve the situation of inactivity within the safe range.For the error for which the weight value is set, the task error vectoret becomes the weight error vector ew in the system. Then, the interaction matrix used in the system is also given the same weight value, that is, the basic interaction matrix J is changed to a weighted interaction matrix Jw=WJI so as to form a new variable weight control law. The form is as follows:(9)vc=−λWJIk+Wetk,q˙k=−λJqk+ςTeck+WJIk+Wetk.The speed control quantity obtained by the weight control law is substituted into the discrete visual servo system model, and the joint incremental model of the system is obtained as(10)Δqk+1=αΔqk+βTrq˙k=αΔqk−λβTrJqk+ςTeck+WJIk+Wetk.LetA∈ℂn×m, if matrix X∈ℂn×m satisfies some or all of the following relations:(11)AXA=A,XAX=X,AXH=AX,XAH=XA.Then,X is called the generalized inverse of A.Specifically, ifX satisfies equation (i), then denote X = A(i), then the set of A(i) is A{i}, A(1) is denoted as A-, and A(1, 2, 3, 4) are recorded as A+. The relevant theorems about the operational properties of generalized inverse matrices are as follows:If we setS∈Cmm×m, T∈Cnn×n and B= SAT, then:(12)SAT+=T−1A+S−1.According to the above theorem, the expanded form of the interaction matrix with weights is considered. The weight matrix is a diagonal matrix of dimension 8 × 8, and the interaction matrix is a matrix of dimension 8 × 6. Moreover, it is considered that the matrix is always in the state of full rank, so the equation is simplified by adding the form of unit matrix to the model, and the calculation is further deduced. It is believed that the above equation (13) can be simplified into the following equation:(13)Δqk+1=αΔqk−λβTrJqk+ςTeck+JIk+etk=αΔqk−λβTrCk+etk.Considering the delay problem caused by sampling, the above equation is further transformed into the following equation:(14)Δqk+1=αΔqk−λβTrCk−d+etk−d.Therefore, the application of the stable gain threshold analysis under the traditional exponential convergence algorithm to the visual field constraint of the weight design is still feasible.


## 2.1.1. Determine the Visual Field Constraint Range of the System

The visual servo control system takes the acquired image features as the amount of information, and the image features reflect the pixel coordinates of the target object in the camera image plane, so the resolution of the vision sensor limits the range of available feature amounts in the system. If the maximum resolution of the camera used isu∗ vr, the visual field constraint range of the system is the size value of the maximum resolution, corresponding to the range of feature quantities in the horizontal and vertical directions, respectively.su and sv represent the image feature values in the horizontal and vertical directions, respectively, su− and sv− represent the lower limit values of the image feature quantities in the horizontal and vertical directions, and su+sv+ represent the upper limit values of the image feature quantities in the horizontal and vertical directions. Then, the mathematical description of the constraint range in the horizontal direction and the constraint range in the vertical direction obtained by the vision sensor is as follows:(4)su∈su−,su+=0,ul,sv∈sv−,sv+=0,vl.The constraint range determined by the resolution of the vision sensor itself in the above is called the physical constraint of the system field of view.


## 2.1.2. Define the Security Range of System Features

When the image feature value is closer to the edge, it is easier to leave the field of view. On the basis of the physical constraints of the field of view, by setting an adjustment parameterρ, it is used to adjust the safety range space of the set feature quantity, which is defined as the safety limit of the field of view. The connection between the two constraint ranges is as follows:(5)su−ρ=su−+ρsu+−su−,su+ρ=su++ρsu+−su−,(6)sv−ρ=sv−+ρsv+−sv−,sv+ρ=sv++ρsv+−sv−.In formulas (5) and (6), ρ satisfies 0≤ρ≤05. When the parameter ρ is set to 0, the security constraints in the system are equivalent to physical constraints, and the coefficient ρ can be adjusted appropriately according to the size of the visual field constraints in the system.The image plane for the restricted field of view is shown in Figure1.Figure 1
Image field-of-view constraint range diagram.According to the prescribed visual field constraints and safety constraints, the calculation method of the weight value corresponding to the system image feature value is given, and the formula is as follows:(7)Wu=e−sui−sum2nsui−su−ρmsuρ−suim,su−ρ<su−i<su+ρ,0,su−≤sui<su−ρ||su+ρ≤sui<su+,Wv=e−svi−svm2nsvi−sv−ρmsv+ρ−svim,sv−ρ<sv−i<sv+ρ,0,sv−≤svi<sv−ρ||sv+ρ≤svi<sv+..In the formula,sum and svm are the median value of the safety constraint range in the horizontal and vertical directions of the image, and n and m are the adjustment coefficients for weight calculation.The median calculation formula is as follows:(8)sum=su−ρ+su+ρ2,svm=sv−ρ+sv+ρ2.The schematic diagram of the weight curve is given. In the case that the physical constraint range of the visual field is 610∗ 180, the adjustment coefficient of the safety range setting in the setting system is p = 0.25, and the values of n and m are shown in Figures 2(a) and 2(b)n = m = 1 is set, and the value of p is as marked in Figures 2(c) and 2(d).Figure 2
Curve diagrams of feature weights. (a) Horizontal direction weight curve diagram, n/m. (b) Vertical directional weight curve diagram, n/m. (c) Horizontal direction weight curve diagram,ρ. (d) Vertical directional weight curve diagram, ρ.
(a)(b)(c)(d)According to the distance between the target feature set by the system and the current actual feature, the values of the parametersp and n, m can be adjusted appropriately so that the system can achieve the desired effect.The system completes visual servoing, that is, when the task error in the system meets the set error value, it is considered that the system reaches the set desired image feature position. The system counts the number of features whose current error value is less than the set error value in real time. For the situation where four target position points are set in the system, there are eight image features. When the number of statistics ≥6, the weight of the feature in the system is reset to 1, which can improve the situation of inactivity within the safe range.For the error for which the weight value is set, the task error vectoret becomes the weight error vector ew in the system. Then, the interaction matrix used in the system is also given the same weight value, that is, the basic interaction matrix J is changed to a weighted interaction matrix Jw=WJI so as to form a new variable weight control law. The form is as follows:(9)vc=−λWJIk+Wetk,q˙k=−λJqk+ςTeck+WJIk+Wetk.The speed control quantity obtained by the weight control law is substituted into the discrete visual servo system model, and the joint incremental model of the system is obtained as(10)Δqk+1=αΔqk+βTrq˙k=αΔqk−λβTrJqk+ςTeck+WJIk+Wetk.LetA∈ℂn×m, if matrix X∈ℂn×m satisfies some or all of the following relations:(11)AXA=A,XAX=X,AXH=AX,XAH=XA.Then,X is called the generalized inverse of A.Specifically, ifX satisfies equation (i), then denote X = A(i), then the set of A(i) is A{i}, A(1) is denoted as A-, and A(1, 2, 3, 4) are recorded as A+. The relevant theorems about the operational properties of generalized inverse matrices are as follows:If we setS∈Cmm×m, T∈Cnn×n and B= SAT, then:(12)SAT+=T−1A+S−1.According to the above theorem, the expanded form of the interaction matrix with weights is considered. The weight matrix is a diagonal matrix of dimension 8 × 8, and the interaction matrix is a matrix of dimension 8 × 6. Moreover, it is considered that the matrix is always in the state of full rank, so the equation is simplified by adding the form of unit matrix to the model, and the calculation is further deduced. It is believed that the above equation (13) can be simplified into the following equation:(13)Δqk+1=αΔqk−λβTrJqk+ςTeck+JIk+etk=αΔqk−λβTrCk+etk.Considering the delay problem caused by sampling, the above equation is further transformed into the following equation:(14)Δqk+1=αΔqk−λβTrCk−d+etk−d.Therefore, the application of the stable gain threshold analysis under the traditional exponential convergence algorithm to the visual field constraint of the weight design is still feasible.


## 2.2. Image Feature Estimation with Delay Compensation

In the robotic arm vision servo control system, the sampling rate of the used vision sensor is generally slower than the communication rate of the robotic arm. Moreover, the sampling period is longer than the control period of the robotic arm, which makes the image features used in the system have a time delay problem, which in turn affects the performance of the control system, and it is mainly reflected in the longer servo time to a certain extent. The relationship between the camera sampling rate, the control frequency of the robotic arm, and the related image feature sequence in the system is shown in Figure3. Starting from the image feature itself, the influence of time delay is reduced by real-time estimation and compensation of the image feature quantity at the current moment of the system.Figure 3
Feature time-series diagram of the visual servo control system. (a) Frequency difference chart and (b) feature timing diagram.
(a)(b)The control period of the robotic arm isT, and the sampling period of the camera is T. The system can obtain hT in real time. However, the image feature information within the sampling interval cannot be obtained, so the actual value obtained is replaced by the estimated value for the image feature within the sampling interval of the vision sensor.The image feature quantity obtained by the system at the current moment is represented ass(k), and the image feature prediction value at the current moment is represented as s(k). Finally, the estimated value of the current moment is applied to the system to realize servo control.This paper mathematically expresses the motion of system image features:(15)uk+1=uk+VukTr,vk+1=vk+VvkTr..In the formula,s2k and Vvk are the image feature velocities at the current moment, uk and vk are the image features at the current moment k.The state variableX(k) of the system is defined as the combination of image feature and image feature velocity, and the output vector Y(k) is the image feature, as follows:(16)Xk=s1ks2ks3ks4kV1kV2kV3kV4kT,Yk=s1ks2ks3ks4k.In the formula,V=VukVvk is the image feature velocity.si=uikvik is the image feature.Combined with the mathematical formula (15) of the characteristic motion, the state description equation is obtained:(17)Xk+1=AXk+wk,Yk=CXk+vk..Among them,(18)A=I8×8TrI8×808×8I8×8,C=I8×808×8.In the formula,A is the system state transition matrix, Cis the observation matrix of the system, w is the process noise matrix with mean 0 and variance Q, and v is the observation noise sequence for the system with mean 0 and variance R.The Kalman filter algorithm is used to complete the calculation of the image feature estimator at the current moment through the state quantity of the system at the previous moment and the predicted quantity at the current moment. The model is as follows, divided into prediction and update parts.


### 2.2.1. Prediction Equation

(19)X1k=AXk,P1k=APkAT+Qk..In the formula,X1(k) is the state predictor at the current moment, P1(k) is the covariance matrix of X1(k), and Q(k) is the state transition covariance matrix.


### 2.2.2. Update Equation

(20)Kk=P1kCTRk+CP1kCT−1,X^k=X1k+KkYk−CX1k,Pk=I−KkCP1k.In the formula,K(k) is the Kalman gain value, X^k is the best estimated value at the current moment, P(k) is the modified covariance matrix, R(k) is the observation noise covariance matrix, and Y(k) is the current image feature quantity predicted by historical data at the current moment.The image feature quantities of the firstm moments of the system are, respectively, recorded as s(1), s(2), …, s(m). The known m sets of data fit a polynomial description equation to expect the estimation of the current image feature value through the polynomial.The equation representation of the polynomial fit is as follows:(21)Y˜k+1=k1X1k+k2X2k.In the formula,X1(k), X2(k) is the image feature quantity in the state variable and the image feature velocity quantity, Y˜k+1 is the estimated value of the feature quantity at the current moment to the feature quantity at the next moment, and k1, k2 is the coefficient value of the image feature amount and the image feature velocity amount. Fitting the coefficients of the polynomial through the acquired historical data makes the objective function f reach the minimum value, and the function is as follows:(22)f=∑k=1m−1∑i=14bksiuk+1−Y˜2i−1k+12+sivk+1−Y˜2ik+12.In the formula,siu, Siv is the horizontal and vertical components of the image feature point at the current moment, Y˜i is the i-th component of the predicted image feature quantity, bk is the weight value of the k-th group of data in the objective function, and the sum is 1.For the assumed content specified by this system, when the system is at timeTr1, its image feature value is the acquired image feature value, that is, the value at time Tc1. The image feature quantity at time Tr1 is used as the image acquisition quantity at each time in the sampling interval, and when the system is at time Tr2, the image feature quantity is the calculated estimated value s^r2. The image feature quantity at time Tr1 is taken as the feature quantity at the previous moment, and the current prediction quantity s¯r2 is calculated by the polynomial obtained from the historical data. After that, by combining the two data, the image features at the current time Tr2 are estimated in one step, and the result value s^r2 is used as the feature quantity at the time Tr2. By analogy, the feature quantity at time Trm of the system is the quantity obtained at time Tr1, and the actual usage quantity at time Trm at the current time is obtained after Trm−1-step estimation through the prediction quantity of historical data. Alternatively, by estimating the number of delay periods for the acquisition amount at the current moment, the system will exhibit faster convergence.


## 2.2.1. Prediction Equation

(19)X1k=AXk,P1k=APkAT+Qk..In the formula,X1(k) is the state predictor at the current moment, P1(k) is the covariance matrix of X1(k), and Q(k) is the state transition covariance matrix.


## 2.2.2. Update Equation

(20)Kk=P1kCTRk+CP1kCT−1,X^k=X1k+KkYk−CX1k,Pk=I−KkCP1k.In the formula,K(k) is the Kalman gain value, X^k is the best estimated value at the current moment, P(k) is the modified covariance matrix, R(k) is the observation noise covariance matrix, and Y(k) is the current image feature quantity predicted by historical data at the current moment.The image feature quantities of the firstm moments of the system are, respectively, recorded as s(1), s(2), …, s(m). The known m sets of data fit a polynomial description equation to expect the estimation of the current image feature value through the polynomial.The equation representation of the polynomial fit is as follows:(21)Y˜k+1=k1X1k+k2X2k.In the formula,X1(k), X2(k) is the image feature quantity in the state variable and the image feature velocity quantity, Y˜k+1 is the estimated value of the feature quantity at the current moment to the feature quantity at the next moment, and k1, k2 is the coefficient value of the image feature amount and the image feature velocity amount. Fitting the coefficients of the polynomial through the acquired historical data makes the objective function f reach the minimum value, and the function is as follows:(22)f=∑k=1m−1∑i=14bksiuk+1−Y˜2i−1k+12+sivk+1−Y˜2ik+12.In the formula,siu, Siv is the horizontal and vertical components of the image feature point at the current moment, Y˜i is the i-th component of the predicted image feature quantity, bk is the weight value of the k-th group of data in the objective function, and the sum is 1.For the assumed content specified by this system, when the system is at timeTr1, its image feature value is the acquired image feature value, that is, the value at time Tc1. The image feature quantity at time Tr1 is used as the image acquisition quantity at each time in the sampling interval, and when the system is at time Tr2, the image feature quantity is the calculated estimated value s^r2. The image feature quantity at time Tr1 is taken as the feature quantity at the previous moment, and the current prediction quantity s¯r2 is calculated by the polynomial obtained from the historical data. After that, by combining the two data, the image features at the current time Tr2 are estimated in one step, and the result value s^r2 is used as the feature quantity at the time Tr2. By analogy, the feature quantity at time Trm of the system is the quantity obtained at time Tr1, and the actual usage quantity at time Trm at the current time is obtained after Trm−1-step estimation through the prediction quantity of historical data. Alternatively, by estimating the number of delay periods for the acquisition amount at the current moment, the system will exhibit faster convergence.


## 3. Navigation Control Method of Indoor Mobile Robot Based on Visual Servo

For each RFID tag, there is a unique ID code inside, and the RFID tag is generally attached to the object to identify it. At the same time, the storage space inside the RFID tag can also be used to store some physical information about the posted object, which can be read and modified by the reader, as shown in Figure4.Figure 4
RFID system composition.For the passive UHF RFID system, the RFID reader transmits radiofrequency signals to the tag through the antenna, and the backscattered signal returned by the tag is received by the RFID reader. The communication process is shown in Figure5. At this time, the information that the reader can read is the ID, RSSI, and phase information of the tag.Figure 5
Tag backscatter communication model.During the process of the robot navigating to the target position, the position information of the target tag in the robot coordinate system is calculated through the target position estimation based on the acquired radiofrequency information. Then, the target position is used as the input information of the controller, and the servo controller of the robot is based on the input position information. After that, this paper combines the path planning algorithm to carry out servo control, and outputs the linear speed and rotation speed of the robot, so that the robot reaches the target label. The RFID servo method based on position information has a high dependence on the target position estimation model, and the navigation performance mainly depends on the positioning result. If the positioning fails or the positioning error is large, it is likely to cause the servo to fail, as shown in Figure6.Figure 6
Navigation diagram of RFID servo robot.Different from the RFID servo method based on position information, the robot does not need to calculate the position information of the target tag in the robot coordinate system in the process of navigating to the target position. It only relies on the characteristics of the RFID radiofrequency information fed back by the target tag to transform and process it. Then, the robot takes the characteristics as input information and then outputs the linear speed and rotation speed of the moving robot through the servo controller, and the robot can move toward the target label and keep approaching the target label. The RFID servo method based on radiofrequency information does not need reference tags, has a good navigation effect, and has a simple system and low cost. Compared with the RFID servo method based on position information, the RFID servo method based on radiofrequency information can directly control the navigation of the robot without calculating the relative pose relationship between the robot and tag. It has the advantages of low system requirements, small computational load, and no need to calibrate the relative pose relationship between the robot and antenna. In addition, it can also avoid the situation that the navigation fails due to a large error in the positioning process. The navigation of the mobile robot based on the RFID servo is shown in Figure7.Figure 7
Mobile robot navigation based on RFID servo. (a) Position-based RFID servo. (b) RF information-based RFID servo.
(a)(b)During the movement of the mobile robot, the area in front of the mobile robot is divided into three orientations: front left, front right, and front right, as shown in Figure8(a). The 30° range in front of the robot is defined as the front, the area on the left side of the front is defined as the front left, and the area on the right is defined as the front right.Figure 8
The division of the robot and the obstacle ahead. (a) The front orientation of the robot is divided. (b) Barrier orientation division.
(a)(b)To ensure that the robot can successfully perceive and avoid obstacles, an RFID tag is posted near each edge of the obstacle. The robot can read the RFID tag attached to the obstacle through the RFID reader, and estimate the angle formed by the currently read RFID tag to the center of the robot and the current direction of the robot. In this way, the angle formed by all readable labels on the obstacle and the current direction of the robot can be solved. According to the maximum angleθmax and the minimum angle θmin formed by the RFID tag on the obstacle and the robot center, the obstacle can be enveloped, as shown in Figure 8(b). Then, obstacles can be divided into corresponding azimuth areas.In order to further increase the difficulty of robot obstacle avoidance, a number of different obstacles are set up in the environment at the same time, and the obstacle avoidance and servo performance of the robot are analyzed through simulation. In the same obstacle environment, there are four obstacles with different shapes and sizes and arbitrary positions in the environment, and two groups of experiments are set in which the robot has different initial poses relative to the target label and obstacles.The initial position of the robot is set to (0, 0), the position of the target label is (300, 100), and the initial orientation of the robot is 0°. Then, we change the relative pose of the robot to the obstacle and target label, the target label position is (210, 150), and the initial orientation of the robot is 90°. The servo and obstacle avoidance processes of the robot are simulated in MobileSim, and the motion trajectories of the robot at different initial poses are obtained as shown in Figure9. Moreover, this paper analyzes the motion trajectory of the robot in MATLAB. When there are many different obstacles in the environment, the robot can navigate to the target position by changing the different initial poses of the robot relative to the target label and obstacles, and the motion trajectory of the robot is relatively smooth. After that, the motion process of the robot is further analyzed, and the change in the heading angle of the robot is shown in Figure 10.Figure 9
Obstacle avoidance trajectory in different poses. (a) Initial orientation angle of 0°, target location (300, 100). (b) Initial orientation angle of 90°, target location (210, 150). (c) Initial orientation angle of 0°, target location (300, 100). (d) Initial orientation angle of 90°, target location (210, 150).
(a)(b)(c)(d)Figure 10
Changes in angle. (a) Initial orientation angle of 0°, target location (300, 100). (b) Initial orientation angle of 90°, target location (210, 150).
(a)(b)It can be seen from Figure9 that the robot navigation system is verified through multiple sets of poses. Through the verification of multiple angles, it is found that the robot can effectively find the target point when navigating from multiple angles, and carry out reasonable path planning, which verifies the performance of the system algorithm in this paper, navigation effect and path decision effect.It can be seen from Figure10 that after setting different obstacles, the system model in this paper can still choose a reasonable obstacle avoidance route during the obstacle avoidance process, which can not only effectively avoid obstacles but also effectively improve movement efficiency, which verifies the algorithm model in this paper, effectiveness.During the movement of the robot, the heading angle changes monotonically in an interval, and the change is slow in the monotonic interval. The robot runs smoothly and the trajectory is smooth most of the time. In the presence of multiple obstacles, the servo navigation algorithm and the obstacle avoidance algorithm also have strong robustness. Therefore, it is verified that the indoor mobile robot navigation control method based on visual servo has certain effects.


## 4. Conclusion

Aiming at the defects in the traditional RFID visual servo navigation method, an innovative navigation method is proposed: an autonomous navigation method for mobile robots based on RFID servo. By using the characteristics of RFID radiofrequency information, the robot can be directly controlled to navigate to the vicinity of the target position without calculating the relative pose of the robot and the target tag. This method has the advantages of no reference label, good navigation effect, low system cost, and high computational efficiency. This paper combines visual servo technology to study the navigation control method of an indoor mobile robot to improve the navigation control effect. In the presence of multiple obstacles, the servo navigation algorithm and the obstacle avoidance algorithm also have strong robustness. Therefore, it is verified that the indoor mobile robot navigation control method based on visual servo has certain effects.



---

*Source: 6422841-2022-08-27.xml*