Shrimp

1 Mô tả hệ thống The Autonomous System Laboratory developed an off-road rover called Shrimp which shows good climbing capabilities – Phòng thí nghiệm hệ thống tự động đã phát triển 1 loại xe vượt chướng ngại vật với khả năng leo dốc tốt với tên gọi là Shrimp . Its passive and non-hyperstatic structure can adapt to a large range of obstacles and therefore allows to limit wheel slip (see Fig. 1). - Nó bị động và có cấu trúc linh hoạt có thể thích nghi với các chướng ngại vật lớn , do đó giúp nó có thể leo cầu thang It has six motorized wheels and is composed of four main parts: the body (on which the rear wheel is attached), the articulated front fork and the two side bogies.– Nó có 6 bánh gắn động cơ và cấu tạo gồm 4 phần chính : Phần thân (trên đó gắn bánh sau) , bánh trước và 2 nhóm bánh 2 bên A detailed description of the mechanical design can be found in [1]. Hình 1 là kết cấu cơ khí cụ thể của nó The control architecture together with an overview of the rover and the experimental setup are depicted in Fig. 1. Cấu trúc điều khiển ,hình dạng cơ bản của nó và mô hình thí nghiệm được giới thiệu ở hình 1 A lightweight laptop clocked at 660 MHz controls the whole system via three communication channels: two RS232 serial ports (com1 and com2) and one IEEE 1394 bus. 1 laptop nhỏ dùng sóng 660mhz để điều khiển toàn bộ hệ thống thông qua 3 kênh giao tiếp : 2 cổng nối tiếp RS232 (com1 và com2) và 1 bus IEEE1394 One can also exchange information with a host computer through an Ethernet link. Mỗi cổng đều có thể trao đổi thông tin với máy tính chủ thông qua 1 đường Ethernet. The first serial link is used to interface an I2C bus on which different slave modules developed at ASL are attached. Cổng nối tiếp đầu tiên được dùng để ghép nối với 1 bus I2C có gắn nhiều modul trạm khác nhau phát triển ở ASL The robot has a motor controller module for each wheel, one servo controller for the steering of the front and back wheel and one angle sensor module for sensing the state of the bogies and the front fork relative to the body. Robot có modul điều khiển động cơ cho từng bánh , 1 điều khiển servo điều khiển lái bánh trước và bánh sau và 1 modul cảm biến đo góc để xác định trạng tháí giữa cặp bánh bên và bánh trước với thân robot The modules dedicated to the motors provide speed, torque and position control. Nhiều modul được sử dụng cho động cơ cung cấp tốc độ, momen xoay và điều khiển vị trí

2. Odometry in 3D Odometry trong môi trường 3D Although odometry is widely used for indoor (2D), its application is limited for rough terrain (3D).Mặc dù odometry được sử dụng rộng rãi trong mặt phẳng (2D), những ứng dụng của nó cho môi trường thực (3D)còn hạn chế A new method, called 3D-Odometry, which extends the standard 2D odometry to the 3D space will be presented. 1 phương pháp mới có tên là Odometry mở rộng các tiêu chuẩn của Odometry 2D sang không gian 3D đã được giới thiệu Since it accounts for transitions, the 3D-Odometry provides better position estimates. Được phát triển cho việc chuyển tiếp , Odometry 3D cung cấp khả năng xác dịnh vị trí tốt hơn 2.1 Theory Nguyên lý The 2D odometry equations are well known and allow estimating the position and the orientation of the robot (x, y, q) in the plane. Phương trình Odometry 2D cho phép xác định vị trí và phương hướng của robot (x,y,q) trong mặt phẳng When the robot has to deal with ground slope changes, it is important to track the z coordinate also. Khi robot phải làm việc trong khu vực có độ cao thay đổi , việc xác định toạ độ cũng rất quan trọng The usual way to solve this issue is to add an inclinometer on the robot. Cách thường dùng để giải quyết vấn đề này là lắp thêm sensor độ cao lên robot This sensor provides the roll and pitch of the robot relative to gravity and allows to know the orientation of the plane on which the robot is currently moving. Sensor này cung cấp con lăn và bước răng của robotliên hệvới trọng lực và cho phép nhận biết hướng của bề mặt robot đang dịch chuyển Using this information and the standard 2D odometry equations it is possible to compute the 3D coordinates of the robot. Sử dụng số liệu này và phương trình cân bằng của Odometry 2D có thể xác định được hướng 3D của robot This method, wich will be refered later as the standard method, works well under the assumption that the ground is relatively smooth and doesn’t have too many slope discontinuities. Phương pháp được coi là phương pháp tiêu chuẩn , thuận lợi cho tính toán với giả thuyết môi trường làm việc tương đối nhẵn bóng và không có nhiều nhấp nhô không liên tục Indeed, the system accumulates errors during transitions because of the planar assumption. Thật vậy , hệ thống sẽ tích luỹ sai số trong suốt quá trình chuyển đổi do giả thuyết mặt phẳng .

In rough terrain this assumption is not verified by definition and the transitions problem must be addressed properly. Trong môi trường không bằng phẳng giả thuyết này không được chấp nhận vì việc kiểm tra và chuyển đổi phải được xác định chính xác The main idea of the 3D-Odometry is to consider the displacement and the direction of motion of the wheels. Khái niệm chính của Odometry 3D là cân nhắc đén sự dịch chuyển và hướng chuyển động của bánh xe For the Shrimp we actually consider the translations of the left and right bogie. Đối với robot Shrimp chúng ta chủ yếu quan tâm đến chuyển động tịnh tiến của bogie bên trái và phải It is possible to compute the displacement ( and ) knowing the translation of the wheels (encoder data ER, EF) and the change of the bogie angle ( ) between the initial and final state (see Fig. 2 (a)).Có thể tính toán sự dịch chuyển ( và ) khi biết độ dịch chuyển của các bánh xe (thông số của các encoder ER , EF) và sự thay đổi góc của bogie ( ) giữa trạng thái ban đầu và kết thúc (xem hình 2(a)) Then a system of 9 equations with 9 unknowns can be solved for computing x0, y0 and z0 (see Fig. 2 (b)). Sau đó giải hệ 9 phưong trình với 9 ẩn số để xác định x0 , y0 và z0 (xem hình 2(b))

Hình 2 (a) ER , EF : dịch chuyển của bánh sau / bánh trước , độ lớn của vector RR’ và FF’ ρ,Φ : góc quay của bánh sau , bánh trước , hướng của sự dịch chuyển R,F : tâm của bánh sau / bánh trước lúc ban đầu R’, F’ : tâm của bánh sau / bánh trước lúc sau C , C’ : tâm của bogie lúc ban đầu / lúc sau ε : góc thay đổi của bogie (không cùng với sự thay đổi của góc trước) h : khoảng cách 2 tâm bánh δ : độ lớn của sự dịch chuyển , độ lớn của vector CC’ δxδy: hình chiếu lên trục x/ycủa vector CC μ : hướng của CC’ , góc C’CD’ γr : góc EDD’

Hình 2(b) b : khoảng cách giữa 2 bogie C , C’ : vị trí ban đầu / lúc sau của bogie bên phải L , L’ : vị trí ban đầu / lúc sau của bogie bên trái O , O’ : vị trí ban đầu / lúc sau của robot ηr , ηl : góc dịch chuyển bên phải / trái π π : mặt phẳng bên phải / trái nr , nl ; vector pháp tuyến của , song song với Oxz πb: mặt phẳng song song với Oxz và chứa C

ShrimpNav Robust position tracking in rough terrain Pierre Lamon

Introduction Up recently, autonomous mobile robots were mostly designed to work within an indoor, yet unstructured, environment. As a consequence, the problem of autonomous navigation has been faced, in most cases, by supposing the world as a flat surface: such assumption allowed researchers to develop navigation algorithms that consider the robot as a material point, and to consequently plan a path to the goal as a curve on a plane. When moving autonomous robots from indoor to outdoor environments many problems arise, especially if robots are requested to operate through unmapped, rough terrain for autonomous exploration tasks. Not only must the robot navigating in such environments avoid colliding with obstacles such as rocks, it must also avoid falling into a pit and avoid travel on terrain that would cause it to tip over. In an unknown environment the robot has to complete the following tasks in order to reach a given goal. Firstly, the robot has to acquire knowledge of the environment with it's onboard sensors. This information allow to identify obstacles and hazardous areas and to plan a safe path through the scene (obstacle avoidance and path planning tasks). Then the onboard controller has to find a sequence of actuators actions in order to follow the planned path (trajectory planning). Finally, while moving along the path the robot acquire new information of the scene and have to update a map (map building). To work properly all the latter tasks must have a good estimation of the robot's position and motion in the environment (localization). One can easily see that a robust position tracking is essential for autonomously navigate on an outdoor, unmapped and rough terrain. This is one of the main objective of the ShrimpNav project.

1. System description The Autonomous System Laboratory developed an off-road rover called Shrimp which shows good climbing capabilities. Its passive and non-hyperstatic structure can adapt to a large range of obstacles and therefore allows to limit wheel slip (see Fig. 1). It has six motorized wheels and is composed of four main parts: the body (on which the rear wheel is attached), the articulated front fork and the two side bogies. A detailed description of the mechanical design can be found in [1]. The control architecture together with an overview of the rover and the experimental setup are depicted in Fig. 1. A lightweight laptop clocked at 660 MHz controls the whole system via three communication channels: two RS232 serial ports (com1 and com2) and one IEEE 1394 bus. One can also exchange information with a host computer through an Ethernet link. The first serial link is used to interface an I2C bus on which different slave modules developed at ASL are attached. The robot has a motor controller module for each wheel, one servo controller for the steering of the front and back wheel and one angle sensor module for sensing the state of the bogies and the front fork relative to the body. The modules dedicated to the motors provide speed, torque and position control.

Figure 1: Sensors configuration, control architecture and experimental setup

2. Odometry in 3D Although odometry is widely used for indoor (2D), its application is limited for rough terrain (3D). A new method, called 3D-Odometry, which extends the standard 2D odometry to the 3D space will be presented. Since it accounts for transitions, the 3D-Odometry provides better position estimates.

2.1 Theory

The 2D odometry equations are well known and allow estimating the position and the orientation of the robot (x, y, q) in the plane. When the robot has to deal with ground slope changes, it is important to track the z coordinate also. The usual way to solve this issue is to add an inclinometer on the robot. This sensor provides the roll and pitch of the robot relative to gravity and allows to know the orientation of the plane on which the robot is currently moving. Using this information and the standard 2D odometry equations it is possible to compute the 3D coordinates of the robot. This method, wich will be refered later as the standard method, works well under the assumption that the ground is relatively smooth and doesn’t have too many slope discontinuities. Indeed, the system accumulates errors during transitions because of the planar assumption. In rough terrain this assumption is not verified by definition and the transitions problem must be addressed properly. The main idea of the 3D-Odometry is to consider the displacement and the direction of motion of the wheels. For the Shrimp we actually consider the translations of the left and right bogie. It is possible to compute the displacement (

and

) knowing the translation of the wheels (encoder data ER, EF) and the change of the

bogie angle ( ) between the initial and final state (see Fig. 2 (a)). Then a system of 9 equations with 9 unknowns can be solved for computing x0, y0 and z0 (see Fig. 2 (b)).

(a) Bogie displacement estimation

(b) 3D displacement estimation

Figure 2: 3D-Odometry main figures and variables definitions 2.2 Experimental results In order to test the equations presented above we drove the robot forward and computed the trajectory online with both our method and the standard method. The system has been tested in two different situations depicted in Fig. 3 (a) and (b) respectively. The true trajectory is an approximation. It is build with characteristic positions that can be easily computed knowing the shape of the obstacle, the geometrical model and the state of the robot. The x and z coordinates of the robot’s final position have been measured for every run. We did several runs for each experiment and computed the relative error. Fig. 3 shows the results corresponding to both experiments. One can see that the 3D-Odometry demonstrates much better performance. The sharper the transitions, the better it does in comparison with the standard method.

(a)

The figure shows three curves. In blue, the true trajectory, in green the trajectory computed with the 3DOdometry and in red the trajectory computed with the standard 3D odometry method.

(b)

The sharper the transitions, the better the 3D-Odometry does in comparison with the standard method

Figure 3: Experimental results The detailed description of this work and the full mathematical derivations can be found in [2]. Click here to see a movie of a full 3D experiment. You can also download the 3D scene of the experiment (ShrimpBest.iv). If you don't have an .iv viewer you can use this simple viewer (viewer.zip).

3. Torque controller Shrimp can adapt to a large range of obstacles and therefore can move smoothly across rough terrain. This behavior allows limited wheel slip. Nevertheless, further effort has to be done at the controller level. A physical model of the robot must be developed in order to optimally control the torques on the wheels. So that, with this model, the knowledge of the rover state and the contact points to the ground, an estimation of the best torque distribution between the six wheels can be done. Furthermore, a good knowledge of the wheel-ground interaction is also required in order to set a slip probability for each wheel. This will facilitate probabilistic sensor fusion between 3D-odometric information [2] and other sensors such as inertial measurement unit, laser scanner and vision.

3.1 The basic idea

A wheel of the robot is balanced if the friction force fulfils the inequation: (1) This case represents static friction. If the static friction force can't balance the system, the wheel slips and the friction force becomes:

(2) In order to avoid wheel slip, the friction force which depends directly from the motor torque M and thr wheel radius r, should satisfy the equation (1)

(3) The above equations suggest that there are two ways to reduce wheel slip. First, assume that µo is known and set:

(4) In fact, it is difficult to know µo precisely because it depends on the kind of wheel-soil interaction. During exploration, the kind of soil interacting with the wheels is changing and can’t be known, which makes µo impossible to pre-determine. Anti-lock break systems in automobiles sense slip and then compensate T until slip is not sensed anymore. But in this case slip has already occurred. Another way to avoid wheel slip is to first assume that the wheel does not slip. It is then possible to calculate the forces T and N as a function of the torque and the result is optimized in order to minimize the ratio T/N. Accounting for the previous assumption:

(5) µn is similar to a friction coefficient. In minimizing this ratio, then minimizing µn, we optimize our chances that this coeffcient is smaller than the real friction coefficient µo. If this is the truth, there is no slip. Therefore, it is possible to minimize the ratio without knowing the real static friction coefficient. The second method is used here, because it is more robust. However, determination of parameters T and N require a model of the mobile robot.

3.2 The quasi-static model For a 3D static model, 6 equations (3 torques and 3 forces) are applied to each body, containing ground reaction forces, gravity forces (weight) and external forces. The resulting model that is used for motion control also includes consideration of rolling resistance and wheel slip. Dynamic forces are considered to be negligible because the speed is low. Therefore, the model is referred to as quasi-static. For the model we assume that the wheel-ground contact angles are known. They can be computed while the robot is moving using the technics presented in [1]. An alternative method using tactical wheels can be found in [4]. It has the advantage to provide the contact points for static conditions also. The Shrimp has 18 parts and is characterised by 6×18 = 108 independent equations describing the static equilibium of each part and involving 14 external ground forces, 6 internal wheel torques and 93 internal forces and torques for a total of 113 unknowns. Of course, it is possible to reduce this set of independent equations because we have no interest in implicitly calculating the internal forces of the system. The variables of interest are the 3 ground contact forces on the front and the back wheel, the 2 ground contact forces on each wheel of the bogies and the 6 wheel torques. This makes 20 unknowns of interest and the system could be reduced to 20 - (113 - 108) = 15 equations. This leads to the following matrix equation:

(11) where M is the model matrix depending on the geometric parameters and the state of the robot, U a vector containing the unknown and R a constant vector.

3.3 Wheels Torque Optimization The controlable inputs of the system are the six wheel torques. Since there are five more unknowns than equations it is possible to write an equation expressing the torques as linearly dependant. The 14 other equations define the external forces as a function of the torques. The model of the Shrimp is indeterminate because there are less equations than variables and the set of solutions is of dimension five (number of wheels -1). The goal of the optimization is to minimize slip. This can be achieved by maximizing the traction forces, which is equivalent to minimizing the function

for the wheels. Figure 4b depicts an example of such a function for a simpler robot referred to as ThreeWheels. Being difficult to do reasoning in five dimensions, this figure will illustrate our optimization method.

(a)

(b)

Figure 4: (a)The ThreeWheels 2D model. This rover belongs to the passively suspended robots family. m4 is a noncontrollable torque generated by a torsion spring with known characteristics. (b) Solution space for the ThreeWheels rover f = max(Mu1, Mu2, Mu3). The functions Mu1, Mu2, Mu3 are hyperbolic and a linear optimisation process is not possible. Optimal solution (circled) minimizing slip and fulfilling the Ni > 0 constraint.

Since the system of equations is non linear, a numerical method is implemented. Our optimization method uses a combination of different algorithms and is depicted in Figure 5. Firstly the intitial solution is checked versus the following constraints: a. Motors saturation: the torques of the optimal solution must be smaller than the maximal availabe motor torque. b. Normal forces : the normal forces Ni must be greater than zero. If this solution is valid, it is taken as the initial solution for the Fixed Point optimization (A). If it doesn’t fulfill the constraints, a valid initial solution is computed using the Simplex Method (B). The optimal solution is then provided either by (A) or the Gradient optimization (C).

(a)

(b)

Figure 5: (a) Our optimization method. (b) The fixed point based algorithm. The quasi-static model (2) is solved with an initial set of torques (1). Block (3) computes an average friction coefficient based on the computed forces (output of module 2). The corresponding torques are computed (4) and fed again in module (2). Twenty iterations are generally sufficient for convergence. 3.4 Experimental results The optimization for the 3 dimensional Shrimp is similar to the method presented in the previous section. The solution space has now five dimensions and one has to account for 18 constraints (with i = 1 … 6).

An example of computed forces and torques is depicted in Figure 6. The execution times for the algorithms A, B and C are 6 ms, 5 ms and 20 ms respectively (on a 1.5 GHz processor).

Figure 6: Forces and torques computed by the optimization. The forces are expressed in the global frame of reference. The user of the interface can change the state of the robot and the contact angles of the wheels interactively. (a) Side view: the pitch, the front fork and the left bogie angles can be modified (b) Right bogie view: the angle of the right bogie can be modified on this view. (c) Decomposed view from rear: the roll angle can be changed on this view. The arrows are the projections of the reaction forces in the global frame of reference. The optimization algorithm has been tested for around 20 thousand states generated automatically considering different angles for each input parameter. 80% of the computed torques correspond to a friction coefficient smaller than 0.6 (a tyre on a dry road has a coefficient of 1). This will certainly allow to limit the slip of the wheels in most of the cases. The execution times for the algorithms A, B and C are 6 ms, 5 ms and 20 ms respectively (1.5 GHz processor). The worst case is about 31 ms. However, as it can be seen in Fig. 5, the majority of the states are handled in 6 ms. A movie of full 3D torque control simulation here

4. Inertial-Odometry sensor fusion Many algorithms related to localization need good pose prediction in order to produce accurate results. This is especially the case for data association algorithms, where false feature matches can lead to the localization system failure. In rough terrain, the field of view can vary significantly between two feature extraction steps, so a good position prediction is necessary to robustly track features. This paper [5] presents a method for combining dead reckoning sensor information in order to provide an initial estimate of the six degrees of freedom of a rough terrain rover. An inertial navigation system (INS) and the wheel encoders are used as sensory inputs. The sensor fusion scheme is based on an extended information filter (EIF) and is extensible to any kind and number of sensors. In order to test the system, the rover has been driven on different kind of obstacles while computing both pure 3D-odometric and fused INS/3Dodometry trajectories. The results show that the use of the INS significantly improves the pose prediction.

Figure 7. The EIF sensor fusion scheme, easily extendable to more sensors. The INS is divided into two logical sensors: an inclinometer and an inertial measurement unit. When the robot is stopped, at time ks, the ZUP (Zero Update Position) becomes active. This allows guarantying fast convergence of the INS biases and no drift while the robot is stopped

Figure 8. Comparison between pure odometry (a) and filtered trajectory (b). The final error [ex, ey, ez, ey] (in meters and degrees) is respectively [0.16, 0.142, 0.014, 18°] and [0.06, 0.029, 0.012, 1.2°] for this run. Figure 7 shows the fusion scheme and figure 8 an experimental result. More details can be found in [5