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文件启动全部节点。
<launch>
<!-- 启动usb驱动摄像头和apriltag识别模块 -->
<include file="$(find apriltag_ros)/launch/zed_detection.launch" />
<!-- 启动轮椅驱动模块 -->
<node pkg="wheel_chair" type="wheel_driver" name="wheel_driver"/>
<!-- 启动cv_guide,并开启image_view订阅视频流信息 -->
<node pkg="wheel_chair" type="cv_guide" name="cv_guide"/>
<node name="image_view_track_image" pkg="image_view" type="image_view" respawn="false" output="screen">
<remap from="image" to="/track_image"/>
<param name="autosize" value="true" />
</node>
<!-- 启动语音识别模块 -->
<node pkg="audio_control" type="audio_control" name="audio_control"/>
</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