上几次介绍了两轮差速机器人的数学模型参见我的博客https://blog.csdn.net/qq_40464599/article/details/107176589和只做了关于航向角的闭环PID控制https://blog.csdn.net/qq_40464599/article/details/107239274

今天这篇文章前几篇会更深入、更具有工程性。

      首先,有一个问题我一直有些模糊,最近考虑到工程实现,所以想了一下。ROS标准的Twist速度信息会向下位机发送一个速度V和角速度w,然后我们需要用v和w去控制两个电机。之前的博客已经推导过,利用运动学反解即可,有如下关系

v_{r}=v_{c}+\frac{w_{c}l}{2}

v_{l}=v_{c}-\frac{w_{c}l}{2}

至此,这个博客的核心就说完啦。

 别小看这两个公式,vr和vl可以认为是控制电机转速的PWM大小,这样的话就有理有根据的把v和w转换成两个电机的转速信息,妙!

然后我们就要对距离进行比例控制有:

            v=k_{1} \times \sqrt{(x-x_goal)^{2}+(y-y_goal)^{2}}

对航向角控制

          w=k_{1} \times (\theta -arctan(\frac{(y-y_goal)}{(x-x_goal)}))

好了,这是我加入的两个闭环控制,显然,你会发现,越靠近目标点,速度越慢。仿真结果相对于上一次真的是好太多,设定终点时(10,10),见下图所示,一点不差走到那里,之前的差不多(10.21,10.21.)是有偏差的。

https://blog.csdn.net/qq_40464599/article/details/107267774

 具体的工程源码参见我的Github:https://github.com/JackJu-HIT/TrajectoryControl

 %%%第一个航向角pid控制器,输出w
    %%%计算当前与目标的朝向误差
        theta_goal=atan((y_goal-y)/(x_goal-x));
        theta_error=theta-theta_goal;
        error_sum=error_sum+theta_error;
        w=-k*(theta_error)+ki*error_sum+kd*(theta_error-pre_error);
        pre_error=theta_error;
 
   %%%%第二个距离的pid控制器,输出v     
        dist=((x-x_goal)^2+(y-y_goal)^2)^(1/2);
        v=k2*dist;
        
 %%%%控制输入,左电机和右侧电机。
 
 vr=v+w*L/2;%控制输入需要你去更改//
 vl=v-w*L/2;%控制输入需要你去更改
 
 
 
 %%%%process model
 
 %%%运动模型
 v=(vl+vr)/2;%中心速度
 w=(vr-vl)/L;
 x=x+v*cos(theta)*T;
 y=y+v*sin(theta)*T;
 theta=theta+w*T;

  上述是我设计的两个PID控制器,用于追踪目标的,设定起点和终点,他会自己寻求一条路线。C++版完整代码请参见我的github:https://github.com/JackJu-HIT/ImuGpsGuiding

   但是对于高更加复杂的情况不知道是我没想到还是pid确实有局限性,下一次准备研究MPC控制器。

                                                                                                                                                                                        20200710

                                                                                                                                                                                        鞠春宇

                                                                                                                                                                                         于研究室