Active Vibration Isolation for A Parallel Wheel-legged Robot based on Dynamic Model

Serving Stewart plat as wheel-legged construction, the most outstanding superiority of proposed wheel-legged hybrid robot (WLHR) is active vibration isolation during rolling on rugged terrain. This paper presents a force-driven control approach based on model predictive control (MPC) to design optimal control input for Stewart parallel wheel-leg that locomotes using swing foot trajectory. Adding adaptive impedance control in outermost loop, controlling framework prevents robot body horizontal and from vibration over rolling motion. Through dynamic model of Stewart mechanism, controller first creates predictive model by combining Newton-Euler equation, Newton-Raphson iteration of forward kinematic solving for current configuration, inverse kinematic calculation of Stewart obtaining desired joint position, and Gain/Integration module determining reference torque. With minimizing control deviation and input as objective function, a novel control optimization formulation generates optimum input for each control duration. These actuating force naturally enables each strut stretching and retracting used to realize six degree-of-freedom (6DOF) motion for Stewart wheel-leg. We exploit the variable-adapting method to reasonably adjust environmental impedance parameters by current position, velocity, force feedback of wheel-leg. This allow us to adequately acknowledge the desired support force tracking, isolating robot from isolation that is generated from unknown terrain. We demon-strate the validation of our control methodology on physical prototype by tracking a Bezier curve and active vibration isolation while the robot is rolling on decelerate strip. Respectively given PI controller and a sort of traditional impedance controller as comparison, a better performance of proposed algorithm was operated and evaluated through displacement and force sensors internally-installed in each cylinder, as well as IMU mounted on robot body.


Introduction
The quadruped machine is the most agile legged robot that can accomplish traversing through swing leg without high-complexity dynamics. Improving the motion efficiency, we introduce a novel design of mobile robot that adopts arbitrary arrangements of legged and wheeled locomotion accounting for different types of terrains. The hybrid robot tightly integrates the wheel on end-effector of wheel-leg. The wheel-leg favors parallel mechanism with inverted Stewart plat instead of classical three-joint serial structure, inheriting existing benefit: the parallel wheel-leg is able to maintain robot body horizontally with modest oscillation as soon as possible while robot is rolling on uneven ground. Even though the position controller for Stewart parallel architecture can achieve pretty high accuracy [1], the foot interaction with environment renders the acceleration variation of robot body. For the above reason, developing a delicate torque controller based on dynamic model and merging adaptive impedance control to handle these interaction is indispensable and crucial. For common vibratable system rather than parallel mechanism, there are several algorithm implemented to sustain specific constant posture or configuration. Five candidate jitter cancellation algorithms are evaluated [2] for active vibration control on a spacecraft testbed, where adaptive linear model predictive control achieved one of best disturbance rejection results with a 66% overall amplitude reduction. A new cooperative model predictive control [3] is developed for Stewart stabilizable system. A new attitude balancing strategy [4] implemented an inverse kinematics solution scheme based on Extended Prediction Self-Adaptive Control (EPSAC) algorithm to generate full body motions that ensure the desired balancing performances for a hybrid wheellegged mobility system. A design procedure for multi-strut vibration isolation platform (VIP) [5] is the determination of optimal damping frequency of tree parameter isolator and isofrequency VIP design with natural frequencies tuned as the optimal damping frequency in three orthogonal directions. To obtain the favorable vibration performance, the dynamic model of the Stewart platform is constructed with flexible hinges [6] and coupled multiple flywheel system(MFS) [7], forming state-space equations for control purposes. To suppress the self-excited vibration owing to flexibility, friction, backlash, coupling, and other nonlinear factors, a nonlinear controller and a fuzzy control algorithm [8] are designed to attenuate the self-excited vibration for the 3-RRR flexible parallel robot. The dynamic model of the Stewart platform is established by the frequency response function (FRF) synthesis method. In the active control loop [9], the direct feedback of integrated forces is combined with the FxLMS-based adaptive feedback to dampen vibration.
As a leg of wheel-legged robot, Stewart platform needs to swing along a trajectory to traverse obstacle through a basic inner-loop controller. Apart from employing position control for Stewart manipulator [10] with Conventional Adaptive Fuzzy Sliding Mode Control method. Nader [11] analyzed dynamic model using Newton-Euler to form a closed-loop dynamic control techniques with force and successfully tracking two trajectories. Incremental Nonlinear Dynamic Inversion [12] addressed the implementation of a high-precision force controller for a Stewart hydraulic robot in existence of parameter uncertainties, validating the good performance of force and position tracking.
Additionally, a common method of disposing interaction with environment is impedance control and force position hybrid control that operates like a passive spring. The MIT-Cheetah [13] imposed a gait-pattern modulator and a leg-trajectory generator consisting of Bezier curve and a tunable amplitude sinusoidal wave, where the proprioceptive impedance control is employed in individual low-level leg controllers. Another position controller for parallel robot [14] utilized the position impedance control to achieve smooth contact with environment, which is combined with Kalman filter to predict stability margin of robots inside ZMP stability observer. Especially for a uncertain terrain, it is difficult to obtain a specific model when the environment stiffness and location are unknown. The TurboQuad robot [15] operates a force-control strategy to regulate the wheel-legs to act like a passive spring, as well as utilizing the stable running dynamics of the spring-loaded inverted pendulum (SLIP) model as a template to initiate pronking and trotting behaviors. A seven-link biped robot [16] coupled the feedforward compensator into fuzzy controller with inverse dynamic and noise reduction capability to track desired motion. A new dynamic method [17] for fully reactive footstep planning of a planar spring-mass hopper characterized a careful characterization of the model dynamics. The LittleDog [18] robot employed QR decomposition to solve the floating-base inverse dynamics and predictive force control to increase robustness in face of unknown perturbations. The core opinion of variable impedance control is to adapt impedance parameters into position and velocity feedback, satisfying desired force value. A new adaptive impedance control [19] is proposed for force tracking which has the capability to track the dynamic desired force and compensate for uncertainties (in terms of unknown geometrical and mechanical properties) in environment.
We present a adaptive impedance control framework with force input based on dynamic model of Stewart wheel-leg to isolate vibration for BIT-NAZA robot. The BIT-NAZA robot is a type of wheel-legged hybrid robot with Stewart parallel mechanism and high payload. The core of our work lied on the Newton-Euler dynamic equation reveals the relationship between strut input force and foot configuration. To obtain a smooth output trajectory of wheel-legged mechanism, we limit the desired velocity and control input increment during each sampling instant. The feedback data is measured from displacement sensor installed in each strut and interactively updated by forward kinematic calculation. Combining with adaptive impedance control in outloop, our methodology enables a sequence of corresponding control input for vibration isolation motion interacting with unknown environment, where the actual state feedback is also necessary to estimate environmental stiffness. For the availability validation of proposed approach, the mentioned method is employed in two kinds of implements working on practical prototype, driving robot to pass through the speed bump with relative stable force and body acceleration variation.

Problem Formulation and System Dynamic Model
One of the most substantial benefits of parallel wheel-legged mechanism is active vibration isolation to decrease attitude variation of robot body while rolling on irregular terrain. Following this trait, the robot can acquire more robust position tracking performance no matter while swinging foot but also inner-loop isolating vibration. transport luggage and keep body balance during rolling motion. In this work, the proposed algorithm is responsible for maintaining body horizontally according to data from displacement and force transducer embedded in each strut, in which the dynamic model is applied within MPC. The wheeled speed consensus control is not concerned [20], leading robot to run at a constant speed.
The block diagram for active vibration isolation is demonstrated in Fig.1, of which different boxes represent effective modules and procedure adjusting single Stewart configuration, i = 1, · · · , 4. Impose the practical stretching length of strut as feedforward of model predictive controller and calculate the current configuration through Newton-Raphson iteration. The current configuration, velocity, and angular speed are combined into dynamic model of Stewart wheel-leg, forming a closed-loop position tracking controller with force input. The purpose of vibration isolation is to alter configuration of wheel-leg to adapt robot in terrain variation under condition that it still can share robot mass with other three wheel-legs, prevent rolling wheels from hanging in the air, and avoid robot body to generate a drastic oscillation. When four wheel-legs stand on their median configuration, the body frame R, foot frame F i , i = 1, . . . , 4, /S11 /S16 /S12 /S13 /S14 /S15 /S11 /S16 /S12 /S13 /S14 /S15
and world frame W are constructed and attached on robot as described in Fig.2. For the purpose of earning high accuracy of controller, we extract single Stewart wheel-leg and construct the corresponding kinematic and dynamic model. The desired position and attitude along trajectory are used to get the desired stretching length for each joint by Stewart inverse kinematics. Target length incorporates actual length of each strut into the proportional-integral (G/s) link and become reference torque of MPC, rendering robot body to maintain horizontal according to practical configuration during interaction with rugged terrain. The forward kinematic calculation is to solve the space configuration of moving platform under the condition that stretching displacement for all of struts is known. The proposed algorithm applies torque control to achieve the vibration isolation. The most outstanding method to evaluate its performance is to measure the acceleration of robot body along vertical direction, we illustrated the experimental data through operating the whole control framework on BIT-NAZA robot. The cardan joints connect struts with moving and base platform, ensuring zero constraint on six degrees of freedom for moving platform. Progressively, invert the Stewart wheel-leg to describe dynamic model arrangement as denoting in Fig.3. The moving frame B and base frame A are fixed on moving and base plats respectively. Thus the rotation matrix from moving frame B to base frame A is Fig. 3 The Stewart coordinate frame definition.
where sX = sinX, cX = cosX, ϕ x , ϕ y , and ϕ z are pitch, roll, and yaw angle of moving platform separately. With position analysis in Fig.3, the vector loop equation of each linkage is acquired. The dynamic model involves the mathematical expression between motion parameter of moving platform (including configuration, velocity, and acceleration) and actuating force of each cylinder. As a result of complicated parallel mechanism with 6-DOFs, the dynamic model of Stewart presents a multi-degree of freedom, multi-variable, and high nonlinear system. There are several approaches allowing multi-rigid system like Stewart mechanism to create dynamic model and containing Newton-Euler equation, Lagrange equation, and Kane method. This work adopts Newton-Euler equation to build the dynamic analysis and rapidly solve its solution in high-dimensional condition.
With ultimate aim at commanding the wheel-leg, the impression on dynamic model resulting from rotational inertia of actuating cylinder emerges really small compared with mass of robot body and wheel group is large. Therefore, the inertia moment of electrical cylinder can be neglected when rotational and sliding friction of cardan joints and cylinders are ignored. The electrical cylinders are simplified as massless connecting rods which only afford the tension and gravity along rod directions. According to force analysis from newton's second law, the motion equation related to center of mass of moving platform in base frame A can be denoted as follows in which m p is mass of moving platform, g signifies gravity acceleration, A f i represents the driving force vector of electrical cylinder i, andẍ p indicates acceleration of moving platform in base frame A. Let B r i be position vector of force action point in moving frame B. On the basis of the applied moment on point B r i from actuating linkage i and generating rotational angular velocity B ω p , compute the Euler rotation equation of moving platform in moving frame B.
The matrix formulation of rotational inertia tensor for moving platform in moving frame B where inertia product along x B axis, y B axis, and z B axis in moving frame B can be expressed as Transform the Euler equation into base frame A The Multiply both sides with A R B , and signify driving force as A f i = A s i f i , the scalar quantity f i represent value of driving forces Make , then the combined form of Newton motion and Euler rotation equation can be printed as

Methodology
Even though the control input on Stewart is force, we still needs configuration and force data of foot measured and calculated through sensors, forward kinematics, and Jacobian matrix to update adaptive impedance parameters. The outermost control loop exploits adaptive impedance control to track desired support force, maintaining whole body stability during rolling process.

Model predictive control
Accounting for features of MPC that are predictive model, receding horizon optimization, feedback correction, and explicit processing constraints, we design a model predictive controller based on torque input. Forecast future output of system and repeatedly optimize a certain index online based on historical performance and future input. In order to prevent model mismatch or environment disturbance from generating control deviation with ideal state, we detect output result at new sampling instant and correct predictive result. The pipeline serves the motion velocity as target control vector y r (k) and evaluates state vector of wheel-leg x(k), getting state vector x(k) of discrete model controller. Solve the output vector y(k) in predictive horizon on the basis of predictive model. With minimizing objective function, the last procedure calculate the optimal control vector u(k), achieving the tracking from output vector y(k) to objective vector y r (k) under effect of control vector.
To prepare for next step that builds a predictive model for Stewart, we transform Eq.(10) as standard state space formulation including state equation and output equation. Set the state , so the state equation can be written asẋ , output equation can be expressed as is position and attitude vector of moving platform in base frame A and An efficient method to accomplish optimized control is selecting control increment as state of objective function like the following formulation, which penalizes the deviation between predictive state and reference input.
in which y r (k + i|t) is desired system output in predictive horizon from current instant k, that is desired trajectory of moving platform in predictive horizon N p . Note that control horizon N c and predictive horizon N p satisfy N c < N p . Q and R are definite weight matrices. The decision variables are trajectory properties for all of the six degrees of freedom. The mentioned inequality constraints softly restrain the system output y(k + i|t), control input u(k + i|t), and variation of control variable u(k + i|t) − u(k + i − 1|t). To guarantee the output trajectory accuracy and input smoothness, we minimize the 2-norm of the state increment which is a kind of hard constraints. The slack factor σ k ≥ 0 is able to ensure that the optimization problem does not become infeasible if defined constraint gets violated. The optimization problem depicted in Eq.(13) can be converted into a standard quadratic problem formulation. During each sampling duration, the computation repeats and only the first value u(k|t) of the optimal control sequence u(t) is employed as the system input until the next sampling time k + 1. It is turned that utilize the state x(k + 1|t) to update the initial condition of Eq.(13) and resolve it.

Adaptive impedance control
The core thought of impedance control is to regard robot as physical construction, environment equals as admittance, and the part of environment that executes corresponding operation as impedance. We exploit a second order linear equation to describe a mass-spring-damping system to describe desired impedance relationship in foot Cartesian system.
in which M f , B f , K f are inertial matrix, impedance matrix, stiffness matrix of desired impedance model respectively.Ẍ r andẊ are desired position and current position of wheel-leg. F is support force from contact ground. Add the error between support force and desired force as input of target impedance model, focusing on following desired force. The new impedance model expression can be written as where force error E = F − F r and F r is desired force offered by foot. Thus expressing formulation of desired impedance equation in frequency-domain is illustrated as follow To visually analyze relation between foot and environment, we construct a equivalent impedance model. x env , m env , b env , and k env are environment position, inertial, damping and stiffness matrix separately, rendering wheel-leg as a spring denoted in Fig.2. Intuitively, position control variable X r is different from environment position x env , foot touching ground generates contact force F and reaches to stable state as well. The stable-state error between F and desired contact force F r relates to individual and environmental impedance parameters. For the purpose of sustaining contact force between foot and environment, we have to keep contacting depth and position input X r constant, namelyẊ r = 0 andẌ r = 0. Eq. (14) can be simplified as The environmental impedance model is exhibited as While foot is touching the ground during stance state, velocity and acceleration become really small. So we just consider environmental stiffness and neglect velocity and acceleration items. The simplified environment model turns into Thus the foot position expression is According to Eq. (18) and Eq. (20), we have It can be transformed into following formulation when foot touches ground and reaches stable state. ( Thus the stable-state error can be expressed as Let contact force of foot F ss → F r , namely E ss → 0. The target position input needs to be sufficed The stable-state error between contact force and desired force can be eliminated once Eq.(25) and Eq.(26) correct desired position X r . However, the environmental stiffness k env and position x env is indispensable to calculate X r , as well as impossible to obtain when the specific terrain template data is unknown. Therefore, we need to estimate those two parameters through enacting adaptive algorithm. Estimating environmental stiffness k env and position x env through contact position and force data, robot follows foot support force through computing position input X r . So estimated value of foot contact forcê Hence, the difference between estimator and practical value can be written aŝ Adjustk envxenv andk env during control process according toF − F, resulting inF → F when t → ∞. Define Lyapunov function as in which P is a positive definite matrix with 2 × 2 dimension. AssumingΨ and uniting Eq.(29), Eq.(30) and Eq.(31), we havė The equation is a positive semi-definite matrix. Combing Eq.(29) and Eq.(32), we have following deduction under the condition that Eq.(31) is satisfied. IfF → F, we have F r → F. In consequence, where µ 1 and µ 2 also are constant. The complete adaptive compensation parameters are listed in the following and control the desired position input through Eq.(25) In order to obtain better performance of the proposed controller, we construct a co-simulation system combining the MATLAB/SIMULINK and ADAMS working on a single Stewart mechanism demonstrated in Fig.Fig.3 with the listed model parameter setting in Table( 1). The design of dangling moving plat can exclude the influence force from ground to robot foot that clearly illustrates the position tracking performance of controller. Similarly, the force and displacement sensors internally embedded in each strut to measure its load-carring force and expansion amount, serving as system feedback and solving the force burden and configuration variation of moving base through Jacobian matrix. Dominate the step and sinusoid as trajectory input to adjust the parameter optimization of controller, in which the sample duration T s is 0.0005s. Progressively, we adopt the traditional Control Variate Technique to mutually schedule parameters containing predictive horizon N p and control horizon N c . Given step signal amplitude 0.15 (it can be set as displacement or angle along X, Y, or Z axis, we test it as X-axis displacement in this case) and input constraint within ±700N, evaluate the tracking performance of MPC for step signal under different N p and N c .
Compared Fig.4 with Fig.5, the variation of predictive horizon N p has smaller impression on system step response. Nevertheless, the control horizon N c which differs from sample duration and stands for frequency how long import the optimal control sequence into system has a significant impact on step response. The Table.2 quantizes the step response for different N c . With increasing N c , the updating frequency of optimal control sequence is decreasing, resulting in the reduction of rising time and steady-state relative error. Additionally, in view of the large calculating amount and weakening real-time computing capacity originated from decreasing N c , we choose predictive horizon of proposed controller as N c = 2.
As for the parameter of Gain/Integration (G/s) module depicted in Fig.1, the gain and integral factor also need to be turned. Define the sinusoidal signal with frequency 2Hz and amplitude 0.15rad as pitch angle reference, the corresponding response of control system does not produce shock while increasing the gain value P, which is exhibited in Fig.6. With the fixed predictive horizon N p = 5 and control horizon N c = 2, the amplitude error of sinusoidal response is 0.0004rad with 0.0005s delaying when we set gain of G/s module as P=0.12, satisfying tracking precision and requirement of dynamic property.

Experiments
After the parameter adjustment step, our experiment with the objective of optimizing the control force input and obtaining supportive performance exposes an interesting challenge whether controller combined with the effect from environment generate a stable control result. Based on the practical requirements of BIT-NAZA robot, both a Bezier trajectory for the corresponding swing leg in legged locomotion and vibration isolation in wheeled motion are examined on physical prototype.

Trajectory tracking
In practical experiment, the internally-embedded motor encoder in each strut can read and transit position, velocity, and related motion information. Both displacement and attitude tracking are tested with a comparison of PI controller. For a quadruped locomotion, robot needs to traverse obstacle in swing phase, where the tracking accuracy of applied Bezier trajectory is of great concern. The operating time of pictorial foot trajectory in Fig.7-(a) from point A to point C is 3s, in which the arch height (distance between A and C) is 320mm and arch width (distance between B and line AC) reaches 80mm. The suggested PI controller with P=25 and I = 0.1, the control horizon N c for both two controllers are 2ms. According to tracking result displayed in Fig.7-(b), the largest position error of PI and MPC are -2.7mm and -0.3 respectively. In the following attitude tracking experiment, select sinusoidal signal with amplitude 0.0873rad as reference attitude input for three different control horizon 1mm, 2mm, and 4mm. Their comparison result is demonstrated in Fig.8 as well as matching time-delay and tracking error are digitized in Table(3). Obviously, the improved performance of MPC based on Stewart dynamic model allows foot to more precisely track desired trajectory than PI controller. The optimal predictive model can therefore be created in a very shot time-span, and the controller may fine-tune control input to any desired value. The comparative result for different N c emerged basically consistent with simulation, it can be concluded that the model construction is precise and optimal force input is generated instantaneously.

Vibration isolation
Rolling on rugged roadway is a common assignment for wheel-legged robot BIT-NAZA. As a emulation, passing deceleration strip is used to validate the availability of controller as showed in Fig.9, where low deceleration strip with 8cm high and 80cm width is implemented in this case. The suggested control method governs the left front foot (i=1) and sets desired motor speed as 600r/min. To enlarge motor torque, we install reducer with reduction ratio of 1:40 between motor and wheel, where wheel diameter is 25.4cm. The IMU is equipped on robot body to monitor acceleration variation along z R -axis. Note that we employ a kind of conventional impedance controller within outermost loop of control framework working on wheel-leg as a comparison [21].
In the first case, the robot got through the deceleration strip with 400r/min for each wheeled actuator. Meantime, APC allows wheel-leg to shrink while adapting to rugged terrain and maintaining stationary on Z-axis relative to horizon ground. The expansion amount and load-carrying force of wheel-leg, shown in Fig.10, are mapped from displacement and force of each cylinder through Jacobian matrix J p in Eq. (10). The top extension of AIC reaches 0.0772m, which is same with height of deceleration strip. Even though the extension amount of position-based impedance control(IC) is basically common with one of adaptive impedance control(AIC), other two features are truly improved. The target value of load-carrying force is 500N assuming that four foot averagely undertake whole robot weight. Without any control method, load-carrying force increases and comes up to top value 953N, whose error is 453N. This phenomenon is generated since the wheel-leg that is rolling upon raised speed bump and lifted which renders right front wheel-leg to roll in air. The deviation relative to desired value of other two controller are IC 55N and AIC 39N separately. The top acceleration of robot body along z-axis after AIC input is smaller than that of IC 0.4m/s 2 . With adaptive control law working on, acceleration had became better and had no fluctuation when wheel leaved deceleration strip compared with IC.
In second case, the motor speed enhances to 600r/min to roll over the deceleration strip whose numerical reaction is exhibited in Fig.11. It is apparent that there is fairly strong shock generated on robot and the most intense force loaded on wheel-leg is about 985N without vibration isolation control. Importing IC, the force impulsion radically decreased and the largest deviation relative to desired value reached 88.8N at the same time acceleration amplitude reduced from 2.31m/s 2 to 0.5m/s 2 . After optimal force input from AIC, the vibration isolation performance is served to furtherly boost, where fluctuation region shrink, top load-carrying force turned into 566N, and top acceleration is closer to 0. Consequently, the proposed AIC accomplished optimum achievement.

Conclusions
We presented a novel control system for a Stewart parallel hybrid wheel-legged robot. Thanks to the dynamic model of Stewart mechanism, the objective function solved optimal control sequence and fresh state iteratively update objective function during predictive horizon to complete innerloop position control. We further showed how to leverage the parameter of our controller to significantly improve the control accuracy. This, in turn, allows users to make trade-off between decreasing calculation amount and enhancing precision. In order to better adapt to unknown terrain, adaptive impedance control is implemented into this structure, estimating environmental stiffness and combining with other impedance parameters of wheel-leg. We demonstrated the effectiveness of our controller by tracking Bezier trajectory for swing foot within single innerloop controller and vibration isolation during rolling motion using intact control framework, all of which were tested on a wheel-legged hybrid robot.