Controlling a quadrotor: thrust and torques

Typically: "How do I... ", "How can I... " questions
Post Reply
Marcus
Posts: 2
Joined: 14 Apr 2019, 11:35

Controlling a quadrotor: thrust and torques

Post by Marcus »

Hi all,

I'm a student and i want to control a quadrotor in the Vrep environment without using the particles control based method.

Since there's no way to control the quadrotor using the motion of the propellers (that as we know generate the thrust and the torques), my control algorithm would directly give as output the trust and the torques that will be simply applied on the quadrotor object using the sim.addForceAndTorque function.

Before implementing my control scheme, as first attempt i tried to implement a simple height control as follows:

Code: Select all

function sysCall_init()
    uav = sim.getObjectAssociatedWithScript(sim.handle_self)
    mass_uav = sim.getShapeMassAndInertia(uav)
    g = -sim.getArrayParameter(sim.arrayparam_gravity)[3]
    z_des = 2
    zV_des = 0
    Kp = 10
    Kv = 10
end

function sysCall_actuation()
    z_curr = sim.getObjectPosition(uav,-1)[3]
    vel,ang = sim.getObjectVelocity(uav,-1)
    zV_curr = vel[3]
    matrix = sim.getObjectMatrix(uav,-1)
    matrix[4] = 0
    matrix[8] = 0
    matrix[12] = 0
    T = mass_uav*(g + Kp*(z_des-z_curr) + Kv*(zV_des-zV_curr)) --height control
    thrust_vrepFrame = sim.multiplyVector(matrix,{0,0,T})
    sim.addForceAndTorque(uav,thrust_vrepFrame,{0,0,0.0})
    
end
and it works fine (notice that the quadrotor starts with z = 1 and orientation = (0,0,0)).
Then, i added an attitude control, while keeping the quadrotor at constant height and starting with orientation = (0,0,0), as follows:

Code: Select all

function sysCall_init()
    uav = sim.getObjectAssociatedWithScript(sim.handle_self)
    mass_uav = sim.getShapeMassAndInertia(uav)
    g = -sim.getArrayParameter(sim.arrayparam_gravity)[3]
    z_des = 1
    zV_des = 0
    Kp = 20
    Kv = 10
    Kp_tau = 0.01
    Kv_tau = 0.01
    ori_des = {(0)*3.14/180,(0)*3.14/180,(10)*3.14/180}--conversion from degree to radiant
    oriV_des = {0,0,0}
end

function sysCall_actuation()
    matrix= sim.getObjectMatrix(uav,-1)
    matrix[4] = 0
    matrix[8] = 0
    matrix[12] = 0
    z_curr = sim.getObjectPosition(uav,-1)[3]
    ori_curr = sim.getObjectOrientation(uav,-1)
    vel,oriV_curr = sim.getObjectVelocity(uav,-1)
    zV_curr = vel[3]
    T = mass_uav*(g + Kp*(z_des-z_curr) + Kv*(zV_des-zV_curr)) --height control
    thrust_vrepFrame = sim.multiplyVector(matrix,{0,0,T})
    tau_phi = Kp_tau*(ori_des[1]-ori_curr[1]) + Kv_tau*(oriV_des[1]-oriV_curr[1])    --wrt angle along x axis of the quadrotor
    tau_alpha = Kp_tau*(ori_des[2]-ori_curr[2]) + Kv_tau*(oriV_des[2]-oriV_curr[2])  --wrt angle along y axis of the quadrotor
    tau_psi = Kp_tau*(ori_des[3]-ori_curr[3]) + Kv_tau*(oriV_des[3]-oriV_curr[3])    --wrt angle along z axis of the quadrotor
    tau = {tau_phi,tau_alpha,tau_psi} --attitude control
    sim.addForceAndTorque(uav,thrust_vrepFrame,tau)
end
I notice that, given phi,alpha and psi the desired orientations around the x,y and z axis respectively,

- if i give only a desired psi (like in the previous code) which is smaller than 31 degree the control works fine (the quadrotor goes to the desired psi), otherwise the quadrotor starts to move in an uncontrollable way
- if i give only a desired alpha, the control works fine for any possible value and the quadrotor starts moving in the y direction as expected.
- if i give only a desired phi, the quadrotor starts to move in an uncontrollable way for any possible value, even small one (e.g., phi = 0.000001 degree)

Besides i also notice that changing the starting psi value leads to an uncontrollable behavior even with only a small desired value for alpha.

So my questions are:
1)Is this behavior in some way related to how addForceAndTorque apply a torque on the quadrotor?
2)Is there a way to correctly apply an attitude control like the one proposed here?
3)If i have a reference velocity, e.g. i want to drag the quadrotor in the y direction, is there a way in Vrep to correctly drag the robot, that is, continuously changing its orientation to reorient the thrust vector, without using particles? I know that there are control methods that starting from a desired pose return a desired thrust and torque but i don't know who to apply these commands in Vrep in a proper way.

Thank you in advance,

Regards

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: Controlling a quadrotor: thrust and torques

Post by coppelia »

Hello,

your problems come from the fact that you are using Euler angles to identify an orientation. This is never a good idea since with Euler angles you can have gimbal lock situations (i.e. values changing very quickly in certain situations). This is what happens when your Euler angles are getting larger.
Quaternions do not have this problem but are more complicated to imagine and work with.
A good method is to use the transformation matrix of your quadrotor and extract values from it, in order to obtain decoupled angular values, i.e. angular values that are independent of the absolute rotation state of your quadrotor when seen from above:

Code: Select all

local m=sim.getObjectMatrix(quadrotor,-1)
local quadrotorAngleLeftRight=math.asin(m[9])
local quadrotorAngleFrontRear=math.asin(m[10])
Cheers

Marcus
Posts: 2
Joined: 14 Apr 2019, 11:35

Re: Controlling a quadrotor: thrust and torques

Post by Marcus »

Hi,
thank you for your reply. I tried your suggestion by substituting ori_curr[1] with quadrotorAngleFrontRear and ori_curr[2] with quadrotorAngleLeftRight but i still have the same behavior. Notice that i rotated the tau vector w.r.t. the current orientation of the quadrotor, i.e.

Code: Select all

 tau = {tau_phi,tau_alpha,tau_psi} --attitude control
 tau = sim.multiplyVector(matrix,tau)
Besides i preserved ori_curr[3] since i don't know how to get it from the object matrix. Can this be the problem? Can you explain me why quadrotorAngleFrontRear and quadrotorAngleLeftRight are retrieved in this way?
By the way i tried also a position+psi control based, using Euler angles and skipping the torque control by setting directly the desired phi, alpha and psi obtained from the desired position and psi, and i noticed that the motion along the x direction of the quadrotor is good while along the y direction there's a drift behavior (the quadrotor moves in the y direction but with a small drift).
Can this problem (and maybe also the previous one) related to how the quadrotor model is build? I'm asking you this question since a tried to apply the position+psi control to a new quadrotor model built from scratch using a CAD model and the steps in http://www.coppeliarobotics.com/helpFil ... torial.htm and the motion along the y direction is without drift (even if the control does not achieve a low error on the position and it does not perform a well behavior while changing the desired yaw angle along the trajectory).

Thank you in advance,

Cheers

coppelia
Site Admin
Posts: 10339
Joined: 14 Dec 2012, 00:25

Re: Controlling a quadrotor: thrust and torques

Post by coppelia »

I think it is very important to get a good understanding and feeling about what a transformation matrix is and what its element or columns are representing. Do not work with Euler angles because of the reasons I already mentioned.
From the transformation matrix you can extract many useful and meaningful values.

Cheers

Post Reply