原創博客:轉載請標明出處:http://www.cnblogs.com/zxouxuewei/
1.每個人在制作自己的機器人時，都會遇到一個問題就是你的機器人實際移動的數據和發布的里程計數據不匹配（更具體一點是誤差太大），導致機器人的移動出現問題，其中自己總結有以下幾點容易出現問題：
1》機器人在通過usart或者can上傳到pc機上的編碼器或者慣性測量單元的頻率和發布里程計數據的頻率問題。
2》在通過編碼器或者慣性測量單元計算里程計信息時參數誤差太大。
3》就是編碼器輸入捕捉的數據有問題或者慣性測量單元的濾波震盪過大。
2.所以矯正線速度和角速度是必須的。矯正之前首先需要一把卷尺和一張顏色鮮艷的紙（因為你可以在紙上畫線不弄臟地板），
3.進行線速度矯正，將卷尺和紙張如下放置。
安裝完成后開始矯正機器人，
首先確保自己的機器人能夠正常訂閱cmd_val話題下的移動數據，其次能夠讀取下位機透傳的數據。以下是我的機器人ros的啟動代碼的代碼，可供參考：
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <iostream>
#include <iomanip>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <math.h>
#include "string.h"
using namespace std;
using namespace boost::asio;
double x = 0.0;
double y = 0.0;
double th = 0.0;
double vx = 0.0;
double vy = 0.0;
double vth = 0.0;
double dt = 0.0;
//Defines the packet protocol for communication between upper and lower computers
#pragma pack(1)
typedef union _Upload_speed_
{
unsigned char buffer[16];
struct _Speed_data_
{
float Header;
float X_speed;
float Y_speed;
float Z_speed;
}Upload_Speed;
}Struct_Union;
#pragma pack(4)
//Initializes the protocol packet data
Struct_Union Reciver_data = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
Struct_Union Transmission_data = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
//Defines the message type to be transmitted geometry_msgs
geometry_msgs::Quaternion odom_quat;
void cmd_velCallback(const geometry_msgs::Twist &twist_aux)
{
geometry_msgs::Twist twist = twist_aux;
Transmission_data.Upload_Speed.Header = header_data;
Transmission_data.Upload_Speed.X_speed = twist_aux.linear.x;
Transmission_data.Upload_Speed.Y_speed = twist_aux.linear.y;
Transmission_data.Upload_Speed.Z_speed = twist_aux.angular.z;
}
int main(int argc, char** argv)
{
unsigned char check_buf[1];
std::string usart_port,robot_frame_id,smoother_cmd_vel;
int baud_data;
float p,i,d;
ros::init(argc, argv, "base_controller");
ros::NodeHandle n;
ros::Time current_time, last_time;
//Gets the parameters in the launch file
ros::NodeHandle nh_private("~");
nh_private.param<std::string>("usart_port", usart_port, "/dev/robot_base");
nh_private.param<int>("baud_data", baud_data, 115200);
nh_private.param<std::string>("robot_frame_id", robot_frame_id, "base_link");
nh_private.param<std::string>("smoother_cmd_vel", smoother_cmd_vel, "/cmd_vel");
//Create a boot node for the underlying driver layer of the robot base_controller
ros::Subscriber cmd_vel_sub = n.subscribe(smoother_cmd_vel, 50, cmd_velCallback);
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
tf::TransformBroadcaster odom_broadcaster;
//Initializes the data related to the boost serial port
io_service iosev;
serial_port sp(iosev, usart_port);
sp.set_option(serial_port::baud_rate(baud_data));
sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
sp.set_option(serial_port::parity(serial_port::parity::none));
sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
sp.set_option(serial_port::character_size(8));
while(ros::ok())
{
ros::spinOnce();
//Gets the cycle of two time slice rotations. The purpose is to calculate the odom message data
current_time = ros::Time::now();
dt = (current_time - last_time).toSec();
last_time = ros::Time::now();
//Read the data from the lower computer
read(sp, buffer(Reciver_data.buffer));
if(Reciver_data.Upload_Speed.Header > 15.4 && Reciver_data.Upload_Speed.Header < 15.6)
{
vx = Reciver_data.Upload_Speed.X_speed;
vy = Reciver_data.Upload_Speed.Y_speed;
vth = Reciver_data.Upload_Speed.Z_speed;
//ROS_INFO("%2f %2f %2f",vx,vy,vth);
}
else
{
//ROS_INFO("------Please wait while the robot is connected!-----");
read(sp, buffer(check_buf));
}
//Send the next bit machine under the cmd_val topic to monitor the speed information
write(sp, buffer(Transmission_data.buffer,16));
//Calculate odometer data
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += delta_th;
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = robot_frame_id;
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
odom_broadcaster.sendTransform(odom_trans);
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
odom.child_frame_id = robot_frame_id;
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;
//Release odometer data between odom-> base_link
odom_pub.publish(odom);
}
iosev.run();
}
然后打開線速度校准節點:
rosrun rbx1_nav calibrate_linear.py
rosrun rqt_reconfigure rqt_reconfigure
點擊start_test開始測試機器人，測試距離為一米，看實際機器人走了多少，記錄下實際移動的值除以目標距離1米，然后用計算出來的值去乘以odom_linear_scale_correction的值，並且修改odom_linear_scale_correction為最終計算出來的值，經過反復多次測試后，你會看到你的機器人非常准確的走了1m。然后用這個最終測試的精確值去修改發布里程計數據時計算的速度，具體是用你的ticks_meter處以odom_linear_scale_correction的值為最終ticks_meter的值。此時線速度矯正到此結束。
4.以同樣的方式去矯正角速度：
rosrun rbx1_nav calibrate_angular.py
rosrun rqt_reconfigure rqt_reconfigure
測試完成后得到odom_angular_scale_correction新的值。然后去修改你的軸距，用你的base_width除以odom_angular_scale_correction得到矯正后的軸距。以下是我矯正后的代碼。(下位機的代碼，確保自己的PID控制器量綱為速度，單位保持統一。階躍超調較小，恢復快，就是PID參數要整定好)
struct Encoder_{ float d_left; float d_right;int enc_left; //wheel encoder readings int enc_right; int left; // actual values coming back from robot int right; }self; float d,th; #define ticks_meter 123077.0 //每米編碼器的值 linear = 2.6 #define base_width 0.31f; //軸距 angule = 0.316
#define robot_timer 0.53f //周期 union Max_Value { unsigned char buf[12]; struct _Float_{
float hander; float _float_vx; float _float_vth; }Float_RAM; }Send_Data; extern int encoder_0;extern int encoder_1; void Updata_velocities_Data() {
u8 i=0; self.right = encoder_0; self.left = encoder_1; if(self.enc_left == 0) { self.d_left = 0; self.d_right = 0; } else { self.d_left = (self.left - self.enc_left) / ticks_meter; self.d_right = (self.right - self.enc_right) / ticks_meter; } self.enc_left = self.left; self.enc_right = self.right; d = ( self.d_left + self.d_right ) / 2; //distance traveled is the average of the two wheels th = ( self.d_right - self.d_left ) / base_width; //this approximation works (in radians) for small angles self.dx = d / robot_timer; //calculate velocities self.dr = th / robot_timer;
Send_Data.Float_RAM.hander = 15.5; Send_Data.Float_RAM._float_vx = self.dx; Send_Data.Float_RAM._float_vth = self.dr; for(i=0;i<12;i++)
sendchar_usart1(Send_Data.buf[i]); }