Optimization problem: obtain linear results using fmincon

Hello everybody, it is the first time that I use this forum then I am sorry if my question will be not complitely clear.

I have a optimization problem that I am trying to solve using fmincon. The objective function is the difference between two frames (position and orientation are considered). I have a robot (with relative joint angles) and I want that the optimization problem minimize the error between "theorical points" and "pratical points". These points define a configuration of the robot or posture. To find the configuration, I want to optimize the value of the joint angles.

The algorithm converges and I am able to find satisfactory results. The problem is that, in some configuration, the optimization algorithm finds some "optimal joint angles" that are in the joint boundaries but they are too much different between them. This leads to have a good minimization of the error but strange behaviour of the robot.

The optimization algorithm starts from a zero configuration (that is perfect and converges immediately) and from this configuration starts to find a good solution for the next step (because we know that it should be in a neighbor of it). The problem is: in the step N the joint angle found is 1.54 (for example) and for the step N+1 should be closer to it (1.76 or 1.30 or something like that). The boundaries are from 0 to 2. It finds a solution that is closer to 0: it is in the boundaries but the variation is too high.

So, I want to ask you how can I do to obtain, in the next N+1 a results that is closer to the previous one (N) in order to obtain a set of value that is linear. I show you the set of results obtained now. As you can see, between 425 and 512 it is 0 but it should be as in the red line (the blu line is the set of value computed with the optimization algorithm while the red line is the theorical set of value)

The Matlabcode:

options = optimset('Display','iter','MaxFunEvals',10000,...
                           'TolFun',1e-6,...
                           'TolX', 1e-6,...
                           'MaxIter',10000,...
                           'Algorithm','active-set');

[fqs,Fval,EXITFLAG] = fmincon('TrMatxRelation',q_init,[],[],[],[],lb,ub,'constraint',options);

TrMatxRelation is the function where it is computed the error (difference between position and orientation of the two points)

q_init is the initial value -> at the step N, q_init corresponds to fqs of the step N-1.

_constraint _function is empty