Where was the state dynamics equations taken ?

[jediofgever]

Hi ,
I am a litlle confused about the equations,

genesis_path_follower/scripts/controllers/kinematic_mpc.py

Line 118 in 7f62ff6

# State Dynamics Constraints

Especially beta , what does beta represents here ?

I would be really happy if you could provide a link to a reference document so I can better understand.

Thank you

[govvijaycal]

It's an implementation of the kinematic bicycle model used in this paper: "Kinematic and dynamic vehicle models for autonomous driving control design", IV 2015.
Link: https://api.semanticscholar.org/CorpusID:207012925 .

You can refer to the book "Vehicle Dynamics and Control", Rajamani Ch. 2, for the derivation details.
Link: https://link.springer.com/book/10.1007/978-1-4614-1433-9

[jediofgever]

One more thing I want to ask is , I see you comment to add obstacle avoidance as a constraint to the casadi. Can we set the those constraints at each iteration for the dynamic obstacle ?,
For the static known obstacle it should be straight forward but not sure if it will be possible for dynamic obstacles

[govvijaycal]

Yeah that's a good point. If you know the structure of the collision avoidance constraint (e.g. it's a simple stop line), you can simply set it as a parameter that is updated online.

But if the number/structure of constraints is constantly changing, a parameter-based approach might not work. You can definitely create a new optimization problem at each time step, but I'm not sure about computational speed.

[govvijaycal]

Easiest thing would be to figure out the stop line based on all obstacles in the scene and just take the most conservative one. Then you can have a single constraint as a parameter.

[jediofgever]

Thanks a lot for the replies. The structure of obstacle is usually a polygon,
where the boundaries of each obstacle is defined by {(x_0,y_0) , . , . , . , .(x_n,y_n)}.
But all in all I am willing to narrow down structure to an approximate circle with (x,y,radius).

Maybe it would be to much to ask but I hope you don't mind;
Could you elaborate your suggestion with a few simple pseudo code ?

My understanding is that;
I can tackle this with something like;

# a circle where its center is located to CoG of car , and covers whole body of car
robot_radius = 1.3
# maximum number of obstacles
MNO = 10
# current obstacles as parameter:  [x_0, y_0, radius_0, ..... x_MNO, y_MNO, radius_MNO]
self.obs_curr  = self.opti.parameter(MNO,3)

for i in range(MNO):
		# Dynamic obstacle constraint
		self.opti.subject_to( sqrt(pow(self.z_curr(0,0) - obs_curr[i,0] , 2)  + pow(self.z_curr(0,1) - obs_curr[i,1] , 2))  >      robot_radius+ obs_curr[i,2]) )

I declare a parameter vector with a size of max obstacles, then after each iteration I update the obstacles.
Assuming my robot's footprint is represented by a circle ,
then the distance between current state and each obstacle should be greater than sum of the robot radius and obstacle radius for a collision free control action.

What do you think about this approach ?
Maybe it shouldn't be z_curr but z_dv ?

[govvijaycal]

So the parameter approach makes the most sense for cases where the number of constraints is fixed but the specific values are time-varying (e.g. you have a speed limit). One question would be if MNO = 10 but only 1 obstacle is present, is there a way to set the parameter value such that the corresponding constraint is inactive. A silly method would be to just put the obstacle region way out of the N-reachable set of the ego vehicle, where N is the planning horizon.

There are several papers that look at collision avoidance using MPC/trajectory optimization, here are a sample of them:
(1) https://arxiv.org/pdf/1711.03449.pdf -> can handle convex obstacles like polygons / ellipsoids
(2) https://ieeexplore.ieee.org/abstract/document/8207782 -> treats other vehicles as time-varying ellipsoids with Gaussian uncertainty
(3) https://ieeexplore.ieee.org/abstract/document/7963821 with code documentation here: https://juliampc.github.io/MAVs/stable/packages/computing/planning/nloptcontrol_planner/main/

However, I think these problems involve changing the number of active constraints at runtime. You could simply try using Casadi but by creating a different optimization problem each time. Or perhaps Opti has a functionality to turn on/off constraints at runtime, not sure.