I have modeled a wheeled mobile robot (WMR) in CoppeliaSim and write a code
in Lua to control the position of the robot in the world coordinate frame
using feedback linearization approach. First, I have simulated my
controller in MATLAB and evaluate the feedback control law. Then I have written a
code in V-REP to control the robot motion. However, the Controller is
merely stable when I use the negative gains in my controller. while the
error dynamics is in the form (de/dt+k*e=0) and the simulation in MATLAB shows that the closed loop system is stable for k>0, I can't understand how the robot is stable using negative k<0 in CoppeliaSim. I obtain the same simulation results in CoppeliaSim when I use the gains used in MATLAB but with minus sign.
here is the code I developed in Lua.
Code: Select all
function sysCall_init()
R=.05
L=.181
d=3.334e-2
R1_handle=sim.getObjectHandle("R1")
R2_handle=sim.getObjectHandle("R2")
Robot_handle=sim.getObjectHandle("Dummy")
end
function sysCall_actuation()
P=sim.getObjectPosition(Robot_handle,-1)
O=sim.getObjectOrientation(Robot_handle,-1)
q={P[1],P[2],O[3]}
qd,dqd=DesiredTrajectory(sim.getSimulationTime())
U=FeedbackControl(q,qd,dqd,-1,-1,R,L,d)
u1=U[1]
u2=U[2]
sim.setJointTargetVelocity(R1_handle,u1)
sim.setJointTargetVelocity(R2_handle,u2)
end
function sysCall_sensing()
-- put your sensing code here
end
function sysCall_cleanup()
-- do some clean-up here
end
-- See the user manual or the available code snippets for additional callback functions and details
function DesiredTrajectory(t)
t=sim.getSimulationTime()
x=.5*math.cos(.2*t)
y=math.sin(.2*t)
dx=-.5*.2*math.sin(.2*t)
dy=.2*math.cos(.2*t)
qd={x,y}
dqd={dx,dy}
return qd,dqd
end
function FeedbackControl(q,qd,dqd,k1,k2,R,L,d)
x=q[1]
y=q[2]
Teta=q[3]
xd=qd[1]
yd=qd[2]
dxd=dqd[1]
dyd=dqd[2]
Input ={(L*dyd*math.cos(Teta) - L*dxd*math.sin(Teta) +
2*d*dxd*math.cos(Teta) + 2*d*dyd*math.sin(Teta) - L*k2*y*math.cos(Teta) +
L*k2*yd*math.cos(Teta) + L*k1*x*math.sin(Teta) - L*k1*xd*math.sin(Teta) -
2*d*k1*x*math.cos(Teta) + 2*d*k1*xd*math.cos(Teta) -
2*d*k2*y*math.sin(Teta) +
2*d*k2*yd*math.sin(Teta))/(2*R*d),(L*dxd*math.sin(Teta) -
L*dyd*math.cos(Teta) + 2*d*dxd*math.cos(Teta) + 2*d*dyd*math.sin(Teta) +
L*k2*y*math.cos(Teta) - L*k2*yd*math.cos(Teta) - L*k1*x*math.sin(Teta) +
L*k1*xd*math.sin(Teta) - 2*d*k1*x*math.cos(Teta) + 2*d*k1*xd*math.cos(Teta)
- 2*d*k2*y*math.sin(Teta) + 2*d*k2*yd*math.sin(Teta))/(2*R*d)}
return Input
end