智能轮椅——视觉寻迹与运动控制

0 概述

基于视觉寻迹与二维码地标的轮椅导航运动控制，可实现直线追踪、地标对齐等运动控制。本文方法已经过测试，效果良好。

1 原理

1.1 轮椅运动

差动轮运动合成

轮椅由差动轮驱动，双轮运动组成轮椅的整体运动，轮椅运动满足基本的合成关系：

v_car = (v1 + v2)/2
w_car = (v2 - v1)/L
符号含义，
v_car，轮椅线速度；
w_car，轮椅角速度；
v1，左轮线速度；
v2，右轮线速度；
L，双轮轮距；

差动轮运动分解

轮椅运动控制指令通常为轮椅自身的线速度和角速度，分解关系如下：

v1 = v_car - 0.5*L*w_car
v2 = v_car + 0.5*L*w_car

将轮椅运动指令传递值电机运动的过程，伪代码表述如下：

常值参数
coeff_w2v = 0.5*L；
线速度分解：
v1 = v_car - coeff_w2v*w_car;
v2 = v_car + coeff_w2v*w_car;
电机驱动电压计算：
mt1_volt = Kp*v1;
mt2_volt = Kp*v2;
电压单位是PWM的千分比，取值单位（-1000,1000）。
阻力死区处理：
if abs(mt_volt) < volt_sat:
	mt_pwm = mt_volt/volt_sat*PWM_DEADZONE;
else  :
	mt_pwm = sign(mt_volt)*(abs(mt_volt) - volt_sat) + PWM_DEADZONE;

特别说明
若要将轮椅的运动控制的更加精细，需要对线速度、角速度的指令的单位统一成国际制单位。信号的传递流程换算规范化处理：
轮椅运动控制输出：spd_linear_ref（单位m/s），spd_angle_ref（单位rad/s）；

1.2 轮椅跟踪直线

直线末端点为直线与成像图上边界的交点，轮椅运动过程中实时跟踪该点。

• 末端点坐标计算

已知直线参数：rho， theta；
计算直线方程参数：
a = cos(theta)， b = sin(theta)， x0 = a*rho， y0 = b*rho；
直线方程表示：
x = x0 - b*t， y = y0 + a*t；
求取直线与图像上边界限交点（x_ref, y_ref）：
y_ref = y = 0（上边界像素纵坐标为0）
=>  t = -y0/a  
=>  x_ref = x0 - b*t

• 末端方位角计算

计算末端坐标点与像素中心的位置偏差：
	dx = x_ref - x_width*0.5;
	dy = y_width*0.8;
符号含义：
	x_width，图像像素宽度；
	y_width，图像像素高度；
计算相对角度：
    phai = - atan2(dx, dy);
角度说明：
	垂直向上为零度；
	逆时针为正方向。

改进方案
计算方位角的方法比较粗略：基准点选择，原始方法采用图像几何中心点作为基准点，应该以轮椅运动中心为基准点；方位角度基准选择，原始方法采用像素坐标y轴负方向作为零度，应该以轮椅运动正前方方向为零度。
轮椅运动中心点与图像几何中心点有偏差，需要标定出该偏差，得出像素坐标与运动中心的关系。若相机的xy轴与轮椅的正前方角度差别较大，也需要精确标出。（思考：什么方法可以精确标定这些参数？）
粗略计算的影响分析：
1、当相机方向与轮椅运动方向对齐时，方位角度基准误差可以忽略，所以安装时对齐相机方向非常重要；
2、原始方法计算方位角不影响正负，仅影响幅值大小，赋值大小仅仅影响算法的调节增益，所以该影响可以忽略。

• 轮椅跟踪直线算法
  轮椅运动过程中实时跟踪直线与上边界的交点，轮椅方位角度跟随直线末端点，线速度保持匀速。
  用伪代码表述算法流程如下：

spd_linear_ref = WHEEL_CHAIR_SPD_HIGH;
min_angle_sat = 0.01;
angle = set_deadzone_sat(phai, min_angle_sat);
spd_angle = kp*angle;
spd_angle_ref = set_max_min(spd_angle, WHEEL_CHAIR_SPD_HIGH, -WHEEL_CHAIR_SPD_HIGH);

1.3 轮椅参考地标运动

在本项目中地标采用april二维码，用视觉识别二维码后可以计算出二维码的ID值，以及相对于相机坐标系的位置和姿态。
轮椅参考地标的运动可分解定点、定向运动，先运动到二维码位置点，在调整朝向至设定方位。

• 定点运动算法

计算位置偏差：二维码（pose.x，pose.y）到达轮椅运动中心（CAMERA_X0，CAMERA_Y0）
dx = tag_x - CAR_X0;
dy = tag_y - CAR_Y0;

判断距离误差：
error_xy = sqrtf(dx*dx + dy*dy);

判断是否到位：
 if (error_xy ： xy_error_sat)：
 	ReachedPlace_in_Length = true;

方位角度控制
	theta = atan2(dy, dx);
	若朝向角度向前，则向前运行对齐地标
	if (abs(theta) < PI/2)：
		 angle_target = 0.0 - theta;
	若朝向角度向后，则向后运行对齐地标
	if (theta > PI/2) ：
		 angle_target = 0.0 - (theta - PI);
	if (theta < - PI/2) ：
		angle_target = 0.0 - (theta + CV_PI);
	角速度与方位角的关系
	spd_angle = kp_angle * angle_target;

线速度控制
   增加死区效应
   linear_target = set_deadzone(dx, max, min) ;
   比例调节
   spd_linear = kp_linear*linear_target;

• 定向运动算法

此过程中线速度保持为零：
目标方位角度
angle_target = Tag_angle + Planning_Direction;
增加死区效应
angle_target = set_deadzone(angle_target, max, min) ;
比例调节
spd_target = kp_target*angle_target;

2 程序框架

基于ROS开发

节点组成及功能

zed_wrapper: ZED相机驱动，启动后实时获取相机图像，并向外发布图像话题；
wheel_driver: 轮椅驱动控制，启动后打开串口与轮椅电机驱动器建立通信连接，当收听到轮椅线速度/角速度话题后，驱动轮椅运动，当一段时间内没有接收到速度指令话题，则将轮椅速度设置为0；
apriltag_ros: 二维码标签的识别与定位，启动后实时获取图像话题后，识别和定位二维码，发布二维码信息话题；
cv_guide: 视觉引导控制；
启动后进入状态机制管理：空闲状态时，发布语音识别使能话题；开始运动时，发布禁止语音识别话题；
开始运动时，检测当前有无二维码地标，若无二维码则进入错误状态；若有二维码，则将当前二维码地标作为起点，接收到的位置目标作为终点，做路径规划，路径规划成功后进入运行状态。
运行过程中，根据引导直线前进，遇到新的二维码地标后，发布广播位置话题，同时调用地图规划模块进行方向查询，获取该地标处的绝对转向角度，进行地标对齐运动；直到运行至终点。
audio_control: 语音识别与命令发送。
接到语音收听使能话题后，开始周期性识别语音片段，收到语音使能后，发布目标位置话题，并记录目标位置值，并播放语音“正在前往xx地”。
接收到广播位置值后，若该至是目标位置值，则播放语音“达到xx地”，否则播报“途径xx地”。

启动流程

最终在运行时，可用一个launch文件启动全部节点。

在调试时，为了各模块在终端打印信息，可开启多个终端，分别启动节点。

$ roslaunch apriltag_ros zed_detection.launch
$ rosrun wheel_chair wheel_driver 
$ rosrun wheel_chair cv_guide
$ rosrun image_view age_view image:=/track_image
$ rosrun audio_control audio_ctrol