机器谱

R313轮全向底盘

图文展示3264(1)

图文展示3264(1)

副标题

绘图(STM32)
SLAM导航

绘图(STM32)

1. 功能说明

      本文示例将实现R313a样机三轮全向底盘绘图(三角形、矩形、三叶草、正弦曲线)的功能。三轮全向底盘绘制图形有两种模式:第一种是自主选择模式(即通过按下开关进行模式的选项);第二种是上位机模式(即通过上位机发送坐标来完成绘制)。这里主要介绍下自主选择模式绘制图形。

2. 结构说明

    R313a样机主要是由R313c样机上安装一个 舵机关节模组 组成。

      三轮全向底盘采用全向福来轮作为执行轮,全向福来轮由主轮和副轮组成,主轮和副轮成垂直分布。三个轮成正三角形分布,两两夹角120度,这种结构使得全向底盘运动灵活、可以在平面内任意方向平移。

三轮全向底盘简图

     三轮全向底盘的全向移动需要三个轮之间的相互配合,具体运动方向和各个轮的转向如下图所示:

三轮全向底盘运动简图(横线表示该轮不转,箭头方向为轮转动方向)

3. 三轮全向底盘的运动学逆解分析

我们做一些必要的前提假设:

① 全向底盘质量分布均匀,每个轮子的大小和质量相同

三个全向轮到中心的距离相等,且两两夹角为120度

③ 全向底盘不会出现打滑

      首先建立一个世界坐标系X′O′Y′,然后再建立机器人自身的坐标系XOY。设中心到轮子的距离为常数L,移动平台自身的角速度为ω,设顺时针为角速度正方向,各个轮子的速度分别为Va,Vb,Vc。移动平台在自身坐标系下的分速度为Vx,Vy。夹角 θ₁ = π / 3,θ₂ = π / 6 , α是两个坐标系的夹角。


逆解:

所谓逆解,就是给定一个世界坐标系下的速度矢量,需要求出三个轮子分别需要给多少速度才可以使机器人在世界坐标系下达到该速度矢量。通过简单的速度矢量运算我们可以列出以下的线性方程组:

该线性方程组变成矩阵形式就像下面这样

       这个解是在机器人自身坐标系下的,当运用到实际场景中还需要一步转换。其实很简单,再乘上一个旋转矩阵就可以了。现在假设我们已经知道 α 的大小了,那么求旋转矩阵就没什么难度了X′O′Y′到XOY的旋转矩阵为:

那么从XOY到X′O′Y′的旋转矩阵就是R(α)的逆矩阵

所以有:

将这个关系代入第一个线性方程中得:

化简得:

4. 电子硬件

本实验中采用了以下硬件:

主控板

STM32主控板

扩展板

STM32扩展板

电池7.4v锂电池
传感器触碰传感器
输出模块OLED显示屏

其它

步进电机、标准舵机


参数

绘制的图案

sin

正弦曲线

Triangle

三角形

Rect

矩形

ThreeLeaf

三叶草


/*------------------------------------------------------------------------------------

  版权说明:Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-03-10 https://www.robotway.com/

  ------------------------------*/

#include "sys.h"

#include "led.h"

#include "usart.h"

#include "delay.h"

#include "math.h"

#include "stdio.h"

#include "string.h"

#include "stdlib.h"

#include "a4988.h"

#include "pwm.h"

#include "key.h"

#include "oled.h"

#include "stdbool.h"


int main(void)

{

NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2

delay_init(168);//初始化延时,168为CPU运行频率

usart_init(9600); //串口初始化

usart3_init(9600);

LED_Init();//LED灯初始化

Stepper_Motor_Init(0); //步进电机引脚初始化

KEY_Init();

OLED_Init(1);

delay_ms(1000);

TIM4_PWM_Init(20000-1,84-1);

Put_Up();delay_ms(1500);

  int receive_touch[2] = {0,0};

bool Flags_draw = false;

OLED_Clear();

OLED_ShowString(30,3,(unsigned char*)"Wait...",16);

while(1)

{

if(touch_keys(1) == 1)

{

  delay_ms(350);

receive_touch[0] += 1;

Flags_draw = false;

}

if(touch_keys(2) == 1)

{

if(Flags_draw)

{

receive_touch[1] = 0;

}

else

{

    delay_ms(350);

  receive_touch[1] = 1;

}

}

  if(receive_touch[0]==1)

{

//printf("data:%d",receive_touch[0]);

  OLED_Clear();

  OLED_ShowString(0,0,(unsigned char*)"Sin",16);

if(receive_touch[1] == 1)

{

OLED_Clear();

OLED_ShowString(0,0,(unsigned char*)"Sin",16);

OLED_ShowString(0,4,(unsigned char*)"Start",16);

receive_touch[1]=0;

delay_ms(1000);

    Put_Down();delay_ms(1000);

    drawSin();

    Put_Up();delay_ms(1000);

}

}

if(receive_touch[0]==2)

{

//printf("data:%d",receive_touch[0]);

  OLED_Clear();

  OLED_ShowString(0,0,(unsigned char*)"Triangle",16);

if(receive_touch[1] == 1)

{

OLED_Clear();

OLED_ShowString(0,0,(unsigned char*)"Triangle",16);

OLED_ShowString(0,4,(unsigned char*)"Start",16);

receive_touch[1]=0;

    Put_Down();delay_ms(1000);

    drawTriangle();

    Put_Up();delay_ms(1000);

}

}



  if(receive_touch[0]==3)

{

//printf("data:%d",receive_touch[0]);

  OLED_Clear();

  OLED_ShowString(0,0,(unsigned char*)"Rect",16);

if(receive_touch[1] == 1)

{

//receive_touch[0]=0;

OLED_Clear();

OLED_ShowString(0,0,(unsigned char*)"Rect",16);

OLED_ShowString(0,4,(unsigned char*)"Start",16);

receive_touch[1]=0;

    Put_Down();delay_ms(1000);

    drawRect();

    Put_Up();delay_ms(1000);

}

}



  if(receive_touch[0]==4)

{

  OLED_Clear();

  OLED_ShowString(0,0,(unsigned char*)"ThreeLeaf",16);

if(receive_touch[1] == 1)

{

OLED_Clear();

OLED_ShowString(0,0,(unsigned char*)"ThreeLeaf",16);

OLED_ShowString(0,4,(unsigned char*)"Start",16);

receive_touch[1]=0;

    //Put_Down();delay_ms(1000);

    drawThreeLeaf();

    Put_Up();delay_ms(1000);

}

}


  if(receive_touch[0]==5)

{

  OLED_Clear();

  OLED_ShowString(8,3,(unsigned char*)"Draw Line End",16);

receive_touch[0]=0;

  receive_touch[1]=0;

  Flags_draw = true;

}

  }

}

按下图进行电路连接:舵机连接在PD15引脚上。

5. 功能实现

编程环境:keil5

功能:当按上边的触碰传感器时,OLED显示屏上会显示出不同的选项(sin/ Triangle/ Rect/ ThreeLeaf);当按下边的触碰传感器时,开始绘制相应的内容(见下表)。

三轮全向底盘绘图的参考例程(USER\test.uvprojx)如下,下面是主程序main.c:

三轮全向底盘绘制三角形、矩形、三叶草、正弦曲线的详细例程可下载文末资料获取。



6. 资料清单

序号

内容
1

【R313】-绘图-例程源代码

2【R313】-绘图-样机3D文件


文件下载
【整体打包】-【R313】三轮全向底盘-绘图-资料附件.zip
14.87MB下载5次下载
上一页 1 下一页

SLAM导航

1. 功能说明

    本文我们利用键盘控制三轮全向底盘R313b)小车运动完成slam建图,并能在已建好的地图里进行自主导航。slam导航可以拆分为三步:

    第一步:能用键盘控制底盘的运动;

    第二步:基于实际场景,用键盘控制底盘小车进行slam建图;

    第三步:基于已建好的地图,模拟实现slam导航。

2. 电子硬件

   本实验中采用了以下硬件:

主控板

Arduino mega2560控制板

扩展板

STM32扩展板

电池11.1V动力电池
电池

树莓派、雷达、显示屏、键盘、鼠标、充电宝/手机充电器


分类

运动指令


键盘快捷键

指令含义

基本运动

i

前进

后退
j左转
l右转
平移J左平移
L右平移
调整角速度与线速度q增大底盘小车最大速度的10%(包含角速度与线速度)
z减小底盘小车最大速度的10%(包含角速度与线速度)
调整线速度w仅仅增大底盘小车线速度的10%
x仅仅减小底盘小车线速度的10%
调整角速度e仅仅增大底盘小车角速度的10%
c仅仅减小底盘小车角速度的10%

除了上面的按键其他按键(如:k)停止

Ctrl+c

程序结束


/*------------------------------------------------------------------------------------

  版权说明:Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-03-16 https://www.robotway.com/

  ------------------------------*/

/***************************************************************************************************

实验功能:

         驱动全向三轮底盘实现前进、后退、左转、右转、左平移、右平移


实验接线:

         步进电机细分:16细分

         步进电机方向引脚:dir: x: 44, y: 32, z: 42, a: 30

         步进电机步进引脚:stp: x: 45, y: 33, z: 43, a: 31


实验现象:

         上电后,小车前进3秒,停止5秒;后退3秒,停止5秒;

         左平移3秒,停止5秒;右平移3秒,停止5秒;左转3秒,停5秒,右转3秒,停5秒,小车停止。

***************************************************************************************************/

//DUE控制版需要启用USE_USBON或USE_NATIVE_USB,UNO不需要

//#define USE_USBCON          //PAOGRAMMING PORT

#define USE_NATIVE_USB      //NATIVE USB PORT

#include <ros.h>

#include <ros/time.h>

#include <geometry_msgs/Vector3.h>

ros::NodeHandle   nh;

#define Action_implement_delay 3000 //单个动作执行所用时间

#define Action_wait_for_delay 5000   //动作与动作之间的间隔时间

#define Set_Speed_Test 0.04

void XYRun(double vx, double vy, double w);

void messageCb( const geometry_msgs::Vector3& vel_cmd)   {XYSetVel(vel_cmd.x, vel_cmd.y, vel_cmd.z);}

ros::Subscriber<geometry_msgs::Vector3> vel_cmd_sub("vel_cmd", &messageCb );


geometry_msgs::Vector3 pose_message;

ros::Publisher pose_feedback_pub("pose_feedback",&pose_message);

geometry_msgs::Vector3 vel_message;

ros::Publisher vel_feedback_pub("vel_feedback",&vel_message);


const int kMessagePubRate = 5;

unsigned long message_pub_time = 0;

const int kReadMotorDeltaT = 50;

unsigned long position_read_time = 0;

float current_x = 0,current_y = 0,current_a = 0;

float current_vx = 0,current_vy = 0,current_va = 0;


void setup()

{

  delay(1000);

  Serial.begin(57600);

  initMotor();

//   Stepper_move_test();

  nh.initNode();

  nh.subscribe(vel_cmd_sub);

  nh.advertise(pose_feedback_pub);

  nh.advertise(vel_feedback_pub);

  XYSetVel(0.0,0.0,0.0);

  position_read_time = millis();

  message_pub_time = millis();

}


void loop()

{

  XYRun();  

  if(millis()>position_read_time)

  {

    XYRead();

    position_read_time += kReadMotorDeltaT;

  }

  if(millis()>message_pub_time)

  {

    pose_message.x = current_x;

    pose_message.y = current_y;

    pose_message.z = current_a;

    vel_message.x = current_vx;

    vel_message.y = current_vy;

    vel_message.z = current_va;

    pose_feedback_pub.publish(&pose_message);

    vel_feedback_pub.publish(&vel_message);

    message_pub_time+=1000.0/kMessagePubRate;

  }

  nh.spinOnce();  

}

按下图进行电路连接:

第一步:先将步进电机与主板的扩展板进行连接(提示:侧重看x、y、z的接线位置,装有雷达的一侧代表底盘小车的车头。雷达左侧的步进电机  

            控制小车的X轴方向;雷达的右侧的步进电机控制小车的Y轴方向;雷达对侧的步进电机控制小车的Z轴方向,具体的接线引脚见下图)

第二步:将主控板的扩展板跟树莓派连接起来(见图中蓝色的线),并用锂电池进行供电。

第三步:将鼠标、键盘、显示屏与树莓派进行连接(见下图)。其中显示屏和树莓派用充电宝供电,鼠标、键盘这里用的是无线的。

第四步:将雷达与树莓派进行连接(建议把雷达连接在树莓派的usb3.0上,见下图)。

经过上边几步的电路连接后,就完成了相应的电路连接(见下图)。

3. 功能实现

操作系统:Ubuntu18.04系统,基于Debian GNU/Linux,支持x86、amd64(即x64)、ARM和ppc架构。

仿真系统:基于开源机器人操作系统ROS melodic和开源软件平台Arduino开发,上位机采用ROS melodic,基于Rviz完成全向移动底盘小车

                slam导航运动规划,采用gazebo完成全向移动底盘小车物理运动仿真;下位机采用Arduino实现对全向移动底盘小车运动的控制。

编程环境:Arduino 1.8.19

3.1 键盘控制底盘小车运动

1)实现思路

      当按下键盘上指定的键时,可以实现三轮全向底盘小车前进、后退、转向、平移;还可以灵活的设置底盘小车的角速度、线速度来调整底盘小车的运动。下面是本实验中规划的控制底盘小车的键盘命令(见下表),可以先熟悉一下,后续在控制底盘小车时会用到。

2)操作步骤

       ① 打开参考例程文件夹(src\Arduino_Programing\Control_FuLaiLun_Car),下载程序Control_FuLaiLun_Car.ino:

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

终端

      成功启动后,可以在终端看到底盘小车的当前speed(线速度)为0.04,turn(角速度)为0.08。【注意:speed指的是线速度的大小(包含方向),turn指的是角速度的大小(包含方向)】

启动后的界面(见下图):

③ 尝试按下键盘命令,控制底盘小车的运动。【注意:一、请先保证终端是激活状态,见上图;二、按键盘时,稍微有点间隔,这样能给底盘小车转动的时间;三、一定要打开扩展板上的电源开关,否则会发现小车位置在rviz中位置虽然变化,但车不会运动】

假如现在希望先增加底盘小车的最大速度,再降低底盘小车的最大速度,则分别按下键盘上q、z命令,便可以观察到终端的结果(见下图)。

更多命令请自行尝试。

3.2 底盘小车同步定位与建图-SLAM

1)实现思路     

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

      Gmapping算法是目前基于激光雷达和里程计方案里面比较可靠和成熟的一个算法, 它基于粒子滤波,采用RBPF的方法,效果稳定。本次三轮全向底盘小车slam建图采用的是gmapping_slam包。下面介绍Gmapping SLAM计算图。Gmapping的作用是根据激光雷达和里程计(Odometry)的信息,对环境地图进行构建,并且对自身状态进行估计。因此它得输入应当包括激光雷达和里程计的数据,而输出应当有自身位置和地图。下面我们从计算图(消息的流向)的角度来看看gmapping算法在实际运行中的结构:

位于中心的是我们运行的slam_gmapping节点,这个节点负责整个gmapping SLAM的工作。它的输入需要有两个:


输入

     /tf以及/tf_static:坐标变换,类型为第一代的tf/tfMessage或第二代的 tf2_msgs/TFMessage 其中一定得提供的有两个tf,一个是base_frame 与 laser_frame 之间的tf,即机器人底盘和激光雷达之间的变换;一个是base_frame 与 odom_frame之间的tf,即底盘和里程计原点之间的坐标变换。odom_frame可以理解为里程计原点所在的坐标系。

      /scan:激光雷达数据,类型为sensor_msgs/LaserScan

      /scan很好理解,Gmapping SLAM所必须的激光雷达数据,而/tf 是一个比较容易忽 视的细节。尽管/tf 这个Topic听起来很简单,但它维护了整个ROS三维世界里的转换关系,而 slam_gmapping要从中读取的数据是base_frame 与 laser_frame 之间的tf,只有这样才能够把周围障碍物变换到机器人坐标系下,更重要的是base_frame 与 odom_frame之间的tf,这个tf 反映了里程计(电机的光电码盘、视觉里程计、IMU)的监测数据,也就是机器人里程计测得走了多少距离,它会把这段变换发布到odom_frame 和 laser_frame 92之间。因此slam_gmapping 会从/tf 中获得机器人里程计的数据。


输出

      /tf:主要是输出 map_frame 和 odom_frame 之间的变换

      /slam_gmapping/entropy:std_msgs/Float64 类型,反映了机器人位姿估计的分 散程度

      /map:slam_gmapping 建立的地图

      /map_metadata:地图的相关信息

     输出的/tf 里又一个很重要的信息,就是map_frame和odom_frame之间的变换,这 其实就是对机器人的定位。通过连通map_frame和odom_frame,这样map_frame与 base_frame 甚至与 laser_frame 都连通了。这样便实现了机器人在地图上的定位。

2)操作步骤

        ① 先搭建一个实际场景,便于建立地图。【注意:为了方便识别,搭建的实际测试地图最好是不规则的区域。如下图测试的场地,在长方形区域的一边是不规则的,这样便于识别建立的仿真地图】

② 启动雷达、键盘、rosserial;打开终端并输入命令:roslaunch robot_navigation_control robot_car.launch (见下图)

③ 启动构建地图服务。按(ctrl+shift+T)打开第二个终端并输入:roslaunch four_macnum_slam four_macnum_slam.launch(见下图)

界面启动后,在rviz中选择“map”,可以在可视化界面看到有地图出现。下面根据实际环境进行建图:

【注意:一、请先保证键盘控制的终端是激活状态(请用鼠标点击一下此终端);二、按键盘时,稍微有点间隔,这样能给底盘小车转动的时间】

      按下键盘相关指令(键盘指令请参考3.1)来控制底盘小车在场地内运行,直至在Rviz内可以看到构建地图的轮廓,如下图所示(注意:地图环境会根据实际场景建立)。

底盘小车在未知环境建立地图

      ④ 为了后续在已知环境中运行底盘小车,需把此次建立的地图进行保存。需要先进入保存地图的文件夹,然后给地图命名,保存即可。例如我们把此次建立的地图命名为your_map_name。【提示:your_map_name为自定义的名称,您也可以按自己的需求进行命名】


下面是本实验中的具体操作

重新打开新终端(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.3 底盘小车导航-Navigation

1)实现思路

        在上述3.2搭建好的实际场景中,实现三轮全向底盘小车的导航效果。     

        具体思路:加载上述3.2建好的地图,设置底盘小车的初始位置和方向,设定目标位置及方向,底盘小车就可以实现导航效果。     

        Navigation与底盘关系:ROS的二维导航功能包,简单来说,就是根据输入的里程计等传感器的信息流和机器人的全局位置,通过导航算  

                                               法,计算得出安全可靠的机器人速度控制指令。但是如何在特定的机器人上实现导航功能包的功能,却是一件较为

                                               复杂的工程,作为导航功能包使用的必要先决条件,机器人必须运行ROS,发布tf变换树,并发布使用ROS消息类  

                                               型的传感器数据。同时为了让机器人更好的完成导航任务,开发者还要根据机器人的外形尺寸和性能, 配置导航功

                                               能包的一些参数。      

ROS中进行导航需要使用到的三个包是:

        ① move_base:根据参照的消息进行路径规划,使移动机器人到达指定的位置;

        ② gmapping:根据激光数据(或者深度数据模拟的激光数据)建立地图;

        ③ amcl:根据已经有的地图进行定位。

基于上图,我们来看看底盘简单对应关系。

2)操作步骤

        ① 将创建的地图应用到导航程序中。

             这里我们需要修改four_macnum_navigation.launch文件,把上述3.2的your_map_name.yaml 添加到此launch文件中。步骤如下:

             打开终端并输入:cd fourmacnum_car_ws/src/four_macnum_navigation/launch

             之后再输入:gedit four_macnum_navigation.launch

② 启动雷达、键盘、rosserial。

    在该终端继续输入:roslaunch robot_navigation_control robot_car.launch(见下图)

③ 按下(ctrl+shift+T)打开第二个终端并输入:roslaunch four_macnum_navigation four_macnum_navigation.launch(见下图)

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

④ 开始调整底盘小车的姿态。

第一步:请先旋转地图,确保地图和实际场景的方向基本一致。

第二步:使用2D Pose Estimate 标定底盘小车位于地图中的初始位置及车头指向(见下图)。

第三步:鼠标点击有roslaunch robot_navigation_control robot_car.launch的终端后,尝试按下键盘命令控制底盘小车运动(键盘命令请参考3.1),最好尽可能多的消除地图中的箭头【这一步实际是确定底盘小车在地图中的实际位置以及车头指向】。下图是多次按下键盘命令控制底盘小车运动后,箭头逐渐减少的效果。

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

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

4. 资料清单

序号

内容
1

【R313】-slam导航-例程源代码

2【R313】-slam导航-样机3D文件


文件下载
【整体打包】-【R313】三轮全向底盘-slam导航-资料附件.zip
16.52MB下载1次下载
上一页 1 下一页
© 2022 机器时代(北京)科技有限公司  版权所有
机器谱——机器人共享方案网站
学习  开发  设计  应用