ROS麦克纳姆轮底盘制作（下）

麦克纳姆轮底盘制作（下）"/>

ROS麦克纳姆轮底盘制作（下）

4 底盘ROS节点

在上一篇博客中我们实现了利用遥控器控制底盘，接下来我们将编写ROS节点接收”cmd_vel”的话题，将”cmd_vel”话题上的数据转化为底盘每个电机的转速并通过串口下发到STM32板上。同时接收下位机传过来的每个电机的转速或者绝对转角（需要有绝对编码器支持），通过麦克纳姆轮逆运动学模型计算出小车在X、Y、W三个方向的位移。

4.1 接收cmd_vel话题命令

首先进入第一部分：cmd_vel_callback函数为接收”cmd_vel”话题的回调函数，由于小车只在平面运动，因此我们只取X,Y方向的速度以及绕Z轴旋转的角速度。首先更加第2章讲解的麦克纳姆轮运动学模型(式1.6和1.7)将X,Y,Z 三个方向的速度转化到每个轮子的线速度（v1,v2,v3,v4）。得到线速度以后，根据轮子的半径将线速度转化为轮子的角速度（单位，弧度/秒）。由于我们使用的是大疆的M3510电机，电机的减速比为1:19，因此轮子的转速和电机的转速之间相差19倍关系，最后我们得到的是电机转速的单位是（弧度每秒）。

由于电机反馈的转速单位是RPM（转/分钟）,为了方便控制，这里我们将单位转化为RPM发送到STM32板上，两者之间的转换关系为：弧度每秒（rad/s）=2*π*RPM(转/分钟)/60。 这部分的代码我单独挑选出来放在了下方：

（这部分完整的代码见GITHUB：）

void cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{float speed_x,speed_y,speed_w;float v1=0,v2=0,v3=0,v4=0;speed_x = msg->linear.x;speed_y = msg->linear.y;speed_w = msg->angular.z;v1 =speed_x-speed_y-WHEEL_K*speed_w;       //转化为每个轮子的线速度v2 =speed_x+speed_y-WHEEL_K*speed_w;v3 =-(speed_x-speed_y+WHEEL_K*speed_w);v4 =-(speed_x+speed_y+WHEEL_K*speed_w);v1 =v1/(2.0*WHEEL_R*WHEEL_PI);    //转换为轮子的速度(弧度/s)v2 =v2/(2.0*WHEEL_R*WHEEL_PI);v3 =v3/(2.0*WHEEL_R*WHEEL_PI);v4 =v4/(2.0*WHEEL_R*WHEEL_PI);v1 =v1*WHEEL_RATIO*60;    //转换为电机速度　单位　RPMv2 =v2*WHEEL_RATIO*60;v3 =v3*WHEEL_RATIO*60;v4 =v4*WHEEL_RATIO*60;send_rpm_to_chassis(v1,v2,v3,v4);	//串口发送函数
}

注：WHEEL_R 表示麦克纳姆轮的半径

        WHEEL_PI 表示π常数

        WHEEL_RATIO 表示减速度比

        WHEEL_K 则表示以车为中心轮子X，Y坐标的绝对值之和

通过这个函数我们就可以接受”cmd_vel”话题上的数据控制底盘运动了。我们利用ROS自带的一个turtlesim包中获取键盘按键的节点来发布控制数据到”cme_vel”话题上。

我们新开一个终端输入：

rosrun turtlesim turtle_teleop_key

然后再开一个终端启动底盘节点：

rosrun mick_robot mick_robot

以下是一个测试效果：（使用turtlesim的时候需要将话题修改为“/turtle1/cmd_vel”）

4.2 发布里程计数据

完成ROS底盘还需要发布里程计数据，接下来我们通过读取电机的绝对转角，统计每个电机转过的总角度，并换成每个轮子转动的位移。随后通过麦克纳姆轮逆运动学模型（式1.14）计算在X,Y方向的位移，以及航向角的变化。以下是转化的代码：

/** ＠function 利用里程计数据实现位置积分* */
float s1=0,s2=0,s3=0,s4=0;
float s1_last=0,s2_last=0,s3_last=0,s4_last=0;
float position_x=0,position_y=0,position_w=0;
void calculate_position_by_odometry(void)
{//方法１：　　计算每个轮子转动的位移，然后利用Ｆ矩阵合成Ｘ,Y,W三个方向的位移float s1_delta=0,s2_delta=0,s3_delta=0,s4_delta=0;float v1=0,v2=0,v3=0,v4=0;float K4_1 = 1.0/(4.0*WHEEL_K);float position_x_delta,position_y_delta,position_w_delta;float linear_x,linear_y,linear_w;s1_last=s1;s2_last=s2;s3_last=s3;s4_last=s4;//轮子转动的圈数乘以　N*２*pi*rs1 =(moto_chassis[0].round_cnt+(moto_chassis[0].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s2 =(moto_chassis[1].round_cnt+(moto_chassis[1].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s3 =(moto_chassis[2].round_cnt+(moto_chassis[2].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s4 =(moto_chassis[3].round_cnt+(moto_chassis[3].total_angle%8192)/8192.0)/WHEEL_RATIO*WHEEL_PI*WHEEL_D ; s1_delta=s1-s1_last; //每个轮子转速的增量s2_delta=s2-s2_last;s3_delta=s3-s3_last;s4_delta=s4-s4_last;//逆运动学模型转化为X、Y方向的位移以及航向角的变化position_x_delta= 0.25*s1_delta+ 0.25*s2_delta+ 0.25*s3_delta+ 0.25*s4_delta;position_y_delta = -0.25*s1_delta+ 0.25*s2_delta- 0.25*s3_delta+ 0.25*s4_delta;position_w_delta = -K4_1*s1_delta-K4_1*s2_delta+K4_1*s3_delta+ K4_1*s4_delta; //w 单位为弧度//以上电时候的坐标作为里程计的全局坐标 position_x=position_x+cos(position_w)*position_x_delta-sin(position_w)*position_y_delta;position_y=position_y+sin(position_w)*position_x_delta+cos(position_w)*position_y_delta;position_w=position_w+position_w_delta;if(position_w>2*WHEEL_PI){position_w=position_w-2*WHEEL_PI;	}else if(position_w<-2*WHEEL_PI){position_w=position_w+2*WHEEL_PI;}else;v1 =    (moto_chassis[0].speed_rpm)/WHEEL_RATIO/60.0*WHEEL_R *WHEEL_PI*2;v2 =    (moto_chassis[1].speed_rpm)/WHEEL_RATIO/60.0*WHEEL_R *WHEEL_PI*2; v3 =    (moto_chassis[2].speed_rpm)/WHEEL_RATIO/60.0*WHEEL_R *WHEEL_PI*2; v4 =    (moto_chassis[3].speed_rpm)/WHEEL_RATIO/60.0*WHEEL_R *WHEEL_PI*2; linear_x = 0.25*v1+ 0.25*v2+ 0.25*v3+ 0.25*v4;linear_y = -0.25*v1+ 0.25*v2- 0.25*v3+ 0.25*v4;linear_w = -K4_1*v1-K4_1*v2+K4_1*v3+ K4_1*v4;cout<<"position_x:  "<<position_x<<"   position_y: " <<position_y<<"   position_w: " <<position_w*57.3<<endl;cout<<"linear_x:  "<<linear_x<<"   linear_y: " <<linear_y<<"   linear_w: " <<linear_w<<endl<<endl;publish_odomtery( position_x,position_y,position_w,linear_x,linear_y,linear_w);//方法２;利用轮子的转速来推算
}

  函数publish_odomtery(....)的作用是将逆运动学模型计算出来的X，Y坐标以及航向角转换为四元数写入到里程计消息类型中（nav_msgs::Odometry）

void publish_odomtery(float  position_x,float position_y,float oriention,float vel_linear_x,float vel_linear_y,float vel_linear_w)
{static tf::TransformBroadcaster odom_broadcaster;  //定义tf对象geometry_msgs::TransformStamped odom_trans;  //创建一个tf发布需要使用的TransformStamped类型消息geometry_msgs::Quaternion odom_quat;   //四元数变量nav_msgs::Odometry odom;  //定义里程计对象//里程计的偏航角需要转换成四元数才能发布odom_quat = tf::createQuaternionMsgFromYaw(oriention);//将偏航角转换成四元数//载入坐标（tf）变换时间戳odom_trans.header.stamp = ros::Time::now();//发布坐标变换的父子坐标系odom_trans.header.frame_id = "odom";     odom_trans.child_frame_id = "base_link";       //tf位置数据：x,y,z,方向odom_trans.transform.translation.x = position_x;odom_trans.transform.translation.y = position_y;odom_trans.transform.translation.z = 0.0;odom_trans.transform.rotation = odom_quat;        //发布tf坐标变化odom_broadcaster.sendTransform(odom_trans);//载入里程计时间戳odom.header.stamp = ros::Time::now(); //里程计的父子坐标系odom.header.frame_id = "odom";odom.child_frame_id = "base_link";       //里程计位置数据：x,y,z,方向odom.pose.pose.position.x = position_x;     odom.pose.pose.position.y = position_y;odom.pose.pose.position.z = 0.0;odom.pose.pose.orientation = odom_quat;       //载入线速度和角速度odom.twist.twist.linear.x = vel_linear_x;odom.twist.twist.linear.y = vel_linear_y;odom.twist.twist.angular.z = vel_linear_w;    //发布里程计odom_pub.publish(odom);
}

基本上底盘ROS节点的主线就是这样，底盘ROS节点主要有三个必须具备的功能：接收cmd_vel数据、发布里程计、发布TF变换。这里我们只是做了一个简化的版本，后续再慢慢的升级完善。例如所有的参数都是写死在程序里面，后续还需要通过launch文件传递参数，并添加一些底盘校准机制，同时这里我们直接使用的是轮子的转速来推算航向，这个并不准确，最好是利用陀螺仪+罗盘融合来计算航向角会更加的准确。其次由于2D的激光雷达有盲区，我们需要在底盘上增加超声波的接口并通过ROS节点进行发布。以及机器人的URDF模型等，我们将在后续的版本中进行添加。

ROS节点完整的代码放在了GITHUB上，欢迎大家下载提问。

5 自主导航测试

接下来我们按照之前配置导航包的步骤（自主导航小车实践（三））配置gmapping和amcl包，然后进行地图扫描，这里需要注意一点：我们在使用gmapping的时候是需要分布/scan 、/odom 、 /tf(base_link和odom之间的变换)、以及/tf(base_link和laser之间的变换) ,前面三个都是底盘会发布的，最后一个一般是根据URDF加载时候发布的，这里我们还没有写机器人3D模型文件，因此我们需要更加安装位置手动发布激光雷达(laser)和底盘（base_link）之间的变换信息（所谓变换就是两个坐标系之间的额相对旋转和平移）。

 <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0.0 0 0.5 -3.1415926 0 0 base_link laser 50" />

这个参数的传入顺序是（x y z qx qy qz qw frame_id child_frame_id  period_in_ms）最后一个参数是发布的频率。

在配置move_base包的时候需要根据底盘的尺寸修改local_costmap_params.yaml和global_costmap_params.yaml两个中的footprint参数，使之适应新底盘，其次是两个文件中的inflation_radius来指明小车距离边界的安全距离，然后其他参数与之前保持默认即可。对于dwa_local_planner_params.yaml文件中XY方向的加速度和速度参数（acc_lim_x、acc_lim_y,max_vel_x,max_vel_y）进行限制，传统的轮式小车是将Y方向的加速度限制为0的，这里我们需要进行修改。需要修改的参数基本就这些，其他的与之前的博客保持一样就可以了，至少跑起来是没有问题的。

高清视频请移步mick_自主导航-高清观看：

欢迎邮件或留言讨论cenruping@vip.qq