机器人制作开源方案 | 桌面级全向底盘--slam导航

机器谱

       机器人的智能移动可应用于物流行业、交通行业，本节内容我们将利用键盘控制全向底盘运动完成slam建图，并能在已建好的地图里进行自主导航。

1. 键盘控制底盘运动
实现思路
       按下键盘上指定的键，实现全向底盘前进、后退、转向、平移；还可以灵活的设置底盘的角速度、线速度来调整底盘的运动。下面是本实验中规划的控制全向底盘的键盘命令（如下表所示），大家可以先熟悉一下，稍后会在控制底盘时用到。

键盘命令及含义表

分类	运动指令
	键盘快捷键	指令含义
基本运动	i	前进
	，	后退
	j	左转
	l	右转
平移	J	左平移
	L	右平移
调整角速度与线速度	q	增大底盘最大速度的10%（包含角速度与线速度）
	z	减小底盘最大速度的10%（包含角速度与线速度）
调整线速度	w	仅仅增大底盘线速度的10%
	x	仅仅减小底盘线速度的10%
调整角速度	e	仅仅增大底盘角速度的10%
	c	仅仅减小底盘角速度的10%
	除了上面按键之外的其它按键（如：k）	停止
	Ctrl+c	程序结束

操作步骤

① 下载文末资料中的参考程序fourmacnum_car_ws\src\Arduino_Program\black_handle_BaseCar\black_handle_BaseCar.ino：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-08-22 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. //DUE控制版需要启用USE_USBON或USE_NATIVE_USB,UNO不需要
    
14. 
    
15. //#define USE_USBCON          //PAOGRAMMING PORT
    
16. 
    
17. //#define USE_NATIVE_USB      //NATIVE USB PORT
    
18. 
    
19. #define ActionDelayTimes 1500
    
20. 
    
21. //#include <SoftwareSerial.h>
    
22. 
    
23. #include <ros.h>
    
24. 
    
25. #include <ros/time.h>
    
26. 
    
27. //#include<ServoTimer2.h>
    
28. 
    
29. #include <geometry_msgs/Vector3.h>
    
30. 
    
31. #define mySerial Serial2
    
32. 
    
33. #define BusServoSerialBaud 115200
    
34. 
    
35. ros::NodeHandle   nh;
    
36. 
    
37. void XYRun(double vx, double vy, double w);
    
38. 
    
39. void messageCb( const geometry_msgs::Vector3& vel_cmd)   {XYSetVel(vel_cmd.x, vel_cmd.y, vel_cmd.z);}
    
40. 
    
41. ros::Subscriber<geometry_msgs::Vector3> vel_cmd_sub("vel_cmd", &messageCb );
    
42. 
    
43. geometry_msgs::Vector3 pose_message;
    
44. 
    
45. ros::Publisher pose_feedback_pub("pose_feedback",&pose_message);
    
46. 
    
47. geometry_msgs::Vector3 vel_message;
    
48. 
    
49. ros::Publisher vel_feedback_pub("vel_feedback",&vel_message);
    
50. 
    
51. //ServoTimer2 myServo[4];//statement servo
    
52. 
    
53. //int servo_port[4] = {10, 11, 12, 13};//x,y,z,a
    
54. 
    
55. //float value_init[4] = {88, 88, 96, 86};//x,y,z,a
    
56. 
    
57. const int kMessagePubRate = 5;
    
58. 
    
59. unsigned long message_pub_time = 0;
    
60. 
    
61. const int kReadMotorDeltaT = 5;
    
62. 
    
63. unsigned long position_read_time = 0;
    
64. 
    
65. float current_x = 0,current_y = 0,current_a = 0;
    
66. 
    
67. float current_vx = 0,current_vy = 0,current_va = 0;
    
68. 
    
69. float wheel_Speed[4]={0,0,0,0};
    
70. 
    
71. char cmd_return[200];
    
72. 
    
73. float sudu = 0.87;
    
74. 
    
75. //SoftwareSerial mySerial(51, 9);
    
76. 
    
77. void setup()
    
78. 
    
79. {
    
80. 
    
81.   delay(1000);
    
82. 
    
83.   Serial.begin(57600);delay(1000);
    
84. 
    
85.   mySerial.begin(BusServoSerialBaud);delay(1000);
    
86. 
    
87.   //Servo_Action_Test();delay(1500);  
    
88. 
    
89.   nh.initNode();
    
90. 
    
91.   nh.subscribe(vel_cmd_sub);
    
92. 
    
93.   nh.advertise(pose_feedback_pub);
    
94. 
    
95.   nh.advertise(vel_feedback_pub);
    
96. 
    
97.   XYSetVel(0.0,0.0,0.0);
    
98. 
    
99.   position_read_time = millis();
    
100. 
     
101.   message_pub_time = millis();
     
102. 
     
103. }
     
104. 
     
105. void loop()
     
106. 
     
107. {
     
108. 
     
109. //   unsigned long times = millis();
     
110. 
     
111. //   set_Single_servo_Vel(1600);delay(5000);
     
112. 
     
113. //   Serial.println(millis()-times);
     
114. 
     
115. //   set_Single_servo_Vel(1500);
     
116. 
     
117. //   while(1){
     
118. 
     
119. //    delay(100);
     
120. 
     
121. //   }
     
122. 
     
123.   if(millis()>position_read_time)
     
124. 
     
125.   {  
     
126. 
     
127.     XYread();
     
128. 
     
129.     position_read_time+=kReadMotorDeltaT;
     
130. 
     
131.   }  
     
132. 
     
133. 
     
134. 
     
135.   if(millis()>message_pub_time)
     
136. 
     
137.   {
     
138. 
     
139.     pose_message.x = current_x;
     
140. 
     
141.     pose_message.y = current_y;
     
142. 
     
143.     pose_message.z = current_a;
     
144. 
     
145.     vel_message.x = current_vx;
     
146. 
     
147.     vel_message.y = current_vy;
     
148. 
     
149.     vel_message.z = current_va;
     
150. 
     
151.     pose_feedback_pub.publish(&pose_message);
     
152. 
     
153.     vel_feedback_pub.publish(&vel_message);
     
154. 
     
155.     message_pub_time+=1000.0/kMessagePubRate;
     
156. 
     
157.   }
     
158. 
     
159.   nh.spinOnce();
     
160. 
     
161. }
     
162. 
     
163. 
     

复制代码

② 启动雷达、键盘、rosserial程序包。打开终端,输入roslaunch robot_navigation_control robot.launch命令（见下图），等待程序的运行启动界面。

成功启动后，可以在终端看到底盘的当前speed（线速度）为0.04，turn（角速度）为0.08。

注意：speed指的是线速度的大小（包含方向），turn指的是角速度的大小（包含方向）。

启动后的界面（如下图所示）:

③ 尝试按下键盘命令，控制底盘的运动。 (注意：请先保证终端是激活状态；按键盘时，稍微有点间隔，给底盘转动的时间)。 假如现在希望先增加底盘的最大速度，再降低底盘的最大速度，则分别按下键盘上q、z命令，则可以观察到终端的结果（如下图所示）。更多命令大家可自行尝试。

2. 底盘同步定位与建图-SLAM实现思路

       利用Gmapping算法对底盘所在的未知环境进行建立地图、同步定位、最后保存此地图。此地图可供后续底盘导航使用。

器材准备

操作步骤

① 下载文末资料中的参考程序fourmacnum_car_ws\src\Arduino_Program\black_handle_BaseCar\black_handle_BaseCar.ino：

1. /*------------------------------------------------------------------------------------
   
2. 
   
3.   版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
   
4. 
   
5.            Distributed under MIT license.See file LICENSE for detail or copy at
   
6. 
   
7.            https://opensource.org/licenses/MIT
   
8. 
   
9.            by 机器谱 2023-08-22 https://www.robotway.com/
   
10. 
    
11.   ------------------------------*/
    
12. 
    
13. //DUE控制版需要启用USE_USBON或USE_NATIVE_USB,UNO不需要
    
14. 
    
15. //#define USE_USBCON          //PAOGRAMMING PORT
    
16. 
    
17. //#define USE_NATIVE_USB      //NATIVE USB PORT
    
18. 
    
19. #define ActionDelayTimes 1500
    
20. 
    
21. //#include <SoftwareSerial.h>
    
22. 
    
23. #include <ros.h>
    
24. 
    
25. #include <ros/time.h>
    
26. 
    
27. //#include<ServoTimer2.h>
    
28. 
    
29. #include <geometry_msgs/Vector3.h>
    
30. 
    
31. #define mySerial Serial2
    
32. 
    
33. #define BusServoSerialBaud 115200
    
34. 
    
35. ros::NodeHandle   nh;
    
36. 
    
37. void XYRun(double vx, double vy, double w);
    
38. 
    
39. void messageCb( const geometry_msgs::Vector3& vel_cmd)   {XYSetVel(vel_cmd.x, vel_cmd.y, vel_cmd.z);}
    
40. 
    
41. ros::Subscriber<geometry_msgs::Vector3> vel_cmd_sub("vel_cmd", &messageCb );
    
42. 
    
43. geometry_msgs::Vector3 pose_message;
    
44. 
    
45. ros::Publisher pose_feedback_pub("pose_feedback",&pose_message);
    
46. 
    
47. geometry_msgs::Vector3 vel_message;
    
48. 
    
49. ros::Publisher vel_feedback_pub("vel_feedback",&vel_message);
    
50. 
    
51. //ServoTimer2 myServo[4];//statement servo
    
52. 
    
53. //int servo_port[4] = {10, 11, 12, 13};//x,y,z,a
    
54. 
    
55. //float value_init[4] = {88, 88, 96, 86};//x,y,z,a
    
56. 
    
57. const int kMessagePubRate = 5;
    
58. 
    
59. unsigned long message_pub_time = 0;
    
60. 
    
61. const int kReadMotorDeltaT = 5;
    
62. 
    
63. unsigned long position_read_time = 0;
    
64. 
    
65. float current_x = 0,current_y = 0,current_a = 0;
    
66. 
    
67. float current_vx = 0,current_vy = 0,current_va = 0;
    
68. 
    
69. float wheel_Speed[4]={0,0,0,0};
    
70. 
    
71. char cmd_return[200];
    
72. 
    
73. float sudu = 0.87;
    
74. 
    
75. //SoftwareSerial mySerial(51, 9);
    
76. 
    
77. void setup()
    
78. 
    
79. {
    
80. 
    
81.   delay(1000);
    
82. 
    
83.   Serial.begin(57600);delay(1000);
    
84. 
    
85.   mySerial.begin(BusServoSerialBaud);delay(1000);
    
86. 
    
87.   //Servo_Action_Test();delay(1500);  
    
88. 
    
89.   nh.initNode();
    
90. 
    
91.   nh.subscribe(vel_cmd_sub);
    
92. 
    
93.   nh.advertise(pose_feedback_pub);
    
94. 
    
95.   nh.advertise(vel_feedback_pub);
    
96. 
    
97.   XYSetVel(0.0,0.0,0.0);
    
98. 
    
99.   position_read_time = millis();
    
100. 
     
101.   message_pub_time = millis();
     
102. 
     
103. }
     
104. 
     
105. void loop()
     
106. 
     
107. {
     
108. 
     
109. //   unsigned long times = millis();
     
110. 
     
111. //   set_Single_servo_Vel(1600);delay(5000);
     
112. 
     
113. //   Serial.println(millis()-times);
     
114. 
     
115. //   set_Single_servo_Vel(1500);
     
116. 
     
117. //   while(1){
     
118. 
     
119. //    delay(100);
     
120. 
     
121. //   }
     
122. 
     
123.   if(millis()>position_read_time)
     
124. 
     
125.   {  
     
126. 
     
127.     XYread();
     
128. 
     
129.     position_read_time+=kReadMotorDeltaT;
     
130. 
     
131.   }  
     
132. 
     
133. 
     
134. 
     
135.   if(millis()>message_pub_time)
     
136. 
     
137.   {
     
138. 
     
139.     pose_message.x = current_x;
     
140. 
     
141.     pose_message.y = current_y;
     
142. 
     
143.     pose_message.z = current_a;
     
144. 
     
145.     vel_message.x = current_vx;
     
146. 
     
147.     vel_message.y = current_vy;
     
148. 
     
149.     vel_message.z = current_va;
     
150. 
     
151.     pose_feedback_pub.publish(&pose_message);
     
152. 
     
153.     vel_feedback_pub.publish(&vel_message);
     
154. 
     
155.     message_pub_time+=1000.0/kMessagePubRate;
     
156. 
     
157.   }
     
158. 
     
159.   nh.spinOnce();
     
160. 
     
161. }

复制代码

② 启动雷达、键盘、rosserial。
        打开终端并输入命令：roslaunch robot_navigation_control robot_car.launch，见下图。

③ 启动构建地图服务。
        重新打开新终端并输入：roslaunch four_macnum_slam four_macnum_slam.launch，见下图。

       界面启动后，在rviz中选择“map”，可以在可视化界面看到有地图出现。
       下面根据实际环境进行建图：按下键盘相关指令（键盘指令请参考上述内容--键盘命令及含义表）来控制全向底盘在场地内遍地运行，直至在Rviz内可以看到构建地图的轮廓，如下图所示（注意实际地图环境不同，出现的轮廓也会不同）。

底盘在未知环境中建立地图

④ 为了后续在已知环境中运行底盘，需把此次建立的地图进行保存。需要先进入保存地图的文件夹，然后给地图命名，保存即可。
下面是实验中的具体操作：
重新打开新终端（ctrl+shift+t）并输入：cd fourmacnum_car_ws/src/four_macnum_navigation/maps

之后输入：rosrun map_server map_saver -f ./your_map_name

返回下图红色框的信息，代表成功保存地图。

查看生成的地图文件
       上面我们用map_server来保存地图，这里我们来简单了解和查看一下刚才保存的地图文件（见下图）。其中：map_server是一个和地图相关的功能包，它可以将已知地图发布出来，供导航和其它功能使用，也可以保存SLAM建立的地图。
       地图文件，通常为pgm格式   
       地图的描述文件，通常为yaml格式

打开your_map_name.pgm（见下图）：

打开your_map_name.yaml ，各个参数如下图所示：

其中占据的概率 occ = (255-color_avg)/255.0，color_avg为RGB三个通道的平均值。

3. 底盘导航—Navigation
实现思路
       加载上述实验中建好的地图，全向底盘进行姿态估计并实现导航效果。
操作步骤
① 将创建的地图应用到导航程序中。这里我们需要修改下four_macnum_navigation.launch文件,将your_map_name.yaml 添加到此launch文件中。步骤如下：
        打开终端并输入：cd fourmacnum_car_ws/src/four_macnum_navigation/launch
        之后再输入：gedit four_macnum_navigation.launch

② 启动雷达、键盘、rosserial。
        在该终端继续输入：roslaunch robot_navigation_control test_one.launch
③ 打开新终端并输入：roslaunch four_macnum_navigation four_macnum_navigation.launch（见下图）。

启动界面后，可以看到许多红色的箭头，这代表底盘的姿态估计还不准确。

④ 开始调整底盘的姿态。
        使用2D Pose Estimate 标定底盘位于地图中的初始位置及车头指向（见下图）。

⑤ 鼠标点击有roslaunch robot_navigation_control test_one.launch的终端后，尝试按下键盘命令控制全向底盘运动（键盘指令请参考上述内容--键盘命令及含义表），最好尽可能多的消除地图中的箭头【这一步作用是确定底盘在地图中的实际位置以及车头指向】。
        下面是多次按下键盘命令控制底盘运动后，箭头逐渐减少的效果。

下图是最后调整完的底盘姿态（即底盘在地图中的实际位置以及车头指向）。

⑥ 尝试给定全向底盘目标位置。
【使用2D Nav Goal】，这里的目标位置包含了目标点及车头指向，在可视化界面点击且转动箭头防线，机器人会移动到指定点的位置。



4. 资料下载
资料内容：程序源代码
资料下载地址：桌面级全向底盘-slam导航 https://www.robotway.com/h-col-260.html