本文转载自知乎：AI与机器人

一、用代码控制YouBot机器人

1.1 YouBot机器人模型

• YouBot机器人包括底盘和机械臂。
• 底盘带有四个麦轮，麦轮分为A轮和B轮，可以前后左右移动。
• 机械臂有5个旋转关节和相应的连杆机构串联组成，末端装有夹持机构。每个关节可以进行单独的控制。在Vrep中已经构建好了基本的关节模型和运动控制接口，只需要调用响应的控制接口来控制各个关节就行了。

1.2 Vrep中基本的代码框架

• V-REP的控制脚本至少包含Main script和Child script。

• Main script对应每一个Scene，一个Scene只有一个。这部分是V-REP软件运行时会自动加载的控制脚本，在实际使用时我们不用修改这里的内容。

• Child script对应每一个模型以及模型当中的部分关节等。每一个模型都会包含一个跟自己绑定的Child script，如果涉及到更加底层的控制过程，我们可以给每一个关节、每一个传感器等都定义一个自己的Child script。因此可以修改YouBot机器人的Child script里面的代码来实现机器人的控制。

• 脚本中都会包含一些基本函数，但不是每一个都一定要包含。一般sysCall_init()、sysCall_cleanup()是默认包含的。V-REP在运行时后台自动调用这些函数，不能修改这4个函数的名字，否则V-REP在运行时无法找到对应的函数。
  
  1.3 用Lua代码控制YouBot

• 初始化函数部分：从V-REP环境中加载各个关节的Handle。Handle是一种类似文件句柄的识别符，给它传递必要的参数来对它进行操作就可以控制对应的模型。Lua语言编写的代码中，所有定义的变量都默认为全局变量，例如在sysCall_init()函数中定义的wheel_joints这个变量就可以被其他函数调用。而想要编写仅用于本函数的局部变量，需要使用local关键字。

-- 初始化YouBot机器人
function sysCall_init()-- 获得机器人的句柄you_bot = sim.getObjectHandle('youBot')-- 获得四个轮子的句柄，写到一个数组里wheel_joints = {-1,-1,-1,-1} -- 前左，后左，后右，前右wheel_joints[1] = sim.getObjectHandle('rollingJoint_fl')wheel_joints[2] = sim.getObjectHandle('rollingJoint_rl')wheel_joints[3] = sim.getObjectHandle('rollingJoint_rr')wheel_joints[4] = sim.getObjectHandle('rollingJoint_fr')-- 获取五个关节的句柄，写到一个数组里arm_joints = {-1,-1,-1,-1,-1} -- set default valuesfor i=0,4,1 doarm_joints[i+1] = sim.getObjectHandle('youBotArmJoint'..i)end-- 初始化时每个关节要到达的位置desired_joint_angles = {0,0,0,0,0}-- 初始化每个关节for i = 1,5,1 dosim.setJointPosition(arm_joints[i], desired_joint_angles[i])end-- 设置最大的关节速度max_joint_velocity = 50*math.pi/180 -- 用户自定义变量，默认是全局变量switch_pos_flag = 0go_forward_flag = 1 -- 机器人向前移动的标志
end

• 定义各个关节如何随时间运动：获取V-REP软件的仿真计时，并根据仿真的时间每隔10秒更新一次底盘轮子的运动方向，以及各个机械臂关节的目标运动角度。
• 轮子的控制过程是直接设定轮子的运动速度和方向，通过调用sim.setJointTargetVelocity()来实现对某个指定轮子的控制过程，根据时间改变了轮子的运动方向。
• 机械臂关节的控制过程是使用的位置控制API函数sim.setJointPosition()。由于该函数将会直接将指定关节设定为目标角度，如果我们让关节直接从某一个位置“跳转”到目标位置，当目标位置与当前位置距离较远时，该关节会以最大运行加速度猛烈运动，会导致该关节的运动发生冲击，引起机器震荡。因此，，在给定机械臂各个关节目标角度的时候，不能直接给一个最后的目标位置，需要设定关节最大的一个运动速度，并通过计算当前位置与目标位置的偏差，将其拆分为“多段”位置分时输入到机器人的关节当中。

-- 控制youbot机器人的关节
function sysCall_actuation() --获取当前关节位置current_joint_angles = {0,0,0,0,0}for i=1,5,1 docurrent_joint_angles[i] = sim.getJointPosition(arm_joints[i])end--最大允许变动范围max_variation_allowed = max_joint_velocity*sim.getSimulationTimeStep()--当前时间simu_time = sim.getSimulationTime()-- 每隔10秒作出以下改变改变if (simu_time % 10 == 0) thenprint('\nSimulation time = '..simu_time)-- 每隔20秒改变前进后退if (simu_time % 20 == 0) thenif go_forward_flag == 1 thengo_forward_flag = -1 -- go backwardelsego_forward_flag = 1 -- go forwardendend-- 每隔10秒设定不同的关节位置，有3个位置if switch_pos_flag == 0 thenprint("Switch to position 1")desired_joint_angles = {180,0,0,0,0}switch_pos_flag = 1elseif switch_pos_flag == 1 thenprint("Switch to position 2")desired_joint_angles = {120*math.pi/180, 30.91*math.pi/180, 52.42*math.pi/180, 72.68*math.pi/180, 0}switch_pos_flag = 2elseprint("Switch to position 3")desired_joint_angles = {-120*math.pi/180, 30.91*math.pi/180, 52.42*math.pi/180, 72.68*math.pi/180, 0}switch_pos_flag = 0end   end-- 设置轮子的运动速度wheel_velocity = go_forward_flag*0.8;sim.setJointTargetVelocity(wheel_joints[1], wheel_velocity)sim.setJointTargetVelocity(wheel_joints[2], wheel_velocity)sim.setJointTargetVelocity(wheel_joints[3], wheel_velocity)sim.setJointTargetVelocity(wheel_joints[4], wheel_velocity)-- 设置关节运动到的位置for i=1,5,1 dodelta = desired_joint_angles[i] - current_joint_angles[i]if (math.abs(delta) > max_variation_allowed) thendelta = max_variation_allowed*delta/math.abs(delta) -- limit the variation of each jointendsim.setJointPosition(arm_joints[i], current_joint_angles[i] + delta)end
end 

二、用键盘控制 YouBot机器人

• 自定义了一个键盘事件检测函数，用来检测键盘上哪些按键被按下了。检测的方式是调用simGetSimulatorMessage()这个函数来获取键盘的按下情况，并通过比对键盘码来设定YouBot的运动方向。

function getKeyboardStatus()--message为键盘是否被按下--data存储键盘按下的信息，然后将该信息传到用户自定义的方向上message,data,data2=simGetSimulatorMessage()if (message==sim_message_keypress) thenif (data[1]==2007) then -- up keyprint("up")move_direction = 1elseif (data[1]==2008) then -- down keyprint("down")move_direction = 2elseif (data[1]==2009) then -- left keyprint("left")move_direction = 3elseif (data[1]==2010) then -- right keyprint("right")move_direction = 4elseif (data[1]==string.byte('q')) then -- q keyprint("q")move_direction = 5elseif (data[1]==string.byte('w')) then -- w keyprint("w")move_direction = 6endif (data[1]==string.byte(' ')) then -- space keywheel_velocity = 0print("stop")elsewheel_velocity = 1endend
end

• 获得了键盘的信息以后，我们需要设定YouBot底盘上每个轮子的运动方向来实现上述的几个功能

function setYouBotMovementDirection()-- Check movement directionif (move_direction == 1) then -- go forwardlf_dire = -1rf_dire = -1lr_dire = -1rr_dire = -1elseif (move_direction == 2) then -- go backwardlf_dire = 1rf_dire = 1lr_dire = 1rr_dire = 1elseif (move_direction == 3) then -- go leftlf_dire = 1rf_dire = -1lr_dire = -1rr_dire = 1elseif (move_direction == 4) then -- go rightlf_dire = -1rf_dire = 1lr_dire = 1rr_dire = -1elseif (move_direction == 5) then -- turn leftlf_dire = 1rf_dire = -1lr_dire = 1rr_dire = -1elseif (move_direction == 6) then -- turn rightlf_dire = -1rf_dire = 1lr_dire = -1rr_dire = 1end
end

• 在sysCall_actuation()这个函数接口里面调用我们刚刚自定义的2个函数来控制每个轮子的运动：

function sysCall_actuation()-- Check keyboard press eventsgetKeyboardStatus()setYouBotMovementDirection()-- 关节臂保持不动for i = 1,5,1 dosim.setJointPosition(arm_joints[i], desired_joint_angles[i])end-- 控制轮子运动sim.setJointTargetVelocity(wheel_joints[1], lf_dire*wheel_velocity)sim.setJointTargetVelocity(wheel_joints[2], lr_dire*wheel_velocity)sim.setJointTargetVelocity(wheel_joints[3], rr_dire*wheel_velocity)sim.setJointTargetVelocity(wheel_joints[4], rf_dire*wheel_velocity)
end 

三、YouBot底盘运动学与路径规划

3.1 底盘运动学

• 麦轮中的A轮向前运动时会提供向左的运动速度，向后时会提供向右的运动速度。B轮的侧向运动方向与A轮刚好相反。

• 逆运动学求解：给定机器人的空间运动轨迹反求4个轮子在各个时刻的运动速度，将底盘的运动状态合并到中心点处，关系为：
  

• 给定世界坐标系X-O-Y如图所示，其中Y的正方向表示YouBot机器人向正前方运动，X的正方向表示YouBot机器人向正右方运动。所有与X、Y正方向同向的量的符号都为正，反之则为负，所有的角度旋转方向的定义遵循右手定则。则底盘的中心点运动情况可以分解为中心点沿X方向运动的速度，沿Y方向运动的速度和绕中心点旋转的角速度：
  
  

• 求解中心点的运动状态与四个轮子的运动状态之间的关系，轮子在前后运动时也会侧向运动，而侧向运动的速度与前后 运动的速度呈线性比例关系，因此向前运动和侧向运动的速度合成为kv1。
  
  
  
  
  
  
  
  3.2 底盘运动学代码

• 在V-REP中，YouBot的4个轮子的正方向与我们定义的正方向刚好相反，因此在计算完成以后4个轮子的速度方向均要反向。完成了YouBot机器人的底盘逆运动学模型建模后，只需要指定中心点位置的运动状态（见公式（1）），即可求出各个轮子的运动状态。

function chassisInverseKinematics(vx, vy, omega, wheel_R, a, b)omega_1 = (vy - vx + (a+b)*omega)/wheel_Romega_2 = (vy + vx - (a+b)*omega)/wheel_Romega_3 = (vy - vx - (a+b)*omega)/wheel_Romega_4 = (vy + vx + (a+b)*omega)/wheel_R-- set the right direction for each wheelv_wheel_1 = -omega_1v_wheel_2 = -omega_2v_wheel_3 = -omega_3v_wheel_4 = -omega_4
end

3.3 路径规划

• 控制机器人进行左右移动和转向。在代码的初始化部分，需要定义dumpy点，以及初始位姿

function sysCall_init()-- 获得机器人句柄，dummy句柄you_bot = sim.getObjectHandle('youBot')you_bot_dummy = sim.getObjectHandle('youBotDummy')-- 轮子的初始句柄wheel_joints = {-1,-1,-1,-1} -- front left, rear left, rear right, front rightwheel_joints[1] = sim.getObjectHandle('rollingJoint_fr')wheel_joints[2] = sim.getObjectHandle('rollingJoint_fl')wheel_joints[3] = sim.getObjectHandle('rollingJoint_rl')wheel_joints[4] = sim.getObjectHandle('rollingJoint_rr')-- 关节的初始句柄arm_joints = {-1,-1,-1,-1,-1} -- set default valuesfor i=0,4,1 doarm_joints[i+1] = sim.getObjectHandle('youBotArmJoint'..i)end-- 关节的初始位置在哪desired_joint_angles = {180*math.pi/180, 30.91*math.pi/180, 52.42*math.pi/180, 72.68*math.pi/180, 0}-- 设置关节的初始位置for i = 1,5,1 dosim.setJointPosition(arm_joints[i], desired_joint_angles[i])end-- 设置最大速度max_joint_velocity = 100*math.pi/180--四个轮子的角速度omega_1 = 0omega_2 = 0omega_3 = 0omega_4 = 0-- youBot的参数wheel_R = 0.05 -- 0.05 ma = 0.165 -- 0.228 mb = 0.228 -- 0.25 m--dummy点后面要去的位置和方向dummy_guider_position = {0, 0, 0}dummy_guider_orientation = {0, 0, 0}circle_path_R = 1theta = 0target_pos = {0, 0, 0}--机器人的初始位置和方向youbot_init_position = {0, 0, 9.5341e-02}sim.setObjectPosition(you_bot, -1, youbot_init_position)youbot_init_orientation = {-1.57, 0, -1.57}sim.setObjectOrientation(you_bot, -1, youbot_init_orientation)err_pos = {0,0,0}err_pos_last = {0,0,0}err_vel = {0,0,0}
end

• 机器人控制部分

function sysCall_actuation()-- 保持机器人关节臂不动for i = 1,5,1 dosim.setJointPosition(arm_joints[i], desired_joint_angles[i])endsimu_time = sim.getSimulationTime()--根据时间设定目标位置和方向if simu_time < 10 thentarget_pos = {0, 1, 0}elseif simu_time < 20 thentarget_pos = {0, 0, 0}elseif simu_time < 30 thentarget_pos = {1, 0, 0}elseif simu_time < 40 thentarget_pos = {0, 0, 0}elseif simu_time < 50 thentarget_pos = {1, 1, 0}elseif simu_time < 60 thentarget_pos = {0, 0, 0}elseif simu_time < 70 thentarget_pos = {0, 0, math.pi/2}elseif simu_time < 80 thentarget_pos = {0, 0, 0}elseif simu_time < 90 thentarget_pos = {0, 0, -math.pi/2}elseif simu_time < 100 thentarget_pos = {0, 0, 0}end--获取dummy点现在的位置和方向current_pos = sim.getObjectPosition(you_bot_dummy,-1)current_orientation = sim.getObjectOrientation(you_bot_dummy, -1)current_pos[3] = current_orientation[3]-- Simple PID controlKP_pos = 1 -- 1KP_omega = 0.3KD_pos = 20  -- 20KD_omega = 0--计算要从当前点到目标点，中心点的运动状态是多少err_pos_last = err_poserr_pos = {target_pos[1] - current_pos[1], target_pos[2] - current_pos[2], target_pos[3] - current_pos[3]}err_vel = {err_pos[1] - err_pos_last[1], err_pos[2] - err_pos_last[2], err_pos[3] - err_pos_last[3]}center_velocity = {KP_pos*err_pos[1] + KD_pos*err_vel[1], KP_pos*err_pos[2] + KD_pos*err_vel[2], KP_omega*err_pos[3] + KD_omega*err_vel[3]}--根据中心点的运动状态，求四个轮子的运动状态chassisInverseKinematics(center_velocity[1], center_velocity[2], center_velocity[3], wheel_R, a, b)-- 设置四个轮子的运动状态sim.setJointTargetVelocity(wheel_joints[1], v_wheel_1)sim.setJointTargetVelocity(wheel_joints[2], v_wheel_2)sim.setJointTargetVelocity(wheel_joints[3], v_wheel_3)sim.setJointTargetVelocity(wheel_joints[4], v_wheel_4)
end