网站备案升级,完整网站源码asp,wordpress 主页插件,西安自动seo最近在做AGV改造项目#xff0c;其中涉及到了odom—base_link的坐标转换#xff0c;那么odom即里程计信息主要由伺服电机自带的增量式编码器求取。
void ROSTCP::cal_pulse(int current , int receive , int delta)
{delta (receive - current);//* M_P…最近在做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; // Vvxy 为位移对时间的微分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_);coutaccumulation_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方向移动0y方向移动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);
