众所周知,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会卡在墙角
添加后丝滑通过~