Variable Robot Geometry Optimization Method to Avoid Tip Over Situations During Slow Motion on Unknown Terrains

This paper presents a parametrized stability control method for special slow motion field mobile robots, based on use cases from border surveillance. The concept uses the centre of gravity (COG) as the virtual centre of motion (VCM). The simplified robot geometry is an input parameter of the model, so it can work with different types of mobile robots, like holonomic-wheeled, differential-wheeled, steered, tracked, wheeled-tracked, segmented, etc. structures. This method resulted in the implementation of a flexible and universal control algorithm for transformable and hybrid drive mobile robots, where every parameter can be changed and recalculated for different applications or even in discrete time steps during the motion at a 3D path. The velocity reference, the angular velocity reference and the optimization parameter (for example gravity compensation) of the robot can be prescribed. The model was implemented in MATLAB and can be compiled to C for measurements and validation with test robots.


Introduction
In most of the cases concerning mobile robot control theory the only implemented algorithm is robot kinematics. These algorithms do not take into consideration the inertias and the masses of the robot (robot dynamics). This method is parallel with industrial robot control methods. At linearly independent joints (for example an XYZ Cartesian TTT organized 3 DOF milling machine) the usual control method is the implementation of inverse kinematics (decentralized control method). [1] [2] With decentralized control method the masses and inertias of the segments do not have significant effects on each other (they have effects just on the position and the orientation of TCP). (They are linearly independent.) In the case of decentralized control theory, the system can be controlled without dynamical equations perfectly. With the centralized control method, we take into consideration the dynamical equations of motion, the interaction between the links, masses, and inertias. [3] [4]

Use Case
Use cases can serve as a good platform to assume the challenges and needs as well as to present the necessity of discussed solutions. In this article the use case is border surveillance. Border surveillance is defined by the Schengen Border Code. Besides protecting the external borders of the Schengen Area, an area almost overlapping with the territory of the European Union, it also has a traditional key role in protection of the national security interests of the country, including fight against terrorism. [5] Border surveillance means "the surveillance of borders between border crossing points and the surveillance of border crossing points outside the fixed opening hours, in order to prevent persons from circumventing border checks." [6: 16] This means border surveillance has three key aspects: • solutions to supervise border sections between border crossing points (at land [including rivers and lakes] as well as at sea); • solutions to supervise border crossing points (border gates) outside opening hours (when they are usually unmanned); • movement control capabilities in order to prevent persons from circumventing border checks. In the last year, a vast number of persons attempted to cross the Schengen external border into Hungary illegally. There were 10,046 persons arrested on the peak day in 2015, 430,607 persons were caught crossing the border illegally in total in 2015. For comparison, the total number in 2014, was 61,664 (according to statistical reports of the Hungarian National Police -published on [40].) Detection itself did not help authorities who faced a huge challenge only because of the sheer numbers. Moreover, if the police stopped them, they attacked the border gates (Röszke, September 16, 2015), but were driven off with tear gas and water cannons. First in realizing the nature of the current migration trend, meaning that the masses of migrants may start a riot at any time, Hungary erected a border fence along its southern border. Additional large waves are expected later in 2016 and some signs show that attempts are becoming more and more aggressive (on February 19, 2016, an individual pointed a gun at the patrols, threatening them with shooting, on the same day a car broke through and tried to run down the officers trying to stop it). Every day, smaller or larger groups try to cut their way through the border fence. This means, that the border surveillance has to be able to cope with such mass breakthrough attempts. To prevent injury to officers and spare manpower for critical events, the Hungarian National Police launched a series of research and innovation actions, aimed at developing autonomous border surveillance capabilities. One of the possible solutions is the authorization of ground robot patrols in the form of slow moving ground robots equipped with sensor arrays, radio and extended power sources with hot-swap function. Some sensors shall be able to capture multi-modal biometric identifiers (e.g. motion picture with movement patterns in normal and infrared [IR] light), enabling later identification of trespassers. [7] Infrastructure close to the border fence is slightly underdeveloped, as it had to be erected hastily in one month, especially manoeuvre roads are in bad condition on rainy days, tents and mobile toilets were hastily deployed to cover the patrol lines along the border fence. Currently, power lines are under construction at several border sections which can serve as a solid base infrastructure for ground surveillance robots covering areas which cannot be covered by power lines and static surveillance equipment. Therefore, control methods for ground robots applicable under such conditions had to be investigated.

Background
Several types of mobile robots exist, such as tracked, wheeled, legged, wheeled-legged, legwheeled, segmented, climbing or hopping. Transformable mobile robots proved that they can be much more effective on terrain, than the single link ones, and can overcome obstacles 2-3 times their wheel diameter. [8] [9] [10] The control methods of these robots are implemented individually for every construction. In most of the cases the control algorithms uses the virtual centre of motion method and the path of the motion describes the path of this point. [8] [11] Usually the centre of gravity is not exactly the same with the virtual centre of motion. Mostly the virtual centre of motion is the geometrical centre of the structure. [8] At the design of the robot mechanics the geometry of the robot can be optimized for the expected terrain. [8] [9] [11] In some cases the control method is also optimized for the surface of the ground. [8] [9] The design and control of a robot for an unexplored terrain requires a different approach. [13] [14] Transformable mobile robots are designed for motion in special environments. With the variable geometry, the structure can optimize its shape to overcome the current obstacle. For example, after the attack of the World Trade Center these types of mobile robots were used in searching for victims. These machines can work in hazardous environments without any risk to human life. [11] [15] The robots can be smaller than dogs and go deeper in small tunnels. The most significant barriers are stairs, stones, and pits. [14] The key factor of this movement is the stability of the robot. In case of an impossible path a new route can be designed, but in case of a tip over situation the mission is unsuccessful. The main parameter of the stability is the effect of gravity. (In case of a relatively slow motion robot the dynamics can be neglected.) Variable transformation shapes give opportunity for the compensation of the gravity effect. [16] The low-level control algorithms are implemented in each structure individually. [17][18][19][20][21][22][23][24][25][26] For the high-level robot action control we can find general structures, and path controller algorithms. [22] [25] [27] The path of the robot motion is also different for example at a differential drive and a holonomic drive. [25] The low-level control algorithm (kinematics) of a transformable and a non-transformable differential drive is also different. At the transformable version of the differential drive the shape of the structure is continuously under optimization during motion. [27] [28] [29] [30] [31] The aim of the paper is to describe a parameterized low level mobile robot control algorithm for slow motion on unknown terrain. [32] [33]

Model Description
Simplifying the robot geometry, we can find a way to define the required parameters for the calculation of inverse kinematics generally. Consider the mass of the robot as a point (centre of gravity). This point is the centre of the robot coordinate system. Draw vectors from the centre of the coordinate system to the contact points between the floor and the wheels. The model can calculate with constant and variable coordinates also. For example, from a holonomic type robot model ( Figure 1) we can sketch a simplified structure ( Figure 2). Of course, the geometrical transformation can change the centre of gravity relatively to the wheels. In this case, all of the structure can be recalculated but this effect is usually negligible. (With future development a segmented robot with more concentrated masses will be defined. [34] [35]) A contact point can be geometrical and constrains perpendicular to the plane of the floor (contact plane), and at a direction in parallel with the contact plane. These parameters also can be variables or constants. (For example, in radio control [RC] cars the steering angle, modifies the direction of the geometrical constrains at the contact points of the steered wheels.) Additional input parameters are the mass of the robot, the gravitational vector and the coefficient of adhesion friction. With this parameter description, we can calculate the normal forces at the contact points and the components of the acceleration (mainly the mass acceleration). This will result in a simple statically model that can be optimized in real-time during motion.
The path planning algorithm defines the velocity and the angular velocity of the COG(s) or centre of a multi robot system. [36] With these data and the parameters mentioned above we can calculate the angular velocities of the wheels (inverse kinematics). With the definition of the acceleration and the angular acceleration of the robot we can calculate the angular velocities of the wheels at discrete time steps. During the control of transformable mobile robots, we can prescribe optimums, maximal, or minimal values of robot parameters, like maximal acceleration utilization, maximal velocity utilization, stability optimum, energy consumption minimum, etc. (Figure 3) This way we can also make sure the robot is going to move slowly in case of a critical tip-over situation to avoid robot damage caused by high motional energy (and we can also neglect dynamic effects of robot motion). [Edited by the authors.] Figure 2. The rendered image of the holonomic robot base with the robot coordinate system, the centre of gravity, and the vectors of the contact points.
[Edited by the authors.]

Calculation Method
The implemented model can be divided into two main parts: basic calculations and optimizations. These can be also divided into different loops and iterations. At the end of the development the code must run on an embedded system or on an embedded computer. [37] The calculation throughput these hardware elements is limited, so the code most be speed optimized. [39] The program details have different priorities. The inverse kinematics has to run on the highest frequency in discrete time. The tangent plane (orientation of ground relatively to the robot) calculations, the normal forces and gravitational force calculations, the geometry transformation, or the stability optimization can run on different priorities and frequencies.
(For example, in case of a constant acceleration vector the recalculation of the tangent plane function is unnecessary. It means the robot is moving on the same oriented flat terrain for a long time.) The time constants of the application define these priorities, frequencies, and the minimal calculation throughput.

Basic calculations
This part of the code requires the input parameters. These parameters can be given in the same frame for different robots. The values of the frames are constants, but the constants can be calculated from any type of MATLAB compatible functions. (For example, the inverse kinematics of multi DOF legs or arms. [38] [39]) The required data of the robot are the followings in robot coordinate system: • vector of the acceleration (provided by a 3-axis accelerometer); • coordinates of the contact points (for example the output of the robot leg's direct geometry functions); • direction of the geometrical constrains and the drives (direction of the steered wheels); • mass of the robot; • value of the adhesion friction; • transformation matrix between the robot coordinate system and the world coordinate system (provided by navigation module); • velocity and angular velocity references of the robot (provided by path planner); • values of the acceleration and the angular acceleration of the robot (provided by path planner). Every parameter is in robot coordinate system (except the references). (The references are usually given in world coordinate system and transferred to robot coordinates.) The program generates the simplified 3D model of the robot from the input parameters. It contains the acceleration vector, the components of the acceleration vector, the concentrated mass, the vector of the contact points, the tangent plane and the normal forces. (Figure 4) The method is implemented in 3D for contact points (e.g. wheels), location and positive integer. As it was mentioned above in most of the cases the acceleration of the robot from the drives is negligible in the ratio of the gravity effect. The input parameters are dimensionless ratios.
(Of course, these parameters can be easily changed to real dimensions.) The first part of the code generates the 3D model, the tangent plane, and the force components. The output of this process is a 3-dimensional free body diagram. The program calculates the distances between the gravitational vector and the contact points. Every distance is projected to the XY plane.
With these distances, we can define a first iteration ratio for the normal forces at the contact points. (Normal forces are between the wheels and the ground.) The program selects three points to find the ground plane in robot coordinate system. Three points define a 3D plane.
Equation (1), where x,y,z are the coordinates of a general point on the plane, x 0 ,y 0 ,z 0 are the coordinates of a wheel contact point, and a,b,c are the coordinates of the normal vector of the plane (can be expressed from three points of the plane).
These three points can be selected from any of the points with different combinations. The program calculates the maximal number of these combinations, where k max in Equation (2) is the maximal number of the combination (and the number of the iterations), and n is the number of the contact points.
For the test of the first iteration, (the validation of the plane) we have to calculate, that the other points are lies on the plane, over the plane, or under the plane from the Equation (1). (I.e. point error.) This step also helps us in case of multiple wheel robots to figure out which 3 wheels are defining a plane. In case the gravitational vector goes through the plane the robot is standing on those three wheels and we can get estimation about the rest of the wheels.
Equation (3) can be expressed in coordinate form like Equations (4)- (6), where d gx ,d gy ,d gz are the coordinates of the intersection point, (D g ), g x ,g y ,g z are the coordinates of the gravitational vector, t 1 [Equation (7)] is a real parameter (t 1 ∈ R) and x 1 ,y 1 ,z 1 are the coordinates of the main points from the plane (so as x 2 ,y 2 ,z 2 and x 3 ,y 3 ,z 3 ).
= (1 − 1 ) In case of a calculation error related to the coordinates of the wheel (x 1 ,y 1 ,z 1 ) the program creates a new combination with a different wheel. In case of a calculation problem with the intersection point (D g ) the program also creates a new combination of wheels to find the three wheels where the robot lies on the ground. The iteration runs maximally once with every combination (n times). These attempts are running in probability sequences. At the first successful case the next step will be the calculation of the gravitational components (force perpendicular to the ground plane and force parallel to the ground plane) and the normal forces (forces between the wheels and the ground plane). The normal component of the gravitational The parallel component of the gravitational vector will be parallel with a direction vector calculated from the D c D g vector. The sum of the normal vectors at the contact points results the normal component of the gravity. From the distances between intersection point of the gravitational vector and the contact points we can define the ratios for the calculation of the normal vectors in Equation (12), where L is the ratio number, n a is the number of the points on the plane (some of the wheels can be over the plane), N g is the normal component of the gravitational vector and |F ni | is the absolute value of normal vector of the i th contact point.
As further development, the inverse kinematics module is under implementation. The program categorizes the structure: • 3 DoF mobile robots: -steered, -non-steered. • 2 DoF mobile robots: steered, -non-steered. A mobile robot usually has two or three DOF on the ground plane. The wheels of the robot can be steered or non-steered.

Optimizations
The firstly implemented optimization of the model is the 3D gravity compensation method. At transformable mobile robots, the geometry can be optimized for the stability of the robot.
A rigid body rolls over when the impact line of the gravitational acceleration is outside of the contact surface. The static torque of the body calculated to the centre of the coordinate system (S MO ) can be expressed as Equation (13), where m is the mass and r is the vector of the elementary points of the mass.

= ∫
The vector of the centre of the body (r s ) can be expressed from Equation (4.13) as Equation (14).
The vector between the centre of the mass and the elementary points (R) can be expressed as Equation (15).
The static torque of the body calculated to the centre of the rigid body (S MS ) can be expressed as Equation (16).
The connection between the torque and the static torque can be expressed as Equation (17), where M A is the torque, calculated to the point A and g is the gravitational vector.
The torque of the force in the point P (F P ) at point B (M B ) can be expressed as Equation (19), where r BP is the vector between point P and B.
If the distance between the intersection point and centre of the polygon (r SDg ) is zero [Equation (20)], the continuance of the g intersects the plane at the center of the polygon. This case results normal distribution of the robot mass on each of the wheels.
= 0 (20) In this case the torque of the gravitational force is zero, so the stability of the robot is maximal. (See Equation 21.) The other key factor of the robot stability is the height of the structure.
The wheels of the robot define a polygon on the ground plane. (Figure 5) The centre of the polygon is the average of all points of the polygon. The polygon can be divided to triangles. The centre of the polygon can be calculated from the centres of the triangles. The centre of a triangle projected to the XY plane can be calculated from the coordinates with the Equations (22) and (23), where s 1x ,s 1y are the coordinates of the center of the first triangle.
If we project all of the coordinates to the XY plane we can calculate the area of the triangles in 2D with Equation (24), where T 1 is the area of the first triangle. From the centres and the areas of the triangles the centre of the polygon can be calculated in 2D with area weighted average with Equations (25) and (26), where s x ,s y are the x and y coordinates of the centre of the polygon.
The z coordinate of the point can be expressed from the plane Equation (1) as Equation (27) form, where s z is the z coordinate of the center of the polygon.
The distance between the centre of the coordinate system and the ground plane (the height of the robot structure) can be expressed as Equation (28), where |OC| is the height.
The distance between the centre of the polygon and the point, where the continuance of the gravitational vector intersects the plane (i.e. stability factor) can be expressed as Equation (29), where |SD g | is the "stability factor".

Implementation of the control method
The block diagram of the control method can be seen on Figure 6.  The variables in the block diagram are the followings: • T g is the transformation matrix of the gravitational vector (when the stability factor is under optimization the change of the gravitational vector must be also simulated); • ϕ,χ,ψ,… are the variables of T g ; •  ̂, ,̂, ,̂, , … are the estimated parameters of the contact points (estimated by the stability optimization, indicated with ξ); •  ± , is the range of the estimated parameters, where the parameters are acceptable; •  ̂, ,̂, ,̂, , … are the estimated parameters of the contact points (estimated by the kinematics optimization, indicated with λ); •  ,̇,̈ are the angular position, the angular velocity and the angular acceleration references of the wheels;

Simulation results
The first simulation of the results is the MATLAB simulation of a basic three wheeled transformable robot geometry. (Figure 7) The position of the P 1 point is variable with the change of α parameter. (Can be tuned between α A to α B .) The distances between the wheels and the centre of gravity are equal at every wheel. Between the first two wheels and the XY plane there is 30.964°. In case of α = 30.964° (α opt. ) and with a horizontal plane |SD g | = 0. [Edited by the authors.] Assuming a horizontal plane the blue function on (Figure 8) is |OC|(α), and the red one is |SD g |(α). As we can see on |OC|(α) the increase of α increases the height of the robot. The red |SD g |(α) function is more important. It has a minimum value at α ≃ 31°. It means that simulation could find the value of α to get back the original optimal structure of the robot. The implementation generates different |SD g |(α i ,γ i ,ϑ i ,…) functions. During the function generation, the program makes logical decisions so the |SD g |(α i ,γ i ,ϑ i ,…) function cannot contain symbolic variables. The second simulation made with the same geometry, but in this case two of the parameters where variables (α,γ). (Figure 9) In case of α = γ = 30.964° (α opt. ,γ opt. ) and a horizontal plane |SD g | = 0 (the maximum of the stability). [Edited by the authors.] In this case three methods were programmed and tested for the calculation of the optimal α and γ values. For comparison: at the worst case in case of |SD g |(α,γ) > 10 the robot tips over.
Of course, on a ramp less stability can be reached, than in case of a flat and horizontal ground. At the first method, the program calculates the |SD g |(α,γ) values at 2,500 times. (50 times α and (multiplied by) 50 times γ) As we can see on ( Figure 8) and ( Figure 9) the |SD g |(α,γ) function has a minimal value at α = γ = 31°. In this case |SD g |(α,γ) = 0.0022. This value is only the 0.022% of the tip over situation. This method could provide the expected results around the original optimal α,γ values. The disadvantage of this method is that the calculation of these points at a normal PC is more than an hour, because of the 2,500 iteration steps. As an advantage of the method it did not use symbolic variables in the |SD g |(α,γ) function and we can visualize the whole "stability map" for different gravitational vectors. At an embedded system, a look up table can contain the values of these maps, although it requires an additional flash memory to store the data points. [Edited by the authors.] The second method is the use of the "fminsearch" MATLAB function (so it solves the min , | |( , ) problem). The results have been the followings: α = 30.9628; γ = 30.9605 and |SD g |(α,γ) = 0.00009092. This value is the 0.0009092% of the tip over situation. It is an advantage; that MATLAB could solve this problem in less than a second with 49 iteration steps. The problem with this method is that α and γ where symbolic variables at the program. At a more difficult case the symbolized equation cannot be generated by the MATLAB implementation, and it could not handle the logical decisions even in this simple case.
The third method is the use of a simple search algorithm in 3 steps. At the first search the program calculates |SD g |(α,γ) values at 25 times. (5 times α and (multiplied by) 5 times γ) This iteration is resulted the followings: α = 30; γ = 30 and |SD g |(α,γ) = 0.0821. This value is the 0.821% of the tip over situation. (Figure 12) [Edited by the authors.] At the following search the program refines the mesh around the previous minimal point. (Figure 12, 1 and 11) The second step required 50 iterations (25 for the first and 25 for the second step) The results are the followings: α = 32; γ = 32 and |SD g |(α,γ) = 0.0566. This value is the 0.566% of the tip over situation. (Figure 13) The third step required 75 iterations (25 for the first, 25 for the second and 25 for the third) The results are the followings: α = 31.2; γ = 31.2 and |SD g |(α,γ) = 0.0131. This value is the 0.131% of the tip over situation. (Figure 14) After 150 iteration (6×25) the algorithm could find a solution, where |SD g |(α,γ) = 0.00037889. This value is the 0.0037889% of the tip over situation.    This method worked without symbolic variables and could run in less than a second. The main disadvantage of the algorithm is that it can find a local minimum value instead of the optimal solution. This problem can be solved with further developments. At the next simulation, MATLAB simulated the robot on a rough terrain. The 3D path on the terrain can be described as Equations (30)- (33), where r(s) is the distance function of the 3D path and the gravitational vector and can be described as Equation (34).
During the 3D path simulation MATLAB worked with the implemented searching algorithm to estimate the optimal geometrical structure arrangement.
At the first test robot performed a slow motion on the path without any stability control methods. The simulation calculated the number of tip over situations at 100 points (with normal distribution of the path) and it resulted, that the robot would tip over on the 72% of the path. (Figure 16, where the red parts mark the tip over on the path.) At the next step the robot performed a slow motion with the stability control method, where the phase shift of the discrete system was implemented, so the control algorithm used the (i -1) th gravitational vector to control the robot at the i th point. This simulation resulted that the robot would tip over only on the 22% of the path. (Figure 17, where the red parts mark the rollover on the path.) The robot had the same orientation along the path so the robot could not turn its variable wheels towards an obstacle. Changing the robot orientation can also improve this number.

Conclusion
This paper presented a universal mobile robot control method for transformable and hybrid drive mobile robots performing slow motion on unknown terrains, inspired by use case of border surveillance. The basic calculations and the stability optimization method could provide the expected results at the MATLAB simulations. We designed two ethologically inspired holonomic mobile robots called "Ethon" (Picture 3) and these were the bases of the validation. Unfortunately, these robots are having fix geometry parameters so we could validate the first simulation only with a 3-axis accelerometer and they were not tested in live environment (at the border) yet.