ROS teb_controller实现高精度PID控制

众所周知，teb_local_planner是一个计算量很大的局部规划器，当和其他线程一起运行，特别是加入了机器视觉等模块后，常常跟不上设定的规划频率，输出的cmd_vel可能只有5Hz，也就是0.2秒规划一次。如果机器人移动速度较快，比如1m/s，那么在20cm内机器人都将以一个速度进行运行，这就带来了非常大的风险。因此，我们要把teb的pose_array的计算结果提取出来，使用PID控制器，以一个高频率输出cmd_vel去跟踪这条轨迹。

首先记录一下不用tf的方向四元组转三元组的方法（因为tf与python3兼容性很不好，博主配环境搞了一下午还是没法兼容，后来直接想办法不用tf里自带的转换函数）。

import transforms3d

mat1 = transforms3d.quaternions.quat2mat([point.pose.orientation.x,point.pose.orientation.y,point.pose.orientation.z,point.pose.orientation.w])
theta = ransforms3d.euler.mat2euler(mat1)[0]

controller的具体思路是对收到的posearray中的x、y、theta、v、w等先进行线性插值，然后利用一个lookahead，前馈控制[v,w]+反馈PID控制[k*ex,k*(ey+etheta)]来输出[v*,w*]

核心代码：

if len(t)>0:
      try:
        xck = interpolate.splrep(t, x)
        yck = interpolate.splrep(t, y)
        vck = interpolate.splrep(t, v)
        thetack = interpolate.splrep(t, theta)
        omegack = interpolate.splrep(t, omega)
        mat=transforms3d.quaternions.quat2mat([pos.orientation.x,pos.orientation.y,pos.orientation.z,pos.orientation.w])
        euler=transforms3d.euler.mat2euler(mat)[0]
        time_from_start = rospy.get_rostime().to_sec()-stamp.to_sec()
        time_ref=time_from_start+look_ahead
        x_n_t=interpolate.splev(time_ref, xck, der=0) # Refer to the init pose
        y_n_t=interpolate.splev(time_ref, yck, der=0) # Refer to the init pose
        v_n_t=interpolate.splev(time_ref, vck, der=0)
        theta_n_t=interpolate.splev(time_ref, thetack, der=0)
        omega_n_t=interpolate.splev(time_ref, omegack, der=0)
      except:
        x_n_t=x[1]
        y_n_t=y[1]
        v_n_t=v[1]
        theta_n_t=theta[1]
        omega_n_t=omega[1]
      x0=pos.position.x-INIT_POSITION[0] # Both coordinate take the initial point as the origin
      y0=pos.position.y-INIT_POSITION[1] # Right is positive x, forward is positive y
      theta0=euler
      if y_n_t>y0:
        fai = np.arctan((y_n_t-y0)/(x_n_t-x0))
        if fai<0:
          fai=fai+np.pi
      else:
        fai = np.arctan((y_n_t-y0)/(x_n_t-x0))
        if fai>0:
          fai=fai-np.pi
      alpha=fai-theta0
      if np.abs(alpha)>np.pi/2:
        f_1=f_2=1
      ld=((y_n_t-y0)**2+(x_n_t-x0)**2)**0.5
      dx=ld*np.cos(alpha)
      dy=ld*np.sin(alpha)
      dtheta=theta_n_t-theta0
      out_v=f_1*v_n_t+(1-f_1)*Kv*dx
      out_w=f_2*omega_n_t+(1-f_2)*Kw*(lambda1*dy+lambda2*dtheta)
      if out_v>max_v:
        out_v=max_v
      if out_w>max_w:
        out_w=max_w
      vel_msg.linear.x = out_v
      vel_msg.angular.z = omega[1]
      
    velocity_publisher.publish(vel_msg)

测试结果：单场景从17s降为11s。

未加controller会卡在墙角

 添加后丝滑通过~