{'fraction_non_alphanumeric': 0.03644454854198384, 'fraction_numerical': 0.009954327204590701, 'mean_word_length': 4.382074877095676, 'pattern_counts': {'":': 0, '<': 3, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 12, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Speed Optimization Control for Wheeled Robot Navigation with Obstacle Avoidance Based on Viability Theory

The navigation efficiency of wheeled robots needs to be further improved. Although related research has proposed various approaches, most of them describe the relationship between the robot and the obstacle roughly. Viability theory concerns the dynamic adaptation of evolutionary systems to the environment. Based on viability, we explore a method that involves robot dynamic model, environmental constraints and navigation control. The method can raise the efficiency of the navigation. We treat the environment as line segments to reduce the computational difficulty for building the viability condition constraints. Although there exists lots of control values which can be used to drive the robot safely to the goal, it is necessary to build an optimization model to select a more efficient control value for the navigation. Our simulation shows that viability theory can precisely describe the link between robotic dynamics and the obstacle, and thus can help the robot to achieve radical high speed navigation in an unknown environment.


INTRODUCTION
Navigation control is an important topic for autonomous robots exploring tasks in unknown environments. In this study, the robot is expected to move from the starting location to the goal location without collision.
The key component of robot navigation is the coordination between sensor data and robot driving control. There are two categories of navigation methods, namely deliberative methods and reactive methods. Deliberative methods are also known as global path planning. These methods are used for robots in environments where complete information about stationary obstacles and slowly moving obstacles are known in advance. Fundamental path planning methods are A*,D* searching methods, see for instance [20]. When complete information about the environment is not available in advance, mobile robots use the current sensor information to decide the next action, which has strong adaptability without prior knowledge of the environment. This is known as reactive methods, see [7]. We focus on the reactive methods since they are more suitable for unknown environment navigation.
Artificial Potential Fields(APF) is an early classical approach for navigation, it is widely used in mobile robotics [19]. The robot is driven by the negative gradient of a sum of potentials from different obstacles and the goal. After APF approach, the Vector Field Histogram (VFH) is developed in [5]. VFH+ uses less parameters than VFH to drive robots more smoothly and safely [30]. VFH-like methods transform obstacles into a histogram description. All free directions of the histogram are candidates for obstacle avoidance. An artificial weighted cost function is used to evaluate the candidate directions for goal guidance.
Then, optimal direction and velocity for navigation are obtained. The obtained direction and velocity need to be implemented by a driving control method, such as Proportion Integration Differentiation (PID) control or other nonlinear controls. Navigation approaches mentioned above belong to classical Nonmodel Based Method, and are not adequate for taking internal robot constraints like shape and dynamics into account, see [24]. It is rather difficult to control a robot traveling in a high speed with dynamic and mechanical constraints. If the acceleration and deceleration are limited, an attempt to turn at a high speed behind an obstacle easily results in collision. Another nonmodel based reactive method -fuzzy control is also widely used, see for instance [1]. This method is based on the human behavior, can be used for controlling many kinds of robots, but it is not precise enough.
In order to control robot safely in a higher speed, the robot dynamic model must be taken into account. Existing model based methods include Curvature Velocity Method(CVM), Dynamic Window Approach(DWA) and Lane Curvature Method, etc., see for instance [12,28,29]. All approaches mentioned above evaluate an objective function and assume that the robot moves along circular arcs. By limiting the searching space of admissible velocities, the problem induced by robot kinematic and dynamic constraints can be solved. While dynamic constraints have been taken into account, these methods are still lacking of accuracy. Taking DWA for instance, the limitation of the robot acceleration is determined by the operator's minds and adjustments. On the other hand, when encountering an obstacle, the robot should move in circular and have the ability to stop when it contacts the obstacle for safety. But in some situations, safety can also be realized without moving in circular and stopping at obstacle. For instance, if the robot moving direction is controlled to be parallel with the outline of the obstacle before crashing with it, safety can also be ensured, what's more, with higher efficiency.
Nowadays, evolutionary methods combined with classic approaches are increasingly used for navigation. Because classical approaches are effective but tend to be trapped in local minima when environment is complex. Evolutionary methods include Genetic Algorithm(GA), Ant Colony Optimization(ACO), Particle Swarm Optimization(PSO), etc. These methods are suitable for path planning in complex environment but act slowly. The combination of classical approaches and evolutionary methods can overcome each other's drawbacks. So improving the efficiency of the classical methods would help to improve the performance of the combined methods. And other studies of navigation, such as humanoid or bionic robot navigation [25,34], machine vision aided navigation [11], multi robots cooperative navigation [8], 3-dimensions navigation [4],and so on, would benefit on it. Because these studies are more or less based on the classic methods. Besides evolutionary navigation methods, searching methods are also designed to overcome local minima, such as VFH* [31], DWA* [10], etc. For these two methods, A* searching is used to look ahead more steps in the local sensitive range for local minima avoidance. But the computation amount of these methods are large. In practice, we need to extract specific information from sensor data at first, then use these useful information for navigation.  The structure of classical reactive methods is shown in Fig. 1. Navigation control can be divided into three levels: bottom level for robot driving, middle level for obstacle avoidance and top level for goal guidance [23]. Studies confirm that different level behaviors need different information. Goal guidance need global environmental information. Obstacle avoidance need local environmental details. Robot driving need internal states, such as velocity, acceleration,etc. As we mentioned above, the robot driving control faces the problem of dynamic constraint, such as acceleration limitation which may conflict with the velocity set values from the obstacle avoidance controller. Therefore, velocity set value of goal guidance control should be chosen under the control range constrained by robot dynamic and the environment. The classical reactive navigation methods are less accurate in general. These methods lack a theoretical support to precisely describe the relationship between environment and robot dynamics, this leads to gaps between each control levels, and the overall behavior of navigation tends to be slow.
Compared with classical reactive methods, global path planning methods which need complete global environmental details for a non-collision path to the goal must combine a robot driving control method to follow the planned path. But this path may be unable to be followed due to the constrains, such as acceleration limitation. Non-Speed Optimization Control for Wheeled Robot Navigation . . . Lei Liu, Yan Gao, Yuepeng Wu model methods like VFH series also have this disadvantage. For model based methods like DWA, both acceleration limitation and obstacle constraints are considered. But the assumption of a constant acceleration/deccelearation limitation dose not conform to the actual robot characters. Both VFH and DWA do not consider the real robot motor characters, so a robot control method such as PID must be combined for motor control.
The existing classical methods describe the relationship between environment and robot dynamics inaccurately. So the speeds of existing classical methods tend to be slow. Therefore, we propose a new control framework based on a new theory for robot navigation. This theory can precisely describe robot characters and the relationship between the robot dynamics and the environment. The framework based on this theory can include all three levels of navigation behavior, and integrates robot, goal and environment information all together.
Viability which is initially introduced by Aubin [2] concerns the dynamic adaptation of evolutionary systems to environment defined by constraints. Viability can be effectively used for system control, stability analysis, security design and other control areas. The outline of viability is: Considering a control system and a set, if for any initial point in this set, there exists a solution of the control system such that it stays in the set for ever. The set is said to be viable under this control system, see for instance [13].
At the moment, the application of viability is in its infancy. Because viability are based on set-valued analysis and nonsmooth analysis, it is elusive for engineering application. In [18], V stability theory which is similar to viability is used for the navigation of a sailing robot. The performance is excellent. But the interval approach for this application needs one minute to give a control signal. This is not suitable for indoor wheeled robot. In [32], a viability kernel is provided by Support Vector Machines(SVM) techniques to make the safety analysis of a high speed vehicle. But this application just uses some notions of viability, and only provide the dangerous warning to the driver. In [26], viability is applied for the aiming control of the submarine camera. This method can provide loose control signal for the underactuated systems, but a nonholonomic control is needed for bottom robot driving cascaded with viability method. The viability problem of linear system have been solved in [9],. The navigation method presented in [24] use Lyapunov function to construct a searching map for planar robot navigation. The performance of navigation is fast. This method is partly same as viability. The safety of robot is ensured by the Control Lyapunov Function theory [27], but this method would take much computing resource to build and search on a predefined map. Recently, we initially confirmed the feasibility of high speed obstacle avoidance of wheeled robots based on viability in [22] which is a preliminary work with a predefined simple environment and an approximate robot model.
We propose a method for robot navigation based on viability. If the robot is considered as a control system, obstacle as the boundary of the region (set). Thus, the process of robot's navigation with obstacle avoidance is just a viability design in the sense of control theory. We use LADAR to explore the unknown environment. The proposed framework is shown in Fig. 2. The raw LADAR data is processed to acquire security line segments which represent the geometric feature of the local environment. This process can help to build the position and velocity boundaries for viability. What's more, this process can avoid the local minima in the sensor's range. When the robot finds its internal states (position, velocity etc.) are on the viability boundaries, we introduce these information into the viability condition constrains module shown in Fig. 2. The module can give the precise admissible ranges of control constrained by self dynamics and environment based on viability. Then, we provides an efficient radical navigation control limited by predefined ranges. The objective function of the optimizer includes the goal's information.  According to the structure of the proposed framework in Fig.2, the paper is organized as follows. In the section 2, viability theory related to the robot is briefly introduced. Then, we design the viability boundary motivated by viability in the section 3. In the section 4, robot LADAR data is processed by line segment extraction method to build viability boundary. In section 5, the navigation optimizer based on viability condition constraints are explained in detail. At last, a simulation is made to demonstrate the feasibility of our method.

SOME NOTIONS OF VIABILITY
In this section, we introduce some notions on viability. For more information of viability, see [2,13,14,16]. Consider a control systeṁ where x ∈ R n is state, u ∈ U is control, f : R n → R n , g : R n → R m+n are Lipschitzian.
Speed Optimization Control for Wheeled Robot Navigation . . .

Lei Liu, Yan Gao, Yuepeng Wu
Definition 1 [2] Let K ⊂ R n . If for any initial point x 0 ∈ K in this set, there exists a solution x(t) of the control system (1), such that it stays in the set forever, x(t) ∈ K, ∀t ≥ 0, then the set K is said to be viable under this control system.
In order to get the viable condition, we first introduce the notion of tangent cone.

Definition 2 [2]
Let K ⊂ R n be nonempty. The tangent cone of K at x ∈ K is defined by: where d K (y) is the distance of a point y ∈ R n to the set K.
In other words, T K (x) is the set of v ∈ R n such that there exists a sequence of real numbers t k > 0, k = 1, 2, . . ., converging to 0 and a sequence of v k ∈ R n , k = 1, 2, . . ., converging to v satisfying Tangent cone is the generalization of tangent plane from smooth case to nonsmooth case. With this concept, the following theorem is on the viability condition: where ∅ is empty.

Fig. 3. Viability condition and tangent cone
In Fig.3, the set K is a nonsmooth ellipse. Tangent cones of states are marked as grey area in Fig. 3. Evidently, if state x is in the interior of K, T K (x) = R n . Hence, (4) always holds for interior of K. In order to verify the viability by the Theorem 1, only states on the boundary need be considered. For instance, starting from boundary state x 1 , at least two control can pull the system back to K. But unfortunately, when the system is on the boundary at state x 2 ,the evolution of the system can't go back to the environment with any available control. In order to use this theory on the navigation of robot, we face two challenges: The first one is to find the boundary of the environment; The second one is to evaluate the relationship between the tangent cone of boundary and the evolution of the system by robot computer in numerical way. It is difficult for us to determine the viability by Theorem 1 under a nonlinear system, see [13], especially to use viability in the engineering field, see [33]. For instance, Support Vector Machine(SVM) method is used for the viability kernel training in [32]. This method consumes lots of time and computing resources for viability kernel training, and the result boundary is rough. So the literature [32] only presents some qualitative safety analysis. But in [15], viability condition for an affine nonlinear control system is given in a region defined by inequality constraints based on the nonsmooth analysis. Fortunately, our wheeled robot is a kind of affine nonlinear system, and the free regions of robot can be represented by inequality constraints.
Consider an affine nonlinear system: wherex ∈ R n is state, u ∈ U is system control, f : R n → R n , g : R n → R m+n are Lipschitzian. Generally there exists limitations on control value in real engineering problem, these limitations can be expressed by: where h i : R m → R, i = 1, · · · , p are convex (not necessary smooth). We suppose the robot environment is expressed by the following inequalities: where ϕ j : R n → R, j = 1, · · · , q are smooth function. ϕ j represent the boundaries of obstacle. From the above analysis, we confirm that viability condition only cares the tangent cone at the boundary. Due to this reason, we must check whether the state x is on the boundary or not. Given is empty set, the state x is interior of the K, otherwise it is a point on the boundary. Thus, the tangent cone of K at boundary state x ∈ K is formulated by Given a x ∈ R n , consider the following inequalities: where u ∈ R m is variable.
Theorem 2 [15] The set K is viable with respect to the system (5) if and only if the inequalities (9) has solutions for any x ∈ K.
Theorem 2 transforms the viability condition Theorem 1 from geometric type to numerical type which is suitable for computing by the embedded computer. The first inequality of (9) gives the constraints of the control. The seond inequality shows the high order relationship of viability between the system (5) and boundaries (7).

WHEELED ROBOT DYNAMICS ANALYSIS AND VIABILITY CONTROL
We choose differential driving wheeled robot as the system, because it is wildly used.

Wheeled Robot Modeling
The existing classical navigation methods generally give the linear velocity and angular velocity set values of the robot, and use other methods to indirectly drive the robot motors. In order to control the motors directly, we introduce the motor dynamics into the model of the robot.The position and orientation of the robot are (x, y) and φ, see Fig. 4. The dynamic equations of the robot is Fig. 4. Wheeled mobile robot model presented as following: where (x, y, φ) is robot position and orientation (pose) vector; (v, ω) are the linear velocity and the angular velocity; τ r1 , τ r2 are the left wheel, right wheel gearbox torques which are proportional to the motor coil current. We use k to represent the induced electromotive force coefficient; g is the ratio of motor gear box; (u 1 , u 2 ) are control voltages; (ω r1 , ω r2 ) are angular velocity of left and right wheels. We represent the motor torque by the difference between control voltage (u 1 , u 2 ) and induced electromotive force kgω r . This difference is proportional to the current of motor coil, because high order self-inductance is very small and can be ignored. Here the angular velocity of the motor is gω r . In our model, we put resistance of the coil into k. β 1 = 1/(bm), β 2 = w/(bI), where m and I are the mass and the moment of the robot; 2w is the distance of two wheels; b is the radius of the wheel. System (10) can be rewritten as the following: Evidently, the constraints of the motor control voltage are where u max is the maximum control voltage of the motor. This value is limited by the voltage of motor controller. Traditional navigation methods frequently limit the linear and angular velocities or accelerations. We only limited the motor control signal. Because according to the robot model (10), the control (u 1 , u 2 ) limitation can also limit the maximal velocities (ẋ,ẏ) and accelerations (v,ω) indirectly. The environmental obstacles are denoted by the following formula:

Viability Boundary Design
Since the control (u 1 , u 2 ) only affects the robot speed (v, ω), and has no direct influence on the robot position (x, y). But the environmental boundary (13) only constrains the position (x, y). That means ∂G ∂v = 0, ∂G ∂ω = 0. If (11) and the gradient of (13) are directly introduced into viability condition (9) as f (x) + g(x)u and ϕ j (x) T , the coefficients of (u 1 , u 2 ) would become zero, the inequalities can not give the solution for (u 1 , u 2 ). So the original boundary (13) should have more states of robot model (11).
In other words, we should raise the dimension of (13) for Speed Optimization Control for Wheeled Robot Navigation . . . Lei Liu, Yan Gao, Yuepeng Wu viability control. i.e., we should limit both position and velocity of the robot by a new boundary with higher dimensions, which includes more robot model states motivated by (9). We design a new boundary as following: where v n is the robot normal velocity towards the boundary G; l n is the distance from the robot to the boundary G; t set is a setting parameter for the collision time, v n is depended on the robot velocity, pose states and geometric feature of G, l n is measured by the robot LADAR in the nearby area. It is not necessary to know the overall information of G to obtain boundary (14). Only the information of G in the detecting range of LADAR is enough. From the information viewpoint, the viability control belongs to classical reactive navigation. The parameter t set is used to set the elasticity of the viability boundary. Smaller the value is, closer the robot is to the obstacle when it begins the obstacle avoidance, and a larger control is needed. Since the control of robot (u 1 , u 2 ) has constraint (12), the parameter t set has a lower bound. On the contrary, when this parameter is set too large, the robot would get viability control far away from the obstacle, long distance LADAR information is needed. It is not necessary to avoid the remote obstacle during navigation. On the other hand, the long distance LADAR information contains many noises. From the viewpoint of designed boundary (14), the normal direction of the obstacle and normal distance between robot and the obstacle should be measured by LADAR. So we have to process original sensor data into a geometric feature to get the information of (14).

LADAR DATA PROCESSING FOR VIABILITY CONTROL
LADAR can obtain accurate range information of the environment near by the robot. This information is given in the form of polar coordinate points. The original data should be processed in order to suit for viability. Extracting geometric feature from the original data can save the computing resources. On the other hand, geometric feature contains information needed by the viability boundary (14). Here we use line segment feature extraction method to process the LADAR data instead of traditional methods which generally change the LADAR data into a histogram. The existing mature line feature extraction methods include Line Tracking (LT) algorithm , Iterative End Point Fit (IEPF) algorithm and Prototype based Fussy Clustering(PFC) algorithm, etc., see [6]. Our method is based on IEPF, because LADAR data can be processed at a high speed by IEPF. In Ref. [17], IEPF algorithm is used to identify road features on a high-speed unmanned vehicle which won the championship thereby. We process LADAR data in three steps: The first step is breakpoint detection; the second step is line extraction; the third step is security transformation.

Breakpoints Detection
The discontinuity of LADAR data can be adaptively found by the breakpoints detection. This discontinuity would be marked as a breakpoint. Let the scan depth value of two consecutive points i,(i − 1) be ρ i , ρ i−1 . If the difference between the two values is greater than an adaptive threshold, these two consecutive scan points are a pair of breakpoints, see [6]. This processing is shown in algorithm 1.

Input:
The offset for sensor noise ε; The angular resolution of the sensor ω; The threshold angle to extrapolate the maximum range value between ρ i and ρ i−1 ,λ; The adaptive threshold between two adjacent scan point depths D max .
The i and (i − 1)th points become breakpoints;

Line Segment Extraction
In the step of line segment extraction, we convert all points of scan data from polar coordinate to cartesian coordinate. The starting and the ending points in a continuous sequence of points are connected as a line segment. Then the other scan points between these two endpoints are examined by our improved IEPF method for viability control. This method processes the convex and concave scan points differently, see Fig. 5. Assuming that LADAR is mounted at the center of the robot, the coordinate (0,0) of Fig. 5 represents the location of the robot. The raw LADAR data with noise is displayed by black square dots. Red cross points represent detected breakpoints. Pink dash line segments are the line features which are extracted from raw LADAR data using the improved IEPF method. Blue lines are obtained by rotating extracted  line segments, and are used for boundaries (13) in our navigation method. Fig. 5(a) shows data processing for concave obstacle. p ns , p ne are breakpoints of a continuous sequence of scan points. The scan points between p ns , p ne are behind the line segment p ns p ne from the robot's view. When there is no goal in the corner, p ns p ne is the extracted line segment. Fig. 5(b) shows data processing for convex obstacle. The data processing is similar with the traditional IEPF algorithm, see [17]. The algorithm splits scan points P = {p ns , · · · , p n , · · · , p ne } between two continuous breakpoints p ns , p ne into two subsets P = {p ns , · · · , p n } and P " = {p n , · · · , p ns }, when the maximum distance l max from scan points p n in the set P to the line p ns p ne is greater than the threshold l * Th . Denote the line p ns p ne as Ax + By + C = 0, C 0, the distance l n of point p n can be calculated by the following: where x n , y n are the coordinates of scan point p n . If the obstacle is convex, l n > 0,and if the obstacle is concave, l n < 0. Our improved IEPF method uses this difference to build line segments at different environmental cases. This method is shown in Algorithm 2.

Security Rotation
The real obstacle may extend beyond the extracted lines, because the raw LADAR data has noise and using line segments to represent real obstacle is inaccurate. In order to keep the robot safe, we need to move the extracted lines toward to the robot. Because viability boundaries (14) is sensitive to the normal direction of obstacle, we rotate the line segment with an angle around the end point nearer to the robot. A new line segment is achieved, see blue lines in Fig. 5. Such that not only the position of obstacle is changed for safety, but also a much more secure normal direction for the viability boundary is provided. We should manually adjust this angle, because this angle is decided by LADAR noise which may vary in different environment.
Algorithm 2 Improved IEPF method for viability control.

Input:
The breakpoints index array, BR; The minimum number of elements in a line segment N * min ; The threshold distance of scan points to endpoints line l * Th .

Output:
The line segments array LS; 1. SET n e to 1 2. WHILE n e N umberOf ScanP oints 3. SET n s to n e 4. n e + +

5.
WHILE n e in not a breakpoint in BR       SET l max to max{l 1 , · · · , l n , · · · , l ne−ns }

17.
IF l max > l * Th THEN

18.
SET n e to index of the l max scan point 19.

ENDWHILE
Wheeled robot should especially pay attention to the security line segment in front of it. This line can be used to build security boundary for obstacle control. On the other hand, doing this will reduce the details of the local environmental information greatly.

CONTROL OF ROBOT WITH LADAR
T sample is denoted as scanning period of LADAR. At the scanning time k, a temporary trajectory frame is created. The initial state of the robot is , ω (k) are the linear and angular velocities which are acquired by the robot sensor at time k. Then the LADAR data is processed in the trajectory frame. As mentioned in section IV, the security line segment in front of robot is important. We Speed Optimization Control for Wheeled Robot Navigation . . . Lei Liu, Yan Gao, Yuepeng Wu define this line in the trajectory frame with the equation y st = ax st + c, c > 0, where a is slope, c is the intercept value. This line segment can be designed for viability boundary according to (14), where The viability boundary can be expressed as following: At time k, when a proper parameter t set is given, the robot finds its own state [0, 0, 0, v (k) , ω (k) ] T is on or beyond the viability boundary (18). At this time, the robot dose not collide with the obstacle y st = ax st + c, c > 0. But the state of the robot has collision with the viability boundary (18) in the high dimension. So we need a piecewise constant control (u 1 , u 2 ) to make the robot states back into the boundary (18). The admissible control range can be obtained by viability condition (9). The viability boundary (18) only relates to the sates x t , y t , φ t , v t of the robot and does not include the state ω t . In order to use viability condition (9), we should change the robot model to have the boundary (18) states [x t , y t , φ t , v t ] T . The gradient of viability boundary (18) related to these states is If [ẋ t ,ẏ t ,φ t ,v t ] T , which includes control (u 1 , u 2 ), is obtained, inequalities (9) would give the admissible control range. Then, an optimizer is designed to select an optimal control in the admissible control range for goal guidance.

Robot Modeling in Temporary Navigation Coordinate
For model (11), the differential equation for the state ω isω The solution of the equation (20)is where C is a constant. Since viability control is piecewise constant at each scan time. The constant C can be solved by initial state ω (k) . Substitute ω (k) into (21).
where C k is the constant at time k and admits Therefor the robot model in temporary navigation coordinate frame at time k is where ω t can be calculated by the following equation: where (u 1(k) , u 2(k) ) is the control at time k. Now we change the robot model to (24) which are suitable for (9) with (19).

Viability Constraints
We observe that (19) is the normal vector of viability boundary in state space (24) states. According to (9), the dot product of (24) and (19) should be less than 0. Theorem 2 means when robot states are on the boundary (18), there should exist a control which can make robot states' velocities be included in the boundary tangent cone for viability. This is the relationship between robot and environment described by viability.
Considering robot model (25), the steering control (u 1 − u 2 ) needs time to affect the angular velocityφ t . In order to introduce the steering control into inequalities (9), Speed Optimization Control for Wheeled Robot Navigation . . . Lei Liu, Yan Gao, Yuepeng Wu a short operation time t < T sample is introduced. According to (25), the angular velocity of time (k + t) iṡ Replacingφ t(k) byφ t(k+ t) in viability condition (9), we have The steering control (u 1 − u 2 ) is introduced to inequalities (28) byφ t(k+ t) . When the robot state is on or beyond the viability boundary (18), the solution control of the inequalities (28) can make the robot in the viability boundary (18) in a short time t. Introducingφ t(k+ t) instead ofφ t(k) into (28) has a error with upper bound. Shorter the operation time t is, smaller this error is.
Since t is short, the orientation angle at time k + t is φ t(k+ t) ≈ φ t(k) +φ t(k+ t) t ≈ 0, and sin(φ t(k+ t) ) ≈ φ t(k+ t) ≈φ t(k+ t) t, cos(φ t(k+ t) ) ≈ 1. The derivative of robot states for viability control becomes tset √ a 2 +1 sin(φ t(k) ) in ϕ n(k) of (28) also need to use the gradient at time k + t. Thus, the gradient of ϕ n at time k is reformulated as Substituting both (29) and (30) into (28), the viability condition is reformulated as The first term in (31) represents the tendency of the robot running out of the viability boundary (18) in X direction. The second term represents the Y direction tendency. The third term supplies negative value for the inequalities by using steering control (u 1(k) − u 2(k) ). The fourth term supplies negative value by decreasing (u 1(k) + u 2(k) ) for deceleration. Except variables (u 1(k) , u 2(k) ), other parameters are all robot states which can be obtained by the sensor.
We expect the robot to keep a high speed while avoiding obstacles. robot need to achieve the goal at the end, so we should build an objective function for the goal. Building a new objective function for goal guidance and speed optimization is a challenge. Thus, we assign the derivativė v t(k) to 0. This means that the deceleration method for the viability control is omitted. The derivative of robot state for viability control at time k becomes The viability condition of (31) becomes: To satisfy the inequalities (33), we have to use steering control. In (31), a negative fourth term which means deceleration can help the system viable. When this term is set to be 0, the steering control (u 1(k) − u 2(k) ) need to be increased to enlarge the negative value of the third term in (31) for viability. When the robot encounters the viability boundary at a high speed, the solution of (33) would decelerate one motor to increase (u 1(k) −u 2(k) ) for turning. This will lead to decreasing the linear velocity. Therefor, the control (u 1(k) , u 2(k) ) for (33) will ensure the fourth term of (31) negative. This leads the robot to be viable. When the parameter t set is smaller, (u 1(k) −u 2(k) ) need to be larger enough to make the robot to go back to the viable region in t < T sample . Noticing that there have constraints of control in (12), the viability boundary parameter t set must be manually adjusted to suit for the constraints of control. Since the LADAR scanning period T sample is short, t would be small.
t is important, because it gives steering control (u 1(k) − u 2(k) ) a chance to affect φ t(k+ t) ≈ φ t(k) +φ t(k+ t) t in one control iteration. For nonholonomic constraint, the robot is not able to instantaneously move in the same direction with the line that passes through the axis of the driven wheels. As we mentioned above in trajectory frame at time k, φ t(k) = 0. That means the robot is not able to instantaneously move along X axis. But with the parameter t, steering control can make the robot moving along the X axis by φ t(k+ t) with limited error. Of course, the parameter t have a lower bound. A smaller t means the robot need a larger steering control to go back into the variable area in t. Since the real time controller would keep the control output unchanged in each control period which is equal to T sample , Speed Optimization Control for Wheeled Robot Navigation . . . Lei Liu, Yan Gao, Yuepeng Wu small t may lead to the robot vibration near the viability boundary. So the parameter t should be chosen properly by the performance of the control.

Optimization Model of Navigation Control
The navigation control of wheeled robots includes two types of behavior: obstacle avoidance and goal guidance. Viability condition (9) gives an admissible control range by inequalities for obstacle avoidance. We can design an optimization model for goal guidance. This change a navigation problem to an optimization problem. The optimization model for the robot navigation is designed as the following: (34) subject to motor control constraint (12), constraint (33) when state is out of (18), is the stable orientation angle velocity of the robot, the value of β is presented in (26). T turning denotes the turning time parameter for the robot turning toward the goal. Here we useφ s T turning to represent the expected direction of the robot orientation angle. Let θ be the target guiding angle and φ be the robot orientation angle presented in (10), the guidance error θ e ∈ [−π, π] would be (φ − θ). The objective function can be written as: We use T turning to adjust the intensity of goal guidance. Smaller turing is, faster the robot turns to the goal. But the overall speed of the robot navigation slows down. This value can be adjusted according to the distance between the robot and the goal. When the distance is far, a bigger value would be used in order to ensure a high speed of the robot. When the goal is near, a smaller value would be used to ensure the robot guiding towards the goal. We also monitor the value of polynomial (31) using the optimized control (u 1 , u 2 ) obtained from (34). A positive value of polynomial (31) means only steering control can't ensure the safety of robot, wheel breaking system should also be used.
The motivation of this work is to increase the speed of the robot navigation, because it is important for improving the efficiency of the robot transportion. That is why optimization model (34) only optimize the steering control. When the robot approach the goal, it can use other mature navigation method, such as VFH+, for deceleration and stopping at the goal.
If the robot states encounter a viability boundary, we set the farther end point of the front security line as the subgoal which information is used for (34). When the robot is obstacle free, we directly use the original goal information for (34). This can effectively avoid local minima.

Simulation of Robot Navigation with LADAR
Based on the data process method of LADAR presented in section IV, the useful information of raw LADAR data is not only the position, but also the tendency of obstacles. So we give a more complex environment to test the proposed method. The environment for simulation is shown in Fig. 6(a). Viability control avoids the obstacle at 1 , 2 , 3 , 4 , 5 , and finally achieve the goal drawn as the yellow dot. We use our IEPF method to process the LADAR data. The optimization problem (34) is solved by Sequential Quadratic Programming(SQP).
The simulation environment is built by National Instrument company softwares: Labview, NI robotic module, NI control and simulation module and other related toolkits of Labview. The length and width of the simulation space is 20 meters, the diameter of the centered circular obstacle is 8 meters. Robot simulation parameters are listed as follows: mass of the robot m is 43.5 kg; inertia moment of rotation I is 1.16 kg·m 2 ; radius of wheels b is 0.0768 m; distance between two wheels 2w is 0.448 m; ratio of the gear box g is 10.6; induced electromotive force constant k is 0.2; the maximum peak control of the motor u max is 126. The LADAR start angle is −120 • ; end angle is 120 • . The simulated LADAR contains 240 scan lines with 30mm noise, LADAR sample period T sample is 0.1sec, this period equals to the period of viability control. We adjust t to 0.05 sec. The simulated controller can run SQP in real time. vs. time(sec) graph at the bottom. In the middle graph, the black line is the direction, the red line is the angular velocity. In the bottom graph, the black line is the control voltage of the right wheel, the red line is the control voltage of the left wheel.
The robot navigates in the simulation environment with a high speed which is at least 4 meters per second as shown in the Velocity vs. Time graph of Fig. 6(b). The black line of the Direction vs. Time graph shows the turning control of robot is smooth. For the Control vs. Time graph, at least one motor is controlled by a voltage that is slightly lower than or equal to u max in the navigation process. This means that there is always a motor keep running at the full speed during obstacle avoidance. The other motor is controlled by SQP method based on viability control and realizes the corresponding steering behavior for obstacle avoidance. The curves in Control vs. Time graph sometimes are sharp, because the LADAR simulation has noise and security lines often change with robot moving. These facts and discretization error disturb the smooth output of the controller. In Fig. 6, robot turns left to avoid non-smooth convex obstacle at 1 and turns left to avoid non-linear convex circular obstacle at 2 . Robot detours around the circular obstacle at 3 . The concave non-smooth object is placed at 4 . Finally the robot avoids the thin barrier and achieves the goal at 5 . The robot LADAR data process at places of 1 , 2 , 4 , 5 is shown in Fig. 7. The elements meaning of Fig. 7 is similar with that of Fig. 5. Only a red thin line indicating the goal direction is added. Data process at 1 is shown in Fig. 7(a). The square obstacle is transformed to blue line segments to represent non-smooth convex obstacles; The data process of the circular obstacle is shown in Fig. 7(b). Here we use three outward blue line segments to represent the original circular obstacle. The non-smooth concave obstacles data process at 4 is shown in Fig. 7(c). We use line segment connecting two breakpoints instead of the raw data. Using this method,useless and trivial information from LADAR can be filtered. The circumstance near the end of navigation is shown in Fig. 7(d).

CONCLUSION
We propose a new navigation method for wheeled robot with LADAR in an unknown environment based on viability. Viability provides a precise description of the relationship between a dynamic system and the environment. When guaranteing viability conditions, the robot can barely reached the maximum speed with an optimized control. Viability gives us a guidance on how to deal with the sensor information for a dynamic system. Our LADAR data process presented in section IV conforms with the viability condition requirements. Based on viability, the boundary of dynamic system should have the same dimensions as state space of the robot. So we raise the dimension of original position boundary of security line segments by defining a time parameter t set which introduces velocity into the designed boundary for viability control. By using parameter t, we solve the control problem of nonholonomic constraint of the robot. At last, we design an optimization model to find a more efficient control for the goal by omitting the deceleration item of the viability condition. All these works constitute a framework shown as Fig. 2.
We find that there have some shortcoming with our method. When we discretize our controller, a discretization error exists disturbing the smooth outcome of the controller. In our work, we use small iteration time t sample to minimize this error. For the future work, we would develop a discretization method to eliminate this error. Other improvement would be on the LADAR data process. We only process LADAR data for necessary information required by viability condition, so breakpoints often abruptly emerge when robot moves, and a new security line segment suddenly appears. This leads the controller gives out a sharp control signal. We would find a more smooth data process method for this problem in our future work.
Compared with classical navigation methods, our method needs smaller computation resources and is more efficient in data processing. Viability control serves as a basic theory for wheeled robot navigation. Theoretically, Speed Optimization Control for Wheeled Robot Navigation . . . Lei Liu, Yan Gao, Yuepeng Wu other kinds of robots may also enjoy the benefit of viability in improving the performance of navigation.