前言

 距离上一次分享已经很久啦，这篇就把后面的定位，二维码，语音的大致思路和大家说说，因为我也要比赛的缘故，具体的代码以及参数就不透露啦，希望大家可以主动思考。

PS：作者来自西安工程大学电子信息系。

正文

  定位

经过这么久的调试相信大家的车子都可以跑了，这个时候是不是都发现小车在转弯的过程中，特别是过U型弯的时候，雷达和地图不匹配的现象十分严重。这是因为小车的odom是轮式里程计发送的，轮式里程计会因为轮子打滑或者别的情况积累误差，这个时候就就应该加入imu来融合定位。相信大部分人使用的就是扩展卡尔曼滤波（EKF）来将轮式里程计和imu进行一个融合，卡尔曼滤波核心的五个公式希望大家有兴趣可以了解一下，这里就教大家如何配置EKF融合。

首先在github上下载robot_pose_ekf这个功能包，不推荐用apt方式安装。下载完成以后咱们对里面的launch文件进行一个修改。

<launch>
	<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
		<param name="output_frame" value="odom"/>  //改成odom只是为了方便后续的调试
		<param name="base_footprint_frame" value="base_link"/>     //根据实际情况修改
		<param name="freq" value="30.0"/>
		<param name="sensor_timeout" value="1.0"/>
		<param name="odom_used" value="true"/>
		<param name="imu_used" value="true"/>
		<param name="vo_used" value="false"/>       //因为不使用视觉里程计所以false
		<remap from="imu_data" to="imu" />
		<!-- 将节点订阅的 imu_data 话题改名为 imu,如果 imu 节点发布的话题是 imu_data 就不用修改 -->
	</node>
</launch>

光是修改launch文件是不够的，因为官方的教程里面并没有发布imu的tf变化，因此我们需要添加imu的tf以及协方差矩阵。

首先关闭base_driver.cpp中odom的tf发布，因为ekf会发布其tf变换，一个树只能有一个父系，如果不关闭会小车会跳动。并且ekf还需要添加协方差矩阵，odom的协方差已经提前写好了，我们只需要将imu的协方差矩阵添加到imu的函数中，具体部分代码如下。

    pravite_nh.param("provide_odom_tf", provide_odom_tf_,false);

    imu_data.orientation_covariance[0] = 1e6;
    imu_data.orientation_covariance[4] = 1e6;
    imu_data.orientation_covariance[8] = 1e-6;
    imu_data.angular_velocity_covariance[0] = 1e6;
    imu_data.angular_velocity_covariance[4] = 1e6;
    imu_data.angular_velocity_covariance[8] = 1e-6;

imu 信息的协方差矩阵中代表机器人航向角的分量方差为 1e-6,而里程计信息的协方差矩阵中机器人姿态分量的协方差为1e6,两个值相差很大。在进行 EKF 融合时,会更相信imu 提供的姿态信息,因为其方差更小。这里我只给出了默认值，具体值因为公平性就不给出了。

在设置好协方差矩阵以后需要对imu的tf进行发布，这里我的做法是将雷达launch文件中的tf发布给注释掉，然后将官方给的sensor_tf_server.py加入到base_driver.launch中，若py运行错误可以尝试将第一行的环境改为python3。ps:注意修改py文件的base_frame，默认好像是base_printfoot。还有个比较坑的地方，就是imu的名字问题，tf树中imu的名字为imu_frame，但是base_driver.cpp中的名字为imu，因此ekf在运行的时候会警告，我们可以在driver_params_mini.yaml中添加。

imu_frame:  imu_frame
laser_frame:  laser_frame

具体配置已经完成了，现在只需要将ekf的launch文件加入进去就行了，如果配置成功tf树中odom到base_link将由ekf发布。

二维码

比赛采用的是aruco二维码，我的思路是用opencv中的aruco库进行识别，至于cv_bridge要不要使用，因为不需要在ros上显示图像，因此我没有使用。只需要将识别出的id号pub出去，然后再使用sub订阅，根据订阅的信息来发布目标点。这里提供一个python识别的例程，具体请自行修改。

import time
import cv2
from cv2 import aruco

cap = cv2.VideoCapture(0)
ret, frame = cap.read()
while ret:
    ret, frame = cap.read()
    h1, w1 = frame.shape[:2]
    # 读取摄像头画面
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
    parameters =  aruco.DetectorParameters_create()
    corners, ids, rejectedImgPoints = aruco.detectMarkers(gray,
                                                          aruco_dict,
                                                          parameters=parameters)
    aruco.drawDetectedMarkers(frame, corners,ids)
    cv2.imshow("frame",frame)
    key = cv2.waitKey(1)
    if key == 27:         # 按esc键退出
        print('esc break...')
        cap.release()
        cv2.destroyAllWindows()
        break
    if key == ord(' '):   # 按空格键保存
#        num = num + 1
#        filename = "frames_%s.jpg" % num  # 保存一张图像
        filename = str(time.time())[:10] + ".jpg"
        cv2.imwrite(filename, frame)

语音

语音分为四个步骤：1、唤醒麦克风。2、发布client调用离线命令词识别。3、发布识别出来的关键词，根据关键词进行目标点发送以及控制。4、语音播报。我们主要写的部分是2、3、4。

首先是发布client调用离线命令词识别。 官方给的client.cpp中已经有了这段代码，但是如果我们想要做到唤醒过后自动识别的话有两种思路：

一、是在hid_test_auto.cpp中找到唤醒成功的代码，然后在后面写一个pub，发送一个flag。client写一个订阅代码，当订阅到flag以后运行serviceClient。

二、直接在hid_test_auto.cpp中唤醒成功的代码后将设置一个全局变量将其置1，然后主函数写一个while当全局变量为1时运行serviceClient，运行后再将其置0。经过测试可以很好的完成任务。

其次，关键词识别后将其pub，然后sub根据其输出的文字对小车进行控制。

最后语音播报：因为语音合成没有适配jetson nano的程序，所以我们只好使用事先录好的音频，按照各个任务节点来播放。

思考

1、当时对定位的问题问了胡老师，他和我说可以先对输入值，如imu的值进行一个处理后再进行融合，以及可以添加更多的信息源，比如激光里程计等多个传感器进行融合。

2、图像矫正是否有必要：我进行图像矫正以后确实图像畸变解决了，但是随之而来的是没有原图清晰，丢失了一些数据。当然图像矫正不是必须的，当时普通组的时候我们没用图像矫正依然可以取得很好的名次。

结束

以上是我对基础任务的所有思路，因为比赛问题没法给出具体的做法，如果大家有更好的思路可以一起交流。另外人物识别不是我在做，所以给不了很好的建议，在此希望大家都能取得好成绩。