一、基本思想
- 室外导航与室内导航的差距在于没有提前建好的地图，定位困难，这里我们应用robot_localization包作为定位的方法，robot_localization的优势就在于多个传感器数据的融合，包括imu、odom、GPS。
- 对于定位困难问题，robot_localization的功能是efk滤波和数据融合，对于GPS信号：navsat_transform_node包就是专门融合GPS信号的。
如上图所示，navsat_transform_node节点的输入有3个：
__nav_msgs/Odometry__ (EKF输出，需要机器人当前的位置)
__sensor_msgs/Imu__ (必须有陀螺仪，需要确定全局朝向)
__sensor_msgs/NavSatFix__ (从导航卫星设备输出)
处理步骤是：
- 将gps数据转换成UTM坐标。UTM（Universal Transverse Mercator，通用横轴墨卡托）是一种通用直角坐标系，是用来标识地图常用的一种标准投影坐标系。某个特定的GPS数据都能对应到某个唯一的UTM坐标，这两者存在一一对应关系（严格说来，需要明确Z轴数据才会一一对应）。因此这种转换是确定的、容易的。
- 使用初始的UTM坐标，EKF/UKF输出和IMU生成从UTM网格到机器人世界框架的（静态）变换T。也就是说，UTM坐标原点在静态坐标系（常常为map）的方位表示。这两个坐标系都是静止的，因此T也是固定不变的。这个T的生成需要用到实时的机器人位姿信息odometry以及动态连续的IMU数据来提供对机器人全局位姿信息。
- 使用T变换所有测量的gps数据。
- 将数据发给EKF/UKF。
- 对于没有地图的问题，这里我们是通过加载一个自定义的空白地图作为全局地图，然后融合move_base进行全局路径规划和局部路径规划。我们做的其实就是配置数据融合节点，和加载自定义空白地图，让其在室外进行全局路径规划和局部路径规划。
二、husky机器人仿真环境模拟
1. 在github上下载husky的melodic版本
git clone -b melodic-devel https://github.com/husky/husky.git
-b <分支名> 的含义是克隆指定的分支，这里我们克隆melodic版本的husky
2.创建室外导航功能包
- 在工作空间src文件夹下创建outdoor_waypoint_nav功能包
catkin_create_pkg outdoor_waypoint_nav roscpp rospy std_msgs tf roslib roslaunch robot_localization
- 创建文件夹存放文件
mkdir launch&&mkdir params rviz_config
- 创建launch,为了方便理解，我们先从最外层的launch文件开始
__ outdoor_waypoint_nav_sim.launch __
<?xml version="1.0"?>
<launch>
<!-- 启动Gazebo仿真环境 -->
<include file="$(find husky_gazebo)/launch/husky_empty_world.launch">
<arg name="world_name" value="$(find husky_gazebo)/worlds/clearpath_playpen.world"/>
<!--<arg name="world_name" value="worlds/empty.world"/>-->
</include>
<!-- 启动Joystick控制 -->
<include file="$(find husky_control)/launch/teleop.launch" />
<!-- Map server -->
<node pkg="map_server" type="map_server" name="stdr_load_home_map" args="$(find husky_navigation)/maps/playpen_map.yaml" />
<!-- 启动gmapping进行地图创建 -->
<!-- <include file="$(find husky_navigation)/launch/gmapping.launch" /> -->
<!-- 启动robot_localization和navsat节点 -->
<include file="$(find outdoor_waypoint_nav)/launch/include/localization_run_sim.launch"/>
<!-- 启动rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find outdoor_waypoint_nav)/rviz_config/model.rviz" />
<!-- 启动move_base进行导航 -->
<include file="$(find husky_navigation)/launch/move_base_nav.launch" />
<!-- 启动安全节点，用于禁用机器人速度命令 -->
<!-- <include file="$(find outdoor_waypoint_nav)/launch/include/safety_node.launch" /> -->
</launch>
在这个launch文件中，我们实现了：
- husky机器人和仿真环境的加载
- 机器人的键盘控制节点
- 加载地图服务
- 启动传感器数据融合节点
- 启动rviz
- 启动move_base进行导航
- 最后节点的注释掉是为了便于理解，在仿真中我们可以忽略这个节点。
- 数据融合节点launch文件
localization_run_sim.launch
<?xml version="1.0"?>
<launch>
<group ns="outdoor_waypoint_nav">
<!-- 加载参数文件 -->
<rosparam command="load" file="$(find outdoor_waypoint_nav)/params/ekf_params.yaml" />
<rosparam command="load" file="$(find outdoor_waypoint_nav)/params/navsat_params_sim.yaml" />
<!-- 启动第一个扩展卡尔曼滤波器节点（用于里程计数据） -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
<!-- 启动第二个扩展卡尔曼滤波器节点（用于地图数据） -->
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
<remap from="odometry/filtered" to="odometry/filtered_map"/>
</node>
<!-- 启动 navsat_transform_node 节点 -->
<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true" output="screen" >
<remap from="odometry/filtered" to="odometry/filtered_map"/>
<remap from="gps/fix" to="/navsat/fix"/>
<remap from="imu/data" to="/imu/data"/>
</node>
</group>
</launch>
这个文件配置了三个 robot_localization 包中的节点，它们共同工作以实现扩展卡尔曼滤波（EKF）和 GPS 数据的融合。
可以启动和配置用于机器人导航和本地化的多个节点，实现传感器数据融合和位姿估计。
- 数据融合launch文件中的节点的参数配置文件,我们将它们放在params文件夹中
ekf_params.yaml
ekf_se_odom:
frequency: 10 # 发布频率，设置为10 Hz
sensor_timeout: 0.1 # 传感器超时，设置为0.1秒
two_d_mode: true # 是否使用二维模式，设置为true
transform_time_offset: 0.0 # 变换时间偏移，设置为0.0秒
transform_timeout: 0.0 # 变换超时，设置为0.0秒
print_diagnostics: true # 是否打印诊断信息，设置为true
debug: false # 是否启用调试模式，设置为false
map_frame: map # 地图帧，设置为map
odom_frame: odom # 里程计帧，设置为odom
base_link_frame: base_link # 基础连接帧，设置为base_link
world_frame: odom # 世界帧，设置为odom
odom0: /husky_velocity_controller/odom # 轮式里程计数据的主题，设置为/husky_velocity_controller/odom
odom0_config: [true, true, false,
false, false, true,
false, false, true,
false, false, false,
false, false, true] # 配置里程计数据的使用维度，具体的布尔值表示是否使用该维度的数据
odom0_queue_size: 10 # 队列大小，设置为10
odom0_nodelay: true # 是否无延迟，设置为true
odom0_differential: false # 是否差分计算，设置为false
odom0_relative: false # 是否相对计算，设置为false
imu0: /imu/data # IMU数据的主题，设置为/imu/data
imu0_config: [false, false, false,
true, true, false,
false, false, false,
true, true, true,
false, false, false] # 配置IMU数据的使用维度
imu0_nodelay: false # 是否无延迟，设置为false
imu0_differential: false # 是否差分计算，设置为false
imu0_relative: false # 是否相对计算，设置为false
imu0_queue_size: 10 # 队列大小，设置为10
imu0_remove_gravitational_acceleration: true # 是否移除重力加速度，设置为true
use_control: false # 是否使用控制输入，设置为false
process_noise_covariance: [0.05, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0,
0, 0, 0.05, 0, 0, 0,
0, 0, 0, 0.1, 0, 0,
0, 0, 0, 0, 0.1, 0,
0, 0, 0, 0, 0, 0.1] # 过程噪声协方差矩阵
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 1e-9] # 初始估计协方差矩阵
ekf_se_map:
frequency: 30 # 发布频率，设置为30 Hz
sensor_timeout: 0.1 # 传感器超时，设置为0.1秒
two_d_mode: true # 是否使用二维模式，设置为true
transform_time_offset: 0.0 # 变换时间偏移，设置为0.0秒
transform_timeout: 0.0 # 变换超时，设置为0.0秒
print_diagnostics: true # 是否打印诊断信息，设置为true
debug: false # 是否启用调试模式，设置为false
map_frame: map # 地图帧，设置为map
odom_frame: odom # 里程计帧，设置为odom
base_link_frame: base_link # 基础连接帧，设置为base_link
world_frame: map # 世界帧，设置为map
odom0: /husky_velocity_controller/odom # 轮式里程计数据的主题，设置为/husky_velocity_controller/odom
odom0_config: [false, false, false,
false, false, true,
false, false, false,
false, false, false,
false, false, true] # 配置里程计数据的使用维度
odom0_queue_size: 10 # 队列大小，设置为10
odom0_nodelay: true # 是否无延迟，设置为true
odom0_differential: false # 是否差分计算，设置为false
odom0_relative: false # 是否相对计算，设置为false
odom1: /outdoor_waypoint_nav/odometry/gps # GPS里程计数据的主题，设置为/outdoor_waypoint_nav/odometry/gps
odom1_config: [true, true, true,
false, false, false,
false, false, false,
false, false, false,
true, true, true] # 配置GPS数据的使用维度
odom1_queue_size: 10 # 队列大小，设置为10
odom1_nodelay: true # 是否无延迟，设置为true
odom1_differential: false # 是否差分计算，设置为false
odom1_relative: false # 是否相对计算，设置为false
imu0: /imu/data # IMU数据的主题，设置为/imu/data
imu0_config: [false, false, false,
true, true, false,
false, false, false,
true, true, true,
false, false, false] # 配置IMU数据的使用维度
imu0_nodelay: false # 是否无延迟，设置为false
imu0_differential: false # 是否差分计算，设置为false
imu0_relative: false # 是否相对计算，设置为false
imu0_queue_size: 10 # 队列大小，设置为10
imu0_remove_gravitational_acceleration: true # 是否移除重力加速度，设置为true
use_control: false # 是否使用控制输入，设置为false
process_noise_covariance: [0.05, 0, 0, 0, 0, 0,
0, 0.05, 0, 0, 0, 0,
0, 0, 0.05, 0, 0, 0,
0, 0, 0, 0.1, 0, 0,
0, 0, 0, 0, 0.1, 0,
0, 0, 0, 0, 0, 0.1] # 过程噪声协方差矩阵
initial_estimate_covariance: [1e-9, 0, 0, 0, 0, 0,
0, 1e-9, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0,
0, 0, 0, 1e-9, 0, 0,
0, 0, 0, 0, 1e-9, 0,
0, 0, 0, 0, 0, 1e-9] # 初始估计协方差矩阵
这个配置文件主要用于机器人导航中的定位部分，结合多个传感器（IMU、里程计、GPS），通过扩展卡尔曼滤波器提供精确的位置估计。
navsat_params_sim.yaml
navsat_transform:
frequency: 30 # 发布频率，设置为30 Hz
delay: 3.0 # 延迟时间，设置为3.0秒
magnetic_declination_radians: 0.00 # 磁偏角，设置为0.00弧度。原注释: 0.168285 # 对应于纬度/经度 43.500718, -80.546638，日期为2017年6月1日
yaw_offset: 1.570796327 # 偏航角偏移，设置为1.570796327弧度。原注释: IMU在朝向磁北时读数为0，而不是东
zero_altitude: true # 是否将高度设置为零，设置为true
broadcast_utm_transform: true # 是否广播UTM变换，设置为true
publish_filtered_gps: true # 是否发布过滤后的GPS数据，设置为true
use_odometry_yaw: false # 是否使用里程计的偏航角，设置为false
wait_for_datum: false # 是否等待基准数据，设置为false
这个配置文件，主要是GPS坐标转换的一些参数。
- 还有一个rviz的配置文件方便更好的查看,注意放到rviz_config文件夹下面。
model.rviz
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /LaserScan1
Splitter Ratio: 0.5
Tree Height: 647
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: LaserScan
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
base_laser:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_laser_mount:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_bumper_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_left_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
front_right_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
imu_link:
Alpha: 1
Show Axes: false
Show Trail: false
inertial_link:
Alpha: 1
Show Axes: false
Show Trail: false
rear_bumper_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
rear_left_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
rear_right_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
top_chassis_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
top_plate_front_link:
Alpha: 1
Show Axes: false
Show Trail: false
top_plate_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
top_plate_rear_link:
Alpha: 1
Show Axes: false
Show Trail: false
user_rail_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/LaserScan
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Flat Squares
Topic: /scan
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/Axes
Enabled: true
Length: 0.5
Name: Map Coord Frame
Radius: 0.009999999776482582
Reference Frame: map
Value: true
- Class: rviz/Axes
Enabled: true
Length: 0.10000000149011612
Name: GPS Coord Frame
Radius: 0.009999999776482582
Reference Frame: GPS_link
Value: true
- Class: rviz/Axes
Enabled: true
Length: 0.10000000149011612
Name: Laser Coord Frame
Radius: 0.009999999776482582
Reference Frame: base_laser
Value: true
- Class: rviz/Axes
Enabled: true
Length: 0.5
Name: Base Coord Frame
Radius: 0.009999999776482582
Reference Frame: base_link
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1000
Name: Raw Wheel Odom
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic: /husky_velocity_controller/odom
Unreliable: false
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 100
Name: GPS Odom
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic: /outdoor_waypoint_nav/odometry/gps
Unreliable: false
Value: true
- Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Alpha: 0.5
Color: 255; 255; 127
Color Style: Unique
Frame: Local
Offset: 1
Scale: 1
Value: true
Position:
Alpha: 0.30000001192092896
Color: 204; 51; 204
Scale: 1
Value: true
Value: true
Enabled: true
Keep: 1000
Name: EKF Filtered Odom
Position Tolerance: 0.10000000149011612
Shape:
Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Color: 255; 25; 0
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Value: Arrow
Topic: /outdoor_waypoint_nav/odometry/filtered_map
Unreliable: false
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Map - Gmapping
Topic: /outdoor_waypoint_nav/map
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 85; 255
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Path Plan
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /move_base/TrajectoryPlannerROS/global_plan
Unreliable: false
Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Class: rviz/Pose
Color: 255; 25; 0
Enabled: true
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Name: Current Goal
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Shape: Arrow
Topic: /move_base/current_goal
Unreliable: false
Value: true
- Alpha: 1
Class: rviz/Polygon
Color: 25; 255; 0
Enabled: true
Name: Robot Footprint
Topic: /move_base/global_costmap/footprint
Unreliable: false
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Local Costmap
Topic: /move_base/local_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
Draw Behind: false
Enabled: true
Name: Global Costmap
Topic: /move_base/global_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/MoveCamera
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/Select
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 28.67530059814453
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.6422230005264282
Y: -0.2260349988937378
Z: 0.9989690184593201
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.9204000234603882
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 6.278570175170898
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 876
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000018c00000312fc0200000005fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000312000000c900ffffff000000010000010f000003dafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000003da000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000046d0000031200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1535
X: 232
Y: 174
- 还有一个move_base的launch文件需要我们添加。
转到src/husky-melodic-devel/husky_navigation/launch文件夹下面.添加：
move_base_nav.launch
<?xml version="1.0"?>
<launch>
<!-- 启动move_base节点 -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base"> <!--output="screen"-->
<!-- 加载全局和局部代价地图的公共参数 -->
<rosparam file="$(find husky_navigation)/params/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find husky_navigation)/params/costmap_common_params.yaml" command="load" ns="local_costmap" />
<!-- 加载局部代价地图参数 -->
<rosparam file="$(find husky_navigation)/params/local_costmap_params.yaml" command="load" />
<!-- 加载全局代价地图参数 -->
<rosparam file="$(find husky_navigation)/params/global_costmap_params.yaml" command="load" />
<!-- 加载局部规划器参数 -->
<rosparam file="$(find husky_navigation)/params/base_local_planner_params.yaml" command="load" />
<!-- 加载move_base参数 -->
<rosparam file="$(find husky_navigation)/params/move_base_params.yaml" command="load" />
<!-- 设置全局规划器 -->
<param name="base_global_planner" type="string" value="navfn/NavfnROS" />
<!-- 设置局部规划器 -->
<param name="base_local_planner" value="base_local_planner/TrajectoryPlannerROS" />
<!-- 重映射里程计数据的主题 -->
<remap from="odom" to="/outdoor_waypoint_nav/odometry/filtered_map" />
<!-- 重映射速度控制命令的主题 -->
<remap from="cmd_vel" to="/cmd_vel_intermediate" />
<!--
重映射地图数据的主题（可选）
<remap from="/map" to="/outdoor_waypoint_nav/map" />
-->
</node>
</launch>
3.空白地图的创建
在src/husky-melodic-devel/husky_navigation中已经有了一个地图的文件，playpen_map.yaml和playpen_map.pgm,yaml中是关于地图的一些参数设置
playpen_map.yaml
image: blank.pgm # 指定地图图像文件的路径，使用自己创建的PGM格式图像。
resolution: 0.050000 # 地图的分辨率，每个像素代表0.05米。
origin: [-50.000000, -50.000000, 0.000000] # 地图的原点，表示地图坐标系中 (0, 0) 点在图像中的位置，单位为米。
negate: 0 # 如果图像的黑白值与占用状态相反，则设置为1；如果图像中的黑色表示占用，白色表示空闲，则设置为0。
occupied_thresh: 0.65 # 判断地图上某个像素是否表示占用（障碍物）的阈值。如果像素值大于此值，则认为该像素表示占用。
free_thresh: 0.196 # 判断地图上某个像素是否表示空闲（自由空间）的阈值。如果像素值小于此值，则认为该像素表示空闲。
这里可以解释一下pgm其实是不保存图像色彩信息的一种图片格式，map_server就可以将这种灰白图片变成珊格兰地图。
__playpen_map.pgm__我的pgm就是一个规则的白色正方形图片。注意，这个地图的大小就决定了全局导航的范围。
地图的实际大小可以通过以下步骤计算：
1. **获取地图的图像尺寸**：首先需要知道地图图像的尺寸（宽度和高度），即图像的像素数。例如，假设地图图像的宽度为 `width_pixels`，高度为 `height_pixels`。
2. **获取地图的分辨率**：地图的分辨率是每个像素代表的实际距离（单位通常为米）。从配置文件中，可以看到 `resolution` 参数，例如上面的配置文件中，`resolution: 0.050000`。
3. **计算实际大小**：将图像的像素尺寸乘以地图的分辨率，就可以得到地图的实际大小。
公式如下：
{实际宽度} = {图像宽度（像素）} 乘以 {分辨率（米/像素）} ]
{实际高度} = {图像高度（像素）} 乘以 {分辨率（米/像素）} ]
####具体示例
假设地图图像的尺寸为 1000 像素（宽度） x 800 像素（高度），分辨率为 0.05 米/像素：
1. **图像尺寸**：
- 宽度：1000 像素
- 高度：800 像素
2. **分辨率**：
- 0.05 米/像素
3. **实际大小计算**：
- 实际宽度 = 1000 像素 x 0.05 米/像素 = 50 米
- 实际高度 = 800 像素 x 0.05 米/像素 = 40 米
这张地图的实际大小为 50 米 x 40 米。
4.尝试运行
当我们将文件配置号之后，就可以尝试运行
roslaunch outdoor_waypoint_nav outdoor_waypoint_nav_sim.launh
rviz中图像说明：
其中有一个里程计的图像是乱的，其实是GPSodom的图像，这这是因为gps只提供了小车的x,y定位数据没有方向信息导致的。
其中，紫色的圆点和原点上的箭头就是卡尔曼滤波节点融合后的里程计信息。
5.报错处理
- 参数错误
Error message :
RLException: environment variable 'HUSKY_MAG_CONFIG' is not set.
The traceback for the exception was written to the log file
解决办法：
工作空间下
export HUSKY_MAG_CONFIG=src/husky-melodic-devel/husky_bringup/config/mag_config_default.yaml
- 运行后小车没有激光雷达，找到
src/husky-melodic-devel/husky_gazebo/launch/spawn_husky.launch这个文件,添加变量和应用变量
<arg name="laser_enabled" default="true"/>
<arg name="ur5_enabled" default="false"/>
<arg name="kinect_enabled" default="false"/>
<include file="$(find husky_description)/launch/description.launch" >
<arg name="robot_namespace" value="$(arg robot_namespace)"/>
<arg name="laser_enabled" value="$(arg laser_enabled)"/>
<arg name="ur5_enabled" value="$(arg ur5_enabled)"/>
<arg name="kinect_enabled" value="$(arg kinect_enabled)"/>
</include>
找到src/husky-melodic-devel/husky_description/launch/description.launch文件
将参数添加到加载xacro的launch语句中
<param name="robot_description" command="$(find xacro)/xacro '$(find husky_description)/urdf/husky.urdf.xacro'
robot_namespace:=$(arg robot_namespace)
laser_enabled:=$(arg laser_enabled)
ur5_enabled:=$(arg ur5_enabled)
kinect_enabled:=$(arg kinect_enabled)"
/>
然后再运行就能看到激光雷达了。
- 局部路径没有代价地图
找到src/husky-melodic-devel/husky_navigation/params/costmap_common_params.yaml文件
将最后一行修改为
laser_scan_sensor: {sensor_frame: base_laser_mount, data_type: LaserScan, topic: scan, marking: true, clearing: true}
这里改变的就是sensor_frame，将它改成了激光雷达底座。这样就可以正常显示局部地图的代价和避障了。
6.参数修改
- 在实际的室外导航中轮子计算的里程计误差很大，所以我们修改efk算法中gps占的比重，让gps占较大比重在src/outdoor_waypoint_nav/params/ekf_params.yaml中
ekf_se_map:
frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
# -------------------------------------
# Wheel odometry:
odom0: /husky_velocity_controller/odom
odom0_config: [false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]
odom0_queue_size: 10
odom0_nodelay: true
odom0_differential: false
odom0_relative: false
# -------------------------------------
# GPS odometry:
odom1: /outdoor_waypoint_nav/odometry/gps
odom1_config: [true, true, false,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_queue_size: 10
odom1_nodelay: true
odom1_differential: false
odom1_relative: false
# --------------------------------------
# imu configure:
imu0: /imu/data
imu0_config: [false, false, false,
true, true, false,
false, false, false,
true, true, true,
true, true, true]
imu0_nodelay: true
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
use_control: false
process_noise_covariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.3]
initial_estimate_covariance: [1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1.0]
publish_tf: true
publish_acceleration: false
predict_to_current_time: true
dynamic_process_noise_covariance: true
- GPS 数据的权重调整：通过降低 GPS 协方差值（如将 GPS 数据中的对角线值设为较小值）来增加其在估计中的权重。例如，可以调整 /outdoor_waypoint_nav/odometry/gps 相关的协方差参数。
- 初始估计协方差：通过调整 initial_estimate_covariance，确保滤波器在起始时更倾向于信任 GPS 数据。设定较高的初始协方差值，可以让滤波器在接收到 GPS 数据后更快地调整自身状态。
- 过程噪声协方差：根据具体应用场景，可以适当调整 process_noise_covariance 参数，以平衡传感器数据融合过程中对不同传感器数据的依赖程度。
- 地图的参数的修改 运行过程中我们发现随着运行时间增加地图晃动加剧，不利于目标点设置，因此，我们设置地图参数在src/husky-melodic-devel/husky_navigation/params/global_costmap_params.yaml中我们设置
global_costmap:
global_frame: odom
robot_base_frame: base_link
update_frequency: 20
publish_frequency: 5
width: 30.0
height: 30.0
static_map: true
rolling_window: false
将rolling_window关闭，static_map保持打开。
7.运行效果
注意：本文档中文件中的中文注释是为了方便理解，如果运行出现问题请将中文注释删除。
同时，对于户外仿真效果，我们可以下载husky官方的gazebo仿真来实现。后续我会更新具体简单的操作，下面也给出了仓库地址。如果不能访问可以尝试复制名称到码云上寻找。
这里rviz中报错是我们没有设置gps的模型底座，不过这里tf用的是base_link不影响结果。
8.参考文章
机器人定位技术：robot_localization
导航原仓库
仿真环仓库
机器人仓库
扫一扫
5007
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
