NAO高尔夫比赛（python初级版）

主逻辑 　　

  主逻辑为：找球——>定位——>击球。

找球

基本思想

  找球的过程可分为两步，即在场地找球和找到球之后走到球附近。
  初级版采用地毯式搜索在场地找球，即走一段停下来然后在原地摇头找球，找到则调整身体，使身体正对着球，并走到球附近，否则继续往前走。
  找到球后的行走过程中，机器人会不断的看球确认，一旦发现其身体方向并没有正对着球，则停止行走，并再次调整身体，直到走到指定位置。　 　

python程序实现

def walkToBall(self, min_ball_d=0.35, max_ball_theta=0.15):
    '''
    行走到球附近，若没有找到球则往前走一点

    参数：
        min_ball_d:最小接近球的距离
        max_ball_theta:最大偏离角
    '''        
    isfindBall = self.findBall()
    # 如果没有找到球则摇头找球
    if isfindBall is False:
        while True:
            isfindBall = self.moveheadToFindball()               
            if isfindBall is False:
                self.motionProxy.moveInit()
                self.motionProxy.moveTo(0.5, 0, 0)
            else:
                break
    
    # 否则找到球，调整机器人身体，使其正对着球
    self.motionProxy.moveTo(0, 0, self.ball_info[2])
    self.motionProxy.angleInterpolationWithSpeed("HeadYaw", 0, 0.1)  # 头部回正

    moveTask_1 = self.motionProxy.post.moveTo(1.5 * self.ball_info[0], 0, 0) 
    while True:
        isfindBall = self.findBall(self.pitchAngle, 0)
        if isfindBall is False:
            self.moveheadToFindball(pitchAngles=[-10], yawAngles=[-15, 15])
        ball_d = ((self.ball_info[0] ** 2 + self.ball_info[1] ** 2) ** 0.5)
        ball_theta = abs(self.ball_info[2])
        time.sleep(0.5)     

        # 头不断往下低,防止看不到球
        Names = ["HeadPitch"]            
        if ball_d > 0.4:
            self.pitchAngle = 0
        elif 0.3 < ball_d < 0.4:
            self.pitchAngle = (-100 * ball_d + 40)
        elif 0.02 < ball_d < 0.3:
            self.pitchAngle = 20
        self.motionProxy.angleInterpolationWithSpeed(Names, self.pitchAngle * rad, 0.1)                                            

        # 到达最小距离内，停止
        if min_ball_d / 3 < ball_d < min_ball_d:
            self.tts.say("I am right.")
            self.motionProxy.stop(moveTask_1)
            time.sleep(0.5)
            self.motionProxy.moveInit()
            break
        # 偏差在范围内，继续走
        elif 0.02 < abs(ball_theta) < max_ball_theta:
            # 到达最小接近球的距离
            if min_ball_d / 3 < ball_d < min_ball_d:
                self.tts.say("I am right.")
                self.motionProxy.stop(moveTask_1)
                time.sleep(0.5)
                self.motionProxy.moveInit()
                break  
            else:
                continue   
        # 超过最大偏离角
        elif abs(ball_theta) > max_ball_theta:
            self.tts.say("I am wrong.")
            self.motionProxy.stop(moveTask_1)
            time.sleep(0.5)
            self.motionProxy.moveInit()               
            self.motionProxy.moveTo(0, 0, self.ball_info[2])   

            moveTask_1 = self.motionProxy.post.moveTo(1.5 * self.ball_info[0], 0, 0)

程序说明

  首先调用看球函数findBall()进行一次找球，如果找到则跳到下面的If判断，否则返回False，进入If结构，即调用摇头找球函数moveheadTofindBall()进行摇头找球，如果还未找到则继续往前走。
  找到球后，首先根据返回的ball_Info属性（ball_info = [ball_x, ball_y, ball_theta]）的第3个值，调整身体位置，然后进入While True循环。
  进入该循环前，首先调用moveTo()函数，并调用其post属性使其进程挂起，即在行走的过程中，可以做其他事情。为了防止机器人的行走有误差，可将moveTo()函数的x参数加入一个系数，以确保机器人一定能走到球附近。
  在行走的过程中，机器人会每隔0.5s看一次球，并通过返回的ball_Info信息来判断是否到达指定位置和身体是否偏移球的位置。
  由于到达指定位置的优先级大于是否偏离球的位置，所以将是否到达指定位置的判断放在前面，即不管身体是否偏移，到达位置即可，（后续会再看一次球，调整回来）。
  如果未到达指定位置，但身体没有偏离到最大偏差角，即继续执行，一旦偏离最大偏差角，则停止行走，并立即调整身体位姿，并重新调用之前的moveTo()函数。
  注：在距离球越近时，机器人应根据距离调整低头的角度，以确定球在视野范围内。在进入走到球附近的程序中，如果没看到球，会进行一次小范围摇头找球，以确定机器人是以看到球的基础上进行校正。

定位

三角定位

基本思想

  定位的主要目的即让机器人调整至最佳击球位置。由于我们采用的是右手握杆击球，即球杆，球和球洞在同一条水平线上。

图1 机器人最佳击球位置
　　由于机器人到达球的附近时，并不一定是最佳击球位置，所以定位是高尔夫比赛中必不可少的一个环节。初级版采用的是三角定位的方式，即调整机器人的位姿，使得机器人、球和球洞之间的三角形为直角三角形。

图2 三角定位(反手锐角)
  如图2所示，红色圆点为球的位置，蓝色圆点为机器人的位置，黄色为黄杆(球洞)。机器人-球-球洞之间的夹角为α,机器人-球洞-球之间的夹角为β,球-机器人-球洞之间的夹角为γ（已知），机器人和球的距离为d（已知），球和球洞的距离为l。
  此时由于α为锐角，而最佳击球位置为直角，所以机器人应该向左绕一个以球为圆心，夹角为θ(θ = 0.5 * pi - α)的圆弧。并且此时应用反手击球。同理可得，如果α为钝角，θ = α - 0.5 * pi，机器人向右绕圆弧。
  如果机器人在另一侧，方法同上，但击球方式应为正手击球。
  但此时存在一个问题，只有黄杆上面的landmark（如图1所示），机器人才能识别出距离，但只有在1米的有效距离内识别出，而机器人可以在很远的距离内识别出黄杆，但无法返回距离。
  所以，在远距离时，只能利用黄杆来定位，但在机器人、球和黄杆组成的三角形中，由于已知的信息较少，无法正确解出三角形，因此我们必须假设一个信息是已知的。
  我们假设β是已知的，并且球和球洞的距离越远，该角度越小。利用这个假设，我们便可以让机器人通过moveTo()函数，使得到达最佳位置，即机器人-球-球洞的夹角为0.5pi。

python程序实现

def moveCircle_stick(self, stickAngle, ball_d, compensateAngle1):
    '''
    绕球走半径为机器人与球的距离的圆弧，分正反手   

    参数:
        stickAngle:黄杆与机器人的角度
        ball_d:机器人与球的距离
        compensateAngle1:补偿角(机器人-杆-球的角度)
    '''
    # 杆-球-机器人的夹角(分锐角和钝角)
    stick_ball_robotAngle = math.pi - compensateAngle1 * rad - abs(stickAngle)

    # 正手打
    if stickAngle >= 0:
        self.isbackhandFlag = False
        if 0 <= stick_ball_robotAngle < 0.5 * math.pi:
            moveAngle = 0.5 * math.pi - stick_ball_robotAngle
            move_d = (ball_d ** 2 + ball_d ** 2 
                        - 2 * ball_d * ball_d * math.cos(moveAngle)) ** 0.5
            self.motionProxy.moveInit()
            self.motionProxy.moveTo(0, -move_d, moveAngle)
        else:
            moveAngle = stick_ball_robotAngle - 0.5 * math.pi
            move_d = (ball_d ** 2 + ball_d ** 2 
                        - 2 * ball_d * ball_d * math.cos(moveAngle)) ** 0.5 
            self.motionProxy.moveInit()
            self.motionProxy.moveTo(0, move_d, -moveAngle)
    # 反手打
    else:
        self.isbackhandFlag = True
        stickAngle = abs(stickAngle)
        if 0 <= stick_ball_robotAngle < 0.5 * math.pi:
            moveAngle = 0.5 * math.pi - stick_ball_robotAngle
            move_d = (ball_d ** 2 + ball_d ** 2 
                        - 2 * ball_d * ball_d * math.cos(0.5 * math.pi - stick_ball_robotAngle)) ** 0.5
            self.motionProxy.moveInit()
            self.motionProxy.moveTo(0, move_d, -moveAngle)
        else:
            moveAngle = stick_ball_robotAngle - 0.5 * math.pi
            move_d = (ball_d ** 2 + ball_d ** 2 
                        - 2 * ball_d * ball_d * math.cos(stick_ball_robotAngle - 0.5 * math.pi)) ** 0.5 
            self.motionProxy.moveInit()
            self.motionProxy.moveTo(0, -move_d, moveAngle)

程序说明

  首先计算出图2的α，用来判断是锐角还是钝角，其次需要判断机器人和黄杆的角度，如果为正值，应该为正手击球，否则应为反手击球。
  在moveTo()的函数中，需要给定的参数为y和θ的值。其中y的值可以用余弦定理求得，而θ为0.5pi与α的差值的绝对值。
  注:机器人以左手为y坐标系正半轴，正前方为x坐标系正半轴，设置moveTo()函数的参数时，要注意正负号。　 　　　

图像定位

基本思想

  经过三角定位后，机器人基本上到达最佳击球位置，但是我们的最佳击球位置不是(0,0),而是(0.20,-0.05),所以经过三角定位后，还需要左右和上下平移。
  之前的方法为利用看球后返回的x和y轴的信息，然后调用moveTo()函数，不断的调整，但实际测试下来，效果并不理想，主要有2个原因。
  第一如果机器人不是正对着球，看球的信息会不太准确，而且由于此时要求精度较高，该误差较大。第二机器人本身精度也有误差，特别是此时只是微小移动，很难快速收敛。
  针对以上问题，我们决定直接利用球在图像中的信息进行调整，即不需要在进行球的返回信息(x和y)计算，而且此时球在图像中的像素值很大，加入一个比例控制系数就会很快的收敛。

python程序实现

def locateWithImage(self, best_ball_x=400, best_ball_y=320, best_ball_radius=28):   
    self.motionProxy.angleInterpolationWithSpeed(["HeadPitch", "HeadYaw"], [20 * rad, 0 * rad], 0.1)     
    self.ballDetect.updateBallData(client="cx", fitting=True)
    centerX, centerY, radius = self.ballDetect.getBallInfoInImage()
    bias_x = centerY - best_ball_y         
    bias_y = centerX - best_ball_x       
    Kp = 0.0008           

    if abs(radius - best_ball_radius) < 15: 
        if (abs(bias_x) < 40 and abs(bias_y) < 20):
            self.tts.say("I am OK")
            return True
        else:
            self.motionProxy.moveInit()
            move_x = Kp * bias_x 
            move_x = -1.0 / 100 if (move_x < 0 and move_x > -1.0 / 100) else move_x
            move_x = 1.0 / 100 if (move_x > 0 and move_x < 1.0 / 100) else move_x
            if abs(bias_x) > 15:
                self.motionProxy.moveTo(-move_x, 0, 0)
                time.sleep(0.5)

            move_y = Kp * bias_y 
            move_y = -1.0 / 100 if (move_y < 0 and move_y > -1.0 / 100) else move_y
            move_y = 1.0 / 100 if (move_y > 0 and move_y < 1.0 / 100) else move_y
            if abs(bias_y) > 15:
                self.motionProxy.moveTo(0, -move_y, 0)
                time.sleep(0.5)

程序说明

  首先要测得最佳击球位置在图像中的像素位置，然后根据实际值与期望值的差得到误差，注意此时图像和机器人的坐标系不一样，然后每次调整位置时，加入一个比例系数Kp，实验测得，经过3-4次调整即可快速的收敛，到达最佳击球位置。
  注：三角定位要和图像定位结合使用，三角定位是为了调整机器人与球洞的角度，而图像定位是为了让其到达指定的(x,y)位置，为了更精准的到达指定位置，每次循环进行1次三角定位和2次图像定位。实验测得，经过3次左右的循环，便可完成定位，而且其效果也比较理想。 　　

击球

基本思想

  考虑到机器人与球洞的位置，我们设计了2种击球方式，即正手击球和反手击球。
  其方向可以通过黄杆或landmark的角度来判断。当角度为正值时，说明球洞在机器人的左手边，由于我们采用右手击球，所以应为正手击球，否则应为反手击球。

python程序实现

def forehandToHitball(self, hitSpeed):
    '''
    正手击球

    参数：
        hitSpeed:击球的力度
    '''
    # 手回正
    names = ["RShoulderPitch", "RShoulderRoll", "RElbowRoll", "RElbowYaw", "RWristYaw"]
    maxSpeedFraction = 0.1
    targetAngles = [[90 * rad, -20 * rad, 5 * rad, 90 * rad, 0 * rad],
                    [80 * rad, -40 * rad, 5 * rad, 90 * rad, 0 * rad],          
                    [50 * rad, -45 * rad, 50 * rad, 90 * rad, -37 * rad],
                    [50 * rad, 5 * rad, 50 * rad, 90 * rad, 20 * rad]]  # 击球      

    for targetAngle in targetAngles:
        if targetAngle == targetAngles[-1]:
            maxSpeedFraction = hitSpeed
        self.motionProxy.angleInterpolationWithSpeed(names, targetAngle, maxSpeedFraction)
        time.sleep(0.5)

def backhandToHitball(self, hitSpeed):
    '''
    反手击球

    参数：
        hitSpeed：击球的力度
    '''        
    names = ["RShoulderPitch", "RShoulderRoll", "RElbowRoll", "RElbowYaw", "RWristYaw"]
    maxSpeedFraction = 0.1

    targetAngles = [[90 * rad, -20 * rad, 5 * rad, 90 * rad, 0 * rad],  # 手回正
                    [80 * rad, -40 * rad, 5 * rad, 90 * rad, 0 * rad],
                    [50 * rad, -45 * rad, 50 * rad, 90 * rad, 0 * rad],
                    [50 * rad, 0 * rad, 70 * rad, 90 * rad, 40 * rad],
                    [60 * rad, 2 * rad, 60 * rad, 90 * rad, 40 * rad],
                    [60 * rad, -20 * rad, 60 * rad, 90 * rad, -20 * rad]]

    for targetAngle in targetAngles:
        if targetAngle == targetAngles[-1]:
            maxSpeedFraction = hitSpeed
        self.motionProxy.angleInterpolationWithSpeed(names, targetAngle, maxSpeedFraction)
        time.sleep(0.5)

程序说明

  无论正手还是反手，都是机器人手臂的一系列关节角变化，通过连续的给每个关节不同的角度，即可实现击球动作。
  注：最后一个关节角的速度是击球时的力度，所以其值应不同于其余值，应该给定一个较大的值。

存在的问题和不足

  初级版的程序虽然整体上可以实现功能，但都是基于理想的情况下，实际测试下来，程序仍存在一些未知的bug，所以在高级版的程序中，我们要对其进行优化，主要包括以下几个方面。

逻辑思想：

1. 所有的找球，看黄杆/landmark的程序必须考虑完整，即分为看到和没看到2种情况，后续的动作一定要基于之前是找到的情况。
2. 当机器人一直往前走仍找不到球，或因为没看到球而认为没有球一直往前走，这种情况下该怎么办？
3. 定位时没有看到球，或者看球错误导致球超出视野范围外，需要让机器人摇头找下球，重新定位。
4. 击球的力度能和距离关联，实现不同的距离给定不同的力度。
5. 当球的位置超过球洞，即机器人需绕道球洞后面进行击球，此时应考虑如何避开球洞。

python程序：

1. 每次摇头找球或找黄杆/landmark时，第一次需要摇头，后面则不需要摇头，调用之前的值即可。
2. 利用try-except对程序进行异常处理，防止实际比赛时，程序报错。

注：本博客采用的视觉算法是我师兄的一篇博客：NAO机器人高尔夫中的视觉系统设计