最近进行了一个AGV改造项目，其中涉及到odom—base_link的坐标变换，odom即里程表信息主要通过伺服电机自带的增量编码器获得。
voidrostcp :3360 cal _ pulse (int current，int receive，int delta ) ) delta=(receive-current )； //* M_PI * 0.2/200000.0； current=receive； } voidrostcp :3360 handle _ speed _ msg ({ cal _ pulse (cur _ left _，recv_left_，delta_left ) )； //左轮手枪移动距离cal_pulse(cur_right_，recv_right_，delta_right )； //右轮移动距离cout ' left : ' delta _ left ' right : ' delta _ right endl； now _=ROS :3360 time :3360 now (； 开始_标志_ (accumulation _ x _=accumulation _ y _=accumulation _ th _=0.0； last_time_=now_； start_flag_=false； 返回； }Delta_time=(now_-last_time_ ).toSec )； double dxy _ ave=(delta _ left delta _ right ) m_pi*wheel_diameter_/) 2.0*200000 )； //dxy_ave是左右轮移动距离的平均值，认为中心点的移动距离doubled th=(delta _ left-delta _ right )/) wheel_track_*20000。 //dth是通过中心点的旋转角度，弧长公式=l/rdoublevxy=dxy _ ave/delta _ time； //V=vxy是作为位移相对于时间的微分的double vth=dth/delta_time； //vth是角速度，表示旋转角度相对于时间的倒数cout ' dxy _ ave : ' dxy _ ave ' DTH 3360 ' DTH endl； doubledx=cos(DTH ) * dxy_ave； //dx中心点处的位移在base_link坐标系xoy轴上的投影doubledy=-sin(DTH ) * dxy_ave； //dy中心点处的位移在base_link坐标系xoy轴上的投影cout 'dx: ' dx ' dy: ' dy endl； cout'cos(accumulation_th_ ) dx-sin ) accumulation _ th _ (dy : ) cos ) accumulation _ th _ (dx-sin )。 accumulation _ x _=(cos (accumulation_th_ ) dx-sin ) accumulation _ th _ ) dy )； base_link坐标系是odom坐标系accumulation _ y _=(sin (accumulation _ th _ ) dxcos(accumulation_th_ ) dy ) )； accumulation_th_ =dth； Gome try _ msgs :3360 quaternion Odom _ quat=TF : createquaternionmsgfromyaw (accumulation _ th )； transform stamped _.header.stamp=ROS :3360 time 3360: now (； transform stamped _.header.frame _ id=Odom _ frame _； transform stamped _.child _ frame _ id=base _ frame _； transform stamped _.transform.translation.x=accumulation _ x _； transform stamped _.transform.translation.y=accumulation _ y _； transform stamped _.transform.translation.z=0.0； //TF 2:四足动物q； //q.se trpy (0，0，accumulation_th_ )； transform stamped _.transform.rotation=Odom _ quat； br _.send transform (transform stamped _； Odom _.header.frame _ id=Odom _ frame _； Odom _.child _ frame _ id=base _ frame _； odom_.header.stamp=now_； Odom _.pose.pose.position.x=accumulation _ x _； Odom _.pose.pose.position.y=accumulation _ y _； odom_.pose.pose.position.z=0； //Odom _.pose.pose.orientation=Odom _ quat； Odom _.pose.pose.orientation.x=Odom _ quat.x； Odom _.pose.pose.orientation.y=Odom _ quat.y； Odom _.pose.pose.orientation.z=Odom _ quat.z； Odom _.pose.pose.orientation.w=Odom _ quat.w； odom_.twist.twist.linear.x=vxy； odom_.twist.twist.linear.y=0； Odom _.twist.twist.angular.z=vth； ODOM_pub_.publish(ODOM_； cout ' accumulation _ x : ' accumulation _ x _ '； accumulation _ y : ' accumulation _ y _ '； accumulation _ th : ' accumulation _ th _ endl； //} last_time_=now_； }
首先求出2个编码器的增量，从当前时刻接收的脉冲数中减去上次时刻接收的脉冲数，则delta_left为左轮的增量，delta_right为右轮的脉冲增量。
cal_pulse(cur_left_，recv_left_，delta_left )； //左轮手枪移动距离cal_pulse(cur_right_，recv_right_，delta_right )； //对右轮移动距离设定初始值，在odom初始时刻x方向为0，y方向为0，旋转角度为0
开始_标志_ (accumulation _ x _=accumulation _ y _=accumulation _ th _=0.0； last_time_=now_； start_flag_=false； 返回； }计算时间间隔，求出速度等信息
elta_time=(now_-last_time_ ).toSec )； 求出左右轮中心点的移动距离，求出两轮车的差动模型v=(VLVR )/2，即在左轮速度上加上右轮速度除以2，
代码中的wheel_diameter为车轮直径，M_PI*wheel_diameter为车轮周长，200000为车轮转一圈，编码器输出的脉冲数，因此M_PI*wheel_diameter 200000是编码器旋转1个脉冲的最后的dxy_ave是根据左编码器增量脉冲和右编码器增量脉冲求出的中心点前进了多少米
double dxy _ ave=(delta _ left delta _ right ) m_pi*wheel_diameter_/) 2.0*200000 )；