成都网站运营免费咨询身高发育

最近在做AGV改造项目，其中涉及到了odom—>base_link的坐标转换，那么odom即里程计信息主要由伺服电机自带的增量式编码器求取。

void ROSTCP::cal_pulse(int &current , int &receive , int &delta)
{delta = (receive - current);//* M_PI * 0.2 / 200000.0;current = receive;
}
void ROSTCP::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::Time::now();if(start_flag_){accumulation_x_ = accumulation_y_ = accumulation_th_ = 0.0;last_time_ = now_;start_flag_ = false;return; }delta_time = (now_ - last_time_).toSec();double dxy_ave = (delta_left + delta_right) * M_PI * wheel_diameter_/(2.0  * 200000);   // dxy_ave为左右轮的移动距离均值，认为是中心点的移动距离double dth = (delta_left - delta_right) / (wheel_track_ * 200000);      // dth 为通过为中心点的转动角度，通过弧长公式 θ = l / R double vxy = dxy_ave / delta_time;                  // V=vxy 为位移对时间的微分double vth = dth / delta_time;                      // vth 为角速度，为转动角度对时间的倒数cout << "dxy_ave: " << dxy_ave << "dth: " << dth << endl;double dx = cos(dth) * dxy_ave;                     //dx中心点处的位移在base_link坐标系xoy轴上的投影double dy = -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_th_) * dy << endl;accumulation_x_ += (cos(accumulation_th_) * dx - sin(accumulation_th_) * dy); //将base_link坐标系转换到odom坐标系accumulation_y_ += (sin(accumulation_th_) * dx + cos(accumulation_th_) * dy);accumulation_th_ += dth;geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(accumulation_th_);transformStamped_.header.stamp = ros::Time::now();transformStamped_.header.frame_id = odom_frame_;transformStamped_.child_frame_id = base_frame_;transformStamped_.transform.translation.x = accumulation_x_;transformStamped_.transform.translation.y = accumulation_y_;transformStamped_.transform.translation.z = 0.0;//tf2::Quaternion q;//q.setRPY(0, 0, accumulation_th_);transformStamped_.transform.rotation = odom_quat;br_.sendTransform(transformStamped_);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_;
}

首先求取两个编码器的增量，用当前时刻收到的脉冲数减去上一时刻收到的脉冲数，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

if(start_flag_){accumulation_x_ = accumulation_y_ = accumulation_th_ = 0.0;last_time_ = now_;start_flag_ = false;return; }

计算时间间隔，用来求取速度等信息

delta_time = (now_ - last_time_).toSec();

求取左右轮中心点移动距离，根据两轮差速模型v = (vl + vr) / 2，即左轮速度加上右轮速度除2，

代码中wheel_diameter为车轮直径，M_PI*wheel_diameter为轮子周长，200000为轮子走一圈，编码器输出的脉冲数，所以M_PI*wheel_diameter/200000为编码器转动一个脉冲，轮子前进多少米。最后dxy_ave为左编码器增量脉冲与右编码器增量脉冲求取的中心点前进了多少米

double dxy_ave = (delta_left + delta_right) * M_PI * wheel_diameter_/(2.0  * 200000);