About the system model and non-quadratic objective function

[BlueeHole]

First of all thank you for sharing this code! But I have a question for this. The vehicle model you use in car.py is non-linear.

def update_state(self, dt):
        A = np.array([
                [1, 0, 0],
                [0, 1, 0],
                [0, 0, 1]
                    ])
        B = np.array([
                [np.sin(self.x[2, 0] + np.pi/2)*dt,  0],
                [np.cos(self.x[2, 0] + np.pi/2)*dt,  0],
                [0           , dt]
                    ])

        vel = np.array([
                    [self.x_dot[0, 0]],
                    [self.x_dot[2, 0]]
                ])
        self.x = A@self.x + B@vel

In the MPC objective function, you directly use this model to derive all future states within the control hoziron, and use QP to solve it.

x, _ = controller_car.get_state()
z_k[:,i] = [x[0, 0], x[1, 0]]
cost += np.sum(self.Q@((desired_state-z_k[:,i])**2))

But in this case, the objective function is not quadratic, right? (it contains sin(x) ). Is there any theoretical basis for doing this? Or did I misunderstand something? Hope for your responding, thank you!

[zainkhan-afk]

Hi,

The car model is constrained to only move in one direction (forward). This means that the motion in the other planar direction is constrained, in this case I have assumed that "forward" is towards the y-axis of the robot. Therefore, the velocity of the robot will be applied in the y direction when its heading is 0, however, when the robot heading is non-zero, the robot will move at an angle and the robot displacement will be in both x and y axis, in other words, when looking at the robot from the world frame, it will appear to have velocity in both x and y axis. To calculate this velocity I take the sine and cosine of the robot heading, this gives me a multiplier (scaling value) that I multiply with the current robot velocity to make it move according to the heading.

You can think of the result after @.*** to be equal to:

[ [delta_y], [delta_x], [delta_theta] ]

The cost function is a quadratic equation, I am summing the squares of the variables and their differences.

On Tue, Mar 26, 2024 at 5:24 PM 莫非 @.***> wrote:

First of all thank you for sharing this code! But I have a question for this. The vehicle model you use in car.py is non-linear.

def update_state(self, dt): A = np.array([ [1, 0, 0], [0, 1, 0], [0, 0, 1] ]) B = np.array([ [np.sin(self.x[2, 0] + np.pi/2)dt, 0], [np.cos(self.x[2, 0] + np.pi/2)dt, 0], [0 , dt] ])

  vel = np.array([
              [self.x_dot[0, 0]],
              [self.x_dot[2, 0]]
          ])
  self.x = ***@***.*** + ***@***.***

In the MPC objective function, you directly use this model to derive all future states within the control hoziron, and use QP to solve it.

x, _ = controller_car.get_state()z_k[:,i] = [x[0, 0], x[1, 0]]cost += np.sum(self.R@(u_k[:,i]**2))

But in this case, the objective function is not quadratic, right? (it contains sin(x) ). Is there any theoretical basis for doing this? Or did I misunderstand something? Hope for your responding, thank you!

— Reply to this email directly, view it on GitHub https://github.com/zainkhan-afk/Differential-Drive-Robot-Navigation/issues/3, or unsubscribe https://github.com/notifications/unsubscribe-auth/AIUWJZIJM3LHTH6YC4PKYA3Y2FSHRAVCNFSM6AAAAABFI2GUMGVHI2DSMVQWIX3LMV43ASLTON2WKOZSGIYDQMJQGQ2TONI . You are receiving this because you are subscribed to this thread.Message ID: @.***>

[BlueeHole]

Thank you for your detailed response! But I still have one question. quadratic should be polynomial, right? But here, at every step, you update x using x\dot=Ax+Bu, here A is non-linear of x. ( because sin(\theta) ). So from the second step of update, u has been coupled with x, and the whole cost function has been non-linear of u. Therefore, the sum of these items would be non-linear of u. That is, the objective function is non-linear. Here is a misunderstanding. I firstly thought SLSQP is for quadratic(polynomial) objective function, but it actually can solve non-linear functions.( from the documentation of scipy) So if I had discovered that SLSQP could be used to solve nonlinear problems from the beginning, I would not have raised this issue. But I happened to have not discovered it before, that's 'lucky', to some degree. To sum up, my question is , whether your cost function is a polynomial of u? I think it's not, so it's not quadratic. I searched other materials and found many people do linearization like this: (sorry it's Chinese, but please check the formula) that is to use Taylor expension on the reference point. Here the θr is a constant (the reference theta )As I think, through this the objective function would be linear and really 'quadratic'. But by this method, we need to provide θ_r and v_r on every point, since A contains θ_r and v_r.